概要
Arduinoでサーボモータ使って、最初と最後を滑らかに動かすためにS字曲線（シグモイド関数：sigmoid function）で動かしてみました。
S字曲線の処理はステートマシンを組み込んで作りこみました。ついでに、クラス化して複数のサーボモータを動かせる様にしました。
S字曲線関数（シグモイド関数：sigmoid function）
αの値で傾きが変わります。10が急激な動作になります。このスケッチでは1〜10の間で設定できる様にしています。
αの数字が大きいと、平らな所が長いのでSigmoidCurveTable[]で使用するエリアを狭めています。
動画
https://www.youtube.com/watch?v=l9K8CA31GJ4
https://twitter.com/masashi_214/status/1227083220480483328
https://twitter.com/masashi_214/status/1227090452517552133
スケッチについて
詳しい説明は、別紙「Arduinoで作るDCCデコーダ」を買ってね（まだ売っていませんw）
sigmoidServo.ino
//--------------------------------------------------------------------------------
// S字曲線（シグモイド関数：sigmoid function）で動かしてみました by aya
// http://maison-dcc.sblo.jp/ http://dcc.client.jp/ https://twitter.com/masashi_214
//--------------------------------------------------------------------------------
#include <Servo.h>
#include <math.h>
#include "SigmoidFunction.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 670 // DM-S0025 400-2100 0-180deg
#define ANGLE_MAX_A 2600 // DM-S0037 1000-2000 0-90deg
// DM-S0037 700-2400 0-180deg
#define ANGLE_MIN_B 500 // 1000 -90deg
#define ANGLE_MAX_B 2000 // 2000 +90deg
unsigned long PreviosTime = 0;
unsigned long PreviosTimeState = 0;
void setup() {
Serial.begin(115200);
PreviosTime = millis();
PreviosTimeState = PreviosTime;
}
void loop() {
static SigmoidFunction ServoA(0,PIN_SERVO1,670,2600,1);
static SigmoidFunction ServoB(1,PIN_SERVO2,400,2100,1);
static int state = 0;
int ret;
if( (millis() - PreviosTime ) >= 10 ){ // 1000:1000msec 前回から1000ms経過したかチェック
PreviosTime = millis();
switch(state){
case 0:
ServoA.exec(0,70);
ServoB.exec(0,70);
ServoA.exec(550,0);
ServoB.exec(550,0);
state = 1;
break;
case 1:
ret = ServoA.stateCheck();
ret = ServoB.stateCheck();
if(ret==3)
state = 2;
break;
case 2:
ServoA.exec(500,70);
ServoB.exec(500,70);
state = 3;
break;
case 3:
ret = ServoA.stateCheck();
ret = ServoB.stateCheck();
break;
default:
break;
}
}
if( (millis() - PreviosTimeState ) >= 1000 ){ // 1000:1000msec 前回から1000ms経過したかチェック
PreviosTimeState = millis();
Serial.println(ServoA.nowState());
}
}
SigmoidFunction.cpp
//--------------------------------------------------------------------------------
// ServoSequenceクラス by aya
// http://maison-dcc.sblo.jp/ http://dcc.client.jp/ https://twitter.com/masashi_214
//------------------------------------------------------------------------------
#include <arduino.h>
#include "SigmoidFunction.h"
// コンストラクタ
SigmoidFunction::SigmoidFunction(char ch,unsigned char port, int amin, int amax, int gain)
{
pinMode(port, OUTPUT);
digitalWrite(port, HIGH);
minAngle = amin;
maxAngle = amax;
tablek = gain;
if(ch == 0)
ServoA.attach(port, minAngle, maxAngle);
if(ch == 1)
ServoB.attach(port, minAngle, maxAngle);
svch = ch;
state = ST_INIT;
lmax = SigmoidCurveTable[gain];
}
int SigmoidFunction::nowState()
{
return state;
}
void SigmoidFunction::exec(int tim, int deg)
{
if(tim == 0){
nowPwm = map(deg,0,180,minAngle,maxAngle);
servoABwite(svch, (int)nowPwm);
}
nextPwm = map(deg,0,180,minAngle,maxAngle);
deltSig = (nextPwm - nowPwm) / (float)tim /100.0;
state = ST_INIT;
}
void SigmoidFunction::servoABwite(char ch, int ref)
{
if(ch == 0){
ServoA.writeMicroseconds(ref);
}
if(ch == 1){
ServoB.writeMicroseconds(ref);
}
}
// SigmoidFunction ステートマシン（状態遷移）
int SigmoidFunction::stateCheck()
{
switch(state){
case ST_INIT:
if(nextPwm - nowPwm < 0){
SigmoidCurve = lmax;
state = ST_DOWN;
} else {
SigmoidCurve = -lmax;
state = ST_UP;
}
break;
case ST_UP:
SigmoidCurve = SigmoidCurve + deltSig;
if( SigmoidCurve > lmax){
servoABwite(svch, (int)nextPwm);
nowPwm = nextPwm;
state = ST_END;
break;
}
ServoDeg = 2000 * (1/ (1 + exp(-tablek*SigmoidCurve) ) );
pos = map(ServoDeg,0,2000,nowPwm,nextPwm);
servoABwite(svch, (int)pos);
break;
case ST_DOWN:
SigmoidCurve = SigmoidCurve + deltSig;
if( SigmoidCurve < -lmax){
servoABwite(svch, (int)nextPwm);
nowPwm = nextPwm;
state = ST_END;
break;
}
ServoDeg = 2000 * (1/ (1 + exp(-tablek*SigmoidCurve) ) );
pos = map(ServoDeg,2000,0,nowPwm,nextPwm);
servoABwite(svch, (int)pos);
break;
case ST_END:
break;
default:
break;
}
return state;
}
SigmoidFunction.h
#ifndef SIGMOIDFUNCTION_H_
#define SIGMOIDFUNCTION_H_
#include <Servo.h>
// 状態基底クラス
class SigmoidFunction
{
public:
SigmoidFunction(char ch,unsigned char port, int amin, int amax, int gain);
int stateCheck();
void servoABwite(char ch, int ref);
int nowState();
void exec(int tim, int deg);
private:
char state = ST_INIT;
char updownFlg;
char svch;
unsigned char port;
float PwmRef;
float deltPwm; // 10msあたりのpwm増加量
float lmax;
float deltSig;
float SigmoidCurve;
float ServoDeg;
int nowPwm;
int nextPwm;
int minAngle;
int maxAngle;
int tablek;
int pos;
//S字曲線（シグモイド曲線）
float SigmoidCurveTable[10+1]={0,5,2.6,1.8,1.3,1,0.8,0.7,0.6,0.5,0.5};
Servo ServoA; // create servo object to control a servo
Servo ServoB; // create servo object to control a servo
enum{
ST_INIT = 0,
ST_DOWN,
ST_UP,
ST_END,
};
enum{
DOWN = 0,
STY,
UP,
};
};
#endif