参数

User/Module/Chassis.c

@@ -131,7 +131,14 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
                     break;
                 case Chassis_Adjust_Sick:
 									
-                    sick_calibration(c, ctrl, out);
+								if(abs(ctrl->cmd_MID360.angle)<1.0f){
+									c->move_vec.Vx = ctrl->cmd_MID360.dis  * 3000;
+								}
+								if(-ctrl->cmd_MID360.dis>0.02f){
+									c->move_vec.Vw = -ctrl->cmd_MID360.angle  * 1000;
+								}
+
+//                    sick_calibration(c, ctrl, out);
 //                    c->to_nuc.send  = (c->sick_cali.is_reach == 1) ? 1 : 0;
                     break;
 

User/Module/config.c

@@ -75,7 +75,7 @@ static const ConfigParam_t param ={
 
     /*投球*/
     .LaunchCfg = {
-    .m2006_init = -125.0f,    // M2006初始角度
+    .m2006_init = -130.0f,    // M2006初始角度
     .m2006_trig = 0.0f,       // M2006触发角度
     .go_pull_pos = -210.0f,   // go上升去合并扳机，扳机位置
     .go_up_speed = 12.0f,     // 上升速度

User/Module/up_utils.c

@@ -171,62 +171,3 @@ int8_t Pass_Sequence_Check(UP_t *u, CMD_t *c) //按键顺序检测，传球，
 
 
 
-#define BIN_SCALE 1000           // 精度：0.001米
-#define BIN_OFFSET 3000          // 最小支持3.000米 → bin 3000
-#define BIN_COUNT 5001           // 支持3.000 ~ 8.000米，(8000-3000+1)
-#define WINDOW_SIZE 50
-
-static float window[WINDOW_SIZE] = {0};
-static int index = 0;
-static int filled = 0;
-static uint16_t freq_map[BIN_COUNT] = {0};  // 用16位计数防止溢出
-
-static float last_output = 0.0f;
-static uint8_t first_run = 1;
-
-// 四舍五入保留3位小数
-static inline float round3f(float x) {
-    return (int)(x * BIN_SCALE + 0.5f) / (float)BIN_SCALE;
-}
-
-float stable_distance_filter(float new_value) {
-    // 1. 四舍五入到0.001
-    float rounded = round3f(new_value);
-    int new_bin = (int)(rounded * BIN_SCALE + 0.5f);
-
-    // 2. 移除旧值计数
-    int old_bin = (int)(round3f(window[index]) * BIN_SCALE + 0.5f);
-    if (filled && old_bin >= BIN_OFFSET && old_bin < BIN_OFFSET + BIN_COUNT) {
-        freq_map[old_bin - BIN_OFFSET]--;
-    }
-
-    // 3. 加入新值到窗口和计数
-    window[index++] = new_value;
-    if (index >= WINDOW_SIZE) {
-        index = 0;
-        filled = 1;
-    }
-    if (new_bin >= BIN_OFFSET && new_bin < BIN_OFFSET + BIN_COUNT) {
-        freq_map[new_bin - BIN_OFFSET]++;
-    }
-
-    // 4. 找出现频率最高的桶
-    int max_bin = -1;
-    uint16_t max_freq = 0;
-    for (int i = 0; i < BIN_COUNT; ++i) {
-        if (freq_map[i] > max_freq) {
-            max_freq = freq_map[i];
-            max_bin = i + BIN_OFFSET;
-        }
-    }
-
-    float current_fixed = max_bin >= 0 ? max_bin / (float)BIN_SCALE : rounded;
-
-    // 5. 判断是否更新输出，差值超过0.1才更新
-    if (first_run || fabsf(current_fixed - last_output) > 0.1f) {
-        last_output = current_fixed;
-        first_run = 0;
-    }
-
-    return last_output;
-}

User/Module/up_utils.h

@@ -53,9 +53,6 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
 int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
 
 
-static inline float round3f(float x) ;
-
-float stable_distance_filter(float new_value) ;
 
 
 #endif