Arduinoでサーボモータを S 字曲線（シグモイド関数：sigmoid function）で動かしてみる

###概要
Arduinoでサーボモータ使って、最初と最後を滑らかに動かすためにS字曲線（シグモイド関数：sigmoid function）で動かしてみました。
S字曲線の処理はステートマシンを組み込んで作りこみました。ついでに、クラス化して複数のサーボモータを動かせる様にしました。

###S字曲線関数（シグモイド関数：sigmoid function）

![servo4.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/111210/a929a013-f54b-3863-5ef8-08449125d912.png)
![servo5.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/111210/e2f3f380-77ad-86f0-64cd-1ab14209ed97.png)

αの値で傾きが変わります。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）

***
```c++: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());
  }
}
```

```cpp: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;
}
```

```cpp: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