From 9a98344f854eefe96ea0fee3ee1dfe805167ef3d Mon Sep 17 00:00:00 2001 From: liumin Date: Wed, 23 Jul 2025 11:46:22 +0800 Subject: [PATCH] =?UTF-8?q?=E8=BD=AC=E9=80=9F=E6=A8=A1=E5=BC=8F=E5=A2=9E?= =?UTF-8?q?=E5=8A=A0=E9=A6=88=E7=94=B5PID?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/app_differential_drive.c | 523 ++++++++++++++--------------------- app/app_differential_drive.h | 25 +- app/app_temp.c | 11 +- interface_can.c | 16 +- 4 files changed, 232 insertions(+), 343 deletions(-) diff --git a/app/app_differential_drive.c b/app/app_differential_drive.c index 209410e..288d1af 100644 --- a/app/app_differential_drive.c +++ b/app/app_differential_drive.c @@ -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 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*(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 ); diff --git a/app/app_differential_drive.h b/app/app_differential_drive.h index ad81607..845e200 100644 --- a/app/app_differential_drive.h +++ b/app/app_differential_drive.h @@ -11,8 +11,8 @@ extern "C" #define SPEED_FITER_NUM 6 -#define SPEED_PID_MODE 0 -#define THROTTLE_PID_MODE 1 +#define SPEED_PID_MODE 1 +#define THROTTLE_PID_MODE 0 #define TURN_MIN_TOUQUE 1 //n*m @@ -23,9 +23,8 @@ extern "C" #define TORQUE_HYSTERESIS_THRESHOLD 0.3f -#define MOTOR_MODE TORQUE_MODE - - +#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)) @@ -71,15 +70,13 @@ typedef struct DiffData float out_left_motor_speed; // 输出左电机速度 float out_right_motor_speed; // 输出右电机速度 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; // 差速速度死区 + 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; diff --git a/app/app_temp.c b/app/app_temp.c index 971db69..9aa0fc6 100644 --- a/app/app_temp.c +++ b/app/app_temp.c @@ -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{} } diff --git a/interface_can.c b/interface_can.c index 9c835a2..7c99347 100644 --- a/interface_can.c +++ b/interface_can.c @@ -407,7 +407,7 @@ void flexcan_Receive_callback_2(flexcan_handle_t *handle, un_motor_input4.arr[i] = buf->dataBuffer[i]; } //ź - publishMessage(&un_motor_input4, 1); +// publishMessage(&un_motor_input4, 1); } else if( RIGHT_REAR_MOTOR_INPUT2 == (buf->id) ) { @@ -1176,7 +1176,7 @@ static void processUnGatherOutput(void *signal_id) (void)signal_id; // DZΪʹã CAN_Send_Msg(&can_handle_0, 0x15000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_gather_output, 8, 18);//ɼ } - +// un_gather_output.bit_data.vehicle_mode = power_data.current_state; static void processUltrasonicOutput(void *signal_id) @@ -1237,7 +1237,7 @@ void canSendAll(void *signal_id) motor_power_cnt ++; - if(motor_power_cnt >= 1000)//1sһ + if(motor_power_cnt >= 10)//1sһ { motor_power_cnt = 0; processMotorOutput3(CanData); @@ -1393,6 +1393,10 @@ void canInterfaceInit(void) subscribe(&un_motor_output1, processMotorOutput1); subscribe(&un_motor_output2, processMotorOutput2); +// subscribe(&un_motor_output3, processMotorOutput3); +// subscribe(&un_motor_output4, processMotorOutput4); + + subscribe(&un_inf_can_kgf_output1, processKgfOutput1); subscribe(&un_inf_can_kgf_output2, processKgfOutput2); @@ -1403,12 +1407,6 @@ void canInterfaceInit(void) subscribe(&un_ultrasonic_output1, processUltrasonicOutput); // - - - - - - // subscribe(&un_h_bridge_output2, processHBridgeOutput2); // subscribe(&un_lifter_output, processLifterOutput);