demo——ver1

#include "background.h"
//  用宏定义定义舵机模式的初始状态以及角度转换率
#define DELTA1 550
#define OPP1 1
#define R1 145
#define DELTA2 490
#define OPP2 0
#define R2 160
#define DELTA3 450
#define OPP3 0
#define R3 160
#define DELTA4 140
#define OPP4 1
#define R4 155
#define DELTA9 510
#define OPP9 0
#define R9 140
#define DELTA10 540
#define OPP10 1
#define R10 140
#define DELTA11 400
#define OPP11 0
#define R11 150
#define DELTA12 520
#define OPP12 1
#define R12 170
#define DELTA14 420
#define OPP14 1
#define R14 150
#define DELTA13 490
#define OPP13 1
#define R13 180
//  用宏定义将舵机号与名字结合，方便修改
#define wheel_LB 6
#define wheel_LF 8
#define wheel_RB 5
#define wheel_RF 7
#define leg_R 1
#define leg_L 3
#define foot_R 2
#define foot_L 4
#define hand_L1 9
#define hand_L2 10
#define hand_L3 11
#define hand_R1 12
#define hand_R2 14
#define hand_R3 13
#define head 14
//  用宏定义将红外测距的id与名字结合，方便修改
#define distance_upL 2
#define distance_upC 0
#define distance_upR 1
#define distance_wheelLF 3
#define distance_wheelLB 5
#define distance_wheelRF 4
#define distance_wheelRB 6
#define distance_belowR 7
//  用宏定义将红外接近的id与名字结合，方便修改
#define approach_L1 1
#define approach_L2 2
#define approach_C 3
#define approach_R2 4
#define approach_R1 5

//  该函数用于标定舵机模式的舵机的位置
//  通过舵机旋转角度与位置值之间成比例的关系
//  通过简单地告诉函数要转多少角度，让其自动进行换算
//  每个舵机当手或脚平直的时候为0
void setservopos(int num,int pos,int speed)
{
 float position = (float)pos/45;
 if(num == 1)
     MFSetServoPos(1, OPP1?(DELTA1+position*R1):(DELTA1-position*R1),speed);
 if(num == 2)
     MFSetServoPos(2,  OPP2?(DELTA2+position*R2):(DELTA2-position*R2),speed);
 if(num == 3)
     MFSetServoPos(3,  OPP3?(DELTA3+position*R3):(DELTA3-position*R3),speed);
 if(num == 4)
     MFSetServoPos(4,  OPP4?(DELTA4+position*R4):(DELTA4-position*R4),speed);
 if(num == 12)
     MFSetServoPos(12, OPP12?(DELTA12+position*R12):(DELTA12-position*R12),speed);
 if(num == 13)
     MFSetServoPos(13, OPP13?(DELTA13+position*R13):(DELTA13-position*R13),speed);
 if(num == 14)
     MFSetServoPos(14,  OPP14?(DELTA14+position*R14):(DELTA14-position*R14),speed);
 if(num == 10)
     MFSetServoPos(10, OPP10?(DELTA10+position*R10):(DELTA10-position*R10),speed);
 if(num == 11)
     MFSetServoPos(11, OPP11?(DELTA11+position*R11):(DELTA11-position*R11),speed);
 if(num == 9)
     MFSetServoPos(9,  OPP9?(DELTA9+position*R12):(DELTA9-position*R9),speed);
}

/************************************ DEFINE ***************************************/

/************************************ LED灯测试 ************************************/
void LED(char dir) {
  int id1 = -1;
  int id2 = -1;
  if (dir == 'l') {id1 = 0; id2 = 0;}
  else if (dir == 'r') {id1 = 6; id2 = 6;}
  else if (dir == 'b') {id1 = 0; id2 = 6;}
  MFDigiOutput(id1, 0);
  MFDigiOutput(id2, 0);
  DelayMS(1000);
  MFDigiOutput(id1, 1);
  MFDigiOutput(id2, 1);
}
/************************************ LED灯测试 ************************************/

/************************************ 获取传感器参数 *******************************/

//   获取具体AD
int AD(int id) {
    return MFGetAD(id);
}

//   获取具体IO
int IO(int id) {
     return MFGetDigiInput(id);
}

//  获取中心坐标
int X() {
    return MFCapGetCenterX();
}
int Y() {
    return MFCapGetCenterY();
}

