转速模式增加馈电PID

app/app_brake.c

@@ -88,7 +88,6 @@ static void brakeTimerProcess(void *signal_id)
         if (shouldApplyBrake())
         {
             brake_data.state = BRAKE_STATE_APPLYING_BRAKE;
-            brake_data.brake_motor_state = 1;
             if( 0 == brake_data.brake_direction)
             {
                brake_data.brake_motor_state = 1;               
@@ -106,8 +105,7 @@ static void brakeTimerProcess(void *signal_id)
         if (shouldReleaseBrake() && power_data.current_state == POWER_WORKING)
         {
             brake_data.state = BRAKE_STATE_RELEASING_BRAKE;
-            brake_data.brake_motor_state = 2;
+
-            brakeOutput(NULL);
             if( 0 == brake_data.brake_direction)
             {
                brake_data.brake_motor_state = 2;               
@@ -116,6 +114,7 @@ static void brakeTimerProcess(void *signal_id)
             {
                brake_data.brake_motor_state = 1;                    
             }         
+            brakeOutput(NULL);
             timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0);
         }
         break;
@@ -151,8 +150,6 @@ static void brakeTimerProcess(void *signal_id)
         setParam("brk_pos", (float)brake_data.brake_position);
         brake_data.old_brake_position = brake_data.brake_position;
         
-        
-        
         printf("writeE2 brake_position = %d\n",brake_data.brake_position);
         
     }

app/app_differential_drive.c

@@ -19,8 +19,100 @@ DiffData diff_data;
 PID_t speed_pid;
 PID_t yaw_rate_pid;
 
+PID_t left_feed_pid;
+PID_t right_feed_pid;
+
+
+/**
+ * @brief 根据挡位和输入转矩计算输出转矩值
+ * @param gear 挡位状态（STATE_FORWARD/STATE_BACKWARD/其他）
+ * @param input_torque 输入转矩值
+ * @return 处理后的转矩值（已包含偏移量和系数）
+ */
+float calculateTorqueOutput(uint8_t gear, float input_torque) 
+{
+    const float OFFSET = 300.0f;    // 偏移量常量
+    const float SCALE_FACTOR = 100.0f;  // 缩放系数
+    const float DEFAULT_VALUE = 30000.0f; // 默认输出值
+
+    float output_torque;
+    
+    if (gear == STATE_FORWARD) 
+    {
+        output_torque = (input_torque + OFFSET) * SCALE_FACTOR;
+    } 
+    else if (gear == STATE_BACKWARD) 
+    {
+        output_torque = (-input_torque + OFFSET) * SCALE_FACTOR;
+    }
+    else 
+    {
+        output_torque = DEFAULT_VALUE;
+    }
+    
+    return output_torque;
+}
+
+/**
+ * @brief 车辆状态控制状态机
+ * @note 根据车速和扭矩方向切换前进/后退状态，带扭矩回滞保护
+ * @param ctx 状态机上下文，包含当前状态(STATE_INIT/FORWARD/BACKWARD)
+ * @param speed 当前车速（单位：km/h），0表示静止状态
+ * @param torque 当前扭矩（单位：Nm），正数表示前进方向，负数表示后退方向
+ */
+void handleVehicleState(MotorState *ctx, float speed, float torque) 
+{
+    switch (*ctx) 
+    {
+        // 初始状态：根据扭矩方向初始化
+        case STATE_INIT:
+        {
+            if (torque >= 0.0f) 
+            {
+                *ctx = STATE_FORWARD;  // 正扭矩进前进档
+            }
+            else 
+            {
+                *ctx = STATE_BACKWARD; // 负扭矩进倒档
+            }
+            break;
+        }
+
+        // 前进状态：零速且反向扭矩超阈值切倒档
+        case STATE_FORWARD:
+        {
+            if ( (speed == 0.0f) && (torque <= -TORQUE_HYSTERESIS_THRESHOLD) ) 
+            {
+                *ctx = STATE_BACKWARD;  // 满足条件切换
+            }
+            else 
+            {
+                *ctx = STATE_FORWARD;   // 否则保持
+            }
+            break;
+        }
+
+        // 倒车状态：零速且正向扭矩超阈值切前进
+        case STATE_BACKWARD:
+        {
+            if ( (speed == 0.0f) && (torque >= TORQUE_HYSTERESIS_THRESHOLD) ) 
+            {
+                *ctx = STATE_FORWARD;   // 满足条件切换
+            }
+            else 
+            {
+                *ctx = STATE_BACKWARD;  // 否则保持
+            }
+            break;
+        }
+        
+        default:;  // 异常处理
+    }
+}
+
+
 // 设置电机输出
-void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint16_t discharge_power)
+void setMotorOutput(float *out_torq, float max_torque, uint16_t left_feed_power,uint16_t right_feed_power, uint16_t discharge_power)
 {
   
     float abs_left_front_speed  = 0;
@@ -29,39 +121,23 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint
     float abs_right_rear_speed  = 0;
     
     // 档位
-    un_motor_output1.bit_data.gear = diff_data.state; // 1 表示前进，2 表示后退，0空挡
+    un_motor_output1.bit_data.gear = (out_torq[0] >= 0) ? 1 : 2; // 1 表示前进，2 表示后退
-    un_motor_output2.bit_data.gear = diff_data.state;
+    un_motor_output2.bit_data.gear = (out_torq[1] >= 0) ? 1 : 2;
-    un_motor_output3.bit_data.gear = diff_data.state; 
+    un_motor_output3.bit_data.gear = (out_torq[2] >= 0) ? 2 : 1; //20250717 2,3电机反相
-    un_motor_output4.bit_data.gear = diff_data.state;    
+    un_motor_output4.bit_data.gear = (out_torq[3] >= 0) ? 2 : 1;
 
-    
+    abs_left_front_speed  = fabsf(out_torq[0]); //根据挡位增加转矩方向
-    //增加系数以及偏移量
+    abs_right_front_speed = fabsf(out_torq[1]);     
-    if(diff_data.state == STATE_FORWARD)//根据挡位来判断，扭矩的正负 
+    abs_left_rear_speed   = fabsf(out_torq[2]);     
-    {
+    abs_right_rear_speed  = fabsf(out_torq[3]);     
-        abs_left_front_speed   = (out_torq[0] + 300.0f) *100.0f;
-        abs_right_front_speed  = (out_torq[1] + 300.0f) *100.0f;
-        abs_left_rear_speed    = (out_torq[2] + 300.0f) *100.0f;
-        abs_right_rear_speed   = (out_torq[3] + 300.0f) *100.0f;   
-    }
-    else if(diff_data.state == STATE_BACKWARD)//倒挡直接修改为负扭矩发送出去
-    {
-        abs_left_front_speed   = (-out_torq[0] + 300.0f) *100.0f;
-        abs_right_front_speed  = (-out_torq[1] + 300.0f) *100.0f;
-        abs_left_rear_speed    = (-out_torq[2] + 300.0f) *100.0f;
-        abs_right_rear_speed   = (-out_torq[3] + 300.0f) *100.0f;      
-    }
-    else//空挡直接发0
-    {
-        abs_left_front_speed  = 0;
-        abs_right_front_speed = 0;
-        abs_left_rear_speed   = 0; 
-        abs_right_rear_speed  = 0;        
-    }
 
  
     // 设置左右电机期望转速
-//    un_motor_output1.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_speed) + 30000); // 20240921 增加偏移量 30000
+    un_motor_output1.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_front_speed) + 30000); // 20240921 增加偏移量 30000
-//    un_motor_output2.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_speed) + 30000); // 20240921 增加偏移量 30000
+    un_motor_output2.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_front_speed) + 30000); // 20240921 增加偏移量 30000
+    un_motor_output3.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_rear_speed) + 30000); // 20240921 增加偏移量 30000
+    un_motor_output4.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_rear_speed) + 30000); // 20240921 增加偏移量 30000
+
 
     // 设置模式为扭矩模式
     un_motor_output1.bit_data.mode = MOTOR_MODE; 
