Gesture Controlled Robotic Arm

#include <Servo.h>
#include "FRobot.h"

//UART send 1~9==>20~180 degree

int val, val2, temp;
void Execute(char c, int v1);
void Execute(int v[]);
void motion(unsigned char m[][4], int steps);

unsigned char home_pos[1][4] = { 45, 45, 45, 45 };
unsigned char hello_dance[6][4] = { {45, 0, 0, 80}, {30, 0, 0, 80},
  {90, 10, 10, 10}, {30, 10, 10, 80},
  {30, 45, 90, 80}, {45, 45, 45, 45}
};
unsigned char pickup_dance[][4] = { {110, 95, 140, 80}, {110, 95, 140, 80}, {110, 95, 140, 30},
  {110, 70, 90, 30}, {180, 70, 90, 30},
  {180, 95, 90, 30}, {180, 95, 150, 30},
  {180, 100, 150, 80}, {180, 70, 90, 80}, {45, 45, 45, 45}
};
FRobot_Arm arm;
void setup()
{
  int i;
  pinMode(13, OUTPUT);
  Serial.begin(9600);//设置波特率为9600
  Serial.println("servo=o_seral_simple ready" ) ;
  arm.attach(10, 9, 8, 7);
  arm.home(45, 45, 45, 45);
  Serial.println("home");
}
int input_cnt = 0;
char command;
const int Servo_Limit[4][2] = { { 0, 180}, {45, 150}, {0, 180}, {10, 80}};
void loop()
{
  int val[4], i;
  char c;
  
  if (!Serial.available())
    return;
 // Serial.print("input cnt "); Serial.println(input_cnt, DEC);
  if (input_cnt > 2) {
    input_cnt = 0;
    val[0] = Serial.parseInt();
    constrain(val[0], Servo_Limit[0][0], Servo_Limit[0][1]);
    if (command == 'x') {
      for (i = 1; i < 4; i++)  {
        val[i] = Serial.parseInt();
        constrain(val[i], Servo_Limit[i][0], Servo_Limit[i][1]);
      }
      arm.Execute(val);
    } else
      Execute(command, val[0]);
    return;
  }
  c = Serial.read();
  input_cnt ++;
  switch (input_cnt) {
    case 1:
      if (c != 'f')  input_cnt = 0;
      break;
    case 2:
      if (c != 'x') input_cnt = 0;
      break;
    case 3:
      command = c;
  //    Serial.print("command = ");
   //   Serial.println(command, DEC);
      break;
  }
}
void Execute(char c, int v1)
{
  char buf[64];
  switch (c) {
    case 'S': case 's':
      sprintf(buf, "A: %d B: %d C: %d D: %d, ",
              arm.read(0), arm.read(1), arm.read(2), arm.read(3));
      Serial.println(buf);
      break;
    case 'b': case 'c': case 'd': case'a':
      sprintf(buf, "move %c to %d", c, v1);
      Serial.println(buf);
      while (!arm.move_step(c - 97, v1));            break; //a ASCII = 97

    case 'T':
    case 't':
      arm.set_delay(v1);
      break;
    case 'p':
      if (v1 == 1) {
        arm.motion(home_pos, 1);
      } else if (v1 == 2) {
        arm.motion(pickup_dance, 10);
      } else if (v1 == 3) arm.motion(hello_dance, 6);
      break;
    case 'L':
      if (v1 > 0)
        digitalWrite(13, HIGH);
      else
        digitalWrite(13, LOW);
      break;
  }

 // Serial.print(c, DEC);
 // Serial.print("  v1="); Serial.println(v1, DEC);
}


//#define DEBUG_MOVE_STEP
class FRobot {
  public:
    FRobot() {
      
      delay_time = 20;
    }
    void home() {}
    
    int read(int i) {
      return servo[i].read();
    }
    void set_delay(int t) {
      delay_time = t;
    }
    int move_step(int which, int target) {
      int pos;
      pos = servo[which].read();
      #ifdef DEBUG_MOVE_STEP
      Serial.print(which, DEC); Serial.print(" Target"); Serial.print(target,DEC);
      Serial.print("cur pos"); Serial.println(pos,DEC);
      #endif
      if (pos < target)
        servo[which].write(pos + 1);
      else if (pos > target)
        servo[which].write(pos - 1);
      else if (pos == target)
        return 1;
      return 0;
    }
    void motion(unsigned char m[][4], int steps)
    {
      int i, j, ret = 0;;
      for (i = 0; i < steps; i++) {
        Serial.print("motion: ");
        Serial.println(i, DEC);
        do {
          ret = 0;
          for (j = 0; j < 4; j++)  ret += move_step(j, m[i][j]);
          delay(delay_time);
        } while (ret < 4);
      }
      Serial.println("motion end");
    }
    void Execute(int val[])
    {
      int j, ret;
      do {
        ret = 0;
        for (j = 0; j < 4; j++)  ret += move_step(j, val[j]);
        delay(delay_time);
      } while (ret < 4);
      Serial.print("move to "); Serial.print(val[0]); Serial.print(val[1]); Serial.print(val[2]); Serial.println(val[3]);
    }

  public:
    Servo *servo;
    int delay_time;
};

class FRobot_Arm : public FRobot {
  public:
    FRobot_Arm(): FRobot() {
      servo = s;
      
    }
    void attach(int p1, int p2, int p3, int p4){
      servo[0].attach(p1);
      servo[1].attach(p2);
      servo[2].attach(p3);
      servo[3].attach(p4);
    }
    void home(int a, int b, int c, int d) {
      servo[0].write(a);
      servo[1].write(b);
      servo[2].write(c);
      servo[3].write(d);
    }
  private:
    Servo s[4];

};