liumin/test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

Compare commits

base: liumin:1b9b848949f6a4b5a46c1c299bf2f02dead0b7b5
Branches Tags
liumin:main liumin:7-8car-chassis liumin:Pan-Tilt-Unit liumin:gimbal liumin:6-car liumin:1car liumin:4while4turn liumin:转速模式
liumin:20251022-7-8号车程序
...
compare: liumin:9a98344f854eefe96ea0fee3ee1dfe805167ef3d
Branches Tags
liumin:7-8car-chassis liumin:Pan-Tilt-Unit liumin:main liumin:gimbal liumin:6-car liumin:1car liumin:4while4turn liumin:转速模式
liumin:20251022-7-8号车程序

3 Commits
1b9b848949 ... 9a98344f85

Author SHA1 Message Date
liumin
9a98344f85 转速模式增加馈电PID 2025-07-23 11:46:22 +08:00
liumin
cdc62d856d 扭矩模式，增加同侧差速PID 2025-07-17 20:20:32 +08:00
liumin
eefa6daf38 增加转向扭矩控制 2025-07-08 21:07:11 +08:00
7 changed files with 2282 additions and 2246 deletions
Expand all files Collapse all files

479
app/app_brake.c
View File

@@ -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 == &ethernet_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(&ethernet_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 == &ethernet_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(&ethernet_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");
}

710
app/app_differential_drive.c
View File

@@ -19,49 +19,125 @@ 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;
float abs_right_front_speed = 0;
float abs_left_rear_speed = 0;
float abs_right_rear_speed = 0;
float abs_right_rear_speed = 0;
// 档位
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;
un_motor_output4.bit_data.gear = diff_data.state;
//增加系数以及偏移量
if(diff_data.state == STATE_FORWARD)//根据挡位来判断，扭矩的正负
{
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.gear = (out_torq[0] >= 0) ? 1 : 2; // 1 表示前进，2 表示后退
un_motor_output2.bit_data.gear = (out_torq[1] >= 0) ? 1 : 2;
un_motor_output3.bit_data.gear = (out_torq[2] >= 0) ? 2 : 1; //20250717 2,3电机反相
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]);
abs_left_rear_speed = fabsf(out_torq[2]);
abs_right_rear_speed = fabsf(out_torq[3]);
// 设置左右电机期望转速
// un_motor_output1.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_speed) + 30000); // 20240921 增加偏移量 30000
// un_motor_output2.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_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_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_output2.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_front_speed );
un_motor_output3.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_rear_speed );
un_motor_output4.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_rear_speed );
// 设置扭矩
un_motor_output1.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
un_motor_output2.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
un_motor_output3.bit_data.set_torque = (uint16_t)(max_torque + 300) * 100; // 20240921 增加偏移量
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_output2.bit_data.feed_power = feed_power;
un_motor_output3.bit_data.feed_power = feed_power;
un_motor_output4.bit_data.feed_power = feed_power;
un_motor_output1.bit_data.feed_power = left_feed_power;
un_motor_output2.bit_data.feed_power = right_feed_power;
un_motor_output3.bit_data.feed_power = left_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 基于转速反比的双电机扭矩分配函数
* @param rpm1 电机1当前转速（单位：rpm）
* @param rpm2 电机2当前转速（单位：rpm）
* @param total_torque 系统总需求扭矩（单位：Nm）
* @param torque1 [out] 电机1分配到的扭矩（单位：Nm）
* @param torque2 [out] 电机2分配到的扭矩（单位：Nm）
* @note 分配原则：转速越高的电机分配扭矩越小，确保负载均衡
* @brief 判断减速状态（最简逻辑） 如果同向或者有一个为0，或者都为0，那么判断绝对值大小，如果期望绝对值小于当前绝对值，那就为减速
* @param target_speed 期望速度（带方向）
* @param current_speed 当前速度（带方向）
* @return 1:减速, 0:加速或保持
*/
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 (fabs(total_torque) < 0.001f) {
*torque1 = 0.0f;
*torque2 = 0.0f;
return;
// 特殊处理双零状态,双零表示刹车
if ( (target_speed == 0.0f && current_speed == 0.0f) )//如果又减速的话也刹车 //|| (0 != des_yaw_rate)
{
return 2; // 驻车
}
// // 保护条件：当两电机均静止时采用平均分配策略
// if (fabs(rpm1) < 0.001f && fabs(rpm2) < 0.001f) {
// *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);
}
// 核心逻辑：方向相反 或 (同向/含零且期望绝对值 < 当前绝对值)
return (signbit(target_speed) != signbit(current_speed)) ||
(fabs(target_speed) < fabs(current_speed));
}
/**
* @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,15 +325,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;
linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
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;
left_speed_mps = linear_velocity_x + yaw_rate;
right_speed_mps = linear_velocity_x - yaw_rate;
//扭矩分配
if(max_torque < left_speed_mps)
{
@@ -353,36 +360,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
// 限制线速度和偏航率
@@ -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 right_speed_mps = linear_velocity_x + rotational_velocity;
float left_speed_mps = linear_velocity_x + rotational_velocity; //20250316 为解决原地转向和直行转向相同，所以把左右输出的速度交换
float right_speed_mps = linear_velocity_x - rotational_velocity;
// 计算轮子周长
float wheel_circumference = (float)getParam("whl_dia") * M_PI;
@@ -415,21 +411,77 @@ 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转
{
left_motor_rpm = 0;
}
if (fabsf(right_motor_rpm) < 50)//速度慢所以设置位10转
{
right_motor_rpm = 0;
}
// if (fabsf(left_motor_rpm) < 20)//速度慢所以设置位10转
// {
// left_motor_rpm = 0;
// }
// if (fabsf(right_motor_rpm) < 20)//速度慢所以设置位10转
// {
// right_motor_rpm = 0;
// }
// 左边电机方向反一下，因为电机安装反了，返回来的数据也要反一下
// left_motor_rpm = -left_motor_rpm;
// 返回计算结果
*left_motor_speed = left_motor_rpm;
*right_motor_speed = right_motor_rpm;
motor_speed[0] = left_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;
}
}
}
// 差速处理函数
@@ -571,65 +555,34 @@ 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);
resetPidIntegral(&yaw_rate_pid);
output_speed = 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;
// }
// }
// if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) )//手柄回中，速度小的时候清0
// {
// resetPidIntegral(&speed_pid);
// resetPidIntegral(&yaw_rate_pid);
// output_speed = 0;
// output_yaw_rate = 0;
// }
// 使用差速车辆动力学模型计算左右电机的期望速度
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)
@@ -637,8 +590,7 @@ static void diffProcess(void *signal_id)
publishMessage(&un_motor_output1, 1);
publishMessage(&un_motor_output2, 1);
publishMessage(&un_motor_output3, 1);
publishMessage(&un_motor_output4, 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_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);
}
/******************************************************************************
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_motor_speed = motor_speed_temp;
diff_data.left_rear_motor_speed = 0;
}
// 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_front_motor_speed = (float)((int16_t)(un_motor_input2.bit_data.speed - 30000)); // 20250502 1号控制器增加反相
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_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 增加反相
diff_data.right_motor_speed = motor_speed_temp;
if( fabs(diff_data.right_front_motor_speed) < 20)//速度死区
{
diff_data.right_front_motor_speed = 0;
}
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");//参数读取设定最大扭矩
}
diffProcess(&diff_data);//计算左右电机期望转速
if((power_data.current_state == POWER_WORKING))//电机上电才运行
{
diffProcess(&diff_data);//计算左右电机期望转速
}
else
{
resetPidIntegral(&speed_pid);
resetPidIntegral(&yaw_rate_pid);
}
}
@@ -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_output2.bit_data.gear = 0;
un_motor_output1.bit_data.gear = 0; // 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,31 +824,63 @@ 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);
// 设置曲率 PID 控制器的参数
setPidParameters(&left_feed_pid,
getParam("mot_kp"),
getParam("mot_ki"),
getParam("mot_kd"),
getParam("mot_il"),
getParam("mot_ol")
);
// 设置曲率 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
);
// 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("right_motor_speed = %f\n",diff_data.right_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("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));
// 单位转换
deffspeed = deffspeed * 0.01f;
deffcurvature = deffcurvature * 0.0001f;
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调用一次

177
app/app_differential_drive.h
View File

@@ -1,84 +1,93 @@
#ifndef APP_DIFFERENTIAL_DRIVE_H
#define APP_DIFFERENTIAL_DRIVE_H
#ifdef __cplusplus
extern "C"
{
#endif
#include "app_config.h"
#define SPEED_FITER_NUM 6
#define SPEED_PID_MODE 0
#define THROTTLE_PID_MODE 1
#define SPEED_MODE 0x01
#define TORQUE_MODE 0x02
#define MOTOR_MODE TORQUE_MODE
#define ALPHA 0.1f // 滤波系数α∈[0.01,0.3]，0.2对应截止频率约10Hz（假设采样周期10ms）
#define LOWPASS_FILTER(speed, prev) (ALPHA * (speed) + (1 - ALPHA) * (prev))
// 状态机内部状态
typedef enum
{
STATE_INIT, ///< 初始状态（转速为0且等待扭矩方向判定）
STATE_FORWARD, ///< 正向旋转状态（扭矩为正）
STATE_BACKWARD, ///< 反向旋转状态（扭矩为负）
} MotorState;
typedef enum
{
MODE_MANUAL, // 手动模式
MODE_AUTO // 自动模式
} ControlMode;
typedef struct DiffData
{
ControlMode mode ; // 控制模式
MotorState state; //当前状态机状态
float desired_speed; // 期望速度
float desired_curvature; // 期望曲率
float left_motor_speed; // 当前左电机速度
float right_motor_speed; // 当前右电机速度
float left_front_motor_speed; // 当前左前电机速度
float right_front_motor_speed; // 当前右前电机速度
float left_rear_motor_speed; // 当前左后电机速度
float right_rear_motor_speed; // 当前右后电机速度
float speed; // 当前车速
float curvature; // 当前曲率
float yaw_rate; // 当前角速度
float desired_yaw_rate; // 期望角速度
float acceleration; // 当前加速度
float deceleration; // 当前减速度
float max_speed; // 最大速度
float desired_acceleration; // 期望加速度
float desired_deceleration; // 期望减速度
uint8_t emergency_stop_switch; // 急停开关
uint8_t remote_emergency_stop; // 遥控器急停开关
uint8_t emergency_stop_state; // 急停状态
float out_left_motor_speed; // 输出左电机速度
float out_right_motor_speed; // 输出右电机速度
float out_torq[4]; //4个电机扭矩
float max_Torq; // 最大扭矩限制
float min_Torq; // 最小扭矩限制
} DiffData;
// 声明外部变量
extern DiffData diff_data;
void diffAppInit(void);
#ifdef __cplusplus
}
#endif
#endif // APP_DIFFERENTIAL_DRIVE_H
#ifndef APP_DIFFERENTIAL_DRIVE_H
#define APP_DIFFERENTIAL_DRIVE_H
#ifdef __cplusplus
extern "C"
{
#endif
#include "app_config.h"
#define SPEED_FITER_NUM 6
#define SPEED_PID_MODE 1
#define THROTTLE_PID_MODE 0
#define TURN_MIN_TOUQUE 1 //n*m
#define SPEED_MODE 0x01
#define TORQUE_MODE 0x02
#define TORQUE_HYSTERESIS_THRESHOLD 0.3f
#define MOTOR_MODE SPEED_MODE
#define ALPHA 0.1f // 滤波系数α∈[0.01,0.3]，0.2对应截止频率约10Hz（假设采样周期10ms）
#define LOWPASS_FILTER(speed, prev) (ALPHA * (speed) + (1 - ALPHA) * (prev))
// 状态机内部状态
typedef enum
{
STATE_INIT, ///< 初始状态（转速为0且等待扭矩方向判定）
STATE_FORWARD, ///< 前进
STATE_BACKWARD, ///< 后退
} MotorState;
typedef enum
{
MODE_MANUAL, // 手动模式
MODE_AUTO // 自动模式
} ControlMode;
typedef struct DiffData
{
ControlMode mode ; // 控制模式
MotorState motor_state[4]; //当前车辆状态
float desired_speed; // 期望速度
float desired_curvature; // 期望曲率
float left_motor_speed; // 当前左电机速度
float right_motor_speed; // 当前右电机速度
float left_front_motor_speed; // 当前左前电机速度
float right_front_motor_speed; // 当前右前电机速度
float left_rear_motor_speed; // 当前左后电机速度
float right_rear_motor_speed; // 当前右后电机速度
float speed; // 当前车速
float curvature; // 当前曲率
float yaw_rate; // 当前角速度
float desired_yaw_rate; // 期望角速度
float acceleration; // 当前加速度
float deceleration; // 当前减速度
float max_speed; // 最大速度
float desired_acceleration; // 期望加速度
float desired_deceleration; // 期望减速度
uint8_t emergency_stop_switch; // 急停开关
uint8_t remote_emergency_stop; // 遥控器急停开关
uint8_t emergency_stop_state; // 急停状态
float out_left_motor_speed; // 输出左电机速度
float out_right_motor_speed; // 输出右电机速度
float out_torq[4]; //4个电机扭矩
float max_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;
// 声明外部变量
extern DiffData diff_data;
void diffAppInit(void);
#ifdef __cplusplus
}
#endif
#endif // APP_DIFFERENTIAL_DRIVE_H

298
app/app_param_manage.h
View File

@@ -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

11
app/app_temp.c
View File

@@ -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{}
}

2851
interface_can.c
View File

File diff suppressed because it is too large Load Diff

2
main.c
View File

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