//  获取有效色数块
int color_block() {
  return MFCapGetSum();
}

/************************************ 获取传感器参数 *******************************/

/************************************ 软开关 *****************************************/
//  通过机器人胸前的红外测距传感器返回的数值来判断机器人是否被激活
void switch_open() {
  while (1) {
    if (AD(distance_upC) > 300 && (AD(distance_upR) > 300 || AD(distance_upL) > 300)) {
      break;
    }
  }
}
/************************************ 软开关 *****************************************/

/************************************ 底盘分解动作 *********************************/
//  停止动作
void stop() {
MFSetServoRotaSpd(wheel_RB, 0);
MFSetServoRotaSpd(wheel_RF, 0);
MFSetServoRotaSpd(wheel_LB, 0);
MFSetServoRotaSpd(wheel_LF, 0);
MFServoAction();
DelayMS(50);
}
//  双腿的动作
void set_posture(int leg, int spd_L, int foot, int spd_F, int delay) {
  setservopos(1, leg, spd_L);
setservopos(3, leg, spd_L);
setservopos(2, foot, spd_F);
setservopos(4, foot, spd_F);
MFServoAction();
DelayMS(delay);
}
//  向前走
void move_forward(int spd, int delay) {
MFSetServoRotaSpd(wheel_RB, -1 * spd);
MFSetServoRotaSpd(wheel_RF, -1 * spd);
MFSetServoRotaSpd(wheel_LB, spd);
MFSetServoRotaSpd(wheel_LF, spd);
MFServoAction();
DelayMS(delay);
}
//  向后走
void move_back(int spd, int delay) {
MFSetServoRotaSpd(wheel_RB, spd);
MFSetServoRotaSpd(wheel_RF, spd);
MFSetServoRotaSpd(wheel_LB, -1 * spd);
MFSetServoRotaSpd(wheel_LF, -1 * spd);
MFServoAction();
DelayMS(delay);
}
//  以1000的速度向左转
void turn_left_fast(int delay, int center) {
    int spd[10];
    int wheel_tmp = (wheel_LB == center ? wheel_LF : wheel_LB);
    for (int i = 0; i < 10; i++) {
      spd[i] = 1000;
    }
    spd[center] = 0;
    spd[wheel_tmp] = spd[wheel_tmp] / 5 * 4;
    MFSetServoRotaSpd(wheel_RB, -1 * spd[wheel_RB]);
    MFSetServoRotaSpd(wheel_RF, -1 * spd[wheel_RF]);
    MFSetServoRotaSpd(wheel_LB, -1 * spd[wheel_LB]);
    MFSetServoRotaSpd(wheel_LF, -1 * spd[wheel_LF]);
    MFServoAction();
    DelayMS(delay);
    stop();
}
// 以400的速度向左转, 用于调整位置
void turn_left_slowly(int delay) {
MFSetServoRotaSpd(wheel_LF, -400);
MFSetServoRotaSpd(wheel_LB, 0);
MFSetServoRotaSpd(wheel_RF, -500);
MFSetServoRotaSpd(wheel_RB, -500);
MFServoAction();
DelayMS(delay);
}
// 左转后用于复原
void turn_left_back(int delay) {
MFSetServoRotaSpd(wheel_LF, 0);
MFSetServoRotaSpd(wheel_LB, 400);
MFSetServoRotaSpd(wheel_RF, 500);
MFSetServoRotaSpd(wheel_RB, 500);
MFServoAction();
DelayMS(delay);
}
// 以1000的速度向右转
void turn_right_fast(int delay, int center) {
    int spd[10];
    int wheel_tmp = (wheel_RB == center ? wheel_RF : wheel_RB);
    for (int i = 0; i < 10; i++) {
      spd[i] = 1000;
    }
    spd[center] = 0;
    spd[wheel_tmp] = spd[wheel_tmp] / 5 * 4;
    MFSetServoRotaSpd(wheel_RB, spd[wheel_RB]);
    MFSetServoRotaSpd(wheel_RF, spd[wheel_RF]);
    MFSetServoRotaSpd(wheel_LB, spd[wheel_LB]);
    MFSetServoRotaSpd(wheel_LF, spd[wheel_LF]);
    MFServoAction();
    DelayMS(delay);
    stop();
}
// 以500的速度向右转, 用于调整位置
void turn_right_slowly(int delay) {
MFSetServoRotaSpd(wheel_LF, 500);
MFSetServoRotaSpd(wheel_LB, 500);
MFSetServoRotaSpd(wheel_RF, 400);
MFSetServoRotaSpd(wheel_RB, 0);
MFServoAction();
DelayMS(delay);
}
// 以500的速度向右转, 用于调整位置
void turn_right_slowly_2(int delay) {
MFSetServoRotaSpd(wheel_LF, 400);
MFSetServoRotaSpd(wheel_LB, 400);
MFSetServoRotaSpd(wheel_RF, 300);
MFSetServoRotaSpd(wheel_RB, 0);
MFServoAction();
DelayMS(delay);
}
// 右转后用于复原
void turn_right_back(int delay) {
MFSetServoRotaSpd(wheel_LF, -500);
MFSetServoRotaSpd(wheel_LB, -500);
MFSetServoRotaSpd(wheel_RF, 0);
MFSetServoRotaSpd(wheel_RB, -400);
MFServoAction();
DelayMS(delay);
}
//  原地旋转
void spin(char dir, int speed, int delay) {
  int spd[10];
  int dir_ = 1;
  if (dir == 'l') dir_ = -1;
  for (int i = wheel_RB; i <= wheel_LF; i++) {
    spd[i] = dir_ * speed;
  }
  MFSetServoRotaSpd(wheel_LF, spd[wheel_LF]);
MFSetServoRotaSpd(wheel_LB, spd[wheel_LB]);
MFSetServoRotaSpd(wheel_RF, spd[wheel_RF]);
MFSetServoRotaSpd(wheel_RB, spd[wheel_RB]);
MFServoAction();
DelayMS(delay);
}
//  后退并调整机器人与后边界的相对位置
//  保证机器人是与后边界垂直的
void move_back_adjust() {
     while (1) {
          move_back(300, 10);
      if (AD(distance_wheelLB) < 165 && AD(distance_wheelRB) >= 165) {
               while (1) {
                     MFSetServoRotaSpd(wheel_LF,500);
                 MFSetServoRotaSpd(wheel_LB, 0);
                 MFSetServoRotaSpd(wheel_RF, 400);
                 MFSetServoRotaSpd(wheel_RB, 400);
                     MFServoAction();
           	         DelayMS(10);
                     if (AD(5) < 165 && AD(6) < 165) {
                          stop();
                          MFSetServoRotaSpd(wheel_LF, -400);
                      MFSetServoRotaSpd(wheel_LB, 0);
                      MFSetServoRotaSpd(wheel_RF, -500);
                      MFSetServoRotaSpd(wheel_RB, -500);
                     	  MFServoAction();
           	              DelayMS(100);
                          stop();
                          break;
                     }
               }
               break;
          }
          else if (AD(distance_wheelRB) < 165 && AD(distance_wheelLB) >= 165) {
               while (1) {
                     MFSetServoRotaSpd(wheel_LF,-400);
                 MFSetServoRotaSpd(wheel_LB, -400);
                 MFSetServoRotaSpd(wheel_RF, -500);
                 MFSetServoRotaSpd(wheel_RB, 0);
                     MFServoAction();
           	         DelayMS(10);
                     if (AD(5) < 165 && AD(6) < 165) {
                          MFSetServoRotaSpd(wheel_LF, -400);
                      MFSetServoRotaSpd(wheel_LB, -400);
                      MFSetServoRotaSpd(wheel_RF, -500);
                      MFSetServoRotaSpd(wheel_RB, 0);
                     	  MFServoAction();
           	              DelayMS(0);
                          //stop();
                          break;
                     }
               }
               break;
          }
          else if (AD(5) < 165 && AD(6) < 165) {
               stop();
               break;
          }
    }
    stop();
    return;
}
//  当robot与目标对准后调整robot与目标之间的距离
void distance(int d) {
  int d1 = 0;
  int d2 = 0;
  d1 = d - 3;
  d2 = d + 3;
  int count = 0;
  while (1) {
      count += 1;
      if ((AD(0) > d1 && AD(0) < d2) || count == 5) {
        stop();
        break;
      }
      if (AD(0) > d) {
        while (1) {
          if (AD(0) <= d) {
            stop();
            break;
          }
          move_back(250, 10);
        }
      } else if (AD(0) < d) {
        while (1) {
          if (AD(0) >= d) {
            stop();
            break;
          }
          move_forward(250, 10);
        }
      }
    }
    return;
}
//  通过方向来改变头上舵机的角度
void turn_head(char dir) {
  if (dir == 'm') {
    MFSetServoPos(15, 556, 500);
MFServoAction();
DelayMS(800);
  } else if (dir == 'r') {
    MFSetServoPos(15, 237, 500);
MFServoAction();
DelayMS(800);
  } else if (dir == 'l') {
    MFSetServoPos(15, 849, 500);
MFServoAction();
DelayMS(800);
  }
}

