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

增加转向扭矩控制

Browse Source
This commit is contained in:
liumin
2025-07-08 21:07:11 +08:00
parent 1b9b848949
commit eefa6daf38
4 changed files with 1673 additions and 1560 deletions
Download Patch File Download Diff File Expand all files Collapse all files

209
app/app_differential_drive.c
View File

@@ -19,6 +19,43 @@ DiffData diff_data;
PID_t speed_pid;
PID_t yaw_rate_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;
}
// 设置电机输出
void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint16_t discharge_power)
{
@@ -29,35 +66,54 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint
float abs_right_rear_speed = 0;
// 档位
un_motor_output1.bit_data.gear = diff_data.state; // 1 表示前进，2 表示后退，0空挡
un_motor_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_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;
}
//增加系数以及偏移量
if(diff_data.state == 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]);
if(STATE_FORWARD == un_motor_output3.bit_data.gear)//把后两台电机反相
{
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;
un_motor_output3.bit_data.gear = STATE_BACKWARD;
}
else if(diff_data.state == STATE_BACKWARD)//倒挡直接修改为负扭矩发送出去
else if(STATE_BACKWARD == un_motor_output3.bit_data.gear)
{
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;
un_motor_output3.bit_data.gear = STATE_FORWARD;
}
else//空挡直接发0
else
{
abs_left_front_speed = 0;
abs_right_front_speed = 0;
abs_left_rear_speed = 0;
abs_right_rear_speed = 0;
un_motor_output3.bit_data.gear = STATE_INIT;
}
if(STATE_FORWARD == un_motor_output4.bit_data.gear)
{
un_motor_output4.bit_data.gear = STATE_BACKWARD;
}
else if(STATE_BACKWARD == un_motor_output4.bit_data.gear)
{
un_motor_output4.bit_data.gear = STATE_FORWARD;
}
else
{
un_motor_output4.bit_data.gear = STATE_INIT;
}
// 设置左右电机期望转速
// un_motor_output1.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_speed) + 30000); // 20240921 增加偏移量 30000
@@ -69,13 +125,18 @@ 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_rotation_speed = 30000;
un_motor_output2.bit_data.set_rotation_speed = 30000;
un_motor_output3.bit_data.set_rotation_speed = 30000;
un_motor_output4.bit_data.set_rotation_speed = 30000;
// 设置馈电功率
un_motor_output1.bit_data.feed_power = feed_power;
un_motor_output2.bit_data.feed_power = feed_power;
@@ -325,8 +386,7 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
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;
@@ -361,9 +421,9 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
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);
// 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);
@@ -490,7 +550,11 @@ void handleVehicleState(DiffData *ctx)
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;
@@ -503,15 +567,19 @@ void handleVehicleState(DiffData *ctx)
//-------------------------------------------
case STATE_FORWARD:
{
if ((ctx->desired_speed < 0.0f) && (ctx->speed == 0.0f))
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))
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; // 新增：其他情况保持前进状态
@@ -533,12 +601,38 @@ void handleVehicleState(DiffData *ctx)
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:;
}
}
@@ -580,6 +674,8 @@ static void diffProcess(void *signal_id)
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();
// 限制输出速度在当前速度和最大加速度计算出来的速度之间
@@ -707,6 +803,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,6 +841,9 @@ 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))//取速度较小的轮速
{
@@ -759,6 +860,8 @@ static void diffInput(void *signal_id)
diff_data.right_front_motor_speed = (float)((int16_t)(un_motor_input2.bit_data.speed - 30000)); // 20250502 1号控制器增加反相
diff_data.right_rear_motor_speed = (float)((int16_t)(un_motor_input4.bit_data.speed - 30000));
diff_data.right_rear_motor_speed = - diff_data.right_rear_motor_speed;//20250708 增加反相
if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速
{
motor_speed_temp = diff_data.right_rear_motor_speed;
@@ -787,18 +890,18 @@ static void diffInput(void *signal_id)
diff_data.desired_curvature = 0.0;
}
if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制
{
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，解决手柄回中，震荡问题
}
else
{
diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
}
// if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制
// {
// 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，解决手柄回中，震荡问题
// }
// else
// {
// diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
// }
diffProcess(&diff_data);//计算左右电机期望转速
}
@@ -814,8 +917,11 @@ void preChargeFinish(void *signal_id)
setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (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);
@@ -866,15 +972,14 @@ void diffParametersInit(void *signal_id)
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);
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));
@@ -882,7 +987,7 @@ void diffParametersInit(void *signal_id)
deffspeed = deffspeed * 0.01f;
deffcurvature = deffcurvature * 0.0001f;
printf("remote speed = %f, remote curvature = %f\n", deffspeed, deffcurvature);
printf(" car state = %d\n", diff_data.state);
timerStart(&diff_app_timer,1000,1);//1s调用一次
}

169
app/app_differential_drive.h
View File

@@ -1,84 +1,85 @@
#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 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, ///< 后退
STATE_STATIC_TURN ///< 原地转向
} 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

2853
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();