概要
Arduinoでサーボモータ使って、設定した角度から角度へ設定した時間で移行するプログラミングができるスケッチを
考えてみました。
ついでに、クラス化して複数のサーボモータを動かせる様にしました（今んとこ２個は確認）
動画
https://twitter.com/masashi_214/status/1228659627123830784
https://twitter.com/masashi_214/status/1228660176900673537
スケッチについて
ptn1とptn2が踏切用のプログラムで、以下の図の様に動作する様にデータを作っております。
詳しい説明は、別紙「Arduinoで作るDCCデコーダ」を買ってね（まだ売っていませんw）
servoSeq.ino
//--------------------------------------------------------------------------------
// サーボモータをスイーププログラムで動かしてみました by aya
// http://maison-dcc.sblo.jp/ http://dcc.client.jp/ https://twitter.com/masashi_214
//--------------------------------------------------------------------------------
# include <math.h>
# include "servoSeq.h"
//各種設定、宣言
# define PIN_F0_F 3 // D3 PD3,PWM
# define PIN_F0_R 4 // D4 PD4
# define PIN_AUX1 5 // D5 PD5
# define PIN_AUX2 6 // D6 PD6
# define PIN_AUX3 7 // D7 PD7
# define PIN_AUX4 8 // D8 PB0
# define PIN_AUX5 9 // D9 PB1,DIGITAL TR,PWM
# define PIN_AUX6 10 // D10 PB2,DIGITAL TR,PWM
# define PIN_AUX7 11 // D11 PB3,DIGITAL TR,PWM
# define MAX_SERVO 2 // サーボの最大数2個
# define PIN_SERVO1 5 // D5 PD5
# define PIN_SERVO2 4 // D4 PD4
# define PIN_LED_DIV1 6 // D6 PD6 SERVO1用DIV(分岐) LED
# define PIN_LED_STR1 7 // D7 PD7 SERVO1用STR(直進) LED
# define PIN_LED_DIV2 8 // D8 PB0 SERVO2用DIV(分岐) LED
# define PIN_LED_STR2 9 // D9 PB1 SERVO2用STR(直進) LED
# define ANGLE_MIN_A 800 // DM-S0025 400-2100 0-180deg
# define ANGLE_MAX_A 2200 // DM-S0037 1000-2000 0-90deg
// DM-S0037 670-2600 0-180deg
# define ANGLE_MIN_B 500 // 1000 -90deg
# define ANGLE_MAX_B 2000 // 2000 +90deg
unsigned long PreviosTime = 0;
unsigned long PreviosTimeState = 0;
//Servo ServoB; // create servo object to control b servo
unsigned int ptn1[10][3]={//ms ,deg
{ 0 ,75 },
{ 200 ,70 }, // 200:2000ms
{ 350 ,10 }, // 350:3500ms
{ 200 , 0 },
{ 600 , 0 },
{ 150 ,10 },
{ 200 ,70 },
{ 150 ,75 },
{ 0 ,999 }};
unsigned int ptn2[10][3]={//ms ,deg
{ 0 ,75 },
{ 400 ,75 },
{ 200 ,70 }, // 200:2000ms
{ 350 ,10 }, // 350:3500ms
{ 200 , 0 },
{ 200 , 0 },
{ 150 ,10 },
{ 200 ,70 },
{ 150 ,75 },
{ 0 ,999 }};
unsigned int ptn3[10][3]={//ms ,deg
{ 0 ,90 },
{ 30 ,0 },
{ 30 ,45 },
{ 30 ,0 },
{ 30 ,90 },
{ 30 ,0 },
{ 30 ,45 },
{ 30 ,0 },
{ 30 ,90 },
{ 0 ,999 }};
void setup() {
Serial.begin(115200);
PreviosTime = millis();
PreviosTimeState = PreviosTime;
}
void loop() {
static ServoSequence ServoA(0,PIN_SERVO1,ptn1,670,2600);
static ServoSequence ServoB(1,PIN_SERVO2,ptn2,400,2100);
if( (millis() - PreviosTime ) >= 10 ){ // 1000:1000msec 前回から1000ms経過したかチェック
PreviosTime = millis();
ServoA.stateCheck();
ServoB.stateCheck();
}
if( (millis() - PreviosTimeState ) >= 1000 ){ // 1000:1000msec 前回から1000ms経過したかチェック
PreviosTimeState = millis();
Serial.println(ServoA.nowState());
}
}
ServoSeq.cpp
//------------------------------------------------------------------------------
// ServoSequenceクラス by aya
// http://maison-dcc.sblo.jp/ http://dcc.client.jp/ https://twitter.com/masashi_214
//------------------------------------------------------------------------------
# include <arduino.h>
# include "servoSeq.h"
// コンストラクタ
ServoSequence::ServoSequence(char ch,unsigned char port, unsigned int pn[10][3],int MinAngle, int MaxAngle)
{
char i;
pinMode(port, OUTPUT);
digitalWrite(port, HIGH);
if(ch == 0)
ServoA.attach(port, MinAngle, MaxAngle);
if(ch == 1)
ServoB.attach(port, MinAngle, MaxAngle);
svch = ch;
state = ST_INIT;
for(i=0 ; i <= 10 ; i++){
if(pn[i][1] == 999){
ptn[i][1] = 999;
break;
}
ptn[i][0] = pn[i][0];
ptn[i][1] = pn[i][1];
ptn[i][2] = (int)map(pn[i][1],0,180,MinAngle,MaxAngle); // 角度→PwmRefに変換
}
}
int ServoSequence::nowState()
{
return state;
}
void ServoSequence::servoABwite(char ch, int ref)
{
if(ch == 0)
ServoA.writeMicroseconds(ref);
if(ch == 1)
ServoB.writeMicroseconds(ref);
}
// ServoSequence ステートマシン（状態遷移）
void ServoSequence::stateCheck()
{
switch(state){
case ST_INIT:
adr = 0;
deltPwm = 0;
state = ST_FARST;
break;
case ST_FARST:
PwmRef = ptn[adr][2];
servoABwite(svch, (int)PwmRef);
adr++;
nowPwm = ptn[adr-1][2];
nextPwm = ptn[adr][2];
deltPwm = (nextPwm - nowPwm) / (float)ptn[adr][0];
if(nextPwm - nowPwm == 0){
updownFlg = STY;
styTime = ptn[adr][0];
state = ST_STAY;
break;
}
else if(nextPwm - nowPwm < 0)
updownFlg = DOWN;
else
updownFlg = UP;
state = ST_RUN;
break;
case ST_STAY:
styTime --;
if(styTime <= 0)
state = ST_NEXT;
break;
case ST_RUN:
PwmRef = PwmRef + deltPwm;
if((updownFlg == DOWN) && (PwmRef <= nextPwm)){
servoABwite(svch, (int)nextPwm);
state = ST_NEXT;
} else if((updownFlg == UP) && (PwmRef >= nextPwm)){
servoABwite(svch, (int)nextPwm);
state = ST_NEXT;
} else {
servoABwite(svch, (int)PwmRef);
}
break;
case ST_NEXT:
adr++;
if(ptn[adr][1]==999){
state = ST_END;
break;
}
nowPwm = ptn[adr-1][2];
nextPwm = ptn[adr][2];
deltPwm = (nextPwm - nowPwm) / (float)ptn[adr][0];
if(nextPwm - nowPwm == 0){
updownFlg = STY;
styTime = ptn[adr][0];
state = ST_STAY;
break;
}
else if(nextPwm - nowPwm < 0)
updownFlg = DOWN;
else
updownFlg = UP;
state = ST_RUN;
break;
case ST_END:
break;
default:
break;
}
}
ServoSeq.h
# ifndef SERVO_H_
# define SERVO_H_
# include <Servo.h>
// 状態基底クラス
class ServoSequence
{
public:
ServoSequence(char ch,unsigned char port, unsigned int pn[10][3],int MinAngle, int MaxAngle);
void stateCheck();
void servoABwite(char ch, int ref);
int nowState();
private:
char state = ST_INIT;
char updownFlg;
char svch;
unsigned char port;
unsigned char adr;
unsigned int ptn[10][3];
float PwmRef;
float deltPwm; // 10msあたりのpwm増加量
int nowPwm;
int nextPwm;
int styTime;
Servo ServoA; // create servo object to control a servo
Servo ServoB; // create servo object to control a servo
enum{
ST_INIT = 0,
ST_FARST,
ST_RUN,
ST_STAY,
ST_NEXT,
ST_END,
};
enum{
DOWN = 0,
STY,
UP,
};
};
# endif