四足机器人

nano

//  ----          ----
// | 00 |        | 03 |
//  ---- -------- ----
//      | 01  02 |
//      |        |
//      | 14  13 |  
//  ---- -------- ----
// | 15 |        | 12 |
//  ----          ----
//
#include <avr/pgmspace.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();


// MG90S servo PWM pulse traveling
const int PWMRES_Min = 1; // PWM Resolution 1
const int PWMRES_Max = 180; // PWM Resolution 180
const int SERVOMIN = 150;
const int SERVOMAX = 600;

// Servos matrix
const int ALLMATRIX = 9;
const int ALLSERVOS = 8; 

int servo_id;

// Servo delay base time
const int BASEDELAYTIME = 10; // 10 ms

// Backup servo value 存放各个舵机当前数值
int Running_Servo_POS [ALLMATRIX];

const int Servo_Map[ALLSERVOS] = {3, 2, 13, 12, 0, 1, 14, 15};

/*************************************    此处舵机归零位置微调，修改数组中相应数值  *************************/
//修正误差值
const int Servo_Bias_Correction[ALLSERVOS] = {-3, 0, 0 , 5 , 0, 5, -5, -10};
/******************************************************************************************************/

// Servo zero position 归零位置
// -----------------------------  ,  ms
const int Servo_Act_0 [ ] PROGMEM = {  135,  45, 135,  45,  45, 135,  45, 135,  500  };

// Start position 起始位置
// -----------------------------    ms
const int Servo_Act_1 [ ] PROGMEM = {  135,  45, 135,  45,  45, 135,  45, 135,  500  };

int Servo_array[11][ALLMATRIX];

// Standby 站立
const int Servo_Prg_1_Step = 2;
const int Servo_Prg_1 [][ALLMATRIX] PROGMEM = {
  //  ,  ms
  {   90,  90,  90,  90,  90,  90,  90,  90,  500  }, // servo center point
  {   70,  90,  90, 110, 110,  90,  90,  70,  500  }, // standby
};

// Forward 前行
const int Servo_Prg_2_Step = 11;
const int Servo_Prg_2[][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   90,  90,  90, 110, 110,  90,  45,  90,  200  }, // leg1,4 up; leg4 fw
  {   70,  90,  90, 110, 110,  90,  45,  70,  200  }, // leg1,4 dn
  {   70,  90,  90,  90,  90,  90,  45,  70,  200  }, // leg2,3 up
  {   70,  45, 135,  90,  90,  90,  90,  70,  200  }, // leg1,4 bk; leg2 fw
  {   70,  45, 135, 110, 110,  90,  90,  70,  200  }, // leg2,3 dn
  {   90,  90, 135, 110, 110,  90,  90,  90,  200  }, // leg1,4 up; leg1 fw
  {   90,  90,  90, 110, 110, 135,  90,  90,  200  }, // leg2,3 bk
  {   70,  90,  90, 110, 110, 135,  90,  70,  200  }, // leg1,4 dn
  {   70,  90,  90, 110,  90, 135,  90,  70,  200  }, // leg3 up
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // leg3 fw dn
};

// Backward 后退
const int Servo_Prg_3_Step = 11;
const int Servo_Prg_3 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   90,  45,  90, 110, 110,  90,  90,  90,  200  }, // leg4,1 up; leg1 fw
  {   70,  45,  90, 110, 110,  90,  90,  70,  200  }, // leg4,1 dn
  {   70,  45,  90,  90,  90,  90,  90,  70,  200  }, // leg3,2 up
  {   70,  90,  90,  90,  90, 135,  45,  70,  200  }, // leg4,1 bk; leg3 fw
  {   70,  90,  90, 110, 110, 135,  45,  70,  200  }, // leg3,2 dn
  {   90,  90,  90, 110, 110, 135,  90,  90,  200  }, // leg4,1 up; leg4 fw
  {   90,  90, 135, 110, 110,  90,  90,  90,  200  }, // leg3,1 bk
  {   70,  90, 135, 110, 110,  90,  90,  70,  200  }, // leg4,1 dn
  {   70,  90, 135,  90, 110,  90,  90,  70,  200  }, // leg2 up
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // leg2 fw dn
};

// Left shift 左移
const int Servo_Prg_4_Step = 11;
const int Servo_Prg_4 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   70,  90,  45,  90,  90,  90,  90,  70,  200  }, // leg3,2 up; leg2 fw
  {   70,  90,  45, 110, 110,  90,  90,  70,  200  }, // leg3,2 dn
  {   90,  90,  45, 110, 110,  90,  90,  90,  200  }, // leg1,4 up
  {   90, 135,  90, 110, 110,  45,  90,  90,  200  }, // leg3,2 bk; leg1 fw
  {   70, 135,  90, 110, 110,  45,  90,  70,  200  }, // leg1,4 dn
  {   70, 135,  90,  90,  90,  90,  90,  70,  200  }, // leg3,2 up; leg3 fw
  {   70,  90,  90,  90,  90,  90, 135,  70,  200  }, // leg1,4 bk
  {   70,  90,  90, 110, 110,  90, 135,  70,  200  }, // leg3,2 dn
  {   70,  90,  90, 110, 110,  90, 135,  90,  200  }, // leg4 up
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // leg4 fw dn
};

// Right shift 右移
const int Servo_Prg_5_Step = 11;
const int Servo_Prg_5 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   70,  90,  90,  90,  90,  45,  90,  70,  200  }, // leg2,3 up; leg3 fw
  {   70,  90,  90, 110, 110,  45,  90,  70,  200  }, // leg2,3 dn
  {   90,  90,  90, 110, 110,  45,  90,  90,  200  }, // leg4,1 up
  {   90,  90,  45, 110, 110,  90, 135,  90,  200  }, // leg2,3 bk; leg4 fw
  {   70,  90,  45, 110, 110,  90, 135,  70,  200  }, // leg4,1 dn
  {   70,  90,  90,  90,  90,  90, 135,  70,  200  }, // leg2,3 up; leg2 fw
  {   70, 135,  90,  90,  90,  90,  90,  70,  200  }, // leg4,1 bk
  {   70, 135,  90, 110, 110,  90,  90,  70,  200  }, // leg2,3 dn
  {   90, 135,  90, 110, 110,  90,  90,  70,  200  }, // leg1 up
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // leg1 fw dn
};

