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

扭矩模式，增加同侧差速PID

Browse Source
This commit is contained in:
liumin
2025-07-17 20:20:32 +08:00
parent eefa6daf38
commit cdc62d856d
5 changed files with 635 additions and 601 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
app/app_brake.c
View File

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

402
app/app_differential_drive.c
View File

@@ -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;
/**
@@ -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:; // 异常处理
}
}
// 设置电机输出
@@ -66,33 +121,19 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint
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(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];
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)//把后两台电机反相
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,13 +423,17 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
}
#if THROTTLE_PID_MODE
float max_torque = (float)getParam("maxTorq");
float left_speed_mps = 0.0f;
float right_speed_mps = 0.0f;
float max_torque = diff_data.max_Torq;
linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
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)
@@ -414,35 +459,24 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
else{}
// printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate);
// adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5);
// adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5);
// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);
motor_speed[0] = left_speed_mps;
motor_speed[0] = left_speed_mps;//加速状态，没有负扭矩，要么前进加速要么后退加速
motor_speed[2] = left_speed_mps;
motor_speed[1] = right_speed_mps;
motor_speed[3] = right_speed_mps;
// adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5);
// 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);
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:;
}
}
// 差速处理函数
@@ -666,22 +598,16 @@ 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);
@@ -690,37 +616,67 @@ static void diffProcess(void *signal_id)
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");//参数读取设定最大扭矩
// }
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,7 +935,37 @@ void diffParametersInit(void *signal_id)
getParam("crv_ol")
);
}
// 设置曲率 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");
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);
@@ -987,12 +983,13 @@ void diffParametersInit(void *signal_id)
deffspeed = deffspeed * 0.01f;
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调用一次

17
app/app_differential_drive.h
View File

@@ -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,7 +47,7 @@ typedef enum
typedef struct DiffData
{
ControlMode mode ; // 控制模式
MotorState state; //当前车辆状态
MotorState motor_state[4]; //当前车辆状态
float desired_speed; // 期望速度
float desired_curvature; // 期望曲率
float left_motor_speed; // 当前左电机速度
@@ -70,6 +73,14 @@ typedef struct DiffData
float out_torq[4]; //4个电机扭矩
float max_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;

6
app/app_param_manage.h
View File

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

2
interface_can.c
View File

@@ -323,7 +323,7 @@ void flexcan_Receive_callback_1(flexcan_handle_t *handle,
un_motor_input3.arr[i] = buf->dataBuffer[i];
}
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
publishMessage(&un_motor_input3, 1);
// publishMessage(&un_motor_input3, 1); //<2F>޸<EFBFBD>Ϊ
}
else if( LEFT_REAR_MOTOR2_INPUT2 == (buf->id) )
{