@@ -69,27 +145,23 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint
     un_motor_output3.bit_data.mode = MOTOR_MODE; 
     un_motor_output4.bit_data.mode = MOTOR_MODE;     
 
-    // 设置最大扭矩
+    // 设置扭矩
-    un_motor_output1.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_front_speed ); 
+    un_motor_output1.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
-    un_motor_output2.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_front_speed );     
+    un_motor_output2.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
-    un_motor_output3.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_rear_speed );     
+    un_motor_output3.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
-    un_motor_output4.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_rear_speed ); 
+    un_motor_output4.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量    
-    
     
     // 设置馈电功率
-    un_motor_output1.bit_data.feed_power = feed_power;
+    un_motor_output1.bit_data.feed_power = left_feed_power;
-    un_motor_output2.bit_data.feed_power = feed_power;
+    un_motor_output2.bit_data.feed_power = right_feed_power;
-    un_motor_output3.bit_data.feed_power = feed_power;
+    un_motor_output3.bit_data.feed_power = left_feed_power;
-    un_motor_output4.bit_data.feed_power = feed_power;    
+    un_motor_output4.bit_data.feed_power = right_feed_power;    
-    
 
     // 设置放电功率
     un_motor_output1.bit_data.discharge_power = discharge_power;
     un_motor_output2.bit_data.discharge_power = discharge_power; 
     un_motor_output3.bit_data.discharge_power = discharge_power;
     un_motor_output4.bit_data.discharge_power = discharge_power;     
-    
-
 }
 
 
@@ -220,93 +292,25 @@ void calculateCurrentState(float dt)
     diff_data.max_speed = calculateMaxSpeed();
     previous_speed = diff_data.speed;
 }
-
-
 /**
- * @brief 基于转速反比的双电机扭矩分配函数
+ * @brief 判断减速状态（最简逻辑）  如果同向或者有一个为0，或者都为0，那么判断绝对值大小，如果期望绝对值小于当前绝对值，那就为减速
- * @param rpm1 电机1当前转速（单位：rpm）
+ * @param target_speed  期望速度（带方向）
- * @param rpm2 电机2当前转速（单位：rpm）
+ * @param current_speed 当前速度（带方向）
- * @param total_torque 系统总需求扭矩（单位：Nm）
+ * @return 1:减速, 0:加速或保持
- * @param torque1 [out] 电机1分配到的扭矩（单位：Nm）
- * @param torque2 [out] 电机2分配到的扭矩（单位：Nm）
- * @note 分配原则：转速越高的电机分配扭矩越小，确保负载均衡
  */