// Turn left 左转
const int Servo_Prg_6_Step = 8;
const int Servo_Prg_6 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   90,  90,  90, 110, 110,  90,  90,  90,  200  }, // leg1,4 up
  {   90, 135,  90, 110, 110,  90, 135,  90,  200  }, // leg1,4 turn
  {   70, 135,  90, 110, 110,  90, 135,  70,  200  }, // leg1,4 dn
  {   70, 135,  90,  90,  90,  90, 135,  70,  200  }, // leg2,3 up
  {   70, 135, 135,  90,  90, 135, 135,  70,  200  }, // leg2,3 turn
  {   70, 135, 135, 110, 110, 135, 135,  70,  200  }, // leg2,3 dn
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // leg1,2,3,4 turn
};

// Turn right 右转
const int Servo_Prg_7_Step = 8;
const int Servo_Prg_7 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   70,  90,  90,  90,  90,  90,  90,  70,  200  }, // leg2,3 up
  {   70,  90,  45,  90,  90,  45,  90,  70,  200  }, // leg2,3 turn
  {   70,  90,  45, 110, 110,  45,  90,  70,  200  }, // leg2,3 dn
  {   90,  90,  45, 110, 110,  45,  90,  90,  200  }, // leg1,4 up
  {   90,  45,  45, 110, 110,  45,  45,  90,  200  }, // leg1,4 turn
  {   70,  45,  45, 110, 110,  45,  45,  70,  200  }, // leg1,4 dn
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // leg1,2,3,4 turn
};

// Lie 趴地
const int Servo_Prg_8_Step = 1;
const int Servo_Prg_8 [][ALLMATRIX] PROGMEM = {
  // 15,   14,  13,  12,   0,   1,   2,  3,  ms
  {  110,  90,  90,  70,  70,  90,  90, 110,  500  }, // leg1,4 up
};

// Say Hi 打招呼
const int Servo_Prg_9_Step = 7;
const int Servo_Prg_9 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90,  90,  90,  90,  90,  90,  400}, // leg2,3,4 dn
  {  170,  90,  90,  90,  90,  90,  90,  90,  400}, // leg1 up
  {  170, 130,  90,  90,  90,  90,  90,  90,  400}, // leg1 left
  {  170,  50,  90,  90,  90,  90,  90,  90,  400}, // leg1 right
  {  170, 130,  90,  90,  90,  90,  90,  90,  400}, // leg1 left
  {  170,  90,  90,  90,  90,  90,  90,  90,  400}, // leg1 right
  {   70,  90,  90,  90,  90,  90,  90,  90,  400}, // leg1 dn
};

// Fighting 战斗
const int Servo_Prg_10_Step = 11;
const int Servo_Prg_10 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {  120,  90,  90, 110,  60,  90,  90,  70,  500  }, // leg1, 2 down
  {  120,  70,  70, 110,  60,  70,  70,  70,  500  }, // body turn left
  {  120, 110, 110, 110,  60, 110, 110,  70,  500  }, // body turn right
  {  120,  70,  70, 110,  60,  70,  70,  70,  500  }, // body turn left
  {  120, 110, 110, 110,  60, 110, 110,  70,  500  }, // body turn right
  {   70,  90,  90,  70, 110,  90,  90, 110,  500  }, // leg1, 2 up ; leg3, 4 down
  {   70,  70,  70,  70, 110,  70,  70, 110,  500  }, // body turn left
  {   70, 110, 110,  70, 110, 110, 110, 110,  500  }, // body turn right
  {   70,  70,  70,  70, 110,  70,  70, 110,  500  }, // body turn left
  {   70, 110, 110,  70, 110, 110, 110, 110,  500  }, // body turn right
  {   70,  90,  90,  70, 110,  90,  90, 110,  500  }  // leg1, 2 up ; leg3, 4 down
};

// Push up 俯卧撑
const int Servo_Prg_11_Step = 11;
const int Servo_Prg_11 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  500  }, // start
  {  100,  90,  90,  80,  80,  90,  90, 100,  600  }, // down
  {   70,  90,  90, 110, 110,  90,  90,  70,  700  }, // up
  {  100,  90,  90,  80,  80,  90,  90, 100,  800  }, // down
  {   70,  90,  90, 110, 110,  90,  90,  70,  900  }, // up
  {  100,  90,  90,  80,  80,  90,  90, 100, 1500  }, // down
  {   70,  90,  90, 110, 110,  90,  90,  70, 2000  }, // up
  {  135,  90,  90,  45,  45,  90,  90, 135,  200  }, // fast down
  {   70,  90,  90,  45,  60,  90,  90, 135,  800  }, // leg1 up
  {   70,  90,  90,  45, 110,  90,  90, 135,  800  }, // leg2 up
  {   70,  90,  90, 110, 110,  90,  90,  70,  800  }  // leg3, leg4 up
};

// Sleep 休眠
const int Servo_Prg_12_Step = 2;
const int Servo_Prg_12 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   30,  90,  90, 150, 150,  90,  90,  30,  500  }, // leg1,4 dn
  {   30,  45, 135, 150, 150, 135,  45,  30,  500  }, // protect myself
};

// 舞步 1
const int Servo_Prg_13_Step = 10;
const int Servo_Prg_13 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   90,  90,  90,  90,  90,  90,  90,  90,  400  }, // leg1,2,3,4 up
  {   50,  90,  90,  90,  90,  90,  90,  90,  400  }, // leg1 dn
  {   90,  90,  90, 130,  90,  90,  90,  90,  400  }, // leg1 up; leg2 dn
  {   90,  90,  90,  90,  90,  90,  90,  50,  400  }, // leg2 up; leg4 dn
  {   90,  90,  90,  90, 130,  90,  90,  90,  400  }, // leg4 up; leg3 dn
  {   50,  90,  90,  90,  90,  90,  90,  90,  400  }, // leg3 up; leg1 dn
  {   90,  90,  90, 130,  90,  90,  90,  90,  400  }, // leg1 up; leg2 dn
  {   90,  90,  90,  90,  90,  90,  90,  50,  400  }, // leg2 up; leg4 dn
  {   90,  90,  90,  90, 130,  90,  90,  90,  400  }, // leg4 up; leg3 dn
  {   90,  90,  90,  90,  90,  90,  90,  90,  400  }, // leg3 up
};

