1189 lines
47 KiB
C
1189 lines
47 KiB
C
#include "app_config.h"
|
||
#include "interface.h"
|
||
|
||
#include "app_frm_monitor.h"
|
||
#include "app_frm_signal.h"
|
||
#include "app_frm_timer.h"
|
||
#include "app_param_manage.h"
|
||
#include "app_pid.h"
|
||
|
||
#include "app_differential_drive.h"
|
||
#include "app_brake.h"
|
||
#include "app_power.h"
|
||
|
||
#include "drive_rs04.h"
|
||
|
||
Timer diff_app_timer;
|
||
Timer diff_app_timer2;
|
||
|
||
Timer diff_app_timer3;
|
||
|
||
// 定义全局变量
|
||
DiffData diff_data;
|
||
|
||
PID_t speed_pid;
|
||
PID_t yaw_rate_pid;
|
||
|
||
PID_t Acc_front_speed_pid;
|
||
PID_t Dec_front_speed_pid;
|
||
|
||
|
||
/**
|
||
* @brief 根据挡位和输入转矩计算输出转矩值
|
||
* @param gear 挡位状态（STATE_FORWARD/STATE_BACKWARD/其他）
|
||
* @param input_torque 输入转矩值
|
||
* @return 处理后的转矩值（已包含偏移量和系数）
|
||
*/
|
||
float calculateTorqueOutput(uint8_t gear, float input_torque)
|
||
{
|
||
const float OFFSET = 300.0f; // 偏移量常量
|
||
const float SCALE_FACTOR = 100.0f; // 缩放系数
|
||
const float DEFAULT_VALUE = 30000.0f; // 默认输出值
|
||
|
||
float output_torque;
|
||
|
||
if (gear == STATE_FORWARD)
|
||
{
|
||
output_torque = (input_torque + OFFSET) * SCALE_FACTOR;
|
||
}
|
||
else if (gear == STATE_BACKWARD)
|
||
{
|
||
output_torque = (-input_torque + OFFSET) * SCALE_FACTOR;
|
||
}
|
||
else
|
||
{
|
||
output_torque = DEFAULT_VALUE;
|
||
}
|
||
|
||
return output_torque;
|
||
}
|
||
|
||
/**
|
||
* @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:; // 异常处理
|
||
}
|
||
}
|
||
|
||
|
||
// 设置电机输出
|
||
void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint16_t discharge_power)
|
||
{
|
||
float abs_left_front_speed = 0;
|
||
float abs_right_front_speed = 0;
|
||
float abs_left_rear_speed = 0;
|
||
float abs_right_rear_speed = 0;
|
||
|
||
static float previous_time2 = 0.0f;
|
||
|
||
float time1 = (float)getCurrentTime();
|
||
float dt = (time1 - previous_time2) / PERIOD_TICK;
|
||
previous_time2 = time1;
|
||
|
||
|
||
abs_left_front_speed = dynamic_current_limit(&diff_data.out_torq_last[0],-diff_data.out_torq[0],dt);
|
||
abs_right_front_speed = dynamic_current_limit(&diff_data.out_torq_last[1],diff_data.out_torq[1],dt);
|
||
abs_left_rear_speed = dynamic_current_limit(&diff_data.out_torq_last[2],-diff_data.out_torq[2],dt);
|
||
abs_right_rear_speed = dynamic_current_limit(&diff_data.out_torq_last[3],diff_data.out_torq[3],dt);
|
||
|
||
setMotorWrite(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1, IQ_REF_INDEX,abs_left_front_speed);
|
||
setMotorWrite(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2, IQ_REF_INDEX,abs_right_front_speed);
|
||
setMotorWrite(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3, IQ_REF_INDEX,abs_left_rear_speed);
|
||
setMotorWrite(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4, IQ_REF_INDEX,abs_right_rear_speed);
|
||
}
|
||
|
||
|
||
|
||
|
||
// 计算当前速度、角速度
|
||
uint8_t calculateCurrentSpeedYawRate(void)
|
||
{
|
||
// 获取轮子周长
|
||
float wheel_circumference = diff_data.wheel_radius * M_PI;
|
||
// 获取减速比
|
||
float gear_ratio = diff_data.gear_ratio;
|
||
if (fabsf(gear_ratio) < EPSILON)
|
||
{
|
||
return 0; // 避免除以0的情况
|
||
}
|
||
// 将电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比
|
||
|
||
|
||
float left_speed_mps = (diff_data.left_motor_speed * wheel_circumference) / (60.0f * gear_ratio);
|
||
float right_speed_mps = (diff_data.right_motor_speed * wheel_circumference) / (60.0f * gear_ratio);
|
||
|
||
// float left_speed_mps = 0;
|
||
// float right_speed_mps = 0;
|
||
|
||
// float left_speed_mps = 0;
|
||
// float right_speed_mps = 0;
|
||
|
||
// 计算当前速度
|
||
diff_data.speed = (left_speed_mps + right_speed_mps) / 2.0f;
|
||
// 计算速度差
|
||
float speed_diff = left_speed_mps - right_speed_mps;
|
||
// 计算角速度
|
||
float wheel_base = diff_data.wheel_base;
|
||
if (fabsf(wheel_base) < EPSILON)
|
||
{
|
||
return 0; // 避免除以0的情况
|
||
}
|
||
diff_data.yaw_rate = speed_diff / wheel_base;
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
// 计算加速度
|
||
float calculateAcceleration(float speed, float previous_speed, float dt)
|
||
{
|
||
if (fabs(dt) < EPSILON)
|
||
{
|
||
return 0; // 避免除以0的情况
|
||
}
|
||
float acceleration = (speed - previous_speed) / dt;
|
||
return acceleration;
|
||
}
|
||
|
||
// 计算减速度
|
||
float calculateDeceleration(float speed, float previous_speed, float dt)
|
||
{
|
||
if (fabs(dt) < EPSILON)
|
||
{
|
||
return 0; // 避免除以0的情况
|
||
}
|
||
float deceleration = calculateAcceleration(previous_speed, speed, dt); // 减速度就是负的加速度
|
||
return deceleration;
|
||
}
|
||
|
||
// 计算最大速度
|
||
float calculateMaxSpeed()
|
||
{
|
||
// 获取最大电机转速 (RPM)
|
||
float max_rpm = diff_data.max_motor_rpm ;
|
||
// 获取轮子周长
|
||
float wheel_circumference = diff_data.wheel_radius * M_PI;
|
||
// 获取减速比
|
||
float gear_ratio = diff_data.gear_ratio;
|
||
if (fabsf(gear_ratio) < EPSILON)
|
||
{
|
||
return 0; // 避免除以0的情况
|
||
}
|
||
// 将最大电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比
|
||
float max_speed = (max_rpm * wheel_circumference) / (60.0f * gear_ratio);
|
||
return max_speed;
|
||
}
|
||
|
||
// 计算最大加速度
|
||
float calculateMaxAcceleration(void)
|
||
{
|
||
// 获取车辆参数
|
||
float max_motor_torque = diff_data.max_Torq; // 最大电机扭矩
|
||
float vehicle_mass = diff_data.vehicle_mass; // 车辆质量
|
||
float wheel_radius = diff_data.wheel_radius / 2.0f; // 轮子半径
|
||
float gear_ratio = diff_data.gear_ratio; // 减速比
|
||
if (fabsf(wheel_radius) < EPSILON || fabsf(vehicle_mass) < EPSILON )
|
||
{
|
||
return 0; // 避免除以0的情况
|
||
}
|
||
// 减速比计算扭矩
|
||
float effective_torque = max_motor_torque * gear_ratio;
|
||
// 计算最大加速度
|
||
float max_acceleration = (effective_torque / wheel_radius) / vehicle_mass;
|
||
return max_acceleration;
|
||
}
|
||
|
||
// 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度
|
||
void calculateCurrentState(float dt)
|
||
{
|
||
static float previous_speed = 0.0f;
|
||
// 更新当前速度和当前角速度
|
||
calculateCurrentSpeedYawRate();
|
||
// 更新加速度、减速度等，根据需要计算
|
||
diff_data.acceleration = calculateAcceleration(diff_data.speed, previous_speed, dt);
|
||
diff_data.deceleration = calculateDeceleration(diff_data.speed, previous_speed, 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 分配原则：转速越高的电机分配扭矩越小，确保负载均衡
|
||
*/
|
||
void distributeTorque(float rpm1, float rpm2, float total_torque, float* torque1, float* torque2, float max_torque, float min_torque)
|
||
{
|
||
|
||
// 总扭矩为0时快速返回
|
||
if (fabs(total_torque) < 0.001f) {
|
||
*torque1 = 0.0f;
|
||
*torque2 = 0.0f;
|
||
return;
|
||
}
|
||
|
||
// // 保护条件：当两电机均静止时采用平均分配策略
|
||
// 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);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
/**
|
||
* @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)
|
||
{
|
||
// 防止速度过低导致不必要的计算
|
||
if (fabs(max_speed) < EPSILON)
|
||
{
|
||
motor_speed[0] = 0.0f;
|
||
motor_speed[1] = 0.0f;
|
||
motor_speed[2] = 0.0f;
|
||
motor_speed[3] = 0.0f;
|
||
return;
|
||
}
|
||
|
||
#if THROTTLE_PID_MODE
|
||
|
||
float left_speed_mps = 0.0f;
|
||
float right_speed_mps = 0.0f;
|
||
|
||
float max_torque = diff_data.max_Torq;//不需要限制，PID输出已经限制了
|
||
// linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
|
||
// yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
|
||
|
||
if( diff_data.min_Torq > fabs(linear_velocity_x) )//20250728 增加死区 解决手柄回中，不停车问题
|
||
{
|
||
linear_velocity_x = 0;
|
||
}
|
||
|
||
left_speed_mps = linear_velocity_x + yaw_rate;
|
||
right_speed_mps = linear_velocity_x - yaw_rate;
|
||
|
||
//扭矩分配
|
||
if(max_torque < left_speed_mps)
|
||
{
|
||
right_speed_mps = right_speed_mps - (left_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差
|
||
left_speed_mps = max_torque;
|
||
}
|
||
else if(-max_torque > left_speed_mps)
|
||
{
|
||
right_speed_mps = right_speed_mps - (left_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差
|
||
left_speed_mps = -max_torque;
|
||
}
|
||
else if(max_torque < right_speed_mps)
|
||
{
|
||
left_speed_mps = left_speed_mps - (right_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差
|
||
right_speed_mps = max_torque;
|
||
}
|
||
else if(-max_torque > right_speed_mps)
|
||
{
|
||
left_speed_mps = left_speed_mps - (right_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差
|
||
right_speed_mps = -max_torque;
|
||
}
|
||
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[2] = left_speed_mps;
|
||
|
||
motor_speed[1] = right_speed_mps;
|
||
motor_speed[3] = right_speed_mps;
|
||
|
||
// 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);
|
||
|
||
#else
|
||
|
||
// 限制线速度和偏航率
|
||
linear_velocity_x = constrain(linear_velocity_x, -max_speed, max_speed);
|
||
float max_yaw_rate = max_speed / diff_data.wheel_base / 2.0f);
|
||
yaw_rate = constrain(yaw_rate, -max_yaw_rate, max_yaw_rate);
|
||
|
||
// 计算旋转速度
|
||
float rotational_velocity = (diff_data.wheel_base / 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 wheel_circumference = diff_data.wheel_radius * M_PI;
|
||
|
||
// 将车辆左右线速度转换为轮子转速 (RPM)
|
||
float left_wheel_rpm = (left_speed_mps * 60.0f) / wheel_circumference;
|
||
float right_wheel_rpm = (right_speed_mps * 60.0f) / wheel_circumference;
|
||
|
||
// 获取减速比
|
||
float gear_ratio = diff_data.gear_ratio;
|
||
|
||
// 将轮子转速转换为电机转速，考虑减速比
|
||
float left_motor_rpm = left_wheel_rpm * gear_ratio;
|
||
float right_motor_rpm = right_wheel_rpm * gear_ratio;
|
||
|
||
// 限制电机的最大和最小转速
|
||
float max_motor_rpm = diff_data.max_motor_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;
|
||
}
|
||
// 左边电机方向反一下，因为电机安装反了，返回来的数据也要反一下
|
||
// left_motor_rpm = -left_motor_rpm;
|
||
// 返回计算结果
|
||
*left_motor_speed = left_motor_rpm;
|
||
*right_motor_speed = right_motor_rpm;
|
||
|
||
#endif
|
||
}
|
||
|
||
// 映射遥控器速度，分为死区、低速区和高速区。
|
||
float mapRemoteControlSpeed(
|
||
float input_speed,
|
||
float deadzone_limit,
|
||
float input_max,
|
||
float output_max,
|
||
float input_slow,
|
||
float output_slow
|
||
)
|
||
{
|
||
float output_speed = 0.0f;
|
||
// 获取输入速度的绝对值
|
||
float abs_input = fabsf(input_speed);
|
||
//diff_data.desired_speed, 0.1, 2, 10, 1, 5
|
||
|
||
// diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5);
|
||
|
||
if (abs_input < deadzone_limit + EPSILON)
|
||
{
|
||
output_speed = 0.0f;// 死区
|
||
}
|
||
else if (abs_input < input_slow + EPSILON)// 低速区
|
||
{
|
||
output_speed = (abs_input - deadzone_limit) * output_slow / (input_slow - deadzone_limit);//
|
||
}
|
||
else if (abs_input <= input_max + EPSILON)// 高速区
|
||
{
|
||
output_speed = output_slow + (abs_input - input_slow) * (output_max - output_slow) / (input_max - input_slow);// 0.2 + （3 - 0.5）* （15-0.2） / （17 - 0.5）
|
||
}
|
||
else // 超出范围
|
||
{
|
||
output_speed = output_max;
|
||
}
|
||
|
||
// 根据原始输入速度的符号恢复方向
|
||
if (input_speed < 0)
|
||
{
|
||
output_speed = -output_speed;
|
||
}
|
||
return output_speed;
|
||
}
|
||
|
||
|
||
|
||
// 差速处理函数
|
||
static void diffProcess(void *signal_id)
|
||
{
|
||
(void)signal_id;
|
||
static float previous_time1 = 0.0f;
|
||
|
||
float time1 = (float)getCurrentTime();
|
||
float dt = (time1 - previous_time1) / PERIOD_TICK;
|
||
previous_time1 = time1;
|
||
|
||
// 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度
|
||
calculateCurrentState(dt);
|
||
|
||
// 当速度小于1时，设定为原地转向 20250321 修改为考虑负号
|
||
if( (diff_data.desired_speed >= 0) && (diff_data.desired_speed <= 1.0f) )
|
||
{
|
||
diff_data.desired_yaw_rate = diff_data.desired_curvature * 1.0f;
|
||
}
|
||
else if( (diff_data.desired_speed < 0) && (diff_data.desired_speed >= -1.0f) )
|
||
{
|
||
diff_data.desired_yaw_rate = diff_data.desired_curvature * -1.0f;
|
||
}
|
||
else
|
||
{
|
||
diff_data.desired_yaw_rate = diff_data.desired_curvature * diff_data.desired_speed;
|
||
}
|
||
|
||
// 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) && ( 10 > fabs(diff_data.left_motor_speed) ) && ( 10 > fabs(diff_data.right_motor_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);
|
||
|
||
if( fabs(diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数
|
||
{
|
||
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( fabs(diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数
|
||
{
|
||
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)//根据大小来限定值为分配扭矩。最小就是0扭矩。
|
||
{
|
||
diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -out_torque[0], out_torque[0]);
|
||
}
|
||
else
|
||
{
|
||
diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, out_torque[0], -out_torque[0]);
|
||
}
|
||
|
||
|
||
if(out_torque[1] > 0)
|
||
{
|
||
diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -out_torque[1], out_torque[1]);
|
||
}
|
||
else
|
||
{
|
||
diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, out_torque[1], -out_torque[1]);
|
||
}
|
||
|
||
diff_data.out_torq[0] = (out_torque[0] + diff_data.left_diff_touue);//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。
|
||
diff_data.out_torq[2] = (out_torque[0] - diff_data.left_diff_touue);
|
||
|
||
diff_data.out_torq[1] = (out_torque[1] + diff_data.right_diff_touue);
|
||
diff_data.out_torq[3] = (out_torque[1] - diff_data.right_diff_touue);
|
||
|
||
out_torque[0] = constrain(out_torque[0], -diff_data.max_Torq, diff_data.max_Torq); //限定最大扭矩
|
||
out_torque[1] = constrain(out_torque[1], -diff_data.max_Torq, diff_data.max_Torq);
|
||
out_torque[2] = constrain(out_torque[2], -diff_data.max_Torq, diff_data.max_Torq);
|
||
out_torque[3] = constrain(out_torque[3], -diff_data.max_Torq, diff_data.max_Torq);
|
||
|
||
// 设置电机输出
|
||
setMotorOutput(&diff_data.out_torq[0],
|
||
diff_data.max_Torq,//
|
||
diff_data.feedPower,
|
||
diff_data.dispPower);
|
||
// 发布左右电机期望转速,电源在工作状态才能发送
|
||
// if (power_data.current_state == POWER_WORKING)
|
||
// {
|
||
publishMessage(&un_motor_output1, 1);
|
||
publishMessage(&un_motor_output2, 1);
|
||
|
||
// }
|
||
// else
|
||
// {
|
||
// motorDisable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1);
|
||
// motorDisable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2);
|
||
// motorDisable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3);
|
||
// motorDisable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4);
|
||
//
|
||
// publishMessage(&un_motor_output1, 1);
|
||
// publishMessage(&un_motor_output2, 1);
|
||
// }
|
||
|
||
|
||
un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(diff_data.speed*10);
|
||
un_can_debug_output.bit_data.desired_speed = (uint8_t)(int8_t)(diff_data.desired_speed*10);
|
||
|
||
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);
|
||
|
||
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;
|
||
}
|
||
else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据
|
||
{
|
||
diff_data.remote_emergency_stop = !(uint8_t)un_remote_control_input.bit_data.switch_b;
|
||
diff_data.mode = un_remote_control_input.bit_data.switch_c == 1 ? MODE_AUTO : MODE_MANUAL;
|
||
|
||
if (diff_data.mode == MODE_MANUAL)
|
||
{
|
||
diff_data.desired_speed = (float)((int16_t)(un_remote_control_input.bit_data.speed));
|
||
diff_data.desired_curvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature));
|
||
// 单位转换
|
||
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_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0.1, 2, 2, 1, 0.5);
|
||
|
||
diff_data.desired_curvature = -diff_data.desired_curvature;
|
||
|
||
if(diff_data.desired_speed >= 0)//20250320 增加根据速度大小来决定方向，解决后退时转弯反向的问题
|
||
{
|
||
diff_data.desired_curvature = diff_data.desired_curvature;
|
||
}
|
||
else
|
||
{
|
||
diff_data.desired_curvature = -diff_data.desired_curvature;
|
||
}
|
||
}
|
||
}
|
||
else if ( (signal_id == &un_manual_computer_input) && (diff_data.mode == MODE_AUTO) )
|
||
{
|
||
diff_data.desired_speed = (float)((int16_t)(un_manual_computer_input.bit_data.set_speed));
|
||
diff_data.desired_curvature = (float)((int16_t)(un_manual_computer_input.bit_data.set_curvature));
|
||
// 单位转换
|
||
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.2, 2, 10, 1, 5);//20250320 修改死区为0.2解决停不住的问题
|
||
diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1);
|
||
}
|
||
else if ( (signal_id == &un_auto_computer_input) && (diff_data.mode == MODE_AUTO) )
|
||
{
|
||
diff_data.desired_speed = (float)((int16_t)(un_auto_computer_input.bit_data.set_speed));
|
||
diff_data.desired_curvature = (float)((int16_t)(un_auto_computer_input.bit_data.set_curvature));
|
||
// 单位转换
|
||
diff_data.desired_speed = diff_data.desired_speed * 0.01f;
|
||
diff_data.desired_curvature = - diff_data.desired_curvature * 0.0001f;// 20241016 增加转弯反相
|
||
// 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
|
||
diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0, 5, 10, 2.5, 5);
|
||
diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1);
|
||
}
|
||
else{}
|
||
|
||
//速度输入处理 20251005 修改为定时处理
|
||
diff_data.left_front_motor_speed = -convertPhysical( SWAP_ENDIAN_16(un_motor_input1.rx_can_data.bit_data.current_velocity),-RS04_ANGULAR_VELOCITY_MAX,RS04_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ) * ANGULAR_VELOCITY_TO_RPM;//速度单位转换，rad/s转换为转/分钟
|
||
diff_data.right_front_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_motor_input2.rx_can_data.bit_data.current_velocity), -RS04_ANGULAR_VELOCITY_MAX,RS04_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ) * ANGULAR_VELOCITY_TO_RPM;//速度单位转换，rad/s转换为转/分钟
|
||
diff_data.left_rear_motor_speed = -convertPhysical( SWAP_ENDIAN_16(un_motor_input3.rx_can_data.bit_data.current_velocity),-RS04_ANGULAR_VELOCITY_MAX,RS04_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ) * ANGULAR_VELOCITY_TO_RPM;//速度单位转换，rad/s转换为转/分钟
|
||
diff_data.right_rear_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_motor_input4.rx_can_data.bit_data.current_velocity), -RS04_ANGULAR_VELOCITY_MAX,RS04_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ) * ANGULAR_VELOCITY_TO_RPM;//速度单位转换，rad/s转换为转/分钟
|
||
|
||
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;//左侧轮速 //motor_speed_temp
|
||
|
||
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 = motor_speed_temp;//右侧轮速
|
||
|
||
// 急停开关
|
||
diff_data.emergency_stop_state = (uint8_t)(diff_data.emergency_stop_switch == app_close() || diff_data.remote_emergency_stop == app_close());
|
||
|
||
// 如果急停被激活,强制设定速度为0,急停包括车上急停开关和遥控器急停开关
|
||
if (diff_data.emergency_stop_state == 1)
|
||
{
|
||
diff_data.desired_speed = 0.0;
|
||
diff_data.desired_curvature = 0.0;
|
||
}
|
||
// 遥控器断线，而且是在手动模式，期望值清0
|
||
if ( (diff_data.mode == MODE_MANUAL) && (0 == un_remote_control_input.bit_data.enable) )
|
||
{
|
||
diff_data.desired_speed = 0.0;
|
||
diff_data.desired_curvature = 0.0;
|
||
}
|
||
|
||
switch(diff_data.motor_init_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据
|
||
{
|
||
case 0:
|
||
// timerStart(&diff_app_timer3, 1000, 0); // 启动定时器，1s
|
||
diff_data.motor_init_state = 1;
|
||
break;
|
||
|
||
case 1:
|
||
if(diff_data.diff_cnt >= 10)//发送5次
|
||
{
|
||
diff_data.diff_cnt = 0;
|
||
diff_data.motor_init_state = 2;
|
||
}
|
||
else
|
||
{
|
||
diff_data.diff_cnt ++;
|
||
diff_data.motor_init_state = 1;
|
||
}
|
||
break;
|
||
|
||
case 2://模式设置
|
||
if(diff_data.diff_cnt >= 5)//发送5次
|
||
{
|
||
diff_data.diff_cnt = 0;
|
||
diff_data.motor_init_state = 3;
|
||
}
|
||
else
|
||
{
|
||
diff_data.diff_cnt ++;
|
||
diff_data.motor_init_state = 2;
|
||
|
||
setMotorMode(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1, CURRENT_MODE);
|
||
setMotorMode(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2, CURRENT_MODE);
|
||
setMotorMode(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3, CURRENT_MODE);
|
||
setMotorMode(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4, CURRENT_MODE);
|
||
|
||
publishMessage(&un_motor_output1, 1);
|
||
publishMessage(&un_motor_output2, 1);
|
||
|
||
// publishMessage(&un_motor_output3, 1);
|
||
// publishMessage(&un_motor_output4, 1);
|
||
}
|
||
break;
|
||
|
||
//------------------------------------------------------------------------------
|
||
case 3:
|
||
if(diff_data.diff_cnt >= 5)//发送5次
|
||
{
|
||
diff_data.diff_cnt = 0;
|
||
diff_data.motor_init_state = 4;
|
||
}
|
||
else
|
||
{
|
||
diff_data.diff_cnt ++;
|
||
diff_data.motor_init_state = 3;
|
||
|
||
motorEnable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1);
|
||
motorEnable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2);
|
||
motorEnable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3);
|
||
motorEnable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4);
|
||
|
||
publishMessage(&un_motor_output1, 1);
|
||
publishMessage(&un_motor_output2, 1);
|
||
// publishMessage(&un_motor_output3, 1);
|
||
// publishMessage(&un_motor_output4, 1);
|
||
}
|
||
break;
|
||
|
||
case 4:
|
||
diff_data.motor_init_state = 4;
|
||
diffProcess(&diff_data);//计算左右电机期望转速
|
||
break;
|
||
|
||
default:break;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
// 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))//电机上电才运行
|
||
// {
|
||
|
||
// diff_data.motor_init_state = 0;
|
||
// }
|
||
// else
|
||
// {
|
||
// resetPidIntegral(&speed_pid);
|
||
// resetPidIntegral(&yaw_rate_pid);
|
||
//
|
||
// diff_data.out_torq[0] = STATE_INIT;
|
||
// diff_data.out_torq[1] = STATE_INIT;
|
||
// diff_data.out_torq[2] = STATE_INIT;
|
||
// diff_data.out_torq[3] = STATE_INIT;
|
||
// }
|
||
timerStart(&diff_app_timer2,1,1);//1ms调用一次
|
||
}
|
||
|
||
|
||
|
||
// 预充完成处理函数
|
||
void preChargeFinish(void *signal_id)
|
||
{
|
||
(void)signal_id;
|
||
// setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr"));
|
||
//-------------------------------------------------------------------------------------------------------------------------------
|
||
switch(diff_data.motor_init_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据
|
||
{
|
||
case 0:
|
||
// timerStart(&diff_app_timer3, 1000, 0); // 启动定时器，1s
|
||
diff_data.motor_init_state = 1;
|
||
break;
|
||
|
||
case 1:
|
||
if(diff_data.diff_cnt >= 10)//发送5次
|
||
{
|
||
diff_data.diff_cnt = 0;
|
||
diff_data.motor_init_state = 2;
|
||
}
|
||
else
|
||
{
|
||
diff_data.diff_cnt ++;
|
||
diff_data.motor_init_state = 1;
|
||
}
|
||
break;
|
||
|
||
case 2://模式设置
|
||
if(diff_data.diff_cnt >= 5)//发送5次
|
||
{
|
||
diff_data.diff_cnt = 0;
|
||
diff_data.motor_init_state = 3;
|
||
}
|
||
else
|
||
{
|
||
diff_data.diff_cnt ++;
|
||
diff_data.motor_init_state = 2;
|
||
|
||
setMotorMode(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1, CURRENT_MODE);
|
||
setMotorMode(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2, CURRENT_MODE);
|
||
setMotorMode(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3, CURRENT_MODE);
|
||
setMotorMode(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4, CURRENT_MODE);
|
||
|
||
publishMessage(&un_motor_output1, 1);
|
||
publishMessage(&un_motor_output2, 1);
|
||
|
||
// publishMessage(&un_motor_output3, 1);
|
||
// publishMessage(&un_motor_output4, 1);
|
||
}
|
||
break;
|
||
|
||
//------------------------------------------------------------------------------
|
||
case 3:
|
||
if(diff_data.diff_cnt >= 5)//发送5次
|
||
{
|
||
diff_data.diff_cnt = 0;
|
||
diff_data.motor_init_state = 4;
|
||
}
|
||
else
|
||
{
|
||
diff_data.diff_cnt ++;
|
||
diff_data.motor_init_state = 3;
|
||
|
||
motorEnable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1);
|
||
motorEnable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2);
|
||
motorEnable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3);
|
||
motorEnable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4);
|
||
|
||
publishMessage(&un_motor_output1, 1);
|
||
publishMessage(&un_motor_output2, 1);
|
||
// publishMessage(&un_motor_output3, 1);
|
||
// publishMessage(&un_motor_output4, 1);
|
||
}
|
||
break;
|
||
|
||
case 4:
|
||
diff_data.motor_init_state = 4;
|
||
publishMessage(&un_motor_output1, 1);//一直使能防止 ，防止can报故障
|
||
publishMessage(&un_motor_output2, 1);
|
||
break;
|
||
|
||
default:break;
|
||
}
|
||
}
|
||
|
||
|
||
void diffParametersInit(void *signal_id)
|
||
{
|
||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||
|
||
if(diff_data.mode == MODE_AUTO)//20250504 自动模式PID
|
||
{
|
||
setPidParameters(&speed_pid,
|
||
getParam("Ospd_kp"),
|
||
getParam("Ospd_ki"),
|
||
getParam("Ospd_kd"),
|
||
getParam("Ospd_il"),
|
||
getParam("Ospd_ol")
|
||
);
|
||
|
||
setPidParameters(&yaw_rate_pid,
|
||
getParam("Ocrv_kp"),
|
||
getParam("Ocrv_ki"),
|
||
getParam("Ocrv_kd"),
|
||
getParam("Ocrv_il"),
|
||
getParam("Ocrv_ol")
|
||
);
|
||
}
|
||
else//手动模式
|
||
{
|
||
setPidParameters(&speed_pid,
|
||
getParam("spd_kp"),
|
||
getParam("spd_ki"),
|
||
getParam("spd_kd"),
|
||
getParam("spd_il"),
|
||
getParam("spd_ol")
|
||
);
|
||
|
||
setPidParameters(&yaw_rate_pid,
|
||
getParam("crv_kp"),
|
||
getParam("crv_ki"),
|
||
getParam("crv_kd"),
|
||
getParam("crv_il"),
|
||
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
|
||
);
|
||
|
||
|
||
if(0 == un_motor_input1.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
|
||
{
|
||
motorEnable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output5);
|
||
publishMessage(&un_motor_output5, 1);
|
||
}
|
||
|
||
if(0 == un_motor_input2.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
|
||
{
|
||
motorEnable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output6);
|
||
publishMessage(&un_motor_output6, 1);
|
||
}
|
||
|
||
if(0 == un_motor_input3.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
|
||
{
|
||
motorEnable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output7);
|
||
publishMessage(&un_motor_output7, 1);
|
||
}
|
||
|
||
if(0 == un_motor_input4.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
|
||
{
|
||
motorEnable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output8);
|
||
publishMessage(&un_motor_output8, 1);
|
||
}
|
||
|
||
|
||
diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩
|
||
diff_data.max_Torq = (float)getParam("maxTorq");
|
||
|
||
diff_data.feedPower = (uint16_t)getParam("feedPwr");
|
||
diff_data.dispPower = (uint16_t)getParam("dispPwr");
|
||
diff_data.vehicle_mass = (float)getParam("VehMass");
|
||
diff_data.wheel_radius = (float)getParam("whl_dia");
|
||
diff_data.gear_ratio = (float)getParam("gRatio");
|
||
diff_data.wheel_base = (float)getParam("whl_bas");
|
||
diff_data.max_motor_rpm = (float)getParam("max_rpm");
|
||
|
||
|
||
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);
|
||
printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
|
||
// printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
|
||
|
||
printf("LF_speed = %f,RF_speed = %f,LR_speed = %f,RR_speed = %f\n",diff_data.left_front_motor_speed,diff_data.right_front_motor_speed,diff_data.left_rear_motor_speed,diff_data.right_rear_motor_speed);
|
||
|
||
// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);
|
||
printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", diff_data.out_torq[0], diff_data.out_torq[1], diff_data.out_torq[2], diff_data.out_torq[3]);
|
||
|
||
float deffspeed = (float)((int16_t)(un_remote_control_input.bit_data.speed));
|
||
float deffcurvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature));
|
||
// 单位转换
|
||
deffspeed = deffspeed * 0.01f;
|
||
deffcurvature = deffcurvature * 0.0001f;
|
||
|
||
// printf("remote_speed: %f, remote_yaw_rate: %f\n", deffspeed, deffcurvature);
|
||
|
||
// printf(" motor_init_state = %d\n", diff_data.motor_init_state);
|
||
|
||
timerStart(&diff_app_timer,1000,1);//1s调用一次
|
||
}
|
||
|
||
|
||
|
||
// 差速初始化函数
|
||
void diffAppInit(void)
|
||
{
|
||
// 初始化 diff_data
|
||
memset(&diff_data, 0, sizeof(DiffData));
|
||
|
||
|
||
// timerInit(&brake_data.brake_timer);
|
||
|
||
|
||
// 订阅相关信号
|
||
subscribe(&un_sw_sample, diffInput); // 急停开关、高压开关
|
||
// subscribe(&un_motor_input1, diffInput);
|
||
// subscribe(&un_motor_input2, diffInput);
|
||
|
||
subscribe(&un_auto_computer_input, diffInput);
|
||
subscribe(&un_manual_computer_input, diffInput);
|
||
subscribe(&un_remote_control_input, diffInput);
|
||
subscribe(&power_data.pre_charge_finish, preChargeFinish);
|
||
|
||
// 初始化速度 PID 控制器
|
||
initializePid(&speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
|
||
// 设置速度 PID 控制器的参数
|
||
setPidParameters(&speed_pid,
|
||
getParam("spd_kp"),
|
||
getParam("spd_ki"),
|
||
getParam("spd_kd"),
|
||
getParam("spd_il"),
|
||
getParam("spd_ol")
|
||
);
|
||
|
||
// 初始化曲率 PID 控制器
|
||
initializePid(&yaw_rate_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
|
||
// 设置曲率 PID 控制器的参数
|
||
setPidParameters(&yaw_rate_pid,
|
||
getParam("crv_kp"),
|
||
getParam("crv_ki"),
|
||
getParam("crv_kd"),
|
||
getParam("crv_il"),
|
||
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调用一次
|
||
|
||
subscribe(&diff_app_timer2, diffInput);
|
||
timerStart(&diff_app_timer2,1,1);//1ms调用一次
|
||
|
||
printf("diffControl: diffAppInit OK \n");
|
||
}
|