WioTermialでサーボモータを動かしてみる。

電子工作

f:id:manpukukoji:20200722193644j:plain
Aruduinoでサーボモータを動かすのは、ライブラリで"servo,write(90);" とか、やると、ちょいちょいと、動いちゃいます。
WioTermialでも同じようにできるのかな？　と、思ってやってみると、コンパイルでエラーメッセージがダァーと吐き出されました。
まぁ、そりゃ、そうですよね。　Arduinoじゃないんだから。
ほんじゃ、まぁ、自前でやってみますか？　と、いうことになりまして、…　とはいえ、タイマー割込みとかそんなテクニックはできるわけもなく、もう、力任せのdigitalWrite とdelayMicroseconds で頑張るわけでした。
まぁ、そうこうするうちに、ちゃんとしたサーボモータのライブラリが出てくるでしょうけどね。
せっかくなので、お勉強を兼ねて、ライブラリもどきを作って、動かしてみることにしました。
仕様としては、以下のような命令を用意してみました。
Myservo servo(2); 　とやって、出力ピンを指定します。
サーボモーターのパルス幅の最大と最低をマイクロ秒で指定することもできます。
Miservo servo(2, 2000,1000); 　と、言ったような感じ。　デフォルトは、指定しなければ、Max.=2400 でMin.=544 になってます。
サーボの制御命令は、以下の2つです。write0と、writeMicroseconds0 で、お尻にゼロがつきます。

  • servo.write0(int angle); 角度で指定します。　60（60度）とか
  • servo.writeMicroseconds0(int ミリ秒);　　パルス幅（ミリ秒）で指定します。　1500（1500msec)とか

また、その位置で停止する期間（ミリ秒）を指定することもできます。
その場合は、

  • servo.write0(60,1000); と、やると、60度の位置で1秒停止します。　同様に
  • servo.writeMicroseconds(1500,2000); と、やると、1500マイクロ秒の位置で2秒間停止します。

とりあえず、そんな感じで、サンプルと合わせて以下に書いときますが、大事なサーボモーター、壊しちゃうと大変なので、あくまでも自己責任でよろしく。
ライブラリの使い方も参考サイトで分かりやすく説明してくれています。

参考サイト

以下のサイトが大変わかりやすく、参考になりました。
nobita-rx7.hatenablog.com


ヘッダファイル　(MyServo.h)

/*
  MyServo.h - WioTerminal Library for Servo control.
  Created by Manpukukoji, July 22, 2020.
*/

#ifndef MyServo_h 
#define MyServo_h 
#include "Arduino.h"

class MyServo{
  public:
    MyServo(int pin);
    MyServo(int pin, int ms_max, int ms_min);
    void write0(int _degree);
    void write0(int _degree, int _dur);
    void writeMicroseconds0(int _m_sec);
    void writeMicroseconds0(int _m_sec, int _dur);
  private:
    int _pin;
    int _m_sec;
    int _ms_max, _ms_min;
    int _dur;
    int _degree;
    int _microsec;
};

#endif

ソースファイル　(MyServo.cpp)

/*
  MyServo.h - WioTerminal Library for Servo control.
  Created by Manpukukoji, July 22, 2020.
*/


#include "Arduino.h" 
#include "MyServo.h"

MyServo::MyServo(int pin){
  pinMode(pin, OUTPUT);
  _pin = pin;
  _ms_max = 2400;
  _ms_min = 544;
}

MyServo::MyServo(int pin, int ms_max, int ms_min){
  pinMode(pin, OUTPUT);
  _pin = pin;
  _ms_max = ms_max;
  _ms_min = ms_min;
}

void MyServo::write0(int degree, int dur) {
   _degree = degree;
   _dur = dur;  
  int _m_sec = (_ms_max-_ms_min)*_degree/180+_ms_min;
  MyServo::writeMicroseconds0(_m_sec, _dur);
}

void MyServo::write0(int degree) {
  _degree = degree;
  int _m_sec = (_ms_max - _ms_min)*_degree/180+_ms_min;
  MyServo::writeMicroseconds0(_m_sec);
}

void MyServo::writeMicroseconds0(int microsec, int dur) {
  _microsec = microsec;
  _dur = dur;
  unsigned long _st_t = millis();
  while(millis()-_st_t < dur) {
    digitalWrite(_pin,HIGH);
    delayMicroseconds(_microsec);
    digitalWrite(_pin,LOW);
    delayMicroseconds(20000 - _microsec);
  }
}

void MyServo::writeMicroseconds0(int microsec) {
  _microsec = microsec;
  unsigned long _st_t = millis();
  while(millis()-_st_t < 5) {
    digitalWrite(_pin,HIGH);
    delayMicroseconds(_microsec);
    digitalWrite(_pin,LOW);
    delayMicroseconds(20000 - _microsec);
  }
}

サンプル実行ファイル　
以下では、WioTerminalのD2ピン（16番ピン）から出力しています。

#include "MyServo.h"

int outpin=D2;
MyServo servo(outpin);

void setup() {
  pinMode(outpin, OUTPUT);
}
int j;
void loop() {
  for (j = 0; j<4; j++){
    for (int i=60; i<120; i++){
      servo.write0(i);
    }  
    for (int i=120; i>60; i--){
      servo.write0(i);
    }
  }
  for (j = 0; j<4; j++){
    for (int i=1163; i<1781; i+=20){
      servo.writeMicroseconds0(i);
    }  
    for (int i=1781; i>1163; i-=20){
      servo.writeMicroseconds0(i);
    }
  }

  for (j = 0; j<4; j++){
    for (int i=60; i<120; i+=30){
      servo.write0(i,1000);
    }  
    for (int i=120; i>60; i-=30){
      servo.write0(i,1000);
    }
  }

}