// 舞步 2
const int Servo_Prg_14_Step = 9;
const int Servo_Prg_14 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  45, 135, 110, 110, 135,  45,  70,  400  }, // leg1,2,3,4 two sides
  {  115,  45, 135,  65, 110, 135,  45,  70,  400  }, // leg1,2 up
  {   70,  45, 135, 110,  65, 135,  45, 115,  400  }, // leg1,2 dn; leg3,4 up
  {  115,  45, 135,  65, 110, 135,  45,  70,  400  }, // leg3,4 dn; leg1,2 up
  {   70,  45, 135, 110,  65, 135,  45, 115,  400  }, // leg1,2 dn; leg3,4 up
  {  115,  45, 135,  65, 110, 135,  45,  70,  400  }, // leg3,4 dn; leg1,2 up
  {   70,  45, 135, 110,  65, 135,  45, 115,  400  }, // leg1,2 dn; leg3,4 up
  {  115,  45, 135,  65, 110, 135,  45,  70,  400  }, // leg3,4 dn; leg1,2 up
  {   75,  45, 135, 105, 110, 135,  45,  70,  400  }, // leg1,2 dn
};

// 舞步 3
const int Servo_Prg_15_Step = 10;
const int Servo_Prg_15 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  45,  45, 110, 110, 135, 135,  70,  400  }, // leg1,2,3,4 bk
  {  110,  45,  45,  60,  70, 135, 135,  70,  400  }, // leg1,2,3 up
  {   70,  45,  45, 110, 110, 135, 135,  70,  400  }, // leg1,2,3 dn
  {  110,  45,  45, 110,  70, 135, 135, 120,  400  }, // leg1,3,4 up
  {   70,  45,  45, 110, 110, 135, 135,  70,  400  }, // leg1,3,4 dn
  {  110,  45,  45,  60,  70, 135, 135,  70,  400  }, // leg1,2,3 up
  {   70,  45,  45, 110, 110, 135, 135,  70,  400  }, // leg1,2,3 dn
  {  110,  45,  45, 110,  70, 135, 135, 120,  400  }, // leg1,3,4 up
  {   70,  45,  45, 110, 110, 135, 135,  70,  400  }, // leg1,3,4 dn
  {   70,  90,  90, 110, 110,  90,  90,  70,  400  }, // standby
};



void Servo_PROGRAM_Zero()
{
  // 备份舵机数值
  for (int Index = 0; Index < ALLMATRIX; Index++) {
    Running_Servo_POS[Index] = pgm_read_byte_near(&Servo_Act_0[Index]);
    Serial.print("Running_Servo_POS[Index]:");Serial.println(Running_Servo_POS[Index]);
  }

  // 载入舵机数值
  for (int iServo = 0; iServo < ALLSERVOS; iServo++) {
    Set_PWM_to_Servo(iServo, Running_Servo_POS[iServo]);
    delay(10);
  }
}

void Set_PWM_to_Servo(int iServo, int iValue)     //设置舵机数值
{
  // 读取 修正误差
  int NewPWM = iValue + Servo_Bias_Correction[iServo] ;
  NewPWM = map(NewPWM, PWMRES_Min, PWMRES_Max, SERVOMIN, SERVOMAX);      //将0-180是舵机相差值换算到 150-600
  pwm.setPWM(Servo_Map[iServo], 0, NewPWM);
}
void Servo_Play_run(int Array[][ALLMATRIX],int iSteps){     //计算舵机变动数值(含延时部分)
    
  int INT_TEMP_A, INT_TEMP_B,INT_TEMP_C, NewPWM ;
  for(int index1=0;index1<iSteps;index1++){
    int TotalTime = pgm_read_word_near(&Array[index1][ALLMATRIX-1]);
    int DelayTime = 4*TotalTime/BASEDELAYTIME;
    for(int timeloop = 1;timeloop<=DelayTime;timeloop++){     
      for(int index2=0;index2<ALLSERVOS;index2++){
        INT_TEMP_A = Running_Servo_POS[index2]; // 舵机目前数值
        INT_TEMP_B = pgm_read_word_near(&Array[index1][index2]); // 舵机目标数值

        //判断舵机数值是将变大还是变小
        if(INT_TEMP_A == INT_TEMP_B){  
          NewPWM = INT_TEMP_B;
          }else if(INT_TEMP_A > INT_TEMP_B){
            INT_TEMP_C = INT_TEMP_A - INT_TEMP_B;  //计算舵机目前数值与目标数值的相差值
            INT_TEMP_C = INT_TEMP_C * timeloop / DelayTime;  
            NewPWM = INT_TEMP_A - INT_TEMP_C;

            Set_PWM_to_Servo(index2, NewPWM);
            
            }else if(INT_TEMP_A < INT_TEMP_B){
              INT_TEMP_C = INT_TEMP_B - INT_TEMP_A;   //计算舵机目前数值与目标数值的相差值
              INT_TEMP_C = INT_TEMP_C * timeloop / DelayTime;
              NewPWM = INT_TEMP_A + INT_TEMP_C;
              
              Set_PWM_to_Servo(index2, NewPWM);
              
              }
      }
      }
    // 保存舵机目前数值
    for (int Index = 0; Index < ALLMATRIX; Index++) {
      Running_Servo_POS[Index] = pgm_read_word_near(&Array[index1][Index]);
    }
    }
  }
  
void setup(){
  pwm.begin();
  pwm.setPWMFreq(60); 
  delay(30);
  Serial.begin(9600);
  Servo_PROGRAM_Zero();   //舵机位置初始化
  }