-void distributeTorque(float rpm1, float rpm2, float total_torque, float* torque1, float* torque2, float max_torque, float min_torque)
+uint8_t is_Decelerating(float target_speed, float current_speed, float des_yaw_rate)
 {
-  
+    // 特殊处理双零状态,双零表示刹车
-    // 总扭矩为0时快速返回
+    if ( (target_speed == 0.0f && current_speed == 0.0f)  )//如果又减速的话也刹车  //|| (0 != des_yaw_rate)
-    if (fabs(total_torque) < 0.001f) {
+    {
-        *torque1 = 0.0f;
+        return 2; // 驻车
-        *torque2 = 0.0f;
-        return;
     }  
-
+    // 核心逻辑：方向相反 或 (同向/含零且期望绝对值 < 当前绝对值)
-//    // 保护条件：当两电机均静止时采用平均分配策略
+    return (signbit(target_speed) != signbit(current_speed)) || 
-//    if (fabs(rpm1) < 0.001f && fabs(rpm2) < 0.001f) {
+           (fabs(target_speed) < fabs(current_speed));
-//        *torque1 = total_torque / 2.0f;
-//        *torque2 = total_torque / 2.0f;
-//        return;
-//    }
-
-    // 计算权重因子（与转速成反比关系）
-    // 注：添加0.001f防止零转速时除零错误，fabs确保负转速正确处理
-    float weight1 = 1.0f / (fabs(rpm1) + 0.001f);
-    float weight2 = 1.0f / (fabs(rpm2) + 0.001f);
-
-    // 归一化计算分配比例
-    float total_weight = weight1 + weight2;
-    *torque1 = total_torque * (weight1 / total_weight);
-    *torque2 = total_torque * (weight2 / total_weight);
-    
-    // 独立限制单侧扭矩（修改核心逻辑）
-    if (fabs(*torque1) > max_torque) {
-        *torque1 = copysignf(max_torque, *torque1);
-    }
-    if (fabs(*torque2) > max_torque) {
-        *torque2 = copysignf(max_torque, *torque2);
-    }   
-    
-    // 仅对非零扭矩应用下限限制
-    if (fabs(*torque1) < min_torque) {
-        *torque1 = copysignf(min_torque, *torque1);
-    }
-    if ( fabs(*torque2) < min_torque) {
-        *torque2 = copysignf(min_torque, *torque2);
-    }    
-    
 }
 
 
-
-/**
- * @brief 根据轮速差动态调整电机扭矩（带非负限制）
- * @param speed_left 左轮速度（单位：rpm或自定义）
- * @param speed_right 右轮速度（单位：rpm或自定义）
- * @param torque_left 左轮扭矩指针（单位：Nm或自定义）
- * @param torque_right 右轮扭矩指针（单位：Nm或自定义）
- * @param threshold 触发调整的速差阈值（单位同轮速）
- * @param k 扭矩调整系数（无量纲，建议0<k<1）
- * @note 函数会直接修改传入的扭矩值，并确保扭矩不小于0
- */
-void adjust_torque_by_speed_diff(float speed_left, float speed_right, 
-                               float* torque_left, float* torque_right,
-                               float threshold, float k) {
-    // 计算轮速差绝对值
-    float speed_diff = fabsf(speed_left - speed_right);
-    
-    if (speed_diff > threshold) {
-        // 计算需要减少的扭矩量（速差超出阈值部分×系数）
-        float torque_reduction = (speed_diff - threshold) * k;
-        
-        if (speed_left > speed_right) {
-            // 左轮过快时减少左扭矩，并限制最小值为0
-            *torque_left = fmaxf(*torque_left - torque_reduction, 0.0f);
-        } else {
-            // 右轮过快时减少右扭矩，并限制最小值为0
-            *torque_right = fmaxf(*torque_right - torque_reduction, 0.0f);
-        }
-    }
-}
 // 计算左右电机速度
 void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max_speed, float *motor_speed)
 {
@@ -321,14 +325,17 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
     }
 
 #if THROTTLE_PID_MODE
-    float max_torque = (float)getParam("maxTorq");
+  
+    float left_speed_mps  = 0.0f;  
+    float right_speed_mps = 0.0f;
+    
+    float max_torque = diff_data.max_Torq;
 
     linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
     yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
     
-    
+    left_speed_mps = linear_velocity_x + yaw_rate;
-    float left_speed_mps = linear_velocity_x + yaw_rate;
+    right_speed_mps = linear_velocity_x - yaw_rate;
-    float right_speed_mps = linear_velocity_x - yaw_rate;  
 
 //扭矩分配		
     if(max_torque < left_speed_mps)
@@ -354,35 +361,24 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
     else{}
     
 //    printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate);   
+//    adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5);     
+//    adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5);      
+//    printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);  
     
-    motor_speed[0] = left_speed_mps;
+    motor_speed[0] = left_speed_mps;//加速状态，没有负扭矩，要么前进加速要么后退加速
     motor_speed[2] = left_speed_mps; 
 
     motor_speed[1] = right_speed_mps;
     motor_speed[3] = right_speed_mps;  
 
-    adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5);     
+    handleVehicleState(&diff_data.motor_state[0], diff_data.left_front_motor_speed,   motor_speed[0]); //通过扭矩以及速度来判断挡位
-    adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5);      
+    handleVehicleState(&diff_data.motor_state[1], diff_data.right_front_motor_speed,  motor_speed[1]); 
-    
+    handleVehicleState(&diff_data.motor_state[2], diff_data.left_rear_motor_speed,    motor_speed[2]); 
-      
+    handleVehicleState(&diff_data.motor_state[3], diff_data.right_rear_motor_speed,   motor_speed[3]); 
-//    printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);  
-    
-  
-    
-    
-    
-    
-    
-    
 
 //    distributeTorque(diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed,2*left_speed_mps,&motor_speed[0],&motor_speed[2],diff_data.max_Torq,diff_data.min_Torq);
 //    distributeTorque(diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed,2*right_speed_mps,&motor_speed[1],&motor_speed[3],diff_data.max_Torq,diff_data.min_Torq);  
     
-//     printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", motor_speed[0], motor_speed[1], motor_speed[2], motor_speed[3]);  
-    
-//    // 返回计算结果
-//    *left_motor_speed = left_speed_mps;
-//    *right_motor_speed = right_speed_mps;
 #else    
     
     // 限制线速度和偏航率
@@ -394,8 +390,8 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
     float rotational_velocity = ((float)getParam("whl_bas") / 2.0f) * yaw_rate;
 
     // 计算车辆左右线速度 (m/s)
-    float left_speed_mps = linear_velocity_x - rotational_velocity;  //20250316 为解决原地转向和直行转向相同，所以把左右输出的速度交换
+    float left_speed_mps = linear_velocity_x + rotational_velocity;  //20250316 为解决原地转向和直行转向相同，所以把左右输出的速度交换
-    float right_speed_mps = linear_velocity_x + rotational_velocity;
+    float right_speed_mps = linear_velocity_x - rotational_velocity;
 
     // 计算轮子周长
     float wheel_circumference = (float)getParam("whl_dia") * M_PI;
@@ -415,20 +411,76 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
     float max_motor_rpm = (float)getParam("max_rpm");
     left_motor_rpm = constrain(left_motor_rpm, -max_motor_rpm, max_motor_rpm);
     right_motor_rpm = constrain(right_motor_rpm, -max_motor_rpm, max_motor_rpm);
