Compare commits
2 Commits
7-8car-cha
...
1car
| Author | SHA1 | Date | |
|---|---|---|---|
| edd1dca567 | |||
| bb55e6618f |
BIN
boards/e3_176_ref/app_demo/eth-xip/sf - 快捷方式.lnk
Normal file
BIN
boards/e3_176_ref/app_demo/eth-xip/sf - 快捷方式.lnk
Normal file
Binary file not shown.
@@ -1,5 +1,6 @@
|
||||
{
|
||||
"files.associations": {
|
||||
"ADC.C": "cpp"
|
||||
"app_config.h": "c",
|
||||
"interface.h": "c"
|
||||
}
|
||||
}
|
||||
@@ -47,25 +47,36 @@ static void brakeOutput(void *signal_id)
|
||||
{
|
||||
case 1: // 电机前进状态
|
||||
un_h_bridge_output.bit_data.channel_01 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.channel_04 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
|
||||
un_h_bridge_output1.bit_data.channel_01 = setBrakeOn();
|
||||
un_h_bridge_output1.bit_data.channel_02 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_03 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.sleep_01 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.sleep_02 = setBrakeOn(); // 正转
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setBrakeOff(); // 抱闸继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF14 = setBrakeOff(); // 抱闸继电器
|
||||
printf("Brake: Motor forward\n");
|
||||
break;
|
||||
|
||||
case 2: // 电机后退状态
|
||||
un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_02 = setBrakeOn();
|
||||
un_h_bridge_output1.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output1.bit_data.channel_02 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.channel_04 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_02 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.channel_03 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.sleep_01 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.sleep_02 = setBrakeOn(); // 反转
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setBrakeOn(); // 抱闸继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF14 = setBrakeOn(); // 抱闸继电器
|
||||
|
||||
printf("Brake: Motor reverse\n");
|
||||
break;
|
||||
|
||||
default:
|
||||
un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_04 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
|
||||
un_h_bridge_output1.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output1.bit_data.channel_02 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_03 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.sleep_01 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.sleep_02 = setBrakeOff(); // 关闭
|
||||
printf("Brake: Motor off\n");
|
||||
break;
|
||||
}
|
||||
@@ -88,14 +99,7 @@ static void brakeTimerProcess(void *signal_id)
|
||||
if (shouldApplyBrake())
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_APPLYING_BRAKE;
|
||||
if( 0 == brake_data.brake_direction)
|
||||
{
|
||||
brake_data.brake_motor_state = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
brake_data.brake_motor_state = 2;
|
||||
}
|
||||
brake_data.brake_motor_state = 1;
|
||||
brakeOutput(NULL);
|
||||
timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 0);
|
||||
}
|
||||
@@ -105,15 +109,7 @@ static void brakeTimerProcess(void *signal_id)
|
||||
if (shouldReleaseBrake() && power_data.current_state == POWER_WORKING)
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_RELEASING_BRAKE;
|
||||
|
||||
if( 0 == brake_data.brake_direction)
|
||||
{
|
||||
brake_data.brake_motor_state = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
brake_data.brake_motor_state = 1;
|
||||
}
|
||||
brake_data.brake_motor_state = 2;
|
||||
brakeOutput(NULL);
|
||||
timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0);
|
||||
}
|
||||
@@ -149,6 +145,7 @@ static void brakeTimerProcess(void *signal_id)
|
||||
{
|
||||
setParam("brk_pos", (float)brake_data.brake_position);
|
||||
brake_data.old_brake_position = brake_data.brake_position;
|
||||
|
||||
|
||||
printf("writeE2 brake_position = %d\n",brake_data.brake_position);
|
||||
|
||||
@@ -184,14 +181,6 @@ static void brakeInput(void *signal_id)
|
||||
|
||||
}
|
||||
|
||||
void paramUpdate(void *signal_id)
|
||||
{
|
||||
brake_data.brake_direction = (uint8_t)getParam("brk_rev");
|
||||
timerStart(&brake_data.brake_param_timer, 1000, 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 修改APP模块的初始化函数
|
||||
void brakeAppInit(void)
|
||||
{
|
||||
@@ -215,7 +204,6 @@ void brakeAppInit(void)
|
||||
timerInit(&brake_data.brake_timer);
|
||||
timerInit(&brake_data.brake_apply_timer);
|
||||
timerInit(&brake_data.brake_release_timer);
|
||||
timerInit(&brake_data.brake_param_timer);
|
||||
|
||||
// 订阅输入信号,处理刹车逻辑
|
||||
subscribe(&un_sw_sample, brakeInput);
|
||||
@@ -223,16 +211,13 @@ void brakeAppInit(void)
|
||||
subscribe(&can_fault_info, brakeInput);
|
||||
subscribe(ðernet_fault_Info, brakeInput);
|
||||
|
||||
|
||||
// 订阅定时器信号,用于状态机的定时处理
|
||||
subscribe(&brake_data.brake_timer, brakeTimerProcess);
|
||||
subscribe(&brake_data.brake_apply_timer, brakeTimerProcess);
|
||||
subscribe(&brake_data.brake_release_timer, brakeTimerProcess);
|
||||
subscribe(&brake_data.brake_param_timer, paramUpdate);
|
||||
|
||||
// 启动定时器，定期调用 brakeTimerProcess
|
||||
timerStart(&brake_data.brake_timer, 500, 1);
|
||||
timerStart(&brake_data.brake_param_timer, 1000, 1);
|
||||
|
||||
|
||||
printf("app_brake: initial OK \n");
|
||||
}
|
||||
|
||||
@@ -29,8 +29,7 @@ typedef struct BrakeSystem
|
||||
uint8_t brake_command; // 刹车命令变量：1表示刹车,2表示释放
|
||||
uint8_t brake_motor_state; // 刹车电机状态变量：0停止,1前进,2后退
|
||||
uint8_t brake_command_in_progress; // 刹车命令执行状态：0表示空闲,1表示正在执行
|
||||
uint8_t brake_direction;
|
||||
|
||||
|
||||
uint8_t emergency_stop_switch; // 急停开关
|
||||
uint8_t remote_emergency_stop; // 遥控器急停开关
|
||||
uint8_t remote_fault; // 遥控器故障
|
||||
@@ -42,7 +41,6 @@ typedef struct BrakeSystem
|
||||
uint8_t old_brake_position; // 旧的刹车位置
|
||||
Timer brake_apply_timer; // 刹车定时器
|
||||
Timer brake_release_timer; // 释放刹车定时器
|
||||
Timer brake_param_timer; //参数更新时间
|
||||
|
||||
} BrakeSystem;
|
||||
|
||||
|
||||
@@ -22,196 +22,48 @@ 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;
|
||||
// // 档位
|
||||
// un_motor_output1.bit_data.gear = (left_speed >= 0) ? 1 : 2; // 1 表示前进，2 表示后退
|
||||
// un_motor_output2.bit_data.gear = (right_speed >= 0) ? 1 : 2;
|
||||
|
||||
// 档位
|
||||
abs_left_front_speed = calculateTorqueOutput(diff_data.motor_state[0], out_torq[0]); //根据挡位增加转矩方向
|
||||
abs_right_front_speed = calculateTorqueOutput(diff_data.motor_state[1], out_torq[1]);
|
||||
abs_left_rear_speed = calculateTorqueOutput(diff_data.motor_state[2], out_torq[2]);
|
||||
abs_right_rear_speed = calculateTorqueOutput(diff_data.motor_state[3], out_torq[3]);
|
||||
|
||||
un_motor_output1.bit_data.gear = diff_data.motor_state[0];
|
||||
un_motor_output2.bit_data.gear = diff_data.motor_state[1];
|
||||
|
||||
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;
|
||||
}
|
||||
// // 计算绝对值并转换
|
||||
int16_t abs_left_front_speed = (int16_t)(out_torq[0]);
|
||||
int16_t abs_right_front_speed = (int16_t)(out_torq[1]);
|
||||
|
||||
int16_t abs_left_rear_speed = (int16_t)(out_torq[2]);
|
||||
int16_t abs_right_rear_speed = (int16_t)(out_torq[3]*0.73); //20251107 修改新电机增加系数 解决不同步问题
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
// 设置左右电机期望转速
|
||||
// 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.mode = MOTOR_MODE;
|
||||
un_motor_output2.bit_data.mode = MOTOR_MODE;
|
||||
un_motor_output3.bit_data.mode = MOTOR_MODE;
|
||||
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.MotCon_1Signal3 = (uint16_t)(-abs_right_front_speed);//20250502方向原因，需要把1号电机控制器的左右电机反相
|
||||
un_motor_output2.bit_data.MotCon_1Signal4 = (uint16_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.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.MotCon_1Signal4 = (uint16_t)(-abs_left_front_speed);
|
||||
un_motor_output2.bit_data.MotCon_1Signal3 = (uint16_t)abs_left_rear_speed;
|
||||
|
||||
|
||||
// 设置放电功率
|
||||
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;
|
||||
|
||||
// // 设置模式为恒速模式
|
||||
// un_motor_output1.bit_data.mode = 0x01;
|
||||
// un_motor_output2.bit_data.mode = 0x01;
|
||||
//
|
||||
// // 设置最大扭矩
|
||||
// un_motor_output1.bit_data.set_torque = (max_torque + 300) * 100; // 20240921 增加偏移量
|
||||
// un_motor_output2.bit_data.set_torque = (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_output1.bit_data.discharge_power = discharge_power;
|
||||
// un_motor_output2.bit_data.discharge_power = discharge_power;
|
||||
|
||||
}
|
||||
|
||||
|
||||
// 限制值在最小值和最大值之间
|
||||
float constrain(float value, float min_val, float max_val)
|
||||
{
|
||||
if (value < min_val)
|
||||
{
|
||||
return min_val;
|
||||
}
|
||||
else if (value > max_val)
|
||||
{
|
||||
return max_val;
|
||||
}
|
||||
else
|
||||
{
|
||||
return value;
|
||||
}
|
||||
}
|
||||
|
||||
// 计算当前速度、角速度
|
||||
uint8_t calculateCurrentSpeedYawRate(void)
|
||||
{
|
||||
@@ -423,22 +275,15 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
|
||||
}
|
||||
|
||||
#if THROTTLE_PID_MODE
|
||||
|
||||
float left_speed_mps = 0.0f;
|
||||
float right_speed_mps = 0.0f;
|
||||
float max_torque = (float)getParam("maxTorq");
|
||||
|
||||
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;
|
||||
linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
|
||||
yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
|
||||
|
||||
|
||||
float left_speed_mps = linear_velocity_x + yaw_rate;
|
||||
float right_speed_mps = linear_velocity_x - yaw_rate;
|
||||
|
||||
//扭矩分配
|
||||
if(max_torque < left_speed_mps)
|
||||
{
|
||||
@@ -462,26 +307,36 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
|
||||
}
|
||||
else{}
|
||||
|
||||
// printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate);
|
||||
// adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5);
|
||||
// adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5);
|
||||
// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);
|
||||
// printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate);
|
||||
|
||||
motor_speed[0] = left_speed_mps;//加速状态，没有负扭矩，要么前进加速要么后退加速
|
||||
motor_speed[0] = left_speed_mps;
|
||||
motor_speed[2] = left_speed_mps;
|
||||
|
||||
motor_speed[1] = right_speed_mps;
|
||||
motor_speed[3] = right_speed_mps;
|
||||
|
||||
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]);
|
||||
|
||||
|
||||
motor_speed[3] = right_speed_mps;
|
||||
|
||||
// adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5);
|
||||
// adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5);
|
||||
//
|
||||
|
||||
// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// distributeTorque(diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed,2*left_speed_mps,&motor_speed[0],&motor_speed[2],diff_data.max_Torq,diff_data.min_Torq);
|
||||
// distributeTorque(diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed,2*right_speed_mps,&motor_speed[1],&motor_speed[3],diff_data.max_Torq,diff_data.min_Torq);
|
||||
|
||||
// printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", motor_speed[0], motor_speed[1], motor_speed[2], motor_speed[3]);
|
||||
|
||||
// // 返回计算结果
|
||||
// *left_motor_speed = left_speed_mps;
|
||||
// *right_motor_speed = right_speed_mps;
|
||||
#else
|
||||
|
||||
// 限制线速度和偏航率
|
||||
@@ -575,7 +430,6 @@ float mapRemoteControlSpeed(
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 差速处理函数
|
||||
static void diffProcess(void *signal_id)
|
||||
{
|
||||
@@ -602,30 +456,41 @@ static void diffProcess(void *signal_id)
|
||||
{
|
||||
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
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral);
|
||||
|
||||
// 使用差速车辆动力学模型计算左右电机的期望速度
|
||||
float out_torque[4] = {0,0,0,0};
|
||||
|
||||
// if(diff_data.desired_yaw_rate != 0)//有转向的情况下下
|
||||
// {
|
||||
// if( (output_yaw_rate > -500) && (output_yaw_rate < 500) )//如果是转向输出在-500~500之间，那么开始原地转向扭矩太小，所以设定最小扭矩。
|
||||
// {
|
||||
// output_yaw_rate = 500;
|
||||
// }
|
||||
// }
|
||||
|
||||
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, &out_torque[0]);
|
||||
|
||||
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;
|
||||
@@ -682,11 +547,23 @@ static void diffProcess(void *signal_id)
|
||||
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);
|
||||
|
||||
|
||||
// printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral);
|
||||
|
||||
|
||||
|
||||
|
||||
// if( (left_speed < 200) && (left_speed > -200) )
|
||||
// {
|
||||
// left_speed = 0;
|
||||
// }
|
||||
//
|
||||
// if( (right_speed < 200) && (right_speed > -200) )
|
||||
// {
|
||||
// right_speed = 0;
|
||||
// }
|
||||
|
||||
|
||||
// 设置电机输出
|
||||
setMotorOutput(&diff_data.out_torq[0],
|
||||
diff_data.max_Torq,//
|
||||
@@ -696,22 +573,9 @@ static void diffProcess(void *signal_id)
|
||||
if (power_data.current_state == POWER_WORKING)
|
||||
{
|
||||
publishMessage(&un_motor_output1, 1);
|
||||
publishMessage(&un_motor_output2, 1);
|
||||
publishMessage(&un_motor_output3, 1);
|
||||
publishMessage(&un_motor_output4, 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);
|
||||
|
||||
}
|
||||
@@ -746,6 +610,11 @@ int16_t Filter(int16_t *s,uint8_t Len)
|
||||
// 差速输入处理函数
|
||||
static void diffInput(void *signal_id)
|
||||
{
|
||||
|
||||
// static float left_speed_fiter[SPEED_FITER_NUM] = {0};
|
||||
// static uint8_t left_speed_cnt = 0;
|
||||
// static float right_speed_fiter[SPEED_FITER_NUM] = {0};
|
||||
// static uint8_t right_speed_cnt = 0;
|
||||
float motor_speed_temp = 0.0f;
|
||||
|
||||
if (signal_id == &un_sw_sample)
|
||||
@@ -765,11 +634,9 @@ 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;
|
||||
|
||||
if(diff_data.desired_speed >= 0)//20250320 增加根据速度大小来决定方向，解决后退时转弯反向的问题
|
||||
{
|
||||
diff_data.desired_curvature = diff_data.desired_curvature;
|
||||
@@ -788,8 +655,8 @@ static void diffInput(void *signal_id)
|
||||
diff_data.desired_speed = diff_data.desired_speed * 0.01f;
|
||||
diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f;
|
||||
// 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
|
||||
diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.2, 2, 10, 1, 5);//20250320 修改死区为0.2解决停不住的问题
|
||||
diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1);
|
||||
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 if ( (signal_id == &un_auto_computer_input) && (diff_data.mode == MODE_AUTO) )
|
||||
{
|
||||
@@ -797,44 +664,68 @@ static void diffInput(void *signal_id)
|
||||
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_curvature = diff_data.desired_curvature * 0.0001f;//
|
||||
// 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
|
||||
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);
|
||||
// diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1);
|
||||
}
|
||||
else if ( (signal_id == &un_motor_input1) || (signal_id == &un_motor_input3) )// 处理第一个电机速度信号（左电机）
|
||||
else if (signal_id == &un_motor_input1)// 处理第一个电机速度信号（左电机）
|
||||
{
|
||||
diff_data.left_front_motor_speed = (float)((int16_t)(un_motor_input1.bit_data.speed - 30000));//20240921 增加偏移量
|
||||
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 增加反相
|
||||
|
||||
diff_data.right_front_motor_speed = -(float)( (int16_t)(un_motor_input1.bit_data.MotCon_1Signal3) ) / 6.0; // 20250502 1号控制器增加反相
|
||||
diff_data.right_rear_motor_speed = -(float)( (int16_t)(un_motor_input2.bit_data.MotCon_1Signal4) ) /6.0;
|
||||
// motor_speed_temp = (motor_speed_temp + (int16_t)un_motor_input2.bit_data.MotCon_1Signal4)/2/6;
|
||||
|
||||
if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速
|
||||
if(fabs(diff_data.right_rear_motor_speed) > fabs(diff_data.right_front_motor_speed))//取速度较小的轮速
|
||||
{
|
||||
motor_speed_temp = diff_data.left_front_motor_speed;
|
||||
motor_speed_temp = diff_data.right_front_motor_speed;
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_speed_temp = diff_data.left_rear_motor_speed;
|
||||
}
|
||||
diff_data.left_motor_speed = motor_speed_temp;
|
||||
}
|
||||
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_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;
|
||||
motor_speed_temp = diff_data.right_rear_motor_speed;
|
||||
}
|
||||
|
||||
diff_data.right_motor_speed = motor_speed_temp;
|
||||
// diff_data.right_motor_speed = LOWPASS_FILTER(motor_speed_temp,right_speed_fiter[0]);
|
||||
diff_data.right_motor_speed = motor_speed_temp;//
|
||||
// right_speed_fiter[0] = diff_data.right_motor_speed;
|
||||
|
||||
|
||||
// if(SPEED_FITER_NUM == right_speed_cnt)//取样4次后滤波
|
||||
// {
|
||||
// right_speed_cnt = 0;
|
||||
// diff_data.right_motor_speed = (float)Filter(right_speed_fiter,SPEED_FITER_NUM)/6.0f;
|
||||
// }
|
||||
|
||||
// printf("right_motor_speed = %f, motor_speed_temp = %d\n",diff_data.right_motor_speed,motor_speed_temp);
|
||||
|
||||
}
|
||||
else if (signal_id == &un_motor_input2)// 处理第二个电机速度信号（右电机）
|
||||
{
|
||||
diff_data.left_front_motor_speed = -(float)( (int16_t)(un_motor_input1.bit_data.MotCon_1Signal4) ) /6.0; // 20250502 1号控制器增加反相
|
||||
diff_data.left_rear_motor_speed = (float)( (int16_t) (un_motor_input2.bit_data.MotCon_1Signal3) ) / 6.0;
|
||||
|
||||
if(fabs(diff_data.left_front_motor_speed) > fabs(diff_data.left_rear_motor_speed))//取速度较小的轮速
|
||||
{
|
||||
motor_speed_temp = diff_data.left_rear_motor_speed;
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_speed_temp = diff_data.left_front_motor_speed;
|
||||
}
|
||||
|
||||
diff_data.left_motor_speed = motor_speed_temp; //
|
||||
// diff_data.left_motor_speed = LOWPASS_FILTER(motor_speed_temp,left_speed_fiter[0]);//低通滤波器
|
||||
// left_speed_fiter[0] = diff_data.left_motor_speed;
|
||||
|
||||
|
||||
// left_speed_fiter[left_speed_cnt] = motor_speed_temp;
|
||||
// left_speed_cnt ++;
|
||||
// if(SPEED_FITER_NUM == left_speed_cnt)//取样4次后滤波
|
||||
// {
|
||||
// left_speed_cnt = 0;
|
||||
// diff_data.left_motor_speed = (float)Filter(left_speed_fiter,SPEED_FITER_NUM)/6.0f;
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
// 急停开关
|
||||
@@ -853,32 +744,20 @@ 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((power_data.current_state == POWER_WORKING))//电机上电才运行
|
||||
if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制
|
||||
{
|
||||
diffProcess(&diff_data);//计算左右电机期望转速
|
||||
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
|
||||
{
|
||||
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;
|
||||
diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
|
||||
}
|
||||
|
||||
|
||||
diffProcess(&diff_data);//计算左右电机期望转速
|
||||
}
|
||||
|
||||
|
||||
@@ -892,15 +771,10 @@ void preChargeFinish(void *signal_id)
|
||||
|
||||
setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr"));
|
||||
// 档位
|
||||
un_motor_output1.bit_data.gear = 0; // 0表示空挡
|
||||
un_motor_output2.bit_data.gear = 0;
|
||||
un_motor_output3.bit_data.gear = 0; // 0表示空挡
|
||||
un_motor_output4.bit_data.gear = 0;
|
||||
|
||||
// un_motor_output1.bit_data.gear = 0; // 0表示空挡
|
||||
// un_motor_output2.bit_data.gear = 0;
|
||||
publishMessage(&un_motor_output1, 1);
|
||||
publishMessage(&un_motor_output2, 1);
|
||||
publishMessage(&un_motor_output3, 1);
|
||||
publishMessage(&un_motor_output4, 1);
|
||||
}
|
||||
|
||||
|
||||
@@ -944,7 +818,8 @@ void diffParametersInit(void *signal_id)
|
||||
getParam("crv_ol")
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 设置曲率 PID 控制器的参数
|
||||
setPidParameters(&Dec_front_speed_pid,
|
||||
getParam("mot_kp"),
|
||||
@@ -961,11 +836,8 @@ void diffParametersInit(void *signal_id)
|
||||
Dec_front_speed_pid.kd,
|
||||
Dec_front_speed_pid.integral_limit,
|
||||
Dec_front_speed_pid.output_limit
|
||||
);
|
||||
|
||||
diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩
|
||||
diff_data.max_Torq = (float)getParam("maxTorq");
|
||||
|
||||
);
|
||||
|
||||
if(0 == (float)getParam("diff_sp"))//20250711 防止参数为0，影响计算。
|
||||
{
|
||||
diff_data.diff_dead_zone = 2;
|
||||
@@ -975,30 +847,42 @@ void diffParametersInit(void *signal_id)
|
||||
diff_data.diff_dead_zone = (float)getParam("diff_sp");//参数读取设定最大扭矩
|
||||
}
|
||||
|
||||
|
||||
|
||||
diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩
|
||||
|
||||
|
||||
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("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("left_motor_speed = %f\n",diff_data.left_motor_speed);
|
||||
printf("right_motor_speed = %f\n",diff_data.right_motor_speed);
|
||||
|
||||
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("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;
|
||||
deffcurvature = deffcurvature * 0.0001f;
|
||||
|
||||
printf("remote_speed: %f, remote_yaw_rate: %f\n", deffspeed, deffcurvature);
|
||||
printf("remote speed = %f, remote curvature = %f\n", deffspeed, deffcurvature);
|
||||
|
||||
|
||||
deffspeed = (float)((int16_t)(un_manual_computer_input.bit_data.set_speed));
|
||||
deffcurvature = (float)((int16_t)(un_manual_computer_input.bit_data.set_curvature));
|
||||
// 单位转换
|
||||
deffspeed = deffspeed * 0.01f;
|
||||
deffcurvature = deffcurvature * 0.0001f;
|
||||
|
||||
printf("manual speed = %f, manual curvature = %f\n", deffspeed, deffcurvature);
|
||||
|
||||
printf(" car state = %d,%d,%d,%d\n", diff_data.motor_state[0],diff_data.motor_state[1],diff_data.motor_state[2],diff_data.motor_state[3]);
|
||||
|
||||
timerStart(&diff_app_timer,1000,1);//1s调用一次
|
||||
}
|
||||
|
||||
|
||||
// 差速初始化函数
|
||||
void diffAppInit(void)
|
||||
{
|
||||
@@ -1058,9 +942,8 @@ void diffAppInit(void)
|
||||
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调用一次
|
||||
|
||||
|
||||
@@ -13,30 +13,14 @@ extern "C"
|
||||
|
||||
#define SPEED_PID_MODE 0
|
||||
#define THROTTLE_PID_MODE 1
|
||||
|
||||
#define TURN_MIN_TOUQUE 1 //n*m
|
||||
|
||||
#define SPEED_MODE 0x01
|
||||
#define TORQUE_MODE 0x02
|
||||
|
||||
|
||||
#define TORQUE_HYSTERESIS_THRESHOLD 0.3f
|
||||
|
||||
|
||||
#define MOTOR_MODE TORQUE_MODE
|
||||
|
||||
|
||||
|
||||
#define ALPHA 0.1f // 滤波系数α∈[0.01,0.3]，0.2对应截止频率约10Hz（假设采样周期10ms）
|
||||
#define LOWPASS_FILTER(speed, prev) (ALPHA * (speed) + (1 - ALPHA) * (prev))
|
||||
|
||||
// 状态机内部状态
|
||||
typedef enum
|
||||
{
|
||||
STATE_INIT, ///< 初始状态（转速为0且等待扭矩方向判定）
|
||||
STATE_FORWARD, ///< 前进
|
||||
STATE_BACKWARD, ///< 后退
|
||||
} MotorState;
|
||||
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
@@ -47,8 +31,7 @@ typedef enum
|
||||
typedef struct DiffData
|
||||
{
|
||||
ControlMode mode ; // 控制模式
|
||||
MotorState motor_state[4]; //当前车辆状态
|
||||
float desired_speed; // 期望速度
|
||||
float desired_speed; // 期望速度
|
||||
float desired_curvature; // 期望曲率
|
||||
float left_motor_speed; // 当前左电机速度
|
||||
float right_motor_speed; // 当前右电机速度
|
||||
@@ -72,15 +55,15 @@ typedef struct DiffData
|
||||
float out_right_motor_speed; // 输出右电机速度
|
||||
float out_torq[4]; //4个电机扭矩
|
||||
float max_Torq; // 最大扭矩限制
|
||||
float min_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 diff_dead_zone; // 差速速度死区
|
||||
|
||||
} DiffData;
|
||||
|
||||
|
||||
|
||||
@@ -10,7 +10,7 @@ extern "C"
|
||||
#include "app_dependence.h"
|
||||
|
||||
#define MAX_SIGNALS 500u // 每个优先级的最大信号数量
|
||||
#define MAX_SUBSCRIBERS 50u // 不同信号的订阅者数量
|
||||
#define MAX_SUBSCRIBERS 100u // 不同信号的订阅者数量
|
||||
#define MAX_CALLBACKS 25u // 每个信号最多支持多少订阅者
|
||||
#define PRIORITY_LEVELS 2u // 优先级层次
|
||||
|
||||
|
||||
@@ -110,41 +110,28 @@ static void lightOutput(void *signal_id)
|
||||
switch (i)
|
||||
{//正常所有灯光熄灭
|
||||
case LIGHT_HEAD://头灯，前面4个灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF05 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF06 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF10 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = state_value;
|
||||
|
||||
un_inf_can_kgf_output2.bit_data.KGF07 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF09 = state_value;
|
||||
break;
|
||||
case LIGHT_TAIL://尾灯，后面4个灯
|
||||
un_inf_can_kgf_output2.bit_data.KGF12 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF13 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF14 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = state_value;
|
||||
break;
|
||||
case LIGHT_LEFT_TURN://左转向，左边4个灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF06 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF12 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF13 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF08 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF12 = state_value;
|
||||
break;
|
||||
case LIGHT_RIGHT_TURN://右转向灯，右边4个灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF05 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF10 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF14 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF10 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF14 = state_value;
|
||||
break;
|
||||
case LIGHT_BRAKE://刹车灯，四个黄灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF10 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF13 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF13 = state_value;
|
||||
break;
|
||||
case LIGHT_ALARM://报警灯，四个红灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF05 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF06 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF12 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF14 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF13 = state_value;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -278,31 +278,21 @@ void processWriteRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip,
|
||||
|
||||
|
||||
void processReadRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) {
|
||||
// 处理读请求的逻辑
|
||||
printf("Processing read request.\n");
|
||||
|
||||
// 先备份原始请求数据
|
||||
UnParamRequest originalRequest;
|
||||
memcpy(&originalRequest, paramRequest, sizeof(UnParamRequest));
|
||||
|
||||
// 清零响应数据
|
||||
// 清零 paramRequest
|
||||
memset(paramRequest, 0, sizeof(UnParamRequest));
|
||||
|
||||
// 处理客户端请求的参数
|
||||
// 先发送信号，然后从结构体读数
|
||||
for (int i = 0; i < 256; ++i) {
|
||||
if (strlen((char *)originalRequest.bit_data.param_name[i]) > 0) {
|
||||
// 复制参数名到响应
|
||||
strcpy((char *)paramRequest->bit_data.param_name[i],
|
||||
(char *)originalRequest.bit_data.param_name[i]);
|
||||
// 读取参数值
|
||||
float readData = readParameter(originalRequest.bit_data.param_name[i]);
|
||||
memcpy(paramRequest->bit_data.data[i], &readData, sizeof(float));
|
||||
|
||||
printf("Read parameter: %s = %f\n",
|
||||
originalRequest.bit_data.param_name[i], readData);
|
||||
if (strlen((char *)paramRequest->bit_data.param_name[i]) > 0) {
|
||||
float readData = readParameter(paramRequest->bit_data.param_name[i]);
|
||||
memcpy(paramRequest->bit_data.data[i], &readData, sizeof(paramRequest->bit_data.data[i]));
|
||||
}
|
||||
}
|
||||
|
||||
// 发送响应 - 直接传递 paramRequest
|
||||
// 发送响应
|
||||
sendParamRequestResponse(paramRequest, sender_ip, sender_port, 0);
|
||||
}
|
||||
|
||||
@@ -310,7 +300,6 @@ void processReadRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, u
|
||||
|
||||
|
||||
|
||||
|
||||
void OnParamSignal(void *data)
|
||||
{
|
||||
RequestContext *signal = (RequestContext *)data;
|
||||
@@ -432,55 +421,37 @@ void paramAppInit(void)
|
||||
// 上电读取所有参数
|
||||
memset(param_manager.arr, 0, sizeof(param_manager.arr));
|
||||
accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION);
|
||||
// whl_bas 轮胎直径<单位：m>
|
||||
// max_rpm 最大转速<单位：rpm/min>
|
||||
// whl_dia 轴距<单位：m>
|
||||
// max_acc 最大加速度
|
||||
// spd_kp 遥控速度P
|
||||
// spd_ki 遥控速度I
|
||||
// spd_kd 遥控速度D
|
||||
// spd_il 遥控速度积分限制
|
||||
// spd_ol 遥控速度PID输出限制
|
||||
// crv_kp 遥控转弯P
|
||||
// crv_ki 遥控转弯I
|
||||
// crv_kd 遥控转弯D
|
||||
// crv_il 遥控转弯积分限制
|
||||
// crv_ol 遥控转弯PID输出限制
|
||||
// brk_on 刹车刹紧时间 单位：ms
|
||||
// brk_off 刹车释放时间 单位：ms
|
||||
// maxTorq 最大扭矩 单位：n*m
|
||||
// feedPwr 最大馈电功率 单位：w
|
||||
// dispPwr 最大放电功率 单位：w
|
||||
// VehMass 车重 单位：kg
|
||||
// gRatio 减速比
|
||||
// prCTime 预充电时间 单位：s
|
||||
// brk_pos 刹车位置
|
||||
// pwr_sta 电源状态
|
||||
// high_sw 高压开关状态
|
||||
// stop_sw 急停开关状态
|
||||
// lightSt 灯光状态
|
||||
// pwr_btn 电源开关状态
|
||||
// sleepTm 休眠时间 单位：min
|
||||
// wakeTm 唤醒时间 单位：min
|
||||
// Ospd_kp 自主速度P
|
||||
// Ospd_ki 自主速度I
|
||||
// Ospd_kd 自主速度D
|
||||
// Ospd_il 自主速度积分限制
|
||||
// Ospd_ol 自主速度PID输出限制
|
||||
// Ocrv_kp 自主转弯P
|
||||
// Ocrv_ki 自主转弯I
|
||||
// Ocrv_kd 自主转弯D
|
||||
// Ocrv_il 自主转弯积分限制
|
||||
// Ocrv_ol 自主转弯PID输出限制
|
||||
// minTorq 输出扭矩死区 单位：n*m
|
||||
// brk_rev 刹车方向
|
||||
// mot_kp 同侧扭矩P参数
|
||||
// mot_ki 同侧扭矩I参数
|
||||
// mot_kd 同侧扭矩d参数
|
||||
// mot_il 同侧扭矩积分限制
|
||||
// mot_ol 同侧扭矩输出限制
|
||||
// diff_sp 同侧扭矩速度差阈值
|
||||
// test 初始化测试参数
|
||||
|
||||
// 初始化每个参数
|
||||
// param_manager.bit_data.whl_bas = 1.5f; // 初始化轮距
|
||||
// param_manager.bit_data.max_rpm = 5500.0f; // 初始化最大转速
|
||||
// param_manager.bit_data.whl_dia = 0.6f; // 初始化轮直径
|
||||
// param_manager.bit_data.max_acc = 1.0f; // 初始化最大加速度
|
||||
// param_manager.bit_data.spd_kp = 5.0f; // 初始化速度控制 KP
|
||||
// param_manager.bit_data.spd_ki = 1.0f; // 初始化速度控制 KI
|
||||
// param_manager.bit_data.spd_kd = 0.0f; // 初始化速度控制 KD
|
||||
// param_manager.bit_data.spd_il = 5.0f; // 初始化速度控制 IL
|
||||
// param_manager.bit_data.spd_ol = 5.0f; // 初始化速度控制 OL
|
||||
// param_manager.bit_data.crv_kp = 1.0f; // 初始化曲线控制 KP
|
||||
// param_manager.bit_data.crv_ki = 0.0f; // 初始化曲线控制 KI
|
||||
// param_manager.bit_data.crv_kd = 0.0f; // 初始化曲线控制 KD
|
||||
// param_manager.bit_data.crv_il = 2.0f; // 初始化曲线控制 IL
|
||||
// param_manager.bit_data.crv_ol = 2.0f; // 初始化曲线控制 OL
|
||||
// param_manager.bit_data.brk_on = 1500.0f; // 初始化制动开启参数
|
||||
// param_manager.bit_data.brk_off = 800.0f; // 初始化制动关闭参数
|
||||
// param_manager.bit_data.maxTorq = 60.0f; // 初始化最大扭矩
|
||||
// param_manager.bit_data.feedPwr = 10000.0f; // 初始化馈电功率
|
||||
// param_manager.bit_data.dispPwr = 10000.0f; // 初始化显示功率
|
||||
// param_manager.bit_data.VehMass = 700.0f; // 初始化车辆质量
|
||||
// param_manager.bit_data.gRatio = 28.0f; // 初始化减速比
|
||||
// param_manager.bit_data.prCTime = 5.0f; // 初始化预充时间
|
||||
// param_manager.bit_data.brk_pos = 0.0f; // 初始化刹车位置, 0表示未刹车
|
||||
// param_manager.bit_data.pwr_sta = 0.0f; // 初始化电源状态
|
||||
// param_manager.bit_data.lightSt = 0.0f; // 初始化灯光状态
|
||||
// param_manager.bit_data.pwr_btn = 0.0f; // 初始化电源按钮状态
|
||||
// param_manager.bit_data.test = 0.0f; // 初始化测试参数
|
||||
|
||||
|
||||
// 订阅信号
|
||||
subscribe(¶m_signal, handleParamOp);
|
||||
subscribe(&request_context, OnParamSignal);// 接收到上位机读写参数信号
|
||||
|
||||
@@ -52,13 +52,15 @@ extern "C"
|
||||
X(Ocrv_il) \
|
||||
X(Ocrv_ol) \
|
||||
X(minTorq) \
|
||||
X(brk_rev) \
|
||||
X(minYpos) \
|
||||
X(maxYpos) \
|
||||
X(mot_kp) \
|
||||
X(mot_ki) \
|
||||
X(mot_kd) \
|
||||
X(mot_il) \
|
||||
X(mot_ol) \
|
||||
X(diff_sp) \
|
||||
X(turn_sp) \
|
||||
X(test)
|
||||
|
||||
// 定义一个包含所有参数名称的结构体
|
||||
|
||||
@@ -43,7 +43,7 @@ static void handlePowerButton(void)
|
||||
if (power_data.remote_power_switch == app_close())
|
||||
{
|
||||
power_button.state = BUTTON_STATE_SHORT_PRESS_DETECTED;
|
||||
timerStart(&power_button.timer, 500, 0); // 启动短按定时器，500ms
|
||||
timerStart(&power_button.timer, 500, 1); // 启动短按定时器，500ms
|
||||
}
|
||||
break;
|
||||
|
||||
@@ -53,7 +53,7 @@ static void handlePowerButton(void)
|
||||
if (power_button.timer.active) // 定时器未到期,短按完成，启动等待长按定时器
|
||||
{
|
||||
power_button.state = BUTTON_STATE_WAIT_FOR_LONG_PRESS;
|
||||
timerStart(&power_button.timer, 500, 0); // 启动等待长按定时器，500ms
|
||||
timerStart(&power_button.timer, 500, 1); // 启动等待长按定时器，500ms
|
||||
}
|
||||
}
|
||||
else if (!power_button.timer.active)// 短按定时器到期，按键仍被按下，视为无效，重置为初始状态
|
||||
@@ -62,11 +62,11 @@ static void handlePowerButton(void)
|
||||
}
|
||||
break;
|
||||
|
||||
case BUTTON_STATE_WAIT_FOR_LONG_PRESS:
|
||||
case BUTTON_STATE_WAIT_FOR_LONG_PRESS:
|
||||
if (power_data.remote_power_switch == app_close())// 检测是否在等待时间内进行长按
|
||||
{
|
||||
power_button.state = BUTTON_STATE_LONG_PRESS;
|
||||
timerStart(&power_button.timer, 1000, 0); // 启动长按定时器，1000ms
|
||||
timerStart(&power_button.timer, 1000, 1); // 启动长按定时器，1000ms
|
||||
}
|
||||
else if (!power_button.timer.active) // 等待长按超时，重置为初始状态
|
||||
{
|
||||
@@ -113,111 +113,124 @@ static void powerOutput(void *signal_id)
|
||||
{
|
||||
case POWER_PRE_CHARGE:
|
||||
publishMessage(&power_data.pre_charge_finish, 1);//发布预充完成信号，100ms发送一次，直到预充完成
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOn(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOn(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOn(); // 上装转台继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOn(); // 上装转台继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 导航计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 导航计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
|
||||
break;
|
||||
|
||||
case POWER_NEUTRAL:
|
||||
publishMessage(&power_data.pre_charge_finish, 1);//20250316增加，发送空挡信号，保证电机控制器高压上电后，发送空挡信号
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOn(); // 上装转台继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOn(); // 上装转台继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 导航计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 导航计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
break;
|
||||
|
||||
|
||||
case POWER_STANDBY:
|
||||
// 初始状态,只开启基本设备
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOff(); // 上装转台继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOff(); // 上装转台继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOff(); // 导航计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOff(); // 导航计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
break;
|
||||
|
||||
case POWER_WORKING:
|
||||
// 工作状态,除预充继电器外所有设备开启
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOn(); // 上装转台继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOn(); // 上装转台继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 导航计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 导航计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
break;
|
||||
|
||||
case POWER_EMERGENCY:
|
||||
// 急停状态,断开高压
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOn(); // 上装转台继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOn(); // 上装转台继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 导航计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 导航计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
break;
|
||||
|
||||
case POWER_SLEEP:
|
||||
// 休眠状态,关闭所有设备
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOff(); // 上装转台继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOff(); // 上装转台继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOff(); // 导航计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOff(); // 导航计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
break;
|
||||
|
||||
default:
|
||||
@@ -235,14 +248,14 @@ static void wakeupProcess(void *signal_id)
|
||||
un_gather_output.bit_data.sleep_duration = (uint16_t)getParam("sleepTm");
|
||||
un_gather_output.bit_data.wakeup_interval = (uint16_t)getParam("wakeTm");
|
||||
|
||||
if(un_gather_output.bit_data.sleep_duration < 5)//最小值限定
|
||||
if(un_gather_output.bit_data.sleep_duration < 10)//最小值限定
|
||||
{
|
||||
un_gather_output.bit_data.sleep_duration = 5;
|
||||
un_gather_output.bit_data.sleep_duration = 10;
|
||||
}
|
||||
|
||||
if(un_gather_output.bit_data.wakeup_interval < 5)//最小值限定
|
||||
if(un_gather_output.bit_data.wakeup_interval < 10)//最小值限定
|
||||
{
|
||||
un_gather_output.bit_data.wakeup_interval = 5;
|
||||
un_gather_output.bit_data.wakeup_interval = 10;
|
||||
}
|
||||
|
||||
un_gather_output.bit_data.vehicle_mode = power_data.current_state;
|
||||
@@ -272,62 +285,27 @@ static void powerTimerProcess(void *signal_id)
|
||||
case POWER_PRE_CHARGE:
|
||||
if (!power_data.timer_pre_charge.active) // 预充时间到
|
||||
{
|
||||
power_data.current_state = POWER_NEUTRAL; // 工作
|
||||
power_data.pre_charge_finish = 1; // 预充完成
|
||||
printf("Power: Transitioning from PRE_CHARGE to POWER_NEUTRAL state\n");
|
||||
}
|
||||
break;
|
||||
case POWER_NEUTRAL://20250316增加，发送空挡信号
|
||||
if (power_data.neutral_cnt >= 5) // 运行5次
|
||||
{
|
||||
power_data.neutral_cnt = 0;
|
||||
power_data.current_state = POWER_WORKING; // 工作
|
||||
power_data.pre_charge_finish = 1; // 预充完成
|
||||
printf("Power: Transitioning from POWER_NEUTRAL to WORKING state\n");
|
||||
printf("Power: Transitioning from PRE_CHARGE to WORKING state\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
power_data.neutral_cnt ++;
|
||||
power_data.current_state = POWER_NEUTRAL; // 空挡
|
||||
power_data.pre_charge_finish = 1; // 预充完成
|
||||
}
|
||||
break;
|
||||
|
||||
break;
|
||||
case POWER_STANDBY:
|
||||
if (power_data.high_voltage_switch == app_open()) // 高压开关断开
|
||||
{
|
||||
power_data.current_state = POWER_SLEEP; // 休眠
|
||||
printf("Power: Transitioning from STANDBY to SLEEP state\n");
|
||||
}
|
||||
else if (power_button.is_power_on == app_close() && power_data.emergency_stop == app_close()) // 遥控器电源开关闭合且急停开关闭合
|
||||
if (power_button.is_power_on == app_close() && power_data.emergency_stop == app_close()) // 遥控器电源开关闭合且急停开关闭合
|
||||
{
|
||||
power_data.current_state = POWER_EMERGENCY; // 急停
|
||||
printf("Power: Transitioning from STANDBY to EMERGENCY state\n");
|
||||
}
|
||||
break;
|
||||
|
||||
case POWER_WORKING:
|
||||
if (power_data.high_voltage_switch == app_open()) // 高压开关断开
|
||||
{
|
||||
power_data.current_state = POWER_SLEEP; // 休眠
|
||||
printf("Power: Transitioning from STANDBY to SLEEP state\n");
|
||||
}
|
||||
else if (power_data.emergency_stop == app_close()) // 急停开关闭合
|
||||
if (power_data.emergency_stop == app_close()) // 急停开关闭合
|
||||
{
|
||||
power_data.current_state = POWER_EMERGENCY; // 急停
|
||||
printf("Power: Transitioning from WORKING to EMERGENCY state\n");
|
||||
|
||||
printf("emergency_stop_switch: %d, remote_emergency_stop: %d\n", power_data.emergency_stop_switch, power_data.remote_emergency_stop); //打印状态
|
||||
printf("remote_stop: %d\n", un_remote_control_input.bit_data.switch_b);
|
||||
printf("Power: Transitioning from WORKING to EMERGENCY state\n");
|
||||
}
|
||||
break;
|
||||
case POWER_EMERGENCY:
|
||||
if (power_data.high_voltage_switch == app_open()) // 高压开关断开
|
||||
{
|
||||
power_data.current_state = POWER_SLEEP; // 休眠
|
||||
printf("Power: Transitioning from EMERGENCY to SLEEP state\n");
|
||||
}
|
||||
else if (power_button.is_power_on == app_open()) // 遥控器电源开关断开
|
||||
if (power_button.is_power_on == app_open()) // 遥控器电源开关断开
|
||||
{
|
||||
power_data.current_state = POWER_STANDBY; // 待机
|
||||
printf("Power: Transitioning from EMERGENCY to STANDBY state\n");
|
||||
@@ -339,13 +317,13 @@ static void powerTimerProcess(void *signal_id)
|
||||
printf("Power: Transitioning from EMERGENCY to PRE_CHARGE state\n");
|
||||
}
|
||||
break;
|
||||
case POWER_SLEEP:
|
||||
if (power_data.high_voltage_switch == app_close()) // 高压开关闭合
|
||||
{
|
||||
power_data.current_state = POWER_STANDBY; // 待机
|
||||
printf("Power: Transitioning from SLEEP to STANDBY state\n");
|
||||
}
|
||||
break;
|
||||
// case POWER_SLEEP:
|
||||
// if (power_data.high_voltage_switch == app_close()) // 高压开关闭合
|
||||
// {
|
||||
// power_data.current_state = POWER_STANDBY; // 待机
|
||||
// printf("Power: Transitioning from SLEEP to STANDBY state\n");
|
||||
// }
|
||||
// break;
|
||||
default:
|
||||
power_data.current_state = POWER_STANDBY; // 待机
|
||||
break;
|
||||
|
||||
@@ -280,7 +280,7 @@ static void requestInput(void *signal_id)
|
||||
}
|
||||
else if(signal_id == &un_motor_input1)
|
||||
{
|
||||
// un_motor_status_output.bit_data.left_wheel_speed = SWAP_ENDIAN_16( (uint16_t)((int16_t)(un_motor_input1.bit_data.MotCon_1Signal4) + 30000) );
|
||||
un_motor_status_output.bit_data.left_wheel_speed = SWAP_ENDIAN_16( (uint16_t)((int16_t)(un_motor_input1.bit_data.MotCon_1Signal4) + 30000) );
|
||||
|
||||
// un_motor_status_output.bit_data.left_torque = ((un_motor_input1.bit_data.torque << 8) | (un_motor_input1.bit_data.torque >> 8));//左侧扭矩
|
||||
// un_motor_status_output.bit_data.left_voltage = ((un_motor_input1.bit_data.bus_voltage << 8) | (un_motor_input1.bit_data.bus_voltage >> 8));//左侧电压
|
||||
@@ -289,7 +289,7 @@ static void requestInput(void *signal_id)
|
||||
}
|
||||
else if(signal_id == &un_motor_input2)
|
||||
{
|
||||
// un_motor_status_output.bit_data.right_wheel_speed = SWAP_ENDIAN_16 ( (uint16_t)((int16_t)(un_motor_input1.bit_data.MotCon_1Signal3) + 30000) );//左侧轮速
|
||||
un_motor_status_output.bit_data.right_wheel_speed = SWAP_ENDIAN_16 ( (uint16_t)((int16_t)(un_motor_input1.bit_data.MotCon_1Signal3) + 30000) );//左侧轮速
|
||||
// un_motor_status_output.bit_data.right_torque = ((un_motor_input2.bit_data.torque << 8) | (un_motor_input2.bit_data.torque >> 8));//右侧扭矩
|
||||
// un_motor_status_output.bit_data.right_fault_code = un_motor_input2.bit_data.fault_code;//右侧故障码
|
||||
// un_motor_status_output.bit_data.right_voltage = ((un_motor_input2.bit_data.bus_voltage << 8) | (un_motor_input2.bit_data.bus_voltage >> 8));//右侧电压
|
||||
|
||||
@@ -11,6 +11,7 @@
|
||||
// 声明 temp_data 变量
|
||||
TempSystem temp_data;
|
||||
|
||||
|
||||
static void handleTemperatureAlarm(int16_t current_temp, float alarm_temp,
|
||||
float critical_temp, float threshold_temp,
|
||||
TempState *state)
|
||||
@@ -86,25 +87,8 @@ static void tempOutput(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
// 电机1风扇 左前
|
||||
// 电机1风扇
|
||||
switch (temp_data.state[0])
|
||||
{
|
||||
case TEMP_NORMAL:
|
||||
un_inf_can_kgf_output1.bit_data.KGF02 = setFanOff();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_02 = 0;
|
||||
break;
|
||||
case TEMP_WARNING:
|
||||
un_inf_can_kgf_output1.bit_data.KGF02 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_02 = 5;
|
||||
break;
|
||||
case TEMP_CRITICAL:
|
||||
un_inf_can_kgf_output1.bit_data.KGF02 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_02 = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
// 电机2风扇 右前
|
||||
switch (temp_data.state[1])
|
||||
{
|
||||
case TEMP_NORMAL:
|
||||
un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇
|
||||
@@ -118,89 +102,39 @@ static void tempOutput(void *signal_id)
|
||||
un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_01 = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
// 电机3风扇 左后
|
||||
switch (temp_data.state[2])
|
||||
}
|
||||
// 电机2风扇
|
||||
switch (temp_data.state[1])
|
||||
{
|
||||
case TEMP_NORMAL:
|
||||
un_inf_can_kgf_output2.bit_data.KGF08 = setFanOff();//电机控制器风扇
|
||||
un_inf_can_kgf_output2.bit_data.pwm_08 = 0;
|
||||
un_inf_can_kgf_output1.bit_data.KGF03 = setFanOff();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_03 = 0;
|
||||
break;
|
||||
case TEMP_WARNING:
|
||||
un_inf_can_kgf_output2.bit_data.KGF08 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output2.bit_data.pwm_08 = 5;
|
||||
un_inf_can_kgf_output1.bit_data.KGF03 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_03 = 5;
|
||||
break;
|
||||
case TEMP_CRITICAL:
|
||||
un_inf_can_kgf_output2.bit_data.KGF08 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output2.bit_data.pwm_08 = 0;
|
||||
un_inf_can_kgf_output1.bit_data.KGF03 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_03 = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
// 电机4风扇 右后
|
||||
switch (temp_data.state[3])
|
||||
{
|
||||
case TEMP_NORMAL:
|
||||
un_inf_can_kgf_output2.bit_data.KGF07 = setFanOff();//电机控制器风扇
|
||||
un_inf_can_kgf_output2.bit_data.pwm_07 = 0;
|
||||
break;
|
||||
case TEMP_WARNING:
|
||||
un_inf_can_kgf_output2.bit_data.KGF07 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output2.bit_data.pwm_07 = 5;
|
||||
break;
|
||||
case TEMP_CRITICAL:
|
||||
un_inf_can_kgf_output2.bit_data.KGF07 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output2.bit_data.pwm_07 = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
// // 电机3风扇
|
||||
// switch (temp_data.state[2])
|
||||
// {
|
||||
// case TEMP_NORMAL:
|
||||
// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇
|
||||
// un_inf_can_kgf_output1.bit_data.pwm_01 = 0;
|
||||
// break;
|
||||
// case TEMP_WARNING:
|
||||
// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇
|
||||
// un_inf_can_kgf_output1.bit_data.pwm_01 = 5;
|
||||
// break;
|
||||
// case TEMP_CRITICAL:
|
||||
// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇
|
||||
// un_inf_can_kgf_output1.bit_data.pwm_01 = 0;
|
||||
// break;
|
||||
// }
|
||||
|
||||
}
|
||||
publishMessage(&un_inf_can_kgf_output1, 1);
|
||||
publishMessage(&un_inf_can_kgf_output2, 1);
|
||||
}
|
||||
|
||||
// 温度状态处理函数
|
||||
static void tempProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
int16_t max_temp[4] = {0,0,0,0};
|
||||
int16_t max_temp[2] = {0,0};
|
||||
|
||||
// 调用按钮处理函数
|
||||
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]);
|
||||
|
||||
handleTemperatureAlarm(max_temp[1], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[1]);
|
||||
|
||||
handleTemperatureAlarm(max_temp[2], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[2]);
|
||||
|
||||
handleTemperatureAlarm(max_temp[3], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[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]);
|
||||
|
||||
handleTemperatureAlarm(max_temp[1], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[1]);
|
||||
|
||||
// if (max_temp[0] >= 60) // 假设60度为危险温度
|
||||
// {
|
||||
@@ -251,15 +185,8 @@ static void tempInput(void *signal_id)
|
||||
{
|
||||
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);
|
||||
}
|
||||
else if(signal_id == &un_motor_temp4)
|
||||
{
|
||||
temp_data.current_temp[3] = ( (int16_t)(un_motor_temp4.bit_data.controller_temp) - 40);
|
||||
}
|
||||
else{}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -278,8 +205,7 @@ void tempAppInit(void)
|
||||
// 订阅输入信号,处理温度逻辑
|
||||
subscribe(&un_motor_temp1, tempInput);
|
||||
subscribe(&un_motor_temp2, tempInput);
|
||||
subscribe(&un_motor_temp3, tempInput);
|
||||
subscribe(&un_motor_temp4, tempInput);
|
||||
|
||||
|
||||
// 启动定时器,每秒检查一次温度
|
||||
subscribe(&temp_data.timer, tempProcess);
|
||||
|
||||
513
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.c
Normal file
513
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.c
Normal file
@@ -0,0 +1,513 @@
|
||||
#include "app_config.h"
|
||||
#include "app_dependence.h"
|
||||
#include "interface.h"
|
||||
#include "app_turntable.h"
|
||||
#include "app_pid.h"
|
||||
#include "app_param_manage.h"
|
||||
|
||||
#include "app_frm_monitor.h"
|
||||
#include "app_frm_signal.h"
|
||||
#include "app_frm_timer.h"
|
||||
|
||||
#include "sdrv_vic.h"
|
||||
|
||||
PID_t turnable_speed_pid;
|
||||
PID_t turnable_position_pid;
|
||||
|
||||
TurnableData turnable_data = {0};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 带死区的原始数据到物理量转换函数（简单版）
|
||||
* @param raw_value 原始16位无符号整数值 [0, 65535]
|
||||
* @param min 物理量最小值（如 -10.0）
|
||||
* @param max 物理量最大值（如 +10.0）
|
||||
* @param deadzone 死区范围（物理量单位，如 1.0 表示 ±1.0 内为死区）
|
||||
* @return 转换后的物理量值（若在死区内返回0，否则返回实际值）
|
||||
*/
|
||||
static float convertPhysical(uint16_t raw_value, float min, float max, float deadzone)
|
||||
{
|
||||
// 1. 计算实际物理量值
|
||||
float physical_value = min + ((float)raw_value / 65535.0f) * (max - min);
|
||||
|
||||
// 2. 判断是否在死区内（绝对值 ≤ deadzone）
|
||||
if (fabs(physical_value) <= deadzone)
|
||||
{
|
||||
return 0.0f; // 死区内返回0
|
||||
}
|
||||
else
|
||||
{
|
||||
return physical_value; // 死区外返回实际值
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 将浮点数转换为uint32_t（按小端序存储）
|
||||
* @param num 输入的浮点数
|
||||
* @return 转换后的uint32_t值（直接内存拷贝结果）
|
||||
* @note 此函数通过内存直接拷贝实现转换，不进行数值计算，结果受平台字节序影响
|
||||
*/
|
||||
uint32_t floatToUint32(float num)
|
||||
{
|
||||
uint32_t result;
|
||||
// 将浮点数的内存数据直接拷贝到uint32_t变量
|
||||
memcpy(&result, &num, sizeof(num));
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 电机失能函数（停止电机运行）
|
||||
* @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
|
||||
* @param master_id 主控制器ID (用于标识发送方)
|
||||
* @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
|
||||
* @return 0: 成功, -1: 参数无效
|
||||
* @note 此函数会修改unsdodata指向的结构体内容，调用后需及时发送CAN报文
|
||||
*/
|
||||
int8_t motorDisable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata)
|
||||
{
|
||||
/* 参数有效性检查 */
|
||||
if (unsdodata == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 设置CAN报文ID域 */
|
||||
unsdodata->tx_can_id.bits.mode = 3; /* 通信模式3：电机失能 */
|
||||
unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */
|
||||
unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */
|
||||
unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */
|
||||
|
||||
/* 清零数据域 */
|
||||
unsdodata->tx_can_data.bit_data.data = 0;
|
||||
unsdodata->tx_can_data.bit_data.index = 0;
|
||||
unsdodata->tx_can_data.bit_data.object_index = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 电机使能函数（启动电机运行）
|
||||
* @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
|
||||
* @param master_id 主控制器ID (用于标识发送方)
|
||||
* @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
|
||||
* @return 0: 成功, -1: 参数无效
|
||||
* @note 通信模式4：电机使能
|
||||
*/
|
||||
int8_t motorEnable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata)
|
||||
{
|
||||
/* 参数有效性检查 */
|
||||
if (unsdodata == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 设置CAN报文ID域 */
|
||||
unsdodata->tx_can_id.bits.mode = 3; /* 通信模式4：电机使能 */
|
||||
unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */
|
||||
unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */
|
||||
unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */
|
||||
|
||||
/* 清零数据域 */
|
||||
unsdodata->tx_can_data.bit_data.data = 0;
|
||||
unsdodata->tx_can_data.bit_data.index = 0;
|
||||
unsdodata->tx_can_data.bit_data.object_index = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置电机运行模式
|
||||
* @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
|
||||
* @param master_id 主控制器ID (用于标识发送方)
|
||||
* @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
|
||||
* @param mode 要设置的模式值 (具体含义需参考电机协议文档)
|
||||
* @return 0: 成功, -1: 参数无效
|
||||
* @note RUM_MODE应为预定义的宏，表示运行模式索引
|
||||
*/
|
||||
int8_t setMotorMode(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint8_t mode)
|
||||
{
|
||||
/* 参数有效性检查 */
|
||||
if (unsdodata == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 设置CAN报文ID域 */
|
||||
unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */
|
||||
unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */
|
||||
unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */
|
||||
unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */
|
||||
|
||||
/* 设置数据域 */
|
||||
unsdodata->tx_can_data.bit_data.index = RUM_MODE; /* 运行模式索引 */
|
||||
unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */
|
||||
unsdodata->tx_can_data.bit_data.data = mode; /* 模式值 */
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 写入电机参数
|
||||
* @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
|
||||
* @param master_id 主控制器ID (用于标识发送方)
|
||||
* @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
|
||||
* @param index 要写入的参数索引 (具体含义需参考电机协议文档)
|
||||
* @param ref 要写入的参数值 (浮点数，会自动转换为uint32_t)
|
||||
* @return 0: 成功, -1: 参数无效
|
||||
* @note 使用floatToUint32函数转换浮点参数
|
||||
*/
|
||||
int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint16_t index, float ref)
|
||||
{
|
||||
/* 参数有效性检查 */
|
||||
if (unsdodata == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 设置CAN报文ID域 */
|
||||
unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */
|
||||
unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */
|
||||
unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */
|
||||
unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */
|
||||
|
||||
/* 设置数据域 */
|
||||
unsdodata->tx_can_data.bit_data.index = index; /* 参数索引 */
|
||||
unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */
|
||||
unsdodata->tx_can_data.bit_data.data = floatToUint32(ref); /* 转换并写入参数值 */
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 动态斜率限制（支持变时间间隔）
|
||||
* @param last_command 上一次的电流指令值
|
||||
* @param target_current 本次目标电流指令
|
||||
* @param delta_time 距离上一次调用的时间间隔 (s)
|
||||
* @return 限制后的安全电流指令
|
||||
*/
|
||||
float dynamic_current_limit(float *last_command, float target_current, float delta_time)
|
||||
{
|
||||
// 计算期望的变化量
|
||||
float desired_change = target_current - *last_command;
|
||||
|
||||
// 计算两种限制
|
||||
float step_limit = MAX_STEP;
|
||||
float time_limit = MAX_DI_DT * delta_time; // 动态计算时间限制
|
||||
|
||||
// 选择更严格的限制
|
||||
float max_allowed_change = (step_limit < time_limit) ? step_limit : time_limit;
|
||||
|
||||
// 应用限制并返回新指令
|
||||
float actual_change = constrain(desired_change, -max_allowed_change, max_allowed_change);
|
||||
*last_command = *last_command + actual_change;//更新过去值
|
||||
|
||||
return *last_command + actual_change;
|
||||
}
|
||||
|
||||
static void setTurnableMotorOutput()
|
||||
{
|
||||
static float previous_time2 = 0.0f;
|
||||
|
||||
float time1 = (float)getCurrentTime();
|
||||
float dt = (time1 - previous_time2) / PERIOD_TICK;
|
||||
previous_time2 = time1;
|
||||
|
||||
// turnable_data.out_pitch_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_pitch_motor_ampere_last,turnable_data.out_pitch_motor_ampere,dt);
|
||||
turnable_data.out_right_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_right_motor_ampere_last,turnable_data.out_right_motor_ampere,dt);
|
||||
turnable_data.out_left_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_left_motor_ampere_last ,turnable_data.out_left_motor_ampere ,dt);
|
||||
|
||||
setMotorWrite(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, LIMIT_SPEED_INDEX,turnable_data.out_pitch_motor_ampere);
|
||||
setMotorWrite(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output4, LOC_REF_INDEX,turnable_data.desired_pitch_position);
|
||||
|
||||
setMotorWrite(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, IQ_REF_INDEX,turnable_data.out_right_motor_ampere_limit);
|
||||
setMotorWrite(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3, IQ_REF_INDEX,turnable_data.out_left_motor_ampere_limit);
|
||||
|
||||
|
||||
|
||||
|
||||
un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(turnable_data.speed*10);
|
||||
un_can_debug_output.bit_data.desired_speed = (uint8_t)(int8_t)(turnable_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)(turnable_data.out_left_motor_ampere_limit * 100);
|
||||
un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(turnable_data.out_right_motor_ampere_limit*100);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
publishMessage(&un_sdo_output1, 1);
|
||||
publishMessage(&un_sdo_output2, 1);
|
||||
publishMessage(&un_sdo_output3, 1);
|
||||
publishMessage(&un_sdo_output4, 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 转台
|
||||
static void turnableProcess(void *signal_id)
|
||||
{
|
||||
|
||||
// static float previous_time1 = 0.0f;
|
||||
|
||||
// float time1 = (float)getCurrentTime();
|
||||
// float dt = (time1 - previous_time1) / PERIOD_TICK;
|
||||
// previous_time1 = time1;
|
||||
// if((turnable_data.current_state == POWER_WORKING))//高压上电才运行
|
||||
// {
|
||||
switch(turnable_data.turnable_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据
|
||||
{
|
||||
case 0:
|
||||
timerStart(&turnable_data.turnable_timer, 3000, 1); // 启动定时器，1s
|
||||
turnable_data.turnable_state = 1;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if (!turnable_data.turnable_timer.active)// 1s定时
|
||||
{
|
||||
turnable_data.turnable_state = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
turnable_data.turnable_state = 1;
|
||||
}
|
||||
break;
|
||||
|
||||
case 2://模式设置
|
||||
if(turnable_data.turnable_cnt >= 5)//发送5次
|
||||
{
|
||||
turnable_data.turnable_cnt = 0;
|
||||
turnable_data.turnable_state = 3;
|
||||
}
|
||||
else
|
||||
{
|
||||
turnable_data.turnable_cnt ++;
|
||||
turnable_data.turnable_state = 2;
|
||||
|
||||
setMotorMode(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, POSITION_MODE_CSP);
|
||||
setMotorMode(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, CURRENT_MODE);
|
||||
setMotorMode(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3, CURRENT_MODE);
|
||||
|
||||
publishMessage(&un_sdo_output1, 1);
|
||||
publishMessage(&un_sdo_output2, 1);
|
||||
publishMessage(&un_sdo_output3, 1);
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
case 3:
|
||||
if(turnable_data.turnable_cnt >= 5)//发送5次
|
||||
{
|
||||
turnable_data.turnable_cnt = 0;
|
||||
turnable_data.turnable_state = 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
turnable_data.turnable_cnt ++;
|
||||
turnable_data.turnable_state = 3;
|
||||
|
||||
motorEnable(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1);
|
||||
motorEnable(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2);
|
||||
motorEnable(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3);
|
||||
|
||||
publishMessage(&un_sdo_output1, 1);
|
||||
publishMessage(&un_sdo_output2, 1);
|
||||
publishMessage(&un_sdo_output3, 1);
|
||||
}
|
||||
break;
|
||||
|
||||
case 4:
|
||||
turnable_data.turnable_cnt = 0;
|
||||
turnable_data.turnable_state = 4;
|
||||
setTurnableMotorOutput();//输出函数
|
||||
break;
|
||||
|
||||
default:break;
|
||||
}
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// turnable_data.turnable_cnt ++;
|
||||
// turnable_data.turnable_state = 0;
|
||||
// }
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void turnableParametersInit(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||||
|
||||
setPidParameters(&turnable_speed_pid,
|
||||
getParam("spd_kp"),
|
||||
getParam("spd_ki"),
|
||||
getParam("spd_kd"),
|
||||
getParam("spd_il"),
|
||||
getParam("spd_ol")
|
||||
);
|
||||
|
||||
// if(0 == un_right_intput.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
|
||||
// {
|
||||
// motorEnable(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output5);
|
||||
// publishMessage(&un_sdo_output5, 1);
|
||||
// }
|
||||
//
|
||||
// if(0 == un_turn_intput.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
|
||||
// {
|
||||
// motorEnable(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output5);
|
||||
// publishMessage(&un_sdo_output5, 1);
|
||||
// }
|
||||
//
|
||||
// if(0 == un_pitch_intput.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
|
||||
// {
|
||||
// motorEnable(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output5);
|
||||
// publishMessage(&un_sdo_output5, 1);
|
||||
// }
|
||||
|
||||
turnable_data.min_pitch_postion = getParam("minYpos"); //俯仰位置最小限制值
|
||||
turnable_data.max_pitch_postion = getParam("maxYpos"); //俯仰位置最大限制值
|
||||
|
||||
printf( "turnable left %f\n",turnable_data.out_left_motor_ampere);
|
||||
printf( "turnable right %f\n",turnable_data.out_right_motor_ampere);
|
||||
printf( "turnable pitch %f\n",turnable_data.out_pitch_motor_ampere);
|
||||
printf( "turnable x_axis %d\n",un_remote_control_input.bit_data.x_axis);
|
||||
// printf( "speed %f\n",turnable_data.speed);
|
||||
// printf( "turnable state %d\n",turnable_data.turnable_state);
|
||||
|
||||
timerStart(&turnable_data.turnable_timer1,1000,1);//1s调用一次
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 差速输入处理函数
|
||||
static void turnableInput(void *signal_id)
|
||||
{
|
||||
if(signal_id == &power_data)//电机上电
|
||||
{
|
||||
turnable_data.current_state = power_data.current_state;
|
||||
}
|
||||
else if(signal_id == &un_computer_turnable_Input)
|
||||
{
|
||||
turnable_data.desired_speed = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_x) );
|
||||
}
|
||||
else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据
|
||||
{
|
||||
float x_axis_temp = (float)(un_remote_control_input.bit_data.x_axis) - REMOTE_ZERO;
|
||||
if( ( x_axis_temp > 50 ) || ( x_axis_temp < -50 ) )
|
||||
{
|
||||
turnable_data.out_left_motor_ampere = 0.01*(x_axis_temp);//计算电流
|
||||
turnable_data.out_right_motor_ampere = turnable_data.out_left_motor_ampere;
|
||||
}
|
||||
else
|
||||
{
|
||||
turnable_data.out_left_motor_ampere = 0;//计算电流
|
||||
turnable_data.out_right_motor_ampere = turnable_data.out_left_motor_ampere;
|
||||
}
|
||||
|
||||
x_axis_temp = (float)(un_remote_control_input.bit_data.y_axis) - REMOTE_ZERO;
|
||||
if(x_axis_temp > 50) //根据Y轴数据来定义
|
||||
{
|
||||
turnable_data.out_pitch_motor_ampere = 0.02*fabs(x_axis_temp);
|
||||
turnable_data.desired_pitch_position = turnable_data.max_pitch_postion;
|
||||
}
|
||||
else if(x_axis_temp < -50)
|
||||
{
|
||||
turnable_data.out_pitch_motor_ampere = 0.02*fabs(x_axis_temp);
|
||||
turnable_data.desired_pitch_position = turnable_data.min_pitch_postion;
|
||||
}
|
||||
else
|
||||
{
|
||||
turnable_data.out_pitch_motor_ampere = 0;
|
||||
}
|
||||
}
|
||||
else{}
|
||||
|
||||
turnable_data.right_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_right_intput.rx_can_data.bit_data.current_velocity),ANGULAR_VELOCITY_MIN,ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE );
|
||||
turnable_data.left_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_turn_intput.rx_can_data.bit_data.current_velocity) ,ANGULAR_VELOCITY_MIN,ANGULAR_VELOCITY_MAX, MOTOR_VELOCITY_DEADZONE );
|
||||
turnable_data.speed = (turnable_data.right_motor_speed + turnable_data.left_motor_speed)/2.0f;
|
||||
|
||||
// 发布左右电机期望转速,电源在工作状态才能发送
|
||||
if ( (power_data.current_state == POWER_STANDBY) || (power_data.current_state == POWER_SLEEP) )//这几种状态可以转转台
|
||||
{
|
||||
turnable_data.turnable_state = 0;//清空状态。保证每次上电都初始化
|
||||
}
|
||||
else
|
||||
{
|
||||
turnableProcess(signal_id);//处理映射
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void turnableInit()
|
||||
{
|
||||
// 初始化速度 PID 控制器
|
||||
initializePid(&turnable_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
|
||||
|
||||
// 设置速度 PID 控制器的参数
|
||||
setPidParameters(&turnable_speed_pid,
|
||||
getParam("spd_kp"),
|
||||
getParam("spd_ki"),
|
||||
getParam("spd_kd"),
|
||||
getParam("spd_il"),
|
||||
getParam("spd_ol")
|
||||
);
|
||||
|
||||
subscribe(&un_computer_turnable_Input, turnableInput);
|
||||
subscribe(&un_remote_control_input, turnableInput);
|
||||
|
||||
timerInit(&turnable_data.turnable_timer);
|
||||
timerInit(&turnable_data.turnable_timer1);
|
||||
|
||||
|
||||
subscribe(&turnable_data.turnable_timer1, turnableParametersInit);
|
||||
timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次
|
||||
|
||||
turnable_data.turnable_state = 0;
|
||||
un_right_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE;
|
||||
un_right_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE;
|
||||
un_right_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE;
|
||||
|
||||
un_turn_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE;
|
||||
un_turn_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE;
|
||||
un_turn_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE;
|
||||
|
||||
un_pitch_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE;
|
||||
un_pitch_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE;
|
||||
un_pitch_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE;
|
||||
|
||||
printf( "turnable: initial OK %d\n",getCurrentTime());
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
121
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.h
Normal file
121
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.h
Normal file
@@ -0,0 +1,121 @@
|
||||
#ifndef TURNTAABLE_H
|
||||
#define TURNTAABLE_H
|
||||
|
||||
|
||||
#include "app_power.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define MASTER_CANID 0xFD
|
||||
|
||||
#define PITCH_MOTOR_CANID 0x7D
|
||||
#define RIGHT_MOTOR_CANID 0x7E
|
||||
#define TURN_MOTOR_CANID 0x7F
|
||||
|
||||
#define PITCH_MOTOR_RxCANID (0x20000FD + (PITCH_MOTOR_CANID << 8)) // 0x2007DFD
|
||||
#define RIGHT_MOTOR_RxCANID (0x20000FD + (RIGHT_MOTOR_CANID << 8)) // 0x2007EFD
|
||||
#define TURN_MOTOR_RxCANID (0x20000FD + (TURN_MOTOR_CANID << 8)) // 0x2007FFD
|
||||
|
||||
#define MOTOR_RxCAN_Mask 0x1F00FFFF //<2F><><EFBFBD><EFBFBD>CAN<41><4E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20>Ƴ<EFBFBD><C6B3><EFBFBD><EFBFBD><EFBFBD>λ
|
||||
|
||||
#define LIMIT_SPEED_INDEX 0x7017//CSP<53><50><EFBFBD>ٶ<EFBFBD>
|
||||
#define LOC_REF_INDEX 0x7016//CSP<53><50>λ<EFBFBD><CEBB>
|
||||
#define IQ_REF_INDEX 0x7006//<2F><><EFBFBD><EFBFBD>ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
#define RUM_MODE 0x7005//modeģʽ
|
||||
|
||||
#define OPERATION_MODE 0 // <20>˿<EFBFBD>ģʽ
|
||||
#define POSITION_MODE_PP 1 // λ<><CEBB>ģʽ (PP - Profile Position)
|
||||
#define VELOCITY_MODE 2 // <20>ٶ<EFBFBD>ģʽ
|
||||
#define CURRENT_MODE 3 // <20><><EFBFBD><EFBFBD>ģʽ
|
||||
#define POSITION_MODE_CSP 5 // λ<><CEBB>ģʽ (CSP - Cyclic Synchronous Position)
|
||||
|
||||
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD>仯<EFBFBD><E4BBAF> (A)
|
||||
#define MAX_STEP 1.0f // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>仯<EFBFBD><E4BBAF> (A)<29><><EFBFBD><EFBFBD>5A<35><41>ʼ
|
||||
#define MAX_DI_DT 1000.0f // <20><><EFBFBD><EFBFBD><EFBFBD>仯<EFBFBD><E4BBAF> (A/s)<29><><EFBFBD><EFBFBD>5000A/s<><73>ʼ
|
||||
|
||||
|
||||
// <20><>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
#define PI 3.14159f // <20>߾<EFBFBD><DFBE>Ȧ<EFBFBD>ֵ
|
||||
|
||||
// <20>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD> (<28><>ӦByte0~1: <20><>ǰ<EFBFBD>Ƕ<EFBFBD>)
|
||||
#define ANGLE_RANGE_MIN (-4.0f * PI) // <20><>С<EFBFBD>Ƕ<EFBFBD>: -4<><34> <20><><EFBFBD><EFBFBD>
|
||||
#define ANGLE_RANGE_MAX (4.0f * PI) // <20><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>: 4<><34> <20><><EFBFBD><EFBFBD>
|
||||
|
||||
// <20><><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD> (<28><>ӦByte2~3: <20><>ǰ<EFBFBD><C7B0><EFBFBD>ٶ<EFBFBD>)
|
||||
#define ANGULAR_VELOCITY_MIN -15.0f // <20><>С<EFBFBD><D0A1><EFBFBD>ٶ<EFBFBD>: -15 rad/s
|
||||
#define ANGULAR_VELOCITY_MAX 15.0f // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>: 15 rad/s
|
||||
|
||||
|
||||
#define MOTOR_VELOCITY_DEADZONE 1.0f// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: 120 Nm
|
||||
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD> (<28><>ӦByte4~5: <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>)
|
||||
#define TORQUE_MIN -120.0f // <20><>С<EFBFBD><D0A1><EFBFBD><EFBFBD>: -120 Nm
|
||||
#define TORQUE_MAX 120.0f // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: 120 Nm
|
||||
|
||||
#define ZERO_VAULE 0x0080 // 32768,<2C><>Ҫ<EFBFBD><D2AA>λ<EFBFBD><CEBB>ǰ
|
||||
|
||||
#define REMOTE_ZERO 980
|
||||
|
||||
|
||||
#define SWAP_ENDIAN_16(x) ((((x) & 0xFF) << 8) | (((x) >> 8) & 0xFF))
|
||||
#define SWAP_ENDIAN_32(x) (((x) << 24) | (((x) & 0xFF00) << 8) | (((x) >> 8) & 0xFF00) | ((x) >> 24))
|
||||
|
||||
|
||||
|
||||
typedef struct TurnableData
|
||||
{
|
||||
uint8_t turnable_state;
|
||||
PowerState current_state; // <20><>ǰ<EFBFBD><C7B0>Դ״̬
|
||||
|
||||
float position_x; //ת̨<D7AA><CCA8><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>x
|
||||
float position_y; //ת̨<D7AA><CCA8><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>y
|
||||
float position_z; //ת̨<D7AA><CCA8><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>z
|
||||
|
||||
float desired_speed; // <20><><EFBFBD><EFBFBD>ת̨<D7AA>ٶ<EFBFBD>
|
||||
float desired_pitch_position; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
|
||||
float desired_horizontal_position; // <20><><EFBFBD><EFBFBD>ˮƽλ<C6BD><CEBB>
|
||||
|
||||
float left_motor_speed; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
|
||||
float right_motor_speed; // <20><>ǰ<EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>ٶ<EFBFBD>
|
||||
float speed; // <20><>ǰת<C7B0><D7AA><EFBFBD>ٶ<EFBFBD>
|
||||
float pitch_position; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
|
||||
float horizontal_position; // <20><>ǰˮƽλ<C6BD><CEBB>
|
||||
|
||||
float max_speed; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
|
||||
float out_left_motor_ampere; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
float out_right_motor_ampere; // <20><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
float out_pitch_motor_ampere; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
float out_left_motor_ampere_last; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
float out_right_motor_ampere_last; // <20><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
float out_pitch_motor_ampere_last; // <20><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
float out_left_motor_ampere_limit; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
||||
float out_right_motor_ampere_limit; // <20><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
||||
float out_pitch_motor_ampere_limit; // <20><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
||||
|
||||
Timer turnable_timer; // <20><>ʱ<EFBFBD><CAB1>
|
||||
Timer turnable_timer1; // <20><>ʱ<EFBFBD><CAB1>
|
||||
Timer turnable_timer2; // <20><>ʱ<EFBFBD><CAB1>
|
||||
uint8_t turnable_cnt;
|
||||
|
||||
float min_pitch_postion; // λ<><CEBB><EFBFBD><EFBFBD>Ϣ
|
||||
float max_pitch_postion; // λ<><CEBB><EFBFBD><EFBFBD>Ϣ
|
||||
|
||||
float max_ampere; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
} TurnableData;
|
||||
|
||||
|
||||
|
||||
void turnableInit();
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // TURNTAABLE_H
|
||||
228
boards/e3_176_ref/app_demo/eth-xip/sf/drive_rs04.c
Normal file
228
boards/e3_176_ref/app_demo/eth-xip/sf/drive_rs04.c
Normal file
@@ -0,0 +1,228 @@
|
||||
#include "interface.h"
|
||||
#include "drive_rs04.h"
|
||||
|
||||
// 限制值在最小值和最大值之间
|
||||
float constrain(float value, float min_val, float max_val)
|
||||
{
|
||||
if (value < min_val)
|
||||
{
|
||||
return min_val;
|
||||
}
|
||||
else if (value > max_val)
|
||||
{
|
||||
return max_val;
|
||||
}
|
||||
else
|
||||
{
|
||||
return value;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 带死区的原始数据到物理量转换函数（简单版）
|
||||
* @param raw_value 原始16位无符号整数值 [0, 65535]
|
||||
* @param min 物理量最小值（如 -10.0）
|
||||
* @param max 物理量最大值（如 +10.0）
|
||||
* @param deadzone 死区范围（物理量单位，如 1.0 表示 ±1.0 内为死区）
|
||||
* @return 转换后的物理量值（若在死区内返回0，否则返回实际值）
|
||||
*/
|
||||
float convertPhysical(uint16_t raw_value, float min, float max, float deadzone)
|
||||
{
|
||||
// 1. 计算实际物理量值
|
||||
float physical_value = min + ((float)raw_value / 65535.0f) * (max - min);
|
||||
|
||||
// 2. 判断是否在死区内（绝对值 ≤ deadzone）
|
||||
if (fabs(physical_value) <= deadzone)
|
||||
{
|
||||
return 0.0f; // 死区内返回0
|
||||
}
|
||||
else
|
||||
{
|
||||
return physical_value; // 死区外返回实际值
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 将浮点数转换为uint32_t（按小端序存储）
|
||||
* @param num 输入的浮点数
|
||||
* @return 转换后的uint32_t值（直接内存拷贝结果）
|
||||
* @note 此函数通过内存直接拷贝实现转换，不进行数值计算，结果受平台字节序影响
|
||||
*/
|
||||
uint32_t floatToUint32(float num)
|
||||
{
|
||||
uint32_t result;
|
||||
// 将浮点数的内存数据直接拷贝到uint32_t变量
|
||||
memcpy(&result, &num, sizeof(num));
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 电机失能函数（停止电机运行）
|
||||
* @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
|
||||
* @param master_id 主控制器ID (用于标识发送方)
|
||||
* @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
|
||||
* @return 0: 成功, -1: 参数无效
|
||||
* @note 此函数会修改unsdodata指向的结构体内容，调用后需及时发送CAN报文
|
||||
*/
|
||||
int8_t motorDisable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata)
|
||||
{
|
||||
/* 参数有效性检查 */
|
||||
if (unsdodata == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 设置CAN报文ID域 */
|
||||
unsdodata->tx_can_id.bits.mode = 3; /* 通信模式3：电机失能 */
|
||||
unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */
|
||||
unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */
|
||||
unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */
|
||||
|
||||
/* 清零数据域 */
|
||||
unsdodata->tx_can_data.bit_data.data = 0;
|
||||
unsdodata->tx_can_data.bit_data.index = 0;
|
||||
unsdodata->tx_can_data.bit_data.object_index = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 电机使能函数（启动电机运行）
|
||||
* @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
|
||||
* @param master_id 主控制器ID (用于标识发送方)
|
||||
* @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
|
||||
* @return 0: 成功, -1: 参数无效
|
||||
* @note 通信模式4：电机使能
|
||||
*/
|
||||
int8_t motorEnable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata)
|
||||
{
|
||||
/* 参数有效性检查 */
|
||||
if (unsdodata == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 设置CAN报文ID域 */
|
||||
unsdodata->tx_can_id.bits.mode = 3; /* 通信模式4：电机使能 */
|
||||
unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */
|
||||
unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */
|
||||
unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */
|
||||
|
||||
/* 清零数据域 */
|
||||
unsdodata->tx_can_data.bit_data.data = 0;
|
||||
unsdodata->tx_can_data.bit_data.index = 0;
|
||||
unsdodata->tx_can_data.bit_data.object_index = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置电机运行模式
|
||||
* @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
|
||||
* @param master_id 主控制器ID (用于标识发送方)
|
||||
* @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
|
||||
* @param mode 要设置的模式值 (具体含义需参考电机协议文档)
|
||||
* @return 0: 成功, -1: 参数无效
|
||||
* @note RUM_MODE应为预定义的宏，表示运行模式索引
|
||||
*/
|
||||
int8_t setMotorMode(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint8_t mode)
|
||||
{
|
||||
/* 参数有效性检查 */
|
||||
if (unsdodata == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 设置CAN报文ID域 */
|
||||
unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */
|
||||
unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */
|
||||
unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */
|
||||
unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */
|
||||
|
||||
/* 设置数据域 */
|
||||
unsdodata->tx_can_data.bit_data.index = RUM_MODE; /* 运行模式索引 */
|
||||
unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */
|
||||
unsdodata->tx_can_data.bit_data.data = mode; /* 模式值 */
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 写入电机参数
|
||||
* @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
|
||||
* @param master_id 主控制器ID (用于标识发送方)
|
||||
* @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
|
||||
* @param index 要写入的参数索引 (具体含义需参考电机协议文档)
|
||||
* @param ref 要写入的参数值 (浮点数，会自动转换为uint32_t)
|
||||
* @return 0: 成功, -1: 参数无效
|
||||
* @note 使用floatToUint32函数转换浮点参数
|
||||
*/
|
||||
int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint16_t index, float ref)
|
||||
{
|
||||
/* 参数有效性检查 */
|
||||
if (unsdodata == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 设置CAN报文ID域 */
|
||||
unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */
|
||||
unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */
|
||||
unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */
|
||||
unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */
|
||||
|
||||
/* 设置数据域 */
|
||||
unsdodata->tx_can_data.bit_data.index = index; /* 参数索引 */
|
||||
unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */
|
||||
unsdodata->tx_can_data.bit_data.data = floatToUint32(ref); /* 转换并写入参数值 */
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 动态斜率限制（支持变时间间隔）
|
||||
* @param last_command 上一次的电流指令值
|
||||
* @param target_current 本次目标电流指令
|
||||
* @param delta_time 距离上一次调用的时间间隔 (s)
|
||||
* @return 限制后的安全电流指令
|
||||
*/
|
||||
float dynamic_current_limit(float *last_command, float target_current, float delta_time)
|
||||
{
|
||||
// 1. 参数有效性检查
|
||||
if (last_command == NULL)
|
||||
{
|
||||
return 0.0f; // 或者返回安全默认值
|
||||
}
|
||||
|
||||
if (!isfinite(*last_command) || !isfinite(target_current) || !isfinite(delta_time)) {
|
||||
return *last_command; // 输入异常时保持原值
|
||||
}
|
||||
|
||||
// 2. 时间间隔安全性检查
|
||||
if (delta_time <= 0.0f) {
|
||||
// 使用最小安全时间间隔或直接返回原值
|
||||
delta_time = 0.001f; // 1ms默认值
|
||||
}
|
||||
|
||||
// 计算期望的变化量
|
||||
float desired_change = target_current - *last_command;
|
||||
|
||||
// 计算两种限制
|
||||
float step_limit = MAX_STEP;
|
||||
float time_limit = MAX_DI_DT * delta_time; // 动态计算时间限制
|
||||
|
||||
// 选择更严格的限制
|
||||
float max_allowed_change = (step_limit < time_limit) ? step_limit : time_limit;
|
||||
|
||||
// 应用限制并返回新指令
|
||||
float actual_change = constrain(desired_change, -max_allowed_change, max_allowed_change);
|
||||
*last_command = *last_command + actual_change;//更新过去值
|
||||
|
||||
return *last_command;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
69
boards/e3_176_ref/app_demo/eth-xip/sf/drive_rs04.h
Normal file
69
boards/e3_176_ref/app_demo/eth-xip/sf/drive_rs04.h
Normal file
@@ -0,0 +1,69 @@
|
||||
#ifndef _DRIVE_RS04_H_
|
||||
#define _DRIVE_RS04_H_
|
||||
|
||||
|
||||
#define MASTER_CANID 0xFD
|
||||
|
||||
#define PI 3.1415926
|
||||
|
||||
#define MOTOR_RxCAN_Mask 0x1F00FFFF //<2F><><EFBFBD><EFBFBD>CAN<41><4E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20>Ƴ<EFBFBD><C6B3><EFBFBD><EFBFBD><EFBFBD>λ
|
||||
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD>仯<EFBFBD><E4BBAF> (A)
|
||||
#define MAX_STEP 1.0f // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>仯<EFBFBD><E4BBAF> (A)<29><><EFBFBD><EFBFBD>5A<35><41>ʼ
|
||||
#define MAX_DI_DT 1000.0f // <20><><EFBFBD><EFBFBD><EFBFBD>仯<EFBFBD><E4BBAF> (A/s)<29><><EFBFBD><EFBFBD>5000A/s<><73>ʼ
|
||||
|
||||
|
||||
#define LIMIT_SPEED_INDEX 0x7017//CSP<53><50><EFBFBD>ٶ<EFBFBD>
|
||||
#define LOC_REF_INDEX 0x7016//CSP<53><50>λ<EFBFBD><CEBB>
|
||||
#define IQ_REF_INDEX 0x7006//<2F><><EFBFBD><EFBFBD>ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
#define SPD_REF 0x700A//<2F>ٶ<EFBFBD>ģʽ <20>ٶ<EFBFBD>ֵ
|
||||
|
||||
#define LIMIT_CUR 0X7018//<2F>ٶ<EFBFBD>λ<EFBFBD><CEBB>ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
|
||||
|
||||
#define RUM_MODE 0x7005//modeģʽ
|
||||
|
||||
#define OPERATION_MODE 0 // <20>˿<EFBFBD>ģʽ
|
||||
#define POSITION_MODE_PP 1 // λ<><CEBB>ģʽ (PP - Profile Position)
|
||||
#define VELOCITY_MODE 2 // <20>ٶ<EFBFBD>ģʽ
|
||||
#define CURRENT_MODE 3 // <20><><EFBFBD><EFBFBD>ģʽ
|
||||
#define POSITION_MODE_CSP 5 // λ<><CEBB>ģʽ (CSP - Cyclic Synchronous Position)
|
||||
|
||||
// <20>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD> (<28><>ӦByte0~1: <20><>ǰ<EFBFBD>Ƕ<EFBFBD>)
|
||||
#define ANGLE_RANGE_MIN (-4.0f * PI) // <20><>С<EFBFBD>Ƕ<EFBFBD>: -4<><34> <20><><EFBFBD><EFBFBD>
|
||||
#define ANGLE_RANGE_MAX (4.0f * PI) // <20><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>: 4<><34> <20><><EFBFBD><EFBFBD>
|
||||
|
||||
// <20><><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD> (<28><>ӦByte2~3: <20><>ǰ<EFBFBD><C7B0><EFBFBD>ٶ<EFBFBD>)
|
||||
#define RS02_ANGULAR_VELOCITY_MAX 20.0f // RS02<30>ͺ<EFBFBD><CDBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>: 20 rad/s
|
||||
#define RS04_ANGULAR_VELOCITY_MAX 15.0f // RS04<30>ͺ<EFBFBD><CDBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>: 15 rad/s
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#define MOTOR_VELOCITY_DEADZONE 0.1f// <20><><EFBFBD><EFBFBD>
|
||||
#define MOTOR_ANGLE_DEADZONE 0.01f// <20><><EFBFBD><EFBFBD>
|
||||
|
||||
#define SWAP_ENDIAN_16(x) ((((x) & 0xFF) << 8) | (((x) >> 8) & 0xFF))
|
||||
#define SWAP_ENDIAN_32(x) (((x) << 24) | (((x) & 0xFF00) << 8) | (((x) >> 8) & 0xFF00) | ((x) >> 24))
|
||||
|
||||
|
||||
|
||||
float constrain(float value, float min_val, float max_val);
|
||||
float convertPhysical(uint16_t raw_value, float min, float max, float deadzone);
|
||||
uint32_t floatToUint32(float num);
|
||||
int8_t motorDisable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata);
|
||||
int8_t motorEnable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata);
|
||||
int8_t setMotorMode(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint8_t mode);
|
||||
int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint16_t index, float ref);
|
||||
float dynamic_current_limit(float *last_command, float target_current, float delta_time);
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // _DRIVE_RS04_H_
|
||||
@@ -7,12 +7,8 @@ UnMotorInput un_motor_input2 ;//电机控制器2
|
||||
UnMotorInput un_motor_input3 ;//电机控制器1 左后侧
|
||||
UnMotorInput un_motor_input4 ;//电机控制器2 右后侧
|
||||
|
||||
|
||||
UnMotorTempInput un_motor_temp1 ;//电机控制器1 左前侧
|
||||
UnMotorTempInput un_motor_temp2 ;//电机控制器2 右前侧
|
||||
UnMotorTempInput un_motor_temp3 ;//电机控制器3 左后侧
|
||||
UnMotorTempInput un_motor_temp4 ;//电机控制器4 右后侧
|
||||
|
||||
UnMotorTempInput un_motor_temp1 ;//电机控制器1 温度
|
||||
UnMotorTempInput un_motor_temp2 ;//电机控制器2 温度
|
||||
|
||||
|
||||
UnBmsInput un_bms_input ;//BMS接收数据
|
||||
@@ -31,8 +27,6 @@ UnWheelSpeedOutput un_wheel_wpeed_output ;//轮速输出
|
||||
UnRemoteControlInput un_remote_control_input ;//遥控器输入
|
||||
|
||||
UnHBridgeOutput un_h_bridge_output ;//H桥输出
|
||||
UnHBridgeOutput un_h_bridge_output1 ;//H桥输出
|
||||
|
||||
|
||||
UnGatherOutput un_gather_output ;//采集模块输出
|
||||
|
||||
@@ -40,6 +34,24 @@ UnUltrasonicInput un_ultrasonic_input1 ;//超声波传感
|
||||
|
||||
UnUltrasonicOutput un_ultrasonic_output1 ;//超声波传感器输出
|
||||
|
||||
//UnHBridgeOutput un_h_bridge_output1 ;//左太阳能板电机
|
||||
//UnHBridgeOutput un_h_bridge_output2 ;//右太阳能板电机
|
||||
//
|
||||
//UnLifterOutput un_lifter_output ;//基站升降杆输出
|
||||
|
||||
|
||||
|
||||
StrTxCanFrame un_sdo_output1 ;//电机1输出
|
||||
StrTxCanFrame un_sdo_output2 ;//电机2输出
|
||||
StrTxCanFrame un_sdo_output3 ;//电机3输出
|
||||
StrTxCanFrame un_sdo_output4 ;//电机3速度输出
|
||||
|
||||
StrTxCanFrame un_sdo_output5 ;//电机使能输出
|
||||
|
||||
StrRxCanFrame un_pitch_intput ;//电机输入
|
||||
StrRxCanFrame un_right_intput ;//电机输入
|
||||
StrRxCanFrame un_turn_intput ;//电机输入
|
||||
|
||||
|
||||
//IO口
|
||||
UnSwSample un_sw_sample ;//采集
|
||||
@@ -50,7 +62,13 @@ UnAutoComputerInput un_auto_computer_input ;//自主计算机
|
||||
UnManualComputerInput un_manual_computer_input ;//自主计算机手动数据
|
||||
UnRequestFrame un_request_frame ;//请求帧
|
||||
|
||||
UnComputerTurnableInput un_computer_turnable_Input ;//转台以太网输入
|
||||
|
||||
|
||||
UnComputerOutput un_computer_output ;//输出给自主计算机
|
||||
|
||||
|
||||
|
||||
|
||||
//输出给上位机
|
||||
UnVehicleInfoOutput un_vehicle_Info_output ;// 车辆信息，输出给上位机
|
||||
@@ -68,5 +86,20 @@ UnCanDebugOutput un_can_debug_output;//调试输出
|
||||
|
||||
|
||||
|
||||
|
||||
// 限制值在最小值和最大值之间
|
||||
float constrain(float value, float min_val, float max_val)
|
||||
{
|
||||
if (value < min_val)
|
||||
{
|
||||
return min_val;
|
||||
}
|
||||
else if (value > max_val)
|
||||
{
|
||||
return max_val;
|
||||
}
|
||||
else
|
||||
{
|
||||
return value;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -25,14 +25,15 @@
|
||||
// 接收电机控制器输入
|
||||
typedef struct _StrMotorInput
|
||||
{
|
||||
//-----接收数据0x101或者0x201----------------------------------------------
|
||||
unsigned int speed : 16; // 转速 偏移量 -30000
|
||||
unsigned int bus_voltage : 16; // 母线电压 系数 0.1 偏移量 -3000
|
||||
unsigned int torque : 16; // 扭矩 系数 0.01 偏移量 -300 实际物理量＝数据×系数＋偏移量
|
||||
unsigned int fault_code : 8; // 故障码
|
||||
unsigned int heartbeat : 8; // 心跳
|
||||
//-----接收数据0x589或者0x189----------------------------------------------
|
||||
uint8_t MotCon_1Signal1 ;
|
||||
uint8_t MotCon_1Signal2 ;
|
||||
uint16_t MotCon_1Signal3 ;
|
||||
uint16_t MotCon_1Signal4 ;
|
||||
uint16_t MotCon_1Signal5 ;
|
||||
|
||||
} StrMotorInput;
|
||||
} StrMotorInput;
|
||||
|
||||
typedef union _UnMotorInput
|
||||
{
|
||||
StrMotorInput bit_data; // 使用定义的结构体变量名
|
||||
@@ -190,11 +191,32 @@ typedef struct _StrManualComputerInput
|
||||
typedef union _UnManualComputerInput
|
||||
{
|
||||
StrManualComputerInput bit_data; // 使用定义的结构体变量名
|
||||
unsigned int arr[sizeof(StrManualComputerInput) / sizeof(unsigned int)]; // 通过结构体类型确定大小
|
||||
uint8_t arr[sizeof(StrManualComputerInput)]; // 通过结构体类型确定大小
|
||||
} UnManualComputerInput;
|
||||
|
||||
|
||||
// 接收转台指令输入
|
||||
typedef struct _StrComputerTurnableInput
|
||||
{
|
||||
// 多字节数据，高位在前，低位在后
|
||||
unsigned int frame_header : 16; // 帧头 固定值0xFFCC
|
||||
unsigned int frame_type : 16; // 帧类型 固定值0x0001
|
||||
unsigned int frame_length : 8; // 帧长 固定值0x19
|
||||
unsigned int heartbeat : 8; // 心跳 按帧累加
|
||||
|
||||
// --- 坐标数据部分 ---
|
||||
int32_t position_x; // X轴坐标
|
||||
int32_t position_y; // Y轴坐标
|
||||
int32_t position_z; // Z轴坐标
|
||||
|
||||
unsigned int crc : 8; // CRC 按字节累加之和，溢出取低8位
|
||||
} StrComputerTurnableInput;
|
||||
|
||||
typedef union _UnComputerTurnableInput
|
||||
{
|
||||
StrComputerTurnableInput bit_data; // 使用定义的结构体变量名
|
||||
uint8_t arr[sizeof(StrComputerTurnableInput)]; // 通过结构体类型确定大小
|
||||
} UnComputerTurnableInput;
|
||||
|
||||
|
||||
|
||||
@@ -236,6 +258,11 @@ typedef struct _StrRemoteControlInput
|
||||
unsigned int reserve3 : 1; // 保留
|
||||
|
||||
unsigned int enable : 8; // 使能
|
||||
|
||||
uint16_t y_axis;
|
||||
uint16_t x_axis;
|
||||
uint16_t reserve4;
|
||||
uint16_t reserve5;
|
||||
} StrRemoteControlInput;
|
||||
|
||||
typedef union _UnRemoteControlInput
|
||||
@@ -313,18 +340,19 @@ typedef union _UnSwSample
|
||||
// 输出到电机控制器
|
||||
typedef struct _StrMotorOutput
|
||||
{
|
||||
//-----发送数据0x201或者0x202----------------------------------------------
|
||||
unsigned int mode : 8; // 模式 0x1恒速模式，0x2恒扭模式，其他无效
|
||||
unsigned int gear : 8; // 档位 0x0空挡模式，0x1前进挡，0x2倒退档，其他无效
|
||||
unsigned int set_torque : 16; // 给定扭矩 系数 0.01 偏移量 -300 实际物理量＝数据×系数＋偏移量
|
||||
unsigned int set_rotation_speed : 16; // 给定转速 偏移量 -30000
|
||||
unsigned int fault_code : 8; // 故障码
|
||||
unsigned int heartbeat : 8; // 心跳
|
||||
//-----发送数据0x401或者0x402----------------------------------------------
|
||||
unsigned int feed_power : 16; // 馈电功率 单位为 W 最大为10KW
|
||||
unsigned int discharge_power : 16; // 放电功率 单位为 W 最大为15kW
|
||||
unsigned int reserve1 : 16; // 保留
|
||||
unsigned int reserve2 : 16; // 保留
|
||||
//-----发送数据0x609或者0x209----------------------------------------------
|
||||
uint8_t MotCon_1Signal1 ;
|
||||
uint8_t MotCon_1Signal2 ;
|
||||
uint16_t MotCon_1Signal3 ;
|
||||
uint16_t MotCon_1Signal4 ;
|
||||
uint16_t MotCon_1Signal5 ;
|
||||
//-----接收数据0x589或者0x189----------------------------------------------
|
||||
uint8_t MotCon_1Signal6 ;
|
||||
uint8_t MotCon_1Signal7 ;
|
||||
uint16_t MotCon_1Signal8 ;
|
||||
uint16_t MotCon_1Signal9 ;
|
||||
uint16_t MotCon_1Signal10 ;
|
||||
|
||||
} StrMotorOutput;
|
||||
|
||||
typedef union _UnMotorOutput
|
||||
@@ -357,26 +385,172 @@ typedef union _UnGatherOutput
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// canoe协议输出
|
||||
typedef struct _StrSdoOutput
|
||||
// CAN ID 解析（联合体形式，支持位域和32位直接访问）
|
||||
typedef union _UnCanIdInfo
|
||||
{
|
||||
//-----发送数据0x601----------------------------------------------
|
||||
unsigned int cmd : 8; // 命令
|
||||
unsigned int object_index : 16; // 索引
|
||||
unsigned int sub_index : 8; // 从索引
|
||||
unsigned int data : 32; // 数据
|
||||
} StrSdoOutput;
|
||||
uint32_t raw; // 32位整型，直接读写整个CAN ID
|
||||
struct
|
||||
{
|
||||
uint32_t motor_id : 8; // 0-7 bit (电机ID)
|
||||
uint32_t data : 16; // 8-23 bit (数据字段)
|
||||
uint32_t mode : 5; // 24-28 bit (模式)
|
||||
uint32_t res : 3; // 29-31 bit (保留位)
|
||||
} bits;
|
||||
|
||||
typedef union _UnSdoOutput
|
||||
} UnCanIdInfo;
|
||||
|
||||
|
||||
// 输出can数据
|
||||
typedef struct _StrTxCanOutput
|
||||
{
|
||||
StrSdoOutput bit_data; // 使用定义的结构体变量名
|
||||
uint8_t arr[sizeof(StrSdoOutput)]; // 通过结构体类型确定大小
|
||||
} UnSdoOutput;
|
||||
uint16_t index; // 索引（类似寄存器地址）
|
||||
uint16_t object_index; // 子索引（通常为0x0000）
|
||||
uint32_t data; // 数据字段
|
||||
} StrTxCanOutput;
|
||||
|
||||
|
||||
// CAN数据区联合体 (8字节，严格遵循图片协议，大端序数据)
|
||||
typedef union _UnTxCanData {
|
||||
StrTxCanOutput bit_data; // 结构化访问
|
||||
uint8_t arr[sizeof(StrTxCanOutput)]; // 字节数组形式（用于原始数据读写）
|
||||
} UnTxCanData;
|
||||
|
||||
|
||||
// 接收CAN帧结构体
|
||||
typedef struct _StrTxCanFrame
|
||||
{
|
||||
UnCanIdInfo tx_can_id; // 接收到的29位CAN ID
|
||||
UnTxCanData tx_can_data; // 接收到的8字节数据区
|
||||
} StrTxCanFrame;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//// CAN ID 解析（联合体形式，支持位域和32位直接访问）
|
||||
//typedef union _UnCanIdInfo
|
||||
//{
|
||||
// struct
|
||||
// {
|
||||
// uint32_t motor_id : 8; // 0-7 bit (电机ID)
|
||||
// uint32_t data : 16; // 8-23 bit (数据字段)
|
||||
// uint32_t mode : 5; // 24-28 bit (模式)
|
||||
// uint32_t res : 3; // 29-31 bit (保留位)
|
||||
// } bits;
|
||||
// uint32_t raw; // 32位整型，直接读写整个CAN ID
|
||||
//} UnCanIdInfo;
|
||||
//
|
||||
//// CANoe协议输出（主结构体）
|
||||
//typedef struct _StrSdoOutput
|
||||
//{
|
||||
// uint16_t index; // 索引（类似寄存器地址）
|
||||
// uint16_t object_index; // 子索引（通常为0x0000）
|
||||
// uint32_t data; // 数据字段
|
||||
// UnCanIdInfo rx_can_id; // CAN ID（联合体形式）
|
||||
//} StrSdoOutput;
|
||||
//
|
||||
//// 主联合体（支持结构体和字节数组访问）
|
||||
//typedef union _UnSdoOutput
|
||||
//{
|
||||
// StrSdoIntput bit_data; // 结构化访问
|
||||
// uint8_t arr[sizeof(StrSdoIntput)]; // 字节数组形式（用于原始数据读写）
|
||||
//} UnSdoIntput;
|
||||
|
||||
|
||||
|
||||
|
||||
// CAN ID 解析联合体 (29位扩展帧，严格遵循图片协议，小端序适配)
|
||||
typedef union _UnRxCanIdInfo {
|
||||
uint32_t raw; // 完整的32位值
|
||||
|
||||
struct {
|
||||
// 注意：小端序下，位域布局从低位到高位（Bit0到Bit31）
|
||||
// 编译器通常从低位开始分配位域
|
||||
|
||||
// 主机CAN_ID (Bit7~Bit0) - 8位 - 最低字节
|
||||
uint32_t host_id : 8; // Bit7~0: 主机CAN_ID
|
||||
|
||||
// 电机状态与故障信息域 (Bit23~Bit8) - 16位
|
||||
uint32_t motor_can_id : 8; // Bit15~8: 当前电机CAN ID
|
||||
uint32_t undervoltage : 1; // Bit16: 欠压故障 (0无1有)
|
||||
uint32_t overcurrent : 1; // Bit17: 过流 (0无1有)
|
||||
uint32_t overtemperature : 1; // Bit18: 过温 (0无1有)
|
||||
uint32_t mag_encoder_fault : 1; // Bit19: 磁编码故障 (0无1有)
|
||||
uint32_t hall_fault : 1; // Bit20: HALL编码故障 (0无1有)
|
||||
uint32_t uncalibrated : 1; // Bit21: 未标定 (0无1有)
|
||||
uint32_t mode_state : 2; // Bit23~22: 模式状态 (0:Reset,1:Cali,2:Motor)
|
||||
|
||||
// 协议标识 (Bit28~Bit24) - 5位
|
||||
uint32_t protocol_id : 5; // Bit28~24: 协议标识（图中为2）
|
||||
|
||||
// 保留位 (Bit31~29) - 3位（图片中未使用）
|
||||
uint32_t reserved : 3; // Bit31~29: 保留位，应设置为0
|
||||
} bits;
|
||||
} UnRxCanIdInfo;
|
||||
|
||||
// 输出can数据
|
||||
typedef struct _StrRxCanOutput
|
||||
{
|
||||
uint16_t current_angle; // Byte0~1: 当前角度 [0~65535]对应(-4π~4π)
|
||||
uint16_t current_velocity; // Byte2~3: 当前角速度 [0~65535]对应(-15rad/s~15rad/s)
|
||||
uint16_t current_torque; // Byte4~5: 当前力矩 [0~65535]对应(-120Nm~120Nm)
|
||||
uint16_t temperature; // Byte6~7: 当前温度: Temp(摄氏度)*10
|
||||
} StrRxCanOutput;
|
||||
|
||||
// CAN数据区联合体 (8字节，严格遵循图片协议，大端序数据)
|
||||
typedef union _UnRxCanData
|
||||
{
|
||||
StrRxCanOutput bit_data; // 结构化访问
|
||||
uint8_t arr[sizeof(StrRxCanOutput)]; // 字节数组形式（用于原始数据读写）
|
||||
} UnRxCanData;
|
||||
|
||||
// 接收CAN帧结构体
|
||||
typedef struct _StrRxCanFrame
|
||||
{
|
||||
UnRxCanIdInfo rx_can_id; // 接收到的29位CAN ID
|
||||
UnRxCanData rx_can_data; // 接收到的8字节数据区
|
||||
} StrRxCanFrame;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//// ID 解析输出
|
||||
//typedef struct _StrCanIdInfo
|
||||
//{
|
||||
////-----发送数据0x601----------------------------------------------
|
||||
// uint32_t motor_id:8;
|
||||
// uint32_t data:16;
|
||||
// uint32_t mode:5;
|
||||
// uint32_t res:3;
|
||||
//} StrCanIdInfo;
|
||||
//
|
||||
//
|
||||
//
|
||||
//// canoe协议输出
|
||||
//typedef struct _StrSdoOutput
|
||||
//{
|
||||
////-----发送数据0x601----------------------------------------------
|
||||
// uint16_t index; //索引，类似寄存器地址
|
||||
// uint16_t object_index; // 从索引 为0x0000
|
||||
// uint32_t data; // 数据
|
||||
// StrCanIdInfo rx_can_id;
|
||||
//} StrSdoOutput;
|
||||
//
|
||||
//typedef union _UnSdoOutput
|
||||
//{
|
||||
// StrSdoOutput bit_data; // 使用定义的结构体变量名
|
||||
// uint8_t arr[sizeof(StrSdoOutput)]; // 通过结构体类型确定大小
|
||||
//} UnSdoOutput;
|
||||
|
||||
|
||||
// 超声波数据发送
|
||||
typedef struct _StrUltrasonicOutput
|
||||
@@ -923,6 +1097,17 @@ extern UnTempModuleInput un_temp_module_input;//温度采集模块
|
||||
extern UnAutoComputerInput un_auto_computer_input;//自主计算机自动数据
|
||||
extern UnManualComputerInput un_manual_computer_input;//自主计算机手动数据
|
||||
|
||||
extern StrTxCanFrame un_sdo_output1 ;//电机1输出
|
||||
extern StrTxCanFrame un_sdo_output2 ;//电机2输出
|
||||
extern StrTxCanFrame un_sdo_output3 ;//电机3输出
|
||||
extern StrTxCanFrame un_sdo_output4 ;//电机3速度输出
|
||||
extern StrTxCanFrame un_sdo_output5 ;//电机使能输出
|
||||
|
||||
extern StrRxCanFrame un_pitch_intput ;//电机输入
|
||||
extern StrRxCanFrame un_right_intput ;//电机输入
|
||||
extern StrRxCanFrame un_turn_intput ;//电机输入
|
||||
|
||||
|
||||
extern UnUltrasonicInput un_ultrasonic_input1;//超声波传感器输入1
|
||||
|
||||
extern UnUltrasonicOutput un_ultrasonic_output1;//超声波传感器输出
|
||||
@@ -935,14 +1120,11 @@ extern UnMotorOutput un_motor_output3; //电机输
|
||||
extern UnMotorOutput un_motor_output4; //电机输出
|
||||
extern UnMotorTempInput un_motor_temp1; //电机控制器1 温度
|
||||
extern UnMotorTempInput un_motor_temp2; //电机控制器2 温度
|
||||
extern UnMotorTempInput un_motor_temp3; //电机控制器3 左后侧
|
||||
extern UnMotorTempInput un_motor_temp4; //电机控制器4 右后侧
|
||||
|
||||
|
||||
extern UnInfCanKGFOutput un_inf_can_kgf_output1;
|
||||
extern UnInfCanKGFOutput un_inf_can_kgf_output2;
|
||||
extern UnHBridgeOutput un_h_bridge_output;
|
||||
extern UnHBridgeOutput un_h_bridge_output1;//H桥输出
|
||||
|
||||
extern UnWheelSpeedOutput un_wheel_wpeed_output;
|
||||
extern UnGatherOutput un_gather_output;//采集模块输出
|
||||
@@ -959,6 +1141,8 @@ extern UnRequestFrame un_request_frame; //请求帧
|
||||
|
||||
extern UnComputerOutput un_computer_output; //输出给自主计算机
|
||||
|
||||
extern UnComputerTurnableInput un_computer_turnable_Input ;//转台以太网输入
|
||||
|
||||
//输出给上位机
|
||||
extern UnVehicleInfoOutput un_vehicle_Info_output; // 车辆信息，输出给上位机
|
||||
extern UnMotorStatusOutput un_motor_status_output; // 电机状态信息，输出给上位机
|
||||
@@ -967,7 +1151,6 @@ extern UnAnalogSignalOutput un_analog_signal_output; // 模拟信号输
|
||||
extern UnRemoteControlOutput un_remote_control_output;// 遥控器数据输出，给上位机
|
||||
extern UnManualControlOutput un_manual_control_output;// 手动控制数据，返回给请求者
|
||||
extern UnAutoControlOutput un_auto_control_output; // 自动控制数据输出，返回给请求者
|
||||
extern UnSdoOutput un_sdo_output ;//转向电机输出
|
||||
|
||||
|
||||
//变量
|
||||
@@ -979,6 +1162,8 @@ extern UnCanDebugOutput un_can_debug_output;//调试输出
|
||||
//函数
|
||||
void canSendAll(void *signal_id);
|
||||
void ethernetSendAll(void *signal_id);
|
||||
float constrain(float value, float min_val, float max_val);
|
||||
|
||||
|
||||
#pragma pack()
|
||||
|
||||
|
||||
@@ -51,6 +51,10 @@ void bootmian(void *signal_id)
|
||||
ssdk_printf(SSDK_INFO, "go to boot!\r\n");
|
||||
wrbyte_24c02(Update_Flg_E2adr,CAN_BOOTLOADER_UPGRADE); //д<><D0B4>־<EFBFBD><D6BE><EFBFBD><EFBFBD>
|
||||
// udelay(3000);//<2F><>ʱ3ms
|
||||
wrbyte_24c02(BOOT_DES_IP, ethernet_parameter.download_ip[0]);
|
||||
wrbyte_24c02(BOOT_DES_IP+1, ethernet_parameter.download_ip[1]);
|
||||
wrbyte_24c02(BOOT_DES_IP+2, ethernet_parameter.download_ip[2]);
|
||||
wrbyte_24c02(BOOT_DES_IP+3, ethernet_parameter.download_ip[3]);
|
||||
|
||||
// sdrv_rstgen_global_reset(&rstctl_glb);
|
||||
for(;;)
|
||||
@@ -80,7 +84,7 @@ void bootmian(void *signal_id)
|
||||
}
|
||||
}
|
||||
|
||||
timerStart(&boot_timer_interface, 100,0);
|
||||
timerStart(&boot_timer_interface, 100,1);
|
||||
|
||||
// printf("bootAPP spend time:%d\n",getCurrentTime() - time_boot);//<2F><><EFBFBD><EFBFBD>app<70><70><EFBFBD>˶ʱ<E0B3A4><CAB1>
|
||||
}
|
||||
@@ -110,7 +114,7 @@ void bootInterfaceInit(void)
|
||||
// <20><><EFBFBD>Ķ<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>źţ<C5BA><C5A3><EFBFBD><EFBFBD>ڶ<EFBFBD>ʱ<EFBFBD>ɼ<EFBFBD>
|
||||
subscribe(&boot_timer_interface, bootmian);
|
||||
|
||||
timerStart(&boot_timer_interface, 100,0); //100ms
|
||||
timerStart(&boot_timer_interface, 100,1); //100ms
|
||||
|
||||
feedWatchdog();//ι<><CEB9>,<2C><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ιһ<CEB9><D2BB>
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -21,34 +21,11 @@
|
||||
#define TX_MB_INDEX (USED_MB_FOR_FIFO)
|
||||
|
||||
|
||||
#define LEFT_FRONT_MOTOR_INPUT1 0x101
|
||||
#define LEFT_FRONT_MOTOR_INPUT2 0x103//<2F>¶<EFBFBD>
|
||||
|
||||
|
||||
#define LEFT_REAR_MOTOR2_INPUT1 0x102
|
||||
#define LEFT_REAR_MOTOR2_INPUT2 0x104//<2F>¶<EFBFBD>
|
||||
|
||||
|
||||
#define RIGHT_FRONT_MOTOR_INPUT1 0x101
|
||||
#define RIGHT_FRONT_MOTOR_INPUT2 0x103//<2F>¶<EFBFBD>
|
||||
|
||||
|
||||
#define RIGHT_REAR_MOTOR_INPUT1 0x102
|
||||
#define RIGHT_REAR_MOTOR_INPUT2 0x104//<2F>¶<EFBFBD>
|
||||
|
||||
|
||||
#define LEFT_FRONT_MOTOR_OUTPUT1 0x201
|
||||
#define LEFT_FRONT_MOTOR_OUTPUT2 0x401
|
||||
|
||||
#define LEFT_REAR_MOTOR_OUTPUT1 0x202
|
||||
#define LEFT_REAR_MOTOR_OUTPUT2 0x402
|
||||
|
||||
#define RIGHT_FRONT_MOTOR_OUTPUT1 0x201
|
||||
#define RIGHT_FRONT_MOTOR_OUTPUT2 0x401
|
||||
|
||||
#define RIGHT_REAR_MOTOR_OUTPUT1 0x202
|
||||
#define RIGHT_REAR_MOTOR_OUTPUT2 0x402
|
||||
#define MOTOR_INPUT_ID_1 0x189
|
||||
#define MOTOR_INPUT_ID_2 0x18A
|
||||
|
||||
#define MOTOR_INPUT_ID_3 0x103
|
||||
#define MOTOR_INPUT_ID_4 0x104
|
||||
|
||||
|
||||
|
||||
@@ -58,9 +35,9 @@
|
||||
//#define MOTOR_INPUT_ID_4 0x10F94708//<2F>Һ<EFBFBD>
|
||||
//
|
||||
//
|
||||
#define MOTOR_INPUT_ID_5 0x10F81708//<2F><>ǰ <20><>λ
|
||||
#define MOTOR_INPUT_ID_6 0x10F82708//<2F><>ǰ
|
||||
#define MOTOR_INPUT_ID_7 0x10F83708//<2F><><EFBFBD><EFBFBD>
|
||||
//#define MOTOR_INPUT_ID_1 0x10F81708//<2F><>ǰ <20><>λ
|
||||
//#define MOTOR_INPUT_ID_2 0x10F82708//<2F><>ǰ
|
||||
//#define MOTOR_INPUT_ID_3 0x10F83708//<2F><><EFBFBD><EFBFBD>
|
||||
#define MOTOR_INPUT_ID_8 0x10F84708//<2F>Һ<EFBFBD>
|
||||
|
||||
|
||||
@@ -69,6 +46,7 @@
|
||||
#define BMS_INPUT_ID1 0x100
|
||||
#define BMS_INPUT_ID2 0x101
|
||||
#define REMOTE_ID 0x12000023
|
||||
#define REMOTE_ID_1 0x12000024
|
||||
#define TEMP_MODULE_INPUT_ID_1 0x15000003
|
||||
|
||||
|
||||
@@ -83,10 +61,7 @@ typedef struct _StrCanFault
|
||||
{
|
||||
uint8_t navigator_count; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ǽ<EFBFBD><C7BC><EFBFBD><EFBFBD><EFBFBD>
|
||||
uint8_t motor1_count; //<2F>ֶ<EFBFBD><D6B6><EFBFBD><EFBFBD>ݼ<EFBFBD><DDBC><EFBFBD><EFBFBD><EFBFBD>
|
||||
uint8_t motor2_count; //<2F>ֶ<EFBFBD><D6B6><EFBFBD><EFBFBD>ݼ<EFBFBD><DDBC><EFBFBD><EFBFBD><EFBFBD>
|
||||
uint8_t motor3_count; //<2F>ֶ<EFBFBD><D6B6><EFBFBD><EFBFBD>ݼ<EFBFBD><DDBC><EFBFBD><EFBFBD><EFBFBD>
|
||||
uint8_t motor4_count; //<2F>ֶ<EFBFBD><D6B6><EFBFBD><EFBFBD>ݼ<EFBFBD><DDBC><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
uint8_t motor2_count; //<2F>ֶ<EFBFBD><D6B6><EFBFBD><EFBFBD>ݼ<EFBFBD><DDBC><EFBFBD><EFBFBD><EFBFBD>
|
||||
uint8_t bms_count; //bms<6D><73><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
uint8_t temperature_count; //<2F>¶ȼ<C2B6><C8BC><EFBFBD><EFBFBD><EFBFBD>
|
||||
uint8_t remote_count; //ң<>ؼ<EFBFBD><D8BC><EFBFBD><EFBFBD><EFBFBD>
|
||||
@@ -94,8 +69,6 @@ typedef struct _StrCanFault
|
||||
uint8_t navigator_state; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
|
||||
uint8_t motor1_state; //<2F><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD>״̬
|
||||
uint8_t motor2_state; //<2F><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD>״̬
|
||||
uint8_t motor3_state; //<2F><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD>״̬
|
||||
uint8_t motor4_state; //<2F><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD>״̬
|
||||
uint8_t bms_state; //bms״̬
|
||||
uint8_t temperature_state; //<2F>¶ȼ<C2B6><C8BC><EFBFBD><EFBFBD><EFBFBD>
|
||||
uint8_t remote_state; //ң<><D2A3>״̬
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -31,6 +31,7 @@
|
||||
#include <app/app_light.h>
|
||||
#include "app/app_request.h"
|
||||
#include "app/app_ultrasonic.h"
|
||||
#include "app/app_turntable.h"
|
||||
|
||||
void testAppInit(void);
|
||||
|
||||
@@ -115,7 +116,7 @@ int main(void)
|
||||
|
||||
|
||||
//打印版本号
|
||||
printf("version: V1.75 \n");
|
||||
printf("version: V1.72 \n");
|
||||
// 初始化框架 放在最前面，解决电机can发送信号累积不处理的问题。
|
||||
|
||||
testAppInit();
|
||||
@@ -129,6 +130,7 @@ int main(void)
|
||||
requestAppInit();
|
||||
canInterfaceInit();
|
||||
bootInterfaceInit();
|
||||
turnableInit();
|
||||
// ultrasonicAppInit();
|
||||
|
||||
printf("All init OK ------ %d\n",getCurrentTime());
|
||||
|
||||
Reference in New Issue
Block a user