void camera(int Hmax, int Hmin, int Smax, int Smin) {
  MFCapSetH(Hmax,Hmin);
  MFCapSetS(Smax,Smin);
  move_forward(1023, 4000);

  stop();
  turn_head('r');
  set_posture(-40, 50, 45, 50, 500);

  while (1) {
    if (X() > 160) LED('l');
    if (color_block() > 500) LED('r');
    if (X() > 160 && color_block() > 500) {
      LED('b');
      stop();
      break;
    } else {
      move_forward(400, 50);
    }
  }

  stop();
  turn_head('m');

  while (1) {
    if (X() < 350) LED('l');
    else if (color_block() > 200) LED('r');
    if (X() < 350 && color_block() > 200) {
      LED('b');
      stop();
      break;
    } else {
      turn_right_slowly_2(100);
    }
  }

  stop();
}
/************************************ 底盘分解动作 *********************************/

/************************************ 手部分解动作 *********************************/

//  Get_Ready 准备动作
void Get_Ready() {
     turn_head('m');
     //   身体向前倾斜
     setservopos(leg_R,-20,50);
     setservopos(foot_R,25,50);
     setservopos(leg_L,-20,50);
     setservopos(foot_L,25,50);
     //   手背在后面
     setservopos(hand_R1,90,150);
     setservopos(hand_R2,80,100);
     setservopos(hand_R3,0,200);
     setservopos(hand_L1,130,300);
     setservopos(hand_L2,80,100);
     setservopos(hand_L3,0,200);
     MFServoAction();
     DelayMS(1500);
}

