Go to Qiita Advent Calendar 2024 Top
LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@masashi_214

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

Last updated at Posted at 2020-02-16

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

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

servo4.png
servo5.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）

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
0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?