+    
     // 当电机转速小于50转时，设置为0
-    if (fabsf(left_motor_rpm) < 50)//速度慢所以设置位10转
+//    if (fabsf(left_motor_rpm) < 20)//速度慢所以设置位10转
-    {
+//    {
-        left_motor_rpm = 0;
+//        left_motor_rpm = 0;
-    }
+//    }
-    if (fabsf(right_motor_rpm) < 50)//速度慢所以设置位10转
+//    if (fabsf(right_motor_rpm) < 20)//速度慢所以设置位10转
-    {
+//    {
-        right_motor_rpm = 0;
+//        right_motor_rpm = 0;
-    }
+//    }
     // 左边电机方向反一下，因为电机安装反了，返回来的数据也要反一下
  //   left_motor_rpm = -left_motor_rpm;
     // 返回计算结果
-    *left_motor_speed = left_motor_rpm;
+    motor_speed[0] = left_motor_rpm;//加速状态，没有负扭矩，要么前进加速要么后退加速
-    *right_motor_speed = right_motor_rpm;
+    motor_speed[2] = left_motor_rpm; 
+
+    motor_speed[1] = right_motor_rpm;
+    motor_speed[3] = right_motor_rpm;
+ 
+    diff_data.left_motor_state = is_Decelerating(left_motor_rpm, diff_data.left_motor_speed, diff_data.desired_yaw_rate);
+    diff_data.right_motor_state = is_Decelerating(right_motor_rpm, diff_data.right_motor_speed, diff_data.desired_yaw_rate);
+    
+//    printf(" left = %d,%d\n", diff_data.left_motor_state,diff_data.right_motor_state);      
+    
+    //馈电PID计算
+    static float previous_time11 = 0.0f;
+
+    float time1 = (float)getCurrentTime();
+    float dt = (time1 - previous_time11) / PERIOD_TICK; 
+    previous_time11 = time1;    
+    
+    float left_feed_pwoer = calculatePidOutput(&left_feed_pid, left_motor_rpm, diff_data.left_motor_speed, 0.0f, dt);//左右馈电PID
+    float right_feed_pwoer = calculatePidOutput(&right_feed_pid, right_motor_rpm, diff_data.right_motor_speed, 0.0f, dt);    
+    
+    if(1 == diff_data.left_motor_state)//根据是否是刹车状态来确定是否设定馈电功率
+    {
+       diff_data.left_motor_feed_power = diff_data.max_feed_power;//20250723 修改为固定值最大值
+    }
+    else if(2 == diff_data.left_motor_state)
+    {
+       diff_data.left_motor_feed_power = diff_data.max_feed_power;
+    }
+    else
+    {
+       diff_data.left_motor_feed_power = 0.0f;      
+    }
+    
+    if(1 == diff_data.right_motor_state)//根据是否是刹车状态来确定是否设定馈电功率
+    {
+       diff_data.right_motor_feed_power = diff_data.max_feed_power;//20250723 修改为固定值最大值
+    }
+    else if(2 == diff_data.left_motor_state)
+    {
+       diff_data.right_motor_feed_power = diff_data.max_feed_power;
+    }    
+    else
+    {
+       diff_data.right_motor_feed_power = 0.0f;      
+    }   
+    
+    //限制最大馈电功率
+    if(diff_data.left_motor_feed_power > diff_data.max_feed_power)
+    {
+       diff_data.left_motor_feed_power = diff_data.max_feed_power;
+    }
+    
+    if(diff_data.right_motor_feed_power > diff_data.max_feed_power)
+    {
+       diff_data.right_motor_feed_power = diff_data.max_feed_power;
+    }    
 
 #endif    
 }
@@ -475,74 +527,6 @@ float mapRemoteControlSpeed(
     return output_speed;
 }
 
-/**
- * @brief 状态机处理函数（修改后版本）
- */
-void handleVehicleState(DiffData *ctx) 
-{
-    switch (ctx->state) 
-    {
-        //-------------------------------------------
-        // 初始状态：根据期望速度方向跳转
-        //-------------------------------------------
-        case STATE_INIT:
-        {
-            if (ctx->desired_speed < 0.0f) 
-            {
-                ctx->state = STATE_BACKWARD;
-            } 
-            else 
-            {
-                ctx->state = STATE_FORWARD;
-            }
-            break;
-        }
-
-        //-------------------------------------------
-        // 前进状态：处理反向指令（新增else分支）
-        //-------------------------------------------
-        case STATE_FORWARD:
-        {
-            if ((ctx->desired_speed < 0.0f) && (ctx->speed == 0.0f)) 
-            {
-                ctx->state = STATE_BACKWARD;  // 零速时允许切换方向
-            } 
-            else if ((ctx->desired_speed < 0.0f) && (ctx->speed != 0.0f)) 
-            {
-                ctx->desired_speed = 0.0f;    // 非零速时清空期望速度
-                ctx->state = STATE_FORWARD;   // 显式保持当前状态
-            }
-            else 
-            {
-                ctx->state = STATE_FORWARD;    // 新增：其他情况保持前进状态
-            }
-            break;
-        }
-
-        //-------------------------------------------
-        // 倒车状态：处理正向指令（新增else分支）
-        //-------------------------------------------
-        case STATE_BACKWARD:
-        {
-            if ((ctx->desired_speed > 0.0f) && (ctx->speed == 0.0f)) 
-            {
-                ctx->state = STATE_FORWARD;   // 零速时允许切换方向
-            } 
-            else if ((ctx->desired_speed > 0.0f) && (ctx->speed != 0.0f)) 
-            {
-                ctx->desired_speed = 0.0f;    // 非零速时清空期望速度
-                ctx->state = STATE_BACKWARD;  // 显式保持当前状态
-            }
-            else 
-            {
-                ctx->state = STATE_BACKWARD;   // 新增：其他情况保持倒车状态
-            }
-            break;
-        }
-    }
-}
-
-
 
 
 // 差速处理函数