//  step1_挥手
void step1_huishou() {
    //   右手伸直
    {
       setservopos(14,0,300);
       setservopos(12,0,300);
       setservopos(13,0,300);
       MFServoAction();
       DelayMS(1000);
    }
    MFMp3Play((char*)&("录音.mp3"),8);
    //   挥手
    int i = 0;
    for (i = 0; i < 4; i++)
    {
       {
           setservopos(14,30,500);
           MFServoAction();
           DelayMS(100);
           setservopos(13,15,500);
           MFServoAction();
           DelayMS(1200);
       }
       {
           setservopos(13,-15,500);
           MFServoAction();
           DelayMS(100);
           setservopos(14,-30,500);
           MFServoAction();
           DelayMS(1200);
       }
    }
Get_Ready();
}

//  step2_抱球
void step2_baoqiu_1() {
    //   手臂转到前面
    {
         //   身体向后倾斜
         setservopos(1,10,50);
         setservopos(2,-10,50);
         setservopos(3,10,50);
         setservopos(4,-10,50);
         //  第二个关节外翻以防打到柱子
         setservopos(12,-90,200);
         setservopos(9,-120,180);
         setservopos(10,-40,150);
         setservopos(14,-40,150);
         MFServoAction();
         DelayMS(6000);
    }
    //   抱住柱子
    {
         setservopos(10,22,200);
         setservopos(14,20,200);
         MFServoAction();
         DelayMS(1500);
    }
    //   手臂上抬
    {
         setservopos(11,10,100);
         setservopos(13,10,100);
         setservopos(12,-50,300);
         setservopos(9,-57,300);
         MFServoAction();
         DelayMS(2000);
    }
    return;
}

void step2_baoqiu_2() {
    setservopos(10,-10,200);
    setservopos(14,-10,200);
    MFServoAction();
    DelayMS(800);
    Get_Ready();
}