void loop(){;
  if(Serial.available()>0){
    char data = Serial.read();
     if(data=='5'){
       Servo_Play_run(Servo_Prg_1, Servo_Prg_1_Step);
      }
      if(data=='8'){
        Servo_Play_run(Servo_Prg_2, Servo_Prg_2_Step);
      }
    if(data=='2'){
        Servo_Play_run(Servo_Prg_3, Servo_Prg_3_Step);
      }
    if(data=='4'){
        Servo_Play_run(Servo_Prg_4, Servo_Prg_4_Step);
      }
    if(data=='6'){
        Servo_Play_run(Servo_Prg_5, Servo_Prg_5_Step);
      }
    if(data=='7'){
        Servo_Play_run(Servo_Prg_6, Servo_Prg_6_Step);
      }
    if(data=='9'){
        Servo_Play_run(Servo_Prg_7, Servo_Prg_7_Step);
      }
    if(data=='a'){
        Servo_Play_run(Servo_Prg_8, Servo_Prg_8_Step);
      }
    if(data=='s'){
        Servo_Play_run(Servo_Prg_9, Servo_Prg_9_Step);
      }
    if(data=='d'){
        Servo_Play_run(Servo_Prg_10, Servo_Prg_10_Step);
      }
    if(data=='f'){
        Servo_Play_run(Servo_Prg_11, Servo_Prg_11_Step);
      }
    if(data=='g'){
        Servo_Play_run(Servo_Prg_12, Servo_Prg_12_Step);
      }
    if(data=='z'){
        Servo_Play_run(Servo_Prg_13, Servo_Prg_13_Step);
      }
    if(data=='x'){
        Servo_Play_run(Servo_Prg_14, Servo_Prg_14_Step);
      }
    if(data=='c'){
        Servo_Play_run(Servo_Prg_15, Servo_Prg_15_Step);
      }
    if(data=='0'){
      for (int Index = 0; Index < ALLMATRIX; Index++) {
        Running_Servo_POS[Index] = Servo_Act_0[Index];
      }
    }
    }
  }

转接板

Nano+PCA9685+蓝牙模块+电源转接板Nano版（含排线）

esp8266

//  ----          ----
// | 00 |        | 03 |
//  ---- -------- ----
//      | 01  02 |
//      |        |
//      | 14  13 |
//  ---- -------- ----
// | 15 |        | 12 |
//  ----          ----
//
// Q1 mini Quadruped Robot (Designed by Jason Workshop)
//
// Firmware created by MiniPlan
// FB Page: http://fb.com/miniPlanrobot
//
// Firmware modified by Jason Workshop
// Website: http://jasonworkshop.com
// FB page: http://fb.com/jasonworkshop
//
// 3D parts designed by Jason Workshop
// http://thingiverse.com/thing:2311678
//
// Related documents and software
// http://q1.jasonworkshop.com
//
// Last update: 22 Sep 2017
//

#include <Servo.h>
#include <EEPROM.h>
#include <ESP8266WiFi.h>
#include <ESP8266WebServer.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// Firmware version
String FW_Version = "Robot-J mini - Quadruped Robot v1.5 (Designed by August)";

// Servos matrix
const int ALLMATRIX = 9;
const int ALLSERVOS = 8; 

// MG90S servo PWM pulse traveling
const int PWMRES_Min = 1; // PWM Resolution 1
const int PWMRES_Max = 180; // PWM Resolution 180
const int SERVOMIN = 150;
const int SERVOMAX = 600;

int servo_id;

// Servo delay base time
const int BASEDELAYTIME = 10; // 10 ms

// AP password
const char WiFiAPPSK[] = "12345678";

// Motion data index
int Servo_PROGRAM;

// Backup servo value
int Running_Servo_POS [ALLMATRIX];

ESP8266WebServer server(80);

const int Servo_Map[ALLSERVOS] = {3, 2, 13, 12, 0, 1, 14, 15};
// Action
// --------------------------------------------------------------------------------

// Servo zero position 歸零位置
// -----------------------------  ,  ms
int Servo_Act_0 [ ] PROGMEM = {  135,  45, 135,  45,  45, 135,  45, 135,  500  };

// Start position 起始位置
// -----------------------------    ms
int Servo_Act_1 [ ] PROGMEM = {  135,  45, 135,  45,  45, 135,  45, 135,  500  };

// Standby 待機
int Servo_Prg_1_Step = 2;
int Servo_Prg_1 [][ALLMATRIX] PROGMEM = {
  //  ,  ms
  {   90,  90,  90,  90,  90,  90,  90,  90,  500  }, // servo center point
  {   70,  90,  90, 110, 110,  90,  90,  70,  500  }, // standby
};

// Forward 前行
int Servo_Prg_2_Step = 11;
int Servo_Prg_2 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   90,  90,  90, 110, 110,  90,  45,  90,  200  }, // leg1,4 up; leg4 fw
  {   70,  90,  90, 110, 110,  90,  45,  70,  200  }, // leg1,4 dn
  {   70,  90,  90,  90,  90,  90,  45,  70,  200  }, // leg2,3 up
  {   70,  45, 135,  90,  90,  90,  90,  70,  200  }, // leg1,4 bk; leg2 fw
  {   70,  45, 135, 110, 110,  90,  90,  70,  200  }, // leg2,3 dn
  {   90,  90, 135, 110, 110,  90,  90,  90,  200  }, // leg1,4 up; leg1 fw
  {   90,  90,  90, 110, 110, 135,  90,  90,  200  }, // leg2,3 bk
  {   70,  90,  90, 110, 110, 135,  90,  70,  200  }, // leg1,4 dn
  {   70,  90,  90, 110,  90, 135,  90,  70,  200  }, // leg3 up
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // leg3 fw dn
};

// Backward 退後
int Servo_Prg_3_Step = 11;
int Servo_Prg_3 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   90,  45,  90, 110, 110,  90,  90,  90,  200  }, // leg4,1 up; leg1 fw
  {   70,  45,  90, 110, 110,  90,  90,  70,  200  }, // leg4,1 dn
  {   70,  45,  90,  90,  90,  90,  90,  70,  200  }, // leg3,2 up
  {   70,  90,  90,  90,  90, 135,  45,  70,  200  }, // leg4,1 bk; leg3 fw
  {   70,  90,  90, 110, 110, 135,  45,  70,  200  }, // leg3,2 dn
  {   90,  90,  90, 110, 110, 135,  90,  90,  200  }, // leg4,1 up; leg4 fw
  {   90,  90, 135, 110, 110,  90,  90,  90,  200  }, // leg3,1 bk
  {   70,  90, 135, 110, 110,  90,  90,  70,  200  }, // leg4,1 dn
  {   70,  90, 135,  90, 110,  90,  90,  70,  200  }, // leg2 up
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // leg2 fw dn
};