@@ -572,9 +556,6 @@ static void diffProcess(void *signal_id)
         diff_data.desired_yaw_rate = diff_data.desired_curvature * diff_data.desired_speed;
     }
     
-    handleVehicleState(&diff_data); //20250704 换挡函数  速度为0才能换挡
-    
-    
 //    printf("desired_speed: %f, desired_yaw: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
     // 使用 PID 控制器计算输出速度和曲率
     float output_speed = calculatePidOutput(&speed_pid, diff_data.desired_speed, diff_data.speed, 0.0f, dt);
@@ -585,51 +566,23 @@ static void diffProcess(void *signal_id)
     // 限制输出速度在当前速度和最大加速度计算出来的速度之间
     // output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt);
 
-    
+    // if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) )//手柄回中，速度小的时候清0 
-    if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) )//手柄回中，速度小的时候清0 
+    // {
-    {
+    //     resetPidIntegral(&speed_pid);
-        resetPidIntegral(&speed_pid);
+    //     resetPidIntegral(&yaw_rate_pid);        
-        resetPidIntegral(&yaw_rate_pid);        
+    //     output_speed = 0;
-        output_speed = 0;
+    //     output_yaw_rate = 0;
-        output_yaw_rate = 0;
+    // }   
-    }   
-   
-//    printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral);    
-    
-    
-//    if(diff_data.desired_yaw_rate != 0)//有转向的情况下下
-//    {
-//        if( (output_yaw_rate > -500) && (output_yaw_rate < 500) )//如果是转向输出在-500~500之间，那么开始原地转向扭矩太小，所以设定最小扭矩。
-//        {
-//           output_yaw_rate = 500;
-//        }
-//    }
-    
     
     // 使用差速车辆动力学模型计算左右电机的期望速度
     computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_torq[0]);
    
 
-    
-    
-    
-    
-    
-//     if( (left_speed < 200) && (left_speed > -200) )
-//     {
-//       left_speed = 0;
-//     }
-//     
-//     if( (right_speed < 200) && (right_speed > -200) )
-//     {
-//       right_speed = 0;
-//     }      
-    
-    
     // 设置电机输出
     setMotorOutput(&diff_data.out_torq[0], 
                    diff_data.max_Torq,//
-                   (uint16_t)getParam("feedPwr"), 
+                   diff_data.left_motor_feed_power, 
+                   diff_data.right_motor_feed_power,                    
                    (uint16_t)getParam("dispPwr"));
     // 发布左右电机期望转速,电源在工作状态才能发送
     if (power_data.current_state == POWER_WORKING)
@@ -638,7 +591,6 @@ static void diffProcess(void *signal_id)
         publishMessage(&un_motor_output2, 1);    
         publishMessage(&un_motor_output3, 1);
         publishMessage(&un_motor_output4, 1); 
-        
     }
     
     
@@ -648,45 +600,15 @@ static void diffProcess(void *signal_id)
     un_can_debug_output.bit_data.curvature = (uint8_t)(int8_t)(diff_data.yaw_rate*10);
     un_can_debug_output.bit_data.desired_curvature = (uint8_t)(int8_t)(diff_data.desired_yaw_rate*10);
     
-    un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(diff_data.left_motor_speed);
+//    un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(diff_data.left_speed_diff);
-    un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_motor_speed);    
+//    un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_speed_diff);    
     
     publishMessage(&diff_data, 1);     
     
 }
- 
-/******************************************************************************
-               Filter();      N个数中取两个
-******************************************************************************/
-int16_t Filter(int16_t *s,uint8_t Len)
-{
-  uint8_t i,j;
-	int16_t temp;
-//降序排序
-  for(i=0;i<Len-1;i++)
-    for(j=i+1;j<Len;j++)
-    {
-      if(*(s+i)>*(s+j))
-      {
-         *(s+i)=*(s+i)^*(s+j);
-         *(s+j)=*(s+j)^*(s+i);
-         *(s+i)=*(s+i)^*(s+j);
-       }
-     }
-  temp=(*(s+Len/2)+*(s+(Len/2-1)))/2;//20210225修改为除以2，负数不能够右移
-  return(temp);
-}
-
-
-
-
-
-
 // 差速输入处理函数
 static void diffInput(void *signal_id)
 { 
-    float motor_speed_temp = 0.0f;
-    
     if (signal_id == &un_sw_sample)
     {
         diff_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;
@@ -707,6 +629,8 @@ static void diffInput(void *signal_id)
             diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5);         
             diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0.1, 2, 2, 1, 0.5);
             