//  推柱子
void step3_tuizhuzi() {
    //   手臂转到前面同时第二个关节向外翻，以防打到柱子 && 身体往后倾斜
    {
    	setservopos(12,-90,200);
    	setservopos(9,-120,180);
    	setservopos(10,-40,150);
    	setservopos(14,-40,150);

      setservopos(1,10,50);
    	setservopos(2,-15,50);
    	setservopos(3,10,50);
    	setservopos(4,-15,50);
    	MFServoAction();
    	DelayMS(4000);
    }
    //   抱着柱子
    {
        setservopos(10,25,150);
        setservopos(14,-5,150);
        setservopos(11,-65,150);
        setservopos(13,-45,150);
        MFServoAction();
        DelayMS(2000);
    }
}

//   松手函数
void step3_tuizhuzi_songshou() {
    {
        setservopos(10,0,150);
    setservopos(14,-30,150);
    //move_forward_veryfast();   //push zhu zi
        MFServoAction();
        DelayMS(800);
  }
}

//  最后推柱子一下的动作
void step3_tuizhuzi_push() {
  move_back(1000, 800);
  stop();

  setservopos(9, -110, 150);
  setservopos(12, -90, 200);
  setservopos(10,35,150);
  setservopos(14,0,150);
  MFServoAction();
  DelayMS(1500);

  while (1) {
    move_forward(500, 10);
    if (AD(distance_wheelLF) < 100) {
      stop();
      break;
    }
  }
  stop();
  setservopos(9, 0, 200);
  setservopos(12, 0, 200);
  MFServoAction();
  DelayMS(1500);
}
//  step4_敲锣
void step4_qiaoluo() {
    //  避免打到头
    {
        setservopos(10, 0, 200);
        setservopos(14, 0, 200);
        setservopos(9, 0, 150);
        setservopos(12, 0, 150);
        MFServoAction();
        DelayMS(1500);
    }
//   两手向前倾斜&&身体回复直立
    {
    	setservopos(1, -58, 125);
    setservopos(3, -58, 125);
      setservopos(2, 45, 100);
    setservopos(4, 45, 100);
  // setservopos(14,0,100);
    	// setservopos(10,0,100);
  MFServoAction();
    	DelayMS(1000);


  setservopos(14,75,100);
    	setservopos(10,80,100);
    	setservopos(12,-110,300);
    	setservopos(13,0,200);
    	setservopos(9,-90,150);
    	setservopos(11,0,200);
    	MFServoAction();
    	DelayMS(4000);
    }
    //   两手向外打，敲击锣
    {
        int i = 0;
    	for (i = 0; i < 3; i++) {
{
       	 setservopos(13,-75,800);
          setservopos(11,-75,800);
          setservopos(10,45,800);
          setservopos(14,45,800);
       	 MFServoAction();
       	 DelayMS(800);

       	 setservopos(13,0,800);
    setservopos(11,0,800);
    setservopos(10,80,800);
          setservopos(14,75,800);
       	 MFServoAction();
       	 DelayMS(800);
}
    	}
    }
    //  避免打到头
    {
        setservopos(10, 0, 200);
        setservopos(14, 0, 200);
        setservopos(9, 0, 150);
        setservopos(12, 0, 150);
        MFServoAction();
        DelayMS(1500);
    }
}

//   step5_dance
void step5_dance() {
    //  手臂举起——standby
    {
        // setservopos(10,0,150);
        setservopos(14,0,150);
        MFServoAction();
        DelayMS(500);

        setservopos(12,0,150);
        // setservopos(9,0,150);
        MFServoAction();
        DelayMS(1500);
    }
    //  两手外张
    {
        // setservopos(10,-90,100);
        // setservopos(14,-90,100);
        setservopos(12, -100, 150);
        setservopos(14, 80, 150);
        MFServoAction();
        DelayMS(2000);
    }
    //  鞠躬
    {
        set_posture(0, 100, 25, 100, 1000);
    }
    //  避免打到头
    {
        setservopos(14, 0, 200);
        setservopos(12, 0, 150);
        MFServoAction();
        DelayMS(1500);
    }
    Get_Ready();
}
/************************************ 手部分解动作 *********************************/

/***************************** sensor && feedback *************************/

//   feedback函数
void feedback(int* result) {
int delay = 0;

  if (result[0] > 0) delay = result[0] * 16;
  else delay = result[0] * (-1) * 18;

if (result[0] > 0) {
turn_left_back(delay);
stop();
} else if (result[0] < 0) {
turn_right_back(delay);
stop();
}
  result[0] = 0;
  return;
}

