秋月 5 finger 制御編（後編） - ブログ／こばさんの wakwak 山歩き

　とりあえずサーボ１つで軽くテストできましたので、５指バージョンかつ 5V 動作に拡張します。
　実験は ESP32 を用いましたため、信号線を 3.3V→5V するレベルコンバートが必要ですが、てっとり早くトランジスタと抵抗で簡単に仕上げてみました。

　ECB じゃなくて EBC のほうが実装的に都合が良かったため 2N3904 を使いましたが、2SC1815 とかでも大丈夫です。

　プログラムのほうは、ライブラリ的なものと、メインと分けてみました。

■ライブラリ的なほう(finger.h)

#ifndef _WAKWAK_FINGER_
#define _WAKWAK_FINGER_

#ifndef ESP32
#include <Servo.h>
#endif

class Finger
{
  public:
         Finger(int pin, bool reverse = false);
    void set(int *job);
    void tick();
    
  protected:
    int *job;
    int job_pos;
    unsigned long wait;
    
  private:
    int pin;
    bool reverse;
#ifdef ESP32
    int channel;
#else
    Servo servo;
#endif
    void setDegree(int degree = -1);
};

#endif

■ライブラリ的なほう(finger.ino)

#include "finger.h"

Finger::Finger(int pin, bool reverse)
{
  Finger::pin = pin;
  Finger::reverse = reverse;
  setDegree();
}

void Finger::set(int *job)
{
  Finger::job = job;
  Finger::wait = 0;
  Finger::job_pos = 0;
}

void Finger::tick()
{
  if(millis() > wait && job[job_pos] >= 0)
  {
    setDegree(job[job_pos]);
    job_pos++;

    wait = millis() + job[job_pos];
    job_pos++;
  }
}

void Finger::setDegree(int degree)
{
#ifdef ESP32
  const int degree_0 = 500;       // 0.5ms at 0 degree
  const int degree_180 = 2400;    // 2.4ms at 180 degree
  const int pulse_width = 20000;  // 20ms
  static int last_channel = 0;
#endif

  if(degree < 0)
  {
    // Initialize
#ifdef ESP32
    channel = last_channel;
    int freq = 1000000 / pulse_width;
    ledcSetup(channel, freq, 10);
    ledcAttachPin(pin, channel);     // for PWM
    last_channel ++;
#else
    servo.attach(pin);
#endif    
  } else {
    if(reverse)
      degree = 180 - degree;
  
#ifdef ESP32
    int pulse = (degree_0 + degree * (degree_180 - degree_0) / 180.0F);
    if(pulse < degree_0)
      pulse = degree_0;
        
    pulse = 1023 * pulse / pulse_width;
    ledcWrite(channel, pulse);
#else
    servo.write(degree);
#endif    
  }
}

■メイン部分

#include "finger.h"

// degree, delay, degree, delay ...
int job_1[] = {110, 5000, 110, 5000, 70, 5000, -1};
int job_2[] = {110, 5000, 70, 5000, 70, 5000, -1};
int job_3[] = {110, 5000, 70, 5000, 70, 5000, -1};
int job_4[] = {110, 5000, 110, 5000, 70, 5000, -1};
int job_5[] = {110, 5000, 110, 5000, 70, 5000, -1};

Finger *fingers[] = { new Finger(23)
                    , new Finger(22)
                    , new Finger(21)
                    , new Finger(19, true)
                    , new Finger(18, true) };

void setup() {
  // put your setup code here, to run once:
  fingers[0]->set(job_1);
  fingers[1]->set(job_2);
  fingers[2]->set(job_3);
  fingers[3]->set(job_4);
  fingers[4]->set(job_5);
}

void loop() {
  for(int i=0; i<sizeof(fingers) / sizeof(fingers[0]); i++)
    fingers[i]->tick();
}