16servo

servo.ino

#include <Adafruit_PWMServoDriver.h>
#include <Wire.h>
 
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVOMIN 150
#define SERVOMAX 600
#define CENTER 375

#define MODE_DEBUG 1
#define MODE_MOTION 2

//関数宣言
void smoothMotion(int[],int,int);

// モード
int mode = MODE_MOTION;

// サーボ変数
unsigned int target = 0;
int pos[]      = { 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500};  //現在位置を示す配列
int home_pos[] = { 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500};  //ホームポジション
int rev[]  = {  -1, -1,  1,  1,  1, -1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1};  //サーボの向き(開く方向がプラス)
// 以下のポジションはホームポジションからの差分(プラスマイナス400まで)
int pos0[]     = {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int pos1a[]     = { 300,  0,  0, -300,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int pos1b[]     = { -300,  0,  0, 300,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int pos2[]     = {  0,300,  0,  0,300,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int pos3[]     = {  0,  0, 300,  0,  0, 300,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int pos4[]     = {  0, 300, 300,  0, 300, 300,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int pos5[]     = { 100, -300,-200, 100, -300, -200,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int pos6a[]     = {  0, -400, 400,  0, -400, 400,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int pos6b[]     = {  0, 400, -400,  0, 400, -400,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};

int pos_d[]     = {  0,100,  300,  0,-300, -100,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int pos_a[]     = {  0,-300, -100,  0, 100,  300,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int pos_w[]     = { -300,  0,  0,  -300,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int pos_s[]     = { 300,  0,  0,  300,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};

// シリアル
char cmd;
 
void setup() {
 Serial.begin(115200);
 pwm.begin();
 pwm.setPWMFreq(60);
 for (int i=0;i<16;i++)
 pwm.setPWM(i,0,CENTER);
 Serial.println("16servo start");
}
 
void loop() {
 if(mode == MODE_DEBUG){
   if (Serial.available()){
    cmd = Serial.read();
    if (cmd == 'a'){
     target = (target-1)%16;
     Serial.print("target ");
     Serial.println(target);
    }else if (cmd == 'd'){
     target = (target+1)%16;
     Serial.print("target ");
     Serial.println(target);
    }else if (cmd == 'w'){
     pos[target]+=2;
     Serial.print("pos ");
     Serial.println(pos[target]);
    }else if (cmd == 's'){
     pos[target]-=2;
     Serial.print("pos ");
     Serial.println(pos[target]);
    }else if (cmd == 'm'){
     mode = MODE_MOTION;
     Serial.println("To MODE_MOTION");
    }
   }
   pwm.setPWM(target, 0, map(pos[target],0,1000,SERVOMIN,SERVOMAX));
 }else if(mode == MODE_MOTION){
   if (Serial.available()){
    cmd = Serial.read();
    if (cmd == '0'){
      smoothMotion(pos0,400,20);//400msecを20等分して動かす
    }else if (cmd == '1'){
      smoothMotion(pos1a,200,40);
      smoothMotion(pos1b,400,40);
      smoothMotion(pos0,200,20);
    }else if (cmd == '2'){
      smoothMotion(pos2,200,10);
    }else if (cmd == '3'){
      smoothMotion(pos3,200,10);
    }else if (cmd == '4'){
      smoothMotion(pos4,200,10);
    }else if (cmd == '5'){
      smoothMotion(pos5,200,10);
    }else if (cmd == '6'){
      smoothMotion(pos6a,400,40);
      smoothMotion(pos6b,400,40);
      smoothMotion(pos0,400,20);
    }else if (cmd == 'd'){
      smoothMotion(pos_d,200,10);
    }else if (cmd == 'a'){
      smoothMotion(pos_a,200,10);
    }else if (cmd == 'w'){
      smoothMotion(pos_w,200,10);
    }else if (cmd == 's'){
      smoothMotion(pos_s,200,10);
    }else if (cmd == 'n'){
     mode = MODE_DEBUG;
     Serial.println("To MODE_DEBUG");
    }
   }
 }
}

void smoothMotion(int target_pos[], int time,int step){
  int buff_pos[16];
  for (int i=0; i <= 15; i++){
    buff_pos[i]=pos[i];
  }
  for(int j=0; j <= step; j++){
    for (int i=0; i <= 15; i++){
      pos[i] = buff_pos[i]+(int)((float)(rev[i]*target_pos[i]+home_pos[i]-buff_pos[i])*(j+1)/step);
      pwm.setPWM(i, 0, map(pos[i],0,1000,SERVOMIN,SERVOMAX));
    }
    delay(time/step);
  }
}