+            diff_data.desired_curvature = -diff_data.desired_curvature;
+            
             if(diff_data.desired_speed >= 0)//20250320 增加根据速度大小来决定方向，解决后退时转弯反向的问题   
             {
                 diff_data.desired_curvature = diff_data.desired_curvature;               
@@ -743,32 +667,58 @@ static void diffInput(void *signal_id)
     {
         diff_data.left_front_motor_speed = (float)((int16_t)(un_motor_input1.bit_data.speed - 30000));//20240921 增加偏移量
         diff_data.left_rear_motor_speed  = (float)((int16_t)(un_motor_input3.bit_data.speed - 30000));//20240921 增加偏移量
+        diff_data.left_rear_motor_speed  = - diff_data.left_rear_motor_speed;//20250708 增加反相   
         
-        if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速
+        if( fabs(diff_data.left_front_motor_speed) < 20)//速度死区
         {
-           motor_speed_temp = diff_data.left_front_motor_speed;
+            diff_data.left_front_motor_speed = 0;
         }
-        else
+        
+        if( fabs(diff_data.left_rear_motor_speed) < 20)//速度死区
         {
-           motor_speed_temp = diff_data.left_rear_motor_speed;           
+            diff_data.left_rear_motor_speed = 0;
         }        
-        diff_data.left_motor_speed = motor_speed_temp;
+        
+//        if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速
+//        {
+//           motor_speed_temp = diff_data.left_front_motor_speed;
+//        }
+//        else
+//        {
+//           motor_speed_temp = diff_data.left_rear_motor_speed;           
+//        }
+//        diff_data.left_motor_speed = motor_speed_temp;
+        
+        diff_data.left_motor_speed = (diff_data.left_front_motor_speed + diff_data.left_rear_motor_speed)/2.0f;        
     }
     else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右电机）
     {
         diff_data.right_front_motor_speed = (float)((int16_t)(un_motor_input2.bit_data.speed - 30000));  // 20250502 1号控制器增加反相 
+        
         diff_data.right_rear_motor_speed  = (float)((int16_t)(un_motor_input4.bit_data.speed - 30000));   
+        diff_data.right_rear_motor_speed  = - diff_data.right_rear_motor_speed;//20250708 增加反相 
 
-        if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速
+        if( fabs(diff_data.right_front_motor_speed) < 20)//速度死区
         {
-           motor_speed_temp = diff_data.right_rear_motor_speed;
+            diff_data.right_front_motor_speed = 0;
-        }
-        else
-        {
-           motor_speed_temp = diff_data.right_front_motor_speed;           
         }
         
-        diff_data.right_motor_speed = motor_speed_temp; 
+        if( fabs(diff_data.right_rear_motor_speed) < 20)//速度死区
+        {
+            diff_data.right_rear_motor_speed = 0;
+        }     
+        
+        
+//        if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速
+//        {
+//           motor_speed_temp = diff_data.right_rear_motor_speed;
+//        }
+//        else
+//        {
+//           motor_speed_temp = diff_data.right_front_motor_speed;           
+//        }
+
+        diff_data.right_motor_speed = (diff_data.right_rear_motor_speed + diff_data.right_front_motor_speed)/2.0f; 
     }
     
     // 急停开关
@@ -789,18 +739,26 @@ static void diffInput(void *signal_id)
     
     if (diff_data.emergency_stop_state == 1)//刹车  20241017 增加的扭矩限制
     {
-       diff_data.max_Torq = 5;//20240403修改。刹车就是5N
+        diff_data.max_Torq = 5;//20240403修改。刹车就是5N
     }
     else if ((0 == diff_data.desired_speed) && (0 == diff_data.desired_curvature) && (diff_data.left_motor_speed > -100) && (diff_data.left_motor_speed < 100)&& (((diff_data.right_motor_speed > -100) && (diff_data.right_motor_speed < 100))))//20240330只有当手柄回中，然后当前已经停止的状态才设置为最小停车扭矩
     {
-       diff_data.max_Torq = 5;//停车 就为0  20250425 修改为5，解决手柄回中，震荡问题
+        diff_data.max_Torq = 5;//停车 就为0  20250425 修改为5，解决手柄回中，震荡问题
     }
     else
     {
-       diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
+        diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
+    }
+    if((power_data.current_state == POWER_WORKING))//电机上电才运行
+    {
+        diffProcess(&diff_data);//计算左右电机期望转速
+    }
+    else
+    {
+        resetPidIntegral(&speed_pid);
+        resetPidIntegral(&yaw_rate_pid);      
     }
 
-    diffProcess(&diff_data);//计算左右电机期望转速
 }
 
 
@@ -812,10 +770,13 @@ void preChargeFinish(void *signal_id)
     
     float out_torq[4] = {0.0f,0.0f,0.0f,0.0f};
 
-    setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr"));
+    setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), 0,0, (uint16_t)getParam("dispPwr"));
     // 档位
-//    un_motor_output1.bit_data.gear = 0; // 0表示空挡
+    un_motor_output1.bit_data.gear = 0; // 0表示空挡
-//    un_motor_output2.bit_data.gear = 0;
+    un_motor_output2.bit_data.gear = 0;
+    un_motor_output3.bit_data.gear = 0; // 0表示空挡
+    un_motor_output4.bit_data.gear = 0;    
+    
     publishMessage(&un_motor_output1, 1);
     publishMessage(&un_motor_output2, 1);
     publishMessage(&un_motor_output3, 1);
@@ -863,18 +824,49 @@ void diffParametersInit(void *signal_id)
                           getParam("crv_ol")
          );       
     }
-    diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩    
     
+    // 设置曲率 PID 控制器的参数
+     setPidParameters(&left_feed_pid,
+                      getParam("mot_kp"),
+                      getParam("mot_ki"),
+                      getParam("mot_kd"),
+                      getParam("mot_il"),
+                      getParam("mot_ol")
+     );     
      
-    // printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
+    // 设置曲率 PID 控制器的参数
-    // printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);   
+     setPidParameters(&right_feed_pid,
+                      left_feed_pid.kp,
+                      left_feed_pid.ki,
+                      left_feed_pid.kd,
+                      left_feed_pid.integral_limit,
+                      left_feed_pid.output_limit    
+     );   
+     
+//    diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩   
+//    diff_data.max_Torq = (float)getParam("maxTorq"); 
+     
+     diff_data.max_feed_power = (uint16_t)getParam("feedPwr");
+    
+    // if(0 == (float)getParam("diff_sp"))//20250711 防止参数为0，影响计算。
+    // {
+    //    diff_data.diff_dead_zone = 2;
+    // }
+    // else
+    // {
+    //    diff_data.diff_dead_zone = (float)getParam("diff_sp");//参数读取设定最大扭矩       
+    // }   
+    
+    printf("left_speed: %f, des_speed: %f,left_feed: %d\n", diff_data.left_motor_speed, diff_data.out_torq[0],diff_data.left_motor_feed_power);    //left_motor_rpm, diff_data.left_motor_speed
+    
+    printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
+    printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);   
 //    printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);     
     
-    // printf("left_motor_speed = %f\n",diff_data.left_motor_speed);
+    printf("LF_speed = %f,RF_speed = %f,LR_speed = %f,RR_speed = %f\n",diff_data.left_front_motor_speed,diff_data.right_front_motor_speed,diff_data.left_rear_motor_speed,diff_data.right_rear_motor_speed);
-    // printf("right_motor_speed = %f\n",diff_data.right_motor_speed);
     
 //    printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);     
