增加转台
This commit is contained in:
6
.vscode/settings.json
vendored
Normal file
6
.vscode/settings.json
vendored
Normal file
@@ -0,0 +1,6 @@
|
||||
{
|
||||
"files.associations": {
|
||||
"app_config.h": "c",
|
||||
"interface.h": "c"
|
||||
}
|
||||
}
|
||||
@@ -97,7 +97,7 @@ static void brakeTimerProcess(void *signal_id)
|
||||
brake_data.brake_motor_state = 2;
|
||||
}
|
||||
brakeOutput(NULL);
|
||||
timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 0);
|
||||
timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 1);
|
||||
}
|
||||
break;
|
||||
|
||||
@@ -115,7 +115,7 @@ static void brakeTimerProcess(void *signal_id)
|
||||
brake_data.brake_motor_state = 1;
|
||||
}
|
||||
brakeOutput(NULL);
|
||||
timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0);
|
||||
timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 1);
|
||||
}
|
||||
break;
|
||||
|
||||
@@ -144,15 +144,15 @@ static void brakeTimerProcess(void *signal_id)
|
||||
brake_data.state = BRAKE_STATE_IDLE;
|
||||
break;
|
||||
}
|
||||
// 如果刹车位置有变化，存入EEPROM
|
||||
if (brake_data.brake_position != brake_data.old_brake_position)
|
||||
{
|
||||
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);
|
||||
|
||||
}
|
||||
// // 如果刹车位置有变化，存入EEPROM
|
||||
// if (brake_data.brake_position != brake_data.old_brake_position)
|
||||
// {
|
||||
// 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);
|
||||
//
|
||||
// }
|
||||
timerStart(&brake_data.brake_timer, 100, 1); // 周期调用
|
||||
}
|
||||
|
||||
|
||||
@@ -11,7 +11,12 @@
|
||||
#include "app_brake.h"
|
||||
#include "app_power.h"
|
||||
|
||||
#include "drive_rs04.h"
|
||||
|
||||
Timer diff_app_timer;
|
||||
Timer diff_app_timer2;
|
||||
|
||||
Timer diff_app_timer3;
|
||||
|
||||
// 定义全局变量
|
||||
DiffData diff_data;
|
||||
@@ -114,111 +119,39 @@ void handleVehicleState(MotorState *ctx, float speed, float torque)
|
||||
// 设置电机输出
|
||||
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;
|
||||
|
||||
// 档位
|
||||
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]);
|
||||
static float previous_time2 = 0.0f;
|
||||
|
||||
un_motor_output1.bit_data.gear = diff_data.motor_state[0];
|
||||
un_motor_output2.bit_data.gear = diff_data.motor_state[1];
|
||||
|
||||
if(STATE_FORWARD == diff_data.motor_state[2])//把后两台电机反相
|
||||
{
|
||||
un_motor_output3.bit_data.gear = STATE_BACKWARD;
|
||||
}
|
||||
else if(STATE_BACKWARD == diff_data.motor_state[2])
|
||||
{
|
||||
un_motor_output3.bit_data.gear = STATE_FORWARD;
|
||||
}
|
||||
else
|
||||
{
|
||||
un_motor_output3.bit_data.gear = STATE_INIT;
|
||||
}
|
||||
|
||||
if(STATE_FORWARD == diff_data.motor_state[3])
|
||||
{
|
||||
un_motor_output4.bit_data.gear = STATE_BACKWARD;
|
||||
}
|
||||
else if(STATE_BACKWARD == diff_data.motor_state[3])
|
||||
{
|
||||
un_motor_output4.bit_data.gear = STATE_FORWARD;
|
||||
}
|
||||
else
|
||||
{
|
||||
un_motor_output4.bit_data.gear = STATE_INIT;
|
||||
}
|
||||
float time1 = (float)getCurrentTime();
|
||||
float dt = (time1 - previous_time2) / PERIOD_TICK;
|
||||
previous_time2 = time1;
|
||||
|
||||
|
||||
// 设置左右电机期望转速
|
||||
// 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.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.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;
|
||||
|
||||
abs_left_front_speed = dynamic_current_limit(&diff_data.out_torq_last[0],-diff_data.out_torq[0],dt);
|
||||
abs_right_front_speed = dynamic_current_limit(&diff_data.out_torq_last[1],diff_data.out_torq[1],dt);
|
||||
abs_left_rear_speed = dynamic_current_limit(&diff_data.out_torq_last[2],-diff_data.out_torq[2],dt);
|
||||
abs_right_rear_speed = dynamic_current_limit(&diff_data.out_torq_last[3],diff_data.out_torq[3],dt);
|
||||
|
||||
setMotorWrite(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1, IQ_REF_INDEX,abs_left_front_speed);
|
||||
setMotorWrite(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2, IQ_REF_INDEX,abs_right_front_speed);
|
||||
setMotorWrite(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3, IQ_REF_INDEX,abs_left_rear_speed);
|
||||
setMotorWrite(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4, IQ_REF_INDEX,abs_right_rear_speed);
|
||||
}
|
||||
|
||||
|
||||
// 限制值在最小值和最大值之间
|
||||
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)
|
||||
{
|
||||
// 获取轮子周长
|
||||
float wheel_circumference = (float)getParam("whl_dia") * M_PI;
|
||||
float wheel_circumference = diff_data.wheel_radius * M_PI;
|
||||
// 获取减速比
|
||||
float gear_ratio = (float)getParam("gRatio");
|
||||
float gear_ratio = diff_data.gear_ratio;
|
||||
if (fabsf(gear_ratio) < EPSILON)
|
||||
{
|
||||
return 0; // 避免除以0的情况
|
||||
@@ -240,7 +173,7 @@ uint8_t calculateCurrentSpeedYawRate(void)
|
||||
// 计算速度差
|
||||
float speed_diff = left_speed_mps - right_speed_mps;
|
||||
// 计算角速度
|
||||
float wheel_base = (float)getParam("whl_bas");
|
||||
float wheel_base = diff_data.wheel_base;
|
||||
if (fabsf(wheel_base) < EPSILON)
|
||||
{
|
||||
return 0; // 避免除以0的情况
|
||||
@@ -277,11 +210,11 @@ float calculateDeceleration(float speed, float previous_speed, float dt)
|
||||
float calculateMaxSpeed()
|
||||
{
|
||||
// 获取最大电机转速 (RPM)
|
||||
float max_rpm = (float)getParam("max_rpm");
|
||||
float max_rpm = diff_data.max_motor_rpm ;
|
||||
// 获取轮子周长
|
||||
float wheel_circumference = (float)getParam("whl_dia") * M_PI;
|
||||
float wheel_circumference = diff_data.wheel_radius * M_PI;
|
||||
// 获取减速比
|
||||
float gear_ratio = (float)getParam("gRatio");
|
||||
float gear_ratio = diff_data.gear_ratio;
|
||||
if (fabsf(gear_ratio) < EPSILON)
|
||||
{
|
||||
return 0; // 避免除以0的情况
|
||||
@@ -295,10 +228,10 @@ float calculateMaxSpeed()
|
||||
float calculateMaxAcceleration(void)
|
||||
{
|
||||
// 获取车辆参数
|
||||
float max_motor_torque = (float)getParam("maxTorq"); // 最大电机扭矩
|
||||
float vehicle_mass = (float)getParam("VehMass"); // 车辆质量
|
||||
float wheel_radius = (float)getParam("whl_dia") / 2.0f; // 轮子半径
|
||||
float gear_ratio = (float)getParam("gRatio"); // 减速比
|
||||
float max_motor_torque = diff_data.max_Torq; // 最大电机扭矩
|
||||
float vehicle_mass = diff_data.vehicle_mass; // 车辆质量
|
||||
float wheel_radius = diff_data.wheel_radius / 2.0f; // 轮子半径
|
||||
float gear_ratio = diff_data.gear_ratio; // 减速比
|
||||
if (fabsf(wheel_radius) < EPSILON || fabsf(vehicle_mass) < EPSILON )
|
||||
{
|
||||
return 0; // 避免除以0的情况
|
||||
@@ -375,7 +308,6 @@ void distributeTorque(float rpm1, float rpm2, float total_torque, float* torque1
|
||||
if ( fabs(*torque2) < min_torque) {
|
||||
*torque2 = copysignf(min_torque, *torque2);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -473,10 +405,10 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
|
||||
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]);
|
||||
// handleVehicleState(&diff_data.motor_state[0], diff_data.left_front_motor_speed, motor_speed[0]); //通过扭矩以及速度来判断挡位
|
||||
// handleVehicleState(&diff_data.motor_state[1], diff_data.right_front_motor_speed, motor_speed[1]);
|
||||
// handleVehicleState(&diff_data.motor_state[2], diff_data.left_rear_motor_speed, motor_speed[2]);
|
||||
// handleVehicleState(&diff_data.motor_state[3], diff_data.right_rear_motor_speed, motor_speed[3]);
|
||||
|
||||
|
||||
// distributeTorque(diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed,2*left_speed_mps,&motor_speed[0],&motor_speed[2],diff_data.max_Torq,diff_data.min_Torq);
|
||||
@@ -486,32 +418,32 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
|
||||
|
||||
// 限制线速度和偏航率
|
||||
linear_velocity_x = constrain(linear_velocity_x, -max_speed, max_speed);
|
||||
float max_yaw_rate = max_speed / ((float)getParam("whl_bas") / 2.0f);
|
||||
float max_yaw_rate = max_speed / diff_data.wheel_base / 2.0f);
|
||||
yaw_rate = constrain(yaw_rate, -max_yaw_rate, max_yaw_rate);
|
||||
|
||||
// 计算旋转速度
|
||||
float rotational_velocity = ((float)getParam("whl_bas") / 2.0f) * yaw_rate;
|
||||
float rotational_velocity = (diff_data.wheel_base / 2.0f) * yaw_rate;
|
||||
|
||||
// 计算车辆左右线速度 (m/s)
|
||||
float left_speed_mps = linear_velocity_x - rotational_velocity; //20250316 为解决原地转向和直行转向相同，所以把左右输出的速度交换
|
||||
float right_speed_mps = linear_velocity_x + rotational_velocity;
|
||||
|
||||
// 计算轮子周长
|
||||
float wheel_circumference = (float)getParam("whl_dia") * M_PI;
|
||||
float wheel_circumference = diff_data.wheel_radius * M_PI;
|
||||
|
||||
// 将车辆左右线速度转换为轮子转速 (RPM)
|
||||
float left_wheel_rpm = (left_speed_mps * 60.0f) / wheel_circumference;
|
||||
float right_wheel_rpm = (right_speed_mps * 60.0f) / wheel_circumference;
|
||||
|
||||
// 获取减速比
|
||||
float gear_ratio = (float)getParam("gRatio");
|
||||
float gear_ratio = diff_data.gear_ratio;
|
||||
|
||||
// 将轮子转速转换为电机转速，考虑减速比
|
||||
float left_motor_rpm = left_wheel_rpm * gear_ratio;
|
||||
float right_motor_rpm = right_wheel_rpm * gear_ratio;
|
||||
|
||||
// 限制电机的最大和最小转速
|
||||
float max_motor_rpm = (float)getParam("max_rpm");
|
||||
float max_motor_rpm = diff_data.max_motor_rpm;
|
||||
left_motor_rpm = constrain(left_motor_rpm, -max_motor_rpm, max_motor_rpm);
|
||||
right_motor_rpm = constrain(right_motor_rpm, -max_motor_rpm, max_motor_rpm);
|
||||
// 当电机转速小于50转时，设置为0
|
||||
@@ -621,7 +553,7 @@ static void diffProcess(void *signal_id)
|
||||
output_yaw_rate = 0;
|
||||
}
|
||||
|
||||
// 使用差速车辆动力学模型计算左右电机的期望速度
|
||||
// 使用差速车辆动力学模型计算左右电机的期望速度
|
||||
float out_torque[4] = {0,0,0,0};
|
||||
// 使用差速车辆动力学模型计算左右电机的期望速度
|
||||
computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque);
|
||||
@@ -671,7 +603,6 @@ static void diffProcess(void *signal_id)
|
||||
diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, out_torque[1], -out_torque[1]);
|
||||
}
|
||||
|
||||
|
||||
diff_data.out_torq[0] = (out_torque[0] + diff_data.left_diff_touue);//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。
|
||||
diff_data.out_torq[2] = (out_torque[0] - diff_data.left_diff_touue);
|
||||
|
||||
@@ -683,24 +614,28 @@ static void diffProcess(void *signal_id)
|
||||
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);
|
||||
|
||||
|
||||
|
||||
// 设置电机输出
|
||||
setMotorOutput(&diff_data.out_torq[0],
|
||||
diff_data.max_Torq,//
|
||||
(uint16_t)getParam("feedPwr"),
|
||||
(uint16_t)getParam("dispPwr"));
|
||||
diff_data.feedPower,
|
||||
diff_data.dispPower);
|
||||
// 发布左右电机期望转速,电源在工作状态才能发送
|
||||
if (power_data.current_state == POWER_WORKING)
|
||||
{
|
||||
// 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);
|
||||
|
||||
}
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// motorDisable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1);
|
||||
// motorDisable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2);
|
||||
// motorDisable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3);
|
||||
// motorDisable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4);
|
||||
//
|
||||
// publishMessage(&un_motor_output1, 1);
|
||||
// publishMessage(&un_motor_output2, 1);
|
||||
// }
|
||||
|
||||
|
||||
un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(diff_data.speed*10);
|
||||
@@ -802,40 +737,33 @@ static void diffInput(void *signal_id)
|
||||
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_motor_input1) || (signal_id == &un_motor_input3) )// 处理第一个电机速度信号（左电机）
|
||||
else{}
|
||||
|
||||
//速度输入处理 20251005 修改为定时处理
|
||||
diff_data.left_front_motor_speed = -convertPhysical( SWAP_ENDIAN_16(un_motor_input1.rx_can_data.bit_data.current_velocity),-RS04_ANGULAR_VELOCITY_MAX,RS04_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ) * ANGULAR_VELOCITY_TO_RPM;//速度单位转换，rad/s转换为转/分钟
|
||||
diff_data.right_front_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_motor_input2.rx_can_data.bit_data.current_velocity), -RS04_ANGULAR_VELOCITY_MAX,RS04_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ) * ANGULAR_VELOCITY_TO_RPM;//速度单位转换，rad/s转换为转/分钟
|
||||
diff_data.left_rear_motor_speed = -convertPhysical( SWAP_ENDIAN_16(un_motor_input3.rx_can_data.bit_data.current_velocity),-RS04_ANGULAR_VELOCITY_MAX,RS04_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ) * ANGULAR_VELOCITY_TO_RPM;//速度单位转换，rad/s转换为转/分钟
|
||||
diff_data.right_rear_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_motor_input4.rx_can_data.bit_data.current_velocity), -RS04_ANGULAR_VELOCITY_MAX,RS04_ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ) * ANGULAR_VELOCITY_TO_RPM;//速度单位转换，rad/s转换为转/分钟
|
||||
|
||||
if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速
|
||||
{
|
||||
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 增加反相
|
||||
|
||||
if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速
|
||||
{
|
||||
motor_speed_temp = diff_data.left_front_motor_speed;
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_speed_temp = diff_data.left_rear_motor_speed;
|
||||
}
|
||||
diff_data.left_motor_speed = motor_speed_temp;
|
||||
motor_speed_temp = diff_data.left_front_motor_speed;
|
||||
}
|
||||
else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右电机）
|
||||
else
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
diff_data.right_motor_speed = motor_speed_temp;
|
||||
motor_speed_temp = diff_data.left_rear_motor_speed;
|
||||
}
|
||||
diff_data.left_motor_speed = motor_speed_temp;//左侧轮速 //motor_speed_temp
|
||||
|
||||
if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速
|
||||
{
|
||||
motor_speed_temp = diff_data.right_rear_motor_speed;
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_speed_temp = diff_data.right_front_motor_speed;
|
||||
}
|
||||
diff_data.right_motor_speed = motor_speed_temp;//右侧轮速
|
||||
|
||||
// 急停开关
|
||||
diff_data.emergency_stop_state = (uint8_t)(diff_data.emergency_stop_switch == app_close() || diff_data.remote_emergency_stop == app_close());
|
||||
@@ -853,6 +781,90 @@ static void diffInput(void *signal_id)
|
||||
diff_data.desired_curvature = 0.0;
|
||||
}
|
||||
|
||||
switch(diff_data.motor_init_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据
|
||||
{
|
||||
case 0:
|
||||
// timerStart(&diff_app_timer3, 1000, 0); // 启动定时器，1s
|
||||
diff_data.motor_init_state = 1;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if(diff_data.diff_cnt >= 10)//发送5次
|
||||
{
|
||||
diff_data.diff_cnt = 0;
|
||||
diff_data.motor_init_state = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.diff_cnt ++;
|
||||
diff_data.motor_init_state = 1;
|
||||
}
|
||||
break;
|
||||
|
||||
case 2://模式设置
|
||||
if(diff_data.diff_cnt >= 5)//发送5次
|
||||
{
|
||||
diff_data.diff_cnt = 0;
|
||||
diff_data.motor_init_state = 3;
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.diff_cnt ++;
|
||||
diff_data.motor_init_state = 2;
|
||||
|
||||
setMotorMode(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1, CURRENT_MODE);
|
||||
setMotorMode(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2, CURRENT_MODE);
|
||||
setMotorMode(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3, CURRENT_MODE);
|
||||
setMotorMode(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4, CURRENT_MODE);
|
||||
|
||||
publishMessage(&un_motor_output1, 1);
|
||||
publishMessage(&un_motor_output2, 1);
|
||||
|
||||
// publishMessage(&un_motor_output3, 1);
|
||||
// publishMessage(&un_motor_output4, 1);
|
||||
}
|
||||
break;
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
case 3:
|
||||
if(diff_data.diff_cnt >= 5)//发送5次
|
||||
{
|
||||
diff_data.diff_cnt = 0;
|
||||
diff_data.motor_init_state = 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.diff_cnt ++;
|
||||
diff_data.motor_init_state = 3;
|
||||
|
||||
motorEnable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1);
|
||||
motorEnable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2);
|
||||
motorEnable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3);
|
||||
motorEnable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4);
|
||||
|
||||
publishMessage(&un_motor_output1, 1);
|
||||
publishMessage(&un_motor_output2, 1);
|
||||
// publishMessage(&un_motor_output3, 1);
|
||||
// publishMessage(&un_motor_output4, 1);
|
||||
}
|
||||
break;
|
||||
|
||||
case 4:
|
||||
diff_data.motor_init_state = 4;
|
||||
diffProcess(&diff_data);//计算左右电机期望转速
|
||||
break;
|
||||
|
||||
default:break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制
|
||||
// {
|
||||
// diff_data.max_Torq = 5;//20240403修改。刹车就是5N
|
||||
@@ -865,20 +877,23 @@ static void diffInput(void *signal_id)
|
||||
// {
|
||||
// diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
|
||||
// }
|
||||
if((power_data.current_state == POWER_WORKING))//电机上电才运行
|
||||
{
|
||||
diffProcess(&diff_data);//计算左右电机期望转速
|
||||
}
|
||||
else
|
||||
{
|
||||
resetPidIntegral(&speed_pid);
|
||||
resetPidIntegral(&yaw_rate_pid);
|
||||
diff_data.motor_state[0] = STATE_INIT;
|
||||
diff_data.motor_state[1] = STATE_INIT;
|
||||
diff_data.motor_state[2] = STATE_INIT;
|
||||
diff_data.motor_state[3] = STATE_INIT;
|
||||
}
|
||||
|
||||
// if((power_data.current_state == POWER_WORKING))//电机上电才运行
|
||||
// {
|
||||
|
||||
// diff_data.motor_init_state = 0;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// resetPidIntegral(&speed_pid);
|
||||
// resetPidIntegral(&yaw_rate_pid);
|
||||
//
|
||||
// diff_data.out_torq[0] = STATE_INIT;
|
||||
// diff_data.out_torq[1] = STATE_INIT;
|
||||
// diff_data.out_torq[2] = STATE_INIT;
|
||||
// diff_data.out_torq[3] = STATE_INIT;
|
||||
// }
|
||||
timerStart(&diff_app_timer2,1,1);//1ms调用一次
|
||||
}
|
||||
|
||||
|
||||
@@ -887,20 +902,84 @@ static void diffInput(void *signal_id)
|
||||
void preChargeFinish(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
// setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr"));
|
||||
//-------------------------------------------------------------------------------------------------------------------------------
|
||||
switch(diff_data.motor_init_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据
|
||||
{
|
||||
case 0:
|
||||
// timerStart(&diff_app_timer3, 1000, 0); // 启动定时器，1s
|
||||
diff_data.motor_init_state = 1;
|
||||
break;
|
||||
|
||||
float out_torq[4] = {0.0f,0.0f,0.0f,0.0f};
|
||||
case 1:
|
||||
if(diff_data.diff_cnt >= 10)//发送5次
|
||||
{
|
||||
diff_data.diff_cnt = 0;
|
||||
diff_data.motor_init_state = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.diff_cnt ++;
|
||||
diff_data.motor_init_state = 1;
|
||||
}
|
||||
break;
|
||||
|
||||
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;
|
||||
case 2://模式设置
|
||||
if(diff_data.diff_cnt >= 5)//发送5次
|
||||
{
|
||||
diff_data.diff_cnt = 0;
|
||||
diff_data.motor_init_state = 3;
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.diff_cnt ++;
|
||||
diff_data.motor_init_state = 2;
|
||||
|
||||
publishMessage(&un_motor_output1, 1);
|
||||
publishMessage(&un_motor_output2, 1);
|
||||
publishMessage(&un_motor_output3, 1);
|
||||
publishMessage(&un_motor_output4, 1);
|
||||
setMotorMode(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1, CURRENT_MODE);
|
||||
setMotorMode(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2, CURRENT_MODE);
|
||||
setMotorMode(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3, CURRENT_MODE);
|
||||
setMotorMode(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4, CURRENT_MODE);
|
||||
|
||||
publishMessage(&un_motor_output1, 1);
|
||||
publishMessage(&un_motor_output2, 1);
|
||||
|
||||
// publishMessage(&un_motor_output3, 1);
|
||||
// publishMessage(&un_motor_output4, 1);
|
||||
}
|
||||
break;
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
case 3:
|
||||
if(diff_data.diff_cnt >= 5)//发送5次
|
||||
{
|
||||
diff_data.diff_cnt = 0;
|
||||
diff_data.motor_init_state = 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.diff_cnt ++;
|
||||
diff_data.motor_init_state = 3;
|
||||
|
||||
motorEnable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1);
|
||||
motorEnable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2);
|
||||
motorEnable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3);
|
||||
motorEnable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4);
|
||||
|
||||
publishMessage(&un_motor_output1, 1);
|
||||
publishMessage(&un_motor_output2, 1);
|
||||
// publishMessage(&un_motor_output3, 1);
|
||||
// publishMessage(&un_motor_output4, 1);
|
||||
}
|
||||
break;
|
||||
|
||||
case 4:
|
||||
diff_data.motor_init_state = 4;
|
||||
publishMessage(&un_motor_output1, 1);//一直使能防止 ，防止can报故障
|
||||
publishMessage(&un_motor_output2, 1);
|
||||
break;
|
||||
|
||||
default:break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -963,9 +1042,44 @@ void diffParametersInit(void *signal_id)
|
||||
Dec_front_speed_pid.output_limit
|
||||
);
|
||||
|
||||
|
||||
if(0 == un_motor_input1.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
|
||||
{
|
||||
motorEnable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output5);
|
||||
publishMessage(&un_motor_output5, 1);
|
||||
}
|
||||
|
||||
if(0 == un_motor_input2.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
|
||||
{
|
||||
motorEnable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output6);
|
||||
publishMessage(&un_motor_output6, 1);
|
||||
}
|
||||
|
||||
if(0 == un_motor_input3.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
|
||||
{
|
||||
motorEnable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output7);
|
||||
publishMessage(&un_motor_output7, 1);
|
||||
}
|
||||
|
||||
if(0 == un_motor_input4.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
|
||||
{
|
||||
motorEnable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output8);
|
||||
publishMessage(&un_motor_output8, 1);
|
||||
}
|
||||
|
||||
|
||||
diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩
|
||||
diff_data.max_Torq = (float)getParam("maxTorq");
|
||||
|
||||
diff_data.feedPower = (uint16_t)getParam("feedPwr");
|
||||
diff_data.dispPower = (uint16_t)getParam("dispPwr");
|
||||
diff_data.vehicle_mass = (float)getParam("VehMass");
|
||||
diff_data.wheel_radius = (float)getParam("whl_dia");
|
||||
diff_data.gear_ratio = (float)getParam("gRatio");
|
||||
diff_data.wheel_base = (float)getParam("whl_bas");
|
||||
diff_data.max_motor_rpm = (float)getParam("max_rpm");
|
||||
|
||||
|
||||
if(0 == (float)getParam("diff_sp"))//20250711 防止参数为0，影响计算。
|
||||
{
|
||||
diff_data.diff_dead_zone = 2;
|
||||
@@ -975,8 +1089,6 @@ void diffParametersInit(void *signal_id)
|
||||
diff_data.diff_dead_zone = (float)getParam("diff_sp");//参数读取设定最大扭矩
|
||||
}
|
||||
|
||||
|
||||
|
||||
printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
|
||||
printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
|
||||
// printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
|
||||
@@ -992,24 +1104,29 @@ void diffParametersInit(void *signal_id)
|
||||
deffspeed = deffspeed * 0.01f;
|
||||
deffcurvature = deffcurvature * 0.0001f;
|
||||
|
||||
printf("remote_speed: %f, remote_yaw_rate: %f\n", deffspeed, deffcurvature);
|
||||
// printf("remote_speed: %f, remote_yaw_rate: %f\n", deffspeed, deffcurvature);
|
||||
|
||||
printf(" car state = %d,%d,%d,%d\n", diff_data.motor_state[0],diff_data.motor_state[1],diff_data.motor_state[2],diff_data.motor_state[3]);
|
||||
// printf(" motor_init_state = %d\n", diff_data.motor_init_state);
|
||||
|
||||
timerStart(&diff_app_timer,1000,1);//1s调用一次
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 差速初始化函数
|
||||
void diffAppInit(void)
|
||||
{
|
||||
// 初始化 diff_data
|
||||
memset(&diff_data, 0, sizeof(DiffData));
|
||||
|
||||
|
||||
// timerInit(&brake_data.brake_timer);
|
||||
|
||||
|
||||
// 订阅相关信号
|
||||
subscribe(&un_sw_sample, diffInput); // 急停开关、高压开关
|
||||
subscribe(&un_motor_input1, diffInput);
|
||||
subscribe(&un_motor_input2, diffInput);
|
||||
|
||||
// subscribe(&un_motor_input1, diffInput);
|
||||
// subscribe(&un_motor_input2, diffInput);
|
||||
|
||||
subscribe(&un_auto_computer_input, diffInput);
|
||||
subscribe(&un_manual_computer_input, diffInput);
|
||||
@@ -1064,5 +1181,8 @@ void diffAppInit(void)
|
||||
subscribe(&diff_app_timer, diffParametersInit);
|
||||
timerStart(&diff_app_timer,1000,1);//1s调用一次
|
||||
|
||||
subscribe(&diff_app_timer2, diffInput);
|
||||
timerStart(&diff_app_timer2,1,1);//1ms调用一次
|
||||
|
||||
printf("diffControl: diffAppInit OK \n");
|
||||
}
|
||||
|
||||
@@ -19,14 +19,36 @@ extern "C"
|
||||
#define SPEED_MODE 0x01
|
||||
#define TORQUE_MODE 0x02
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
#define ANGULAR_VELOCITY_TO_RPM 30.0 / PI
|
||||
|
||||
|
||||
#define TORQUE_HYSTERESIS_THRESHOLD 0.3f
|
||||
|
||||
|
||||
#define MOTOR_MODE TORQUE_MODE
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//轮毂电机
|
||||
#define FRONT_LEFT_MOTOR_CANID 0x7F
|
||||
#define FRONT_RIGHT_MOTOR_CANID 0x7F
|
||||
#define REAR_LEFT_MOTOR_CANID 0x7F
|
||||
#define REAR_RIGHT_MOTOR_CANID 0x7F
|
||||
|
||||
|
||||
#define FRONT_LEFT_MOTOR_RxCANID (0x20000FD + (FRONT_LEFT_MOTOR_CANID << 8)) // 0x2007CFD
|
||||
#define FRONT_RIGHT_MOTOR_RxCANID (0x20000FD + (FRONT_RIGHT_MOTOR_CANID << 8)) // 0x2007DFD
|
||||
#define REAR_LEFT_MOTOR_RxCANID (0x20000FD + (REAR_LEFT_MOTOR_CANID << 8)) // 0x2007EFD
|
||||
#define REAR_RIGHT_MOTOR_RxCANID (0x20000FD + (REAR_RIGHT_MOTOR_CANID << 8)) // 0x2007FFD
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#define ALPHA 0.1f // 滤波系数α∈[0.01,0.3]，0.2对应截止频率约10Hz（假设采样周期10ms）
|
||||
#define LOWPASS_FILTER(speed, prev) (ALPHA * (speed) + (1 - ALPHA) * (prev))
|
||||
|
||||
@@ -46,6 +68,8 @@ typedef enum
|
||||
|
||||
typedef struct DiffData
|
||||
{
|
||||
uint16_t diff_cnt;
|
||||
uint8_t motor_init_state; // 电机状态
|
||||
ControlMode mode ; // 控制模式
|
||||
MotorState motor_state[4]; //当前车辆状态
|
||||
float desired_speed; // 期望速度
|
||||
@@ -71,6 +95,11 @@ typedef struct DiffData
|
||||
float out_left_motor_speed; // 输出左电机速度
|
||||
float out_right_motor_speed; // 输出右电机速度
|
||||
float out_torq[4]; //4个电机扭矩
|
||||
float out_torq_last[4]; //4个电机扭矩
|
||||
|
||||
uint16_t feedPower; //馈电功率
|
||||
uint16_t dispPower; //放电功率
|
||||
|
||||
float max_Torq; // 最大扭矩限制
|
||||
float min_Torq; // 最小扭矩限制
|
||||
|
||||
@@ -81,6 +110,13 @@ typedef struct DiffData
|
||||
float right_diff_touue; // 右侧扭矩差
|
||||
float diff_dead_zone; // 差速速度死区
|
||||
|
||||
float vehicle_mass; // 车重
|
||||
float wheel_radius; // 轮胎直径
|
||||
float gear_ratio; // 减速比
|
||||
float wheel_base; // 轴距
|
||||
float max_motor_rpm; // 电机最大速度
|
||||
|
||||
|
||||
} 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,22 @@ 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_output1.bit_data.KGF13 = state_value;
|
||||
break;
|
||||
case LIGHT_TAIL://尾灯，后面4个灯
|
||||
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.KGF14 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF14 = 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_output1.bit_data.KGF13 = 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_output1.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_output1.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_output1.bit_data.KGF14 = state_value;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -41,8 +41,8 @@ extern "C"
|
||||
X(pwr_btn) \
|
||||
X(sleepTm) \
|
||||
X(wakeTm) \
|
||||
X(Ospd_kp) \
|
||||
X(Ospd_ki) \
|
||||
X(minYpos) \
|
||||
X(maxYpos) \
|
||||
X(Ospd_kd) \
|
||||
X(Ospd_il) \
|
||||
X(Ospd_ol) \
|
||||
@@ -59,6 +59,10 @@ extern "C"
|
||||
X(mot_il) \
|
||||
X(mot_ol) \
|
||||
X(diff_sp) \
|
||||
X(turn_sp) \
|
||||
X(pit_sp) \
|
||||
X(xy_pos) \
|
||||
X(z_pos) \
|
||||
X(test)
|
||||
|
||||
// 定义一个包含所有参数名称的结构体
|
||||
|
||||
382
app/app_power.c
382
app/app_power.c
@@ -34,73 +34,73 @@ typedef struct {
|
||||
PowerSystem power_data;
|
||||
static PowerButton power_button = {BUTTON_STATE_INITIAL, 0, 0, 0, {0},0};
|
||||
|
||||
// 电源按钮处理函数
|
||||
static void handlePowerButton(void)
|
||||
{
|
||||
switch (power_button.state)
|
||||
{
|
||||
case BUTTON_STATE_INITIAL:
|
||||
if (power_data.remote_power_switch == app_close())
|
||||
{
|
||||
power_button.state = BUTTON_STATE_SHORT_PRESS_DETECTED;
|
||||
timerStart(&power_button.timer, 500, 0); // 启动短按定时器，500ms
|
||||
}
|
||||
break;
|
||||
|
||||
case BUTTON_STATE_SHORT_PRESS_DETECTED:
|
||||
if (power_data.remote_power_switch == app_open())
|
||||
{
|
||||
if (power_button.timer.active) // 定时器未到期,短按完成，启动等待长按定时器
|
||||
{
|
||||
power_button.state = BUTTON_STATE_WAIT_FOR_LONG_PRESS;
|
||||
timerStart(&power_button.timer, 500, 0); // 启动等待长按定时器，500ms
|
||||
}
|
||||
}
|
||||
else if (!power_button.timer.active)// 短按定时器到期，按键仍被按下，视为无效，重置为初始状态
|
||||
{
|
||||
power_button.state = BUTTON_STATE_INITIAL;
|
||||
}
|
||||
break;
|
||||
|
||||
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
|
||||
}
|
||||
else if (!power_button.timer.active) // 等待长按超时，重置为初始状态
|
||||
{
|
||||
power_button.state = BUTTON_STATE_INITIAL;
|
||||
}
|
||||
break;
|
||||
|
||||
case BUTTON_STATE_LONG_PRESS:
|
||||
|
||||
if (!power_button.timer.active)// 长按完成，切换电源状态 20250423 修改不需要判断松开按键就打开控制器
|
||||
{
|
||||
power_button.is_power_on = !power_button.is_power_on;
|
||||
printf("PowerButton: is_power_on = %d\n", power_button.is_power_on);
|
||||
power_button.state = BUTTON_STATE_LONG_PRESS_WAIT;
|
||||
}
|
||||
else if(power_data.remote_power_switch == app_open())
|
||||
{
|
||||
power_button.state = BUTTON_STATE_INITIAL;
|
||||
printf("Long press for short duration");
|
||||
}
|
||||
else
|
||||
break;
|
||||
|
||||
case BUTTON_STATE_LONG_PRESS_WAIT:
|
||||
if (power_data.remote_power_switch == app_open())// 检测按键释放
|
||||
{
|
||||
power_button.state = BUTTON_STATE_INITIAL;
|
||||
printf("Release the button");
|
||||
}
|
||||
default:
|
||||
power_button.state = BUTTON_STATE_INITIAL;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//// 电源按钮处理函数
|
||||
//static void handlePowerButton(void)
|
||||
//{
|
||||
// switch (power_button.state)
|
||||
// {
|
||||
// case BUTTON_STATE_INITIAL:
|
||||
// if (power_data.remote_power_switch == app_close())
|
||||
// {
|
||||
// power_button.state = BUTTON_STATE_SHORT_PRESS_DETECTED;
|
||||
// timerStart(&power_button.timer, 500, 0); // 启动短按定时器，500ms
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case BUTTON_STATE_SHORT_PRESS_DETECTED:
|
||||
// if (power_data.remote_power_switch == app_open())
|
||||
// {
|
||||
// if (power_button.timer.active) // 定时器未到期,短按完成，启动等待长按定时器
|
||||
// {
|
||||
// power_button.state = BUTTON_STATE_WAIT_FOR_LONG_PRESS;
|
||||
// timerStart(&power_button.timer, 500, 0); // 启动等待长按定时器，500ms
|
||||
// }
|
||||
// }
|
||||
// else if (!power_button.timer.active)// 短按定时器到期，按键仍被按下，视为无效，重置为初始状态
|
||||
// {
|
||||
// power_button.state = BUTTON_STATE_INITIAL;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// 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
|
||||
// }
|
||||
// else if (!power_button.timer.active) // 等待长按超时，重置为初始状态
|
||||
// {
|
||||
// power_button.state = BUTTON_STATE_INITIAL;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case BUTTON_STATE_LONG_PRESS:
|
||||
//
|
||||
// if (!power_button.timer.active)// 长按完成，切换电源状态 20250423 修改不需要判断松开按键就打开控制器
|
||||
// {
|
||||
// power_button.is_power_on = !power_button.is_power_on;
|
||||
// printf("PowerButton: is_power_on = %d\n", power_button.is_power_on);
|
||||
// power_button.state = BUTTON_STATE_LONG_PRESS_WAIT;
|
||||
// }
|
||||
// else if(power_data.remote_power_switch == app_open())
|
||||
// {
|
||||
// power_button.state = BUTTON_STATE_INITIAL;
|
||||
// printf("Long press for short duration");
|
||||
// }
|
||||
// else
|
||||
// break;
|
||||
//
|
||||
// case BUTTON_STATE_LONG_PRESS_WAIT:
|
||||
// if (power_data.remote_power_switch == app_open())// 检测按键释放
|
||||
// {
|
||||
// power_button.state = BUTTON_STATE_INITIAL;
|
||||
// printf("Release the button");
|
||||
// }
|
||||
// default:
|
||||
// power_button.state = BUTTON_STATE_INITIAL;
|
||||
// break;
|
||||
// }
|
||||
//}
|
||||
|
||||
|
||||
// 输出处理函数
|
||||
@@ -113,99 +113,111 @@ 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.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_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = 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_output1.bit_data.KGF01 = setPowerOff(); // E3
|
||||
un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪
|
||||
un_inf_can_kgf_output1.bit_data.KGF03 = 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_output1.bit_data.KGF09 = 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.KGF13 = setPowerOn(); // 前左右红灯
|
||||
// un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 后左右红灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头
|
||||
un_inf_can_kgf_output1.bit_data.KGF16 = 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.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_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = 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_output1.bit_data.KGF01 = setPowerOff(); // E3
|
||||
un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪
|
||||
un_inf_can_kgf_output1.bit_data.KGF03 = 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_output1.bit_data.KGF09 = 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.KGF13 = setPowerOn(); // 前左右红灯
|
||||
// un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 后左右红灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头
|
||||
un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 遥控器
|
||||
break;
|
||||
|
||||
|
||||
case POWER_STANDBY:
|
||||
// 初始状态,只开启基本设备
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = 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_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = 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_output1.bit_data.KGF01 = setPowerOff(); // E3
|
||||
un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪
|
||||
un_inf_can_kgf_output1.bit_data.KGF03 = 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_output1.bit_data.KGF09 = 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.KGF13 = setPowerOn(); // 前左右红灯
|
||||
// un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 后左右红灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头
|
||||
un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 遥控器
|
||||
break;
|
||||
|
||||
case POWER_WORKING:
|
||||
// 工作状态,除预充继电器外所有设备开启
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
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_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = 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_output1.bit_data.KGF01 = setPowerOff(); // E3
|
||||
un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪
|
||||
un_inf_can_kgf_output1.bit_data.KGF03 = 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_output1.bit_data.KGF09 = 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.KGF13 = setPowerOn(); // 前左右红灯
|
||||
// un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 后左右红灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头
|
||||
un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 遥控器
|
||||
break;
|
||||
|
||||
case POWER_EMERGENCY:
|
||||
// 急停状态,断开高压
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = 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 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = 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_output1.bit_data.KGF01 = setPowerOff(); // E3
|
||||
un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪
|
||||
un_inf_can_kgf_output1.bit_data.KGF03 = 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_output1.bit_data.KGF09 = 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.KGF13 = setPowerOn(); // 前左右红灯
|
||||
// un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 后左右红灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头
|
||||
un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 遥控器
|
||||
break;
|
||||
|
||||
case POWER_SLEEP:
|
||||
// 休眠状态,关闭所有设备
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = 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_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = 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_output1.bit_data.KGF01 = setPowerOff(); // E3
|
||||
un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪
|
||||
un_inf_can_kgf_output1.bit_data.KGF03 = 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_output1.bit_data.KGF09 = 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.KGF13 = setPowerOn(); // 前左右红灯
|
||||
// un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 后左右红灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头
|
||||
un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 遥控器
|
||||
break;
|
||||
|
||||
default:
|
||||
@@ -213,7 +225,7 @@ static void powerOutput(void *signal_id)
|
||||
}
|
||||
publishMessage(&power_data, 1);
|
||||
publishMessage(&un_inf_can_kgf_output1, 1);
|
||||
publishMessage(&un_inf_can_kgf_output2, 1);
|
||||
// publishMessage(&un_inf_can_kgf_output2, 1);
|
||||
}
|
||||
|
||||
static void wakeupProcess(void *signal_id)
|
||||
@@ -246,13 +258,13 @@ static void powerTimerProcess(void *signal_id)
|
||||
(void)signal_id;
|
||||
|
||||
// 调用电源按钮处理函数
|
||||
handlePowerButton();
|
||||
// handlePowerButton();
|
||||
// 电源按钮状态有变化，保存到参数
|
||||
if (power_button.is_power_on != power_button.old_is_power_on)
|
||||
{
|
||||
setParam("pwr_btn", (float)power_button.is_power_on);
|
||||
power_button.old_is_power_on = power_button.is_power_on;
|
||||
}
|
||||
// if (power_button.is_power_on != power_button.old_is_power_on)
|
||||
// {
|
||||
// setParam("pwr_btn", (float)power_button.is_power_on);
|
||||
// power_button.old_is_power_on = power_button.is_power_on;
|
||||
// }
|
||||
|
||||
// 状态转换逻辑
|
||||
switch (power_data.current_state)
|
||||
@@ -282,25 +294,27 @@ static void powerTimerProcess(void *signal_id)
|
||||
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()) // 遥控器电源开关闭合且急停开关闭合
|
||||
{
|
||||
power_data.current_state = POWER_EMERGENCY; // 急停
|
||||
printf("Power: Transitioning from STANDBY to EMERGENCY state\n");
|
||||
}
|
||||
|
||||
power_data.current_state = POWER_WORKING; // 休眠 20251005 修改不需要休眠，直接上电
|
||||
// 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()) // 遥控器电源开关闭合且急停开关闭合
|
||||
// {
|
||||
// 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.high_voltage_switch == app_open()) // 高压开关断开
|
||||
// {
|
||||
// power_data.current_state = POWER_SLEEP; // 休眠
|
||||
// printf("Power: Transitioning from STANDBY to SLEEP state\n");
|
||||
//
|
||||
if (power_data.emergency_stop == app_close()) // 急停开关闭合
|
||||
{
|
||||
power_data.current_state = POWER_EMERGENCY; // 急停
|
||||
printf("Power: Transitioning from WORKING to EMERGENCY state\n");
|
||||
@@ -310,17 +324,17 @@ static void powerTimerProcess(void *signal_id)
|
||||
}
|
||||
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()) // 遥控器电源开关断开
|
||||
{
|
||||
power_data.current_state = POWER_STANDBY; // 待机
|
||||
printf("Power: Transitioning from EMERGENCY to STANDBY state\n");
|
||||
}
|
||||
else if (power_data.emergency_stop == app_open()) // 急停断开
|
||||
// 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()) // 遥控器电源开关断开 //20251005 修改不需要电源开关
|
||||
// {
|
||||
// power_data.current_state = POWER_STANDBY; // 待机
|
||||
// printf("Power: Transitioning from EMERGENCY to STANDBY state\n");
|
||||
// }
|
||||
if (power_data.emergency_stop == app_open()) // 急停断开
|
||||
{
|
||||
power_data.current_state = POWER_PRE_CHARGE; // 预充
|
||||
timerStart(&power_data.timer_pre_charge, (uint32_t)(getParam("prCTime") * 1000), 1); // 启动预充定时器
|
||||
@@ -362,10 +376,13 @@ static void powerInput(void *signal_id)
|
||||
memcpy(&old_data, &power_data, sizeof(PowerSystem));
|
||||
|
||||
// 填充数据
|
||||
power_data.emergency_stop_switch = 0;//急停开关 20251005 修改四轮四转车无急停开关以及采集模块 高压
|
||||
power_data.high_voltage_switch = 1;//高压开关
|
||||
|
||||
if (signal_id == &un_sw_sample)
|
||||
{
|
||||
power_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;//急停开关
|
||||
power_data.high_voltage_switch = (uint8_t)un_sw_sample.bit_data.High_voltage_switch;//高压开关
|
||||
// power_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;//急停开关
|
||||
// power_data.high_voltage_switch = (uint8_t)un_sw_sample.bit_data.High_voltage_switch;//高压开关
|
||||
}
|
||||
else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据
|
||||
{
|
||||
@@ -410,6 +427,21 @@ void powerAppInit(void)
|
||||
power_data.emergency_stop = (uint8_t)getParam("stop_sw");
|
||||
power_data.old_emergency_stop = power_data.emergency_stop;
|
||||
|
||||
un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOff(); // E3 初始化上电默认打开
|
||||
un_inf_can_kgf_output1.bit_data.KGF02 = setPowerOn(); // 导航仪
|
||||
un_inf_can_kgf_output1.bit_data.KGF03 = 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_output1.bit_data.KGF09 = 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.KGF13 = setPowerOn(); // 前左右红灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 后左右红灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF15 = setPowerOn(); // 网络摄像头
|
||||
un_inf_can_kgf_output1.bit_data.KGF16 = setPowerOn(); // 遥控器
|
||||
|
||||
// 订阅输入信号
|
||||
subscribe(&un_sw_sample, powerInput); // 急停开关、高压开关
|
||||
subscribe(&un_remote_control_input, powerInput); // 遥控器电源开关
|
||||
|
||||
403
app/app_turntable.c
Normal file
403
app/app_turntable.c
Normal file
@@ -0,0 +1,403 @@
|
||||
#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 "drive_rs04.h"
|
||||
#include "sdrv_vic.h"
|
||||
#include <math.h>
|
||||
|
||||
PID_t turnable_speed_pid;
|
||||
PID_t turnable_position_pid;
|
||||
|
||||
TurnableData turnable_data = {0};
|
||||
|
||||
|
||||
/**
|
||||
* @brief 将笛卡尔坐标 (x,y,z) 转换为球坐标 (r,θ,φ)
|
||||
*
|
||||
* @param x X轴坐标值（单位：米）
|
||||
* @param y Y轴坐标值（单位：米）
|
||||
* @param z Z轴坐标值（单位：米）
|
||||
* @param[out] out 输出球坐标结构体指针
|
||||
* @return bool 转换是否成功：
|
||||
* - true: 转换成功
|
||||
* - false: 输入无效（包含NaN/INF或out为NULL）
|
||||
*
|
||||
* @note 特殊输入处理：
|
||||
* 1. 如果输入包含 NaN 或无穷大，返回 false
|
||||
* 2. 当 r < 1e-10 时视为原点，设置 out=(0, 0, 0)
|
||||
* 3. 在Z轴附近 (|z/r| ≈ 1) 时自动截断到 [-1,1] 保证数值稳定性
|
||||
*
|
||||
* @warning 使用要求：
|
||||
* - 必须检查返回值，不能直接使用out内容
|
||||
* - out指针必须指向有效内存
|
||||
*
|
||||
* @example 正确用法：
|
||||
* SphericalCoordinate sph;
|
||||
* if (cartesianToSpherical(1.0f, 0.0f, 0.0f, &sph)) {
|
||||
* // 使用sph...
|
||||
* }
|
||||
*/
|
||||
uint8_t cartesianToSpherical(float x, float y, float z, SphericalCoordinate* out)
|
||||
{
|
||||
// 参数有效性检查（防御性编程）
|
||||
if (!out || !isfinite(x) || !isfinite(y) || !isfinite(z)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 计算径向距离 r = √(x² + y² + z²)
|
||||
out->r = sqrtf(x * x + y * y + z * z);
|
||||
|
||||
// 原点判定（使用容差避免浮点误差）
|
||||
if (out->r < 1e-10f) {
|
||||
out->theta = 0.0f;
|
||||
out->phi = 0.0f;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// 极角 θ = acos(z/r) 的数值稳定性处理
|
||||
float z_over_r = z / out->r;
|
||||
if (z_over_r > 1.0f) z_over_r = 1.0f; // 处理上溢出
|
||||
if (z_over_r < -1.0f) z_over_r = -1.0f; // 处理下溢出
|
||||
out->theta = acosf(z_over_r);
|
||||
|
||||
// 方位角 φ = atan2(y, x)
|
||||
out->phi = atan2f(y, x);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// 计算CRC8校验（多项式 x^8 + 1，简单的累加和）
|
||||
uint8_t encoder_calculate_crc(const uint8_t* data, uint8_t length)
|
||||
{
|
||||
uint8_t crc = 0x00;
|
||||
for (int i = 0; i < length; i++) {
|
||||
crc += data[i];
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
static void setTurnableMotorOutput()
|
||||
{
|
||||
|
||||
turnable_data.out_pitch_motor_ampere = constrain(turnable_data.out_pitch_motor_ampere, -RS02_ANGULAR_VELOCITY_MAX, RS02_ANGULAR_VELOCITY_MAX);
|
||||
turnable_data.out_left_motor_ampere = constrain(turnable_data.out_left_motor_ampere, -RS02_ANGULAR_VELOCITY_MAX, RS02_ANGULAR_VELOCITY_MAX);
|
||||
|
||||
|
||||
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, SPD_REF,turnable_data.out_left_motor_ampere);
|
||||
|
||||
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_output4, 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 转台
|
||||
static void turnableProcess(void *signal_id)
|
||||
{
|
||||
// if((turnable_data.current_state == POWER_WORKING))//高压上电才运行
|
||||
// {
|
||||
switch(turnable_data.turnable_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据
|
||||
{
|
||||
case 0:
|
||||
timerStart(&turnable_data.turnable_timer, 1000, 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, VELOCITY_MODE);
|
||||
|
||||
setMotorWrite(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output5, LIMIT_CUR,5); //设置最大电流为5A
|
||||
|
||||
publishMessage(&un_sdo_output1, 1);
|
||||
publishMessage(&un_sdo_output2, 1);
|
||||
publishMessage(&un_sdo_output5, 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);
|
||||
|
||||
publishMessage(&un_sdo_output1, 1);
|
||||
publishMessage(&un_sdo_output2, 1);
|
||||
}
|
||||
break;
|
||||
|
||||
case 4:
|
||||
turnable_data.turnable_cnt = 0;
|
||||
turnable_data.turnable_state = 4;
|
||||
|
||||
setTurnableMotorOutput();//输出函数
|
||||
break;
|
||||
|
||||
default:break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void turnableParametersInit(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||||
|
||||
if(0 == un_right_intput.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
|
||||
{
|
||||
motorEnable(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output3);
|
||||
publishMessage(&un_sdo_output3, 1);
|
||||
}
|
||||
|
||||
if(0 == un_pitch_intput.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
|
||||
{
|
||||
motorEnable(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output3);
|
||||
publishMessage(&un_sdo_output3, 1);
|
||||
}
|
||||
|
||||
|
||||
turnable_data.desired_horizontal_speed = getParam("turn_sp");
|
||||
turnable_data.desired_pitch_speed = getParam("pit_sp");
|
||||
|
||||
turnable_data.min_pitch_postion = getParam("minYpos"); //俯仰位置最小限制值
|
||||
turnable_data.max_pitch_postion = getParam("maxYpos"); //俯仰位置最大限制值
|
||||
|
||||
printf( "turnable left A %f\n",turnable_data.out_left_motor_ampere);
|
||||
printf( "turnable right A %f\n",turnable_data.out_right_motor_ampere);
|
||||
printf( "turnable pitch A %f\n",turnable_data.out_pitch_motor_ampere);
|
||||
// printf( "desired speed %f\n",turnable_data.desired_speed);
|
||||
printf( "speed %f\n",turnable_data.speed);
|
||||
printf( "turnable state %d\n",turnable_data.turnable_state);
|
||||
|
||||
timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
static void dataRequest(void *signal_id)
|
||||
{
|
||||
str_magnetic_encoder.magnetic_data = ENCODER_HEADER;
|
||||
|
||||
publishMessage(&str_magnetic_encoder, 1);
|
||||
timerStart(&turnable_data.turnable_timer3,100,1);//100ms调用一次
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 差速输入处理函数
|
||||
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.position_x = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_x) );
|
||||
turnable_data.position_y = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_y) );
|
||||
turnable_data.position_z = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_z) );
|
||||
}
|
||||
else if(signal_id == &un_encoder_data_input)
|
||||
{
|
||||
if( un_encoder_data_input.arr[sizeof(un_encoder_data_input)-1] == encoder_calculate_crc(&un_encoder_data_input.arr[0], sizeof(un_encoder_data_input)-1) )//CRC校验
|
||||
{
|
||||
turnable_data.horizontal_position = (float)(un_encoder_data_input.bit_data.abs_value)*2.0f*PI/ENCODER_MAX_COUNTS;//将数据转换为实际角度
|
||||
}
|
||||
}
|
||||
else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据
|
||||
{
|
||||
// diff_data.remote_emergency_stop = !(uint8_t)un_remote_control_input.bit_data.switch_b;
|
||||
// diff_data.mode = un_remote_control_input.bit_data.switch_c =
|
||||
|
||||
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.02*(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.01*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.01*fabs(x_axis_temp);
|
||||
turnable_data.desired_pitch_position = turnable_data.min_pitch_postion;
|
||||
}
|
||||
else
|
||||
{
|
||||
turnable_data.out_pitch_motor_ampere = 0;
|
||||
}
|
||||
}
|
||||
else if(signal_id == &un_pitch_intput)
|
||||
{
|
||||
turnable_data.pitch_position = convertPhysical( SWAP_ENDIAN_16(un_right_intput.rx_can_data.bit_data.current_angle),-RS02_ANGULAR_VELOCITY_MAX,RS02_ANGULAR_VELOCITY_MAX,MOTOR_ANGLE_DEADZONE );
|
||||
}
|
||||
else{}
|
||||
|
||||
turnable_data.right_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_right_intput.rx_can_data.bit_data.current_velocity),-RS02_ANGULAR_VELOCITY_MAX,RS02_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);//处理映射
|
||||
}
|
||||
|
||||
// timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次
|
||||
}
|
||||
|
||||
|
||||
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")
|
||||
// );
|
||||
|
||||
//目标参数初始化
|
||||
str_magnetic_encoder.ip[0] = 192;
|
||||
str_magnetic_encoder.ip[1] = 168;
|
||||
str_magnetic_encoder.ip[2] = 17;
|
||||
str_magnetic_encoder.ip[3] = 33;
|
||||
str_magnetic_encoder.port = 2011;
|
||||
|
||||
subscribe(&un_remote_control_input, turnableInput);
|
||||
subscribe(&un_computer_turnable_Input, turnableInput);
|
||||
subscribe(&un_pitch_intput, turnableInput);
|
||||
|
||||
timerInit(&turnable_data.turnable_timer);
|
||||
timerInit(&turnable_data.turnable_timer1);
|
||||
|
||||
// timerInit(&turnable_data.turnable_timer2);
|
||||
// subscribe(&turnable_data.turnable_timer2, turnableInput);
|
||||
//
|
||||
// timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次
|
||||
|
||||
subscribe(&turnable_data.turnable_timer1, turnableParametersInit);
|
||||
timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次
|
||||
|
||||
timerInit(&turnable_data.turnable_timer3);
|
||||
subscribe(&turnable_data.turnable_timer3, dataRequest);
|
||||
timerStart(&turnable_data.turnable_timer3,100,1);//100ms调用一次
|
||||
|
||||
subscribe(&un_encoder_data_input, turnableInput);
|
||||
|
||||
|
||||
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_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());
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
104
app/app_turntable.h
Normal file
104
app/app_turntable.h
Normal file
@@ -0,0 +1,104 @@
|
||||
#ifndef TURNTAABLE_H
|
||||
#define TURNTAABLE_H
|
||||
|
||||
|
||||
#include "app_power.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
//编码器协议头
|
||||
|
||||
#define ENCODER_HEADER 0x1A
|
||||
#define ENCODER_MAX_COUNTS 0x1FFFFF// 最大位21位
|
||||
|
||||
#define ENCODER_PORT 2011
|
||||
|
||||
|
||||
//转台电机
|
||||
#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
|
||||
|
||||
|
||||
|
||||
// 力矩量程定义 (对应Byte4~5: 当前力矩)
|
||||
#define TORQUE_MIN -120.0f // 最小力矩: -120 Nm
|
||||
#define TORQUE_MAX 120.0f // 最大力矩: 120 Nm
|
||||
|
||||
#define ZERO_VAULE 0x0080 // 32768,需要高位在前
|
||||
|
||||
#define REMOTE_ZERO 1022
|
||||
|
||||
|
||||
|
||||
typedef struct {
|
||||
float r; // 径向距离
|
||||
float theta; // 极角(与Z轴的夹角，弧度制，范围[0, π])
|
||||
float phi; // 方位角(XY平面内与X轴的夹角，弧度制，范围[-π, π])
|
||||
} SphericalCoordinate;
|
||||
|
||||
|
||||
|
||||
typedef struct TurnableData
|
||||
{
|
||||
uint8_t turnable_state;
|
||||
PowerState current_state; // 当前电源状态
|
||||
|
||||
float position_x; //转台相对位置x
|
||||
float position_y; //转台相对位置y
|
||||
float position_z; //转台相对位置z
|
||||
|
||||
float desired_pitch_speed; // 期望俯仰位置
|
||||
float desired_horizontal_speed; // 期望水平位置
|
||||
float desired_pitch_position; // 期望俯仰位置
|
||||
float desired_horizontal_position; // 期望水平位置
|
||||
|
||||
float left_motor_speed; // 当前左电机速度
|
||||
float right_motor_speed; // 当前右电机速度
|
||||
float speed; // 当前转盘速度
|
||||
float pitch_position; // 当前俯仰位置
|
||||
float horizontal_position; // 当前水平位置
|
||||
|
||||
float max_speed; // 最大速度
|
||||
float out_left_motor_ampere; // 输出左电机电流
|
||||
float out_right_motor_ampere; // 输出右电机电流
|
||||
float out_pitch_motor_ampere; // 输出右电机电流
|
||||
|
||||
float out_left_motor_ampere_last; // 输出左电机电流
|
||||
float out_right_motor_ampere_last; // 输出右电机电流
|
||||
float out_pitch_motor_ampere_last; // 输出右电机电流
|
||||
|
||||
float out_left_motor_ampere_limit; // 输出左电机电流限制值
|
||||
float out_right_motor_ampere_limit; // 输出右电机电流限制值
|
||||
float out_pitch_motor_ampere_limit; // 输出右电机电流限制值
|
||||
|
||||
Timer turnable_timer; // 定时器
|
||||
Timer turnable_timer1; // 定时器
|
||||
Timer turnable_timer2; // 定时器
|
||||
Timer turnable_timer3; // 定时器
|
||||
|
||||
uint8_t turnable_cnt;
|
||||
|
||||
float max_ampere; // 最大电流限制
|
||||
|
||||
float min_pitch_postion; // 位置信息
|
||||
float max_pitch_postion; // 位置信息
|
||||
} TurnableData;
|
||||
|
||||
|
||||
|
||||
void turnableInit();
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // TURNTAABLE_H
|
||||
127
app_brake.c
127
app_brake.c
@@ -1,127 +0,0 @@
|
||||
#include "app_config.h"
|
||||
|
||||
//定义并初始化
|
||||
BrakeSystem brake_system = {
|
||||
.brake_timer = {0, 0, 0, 0, 0}, // 初始化 Timer 结构体
|
||||
.brake_command = 0,
|
||||
.brake_motor_state = 0,
|
||||
.brake_command_in_progress = 0,
|
||||
.emergency_stop_switch = 0,
|
||||
.remote_emergency_stop = 0,
|
||||
.remote_fault = 0,
|
||||
.can_bus_fault = 0,
|
||||
.ethernet_fault = 0,
|
||||
.mode_signal = 0
|
||||
};
|
||||
|
||||
|
||||
// 处理所有输入信号的函数
|
||||
static void processInputSignals(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||||
|
||||
// 如果当前有刹车命令在执行，忽略新的输入信号
|
||||
if (brake_system.brake_command_in_progress)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (brake_system.emergency_stop_switch || brake_system.remote_emergency_stop ||
|
||||
(brake_system.mode_signal == 0 && brake_system.remote_fault) || // 手动模式下的遥控器故障
|
||||
(brake_system.mode_signal == 1 && (brake_system.can_bus_fault || brake_system.ethernet_fault))) // 自动模式下的CAN总线或以太网通信故障
|
||||
{
|
||||
brake_system.brake_command = 1; // 发送刹车指令
|
||||
publishMessage(&brake_system.brake_command, 1);
|
||||
brake_system.brake_command_in_progress = 1; // 标记刹车命令正在执行
|
||||
}
|
||||
else
|
||||
{
|
||||
brake_system.brake_command = 2; // 发送释放刹车指令
|
||||
publishMessage(&brake_system.brake_command, 1);
|
||||
brake_system.brake_command_in_progress = 1; // 标记释放命令正在执行
|
||||
}
|
||||
}
|
||||
|
||||
// 刹车信号处理函数
|
||||
static void brakeProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||||
int signal = *(int *)signal_id;
|
||||
|
||||
if (signal == 1) // 刹车
|
||||
{
|
||||
brake_system.brake_motor_state = 1;
|
||||
publishMessage(&brake_system.brake_motor_state, 1); // 状态改变时发送刹车电机状态信号
|
||||
timerInit(&brake_system.brake_timer, 1500); // 控制刹车电机前进,保持1.5秒
|
||||
}
|
||||
else if (signal == 2) // 释放刹车
|
||||
{
|
||||
brake_system.brake_motor_state = 2;
|
||||
publishMessage(&brake_system.brake_motor_state, 1); // 状态改变时发送刹车电机状态信号
|
||||
timerInit(&brake_system.brake_timer, 800); // 控制刹车电机后退,保持0.8秒
|
||||
}
|
||||
|
||||
timerStart(&brake_system.brake_timer);
|
||||
}
|
||||
|
||||
// 定时器信号处理函数
|
||||
static void brakeTimerProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||||
|
||||
brake_system.brake_motor_state = 0;
|
||||
brake_system.brake_command_in_progress = 0; // 刹车命令执行完成，重置状态
|
||||
publishMessage(&brake_system.brake_motor_state, 1); // 状态改变时发送刹车电机状态信号
|
||||
}
|
||||
|
||||
// 输出处理函数
|
||||
static void outputProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||||
//根据电机状态，填充发送数据结构，发送信号
|
||||
switch (brake_system.brake_motor_state) {
|
||||
case 1: // 电机前进状态
|
||||
UnHBridgeOutput.brake_motor1 = 0;
|
||||
UnHBridgeOutput.brake_motor2 = 1;
|
||||
UnHBridgeOutput.sleep_01 = 0;
|
||||
break;
|
||||
|
||||
case 2: // 电机后退状态
|
||||
UnHBridgeOutput.brake_motor1 = 1;
|
||||
UnHBridgeOutput.brake_motor2 = 0;
|
||||
UnHBridgeOutput.sleep_01 = 0;
|
||||
break;
|
||||
default:
|
||||
UnHBridgeOutput.brake_motor1 = 0;
|
||||
UnHBridgeOutput.brake_motor2 = 0;
|
||||
UnHBridgeOutput.sleep_01 = 0;
|
||||
break;
|
||||
}
|
||||
publishMessage(&UnHBridgeOutput, 1);
|
||||
}
|
||||
|
||||
// APP模块的初始化
|
||||
void brakeAppInit(void)
|
||||
{
|
||||
// 初始化定时器，使用 brake_timer 的地址作为信号ID
|
||||
timerInit(&brake_system.brake_timer, 0);
|
||||
|
||||
// 订阅刹车命令
|
||||
subscribe(&brake_system.brake_command, brakeProcess);
|
||||
|
||||
// 订阅定时器信号，用于停止电机
|
||||
subscribe(&brake_system.brake_timer, brakeTimerProcess);
|
||||
|
||||
// 订阅电机状态
|
||||
subscribe(&brake_system.brake_motor_state, outputProcess);
|
||||
|
||||
// 订阅输入信号，处理刹车逻辑
|
||||
subscribe(&UnSwSample, processInputSignals);
|
||||
subscribe(&UnRemoteControlInput, processInputSignals);
|
||||
subscribe(&brake_system.remote_fault, processInputSignals);
|
||||
subscribe(&brake_system.can_bus_fault, processInputSignals);
|
||||
subscribe(&brake_system.ethernet_fault, processInputSignals);
|
||||
subscribe(&brake_system.mode_signal, processInputSignals);
|
||||
|
||||
|
||||
}
|
||||
34
app_brake.h
34
app_brake.h
@@ -1,34 +0,0 @@
|
||||
#ifndef APP_BRAKE_H
|
||||
#define APP_BRAKE_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
#include "app_config.h"
|
||||
|
||||
|
||||
typedef struct {
|
||||
Timer brake_timer; // 定时器结构体
|
||||
int brake_command; // 刹车命令变量：1表示刹车，2表示释放
|
||||
int brake_motor_state; // 刹车电机状态变量：0停止，1前进，2后退
|
||||
int brake_command_in_progress; // 刹车命令执行状态：0表示空闲，1表示正在执行
|
||||
//输入
|
||||
int emergency_stop_switch; // 急停开关
|
||||
int remote_emergency_stop; // 遥控器急停开关
|
||||
int remote_fault; // 遥控器故障
|
||||
int can_bus_fault; // CAN总线故障
|
||||
int ethernet_fault; // 以太网通信故障
|
||||
int mode_signal; // 模式信号：0表示手动模式，1表示自动模式
|
||||
} BrakeSystem;
|
||||
|
||||
// 在头文件中声明外部变量
|
||||
extern BrakeSystem brake_system;
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // APP_BRAKE_H
|
||||
460
app_turntable.c
460
app_turntable.c
@@ -1,460 +0,0 @@
|
||||
#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, IQ_REF_INDEX,turnable_data.out_pitch_motor_ampere_limit);
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 转台
|
||||
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, 1000, 0); // 启动定时器，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.out_left_motor_ampere = calculatePidOutput(&turnable_speed_pid, turnable_data.desired_speed, turnable_data.speed, 0.0f, dt);
|
||||
|
||||
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")
|
||||
);
|
||||
|
||||
printf( "turnable left A %f\n",turnable_data.out_left_motor_ampere);
|
||||
printf( "turnable right A %f\n",turnable_data.out_right_motor_ampere);
|
||||
printf( "turnable pitch A %f\n",turnable_data.out_pitch_motor_ampere);
|
||||
printf( "desired speed %f\n",turnable_data.desired_speed);
|
||||
printf( "speed %f\n",turnable_data.speed);
|
||||
printf( "turnable state %d\n",turnable_data.turnable_state);
|
||||
|
||||
timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 差速输入处理函数
|
||||
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{}
|
||||
|
||||
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;
|
||||
|
||||
turnableProcess(signal_id);//处理映射
|
||||
timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次
|
||||
}
|
||||
|
||||
|
||||
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);
|
||||
|
||||
timerInit(&turnable_data.turnable_timer);
|
||||
timerInit(&turnable_data.turnable_timer1);
|
||||
|
||||
timerInit(&turnable_data.turnable_timer2);
|
||||
subscribe(&turnable_data.turnable_timer2, turnableInput);
|
||||
|
||||
timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次
|
||||
|
||||
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());
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
119
app_turntable.h
119
app_turntable.h
@@ -1,119 +0,0 @@
|
||||
#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 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><D2B5><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 max_ampere; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
} TurnableData;
|
||||
|
||||
|
||||
|
||||
void turnableInit();
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // TURNTAABLE_H
|
||||
212
drive_rs04.c
Normal file
212
drive_rs04.c
Normal file
@@ -0,0 +1,212 @@
|
||||
#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)
|
||||
{
|
||||
// 计算期望的变化量
|
||||
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
drive_rs04.h
Normal file
69
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_
|
||||
43
interface.c
43
interface.c
@@ -2,10 +2,10 @@
|
||||
|
||||
#include "interface_config.h"
|
||||
|
||||
UnMotorInput un_motor_input1 ;//电机控制器1 左前侧
|
||||
UnMotorInput un_motor_input2 ;//电机控制器2 右前侧
|
||||
UnMotorInput un_motor_input3 ;//电机控制器1 左后侧
|
||||
UnMotorInput un_motor_input4 ;//电机控制器2 右后侧
|
||||
StrRxCanFrame un_motor_input1 ;//电机控制器1 左前侧
|
||||
StrRxCanFrame un_motor_input2 ;//电机控制器2 右前侧
|
||||
StrRxCanFrame un_motor_input3 ;//电机控制器1 左后侧
|
||||
StrRxCanFrame un_motor_input4 ;//电机控制器2 右后侧
|
||||
|
||||
|
||||
UnMotorTempInput un_motor_temp1 ;//电机控制器1 左前侧
|
||||
@@ -18,10 +18,16 @@ UnMotorTempInput un_motor_temp4 ;//电机控制器4
|
||||
UnBmsInput un_bms_input ;//BMS接收数据
|
||||
UnTempModuleInput un_temp_module_input ;//温度采集模块
|
||||
|
||||
UnMotorOutput un_motor_output1 ;//电机输出
|
||||
UnMotorOutput un_motor_output2 ;//电机输出
|
||||
UnMotorOutput un_motor_output3 ;//电机输出
|
||||
UnMotorOutput un_motor_output4 ;//电机输出
|
||||
StrTxCanFrame un_motor_output1 ;//电机输出
|
||||
StrTxCanFrame un_motor_output2 ;//电机输出
|
||||
StrTxCanFrame un_motor_output3 ;//电机输出
|
||||
StrTxCanFrame un_motor_output4 ;//电机输出
|
||||
|
||||
StrTxCanFrame un_motor_output5 ;//电机输出
|
||||
StrTxCanFrame un_motor_output6 ;//电机输出
|
||||
StrTxCanFrame un_motor_output7 ;//电机输出
|
||||
StrTxCanFrame un_motor_output8 ;//电机输出
|
||||
|
||||
|
||||
UnInfCanKGFOutput un_inf_can_kgf_output1 ;//kgf输出
|
||||
UnInfCanKGFOutput un_inf_can_kgf_output2 ;
|
||||
@@ -41,6 +47,21 @@ UnUltrasonicInput un_ultrasonic_input1 ;//超声波传感
|
||||
UnUltrasonicOutput un_ultrasonic_output1 ;//超声波传感器输出
|
||||
|
||||
|
||||
StrTxCanFrame un_sdo_output1 ;//电机1输出
|
||||
StrTxCanFrame un_sdo_output2 ;//电机2输出
|
||||
StrTxCanFrame un_sdo_output3 ;//电机3输出
|
||||
StrTxCanFrame un_sdo_output4 ;//电机4输出
|
||||
StrTxCanFrame un_sdo_output5 ;//电机5输出
|
||||
|
||||
|
||||
|
||||
|
||||
StrRxCanFrame un_pitch_intput ;//电机输入
|
||||
StrRxCanFrame un_right_intput ;//电机输入
|
||||
|
||||
|
||||
|
||||
|
||||
//IO口
|
||||
UnSwSample un_sw_sample ;//采集
|
||||
|
||||
@@ -52,6 +73,12 @@ UnRequestFrame un_request_frame ;//请求帧
|
||||
|
||||
UnComputerOutput un_computer_output ;//输出给自主计算机
|
||||
|
||||
UnComputerTurnableInput un_computer_turnable_Input ;//转台以太网输入
|
||||
|
||||
UnEncoderData un_encoder_data_input ;
|
||||
|
||||
StrMagneticEncoder str_magnetic_encoder ;//编码器请求
|
||||
|
||||
//输出给上位机
|
||||
UnVehicleInfoOutput un_vehicle_Info_output ;// 车辆信息，输出给上位机
|
||||
UnMotorStatusOutput un_motor_status_output ;// 电机状态信息，输出给上位机
|
||||
|
||||
252
interface.h
252
interface.h
@@ -59,14 +59,6 @@ typedef union _UnCanDebugOutput
|
||||
} UnCanDebugOutput;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
typedef struct _StrMotorTempInput
|
||||
{
|
||||
//-----接收数据0x103或者0x104----------------------------------------------
|
||||
@@ -86,31 +78,15 @@ typedef union _UnMotorTempInput
|
||||
uint8_t arr[sizeof(StrMotorInput)]; // 通过结构体类型确定大小
|
||||
} UnMotorTempInput;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 接收BMS输入
|
||||
typedef struct _StrBmsInput
|
||||
{
|
||||
//----接收0x100----------------------------------
|
||||
// 多字节数据，高位在前，低位在后
|
||||
unsigned int bus_voltage : 16; // 母线电压 单位为10mV
|
||||
unsigned int bus_current : 16; // 母线电流 单位为10mA
|
||||
unsigned int remainder_capacity : 16; // 剩余容量 单位为10mAh 充电为正，放电为负
|
||||
unsigned int crc1 : 16; //crc
|
||||
//----接收0x101----------------------------------
|
||||
// 多字节数据，高位在前，低位在后
|
||||
unsigned int full_capacity : 16; // 充满容量 单位为10mAh
|
||||
unsigned int Discharge_times : 16; // 放电循环次数 单位为1次
|
||||
unsigned int soc : 16; // soc 1%
|
||||
unsigned int crc2 : 16; //crc
|
||||
//-----接收BMS数据----------------------------------------------
|
||||
uint16_t bus_voltage; // 总电压 系数 0.1V/bit
|
||||
uint16_t bus_current; // 电流 系数 0.1A/bit 偏移量 -30000
|
||||
uint16_t soc; // SOC 系数 0.1%/bit
|
||||
uint8_t Life; // 生命周期 范围 0~255
|
||||
uint8_t Reserve; // 保留位
|
||||
} StrBmsInput;
|
||||
|
||||
typedef union _UnBmsInput
|
||||
@@ -169,9 +145,6 @@ typedef union _UnAutoComputerInput
|
||||
} UnAutoComputerInput;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 接收自主计算机手动输入
|
||||
|
||||
typedef struct _StrManualComputerInput
|
||||
@@ -193,14 +166,6 @@ typedef union _UnManualComputerInput
|
||||
unsigned int arr[sizeof(StrManualComputerInput) / sizeof(unsigned int)]; // 通过结构体类型确定大小
|
||||
} UnManualComputerInput;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 接收请求帧
|
||||
typedef struct _StrRequestFrame
|
||||
{
|
||||
@@ -236,6 +201,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
|
||||
@@ -244,6 +214,14 @@ typedef union _UnRemoteControlInput
|
||||
uint8_t arr[sizeof(StrRemoteControlInput)]; // 通过结构体类型确定大小
|
||||
} UnRemoteControlInput;
|
||||
|
||||
// 磁编
|
||||
typedef struct _StrMagneticEncoder
|
||||
{
|
||||
uint8_t ip[4]; // IPv4
|
||||
uint16_t port; // 端口
|
||||
uint8_t magnetic_data; // 协议类型（1A）
|
||||
} StrMagneticEncoder;
|
||||
|
||||
|
||||
//-----IO口---------------------------------------------------------------
|
||||
// 从IO口输入
|
||||
@@ -664,6 +642,103 @@ typedef union _UnrLifterOutput
|
||||
} UnLifterOutput;
|
||||
|
||||
|
||||
// CAN ID 解析（联合体形式，支持位域和32位直接访问）
|
||||
typedef union _UnCanIdInfo
|
||||
{
|
||||
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;
|
||||
|
||||
} UnCanIdInfo;
|
||||
|
||||
|
||||
// 输出can数据
|
||||
typedef struct _StrTxCanOutput
|
||||
{
|
||||
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 解析联合体 (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;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//-----以太网-------------------------------------------------------------
|
||||
// 输出给自主计算机
|
||||
typedef struct _StrComputerOutput
|
||||
@@ -895,6 +970,57 @@ typedef union _UnAutoControlOutput
|
||||
} UnAutoControlOutput;
|
||||
|
||||
|
||||
// 接收转台指令输入
|
||||
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;
|
||||
|
||||
|
||||
|
||||
// 编码器数据帧结构体（针对0x1A命令的响应）
|
||||
typedef struct _StrEncoderData {
|
||||
//--------------------------------------------------
|
||||
// 编码器数据帧 - 对应 0x1A 命令的响应格式（10字节）
|
||||
unsigned int cf : 8; // 命令识别符 固定值0x1A
|
||||
unsigned int sf : 8; // 编码器状态 1byte
|
||||
unsigned int abs_value : 24; // 角度数据 3byte LSB（重命名：abs_value）
|
||||
unsigned int enid : 8; // 编码器ID 1byte
|
||||
unsigned int abm : 24; // 圈数数据 3byte LSB
|
||||
unsigned int almc : 8; // 编码器故障 1byte
|
||||
unsigned int crc : 8; // CRC校验 多项式x^8+1
|
||||
} StrEncoderData;
|
||||
|
||||
// 联合体定义，便于字节数组访问
|
||||
typedef union _UnEncoderData {
|
||||
StrEncoderData bit_data; // 位域方式访问
|
||||
unsigned char arr[sizeof(StrEncoderData)]; // 字节数组方式访问
|
||||
} UnEncoderData;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -914,10 +1040,11 @@ typedef union _UnVoltageSignalOutput
|
||||
|
||||
|
||||
//外部数据结构声明
|
||||
extern UnMotorInput un_motor_input1 ;//电机控制器1 左侧
|
||||
extern UnMotorInput un_motor_input2 ;//电机控制器2 右侧
|
||||
extern UnMotorInput un_motor_input3 ;//电机控制器1 左后侧
|
||||
extern UnMotorInput un_motor_input4 ;//电机控制器2 右后侧
|
||||
extern StrRxCanFrame un_motor_input1 ;//电机控制器1 左侧
|
||||
extern StrRxCanFrame un_motor_input2 ;//电机控制器2 右侧
|
||||
extern StrRxCanFrame un_motor_input3 ;//电机控制器1 左后侧
|
||||
extern StrRxCanFrame un_motor_input4 ;//电机控制器2 右后侧
|
||||
|
||||
extern UnBmsInput un_bms_input ;//BMS接收数据
|
||||
extern UnTempModuleInput un_temp_module_input;//温度采集模块
|
||||
extern UnAutoComputerInput un_auto_computer_input;//自主计算机自动数据
|
||||
@@ -929,10 +1056,18 @@ extern UnUltrasonicOutput un_ultrasonic_output1;//超声波传感器输出
|
||||
|
||||
|
||||
|
||||
extern UnMotorOutput un_motor_output1; //电机输出
|
||||
extern UnMotorOutput un_motor_output2; //电机输出
|
||||
extern UnMotorOutput un_motor_output3; //电机输出
|
||||
extern UnMotorOutput un_motor_output4; //电机输出
|
||||
extern StrTxCanFrame un_motor_output1; //电机输出
|
||||
extern StrTxCanFrame un_motor_output2; //电机输出
|
||||
extern StrTxCanFrame un_motor_output3; //电机输出
|
||||
extern StrTxCanFrame un_motor_output4; //电机输出
|
||||
|
||||
|
||||
extern StrTxCanFrame un_motor_output5 ;//电机输出
|
||||
extern StrTxCanFrame un_motor_output6 ;//电机输出
|
||||
extern StrTxCanFrame un_motor_output7 ;//电机输出
|
||||
extern StrTxCanFrame un_motor_output8 ;//电机输出
|
||||
|
||||
|
||||
extern UnMotorTempInput un_motor_temp1; //电机控制器1 温度
|
||||
extern UnMotorTempInput un_motor_temp2; //电机控制器2 温度
|
||||
extern UnMotorTempInput un_motor_temp3; //电机控制器3 左后侧
|
||||
@@ -958,6 +1093,11 @@ extern UnSwSample un_sw_sample;
|
||||
extern UnRequestFrame un_request_frame; //请求帧
|
||||
|
||||
extern UnComputerOutput un_computer_output; //输出给自主计算机
|
||||
extern UnComputerTurnableInput un_computer_turnable_Input ;//转台以太网输入
|
||||
|
||||
extern UnEncoderData un_encoder_data_input ;
|
||||
|
||||
extern StrMagneticEncoder str_magnetic_encoder ;//编码器请求
|
||||
|
||||
//输出给上位机
|
||||
extern UnVehicleInfoOutput un_vehicle_Info_output; // 车辆信息，输出给上位机
|
||||
@@ -969,6 +1109,20 @@ extern UnManualControlOutput un_manual_control_output;// 手动控制数
|
||||
extern UnAutoControlOutput un_auto_control_output; // 自动控制数据输出，返回给请求者
|
||||
extern UnSdoOutput un_sdo_output ;//转向电机输出
|
||||
|
||||
extern StrTxCanFrame un_sdo_output1 ;//电机1输出
|
||||
extern StrTxCanFrame un_sdo_output2 ;//电机2输出
|
||||
extern StrTxCanFrame un_sdo_output3 ;//电机3输出
|
||||
extern StrTxCanFrame un_sdo_output4 ;//电机4输出
|
||||
extern StrTxCanFrame un_sdo_output5 ;//电机5输出
|
||||
|
||||
extern StrTxCanFrame un_motor_output5 ;//电机1输出
|
||||
extern StrTxCanFrame un_motor_output6 ;//电机2输出
|
||||
extern StrTxCanFrame un_motor_output7 ;//电机3输出
|
||||
extern StrTxCanFrame un_motor_output8 ;//电机4输出
|
||||
|
||||
extern StrRxCanFrame un_pitch_intput ;//电机输入
|
||||
extern StrRxCanFrame un_right_intput ;//电机输入
|
||||
|
||||
|
||||
//变量
|
||||
extern uint8_t test_app[26];
|
||||
|
||||
@@ -80,7 +80,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 +110,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>
|
||||
|
||||
|
||||
371
interface_can.c
371
interface_can.c
@@ -3,14 +3,16 @@
|
||||
#include "debug.h"
|
||||
#include "interface_config.h"
|
||||
#include "app/app_config.h"
|
||||
#define TX_PADDING_VAL (0xA5)
|
||||
#include "sdrv_vic.h"
|
||||
#include "intrinsics.h"
|
||||
#include "app/app_differential_drive.h"
|
||||
#include "app/app_param_manage.h"
|
||||
#include "app/app_ultrasonic.h"
|
||||
#include "app/app_turntable.h"
|
||||
#include "drive_rs04.h"
|
||||
|
||||
|
||||
#define TX_PADDING_VAL (0xA5)
|
||||
|
||||
uint32_t OTA_CANTxID = 0x02;//Ĭ<>Ϸ<EFBFBD><CFB7><EFBFBD>IDΪ0x02
|
||||
uint32_t OTA_CANRxID = 0x01;//Ĭ<>Ͻ<EFBFBD><CFBD><EFBFBD>IDΪ0x01
|
||||
@@ -296,45 +298,15 @@ void flexcan_Receive_callback_1(flexcan_handle_t *handle,
|
||||
break;
|
||||
|
||||
case FLEXCAN_RX_FIFO_IDLE:
|
||||
if(LEFT_FRONT_MOTOR_INPUT1 == (buf->id))
|
||||
if( FRONT_LEFT_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) )
|
||||
{
|
||||
can_fault_info.bit_data.motor1_count ++;
|
||||
un_motor_input1.rx_can_id.raw = (buf->id);
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_motor_input1.arr[i] = buf->dataBuffer[i];
|
||||
un_motor_input1.rx_can_data.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
publishMessage(&un_motor_input1, 1);
|
||||
|
||||
}
|
||||
else if( LEFT_FRONT_MOTOR_INPUT2 == (buf->id) )
|
||||
{
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_motor_temp1.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
publishMessage(&un_motor_temp1, 1);//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
}
|
||||
else if( LEFT_REAR_MOTOR2_INPUT1 == (buf->id) )//
|
||||
{
|
||||
can_fault_info.bit_data.motor3_count ++;
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_motor_input3.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
// publishMessage(&un_motor_input3, 1); //<2F><EFBFBD>Ϊ
|
||||
}
|
||||
else if( LEFT_REAR_MOTOR2_INPUT2 == (buf->id) )
|
||||
{
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_motor_temp3.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
publishMessage(&un_motor_temp3, 1);//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
}
|
||||
else{}
|
||||
break;
|
||||
|
||||
case FLEXCAN_TX_IDLE:
|
||||
@@ -380,43 +352,14 @@ void flexcan_Receive_callback_2(flexcan_handle_t *handle,
|
||||
break;
|
||||
|
||||
case FLEXCAN_RX_FIFO_IDLE:
|
||||
if(RIGHT_FRONT_MOTOR_INPUT1 == (buf->id))
|
||||
if( FRONT_RIGHT_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) )
|
||||
{
|
||||
can_fault_info.bit_data.motor2_count ++;
|
||||
un_motor_input2.rx_can_id.raw = (buf->id);
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_motor_input2.arr[i] = buf->dataBuffer[i];
|
||||
un_motor_input2.rx_can_data.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
publishMessage(&un_motor_input2, 1);
|
||||
}
|
||||
else if( RIGHT_FRONT_MOTOR_INPUT2 == (buf->id) )
|
||||
{
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_motor_temp2.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
publishMessage(&un_motor_temp2, 1);//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
}
|
||||
else if( RIGHT_REAR_MOTOR_INPUT1 == (buf->id) )//
|
||||
{
|
||||
can_fault_info.bit_data.motor4_count ++;
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_motor_input4.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
publishMessage(&un_motor_input4, 1);
|
||||
}
|
||||
else if( RIGHT_REAR_MOTOR_INPUT2 == (buf->id) )
|
||||
{
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_motor_temp4.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
publishMessage(&un_motor_temp4, 1);//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
|
||||
}
|
||||
else{}
|
||||
break;
|
||||
@@ -465,23 +408,14 @@ void flexcan_Receive_callback_3(flexcan_handle_t *handle,
|
||||
|
||||
case FLEXCAN_RX_FIFO_IDLE:
|
||||
|
||||
if(BMS_INPUT_ID1 == (buf->id))//BMS
|
||||
if( REAR_LEFT_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) )
|
||||
{
|
||||
can_fault_info.bit_data.bms_count ++;
|
||||
un_motor_input3.rx_can_id.raw = (buf->id);
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_bms_input.arr[i] = buf->dataBuffer[i];
|
||||
un_motor_input3.rx_can_data.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
publishMessage(&un_bms_input, 1);
|
||||
}
|
||||
else if(BMS_INPUT_ID2 == (buf->id))//BMS
|
||||
{
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_bms_input.arr[i+8] = buf->dataBuffer[i];
|
||||
}
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
publishMessage(&un_bms_input, 1);
|
||||
|
||||
}
|
||||
else{}
|
||||
|
||||
@@ -520,12 +454,24 @@ void flexcan_Receive_callback_4(flexcan_handle_t *handle,
|
||||
void *userData)
|
||||
{
|
||||
//--------------------------------------------------------------
|
||||
flexcan_frame_t *buf = (flexcan_frame_t *)userData;
|
||||
uint8_t i = 0;
|
||||
|
||||
switch (status)
|
||||
{
|
||||
case FLEXCAN_RX_IDLE:
|
||||
break;
|
||||
|
||||
case FLEXCAN_RX_FIFO_IDLE:
|
||||
if( REAR_RIGHT_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) )
|
||||
{
|
||||
un_motor_input4.rx_can_id.raw = (buf->id);
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_motor_input4.rx_can_data.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
}
|
||||
else{}
|
||||
|
||||
break;
|
||||
|
||||
@@ -561,6 +507,8 @@ void flexcan_Receive_callback_5(flexcan_handle_t *handle,
|
||||
flexcan_status_e status, uint32_t result,
|
||||
void *userData)
|
||||
{
|
||||
flexcan_frame_t *buf = (flexcan_frame_t *)userData;
|
||||
uint8_t i = 0;
|
||||
//--------------------------------------------------------------
|
||||
switch (status)
|
||||
{
|
||||
@@ -568,7 +516,12 @@ void flexcan_Receive_callback_5(flexcan_handle_t *handle,
|
||||
break;
|
||||
|
||||
case FLEXCAN_RX_FIFO_IDLE:
|
||||
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_bms_input.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
publishMessage(&un_bms_input, 1);
|
||||
break;
|
||||
|
||||
case FLEXCAN_TX_IDLE:
|
||||
@@ -633,6 +586,24 @@ void flexcan_Receive_callback_6(flexcan_handle_t *handle,
|
||||
}
|
||||
publishMessage(&un_ultrasonic_input1, 1);
|
||||
}
|
||||
else if( RIGHT_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) )
|
||||
{
|
||||
un_right_intput.rx_can_id.raw = (buf->id);
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_right_intput.rx_can_data.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
// publishMessage(&un_right_intput, 1);
|
||||
}
|
||||
else if( PITCH_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) )
|
||||
{
|
||||
un_pitch_intput.rx_can_id.raw = (buf->id);
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_pitch_intput.rx_can_data.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
// publishMessage(&un_pitch_intput, 1);
|
||||
}
|
||||
else{}
|
||||
break;
|
||||
|
||||
@@ -689,8 +660,13 @@ void flexcan_Receive_callback_7(flexcan_handle_t *handle,
|
||||
}
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>
|
||||
publishMessage(&un_remote_control_input, 1);
|
||||
|
||||
// printf("ori_remote_stop: %d\n", un_remote_control_input.bit_data.switch_b);
|
||||
}
|
||||
else if(REMOTE_ID_1 == (buf->id))
|
||||
{
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_remote_control_input.arr[i+8] = buf->dataBuffer[i];
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -898,11 +874,11 @@ void initialization_All_Flexcan(void)
|
||||
{
|
||||
sdrv_gpio_set_pin_output_level(GPIO_H12, 0); //CANʹ<4E><CAB9>
|
||||
|
||||
initialization_Flexcan(CAN_INDEX_0, &can_0_config, &can_0_fifo_config, flexcan_Receive_callback_0);//250k
|
||||
initialization_Flexcan(CAN_INDEX_0, &can_0_config, &can_0_fifo_config, flexcan_Receive_callback_0);//1M
|
||||
|
||||
initialization_Flexcan(CAN_INDEX_1, &can_1_config, &can_1_fifo_config, flexcan_Receive_callback_1);//250k
|
||||
initialization_Flexcan(CAN_INDEX_1, &can_1_config, &can_1_fifo_config, flexcan_Receive_callback_1);//1M
|
||||
|
||||
initialization_Flexcan(CAN_INDEX_2, &can_2_config, &can_2_fifo_config, flexcan_Receive_callback_2);//250k
|
||||
initialization_Flexcan(CAN_INDEX_2, &can_2_config, &can_2_fifo_config, flexcan_Receive_callback_2);//1M
|
||||
|
||||
initialization_Flexcan(CAN_INDEX_3, &can_3_config, &can_3_fifo_config, flexcan_Receive_callback_3);//1M
|
||||
|
||||
@@ -1041,85 +1017,112 @@ void canTimerProcess(void *signal_id)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//static void processSdoOutput1(void *signal_id)
|
||||
//{
|
||||
// (void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output1, 8, 15);//
|
||||
//}
|
||||
//
|
||||
//static void processSdoOutput2(void *signal_id)
|
||||
//{
|
||||
// (void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output2, 8, 16);
|
||||
//}
|
||||
//
|
||||
//static void processSdoOutput3(void *signal_id)
|
||||
//{
|
||||
// (void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output3, 8, 17);
|
||||
//}
|
||||
//
|
||||
//static void processSdoOutput4(void *signal_id)
|
||||
//{
|
||||
// (void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output4, 8, 18);
|
||||
//}
|
||||
//
|
||||
//static void processSdoOutput5(void *signal_id)
|
||||
//{
|
||||
// (void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output5, 8, 19);
|
||||
//}
|
||||
//
|
||||
//
|
||||
//static void processSdoOutput7(void *signal_id)
|
||||
//{
|
||||
// (void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output7, 8, 20);
|
||||
//}
|
||||
|
||||
|
||||
|
||||
static void processMotorOutput1(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_1, LEFT_FRONT_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1, 8, 15);//<2F><><EFBFBD><EFBFBD>1Ť<31>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_1, LEFT_REAR_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3, 8, 16);//<2F><><EFBFBD><EFBFBD>1Ť<31>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_1, un_motor_output1.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1.tx_can_data.arr[0], 8, 15);//<2F><><EFBFBD><EFBFBD>1Ť<31>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_3, un_motor_output3.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3.tx_can_data.arr[0], 8, 16);//<2F><><EFBFBD><EFBFBD>1Ť<31>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
static void processMotorOutput2(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_2, RIGHT_FRONT_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2, 8, 15);//<2F><><EFBFBD><EFBFBD>2Ť<32>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_2, RIGHT_REAR_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4, 8, 16);//<2F><><EFBFBD><EFBFBD>2Ť<32>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_2, un_motor_output2.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2.tx_can_data.arr[0], 8, 15);//<2F><><EFBFBD><EFBFBD>2Ť<32>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_4, un_motor_output4.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4.tx_can_data.arr[0], 8, 16);//<2F><><EFBFBD><EFBFBD>2Ť<32>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
static void processMotorOutput3(void *signal_id)
|
||||
|
||||
static void processSdoOutput1(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_6, un_sdo_output1.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output1.tx_can_data.arr[0], 8, 15);//<2F><><EFBFBD><EFBFBD>1Ť<31>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_6, un_sdo_output4.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output4.tx_can_data.arr[0], 8, 16);//<2F><><EFBFBD><EFBFBD>1Ť<31>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
|
||||
|
||||
static void processmotorOutput5(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_6, un_sdo_output5.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output5.tx_can_data.arr[0], 8, 19);//<2F><><EFBFBD><EFBFBD>2Ť<32>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
static void processSdoOutput2(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_6, un_sdo_output2.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output2.tx_can_data.arr[0], 8, 17);//<2F><><EFBFBD><EFBFBD>2Ť<32>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
static void processSdoOutput3(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
CAN_Send_Msg(&can_handle_1, LEFT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1.arr[8], 8, 17);//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_1, LEFT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3.arr[8], 8, 18);//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
switch(un_sdo_output3.tx_can_id.raw)
|
||||
{
|
||||
case RIGHT_MOTOR_CANID:
|
||||
CAN_Send_Msg(&can_handle_6, un_sdo_output3.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output3.tx_can_data.arr[0], 8, 18);//<2F><><EFBFBD><EFBFBD>2Ť<32>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
break;
|
||||
|
||||
case PITCH_MOTOR_CANID:
|
||||
CAN_Send_Msg(&can_handle_6, un_sdo_output3.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output3.tx_can_data.arr[0], 8, 18);//<2F><><EFBFBD><EFBFBD>2Ť<32>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
break;
|
||||
default:;
|
||||
}
|
||||
}
|
||||
|
||||
static void processMotorOutput4(void *signal_id)
|
||||
static void processSdoOutput5(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_2, RIGHT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2.arr[8], 8, 17);//<2F><><EFBFBD><EFBFBD>2<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_2, RIGHT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4.arr[8], 8, 18);//<2F><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_1, un_motor_output5.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output5.tx_can_data.arr[0], 8, 17);//<2F><><EFBFBD><EFBFBD>2Ť<EFBFBD>غ<EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
static void processSdoOutput6(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_2, un_motor_output6.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output6.tx_can_data.arr[0], 8, 17);//<2F><><EFBFBD><EFBFBD>2Ť<32>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
|
||||
static void processSdoOutput7(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_3, un_motor_output7.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output7.tx_can_data.arr[0], 8, 17);//<2F><><EFBFBD><EFBFBD>2Ť<32>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
static void processSdoOutput8(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_4, un_motor_output8.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output8.tx_can_data.arr[0], 8, 17);//<2F><><EFBFBD><EFBFBD>2Ť<32>غ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//static void processMotorOutput3(void *signal_id)
|
||||
//{
|
||||
// (void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
//
|
||||
//// CAN_Send_Msg(&can_handle_1, LEFT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1.arr[8], 8, 17);//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
//// CAN_Send_Msg(&can_handle_1, LEFT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3.arr[8], 8, 18);//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
//
|
||||
//}
|
||||
//
|
||||
//static void processMotorOutput4(void *signal_id)
|
||||
//{
|
||||
// (void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
//// CAN_Send_Msg(&can_handle_2, RIGHT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2.arr[8], 8, 17);//<2F><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
//// CAN_Send_Msg(&can_handle_2, RIGHT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4.arr[8], 8, 18);//<2F><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
//}
|
||||
|
||||
static void processKgfOutput1(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
if(1 == ecu_online)//20250318 <20><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>߲<EFBFBD><DFB2>ܷ<EFBFBD><DCB7>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>E3ͬʱ<CDAC><CAB1><EFBFBD><EFBFBD>
|
||||
{
|
||||
// if(1 == ecu_online)//20250318 <20><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>߲<EFBFBD><DFB2>ܷ<EFBFBD><DCB7>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>E3ͬʱ<CDAC><CAB1><EFBFBD><EFBFBD>
|
||||
// {
|
||||
un_inf_can_kgf_output1.bit_data.can_rx5 = 0xE3;
|
||||
CAN_Send_Msg(&can_handle_0, 0x11000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_inf_can_kgf_output1, 8, 15);//KGF1
|
||||
}
|
||||
// }
|
||||
}
|
||||
|
||||
static void processKgfOutput2(void *signal_id)
|
||||
@@ -1133,16 +1136,6 @@ static void processKgfOutput2(void *signal_id)
|
||||
|
||||
}
|
||||
|
||||
//static void processKgfOutput2(void *signal_id)
|
||||
//{
|
||||
// (void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// if(1 == ecu_online)//20250318 <20><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>߲<EFBFBD><DFB2>ܷ<EFBFBD><DCB7>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>E3ͬʱ<CDAC><CAB1><EFBFBD><EFBFBD>
|
||||
// {
|
||||
// un_inf_can_kgf_output2.bit_data.can_rx5 = 0xE3;
|
||||
// CAN_Send_Msg(&can_handle_0, 0x14000002, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_inf_can_kgf_output2, 8, 16);//KGF2
|
||||
// }
|
||||
//
|
||||
//}
|
||||
|
||||
|
||||
|
||||
@@ -1161,7 +1154,7 @@ static void processWheelSpeedOutput(void *signal_id)
|
||||
un_wheel_wpeed_output.bit_data.right_front = (uint16_t)((int16_t)(CANPressSpeedTemp));//ת<><D7AA>*1.8*60*100/1000/ ת/<2F><><EFBFBD>ӡ<EFBFBD><D3A1><EFBFBD><EFBFBD><EFBFBD>km/h һȦ1.8<EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>100<30><30><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>Ϊ0.01<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD> 20240629 <20><><EFBFBD>ٱȲ<D9B1><C8B2><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD>
|
||||
un_wheel_wpeed_output.bit_data.right_rear = (uint16_t)((int16_t)(CANPressSpeedTemp));//<2F><>ת<EFBFBD><D7AA>Ϊint<6E><74><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><EFBFBD><DEB7><EFBFBD>
|
||||
|
||||
CAN_Send_Msg(&can_handle_4, 0x98, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_wheel_wpeed_output, 8, 15);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_7, 0x98, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_wheel_wpeed_output, 8, 15);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
static void processHBridgeOutput(void *signal_id)
|
||||
@@ -1205,11 +1198,11 @@ static void processUltrasonicOutput(void *signal_id)
|
||||
void canSendAll(void *signal_id)
|
||||
{
|
||||
static uint16_t wheel_speed_cnt = 0;
|
||||
// static uint8_t kgf_cnt = 0;
|
||||
static uint16_t kgf_cnt = 0;
|
||||
static uint16_t bms_cnt1 = 0;
|
||||
static uint16_t bms_cnt2 = 0;
|
||||
// static uint16_t bms_cnt2 = 0;
|
||||
// static uint16_t motor_speed_cnt = 0;
|
||||
static uint16_t motor_power_cnt = 0;
|
||||
// static uint16_t motor_power_cnt = 0;
|
||||
// static uint8_t h_bridge_cnt = 0;
|
||||
uint8_t CanData[8] = {0,0,0,0,0,0,0,0};
|
||||
//-------------------------------------------------------------------------
|
||||
@@ -1222,27 +1215,18 @@ void canSendAll(void *signal_id)
|
||||
|
||||
CanData[0] = 0x5A;
|
||||
|
||||
CAN_Send_Msg(&can_handle_3, 0x100, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 15);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
bms_cnt2 ++;
|
||||
if(bms_cnt2 >= 1100)//1s
|
||||
{
|
||||
bms_cnt2 = 0;
|
||||
|
||||
CanData[0] = 0x5A;
|
||||
|
||||
CAN_Send_Msg(&can_handle_3, 0x101, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_5, BMS_REQUEST_ID, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 1, 15);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
|
||||
motor_power_cnt ++;
|
||||
if(motor_power_cnt >= 1000)//1s<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
|
||||
{
|
||||
motor_power_cnt = 0;
|
||||
processMotorOutput3(CanData);
|
||||
processMotorOutput4(CanData);
|
||||
}
|
||||
|
||||
// motor_power_cnt ++;
|
||||
// if(motor_power_cnt >= 1000)//1s<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
|
||||
// {
|
||||
// motor_power_cnt = 0;
|
||||
// processMotorOutput3(CanData);
|
||||
// processMotorOutput4(CanData);
|
||||
// }
|
||||
|
||||
// motor_power_cnt ++;
|
||||
// if(motor_power_cnt >= 10)//<2F><>ȡת<C8A1><D7AA> 20ms<6D><73><EFBFBD><EFBFBD>
|
||||
@@ -1258,14 +1242,14 @@ void canSendAll(void *signal_id)
|
||||
// processMotorOutput4(CanData);
|
||||
// }
|
||||
|
||||
// kgf_cnt ++;
|
||||
// if(kgf_cnt >= 5)//20210312 <20><><EFBFBD><EFBFBD>CANͨѶģ<D1B6><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 20210710<31><EFBFBD>Ϊ50ms<6D><73><EFBFBD><EFBFBD>
|
||||
// {
|
||||
// kgf_cnt = 0;
|
||||
//
|
||||
// processKgfOutput1(CanData);
|
||||
kgf_cnt ++;
|
||||
if(kgf_cnt >= 1000)//20210312 <20><><EFBFBD><EFBFBD>CANͨѶģ<D1B6><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 20210710<31><EFBFBD>Ϊ50ms<6D><73><EFBFBD><EFBFBD>
|
||||
{
|
||||
kgf_cnt = 0;
|
||||
|
||||
processKgfOutput1(CanData);
|
||||
// processKgfOutput2(CanData);
|
||||
// }
|
||||
}
|
||||
//
|
||||
//<2F><><EFBFBD>ٷ<EFBFBD><D9B7><EFBFBD>
|
||||
wheel_speed_cnt ++;
|
||||
@@ -1275,14 +1259,14 @@ void canSendAll(void *signal_id)
|
||||
|
||||
processWheelSpeedOutput(CanData);
|
||||
|
||||
CAN_Send_Msg(&can_handle_0, 0x120, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_1, 0x121, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_2, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_3, 0x123, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_4, 0x124, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_5, 0x125, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_6, 0x126, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, &un_can_debug_output.arr[0], 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_7, 0x127, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_0, 0x120, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_1, 0x121, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_2, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_3, 0x123, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_4, 0x124, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_5, 0x125, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_6, 0x126, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, &un_can_debug_output.arr[0], 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_7, 0x127, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
//
|
||||
|
||||
@@ -1394,6 +1378,19 @@ void canInterfaceInit(void)
|
||||
subscribe(&un_motor_output1, processMotorOutput1);
|
||||
subscribe(&un_motor_output2, processMotorOutput2);
|
||||
|
||||
subscribe(&un_sdo_output1, processSdoOutput1);
|
||||
subscribe(&un_sdo_output2, processSdoOutput2);
|
||||
subscribe(&un_sdo_output3, processSdoOutput3);
|
||||
subscribe(&un_sdo_output5, processmotorOutput5);
|
||||
|
||||
subscribe(&un_motor_output5, processSdoOutput5);
|
||||
subscribe(&un_motor_output6, processSdoOutput6);
|
||||
subscribe(&un_motor_output7, processSdoOutput7);
|
||||
subscribe(&un_motor_output8, processSdoOutput8);
|
||||
|
||||
|
||||
|
||||
|
||||
subscribe(&un_inf_can_kgf_output1, processKgfOutput1);
|
||||
subscribe(&un_inf_can_kgf_output2, processKgfOutput2);
|
||||
subscribe(&un_wheel_wpeed_output, processWheelSpeedOutput);
|
||||
|
||||
@@ -66,9 +66,11 @@
|
||||
|
||||
|
||||
|
||||
#define BMS_INPUT_ID1 0x100
|
||||
#define BMS_INPUT_ID2 0x101
|
||||
#define BMS_INPUT_ID1 0x4028001
|
||||
#define BMS_REQUEST_ID 0x400FF80
|
||||
|
||||
#define REMOTE_ID 0x12000023
|
||||
#define REMOTE_ID_1 0x12000024
|
||||
#define TEMP_MODULE_INPUT_ID_1 0x15000003
|
||||
|
||||
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
#include "sdrv_eth.h"
|
||||
#include "app/app_param_manage.h"
|
||||
#include "app/app_differential_drive.h"
|
||||
|
||||
#include "app/app_turntable.h"
|
||||
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
#define MAX_BUFFER_SIZE (sizeof(((RequestContext *)0)->param_request->arr))
|
||||
@@ -273,6 +273,24 @@ void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_ad
|
||||
// printf("Autorecive len:%d\n",len);
|
||||
// udp_sendto(upcb, p, addr, port);
|
||||
}
|
||||
else if(ENCODER_HEADER == buf[0])//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
{
|
||||
if( (p->len) >= sizeof(un_encoder_data_input) )//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD>ȳ<EFBFBD><C8B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1>С<EFBFBD><D0A1><EFBFBD><EFBFBD>
|
||||
{
|
||||
len = sizeof(un_encoder_data_input);
|
||||
}
|
||||
else
|
||||
{
|
||||
len = p->len;
|
||||
}
|
||||
|
||||
for(i = 0; i < len; i++)
|
||||
{
|
||||
un_encoder_data_input.arr[i] = buf[i];
|
||||
}
|
||||
publishMessage(&un_encoder_data_input, 1);
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
}
|
||||
@@ -435,10 +453,12 @@ static void autoControlOutput(void *signal_id)
|
||||
UdpSendToData(UDPCB_2,(uint8_t *)(&un_auto_control_output), sizeof(un_auto_control_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port);
|
||||
}
|
||||
|
||||
static void magneticEncoderOutput(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
|
||||
|
||||
|
||||
UdpSendToData(UDPCB_2,(uint8_t *)(&str_magnetic_encoder), 1, (uint8_t *)&str_magnetic_encoder.ip[0], str_magnetic_encoder.port);
|
||||
}
|
||||
|
||||
//<2F><>̫<EFBFBD><CCAB> <20><><EFBFBD><EFBFBD>
|
||||
void ethernetSendAll(void *signal_id)
|
||||
@@ -647,6 +667,9 @@ void ethernetInterfaceInit(void)
|
||||
subscribe(&un_manual_control_output, manualControlOutput);
|
||||
subscribe(&un_auto_control_output, autoControlOutput);
|
||||
|
||||
subscribe(&str_magnetic_encoder, magneticEncoderOutput);
|
||||
|
||||
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2>ʹ<EFBFBD>þ<EFBFBD>̬<EFBFBD><CCAC>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>ص<EFBFBD>
|
||||
subscribe(&request_send, OnParamSend);
|
||||
|
||||
|
||||
9
main.c
9
main.c
@@ -31,6 +31,8 @@
|
||||
#include <app/app_light.h>
|
||||
#include "app/app_request.h"
|
||||
#include "app/app_ultrasonic.h"
|
||||
#include "app/app_turntable.h"
|
||||
|
||||
|
||||
void testAppInit(void);
|
||||
|
||||
@@ -113,7 +115,6 @@ int main(void)
|
||||
|
||||
printf("CAN_Send_Msg can_handle_6 OK %d\n",getCurrentTime());
|
||||
|
||||
|
||||
//打印版本号
|
||||
printf("version: V1.75 \n");
|
||||
// 初始化框架 放在最前面，解决电机can发送信号累积不处理的问题。
|
||||
@@ -121,16 +122,16 @@ int main(void)
|
||||
testAppInit();
|
||||
paramAppInit();
|
||||
diffAppInit();
|
||||
brakeAppInit();
|
||||
// brakeAppInit();
|
||||
powerAppInit(); //电源管理
|
||||
tempAppInit(); //温度
|
||||
// tempAppInit(); //温度
|
||||
lightAppInit(); //灯光
|
||||
ethernetInterfaceInit(); //以太网先初始化
|
||||
requestAppInit();
|
||||
canInterfaceInit();
|
||||
bootInterfaceInit();
|
||||
// ultrasonicAppInit();
|
||||
|
||||
turnableInit();
|
||||
printf("All init OK ------ %d\n",getCurrentTime());
|
||||
|
||||
for (;;)
|
||||
|
||||
Reference in New Issue
Block a user