//   sensor函数
void sensor(int step, int delay[]) {
    int d = 0;
    if (step == 1) d = 250;
    else if (step == 2) d = 210;
    else if (step == 3) d = 280;

if (IO(approach_L1) + IO(approach_L2) != 2) {
            while (1) {
              turn_left_slowly(10);
      delay[0] = delay[0] + 1;
              if (IO(approach_C) == 0 && (IO(approach_R1) + IO(approach_R2)+ IO(approach_L2) + IO(approach_L1) == 4)) {
                stop();
      while (1) {
      move_forward(300, 10);
                  if (AD(distance_upC) > d || AD(distance_upR) > d - 8|| AD(distance_upL) > d - 8) {
                    stop();
                    break;
                  }
      }
                break;
              }
            }
      } else if (IO(approach_R2) + IO(approach_R1) != 2) {
            while (1) {
              turn_right_slowly(10);
      delay[0] = delay[0] - 1;
              if (IO(approach_C) == 0 && (IO(approach_R1) + IO(approach_R2) + IO(approach_L1) + IO(approach_L2) == 4)) {
                stop();
                while (1) {
      move_forward(300, 10);
                  if (AD(distance_upC) > d || AD(distance_upR) > d - 8|| AD(distance_upL) > d - 8) {
                    stop();
                    break;
                  }
      }
                break;
              }
            }
      } else if (IO(approach_C) == 0 && IO(approach_R1) + IO(approach_R2) + IO(approach_L1) + IO(approach_L2) == 4) {
      stop();
      delay[0] = delay[0];
      while (1) {
    	  move_forward(300, 10);
              if (AD(distance_upC) > d || AD(distance_upL) > d - 8 || AD(distance_upR) > d - 8) {
        stop();
        break;
              }
      }
      }
      return;
}

//   用于跳出外部循环的sensor_2
int sensor_2(int step) {
     int d = 0;
     if (step == 1) d = 250;
     else if (step == 2) d = 210;
     else if (step == 3) d = 280;
     if (IO(approach_C) == 0 &&
        (IO(approach_R2) + IO(approach_R1) + IO(approach_L2) + IO(approach_L1) == 4) &&
        (AD(distance_upC) > d || AD(distance_upR) > d - 8 || AD(distance_upL) > d) ) return 1;
     return 0;
}

/***************************** sensor && feedback *************************/
//  全局变量
int delay_flag[1] = {0};
/***************************** 分部流程 ***********************************/

//   step1
void step1() {
Get_Ready();
  set_posture(-45, 125, 45, 100, 2000);
move_forward(1023, 6500);
stop();
DelayMS(100);

  set_posture(15, 125, 0, 100, 1000);
  turn_right_fast(2000, wheel_RB);//turn right 90 degree

move_back_adjust();
  stop();
  set_posture(-25, 125, 25, 100, 500);

while(1)                //sensor
{
move_forward(1000, 10);
sensor(1, delay_flag);
if(sensor_2(1) == 1)
{
stop();
break;
}
  }
  step1_huishou();
feedback(delay_flag);
}

//   step2
void step2() {
 	move_back(1000, 2400);   //go back
stop();

  turn_left_fast(2450, wheel_LB);

while (1) {
move_forward(1000, 10);   //go forward
sensor(2, delay_flag);
if(sensor_2(2) == 1)
{
stop();
break;
}
}

  distance(205);
  stop();

step2_baoqiu_1();    //get the ball
  feedback(delay_flag);
  while (1) {
    move_back(1000, 10);
    if (AD(distance_wheelRB) < 165 || AD(distance_wheelLB) < 165) {
      stop();
      break;
    }
  }
  stop();

  move_forward(500, 300);
  stop();
  spin('r', 1000, 1700);
  stop();
  move_forward(500, 500);
  stop();
  step2_baoqiu_2();
  spin('r', 1000, 3300);
  stop();

  // move_forward(1000, 1000);
  // stop();

move_back_adjust();
  stop();

  // step2_baoqiu_2();

while (1) {
    sensor(3, delay_flag);
if(sensor_2(3) == 1) {
stop();
  break;
}
    move_forward(1000, 10);
  }
  stop();

  step3_tuizhuzi();   //tui zhu zi
  feedback(delay_flag);

move_forward(1000, 3500);   //push zhu zi

step3_tuizhuzi_songshou();    //song shou
  while (1) {
    move_forward(500, 10);
    if (AD(distance_wheelLF) < 100) {
      stop();
      break;
    }
  }

  step3_tuizhuzi_push();

Get_Ready();
}