// Left shift 左移
int Servo_Prg_4_Step = 11;
int Servo_Prg_4 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   70,  90,  45,  90,  90,  90,  90,  70,  200  }, // leg3,2 up; leg2 fw
  {   70,  90,  45, 110, 110,  90,  90,  70,  200  }, // leg3,2 dn
  {   90,  90,  45, 110, 110,  90,  90,  90,  200  }, // leg1,4 up
  {   90, 135,  90, 110, 110,  45,  90,  90,  200  }, // leg3,2 bk; leg1 fw
  {   70, 135,  90, 110, 110,  45,  90,  70,  200  }, // leg1,4 dn
  {   70, 135,  90,  90,  90,  90,  90,  70,  200  }, // leg3,2 up; leg3 fw
  {   70,  90,  90,  90,  90,  90, 135,  70,  200  }, // leg1,4 bk
  {   70,  90,  90, 110, 110,  90, 135,  70,  200  }, // leg3,2 dn
  {   70,  90,  90, 110, 110,  90, 135,  90,  200  }, // leg4 up
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // leg4 fw dn
};

// Right shift 右移
int Servo_Prg_5_Step = 11;
int Servo_Prg_5 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   70,  90,  90,  90,  90,  45,  90,  70,  200  }, // leg2,3 up; leg3 fw
  {   70,  90,  90, 110, 110,  45,  90,  70,  200  }, // leg2,3 dn
  {   90,  90,  90, 110, 110,  45,  90,  90,  200  }, // leg4,1 up
  {   90,  90,  45, 110, 110,  90, 135,  90,  200  }, // leg2,3 bk; leg4 fw
  {   70,  90,  45, 110, 110,  90, 135,  70,  200  }, // leg4,1 dn
  {   70,  90,  90,  90,  90,  90, 135,  70,  200  }, // leg2,3 up; leg2 fw
  {   70, 135,  90,  90,  90,  90,  90,  70,  200  }, // leg4,1 bk
  {   70, 135,  90, 110, 110,  90,  90,  70,  200  }, // leg2,3 dn
  {   90, 135,  90, 110, 110,  90,  90,  70,  200  }, // leg1 up
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // leg1 fw dn
};

// Turn left 左轉leg
int Servo_Prg_6_Step = 8;
int Servo_Prg_6 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   90,  90,  90, 110, 110,  90,  90,  90,  200  }, // leg1,4 up
  {   90, 135,  90, 110, 110,  90, 135,  90,  200  }, // leg1,4 turn
  {   70, 135,  90, 110, 110,  90, 135,  70,  200  }, // leg1,4 dn
  {   70, 135,  90,  90,  90,  90, 135,  70,  200  }, // leg2,3 up
  {   70, 135, 135,  90,  90, 135, 135,  70,  200  }, // leg2,3 turn
  {   70, 135, 135, 110, 110, 135, 135,  70,  200  }, // leg2,3 dn
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // leg1,2,3,4 turn
};

// Turn right 右轉
int Servo_Prg_7_Step = 8;
int Servo_Prg_7 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   70,  90,  90,  90,  90,  90,  90,  70,  200  }, // leg2,3 up
  {   70,  90,  45,  90,  90,  45,  90,  70,  200  }, // leg2,3 turn
  {   70,  90,  45, 110, 110,  45,  90,  70,  200  }, // leg2,3 dn
  {   90,  90,  45, 110, 110,  45,  90,  90,  200  }, // leg1,4 up
  {   90,  45,  45, 110, 110,  45,  45,  90,  200  }, // leg1,4 turn
  {   70,  45,  45, 110, 110,  45,  45,  70,  200  }, // leg1,4 dn
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // leg1,2,3,4 turn
};

// Lie 趴地
int Servo_Prg_8_Step = 1;
int Servo_Prg_8 [][ALLMATRIX] PROGMEM = {
  // 15,   14,  13,  12,   0,   1,   2,  3,  ms
  {  110,  90,  90,  70,  70,  90,  90, 110,  500  }, // leg1,4 up
};

// Say Hi 打招呼
int Servo_Prg_9_Step = 7;
int Servo_Prg_9 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90,  90,  90,  90,  90,  90,  400}, // leg2,3,4 dn
  {  170,  90,  90,  90,  90,  90,  90,  90,  400}, // leg1 up
  {  170, 130,  90,  90,  90,  90,  90,  90,  400}, // leg1 left
  {  170,  50,  90,  90,  90,  90,  90,  90,  400}, // leg1 right
  {  170, 130,  90,  90,  90,  90,  90,  90,  400}, // leg1 left
  {  170,  90,  90,  90,  90,  90,  90,  90,  400}, // leg1 right
  {   70,  90,  90,  90,  90,  90,  90,  90,  400}, // leg1 dn
};

// Fighting 戰鬥姿態
int Servo_Prg_10_Step = 11;
int Servo_Prg_10 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {  120,  90,  90, 110,  60,  90,  90,  70,  500  }, // leg1, 2 down
  {  120,  70,  70, 110,  60,  70,  70,  70,  500  }, // body turn left
  {  120, 110, 110, 110,  60, 110, 110,  70,  500  }, // body turn right
  {  120,  70,  70, 110,  60,  70,  70,  70,  500  }, // body turn left
  {  120, 110, 110, 110,  60, 110, 110,  70,  500  }, // body turn right
  {   70,  90,  90,  70, 110,  90,  90, 110,  500  }, // leg1, 2 up ; leg3, 4 down
  {   70,  70,  70,  70, 110,  70,  70, 110,  500  }, // body turn left
  {   70, 110, 110,  70, 110, 110, 110, 110,  500  }, // body turn right
  {   70,  70,  70,  70, 110,  70,  70, 110,  500  }, // body turn left
  {   70, 110, 110,  70, 110, 110, 110, 110,  500  }, // body turn right
  {   70,  90,  90,  70, 110,  90,  90, 110,  500  }  // leg1, 2 up ; leg3, 4 down
};

