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

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

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

クムクムロボットのモータポート
作るプログラムの仕様
1. プログラム

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

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

Arduinoとしてのポート	部位
4	右腕
5	右足の付け根
6	右足
7	顔
8	左足
9	左足の付け根
10	左腕
11	モーターへの電源供給制御

作るプログラムの仕様

今日作るプログラムは、両腕を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) ;
}

#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) ;

}