-//    printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", diff_data.out_torq[0], diff_data.out_torq[1], diff_data.out_torq[2], diff_data.out_torq[3]);    
+    printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", diff_data.out_torq[0], diff_data.out_torq[1], diff_data.out_torq[2], diff_data.out_torq[3]);    
     
     float deffspeed  = (float)((int16_t)(un_remote_control_input.bit_data.speed));
     float deffcurvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature));
@@ -882,12 +874,13 @@ void diffParametersInit(void *signal_id)
     deffspeed = deffspeed * 0.01f;
     deffcurvature = deffcurvature * 0.0001f;   
     
-    printf("remote speed = %f, remote curvature = %f\n", deffspeed, deffcurvature);       
+    printf("remote_speed: %f, remote_yaw_rate: %f\n", deffspeed, deffcurvature);    
+    
+    printf(" left = %d,%d\n", diff_data.left_motor_state,diff_data.right_motor_state);       
      
     timerStart(&diff_app_timer,1000,1);//1s调用一次 
 }
 
-
 // 差速初始化函数
 void diffAppInit(void)
 {
@@ -927,6 +920,29 @@ void diffAppInit(void)
                       getParam("crv_ol")
      );
 
+    // 初始化减速 PID 控制器
+    initializePid(&left_feed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
+    // 设置 PID 控制器的参数
+     setPidParameters(&left_feed_pid,
+                      getParam("mot_kp"),
+                      getParam("mot_ki"),
+                      getParam("mot_kd"),
+                      getParam("mot_il"),
+                      getParam("mot_ol")
+     );     
+     
+    // 初始化加速 PID 控制器
+    initializePid(&right_feed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
+    // 设置 PID 控制器的参数
+     setPidParameters(&right_feed_pid,
+                      left_feed_pid.kp,
+                      left_feed_pid.ki,
+                      left_feed_pid.kd,
+                      left_feed_pid.integral_limit,
+                      left_feed_pid.output_limit
+     );            
+     
+     
     subscribe(&diff_app_timer, diffParametersInit);
     timerStart(&diff_app_timer,1000,1);//1s调用一次 
 

app/app_differential_drive.h

@@ -11,16 +11,19 @@ extern "C"
 #define  SPEED_FITER_NUM       6  
   
   
-#define  SPEED_PID_MODE        0
+#define  SPEED_PID_MODE        1
-#define  THROTTLE_PID_MODE     1  
+#define  THROTTLE_PID_MODE     0  
-  
   
+#define  TURN_MIN_TOUQUE       1   //n*m
 
 #define  SPEED_MODE          0x01
 #define  TORQUE_MODE         0x02   
   
-#define  MOTOR_MODE          TORQUE_MODE  
   
+#define  TORQUE_HYSTERESIS_THRESHOLD           0.3f   
+  
+  
+#define  MOTOR_MODE          SPEED_MODE  
  
   
 #define  ALPHA                 0.1f  // 滤波系数α∈[0.01,0.3]，0.2对应截止频率约10Hz（假设采样周期10ms）
@@ -30,8 +33,8 @@ extern "C"
 typedef enum
 {
     STATE_INIT,        ///< 初始状态（转速为0且等待扭矩方向判定）
-    STATE_FORWARD,     ///< 正向旋转状态（扭矩为正）
+    STATE_FORWARD,     ///< 前进
-    STATE_BACKWARD,    ///< 反向旋转状态（扭矩为负）
+    STATE_BACKWARD,    ///< 后退
 } MotorState;  
   
 typedef enum
@@ -43,7 +46,7 @@ typedef enum
 typedef struct DiffData
 {
     ControlMode mode ;               // 控制模式
-    MotorState state;                //当前状态机状态        
+    MotorState motor_state[4];       //当前车辆状态    
     float desired_speed;             // 期望速度    
     float desired_curvature;         // 期望曲率
     float left_motor_speed;          // 当前左电机速度
@@ -67,8 +70,14 @@ typedef struct DiffData
     float out_left_motor_speed;      // 输出左电机速度
     float out_right_motor_speed;     // 输出右电机速度
     float out_torq[4];               //4个电机扭矩
-    float max_Torq;                 // 最大扭矩限制   
+    float max_Torq;                  // 最大扭矩限制   
-    float min_Torq;                 // 最小扭矩限制      
+    float min_Torq;                  // 最小扭矩限制 
+    uint16_t  left_motor_feed_power;     // 左侧馈电功率 
+    uint16_t  right_motor_feed_power;    // 右侧馈电功率
+    uint8_t   left_motor_state;        //左侧电机状态，1刹车，0停下或加速  2驻车
+    uint8_t   right_motor_state;       //右侧电机状态，1刹车，0停下或加速 
+    uint16_t  max_feed_power;         //最大馈电功率     
+    
 } DiffData;
 
 

app/app_param_manage.h

@@ -53,6 +53,12 @@ extern "C"
     X(Ocrv_ol)  \
     X(minTorq)  \
     X(brk_rev)  \
+    X(mot_kp)  \
+    X(mot_ki)  \
+    X(mot_kd)  \
+    X(mot_il)  \
+    X(mot_ol)  \
+    X(diff_sp) \
     X(test)
 
 // 定义一个包含所有参数名称的结构体

app/app_temp.c

@@ -184,6 +184,9 @@ static void tempProcess(void *signal_id)
     
     max_temp[0] = temp_data.current_temp[0];
     max_temp[1] = temp_data.current_temp[1];    
+    max_temp[2] = temp_data.current_temp[2];
+    max_temp[3] = temp_data.current_temp[3];     
+    
     
 //    printf("motor1 temp: %d, motor2 temp: %d\n", max_temp[0], max_temp[1]);  
     handleTemperatureAlarm(max_temp[0], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[0]);              
@@ -239,19 +242,19 @@ static void tempInput(void *signal_id)
     // 填充数据
     if (signal_id == &un_motor_temp1)
     {
-        temp_data.current_temp[0] = ( (int16_t)(un_motor_temp1.bit_data.controller_temp) - 40);//40偏移量
+        temp_data.current_temp[0] =  (int16_t)( (un_motor_temp1.bit_data.controller_temp) - 40 );//40偏移量
     }
     else if(signal_id == &un_motor_temp2)
     {
-        temp_data.current_temp[1] = ( (int16_t)(un_motor_temp2.bit_data.controller_temp) - 40);      
+        temp_data.current_temp[1] = (int16_t)( (un_motor_temp2.bit_data.controller_temp) - 40 );      
     }
     else if(signal_id == &un_motor_temp3)
     {
-        temp_data.current_temp[2] = ( (int16_t)(un_motor_temp3.bit_data.controller_temp) - 40);      
+        temp_data.current_temp[2] =  (int16_t)( (un_motor_temp3.bit_data.controller_temp) - 40 );      
     }    
     else if(signal_id == &un_motor_temp4)
     {
-        temp_data.current_temp[3] = ( (int16_t)(un_motor_temp4.bit_data.controller_temp) - 40);      
+        temp_data.current_temp[3] =  (int16_t)( (un_motor_temp4.bit_data.controller_temp) - 40 );      
     }    
     else{}
 }

interface_can.c

@@ -323,7 +323,7 @@ void flexcan_Receive_callback_1(flexcan_handle_t *handle,
                     un_motor_input3.arr[i] = buf->dataBuffer[i];
                 }
                 //<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
-                publishMessage(&un_motor_input3, 1);               
+//                publishMessage(&un_motor_input3, 1);    //<2F>޸<EFBFBD>Ϊ           
             } 
             else if( LEFT_REAR_MOTOR2_INPUT2 == (buf->id) )
             {
@@ -407,7 +407,7 @@ void flexcan_Receive_callback_2(flexcan_handle_t *handle,
                     un_motor_input4.arr[i] = buf->dataBuffer[i];
                 }
                 //<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
-                publishMessage(&un_motor_input4, 1);               
+//                publishMessage(&un_motor_input4, 1);               
             } 
             else if( RIGHT_REAR_MOTOR_INPUT2 == (buf->id) )
             {
@@ -1100,7 +1100,7 @@ static void processMotorOutput3(void *signal_id)
    (void)signal_id;  // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 
    CAN_Send_Msg(&can_handle_1, LEFT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1.arr[8], 8, 17);//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-   CAN_Send_Msg(&can_handle_1, LEFT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3.arr[8], 8, 17);//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>   
+   CAN_Send_Msg(&can_handle_1, LEFT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3.arr[8], 8, 18);//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>   
    
 }
 