// Push up 掌上壓
int Servo_Prg_11_Step = 11;
int Servo_Prg_11 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  500  }, // start
  {  100,  90,  90,  80,  80,  90,  90, 100,  600  }, // down
  {   70,  90,  90, 110, 110,  90,  90,  70,  700  }, // up
  {  100,  90,  90,  80,  80,  90,  90, 100,  800  }, // down
  {   70,  90,  90, 110, 110,  90,  90,  70,  900  }, // up
  {  100,  90,  90,  80,  80,  90,  90, 100, 1500  }, // down
  {   70,  90,  90, 110, 110,  90,  90,  70, 2000  }, // up
  {  135,  90,  90,  45,  45,  90,  90, 135,  200  }, // fast down
  {   70,  90,  90,  45,  60,  90,  90, 135,  800  }, // leg1 up
  {   70,  90,  90,  45, 110,  90,  90, 135,  800  }, // leg2 up
  {   70,  90,  90, 110, 110,  90,  90,  70,  800  }  // leg3, leg4 up
};

// Sleep 睡眠姿勢
int Servo_Prg_12_Step = 2;
int Servo_Prg_12 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   30,  90,  90, 150, 150,  90,  90,  30,  500  }, // leg1,4 dn
  {   30,  45, 135, 150, 150, 135,  45,  30,  500  }, // protect myself
};

// 舞步 1
int Servo_Prg_13_Step = 10;
int Servo_Prg_13 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   90,  90,  90,  90,  90,  90,  90,  90,  400  }, // leg1,2,3,4 up
  {   50,  90,  90,  90,  90,  90,  90,  90,  400  }, // leg1 dn
  {   90,  90,  90, 130,  90,  90,  90,  90,  400  }, // leg1 up; leg2 dn
  {   90,  90,  90,  90,  90,  90,  90,  50,  400  }, // leg2 up; leg4 dn
  {   90,  90,  90,  90, 130,  90,  90,  90,  400  }, // leg4 up; leg3 dn
  {   50,  90,  90,  90,  90,  90,  90,  90,  400  }, // leg3 up; leg1 dn
  {   90,  90,  90, 130,  90,  90,  90,  90,  400  }, // leg1 up; leg2 dn
  {   90,  90,  90,  90,  90,  90,  90,  50,  400  }, // leg2 up; leg4 dn
  {   90,  90,  90,  90, 130,  90,  90,  90,  400  }, // leg4 up; leg3 dn
  {   90,  90,  90,  90,  90,  90,  90,  90,  400  }, // leg3 up
};

// 舞步 2
int Servo_Prg_14_Step = 9;
int Servo_Prg_14 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  45, 135, 110, 110, 135,  45,  70,  400  }, // leg1,2,3,4 two sides
  {  115,  45, 135,  65, 110, 135,  45,  70,  400  }, // leg1,2 up
  {   70,  45, 135, 110,  65, 135,  45, 115,  400  }, // leg1,2 dn; leg3,4 up
  {  115,  45, 135,  65, 110, 135,  45,  70,  400  }, // leg3,4 dn; leg1,2 up
  {   70,  45, 135, 110,  65, 135,  45, 115,  400  }, // leg1,2 dn; leg3,4 up
  {  115,  45, 135,  65, 110, 135,  45,  70,  400  }, // leg3,4 dn; leg1,2 up
  {   70,  45, 135, 110,  65, 135,  45, 115,  400  }, // leg1,2 dn; leg3,4 up
  {  115,  45, 135,  65, 110, 135,  45,  70,  400  }, // leg3,4 dn; leg1,2 up
  {   75,  45, 135, 105, 110, 135,  45,  70,  400  }, // leg1,2 dn
};

// 舞步 3
int Servo_Prg_15_Step = 10;
int Servo_Prg_15 [][ALLMATRIX] PROGMEM = {
  //  15,   14,  13,  12,   0,   1,   2,  3,  ms
  {   70,  45,  45, 110, 110, 135, 135,  70,  400  }, // leg1,2,3,4 bk
  {  110,  45,  45,  60,  70, 135, 135,  70,  400  }, // leg1,2,3 up
  {   70,  45,  45, 110, 110, 135, 135,  70,  400  }, // leg1,2,3 dn
  {  110,  45,  45, 110,  70, 135, 135, 120,  400  }, // leg1,3,4 up
  {   70,  45,  45, 110, 110, 135, 135,  70,  400  }, // leg1,3,4 dn
  {  110,  45,  45,  60,  70, 135, 135,  70,  400  }, // leg1,2,3 up
  {   70,  45,  45, 110, 110, 135, 135,  70,  400  }, // leg1,2,3 dn
  {  110,  45,  45, 110,  70, 135, 135, 120,  400  }, // leg1,3,4 up
  {   70,  45,  45, 110, 110, 135, 135,  70,  400  }, // leg1,3,4 dn
  {   70,  90,  90, 110, 110,  90,  90,  70,  400  }, // standby
};

// --------------------------------------------------------------------------------



// Servo
// --------------------------------------------------------------------------------

void Set_PWM_to_Servo(int iServo, int iValue)
{
  // 讀取 EEPROM 修正誤差
  int NewPWM = iValue + (int8_t)EEPROM.read(iServo);

  NewPWM = map(NewPWM, PWMRES_Min, PWMRES_Max, SERVOMIN, SERVOMAX);
  pwm.setPWM(Servo_Map[iServo], 0, NewPWM);
}

void Servo_PROGRAM_Zero()
{
  // 清除備份目前馬達數值
  for (int Index = 0; Index < ALLMATRIX; Index++) {
    Running_Servo_POS[Index] = Servo_Act_0[Index];
  }

  // 重新載入馬達預設數值
  for (int iServo = 0; iServo < ALLSERVOS; iServo++) {
    Set_PWM_to_Servo(iServo, Running_Servo_POS[iServo]);
    delay(10);
  }
}

void Servo_PROGRAM_Center()
{
  // 清除備份目前馬達數值
  for (int Index = 0; Index < ALLMATRIX; Index++) {
    Running_Servo_POS[Index] = Servo_Act_1[Index];
  }

  // 重新載入馬達預設數值
  for (int iServo = 0; iServo < ALLSERVOS; iServo++) {
    Set_PWM_to_Servo(iServo, Running_Servo_POS[iServo]);
    delay(10);
  }
}

