Arduino簡単二足歩行ロボットのモーション修正そしてMacのコントロールアプリで動かす

2017/10/25
00:48

Arduino簡単二足歩行ロボットのモーション修正そしてMacのコントロールアプリで動かす

こんにちはRoboTakaoです。ようこそ「極力ローコストロボット製作ブログ」にお越しくださいました。

今回はロボットのモーションを多少修正して、ついでにXcodeで作ったコントロール用アプリでワイヤレスコントロールします。

コントロール用アプリ

これは以前も紹介したものですが、Macで動くようにXcodeで作ったアプリです。

以前のモーション作成アプリの記事

ステップごとにモーションを作成する機能と方向指示ボタンで命令を送れるようにしています。

まずはモーションを前後、左右で作り、それぞれをスケッチに組み込んでします。

結果

スケッチ


#include <Servo.h> 
 
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;

// RU,RB,LU,LB
int ang0[4]={90,89,85,87};
int ang[16][4]={
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0},
  {0,0,0,0}};
int stp[2][4]={{0,0,0,0},{0,0,0,0}};
int fw[13][4]={
  {0,0,0,0},
  {0,-15,0,-15},
  {-15,-15,-15,-15},
  {-15,0,-15,0},
  {-15,15,-15,15},
  {15,15,15,15},
  {15,0,15,0},
  {15,-15,15,-15},
  {-15,-15,-15,-15},
  {-15,0,-15,0},
  {-15,15,-15,15},
  {0,15,0,15},
  {0,0,0,0}};
int rh[16][4]={
  {0,0,0,0},
  {0,-15,0,-20},
  {-15,-15,-10,-20},
  {-15,0,-10,0},
  {-15,20,-10,15},
  {0,20,-10,15},
  {0,0,-10,0},
  {0,-15,-10,-20},
  {-10,-15,-5,-20},
  {-10,0,5,0},
  {-10,20,-5,15},
  {0,20,-5,15},
  {0,0,-5,0},
  {0,-15,-5,-20},
  {0,-15,0,-20},
  {0,0,0,0}};
int lh[16][4]={
  {0,0,0,0},
  {0,20,0,15},
  {10,20,15,15},
  {10,0,15,0},
  {10,-15,15,-20},
  {10,-15,0,-20},
  {10,0,0,0},
  {10,20,0,15},
  {5,20,10,15},
  {5,0,10,0},
  {5,-15,10,-20},
  {5,-15,0,-20},
  {5,0,0,0},
  {5,20,0,15},
  {0,20,0,15},
  {0,0,0,0}};
int bw[13][4]={
  {0,0,0,0},
  {0,-15,0,-20},
  {15,-15,15,-20},
  {15,0,15,0},
  {15,25,15,15},
  {-15,25,-15,15},
  {-15,0,-15,0},
  {-15,-15,-15,-20},
  {15,-15,15,-20},
  {15,0,15,0},
  {15,25,15,15},
  {0,25,0,15},
  {0,0,0,0}};
int delection = 0;
int ts=300;
int td=10;
int a[4],b[4];
int n;

void setup() 
{ 
  myservo2.attach(2);
  myservo3.attach(3);
  myservo4.attach(4);
  myservo5.attach(5);
  Serial.begin(9600);
  
} 

void loop() 
{ 
  delection = Serial.read();
  switch (delection) {
    case 70: // F FWD
      Serial.println("FWD") ; 
      forward();
      walk(12);
      break;
    case 76: // L Left
      Serial.println("Left") ;
      left();
      walk(15);
      break;      
    case 82: // R Right
      Serial.println("Right") ;
      right();
      walk(15);
      break;
    case 66: // B back
      Serial.println("Back") ;
      back();
      walk(12);
      break;
    default: // S Stay
      Serial.println("Stay") ;
      stay();
      walk(1);
      break;
    
  }
  delay(100);
} 

void forward()
{
  for (int i=0; i <=12; i++){
    for (int j=0; j <=3; j++){
      ang[i][j] = fw[i][j];
    }
  }
}

void left()
{
  for (int i=0; i <=15; i++){
    for (int j=0; j <=3; j++){
      ang[i][j] = lh[i][j];
    }
  }
}

void right()
{
  for (int i=0; i <=15; i++){
    for (int j=0; j <=3; j++){
      ang[i][j] = rh[i][j];
    }
  }
}

void back()
{
  for (int i=0; i <=12; i++){
    for (int j=0; j <=3; j++){
      ang[i][j] = bw[i][j];
    }
  }
}

void stay()
{  
  for (int i=0; i <=1; i++){
    for (int j=0; j <=3; j++){
      ang[i][j] = stp[i][j];
    }
  }
}

void walk(int st) 
{ 
  for (int i=0; i <=st; i++){
    for (int j=0; j <=3 ; j++){
      if (i == st) n = st;
      else n = i+1;
      a[j] = ang[n][j] - ang[i][j];
      b[j] = ang[i][j];
    }
    for (int k=0; k <=td-1 ; k++){
        myservo2.write(ang0[0]+a[0]*k/td+b[0]);
        myservo3.write(ang0[1]+a[1]*k/td+b[1]);
        myservo4.write(ang0[2]+a[2]*k/td+b[2]);
        myservo5.write(ang0[3]+a[3]*k/td+b[3]);
        delay(ts/td);
    }
  }
}