タイマー割り込みを使ってロボットのサーボモータを動かそう

クムクムロボット[QX-001R3J]で利用できるタイマー割り込み[FlexiTimer2]を使ってクムクムの腕と顔のサーボモータを非同期で動かすプログラムを作ってみます。

顔は腕より細かい単位でうごかしたいので、サーボクラスのwriteMicrosecondsを使ってモータのパルス単位での回転をしてみます。

 

クムクムロボットのモータポート

クムクムのサーボモータについては、チュートリアルの[6.モータを動かそう] にもまとめたりますがもう一度下記に記述します。

作るプログラムの仕様

今日作るプログラムは、両腕を1秒ごとに上下に動かし顔を1ミリ秒ごとに1パルスずつ動かそうというものです。

プログラムとしては、両腕は0度に動かし1秒まったら180度に動かす！という動作をメインのloop関数の中で行います。
顔についてはそれとはまったく別に [Arduinoタイマー割り込みをクムクムロボットで試す]で使ったFlexiTimer2を使って動かします。

プログラム

#include<Servo.h >
#include<FlexiTimer2.h >

#define PLS_MIN    500  //モータパルス最小
#define PLS_MAX   2500  //モータパルス最大
#define MOT_FACE     7  //顔モータのピン番号
#define MOT_LARM    10  //左腕モータピン番号
#define MOT_RARM     4  //右腕モータピン番号
#define MOT_POWER   11  //モータ電源

int PLS=500;            //回転させるパルス
int DST=1;              //回転方向決定パルス
Servo SV_FACE;          //顔モータ割り当てクラス
Servo SV_LARM;          //右腕モータ割り当てクラス
Servo SV_RARM;          //左腕ータ割り当てクラス

//
//  定期的に実行される関数
//
void MoveMotor() {
  PLS+=DST;                        //回転位置のパルスを計算
  SV_FACE.writeMicroseconds(PLS);  //顔をパルス位置に回転
  //Serial.println(PLS);
  if(PLS==PLS_MAX)DST=-1;          //最大パルスにきたら反転
  if(PLS==PLS_MIN)DST=1;           //最小パルスにきたら反転
}
//
//  セットアップ関数
//
void setup() {
  //Serial.begin(9600);
  //while (!Serial) { ; } //Leonardoの場合にはこの部分が必要 
  
  //ポートの設定
  pinMode(MOT_FACE, OUTPUT);    // クムクムの顔のモータ
  pinMode(MOT_RARM, OUTPUT);    // クムクムの顔のモータ
  pinMode(MOT_LARM, OUTPUT);    // クムクムの顔のモータ
  pinMode(MOT_POWER,OUTPUT);     // クムクムのモータ電源制御
  digitalWrite(MOT_POWER,HIGH) ; // 電源ON
  
  //割り当て初期化
  SV_FACE.attach(MOT_FACE,PLS_MIN,PLS_MAX); // 顔モータ割り当て
  SV_LARM.attach(MOT_LARM); // 左腕割り当て
  SV_RARM.attach(MOT_RARM); // 右腕割り当て
  SV_FACE.writeMicroseconds(PLS);
  delay(1000) ;
  
  //タイマースタート
  FlexiTimer2::set(1, MoveMotor); 
  FlexiTimer2::start();             
}
//
//メインループ
//
void loop() {
  //腕0
  SV_LARM.write(0) ;
  SV_RARM.write(0) ;
  delay(1000) ;
  //腕180
  SV_LARM.write(180) ;
  SV_RARM.write(180) ;
  delay(1000) ;
}