@@ -19,8 +19,8 @@ DiffData diff_data;
PID_t speed_pid ;
PID_t yaw_rate_pid ;
PID_t Acc_front_sp eed_pid;
PID_t Dec_front_sp eed_pid;
PID_t left_f eed_pid;
PID_t right_f eed_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 = calculateTorqueO utput ( diff _data. motor_state [ 0 ] , out_torq [ 0 ] ) ; //根据挡位增加转矩方向
abs_right_front_speed = calculateTorqueO utput ( diff _data. motor_state [ 1 ] , out_torq [ 1 ] ) ;
abs_left_rear_speed = calculateTorqueO utput ( diff _data. motor_state [ 2 ] , out_torq [ 2 ] ) ;
abs_right_rear_speed = calculateTorqueO utput( diff _data. motor_state [ 3 ] , out_torq [ 3 ] ) ;
un_motor_o utput1 . bit _data. gear = ( out_torq [ 0 ] > = 0 ) ? 1 : 2 ; // 1 表示前进，
un_motor_o utput2 . bit _data. gear = ( out_torq [ 1 ] > = 0 ) ? 1 : 2 ;
un_motor_o utput3 . bit _data. gear = ( out_torq [ 2 ] > = 0 ) ? 2 : 1 ; //20250717 2,3电机反相
un_motor_o utput4 . 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
. . = ( ( ) ( front_speed ) + 30000 ) ;
. . = ( ( ) ( front_speed ) + 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当前转速（ ： ）
* @param rpm2 电机2当前转速（ ： ）
* @param total_torque 系统总需求扭矩（ ： ）
* @param torque1 [out] 电机1分配到的扭矩（ ： ）
* @param torque2 [out] 电机2分配到的扭矩（ ： ）
* @note 分配原则：转速越高的电机分配扭矩越小，确保负载均衡
* @brief 判断减速状态（最简逻辑） 如果同向或者有一个为0， ， ， ，
* @param target_speed 期望速度（带方向 ）
* @param current_speed 当前速度（带方向 ）
* @return 1:减速, 0:加速或保持
*/
void d istributeTorque ( float rpm1 , float rpm2 , float total_torque , float * torque1 , float * torque2 , float max_torque , float min_torqu e)
uint8_t is_Decelerating ( float target_speed , float current_speed , float des_yaw_rat e )
{
// 总扭矩为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;
// }
// 计算权重因子（与转速成反比关系）
// 注： ，
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 左轮速度（ ： ）
* @param speed_right 右轮速度（ ： ）
* @param torque_left 左轮扭矩指针（ ： ）
* @param torque_right 右轮扭矩指针（ ： ）
* @param threshold 触发调整的速差阈值（单位同轮速）
* @param k 扭矩调整系数（ ， ）
* @note 函数会直接修改传入的扭矩值，
*/
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 ) {
// 左轮过快时减少左扭矩，
* torque_left = fmaxf ( * torque_left - torque_reduction , 0.0f ) ;
} else {
// 右轮过快时减少右扭矩，
* 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转时，
( fabsf ( left_motor_rpm ) < 50 )
{
= 0 ;
}
( fabsf ( right_motor_rpm ) < 50 )
{
= 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= righ t_motor_rpm;
[ 0 ] = left_motor_rpm ; //加速状态，没有负扭矩，要么前进加速要么后退加速
[ 2 ] = lef t_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 = = . ) & & ( 0 = = . ) ) ，
{
( & ) ;
( & ) ;
= 0 ;
= 0 ;
}
// if( (0 == diff_data. desired_yaw_rate) && (0 == diff_data. desired_speed) ) //手柄回中，
// {
// 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倍，
{
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倍，
{
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 ; //因为每一个电机都是相同的扭矩，
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 ) ;
. . = ( uint16_t ) ( int16_t ) ( . ) ;
. . = ( uint16_t ) ( int16_t ) ( . ) ;
// 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 , , 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"); //参数读取设定最大扭矩
// }
( . = = 1 )
{
. = 5 ;
}
if ( ( 0 = = . ) & & ( 0 = = . ) & & ( . > - 100 ) & & ( . < 100 ) & & ( ( ( . > - 100 ) & & ( . < 100 ) ) ) ) ，
{
. = 5 ; ， ，
}
{
. = ( ) ( " 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_sp eed_pid,
setPidParameters ( & left_f eed_pid,
getParam ( " mot_kp " ) ,
getParam ( " mot_ki " ) ,
getParam ( " mot_kd " ) ,
@@ -946,27 +835,29 @@ void diffParametersInit(void *signal_id)
) ;
// 设置曲率 PID 控制器的参数
setPidParameters ( & Acc_front_sp eed_pid,
Dec_front_sp eed_pid. kp ,
Dec_front_sp eed_pid. ki ,
Dec_front_sp eed_pid. kd ,
Dec_front_sp eed_pid. integral_limit ,
Dec_front_sp eed_pid. output_limit
setPidParameters ( & right_f eed_pid,
left_f eed_pid. kp ,
left_f eed_pid. ki ,
left_f eed_pid. kd ,
left_f eed_pid. integral_limit ,
left_f eed_pid. output_limit
) ;
. = ( ) ( " minTorq " ) ;
. = ( float ) getParam ( " maxTorq " ) ;
if ( 0 = = ( floa t) 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 . [ 1 ] diff_data . motor_state [ 2 ] , diff_data . [ 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_sp eed_pid, PID_MODE_DERIVATIVE_CALC , 0.0001f ) ;
initializePid ( & left_f eed_pid, PID_MODE_DERIVATIVE_CALC , 0.0001f ) ;
// 设置 PID 控制器的参数
setPidParameters ( & Dec_front_sp eed_pid,
setPidParameters ( & left_f eed_pid,
getParam ( " mot_kp " ) ,
getParam ( " mot_ki " ) ,
getParam ( " mot_kd " ) ,
@@ -1041,14 +932,14 @@ void diffAppInit(void)
) ;
// 初始化加速 PID 控制器
initializePid ( & Acc_front_sp eed_pid, PID_MODE_DERIVATIVE_CALC , 0.0001f ) ;
initializePid ( & right_f eed_pid, PID_MODE_DERIVATIVE_CALC , 0.0001f ) ;
// 设置 PID 控制器的参数
setPidParameters ( & Acc_front_sp eed_pid,
Dec_front_sp eed_pid. kp ,
Dec_front_sp eed_pid. ki ,
Dec_front_sp eed_pid. kd ,
Dec_front_sp eed_pid. integral_limit ,
Dec_front_sp eed_pid. output_limit
setPidParameters ( & right_f eed_pid,
left_f eed_pid. kp ,
left_f eed_pid. ki ,
left_f eed_pid. kd ,
left_f eed_pid. integral_limit ,
left_f eed_pid. output_limit
) ;
