【Arduino】アナログサーボをいい感じに動かす

どうもcaketetuです。今回はArduinoでアナログサーボを動かすテストです。

ただ動かすだけでは味気ないので、使いやすくスムーズに動かせるようにしてみます。

・とりあえず動かしてみる

まずはよくあるライブラリを使った簡単な実装です。

#include <Servo.h>

#define SERVO_PIN 5

Servo myservo;
 
void setup() {
    Serial.begin(115200);
    myservo.attach(SERVO_PIN);    //サーボ初期化
    myservo.write(90);            //初期角度90に設定
    delay(500);                   //移動まで待つ
}
 
void loop() {
    myservo.write(0);
    Serial.println("Servo move 0");
    delay(1000);
    myservo.write(90);
    Serial.println("Servo move 90");
    delay(1000);
    myservo.write(180);
    Serial.println("Servo move 180");
    delay(1000);
}

動作の様子です

・スムーズに動かしてみる

上のような簡単な実装では角度を与えたときにサーボの能力限界の速さで

動こうとするため、ゆっくり動かすのは不可能です。よって随時Delayを入れて

少しづつ指示を出すようにします。具体的には、指定した角度に何msで

移動せよという関数を作ります。

#include <Servo.h>

#define SERVO_PIN 5

int servo_move(int pos, int msec );

Servo servo;
int pos_now;    //角度保存用
 
void setup() {
    Serial.begin(115200);
    servo.attach(SERVO_PIN);    //サーボ初期化
    servo.write(90);            //初期角度90に設定
    delay(500);                 //移動まで待つ
    pos_now=90;                 //現在角度保存
    servo.detach();             //サーボピン解除
}
 
void loop() {
    //myservo.write(0);
    servo_move(0,1000);
    Serial.println(pos_now,DEC);
    Serial.println("Servo move 0");
    //myservo.write(90);
    servo_move(90,1000);
    Serial.println(pos_now,DEC);
    Serial.println("Servo move 90");
    //myservo.write(180);
    servo_move(180,1000);
    Serial.println(pos_now,DEC);
    Serial.println("Servo move 180");
}

//----------------------------------------------------------
//    サーボ動作関数    deg;-90~90 msec: 移動時間[ms]
//----------------------------------------------------------
int servo_move(int pos, int msec ){
  if(pos>180 || pos<0) return -1;    //posが範囲外ならリターン
  int d_pos = pos - pos_now;          //角度偏差計算
  int s_delay = abs(int(msec/d_pos)); //角度偏差からディレイ時間計算
  if(s_delay<0) return -1;            //ディレイ時間が0以下ならリターン(速すぎるため)
  
  servo.attach(SERVO_PIN);    //サーボ初期化
  
  if (d_pos > 0) { //d_posが0以上の時、プラス向きに動作
    for (int i = pos_now; i<=pos; i++) {
        servo.write(i);   //サーボ書き込み
        delay(s_delay);   //s_delayまで待つ
        pos_now = i;      //pos_now保存
    }
  }else if(d_pos < 0) { //d_posが0以下の時、マイナス向きに動作
    for(int i = pos_now; i>=pos; i--) {
        servo.write(i);   //サーボ書き込み
        delay(s_delay);   //s_delayまで待つ
        pos_now = i;      //pos_now保存
    }
  }else{ //d_posが0の時
  }
  
  servo.detach();   //サーボピン解除
}

動作の様子です

サーボの動きがゆっくり、スムーズなものになりました。

・割込みで実行してみる

ここまではメイン関数に書いていましたが、メイン関数はLCDの描写や通信

なんかで忙しいのでバックグラウンドで動くようにしたい。

ついでにサーボを動かした後はサーボをOFFし余計な電力消費や騒音を抑えます。

#include <MsTimer2.h>
#include <Servo.h>

#define SERVO_PIN 5

int servo_set(int pos, int msec);

Servo servo;                //サーボクラス
volatile int pos_now=90;    //角度保存用
volatile int pos_target=90; //ターゲットポジション
volatile int servo_flg=0;   //サーボ移動フラグ

//サーボタイマー
void timer_servo() {
   if(pos_target>pos_now){
      servo_flg = 1;
      pos_now++;
      servo.write(pos_now);
   }else if(pos_target<pos_now){
      servo_flg = 1;
      pos_now--;
      servo.write(pos_now);
   }else{
      servo_flg = 0;
      servo.detach();   //サーボピン解除
      MsTimer2::stop();
   }
}

//サーボ角度セット
int servo_set(int pos, int msec) {
    if(pos>180 || pos<0) return -1;    //posが範囲外ならリターン
    if(pos == pos_now)   return -1;    //pos=pos_nowならリターン
    int d_pos = pos-pos_now;          //角度偏差計算
    int s_delay = abs(int(msec/d_pos)); //角度偏差からディレイ時間計算
    pos_target = pos;
    servo.attach(SERVO_PIN);    //サーボ初期化
    servo_flg = 1;              //フラグON
    MsTimer2::set(s_delay, timer_servo);  //タイマセット
    MsTimer2::start();                    //タイマスタート
    return 1;           //正常リターン
}
 
void setup() {
    Serial.begin(115200);       //シリアルセット
    servo.attach(SERVO_PIN);    //サーボ初期化
    servo.write(90);            //初期角度90に設定
    delay(500);                 //移動まで待つ
    pos_now=90;                 //現在角度保存
    pos_target=90;              //現在角度保存
    servo.detach();             //サーボピン解除
}
 
void loop() {
    servo_set(0,1000);
    while(servo_flg);
    //delay(5000);
    servo_set(90,1000);
    while(servo_flg);
    //delay(5000);
    servo_set(180,1000);
    while(servo_flg);
    //delay(5000);
}

動きは変わらないので動画は割愛します。サーボの動きは同じですが、

サーボ動作中もメイン関数で色々できるのでより効率的なプログラミングができます。

今回は以上です。実はまだ割込みを細かくするだとかサーボの指示をDuty比で

指示できる関数を使ったりだとかまだ改善の余地はあります。もっと精密な動作

が必要になった時は実装してみようかなと思います。