@@ -1108,13 +1108,18 @@ static void processMotorOutput4(void *signal_id)
 {  
    (void)signal_id;  // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>  
    CAN_Send_Msg(&can_handle_2, RIGHT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2.arr[8], 8, 17);//<2F><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-   CAN_Send_Msg(&can_handle_2, RIGHT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4.arr[8], 8, 17);//<2F><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>   
+   CAN_Send_Msg(&can_handle_2, RIGHT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4.arr[8], 8, 18);//<2F><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>   
 }
 
 static void processKgfOutput1(void *signal_id)
 {  
    (void)signal_id;  // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>  
-   CAN_Send_Msg(&can_handle_0, 0x11000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_inf_can_kgf_output1, 8, 15);//KGF1 
+   
+   if(1 == ecu_online)//20250318 <20>޸<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>߲<EFBFBD><DFB2>ܷ<EFBFBD><DCB7>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>E3ͬʱ<CDAC><CAB1><EFBFBD><EFBFBD>
+   {
+       un_inf_can_kgf_output1.bit_data.can_rx5 = 0xE3;
+       CAN_Send_Msg(&can_handle_0, 0x11000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_inf_can_kgf_output1, 8, 15);//KGF1     
+   }     
 }
 
 static void processKgfOutput2(void *signal_id)
@@ -1171,7 +1176,7 @@ static void processUnGatherOutput(void *signal_id)
    (void)signal_id;  // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>  
    CAN_Send_Msg(&can_handle_0, 0x15000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_gather_output, 8, 18);//<2F>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD>  
 }
-
+//    un_gather_output.bit_data.vehicle_mode = power_data.current_state;
 
 
 static void processUltrasonicOutput(void *signal_id)
@@ -1232,7 +1237,7 @@ void canSendAll(void *signal_id)
     
 
     motor_power_cnt ++;
-    if(motor_power_cnt >= 1000)//1s<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
+    if(motor_power_cnt >= 10)//1s<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
     {
         motor_power_cnt = 0;
         processMotorOutput3(CanData);
@@ -1385,8 +1390,14 @@ void canInterfaceInit(void)
 
     subscribe(&can_timer_interface1, canSendAll);
     subscribe(&can_timer_interface2, canInterfaceTimerProcess);
+    
     subscribe(&un_motor_output1, processMotorOutput1);
     subscribe(&un_motor_output2, processMotorOutput2);    
+//    subscribe(&un_motor_output3, processMotorOutput3);
+//    subscribe(&un_motor_output4, processMotorOutput4);  
+
+
+    
     subscribe(&un_inf_can_kgf_output1, processKgfOutput1);
     subscribe(&un_inf_can_kgf_output2, processKgfOutput2);
     subscribe(&un_wheel_wpeed_output, processWheelSpeedOutput);
@@ -1396,12 +1407,6 @@ void canInterfaceInit(void)
     subscribe(&un_ultrasonic_output1, processUltrasonicOutput);  // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
     
     
-    
-
-
-    
-    
-    
 //    subscribe(&un_h_bridge_output2, processHBridgeOutput2);    
 //    subscribe(&un_lifter_output, processLifterOutput); 
     

main.c

@@ -115,7 +115,7 @@ int main(void)
     
     
     //打印版本号
-    printf("version: V1.72 \n");
+    printf("version: V1.75 \n");
     // 初始化框架 放在最前面，解决电机can发送信号累积不处理的问题。   
 
     testAppInit();