增加主动接球动作，并增加5.5g金属舵机前档step模型文件

v1.4/platformio/src/SerialParser.cpp

@@ -21,7 +21,7 @@ void serialReceiveProcess() {
       } else if (strstr(serial_buf, "DX:") != NULL && strstr(serial_buf, "DY:") != NULL) {
         parseCameraDeviationCommand(serial_buf); // 处理摄像头偏差控制指令 DX:90,DY:45;
       } else if (buf_idx > 0) {
-        Serial.println("指令格式错误！请使用正确格式");
+        // Serial.println("指令格式错误！请使用正确格式");
       }
       buf_idx = 0;                 // 重置缓冲区索引
     } else if (buf_idx < 31) {     // 防止缓冲区溢出
@@ -64,7 +64,7 @@ void parseCameraDeviationCommand(char* cmd) {
   int dy = atoi(dyPtr + 3);  // "DY:"占3个字符，跳过它们取数值
 
   // 控制摄像头上下左右
-  setCameraMoveAngles(dx, dy);
+  kick_ball_main(dx, dy);
 
 }
 
@@ -128,12 +128,30 @@ void parseSingleParam(char* cmd) {
     pid_trace_joyx.D = param_val;
 
   } else if (strcmp(param_name, "pid_trace_joyy_p") == 0) {
-    pid_trace_joyy_p = param_val;
+    pid_trace_joyy.P = param_val;
   } else if (strcmp(param_name, "pid_trace_joyy_d") == 0) {
-    pid_trace_joyy_d = param_val;
+    pid_trace_joyy.D = param_val;
+  
+
+  } else if (strcmp(param_name, "pid_cam_p") == 0) {
+    pid_cam_p = param_val;
+  } else if (strcmp(param_name, "pid_cam_d") == 0) {
+    pid_cam_d = param_val;
+
+  } else if (strcmp(param_name, "chase_ball_s2") == 0) {
+    chase_ball_s2 = param_val;
+  } else if (strcmp(param_name, "ob_ball_dy") == 0) {
+    ob_ball_dy = param_val;
+
+  } else if (strcmp(param_name, "kick_ball_dy") == 0) {
+    kick_ball_dy = param_val;
+
+  } else if (strcmp(param_name, "kick_over_count_max") == 0) {
+    kick_over_count_max = param_val;
+
+  } else if (strcmp(param_name, "leg_height_base") == 0) {
+    leg_height_base = param_val;
 
-  } else if (strcmp(param_name, "kick_time_limit") == 0) {
-    kick_time_limit = param_val;
 
   } else if (strcmp(param_name, "pitch_adjust") == 0) {
     pitch_adjust = param_val;

v1.4/platformio/src/SerialParser.h

@@ -15,10 +15,17 @@ extern float dragon_joyx_factor;
 
 
 extern PIDController pid_trace_joyx;
-extern float pid_trace_joyy_p;
+extern PIDController pid_trace_joyy;
-extern float pid_trace_joyy_d;
+extern float pid_cam_p;
+extern float pid_cam_d;
 
-extern int kick_time_limit;
+extern int chase_ball_s2; 
+extern int ob_ball_dy; 
+extern int kick_ball_dy; 
+extern int kick_over_count_max; 
+
+
+extern float leg_height_base; 
 
 extern float pitch_adjust;
 
@@ -52,9 +59,25 @@ void parseSingleParam(char* cmd);
 void parseServoAngleCommand(char* cmd);
 void parseCameraDeviationCommand(char* cmd);
 
-void setServoAngles(int s1, int s2);
+// 踢球相关函数声明
-void setCameraMoveAngles(int dx, int dy);
+
+void kick_ball_loop();
+void kick_ball_main(int dx, int dy);
+
+void chase_ball(int dy);
+void kick_ball(int dy);
+void aim_ob ( int dx, int dy, String ob);
+void aim_ball(int dx, int dy);
+void aim_face ( int dx, int dy);
+
+
 void setCarmeraFixServoAngles(int servo_angle);
 void setFrontierServoAngles(int servo_angle);
 
+void setServoAngles(int s1, int s2);
+
+void cancel_trace_mode();
+
+void SerialToMaixcam(String cat, String str);
+
 #endif // SERIAL_PARSER_H

v1.4/platformio/src/main.cpp

@@ -25,7 +25,7 @@ void yaw_loop();
 void leg_loop();
 void jump_loop();
 
-void cancel_trace_mode();
+
 
 // 跨越障碍函数
 void obstacle_detect(); // 越障高度检测
@@ -125,7 +125,7 @@ float roll_adjust = 0.0f;
 float original_roll_adjust = roll_adjust;  
 
 // 机身高度
-float leg_height_base = 20.0f;  // 改为全局变量，初始值20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
+float leg_height_base = 15.0f;  // 改为全局变量，初始值20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
 float original_leg_height_base = leg_height_base ; 
 
 #define SERVO0_MIN  2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机
@@ -276,23 +276,35 @@ Servo cam_fix_servo;
 
 // 追踪模式
 bool trace_mode = false; // 追踪模式整体标志
-bool trace_stand_mode = false; // 原地站立追踪模式
+bool kick_place_mode = false; // 原地踢球模式 aim_ball kick_ball
+bool kick_run_mode = false; // 追逐踢球模式 aim_ball chase_ball aim_face kick_ball
+String kick_status = ""; // aim_ball aim_face aim_ball_over aim_face_over kick_ball_over
 bool trace_ob_flag = false; // 是否追踪到位
-int joyx_servo = 9999;
+
+int dx_to_joyx = 9999;
+int dy_to_joyy = 9999;
+int ob_y = 0; // 物体离图像下边缘的距离
 
 PIDController pid_trace_joyx(0.5, 0, 0, 100000, 100);
+PIDController pid_trace_joyy(0.2, 0, 0, 100000, 100);
+
+int chase_ball_s2 = 20;
+int ob_ball_dy = 100;
+int kick_ball_dy = 60;
+int kick_over_count = 0; // 运行踢球后技术
+int kick_over_count_max = 150;
 
 // 摄像头舵机
-int current_s2 = 90;       // 记录舵机最新角度，初始为中位
+int current_s1 = 0;       // 前档舵机实时角度
+int current_s2 = 90;       // 摄像头舵机实时角度
+
 // 摄像头舵机 PD 控制器参数
-float pid_trace_joyy_p = 0.05f;   // 比例系数（P） 0.07f
+float pid_cam_p = 0.07f;   // 比例系数（P） 0.07f
-float pid_trace_joyy_d = 0.01f;    // 微分系数（D），可根据实际调试调整
+float pid_cam_d = 0.05f;    // 微分系数（D），可根据实际调试调整
 int pid_trace_joyy_output_limit = 10; // PD输出限幅（单次最大调整角度）
 int last_dy = 0;                   // 上一次的y轴偏差
 unsigned long last_dy_time = 0;    // 上一次计算偏差的时间戳
 
-unsigned long kick_time = 0; // 记录开始踢球的瞬间
-int kick_time_limit = 10; // 100ms 内，不允许再踢球
 
 void setup()
 {
@@ -419,6 +431,9 @@ void loop()
   
   runActionSequence();    // 执行动作序列 
 
+  kick_ball_loop(); // 踢球控制循环
+
+
 
   // 记录上一次的遥控数据数据
   wrobot.joyx_last = wrobot.joyx;
@@ -609,6 +624,15 @@ void lqr_balance_loop()
   
   gyro_control = pid_gyro(LQR_gyro);
  
+
+  if(kick_run_mode){
+    if(dy_to_joyy!= 9999){
+      wrobot.joyy = dy_to_joyy;
+    }else{
+      wrobot.joyy = 0;
+    }   
+  }
+
   // 前进后退跳跃的系数都不一样
   float speed_target_coeff = 0.1; 
   if( jump_flag ){ // 跳跃
@@ -962,8 +986,8 @@ void yaw_loop()
   }
 
   if(trace_mode){
-    if(joyx_servo!= 9999){
+    if(dx_to_joyx!= 9999){
-      wrobot.joyx = joyx_servo;
+      wrobot.joyx = dx_to_joyx;
     }else{
       wrobot.joyx = 0;
     }   
@@ -1168,14 +1192,14 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
     }
 
     // 截图键 + 向上键
-    if(remoteData.btnShare && remoteData.btnDirUp && !last_btnDirUp) { 
+    if(remoteData.btnShare && remoteData.btnDirUp && last_btnDirUp) { 
-      int new_s2 = current_s2 + 5; // ±1°调整 测试
+      int new_s2 = current_s2 + 1; // ±1°调整 测试
       setCarmeraFixServoAngles(new_s2);  // 摄像头舵机上下运动
     }
 
     // 截图键 + 向下键
-    if(remoteData.btnShare && remoteData.btnDirDown && !last_btnDirDown) { 
+    if(remoteData.btnShare && remoteData.btnDirDown && last_btnDirDown) { 
-      int new_s2 = current_s2 - 5; // ±1°调整 测试
+      int new_s2 = current_s2 - 1; // ±1°调整 测试
       setCarmeraFixServoAngles(new_s2);  // 摄像头舵机上下运动
     }
 
@@ -1194,13 +1218,12 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
     last_btnDirLeft = remoteData.btnDirLeft;
 
 
-    // 还原
+    // 还原姿态
     if (remoteData.btnLS) {
       roll_adjust = original_roll_adjust; 
       pitch_adjust = 0.0f;
       leg_height_base = original_leg_height_base;
       pitch_zeropoint = original_pitch_zeropoint;
-      setCarmeraFixServoAngles(90);// 摄像头舵机还原
     }
 
     //  右扳机 高度向下 ：原始0-1023
@@ -1214,13 +1237,17 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
     startColorSequenceBlink();
   }
 
-  // 控制前档舵机，追踪模式下不可用，因为是自动控制
+  // 左扳机 控制前档舵机，追踪模式下不可用，因为是自动控制
   if(remoteData.trigLT && !trace_mode){
     int servo_angle = map(remoteData.trigLT, 0, 1023, 0, 180);
     setFrontierServoAngles(servo_angle);
   }
+  // 左扳机 强制回到零点
+  if(!remoteData.trigLT && !trace_mode){
+    setFrontierServoAngles(0);
+  }
 
-  // 跳跃 右摇杆按键
+  // 右摇杆按键 跳跃 
   if (remoteData.btnRS && !last_btnRS  && robot_enabled) { // 1=true，0=false
     wrobot.jump = 1; // 跳跃
     place_jump_flag = 1; // 跳跃
@@ -1229,7 +1256,6 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
   }
   last_btnRS = remoteData.btnRS; 
 
-
   // 向左跳跃
   if (remoteData.btnX && !last_btnX  && robot_enabled) { 
     wrobot.jump = 1;
@@ -1244,18 +1270,27 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
   }
   last_btnB = remoteData.btnB; 
 
-  // 向前跳跃
+  // 向前跳跃（非踢球模式）
-  if (remoteData.btnY && !last_btnY  && robot_enabled) { 
+  if (remoteData.btnY && !last_btnY  && robot_enabled && !trace_mode) { 
     wrobot.jump = 1;
     forward_jump_flag = 1;
   }
   last_btnY = remoteData.btnY; 
 
-  // 向后跳跃
+  // 向后跳跃（非踢球模式）
-  if (remoteData.btnA && !last_btnA  && robot_enabled) { 
+  if (remoteData.btnA && !last_btnA  && robot_enabled && !trace_mode) { 
     wrobot.jump = 1;
     back_jump_flag = 1;
   }
+  // 追逐踢球模式        @@@@@@
+  if (remoteData.btnA && !last_btnA  && robot_enabled && trace_mode) { 
+    kick_run_mode = !kick_run_mode;
+    if(kick_run_mode){
+      kick_place_mode = false; // 原地踢球模式
+    }else{
+      kick_place_mode = true; // 原地踢球模式
+    }
+  }
   last_btnA = remoteData.btnA; 
 
   // trigRT 根据扳机键按压力度调整振动力度
@@ -1276,46 +1311,19 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
   // 越障模式进入过程取消
   if (trigRT == 0 && last_trigRT > 0) {
     obstacle_cancel(); 
-    setLEDColor(CRGB::Black);
   }
   last_trigRT = trigRT;
 
-  //  追踪模式 rgb：蓝色
+  //  原地踢球模式
   if (remoteData.btnSelect && !last_btnSelect && !remoteData.btnShare) { 
     trace_mode = !trace_mode;
     if(trace_mode){
-        setLEDColor(CRGB::Blue);
+      kick_place_mode = true; // 原地踢球模式
-        setCarmeraFixServoAngles(120);  // 摄像头舵机上下运动
+      kick_run_mode = false; // 运动踢球模式
     }else {
       cancel_trace_mode();
     }
   }
-  // 已经追踪到物体 rgb：绿色
-  if (trace_mode) {
-    if(trace_ob_flag){ 
-        setLEDColor(CRGB::Green); 
-      }else{
-        setLEDColor(CRGB::Blue);
-    }
-  }
-
-  //  原地站立追踪模式 rgb：蓝色
-  // if (remoteData.btnSelect && !last_btnSelect && remoteData.btnShare) { 
-  //     trace_stand_mode = !trace_stand_mode;
-  //     if (trace_stand_mode) {
-  //       setLEDColor(CRGB::Blue);
-  //     }else{
-  //       cancel_trace_mode();
-  //     }
-  // }
-  // // 已经追踪到物体 rgb：绿色
-  // if (trace_stand_mode) {
-  //   if(trace_ob_flag){ 
-  //       setLEDColor(CRGB::Green); 
-  //     }else{
-  //       setLEDColor(CRGB::Blue);
-  //   }
-  // }
   
   // 舵机中位校准 中间十字左右键同时按下
   if (calibrationOfs_flag == 0 && remoteData.btnSelect && last_btnSelect && remoteData.btnStart && last_btnStart) { 
@@ -1519,79 +1527,174 @@ void obstacle_loop()
         sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
       }
   }
-
-  // 舵机收缩完毕
-  // if( obstacle_status == "motor_go" &&  obstacle_motor_go_count > 10 
-  //     && !sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){ 
-  //   obstacle_cancel();
-  //   resetZeroPoint();   
-  //   startLEDBlink(CRGB::Green, 100,  2 );
-  // }
 }
 
-
 void obstacle_cancel()
 {
     obstacle_btn_down = 0;
     obstacle_status = "";
-    
     obstacle_enconter_count = 0;
-
     obstacle_motor_go_count = 0;
-
     obstacle_pitch_adjust_count = 0;
-
     pitch_adjust = 0.0f; //负数前倾，正数后倾
     obstacle_mode = false; // 跳出越障模式
+
+    setLEDColor(CRGB::Black);
+}
+
+void kick_ball_loop(){
+  if(trace_mode){
+      if (kick_run_mode) { // 运动追踪模式
+        setLEDColor(CRGB::Pink); 
+      }else{
+        setLEDColor(CRGB::Blue); 
+      }
+  }
+
+  // 动态调整高度
+  if(wrobot.joyy == 0 ){
+      if(current_s1 > 90){
+        leg_height_base = 30.0f;
+      }else if(current_s1 > 10){
+        leg_height_base = constrain(current_s1, 15.0f, 30.0f);
+      }else{
+        leg_height_base = original_leg_height_base;
+      }
+  }
+
+    // if(kick_run_mode && last_kick_run_mode != kick_run_mode){
+    //   SerialToMaixcam("kick_mode", "kick_run_mode");
+    // }
+    // if(!kick_run_mode && last_kick_run_mode != kick_run_mode){
+    //   SerialToMaixcam("kick_mode", "kick_place_mode");
+    // }
+    // last_kick_run_mode = kick_run_mode;
+
+    // 在运动踢球时，踢好球，需要有一段停止前进时间
+    if(kick_over_count > 0 && kick_over_count < kick_over_count_max){
+      kick_over_count ++;
+    }else{
+      kick_over_count = 0;
+    }
+
 }
 
 // 通过偏差deviation 控制摄像头上下左右运动
-void setCameraMoveAngles(int dx, int dy) {
+void kick_ball_main(int dx, int dy) {
   
-
+  // 不是追踪模式
-  if(!trace_mode or dx == 9999 or dy == 9999){
+  if(!trace_mode){
-    joyx_servo = 9999;
+    dx_to_joyx = 9999; // 不左右旋转
+    dy_to_joyy = 9999; // 不前后追逐球
     trace_ob_flag = false;
-    // 重置PD控制器状态
+    last_dy = 0;  // 重置相机舵机PID控制器状态
-    last_dy = 0;
     last_dy_time = 0;
+
+    return;
+  }
+  // 追踪模式，找不到目标, 略微低头
+  if( trace_mode  &&  (dx == 9999 or dy == 9999) ){
+    dx_to_joyx = 9999; // 不左右旋转
+    dy_to_joyy = 9999; // 不前后追逐球
+    trace_ob_flag = false;
+    last_dy = 0;  // 重置相机舵机PID控制器状态
+    last_dy_time = 0;
+
+    setCarmeraFixServoAngles(60); 
+
     return;
   }
   
-  // 判断小车正面是否朝向物体，只需要一个区间就可以，不用必须到图像中心
+ 
-  if( abs(dx) < 40){
+  // 原地踢球模式
+  if(kick_place_mode){
+    aim_ball(dx,dy);
+    kick_ball(dy);
+  }
+  
+  // 运动踢球模式
+  if(kick_run_mode){
+    aim_ball(dx,dy);
+    
+    if(kick_over_count){ // 踢好球之后，需要向后推
+      dy_to_joyy = -50;
+    }else{
+      chase_ball(dy);
+    }
+   
+    kick_ball(dy);
+  }
+
+}
+
+// 追逐球
+void chase_ball(int dy){
+    // 物体在上方 dy趋近-100
+    // 物体在下方 dy趋近 100
+    // 中间 dy=0
+    // 球已在脚下,停止向前运动
+    // Serial.println("current_s2: " + String(current_s2) + " dy: " + String(dy));
+  //  if(current_s2 < 20 && dy > 50 && dy < 120 && trace_ob_flag){ 
+   if(current_s2 < chase_ball_s2 && dy > ob_ball_dy && dy < 120 && trace_ob_flag){ 
+      dy_to_joyy = 9999;
+      kick_status = "chase_ball_over";
+      SerialToMaixcam("kick_status", kick_status);
+    }else{ // 通过pid 向前运动
+      ob_y = ob_ball_dy - dy; 
+      dy_to_joyy = pid_trace_joyy(ob_y);
+      kick_status = "chase_ball";
+      SerialToMaixcam("kick_status", kick_status);
+    }
+}
+
+// 踢球 同时控制机身高度
+void kick_ball(int dy){
+  // if(current_s2 < 20 && dy > 20 && dy < 120){ // 球到车身下方了，打球
+  if(current_s2 < 10 && dy > kick_ball_dy && dy < 120){ // 球到车身下方了，打球
+      setFrontierServoAngles(0); // 踢球
+      leg_height_base = original_leg_height_base; // 上升
+      kick_status = "kick_ball_over";
+      SerialToMaixcam("kick_status", kick_status);
+
+      kick_over_count = 1; 
+
+    }else { // 追踪到目标
+      setFrontierServoAngles(160); // 打开挡板，收到后方
+     leg_height_base = 30.0f; // 下降
+    }
+}
+
+
+void aim_ob ( int dx, int dy, String ob){
+  //----- 控制左右 使小车正面是否朝向物体--------
+  // 物体在左方 dx趋近-150
+  // 物体在右方 dx趋近 150
+  // 中间 dx=0
+  if( abs(dx) < 50){ // 只需要一个区间
     trace_ob_flag = true;
-    joyx_servo = 9999;
+    dx_to_joyx = 9999; // 已经追踪到了目标，不需要再左右调整旋转
-    pitch_adjust = 0.0f;
+    pitch_adjust = 0.0f; // debug
   }else{
     trace_ob_flag = false;
   }
 
-  // 100ms 内，不允许再踢球,摄像头强行朝上
+    // 左右旋转
-  if(kick_time > 0 && kick_time <= kick_time_limit){ 
+  if(!trace_ob_flag){ 
-    kick_time ++;
+
-    setCarmeraFixServoAngles(130); 
+    int joyx_derection = (dx > 0) ? -1 : 1;
-    return;
+    dx_to_joyx = pid_trace_joyx(abs(dx)) * joyx_derection;
-  }
+    kick_status =  "aim_" + ob;
-  if(kick_time > kick_time_limit){ // 超过限制归零，准备下次跟踪踢球
+    SerialToMaixcam("kick_status", "aim_" + ob);
-    kick_time = 0;
+    
+  }else{ // 已经正面朝向物体
+    kick_status = "aim_" + ob + "_over";
+    SerialToMaixcam("kick_status", "aim_" + ob + "_over");
   }
 
-
+  //----- 控制上下 相机舵机控制-----------
-  // 正面追踪到物体，打开挡板，下降高度
+  // 物体在上方 dy趋近-100
-  if(current_s2 < 60 && dy > -20 ){ // 球到车身下方了，打球
+  // 物体在下方 dy趋近 100
-    setFrontierServoAngles(0); // 打球，挡板回到前方
+  // 中间 dy = 0
-    leg_height_base = original_leg_height_base; // 上升
-    kick_time = 1;
-
-  }else if(trace_ob_flag){ // 追踪到目标
-    setFrontierServoAngles(180); // 打开挡板，收到后方
-    leg_height_base = 30.0f; // 下降
-  }else{
-    leg_height_base = original_leg_height_base; // 原来高度
-  }
-
-  // 上下旋转舵机 - PD控制实现
   if(abs(dy) > 10){ // 偏差超过阈值才进行调整
     unsigned long current_time = millis();
     // 首次执行初始化时间戳
@@ -1602,8 +1705,8 @@ void setCameraMoveAngles(int dx, int dy) {
     
     // 计算new_s2
     unsigned long dt = max((unsigned long)1, current_time - last_dy_time);
-    float d_term = pid_trace_joyy_d * (dy - last_dy) / (float)dt * 10;
+    float d_term = pid_cam_d * (dy - last_dy) / (float)dt * 10;
-    float p_term = pid_trace_joyy_p * dy;
+    float p_term = pid_cam_p * dy;
     float pd_output = p_term + d_term;
     pd_output = constrain(pd_output, -pid_trace_joyy_output_limit, pid_trace_joyy_output_limit);
     int new_s2 = current_s2 - (int)pd_output;
@@ -1620,43 +1723,54 @@ void setCameraMoveAngles(int dx, int dy) {
     last_dy = 0;
   }
 
-  // 左右旋转，foc电机
-  if(!trace_ob_flag){ 
-    int joyx_derection = (dx > 0) ? -1 : 1;
-    joyx_servo = pid_trace_joyx(abs(dx)) * joyx_derection;
 }
-  
+void aim_ball ( int dx, int dy){
+    aim_ob ( dx, dy, "ball");
+}
+void aim_face ( int dx, int dy){
+    aim_ob ( dx, dy, "face");
 }
 
 // 控制摄像头舵机
 void setCarmeraFixServoAngles(int servo_angle) {
-  if (abs(servo_angle - current_s2) > 1) {
+    servo_angle = constrain(servo_angle, 0, 180);
-    servo_angle = constrain(servo_angle, 45, 180); // 60 -180
     cam_fix_servo.write(servo_angle);
     current_s2 = servo_angle;
 }
-}
 // 控制前档舵机
 void setFrontierServoAngles(int servo_angle) {
-    servo_angle = constrain(servo_angle, 0, 180);
+  current_s1 = constrain(servo_angle, 0, 180);
-    frontier_servo.write(servo_angle);
+  frontier_servo.write(current_s1);
 }
 
 // 通过角度控制舵机
 void setServoAngles(int s1, int s2) {
-  // s1 前档舵
+  setFrontierServoAngles(s1); // s1 前档舵
-  setFrontierServoAngles(s1);
+  setCarmeraFixServoAngles(s2); // s2 摄像头舵机
- 
-  // s2 摄像头舵机
-  setCarmeraFixServoAngles(s2);
 }
 
 // 退出追踪模式
 void cancel_trace_mode(){
   trace_mode = false;
-  trace_stand_mode = false;
+  kick_place_mode = false;
-  joyx_servo = 9999;
+  kick_run_mode = false;
+
+  dx_to_joyx = 9999;
+  dy_to_joyy = 9999;
+
+  pid_trace_joyx.error_prev = 0.0f;
+  pid_trace_joyy.error_prev = 0.0f;
+
   setLEDColor(CRGB::Black);
   leg_height_base = original_leg_height_base; // 上升
-  // setFrontierServoAngles(0);
+
+  setFrontierServoAngles(0);
+
+}
+
+void SerialToMaixcam(String cat, String str){
+  // if(str == "chase_ball_over" or str == "aim_face_over"){
+  //    Serial.println( cat + "=" + str + ";" );
+  // }
+   
 }