void Servo_PROGRAM_Run(int iMatrix[][ALLMATRIX], int iSteps)
{
  int INT_TEMP_A, INT_TEMP_B, INT_TEMP_C;

  for (int MainLoopIndex = 0; MainLoopIndex < iSteps; MainLoopIndex++) { // iSteps 步驟主迴圈

    int InterTotalTime = iMatrix[MainLoopIndex][ALLMATRIX - 1]; // InterTotalTime 此步驟總時間

    int InterDelayCounter = InterTotalTime / BASEDELAYTIME; // InterDelayCounter 此步驟基本延遲次數

    for (int InterStepLoop = 0; InterStepLoop < InterDelayCounter; InterStepLoop++) { // 內差次數迴圈

      for (int ServoIndex = 0; ServoIndex < ALLSERVOS; ServoIndex++) { // 馬達主迴圈

        INT_TEMP_A = Running_Servo_POS[ServoIndex]; // 馬達現在位置
        INT_TEMP_B = iMatrix[MainLoopIndex][ServoIndex]; // 馬達目標位置

        if (INT_TEMP_A == INT_TEMP_B) { // 馬達數值不變
          INT_TEMP_C = INT_TEMP_B;
        } else if (INT_TEMP_A > INT_TEMP_B) { // 馬達數值減少
          INT_TEMP_C =  map(BASEDELAYTIME * InterStepLoop, 0, InterTotalTime, 0, INT_TEMP_A - INT_TEMP_B); // PWM內差值 = map(執行次數時間累加, 開始時間, 結束時間, 內差起始值, 內差最大值)
          if (INT_TEMP_A - INT_TEMP_C >= INT_TEMP_B) {
            Set_PWM_to_Servo(ServoIndex, INT_TEMP_A - INT_TEMP_C);
          }
        } else if (INT_TEMP_A < INT_TEMP_B) { // 馬達數值增加
          INT_TEMP_C =  map(BASEDELAYTIME * InterStepLoop, 0, InterTotalTime, 0, INT_TEMP_B - INT_TEMP_A); // PWM內差值 = map(執行次數時間累加, 開始時間, 結束時間, 內差起始值, 內差最大值)
          if (INT_TEMP_A + INT_TEMP_C <= INT_TEMP_B) {
            Set_PWM_to_Servo(ServoIndex, INT_TEMP_A + INT_TEMP_C);
          }
        }

      }

      delay(BASEDELAYTIME);
    }

    // 備份目前馬達數值
    for (int Index = 0; Index < ALLMATRIX; Index++) {
      Running_Servo_POS[Index] = iMatrix[MainLoopIndex][Index];
    }
  }
}

void writeKeyValue(int8_t key, int8_t value)
{
  EEPROM.write(key, value);
  EEPROM.commit();
}

int8_t readKeyValue(int8_t key)
{
  Serial.println("read");
  Serial.println(key);

  int8_t value = EEPROM.read(key);
}

// --------------------------------------------------------------------------------



// Handle
// --------------------------------------------------------------------------------

void handleAction(WiFiClient client, String req, HTTPMethod method)
{
  server.send(200, "text/plain", "Hello!");
}

void handleSave()
{
  String key = server.arg("key");
  String value = server.arg("value");

  int8_t keyInt = key.toInt();
  int8_t valueInt = value.toInt();

  delay(50);

  if (keyInt == 100) {
    writeKeyValue(0, 0);
    writeKeyValue(1, 0);
    writeKeyValue(2, 0);
    writeKeyValue(3, 0);
    writeKeyValue(4, 0);
    writeKeyValue(5, 0);
    writeKeyValue(6, 0);
    writeKeyValue(7, 0);
  } else {
    if (valueInt >= -124 && valueInt <= 124) { // 確認資料介於 -124 ~ 124
      writeKeyValue(keyInt, valueInt); // 儲存校正值
    }
  }
  delay(10);

  server.send(200, "text/html", "(key, value)=(" + key + "," + value + ")");
}

void handleController()
{
  String pm = server.arg("pm");
  String servo = server.arg("servo");

  if (pm != "") {
    Servo_PROGRAM = pm.toInt();
  }

  if (servo != "") {
    servo_id = servo.toInt();
    String ival = server.arg("value");
    Set_PWM_to_Servo(servo_id, ival.toInt());
  }

  server.send(200, "text/html", "(pm)=(" + pm + ")");
}

void handleOnLine()
{
  String m0 = server.arg("m0");
  String m1 = server.arg("m1");
  String m2 = server.arg("m2");
  String m3 = server.arg("m3");
  String m4 = server.arg("m4");
  String m5 = server.arg("m5");
  String m6 = server.arg("m6");
  String m7 = server.arg("m7");
  String t1 = server.arg("t1");

  int Servo_Prg_tmp [][ALLMATRIX] = {
    //        15,         14,         13,         12,          0,          1,          2,          3,      Run Time
    { m0.toInt(), m1.toInt(), m2.toInt(), m3.toInt(), m4.toInt(), m5.toInt(), m6.toInt(), m7.toInt(), t1.toInt() }
  };

  Servo_PROGRAM_Run(Servo_Prg_tmp, 1);

  server.send(200, "text/html", "(m0, m1)=(" + m0 + "," + m1 + ")");
}

// --------------------------------------------------------------------------------



// WebServer
// --------------------------------------------------------------------------------

void enableWebServer()
{
  HTTPMethod getMethod = HTTP_GET;

  server.on("/controller", getMethod, handleController);
  server.on("/save", getMethod, handleSave);
  server.on("/", getMethod, handleIndex);
  server.on("/editor", getMethod, handleEditor);
  server.on("/zero", getMethod, handleZero);
  server.on("/setting", getMethod, handleSetting);
  server.on("/online", getMethod, handleOnLine);

  server.begin();
}

// --------------------------------------------------------------------------------



// Setup
// --------------------------------------------------------------------------------

