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-23 11:46:22 +08:00
parent cdc62d856d
commit 9a98344f85
4 changed files with 232 additions and 343 deletions
Download Patch File Download Diff File Expand all files Collapse all files

523
app/app_differential_drive.c
View File

@@ -19,8 +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;
PID_t left_feed_pid;
PID_t right_feed_pid;
/**
@@ -112,7 +112,7 @@ void handleVehicleState(MotorState *ctx, float speed, float torque)
// 设置电机输出
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;
@@ -121,44 +121,23 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint
float abs_right_rear_speed = 0;
// 档位
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 = (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;
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 == diff_data.motor_state[2])//把后两台电机反相
{
un_motor_output3.bit_data.gear = STATE_BACKWARD;
}
else if(STATE_BACKWARD == diff_data.motor_state[2])
{
un_motor_output3.bit_data.gear = STATE_FORWARD;
}
else
{
un_motor_output3.bit_data.gear = STATE_INIT;
}
if(STATE_FORWARD == diff_data.motor_state[3])
{
un_motor_output4.bit_data.gear = STATE_BACKWARD;
}
else if(STATE_BACKWARD == diff_data.motor_state[3])
{
un_motor_output4.bit_data.gear = STATE_FORWARD;
}
else
{
un_motor_output4.bit_data.gear = STATE_INIT;
}
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;
@@ -167,31 +146,22 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint
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.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;
}
@@ -322,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)
{
@@ -488,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;
@@ -509,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
}
@@ -608,80 +566,23 @@ static void diffProcess(void *signal_id)
// 限制输出速度在当前速度和最大加速度计算出来的速度之间
// output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt);
if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) )//手柄回中，速度小的时候清0
{
resetPidIntegral(&speed_pid);
resetPidIntegral(&yaw_rate_pid);
output_speed = 0;
output_yaw_rate = 0;
}
// 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;
// }
// 使用差速车辆动力学模型计算左右电机的期望速度
float out_torque[4] = {0,0,0,0};
// 使用差速车辆动力学模型计算左右电机的期望速度
computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque);
computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_torq[0]);
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);
// 设置电机输出
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)
@@ -689,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);
}
@@ -700,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_speed_diff);
un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_speed_diff);
// 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;
@@ -756,7 +626,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, 1, 20, 5, 5, 0.5);
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;
@@ -799,33 +669,56 @@ 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))//取速度较小的轮速
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号控制器增加反相
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;
}
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;
}
// 急停开关
@@ -844,30 +737,26 @@ 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");//参数读取设定最大扭矩
}
if((power_data.current_state == POWER_WORKING))//电机上电才运行
{
diffProcess(&diff_data);//计算左右电机期望转速
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;
resetPidIntegral(&yaw_rate_pid);
}
}
@@ -881,7 +770,7 @@ 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;
@@ -937,7 +826,7 @@ void diffParametersInit(void *signal_id)
}
// 设置曲率 PID 控制器的参数
setPidParameters(&Dec_front_speed_pid,
setPidParameters(&left_feed_pid,
getParam("mot_kp"),
getParam("mot_ki"),
getParam("mot_kd"),
@@ -946,27 +835,29 @@ void diffParametersInit(void *signal_id)
);
// 设置曲率 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
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");
if(0 == (float)getParam("diff_sp"))//20250711 防止参数为0，影响计算。
{
diff_data.diff_dead_zone = 2;
}
else
{
diff_data.diff_dead_zone = (float)getParam("diff_sp");//参数读取设定最大扭矩
}
// 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);
@@ -985,7 +876,7 @@ void diffParametersInit(void *signal_id)
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]);
printf(" left = %d,%d\n", diff_data.left_motor_state,diff_data.right_motor_state);
timerStart(&diff_app_timer,1000,1);//1s调用一次
}
@@ -1030,9 +921,9 @@ void diffAppInit(void)
);
// 初始化减速 PID 控制器
initializePid(&Dec_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
initializePid(&left_feed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
// 设置 PID 控制器的参数
setPidParameters(&Dec_front_speed_pid,
setPidParameters(&left_feed_pid,
getParam("mot_kp"),
getParam("mot_ki"),
getParam("mot_kd"),
@@ -1041,14 +932,14 @@ void diffAppInit(void)
);
// 初始化加速 PID 控制器
initializePid(&Acc_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
initializePid(&right_feed_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
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
);