//   step3
void step3() {
//spin 180 degree
  spin('r', 1000, 3200);

  move_forward(1000, 1000);
  move_back_adjust();
  stop();

  move_forward(1000, 1900);

  // turn to left 90 degree
turn_left_fast(2450, wheel_LB);
  stop();

  while (1) {
    if (AD(distance_upC) > 165 && (AD(distance_upR) > 160 || AD(distance_upL) > 160)) {
      stop();
      break;
    }
  move_forward(1000, 10);
  }
stop();

step4_qiaoluo();

  Get_Ready();
}

//   step4
void step4() {
   move_back(1000, 3300);
   stop();

   turn_right_fast(2450, wheel_RB);

   move_forward(1000, 2100);
   stop();

   turn_left_fast(2400, wheel_LB);

   step5_dance();

}

//    step5
void step5() {
     int delay = 0;
     while (1) {
         move_back(1000, 10);
         if (AD(distance_wheelRB) < 165 || AD(distance_wheelLB) < 165) {
             stop();
             break;
         }
     }

     move_forward(1000, 1000);
     move_back_adjust();
     stop();

     //   turn_right_90degree_5hao();
     turn_right_fast(2300, wheel_RB);

     while (1) {
         move_forward(700, 10);
         if (AD(distance_belowR) < 110) {
            delay = 0;
            int r = 0;
            while (1) {
              MFSetServoRotaSpd(8, -400);
            MFSetServoRotaSpd(6, 0);
            MFSetServoRotaSpd(7, -500);
            MFSetServoRotaSpd(5, -500);
              MFServoAction();
              DelayMS(10);
              r = r + 1;

              if (AD(distance_belowR) >= 110) {
                delay = r * 15;
                MFSetServoRotaSpd(8, 0);
              MFSetServoRotaSpd(6, 400);
              MFSetServoRotaSpd(7, 500);
              MFSetServoRotaSpd(5, 500);
              MFServoAction();
                DelayMS(delay);
                stop();
                break;
              }
            }
            //  continue;
         }
         //if (io[1] + io[2] + io[3] + io[4] + io[5] != 5) {
         if (IO(approach_R1) + IO(approach_R2) + IO(approach_L1) + IO(approach_L2) != 4) {
            stop();
            Get_Ready();
            break;
         }

         if ((AD(distance_wheelRB) + AD(distance_wheelLB) - AD(distance_wheelRF) - AD(distance_wheelLF)) >= 220) {
           setservopos(1, 30, 125);    //make yourself lower
         setservopos(3, 30, 125);
         setservopos(2, -25, 100);
         setservopos(4, -25, 100);
         MFServoAction();
         DelayMS(1000);
         }
     }
     return;
}

/***************************** 分部流程 ***********************************/

/************************************ 主函数 ***************************************/
int main(int argc, char * argv[])
{
    MFCapOpen();
    MFInit();
    MFSetPortDirect(0x00000041);
    MFADEnable(0);
    MFADEnable(1);
    MFADEnable(2);
    MFADEnable(3);
    MFADEnable(4);
    MFADEnable(5);
    MFADEnable(6);
    MFADEnable(7);
    MFDigiInit(100);
    DelayMS(100);
    MFADInit(100);
    MFSetServoMode(1,0);
    MFSetServoMode(2,0);
    MFSetServoMode(3,0);
    MFSetServoMode(4,0);
    MFSetServoMode(5,1);
    MFSetServoMode(6,1);
    MFSetServoMode(7,1);
    MFSetServoMode(8,1);
    MFSetServoMode(9,0);
    MFSetServoMode(10,0);
    MFSetServoMode(11,0);
    MFSetServoMode(12,0);
    MFSetServoMode(13,0);
    MFSetServoMode(14,0);
    MFSetServoMode(15,0);
    MFDigiOutput(0, 1);
    MFDigiOutput(6, 1);

    Get_Ready();
    while (1) {
      switch_open();
/*
      step1();
      step2();
      step3();
      step4();
      step5();
*/
      camera(180, 116, 120, 70);
    }
    return 0;
}
/************************************ 主函数 ***************************************/