void setup(void)
{
  Serial.begin(115200);
  Serial.println("Robot-J mini Start!");

  // PWMServoDriver
  pwm.begin();
  pwm.setPWMFreq(60);   // servos run at 60Hz updates
  
  // AP SSID Name
  uint8_t mac[WL_MAC_ADDR_LENGTH];
  WiFi.softAPmacAddress(mac);
  String macID = String(mac[WL_MAC_ADDR_LENGTH - 2], HEX) + String(mac[WL_MAC_ADDR_LENGTH - 1], HEX);
  macID.toUpperCase();

  String AP_NameString = "Robot-J mini - " + macID;

  char AP_NameChar[AP_NameString.length() + 1];
  memset(AP_NameChar, 0, AP_NameString.length() + 1);

  for (int i = 0; i < AP_NameString.length(); i++)
    AP_NameChar[i] = AP_NameString.charAt(i);
  WiFi.mode(WIFI_AP);
  WiFi.softAP(AP_NameChar, WiFiAPPSK);
  IPAddress myIP = WiFi.softAPIP();

  // EEPROM
  EEPROM.begin(512);
  delay(10);

  // 清除備份目前馬達數值
  for (int Index = 0; Index < ALLMATRIX; Index++) {
    Running_Servo_POS[Index] = Servo_Act_0[Index];
  }

  // 自動歸零 增加組裝便利性
  Servo_PROGRAM_Zero();

  // 網頁生成
  enableWebServer();
  //Servo_PROGRAM = 101;
}

// --------------------------------------------------------------------------------



// Loop
// --------------------------------------------------------------------------------

void loop(void)
{
  // 網頁建構
  server.handleClient();

  if (Servo_PROGRAM >= 1 ) {
    switch (Servo_PROGRAM) {
      case 1: // Standby 待機
        Servo_PROGRAM_Run(Servo_Prg_1, Servo_Prg_1_Step);
        break;
      case 2: // Forward 前行
        Servo_PROGRAM_Run(Servo_Prg_2, Servo_Prg_2_Step);
        break;
      case 3: // Backward 退後
        Servo_PROGRAM_Run(Servo_Prg_3, Servo_Prg_3_Step);
        break;
      case 4: // Left shift 左移
        Servo_PROGRAM_Run(Servo_Prg_4, Servo_Prg_4_Step);
        break;
      case 5: // Right shift 右移
        Servo_PROGRAM_Run(Servo_Prg_5, Servo_Prg_5_Step);
        break;
      case 6: // Turn left 左轉
        Servo_PROGRAM_Run(Servo_Prg_6, Servo_Prg_6_Step);
        break;
      case 7: // Turn right 右轉
        Servo_PROGRAM_Run(Servo_Prg_7, Servo_Prg_7_Step);
        break;
      case 8: // Lie 趴地
        Servo_PROGRAM_Run(Servo_Prg_8, Servo_Prg_8_Step);
        break;
      case 9: // Say Hi 打招呼
        Servo_PROGRAM_Run(Servo_Prg_9, Servo_Prg_9_Step);
        Servo_PROGRAM_Run(Servo_Prg_1, Servo_Prg_1_Step);
        break;
      case 10: // Fighting 戰鬥姿態
        Servo_PROGRAM_Run(Servo_Prg_10, Servo_Prg_10_Step);
        break;
      case 11: // Push up 掌上壓
        Servo_PROGRAM_Run(Servo_Prg_11, Servo_Prg_11_Step);
        break;
      case 12: // Sleep 睡眠姿勢
        Servo_PROGRAM_Run(Servo_Prg_1, Servo_Prg_1_Step);
        Servo_PROGRAM_Run(Servo_Prg_12, Servo_Prg_12_Step);
        break;
      case 13: // 舞步 1
        Servo_PROGRAM_Run(Servo_Prg_13, Servo_Prg_13_Step);
        break;
      case 14: // 舞步 2
        Servo_PROGRAM_Run(Servo_Prg_14, Servo_Prg_14_Step);
        break;
      case 15: // 舞步 3
        Servo_PROGRAM_Run(Servo_Prg_15, Servo_Prg_15_Step);
        break;
      case 99: // 待機
        Servo_PROGRAM_Center();
        delay(300);
        break;
      case 100: // 歸零姿勢
        Servo_PROGRAM_Zero();
        delay(300);
      case 101:
        Servo_PROGRAM_Run(Servo_Prg_1, Servo_Prg_1_Step);
        Servo_PROGRAM_Center();
        delay(1000);
        Servo_PROGRAM_Run(Servo_Prg_2, Servo_Prg_2_Step);
        Servo_PROGRAM_Center();
        delay(1000);
        Servo_PROGRAM_Run(Servo_Prg_3, Servo_Prg_3_Step);
        Servo_PROGRAM_Center();
        delay(1000);        
        Servo_PROGRAM_Run(Servo_Prg_4, Servo_Prg_4_Step);
        Servo_PROGRAM_Center();
        delay(1000);    
        Servo_PROGRAM_Run(Servo_Prg_5, Servo_Prg_5_Step);
        Servo_PROGRAM_Center();
        delay(1000); 
        Servo_PROGRAM_Run(Servo_Prg_6, Servo_Prg_6_Step);
        Servo_PROGRAM_Center();
        delay(1000);    
        Servo_PROGRAM_Run(Servo_Prg_7, Servo_Prg_7_Step);
        Servo_PROGRAM_Center();
        delay(1000);   
        Servo_PROGRAM_Run(Servo_Prg_8, Servo_Prg_8_Step);
        Servo_PROGRAM_Center();
        delay(1000);         
        Servo_PROGRAM_Run(Servo_Prg_9, Servo_Prg_9_Step);
        Servo_PROGRAM_Run(Servo_Prg_1, Servo_Prg_1_Step); 
        Servo_PROGRAM_Center();
        delay(1000);     
        Servo_PROGRAM_Run(Servo_Prg_10, Servo_Prg_10_Step);
        Servo_PROGRAM_Center();
        delay(1000); 
                Servo_PROGRAM_Run(Servo_Prg_11, Servo_Prg_11_Step);
        Servo_PROGRAM_Center();
        delay(1000); 
                Servo_PROGRAM_Run(Servo_Prg_12, Servo_Prg_12_Step);
        Servo_PROGRAM_Center();
        delay(1000); 
                Servo_PROGRAM_Run(Servo_Prg_13, Servo_Prg_13_Step);
        Servo_PROGRAM_Center();
        delay(1000); 
                Servo_PROGRAM_Run(Servo_Prg_14, Servo_Prg_14_Step);
        Servo_PROGRAM_Center();
        delay(1000); 
                Servo_PROGRAM_Run(Servo_Prg_15, Servo_Prg_15_Step);
        Servo_PROGRAM_Center();
        delay(1000); 
        break;
    }
    Servo_PROGRAM = 0;
  }
}

// --------------------------------------------------------------------------------


// --------------------------------------------------------------------------------