From cdc62d856db3a2b878ee720fbee9fa5c40a9c71d Mon Sep 17 00:00:00 2001 From: liumin Date: Thu, 17 Jul 2025 20:20:32 +0800 Subject: [PATCH] =?UTF-8?q?=E6=89=AD=E7=9F=A9=E6=A8=A1=E5=BC=8F=EF=BC=8C?= =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=90=8C=E4=BE=A7=E5=B7=AE=E9=80=9FPID?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/app_brake.c | 479 +++++++++++++++++------------------ app/app_differential_drive.c | 436 ++++++++++++++++--------------- app/app_differential_drive.h | 21 +- app/app_param_manage.h | 298 +++++++++++----------- interface_can.c | 2 +- 5 files changed, 635 insertions(+), 601 deletions(-) diff --git a/app/app_brake.c b/app/app_brake.c index f26d135..eb882c4 100644 --- a/app/app_brake.c +++ b/app/app_brake.c @@ -1,241 +1,238 @@ -#include "app_config.h" -#include "interface.h" -#include "app_frm_monitor.h" -#include "app_frm_signal.h" -#include "app_frm_timer.h" -#include "app_param_manage.h" - -#include "app_power.h" - -#include "app_brake.h" -#include "app_differential_drive.h" - -// 使用内联函数 -static inline uint8_t setBrakeOn(void) { return 1; } -static inline uint8_t setBrakeOff(void) { return 0; } - -BrakeSystem brake_data; - -// 判断是否需要刹车 -static uint8_t shouldApplyBrake() -{ - return (brake_data.emergency_stop_switch || - brake_data.remote_emergency_stop || - (brake_data.mode_signal == 0 && brake_data.remote_fault) || - (brake_data.mode_signal == 1 && brake_data.can_bus_fault));//20241021 修改不计算以太网故障 -// (brake_data.mode_signal == 1 && (brake_data.can_bus_fault || brake_data.ethernet_fault))); -} - -// 判断是否需要释放刹车 -static uint8_t shouldReleaseBrake() -{ - return (!brake_data.emergency_stop_switch && - !brake_data.remote_emergency_stop && - ((brake_data.mode_signal == 0 && !brake_data.remote_fault) || - (brake_data.mode_signal == 1 && !brake_data.can_bus_fault)));//20241021 修改不计算以太网故障 -// (brake_data.mode_signal == 1 && (!brake_data.can_bus_fault && !brake_data.ethernet_fault)))); -} - - - -// 输出处理函数 -static void brakeOutput(void *signal_id) -{ - (void)signal_id; - // 根据电机状态，填充发送数据结构，发送信号 - switch (brake_data.brake_motor_state) - { - case 1: // 电机前进状态 - un_h_bridge_output.bit_data.channel_01 = setBrakeOn(); - un_h_bridge_output.bit_data.channel_02 = setBrakeOff(); - un_h_bridge_output1.bit_data.channel_01 = setBrakeOn(); - un_h_bridge_output1.bit_data.channel_02 = setBrakeOff(); - printf("Brake: Motor forward\n"); - break; - - case 2: // 电机后退状态 - un_h_bridge_output.bit_data.channel_01 = setBrakeOff(); - un_h_bridge_output.bit_data.channel_02 = setBrakeOn(); - un_h_bridge_output1.bit_data.channel_01 = setBrakeOff(); - un_h_bridge_output1.bit_data.channel_02 = setBrakeOn(); - printf("Brake: Motor reverse\n"); - break; - - default: - un_h_bridge_output.bit_data.channel_01 = setBrakeOff(); - un_h_bridge_output.bit_data.channel_02 = setBrakeOff(); - un_h_bridge_output1.bit_data.channel_01 = setBrakeOff(); - un_h_bridge_output1.bit_data.channel_02 = setBrakeOff(); - printf("Brake: Motor off\n"); - break; - } - - publishMessage(&un_h_bridge_output, 1); - publishMessage(&un_inf_can_kgf_output1, 1); -} - - - -// 修改刹车定时器处理函数 -static void brakeTimerProcess(void *signal_id) -{ - (void)signal_id; - -//#ifdef OIL_BRAKE - switch (brake_data.state) - { - case BRAKE_STATE_IDLE: - 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; - } - else - { - brake_data.brake_motor_state = 2; - } - brakeOutput(NULL); - timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 0); - } - break; - - case BRAKE_STATE_BRAKE_ON: - 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; - } - else - { - brake_data.brake_motor_state = 1; - } - timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0); - } - break; - - case BRAKE_STATE_APPLYING_BRAKE: - if (!brake_data.brake_apply_timer.active) - { - brake_data.state = BRAKE_STATE_BRAKE_ON; - brake_data.brake_motor_state = 0; - brakeOutput(NULL); - brake_data.brake_position = 1; // 刹车位置：1表示刹车 - } - break; - - case BRAKE_STATE_RELEASING_BRAKE: - if (!brake_data.brake_release_timer.active) - { - brake_data.state = BRAKE_STATE_IDLE; - brake_data.brake_motor_state = 0; - brakeOutput(NULL); - brake_data.brake_position = 0; // 刹车位置：0表示未刹车 - } - break; - - default: - printf("ERROR: Unknown state\n"); - brake_data.state = BRAKE_STATE_IDLE; - break; - } - // 如果刹车位置有变化，存入EEPROM - if (brake_data.brake_position != brake_data.old_brake_position) - { - 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); - - } - timerStart(&brake_data.brake_timer, 100, 1); // 周期调用 -} - - -// 处理所有输入信号的函数 -static void brakeInput(void *signal_id) -{ -// BrakeSystem old_data = brake_data; // 定义并初始化old_data - - // 填充数据 - if (signal_id == &un_sw_sample) - { - brake_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch; - } - else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 - { - brake_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1; - brake_data.mode_signal = ((uint8_t)un_remote_control_input.bit_data.switch_c == 1) ? 1 : 0; - } - else if (signal_id == &can_fault_info) - { - brake_data.remote_fault = !can_fault_info.bit_data.remote_state; - brake_data.can_bus_fault = !can_fault_info.bit_data.motor1_state || !can_fault_info.bit_data.motor2_state || !can_fault_info.bit_data.navigator_state; - } - else if (signal_id == ðernet_fault_Info) - { - brake_data.ethernet_fault = !ethernet_fault_Info.bit_data.auto_state && !ethernet_fault_Info.bit_data.manual_state; - } - -} - -void paramUpdate(void *signal_id) -{ - brake_data.brake_direction = (uint8_t)getParam("brk_rev"); - timerStart(&brake_data.brake_param_timer, 1000, 1); -} - - - -// 修改APP模块的初始化函数 -void brakeAppInit(void) -{ - // 初始化 - memset(&brake_data, 0, sizeof(BrakeSystem)); - brake_data.state = BRAKE_STATE_IDLE; - // 初始化时恢复刹车位置 - brake_data.brake_position = (uint8_t)getParam("brk_pos"); - brake_data.old_brake_position = brake_data.brake_position; - // 根据刹车位置恢复刹车状态 - if (brake_data.brake_position == 1) - { - brake_data.state = BRAKE_STATE_BRAKE_ON; - } - else - { - brake_data.state = BRAKE_STATE_IDLE; - } - - // 初始化定时器 - timerInit(&brake_data.brake_timer); - timerInit(&brake_data.brake_apply_timer); - timerInit(&brake_data.brake_release_timer); - timerInit(&brake_data.brake_param_timer); - - // 订阅输入信号,处理刹车逻辑 - subscribe(&un_sw_sample, brakeInput); - subscribe(&un_remote_control_input, brakeInput); - subscribe(&can_fault_info, brakeInput); - subscribe(ðernet_fault_Info, brakeInput); - - - // 订阅定时器信号,用于状态机的定时处理 - subscribe(&brake_data.brake_timer, brakeTimerProcess); - subscribe(&brake_data.brake_apply_timer, brakeTimerProcess); - subscribe(&brake_data.brake_release_timer, brakeTimerProcess); - subscribe(&brake_data.brake_param_timer, paramUpdate); - - // 启动定时器，定期调用 brakeTimerProcess - timerStart(&brake_data.brake_timer, 500, 1); - timerStart(&brake_data.brake_param_timer, 1000, 1); - - printf("app_brake: initial OK \n"); -} +#include "app_config.h" +#include "interface.h" +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" + +#include "app_power.h" + +#include "app_brake.h" +#include "app_differential_drive.h" + +// 使用内联函数 +static inline uint8_t setBrakeOn(void) { return 1; } +static inline uint8_t setBrakeOff(void) { return 0; } + +BrakeSystem brake_data; + +// 判断是否需要刹车 +static uint8_t shouldApplyBrake() +{ + return (brake_data.emergency_stop_switch || + brake_data.remote_emergency_stop || + (brake_data.mode_signal == 0 && brake_data.remote_fault) || + (brake_data.mode_signal == 1 && brake_data.can_bus_fault));//20241021 修改不计算以太网故障 +// (brake_data.mode_signal == 1 && (brake_data.can_bus_fault || brake_data.ethernet_fault))); +} + +// 判断是否需要释放刹车 +static uint8_t shouldReleaseBrake() +{ + return (!brake_data.emergency_stop_switch && + !brake_data.remote_emergency_stop && + ((brake_data.mode_signal == 0 && !brake_data.remote_fault) || + (brake_data.mode_signal == 1 && !brake_data.can_bus_fault)));//20241021 修改不计算以太网故障 +// (brake_data.mode_signal == 1 && (!brake_data.can_bus_fault && !brake_data.ethernet_fault)))); +} + + + +// 输出处理函数 +static void brakeOutput(void *signal_id) +{ + (void)signal_id; + // 根据电机状态，填充发送数据结构，发送信号 + switch (brake_data.brake_motor_state) + { + case 1: // 电机前进状态 + un_h_bridge_output.bit_data.channel_01 = setBrakeOn(); + un_h_bridge_output.bit_data.channel_02 = setBrakeOff(); + un_h_bridge_output1.bit_data.channel_01 = setBrakeOn(); + un_h_bridge_output1.bit_data.channel_02 = setBrakeOff(); + printf("Brake: Motor forward\n"); + break; + + case 2: // 电机后退状态 + un_h_bridge_output.bit_data.channel_01 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_02 = setBrakeOn(); + un_h_bridge_output1.bit_data.channel_01 = setBrakeOff(); + un_h_bridge_output1.bit_data.channel_02 = setBrakeOn(); + printf("Brake: Motor reverse\n"); + break; + + default: + un_h_bridge_output.bit_data.channel_01 = setBrakeOff(); + un_h_bridge_output.bit_data.channel_02 = setBrakeOff(); + un_h_bridge_output1.bit_data.channel_01 = setBrakeOff(); + un_h_bridge_output1.bit_data.channel_02 = setBrakeOff(); + printf("Brake: Motor off\n"); + break; + } + + publishMessage(&un_h_bridge_output, 1); + publishMessage(&un_inf_can_kgf_output1, 1); +} + + + +// 修改刹车定时器处理函数 +static void brakeTimerProcess(void *signal_id) +{ + (void)signal_id; + +//#ifdef OIL_BRAKE + switch (brake_data.state) + { + case BRAKE_STATE_IDLE: + if (shouldApplyBrake()) + { + brake_data.state = BRAKE_STATE_APPLYING_BRAKE; + if( 0 == brake_data.brake_direction) + { + brake_data.brake_motor_state = 1; + } + else + { + brake_data.brake_motor_state = 2; + } + brakeOutput(NULL); + timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 0); + } + break; + + case BRAKE_STATE_BRAKE_ON: + if (shouldReleaseBrake() && power_data.current_state == POWER_WORKING) + { + brake_data.state = BRAKE_STATE_RELEASING_BRAKE; + + if( 0 == brake_data.brake_direction) + { + brake_data.brake_motor_state = 2; + } + else + { + brake_data.brake_motor_state = 1; + } + brakeOutput(NULL); + timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0); + } + break; + + case BRAKE_STATE_APPLYING_BRAKE: + if (!brake_data.brake_apply_timer.active) + { + brake_data.state = BRAKE_STATE_BRAKE_ON; + brake_data.brake_motor_state = 0; + brakeOutput(NULL); + brake_data.brake_position = 1; // 刹车位置：1表示刹车 + } + break; + + case BRAKE_STATE_RELEASING_BRAKE: + if (!brake_data.brake_release_timer.active) + { + brake_data.state = BRAKE_STATE_IDLE; + brake_data.brake_motor_state = 0; + brakeOutput(NULL); + brake_data.brake_position = 0; // 刹车位置：0表示未刹车 + } + break; + + default: + printf("ERROR: Unknown state\n"); + brake_data.state = BRAKE_STATE_IDLE; + break; + } + // 如果刹车位置有变化，存入EEPROM + if (brake_data.brake_position != brake_data.old_brake_position) + { + 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); + + } + timerStart(&brake_data.brake_timer, 100, 1); // 周期调用 +} + + +// 处理所有输入信号的函数 +static void brakeInput(void *signal_id) +{ +// BrakeSystem old_data = brake_data; // 定义并初始化old_data + + // 填充数据 + if (signal_id == &un_sw_sample) + { + brake_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch; + } + else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 + { + brake_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1; + brake_data.mode_signal = ((uint8_t)un_remote_control_input.bit_data.switch_c == 1) ? 1 : 0; + } + else if (signal_id == &can_fault_info) + { + brake_data.remote_fault = !can_fault_info.bit_data.remote_state; + brake_data.can_bus_fault = !can_fault_info.bit_data.motor1_state || !can_fault_info.bit_data.motor2_state || !can_fault_info.bit_data.navigator_state; + } + else if (signal_id == ðernet_fault_Info) + { + brake_data.ethernet_fault = !ethernet_fault_Info.bit_data.auto_state && !ethernet_fault_Info.bit_data.manual_state; + } + +} + +void paramUpdate(void *signal_id) +{ + brake_data.brake_direction = (uint8_t)getParam("brk_rev"); + timerStart(&brake_data.brake_param_timer, 1000, 1); +} + + + +// 修改APP模块的初始化函数 +void brakeAppInit(void) +{ + // 初始化 + memset(&brake_data, 0, sizeof(BrakeSystem)); + brake_data.state = BRAKE_STATE_IDLE; + // 初始化时恢复刹车位置 + brake_data.brake_position = (uint8_t)getParam("brk_pos"); + brake_data.old_brake_position = brake_data.brake_position; + // 根据刹车位置恢复刹车状态 + if (brake_data.brake_position == 1) + { + brake_data.state = BRAKE_STATE_BRAKE_ON; + } + else + { + brake_data.state = BRAKE_STATE_IDLE; + } + + // 初始化定时器 + timerInit(&brake_data.brake_timer); + timerInit(&brake_data.brake_apply_timer); + timerInit(&brake_data.brake_release_timer); + timerInit(&brake_data.brake_param_timer); + + // 订阅输入信号,处理刹车逻辑 + subscribe(&un_sw_sample, brakeInput); + subscribe(&un_remote_control_input, brakeInput); + subscribe(&can_fault_info, brakeInput); + subscribe(ðernet_fault_Info, brakeInput); + + + // 订阅定时器信号,用于状态机的定时处理 + subscribe(&brake_data.brake_timer, brakeTimerProcess); + subscribe(&brake_data.brake_apply_timer, brakeTimerProcess); + subscribe(&brake_data.brake_release_timer, brakeTimerProcess); + subscribe(&brake_data.brake_param_timer, paramUpdate); + + // 启动定时器，定期调用 brakeTimerProcess + timerStart(&brake_data.brake_timer, 500, 1); + timerStart(&brake_data.brake_param_timer, 1000, 1); + + printf("app_brake: initial OK \n"); +} diff --git a/app/app_differential_drive.c b/app/app_differential_drive.c index 567238c..209410e 100644 --- a/app/app_differential_drive.c +++ b/app/app_differential_drive.c @@ -19,7 +19,8 @@ DiffData diff_data; PID_t speed_pid; PID_t yaw_rate_pid; - +PID_t Acc_front_speed_pid; +PID_t Dec_front_speed_pid; /** @@ -43,7 +44,7 @@ float calculateTorqueOutput(uint8_t gear, float input_torque) else if (gear == STATE_BACKWARD) { output_torque = (-input_torque + OFFSET) * SCALE_FACTOR; - } + } else { output_torque = DEFAULT_VALUE; @@ -52,8 +53,62 @@ float calculateTorqueOutput(uint8_t gear, float input_torque) 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:; // 异常处理 + } +} // 设置电机输出 @@ -63,36 +118,22 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint float abs_left_front_speed = 0; float abs_right_front_speed = 0; float abs_left_rear_speed = 0; - float abs_right_rear_speed = 0; + float abs_right_rear_speed = 0; // 档位 - if(diff_data.state != STATE_STATIC_TURN) - { - un_motor_output1.bit_data.gear = diff_data.state; // 1 表示前进，2 表示后退，0空挡 - un_motor_output2.bit_data.gear = diff_data.state; - un_motor_output3.bit_data.gear = diff_data.state; // 1 表示前进，2 表示后退，0空挡 - un_motor_output4.bit_data.gear = diff_data.state; - } - else//原地转向状态的话，根据扭矩大小确定方向 - { - un_motor_output1.bit_data.gear = (out_torq[0] < 0) ? STATE_BACKWARD : STATE_FORWARD; - un_motor_output2.bit_data.gear = (out_torq[1] < 0) ? STATE_BACKWARD : STATE_FORWARD; - un_motor_output3.bit_data.gear = (out_torq[2] < 0) ? STATE_BACKWARD : STATE_FORWARD; - un_motor_output4.bit_data.gear = (out_torq[3] < 0) ? STATE_BACKWARD : STATE_FORWARD; - } - - - abs_left_front_speed = calculateTorqueOutput(un_motor_output1.bit_data.gear, out_torq[0]); //根据挡位增加转矩方向 - abs_right_front_speed = calculateTorqueOutput(un_motor_output2.bit_data.gear, out_torq[1]); - abs_left_rear_speed = calculateTorqueOutput(un_motor_output3.bit_data.gear, out_torq[2]); - abs_right_rear_speed = calculateTorqueOutput(un_motor_output4.bit_data.gear, out_torq[3]); - + abs_left_front_speed = calculateTorqueOutput(diff_data.motor_state[0], out_torq[0]); //根据挡位增加转矩方向 + abs_right_front_speed = calculateTorqueOutput(diff_data.motor_state[1], out_torq[1]); + abs_left_rear_speed = calculateTorqueOutput(diff_data.motor_state[2], out_torq[2]); + abs_right_rear_speed = calculateTorqueOutput(diff_data.motor_state[3], out_torq[3]); + + un_motor_output1.bit_data.gear = diff_data.motor_state[0]; + un_motor_output2.bit_data.gear = diff_data.motor_state[1]; - if(STATE_FORWARD == un_motor_output3.bit_data.gear)//把后两台电机反相 + if(STATE_FORWARD == diff_data.motor_state[2])//把后两台电机反相 { un_motor_output3.bit_data.gear = STATE_BACKWARD; } - else if(STATE_BACKWARD == un_motor_output3.bit_data.gear) + else if(STATE_BACKWARD == diff_data.motor_state[2]) { un_motor_output3.bit_data.gear = STATE_FORWARD; } @@ -101,11 +142,11 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint un_motor_output3.bit_data.gear = STATE_INIT; } - if(STATE_FORWARD == un_motor_output4.bit_data.gear) + if(STATE_FORWARD == diff_data.motor_state[3]) { un_motor_output4.bit_data.gear = STATE_BACKWARD; } - else if(STATE_BACKWARD == un_motor_output4.bit_data.gear) + else if(STATE_BACKWARD == diff_data.motor_state[3]) { un_motor_output4.bit_data.gear = STATE_FORWARD; } @@ -382,14 +423,18 @@ 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); - - float left_speed_mps = linear_velocity_x + yaw_rate; - float right_speed_mps = linear_velocity_x - yaw_rate; + yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque); + left_speed_mps = linear_velocity_x + yaw_rate; + right_speed_mps = linear_velocity_x - yaw_rate; + //扭矩分配 if(max_torque < left_speed_mps) { @@ -413,36 +458,25 @@ 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); +// 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); -// 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[3] = right_speed_mps; + + handleVehicleState(&diff_data.motor_state[0], diff_data.left_front_motor_speed, motor_speed[0]); //通过扭矩以及速度来判断挡位 + 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]); + // 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 // 限制线速度和偏航率 @@ -535,108 +569,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 if( (ctx->desired_speed == 0.0f) && (ctx->desired_curvature != 0) )//无速度有转向 - { - ctx->state = STATE_STATIC_TURN; - } - 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 if( (ctx->desired_speed == 0.0f) && (ctx->speed == 0.0f) && (ctx->desired_curvature != 0.0f) ) - { - ctx->state = STATE_STATIC_TURN; // 原地转向 - } - 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 if( (ctx->desired_speed == 0.0f) && (ctx->speed == 0.0f) && (ctx->desired_curvature != 0.0f) ) - { - ctx->state = STATE_STATIC_TURN; // 原地转向 - } - else - { - ctx->state = STATE_BACKWARD; // 新增：其他情况保持倒车状态 - } - break; - } - - //------------------------------------------- - // 原地转向状态 - //------------------------------------------- - case STATE_STATIC_TURN: - { - 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->state = STATE_BACKWARD; // 显式保持当前状态 - } - else - { - ctx->state = STATE_STATIC_TURN; // 原地转向 - } - break; - } - - default:; - } -} - - // 差速处理函数 @@ -665,23 +597,17 @@ 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); float output_yaw_rate = calculatePidOutput(&yaw_rate_pid, diff_data.desired_yaw_rate, diff_data.yaw_rate, 0.0f, dt); - - // 计算最大加速度，用函数计算 float max_acceleration = calculateMaxAcceleration(); // 限制输出速度在当前速度和最大加速度计算出来的速度之间 // 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 { resetPidIntegral(&speed_pid); @@ -689,38 +615,68 @@ static void diffProcess(void *signal_id) output_speed = 0; output_yaw_rate = 0; } + + // 使用差速车辆动力学模型计算左右电机的期望速度 + float out_torque[4] = {0,0,0,0}; + // 使用差速车辆动力学模型计算左右电机的期望速度 + computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque); + + if( ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍 + { + diff_data.left_speed_diff = diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed; + diff_data.left_diff_touue = calculatePidOutput(&Acc_front_speed_pid, 0.0f, diff_data.left_speed_diff, 0.0f, dt); //左侧转速差PID + } + else + { + diff_data.left_speed_diff = 0; + Acc_front_speed_pid.integral = 0; + diff_data.left_diff_touue = 0; + } + + + if( ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍 + { + diff_data.right_speed_diff = diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed; + diff_data.right_diff_touue = calculatePidOutput(&Dec_front_speed_pid, 0.0f, diff_data.right_speed_diff, 0.0f, dt); //左侧转速差PID + } + else + { + diff_data.right_speed_diff = 0; + Dec_front_speed_pid.integral = 0; + diff_data.right_diff_touue = 0; + } + + if(out_torque[0] > 0) + { + diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -2*out_torque[0], 2*out_torque[0]); + } + else + { + diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, 2*out_torque[0], -2*out_torque[0]); + } + + + if(out_torque[1] > 0) + { + diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -2*out_torque[1], 2*out_torque[1]); + } + else + { + diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, 2*out_torque[1], -2*out_torque[1]); + } + + + diff_data.out_torq[0] = (2*out_torque[0] + diff_data.left_diff_touue)/2.0f;//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。 + diff_data.out_torq[2] = (2*out_torque[0] - diff_data.left_diff_touue)/2.0f; + + diff_data.out_torq[1] = (2*out_torque[1] + diff_data.right_diff_touue)/2.0f; + diff_data.out_torq[3] = (2*out_torque[1] - diff_data.right_diff_touue)/2.0f; + + // 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], @@ -744,8 +700,8 @@ 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_right_out = (uint16_t)(int16_t)(diff_data.right_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_speed_diff); publishMessage(&diff_data, 1); @@ -800,7 +756,7 @@ static void diffInput(void *signal_id) diff_data.desired_speed = diff_data.desired_speed * 0.01f; diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f; // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 - diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5); + diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 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; @@ -843,8 +799,6 @@ static void diffInput(void *signal_id) 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))//取速度较小的轮速 { motor_speed_temp = diff_data.left_front_motor_speed; @@ -902,8 +856,20 @@ static void diffInput(void *signal_id) // { // diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩 // } - - diffProcess(&diff_data);//计算左右电机期望转速 + if((power_data.current_state == POWER_WORKING))//电机上电才运行 + { + diffProcess(&diff_data);//计算左右电机期望转速 + } + else + { + resetPidIntegral(&speed_pid); + resetPidIntegral(&yaw_rate_pid); + diff_data.motor_state[0] = STATE_INIT; + diff_data.motor_state[1] = STATE_INIT; + diff_data.motor_state[2] = STATE_INIT; + diff_data.motor_state[3] = STATE_INIT; + } + } @@ -969,14 +935,44 @@ void diffParametersInit(void *signal_id) getParam("crv_ol") ); } - diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩 - - 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); + // 设置曲率 PID 控制器的参数 + setPidParameters(&Dec_front_speed_pid, + getParam("mot_kp"), + getParam("mot_ki"), + getParam("mot_kd"), + getParam("mot_il"), + getParam("mot_ol") + ); + + // 设置曲率 PID 控制器的参数 + setPidParameters(&Acc_front_speed_pid, + Dec_front_speed_pid.kp, + Dec_front_speed_pid.ki, + Dec_front_speed_pid.kd, + Dec_front_speed_pid.integral_limit, + Dec_front_speed_pid.output_limit + ); + + diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩 + diff_data.max_Torq = (float)getParam("maxTorq"); - 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); + 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("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("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("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]); @@ -985,14 +981,15 @@ void diffParametersInit(void *signal_id) float deffcurvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature)); // 单位转换 deffspeed = deffspeed * 0.01f; - deffcurvature = deffcurvature * 0.0001f; + deffcurvature = deffcurvature * 0.0001f; - printf(" car state = %d\n", diff_data.state); + printf("remote_speed: %f, remote_yaw_rate: %f\n", deffspeed, deffcurvature); + + printf(" car state = %d,%d,%d,%d\n", diff_data.motor_state[0],diff_data.motor_state[1],diff_data.motor_state[2],diff_data.motor_state[3]); timerStart(&diff_app_timer,1000,1);//1s调用一次 } - // 差速初始化函数 void diffAppInit(void) { @@ -1032,6 +1029,29 @@ void diffAppInit(void) getParam("crv_ol") ); + // 初始化减速 PID 控制器 + initializePid(&Dec_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置 PID 控制器的参数 + setPidParameters(&Dec_front_speed_pid, + getParam("mot_kp"), + getParam("mot_ki"), + getParam("mot_kd"), + getParam("mot_il"), + getParam("mot_ol") + ); + + // 初始化加速 PID 控制器 + initializePid(&Acc_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置 PID 控制器的参数 + setPidParameters(&Acc_front_speed_pid, + Dec_front_speed_pid.kp, + Dec_front_speed_pid.ki, + Dec_front_speed_pid.kd, + Dec_front_speed_pid.integral_limit, + Dec_front_speed_pid.output_limit + ); + + subscribe(&diff_app_timer, diffParametersInit); timerStart(&diff_app_timer,1000,1);//1s调用一次 diff --git a/app/app_differential_drive.h b/app/app_differential_drive.h index 9afb129..ad81607 100644 --- a/app/app_differential_drive.h +++ b/app/app_differential_drive.h @@ -14,11 +14,15 @@ extern "C" #define SPEED_PID_MODE 0 #define THROTTLE_PID_MODE 1 - +#define TURN_MIN_TOUQUE 1 //n*m #define SPEED_MODE 0x01 #define TORQUE_MODE 0x02 + +#define TORQUE_HYSTERESIS_THRESHOLD 0.3f + + #define MOTOR_MODE TORQUE_MODE @@ -32,7 +36,6 @@ typedef enum STATE_INIT, ///< 初始状态（转速为0且等待扭矩方向判定） STATE_FORWARD, ///< 前进 STATE_BACKWARD, ///< 后退 - STATE_STATIC_TURN ///< 原地转向 } MotorState; typedef enum @@ -44,8 +47,8 @@ typedef enum typedef struct DiffData { ControlMode mode ; // 控制模式 - MotorState state; //当前车辆状态 - float desired_speed; // 期望速度 + MotorState motor_state[4]; //当前车辆状态 + float desired_speed; // 期望速度 float desired_curvature; // 期望曲率 float left_motor_speed; // 当前左电机速度 float right_motor_speed; // 当前右电机速度 @@ -69,7 +72,15 @@ typedef struct DiffData float out_right_motor_speed; // 输出右电机速度 float out_torq[4]; //4个电机扭矩 float max_Torq; // 最大扭矩限制 - float min_Torq; // 最小扭矩限制 + float min_Torq; // 最小扭矩限制 + + float left_speed_diff; // 左侧转速差 + float right_speed_diff; // 右侧转速差 + + float left_diff_touue; // 左侧扭矩差 + float right_diff_touue; // 右侧扭矩差 + float diff_dead_zone; // 差速速度死区 + } DiffData; diff --git a/app/app_param_manage.h b/app/app_param_manage.h index b4ccc48..1c71921 100644 --- a/app/app_param_manage.h +++ b/app/app_param_manage.h @@ -1,146 +1,152 @@ -#ifndef APP_PARAM_MANAGE_H -#define APP_PARAM_MANAGE_H - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "app_config.h" - - -// 定义参数列表宏 -#define PARAM_LIST \ - X(whl_bas) \ - X(max_rpm) \ - X(whl_dia) \ - X(max_acc) \ - X(spd_kp) \ - X(spd_ki) \ - X(spd_kd) \ - X(spd_il) \ - X(spd_ol) \ - X(crv_kp) \ - X(crv_ki) \ - X(crv_kd) \ - X(crv_il) \ - X(crv_ol) \ - X(brk_on) \ - X(brk_off) \ - X(maxTorq) \ - X(feedPwr) \ - X(dispPwr) \ - X(VehMass) \ - X(gRatio) \ - X(prCTime) \ - X(brk_pos) \ - X(pwr_sta) \ - X(high_sw) \ - X(stop_sw) \ - X(lightSt) \ - X(pwr_btn) \ - X(sleepTm) \ - X(wakeTm) \ - X(Ospd_kp) \ - X(Ospd_ki) \ - X(Ospd_kd) \ - X(Ospd_il) \ - X(Ospd_ol) \ - X(Ocrv_kp) \ - X(Ocrv_ki) \ - X(Ocrv_kd) \ - X(Ocrv_il) \ - X(Ocrv_ol) \ - X(minTorq) \ - X(brk_rev) \ - X(test) - -// 定义一个包含所有参数名称的结构体 -typedef struct { - #define X(name) const char* name; - PARAM_LIST - #undef X -} ParamNames; - -// 参数结构体，不能使用位域，不要超过E2的大小, 1K byte -typedef struct -{ - #define X(name) float name; - PARAM_LIST - #undef X -} ParamData; - -typedef union -{ - ParamData bit_data; // 使用定义的结构体变量名 - uint8_t arr[sizeof(ParamData)]; // 通过结构体类型确定大小 -} UnParamManager; - -// 定义信号操作类型 -typedef enum -{ - READ_OPERATION, - WRITE_OPERATION -} OperationType; - -// 定义信号数据结构 -typedef struct -{ - void *param_ptr; // 参数数据的指针 - OperationType type; // 操作类型 - size_t offset; // 参数在结构体中的偏移 - size_t size; // 参数大小 -} ParamSignal; - -#pragma pack(push, 1) - -typedef struct _StrParamRequest { - //-------------------------------------------------- - uint16_t frame_header; // 帧头 固定值0xFF80 (16位) - uint16_t frame_type; // 帧类型 固定值0x002A (16位) - uint16_t frame_length; // 帧长 根据参数数据的长度动态设置 (16位) - uint8_t accumulated; // 累加值 按帧累加 (8位) - uint16_t request_id; // 请求帧ID 请求ID 100表示读，101 表示写参数 (16位) - char param_name[256][8]; // 参数名称 标识要写入或读出的参数 - uint8_t data[256][4]; // 数据 用于写入或读出的参数值，一个参数最大4字节 (8位*4) - uint8_t crc; // CRC 按字节累加之和 取低8位 (8位) -} StrParamRequest; - -typedef union _UnParamRequest { - StrParamRequest bit_data; // 使用定义的结构体变量名 - unsigned char arr[sizeof(StrParamRequest)]; // 通过结构体类型确定大小 -} UnParamRequest; - -typedef struct { - UnParamRequest *param_request; // 指向 UnParamRequest 的指针 - uint32_t sender_ip; // 发送方的 IP 地址（使用标准的32位整数表示） - uint16_t sender_port; // 发送方的端口号（使用标准的16位整数表示） -} RequestContext; - -#pragma pack(pop) - -extern UnParamRequest un_param_request;// 声明用于参数响应的帧实例 -extern RequestContext request_context; - -// 声明全局信号实例 -extern ParamNames param_names; -extern UnParamManager param_manager; // 全局参数管理实例 -extern ParamSignal param_signal; -extern uint8_t read_write_e2_finished; -extern RequestContext request_send;// 待发送的参数请求信号 - -uint8_t access_eeprom(size_t offset, void *data, size_t size, OperationType type); -void paramAppInit(void); - -// 在适当的位置添加以下函数声明 -float getParam(const char *param_name); -uint8_t setParam(const char *param_name, float value); -void printParams(void); -void handleParamOp(void *data); -void OnParamSignal(void *data); - -#ifdef __cplusplus -} -#endif - -#endif // APP_PARAM_MANAGE_H +#ifndef APP_PARAM_MANAGE_H +#define APP_PARAM_MANAGE_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "app_config.h" + + +// 定义参数列表宏 +#define PARAM_LIST \ + X(whl_bas) \ + X(max_rpm) \ + X(whl_dia) \ + X(max_acc) \ + X(spd_kp) \ + X(spd_ki) \ + X(spd_kd) \ + X(spd_il) \ + X(spd_ol) \ + X(crv_kp) \ + X(crv_ki) \ + X(crv_kd) \ + X(crv_il) \ + X(crv_ol) \ + X(brk_on) \ + X(brk_off) \ + X(maxTorq) \ + X(feedPwr) \ + X(dispPwr) \ + X(VehMass) \ + X(gRatio) \ + X(prCTime) \ + X(brk_pos) \ + X(pwr_sta) \ + X(high_sw) \ + X(stop_sw) \ + X(lightSt) \ + X(pwr_btn) \ + X(sleepTm) \ + X(wakeTm) \ + X(Ospd_kp) \ + X(Ospd_ki) \ + X(Ospd_kd) \ + X(Ospd_il) \ + X(Ospd_ol) \ + X(Ocrv_kp) \ + X(Ocrv_ki) \ + X(Ocrv_kd) \ + X(Ocrv_il) \ + 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) + +// 定义一个包含所有参数名称的结构体 +typedef struct { + #define X(name) const char* name; + PARAM_LIST + #undef X +} ParamNames; + +// 参数结构体，不能使用位域，不要超过E2的大小, 1K byte +typedef struct +{ + #define X(name) float name; + PARAM_LIST + #undef X +} ParamData; + +typedef union +{ + ParamData bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(ParamData)]; // 通过结构体类型确定大小 +} UnParamManager; + +// 定义信号操作类型 +typedef enum +{ + READ_OPERATION, + WRITE_OPERATION +} OperationType; + +// 定义信号数据结构 +typedef struct +{ + void *param_ptr; // 参数数据的指针 + OperationType type; // 操作类型 + size_t offset; // 参数在结构体中的偏移 + size_t size; // 参数大小 +} ParamSignal; + +#pragma pack(push, 1) + +typedef struct _StrParamRequest { + //-------------------------------------------------- + uint16_t frame_header; // 帧头 固定值0xFF80 (16位) + uint16_t frame_type; // 帧类型 固定值0x002A (16位) + uint16_t frame_length; // 帧长 根据参数数据的长度动态设置 (16位) + uint8_t accumulated; // 累加值 按帧累加 (8位) + uint16_t request_id; // 请求帧ID 请求ID 100表示读，101 表示写参数 (16位) + char param_name[256][8]; // 参数名称 标识要写入或读出的参数 + uint8_t data[256][4]; // 数据 用于写入或读出的参数值，一个参数最大4字节 (8位*4) + uint8_t crc; // CRC 按字节累加之和 取低8位 (8位) +} StrParamRequest; + +typedef union _UnParamRequest { + StrParamRequest bit_data; // 使用定义的结构体变量名 + unsigned char arr[sizeof(StrParamRequest)]; // 通过结构体类型确定大小 +} UnParamRequest; + +typedef struct { + UnParamRequest *param_request; // 指向 UnParamRequest 的指针 + uint32_t sender_ip; // 发送方的 IP 地址（使用标准的32位整数表示） + uint16_t sender_port; // 发送方的端口号（使用标准的16位整数表示） +} RequestContext; + +#pragma pack(pop) + +extern UnParamRequest un_param_request;// 声明用于参数响应的帧实例 +extern RequestContext request_context; + +// 声明全局信号实例 +extern ParamNames param_names; +extern UnParamManager param_manager; // 全局参数管理实例 +extern ParamSignal param_signal; +extern uint8_t read_write_e2_finished; +extern RequestContext request_send;// 待发送的参数请求信号 + +uint8_t access_eeprom(size_t offset, void *data, size_t size, OperationType type); +void paramAppInit(void); + +// 在适当的位置添加以下函数声明 +float getParam(const char *param_name); +uint8_t setParam(const char *param_name, float value); +void printParams(void); +void handleParamOp(void *data); +void OnParamSignal(void *data); + +#ifdef __cplusplus +} +#endif + +#endif // APP_PARAM_MANAGE_H diff --git a/interface_can.c b/interface_can.c index a356b85..9c835a2 100644 --- a/interface_can.c +++ b/interface_can.c @@ -323,7 +323,7 @@ void flexcan_Receive_callback_1(flexcan_handle_t *handle, un_motor_input3.arr[i] = buf->dataBuffer[i]; } //ź - publishMessage(&un_motor_input3, 1); +// publishMessage(&un_motor_input3, 1); //޸Ϊ } else if( LEFT_REAR_MOTOR2_INPUT2 == (buf->id) ) {