liumin/test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

第一次提交

Browse Source
This commit is contained in:
liumin
2025-11-14 19:29:44 +08:00
parent 846bd3bbda
commit bb55e6618f
20 changed files with 2666 additions and 1466 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_brake.c
View File

@@ -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); // 周期调用
}

538
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c
View File

@@ -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,48 @@ 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;
static uint32_t previous_time2 = 0;
uint32_t time1 = getCurrentTime();
uint32_t dt_ticks;
// 档位
abs_left_front_speed = calculateTorqueOutput(diff_data.motor_state[0], out_torq[0]); //根据挡位增加转矩方向
abs_right_front_speed = calculateTorqueOutput(diff_data.motor_state[1], out_torq[1]);
abs_left_rear_speed = calculateTorqueOutput(diff_data.motor_state[2], out_torq[2]);
abs_right_rear_speed = calculateTorqueOutput(diff_data.motor_state[3], out_torq[3]);
un_motor_output1.bit_data.gear = diff_data.motor_state[0];
un_motor_output2.bit_data.gear = diff_data.motor_state[1];
if(STATE_FORWARD == diff_data.motor_state[2])//把后两台电机反相
if(time1 < previous_time2)//如果当前时间小于上一次保存的时间，那表示溢出了。
{
un_motor_output3.bit_data.gear = STATE_BACKWARD;
dt_ticks = (0xFFFFFFFF - previous_time2) + time1 + 1;
}
else if(STATE_BACKWARD == diff_data.motor_state[2])
else
{
un_motor_output3.bit_data.gear = STATE_FORWARD;
dt_ticks = (time1 - previous_time2) ;
}
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 dt = (float)dt_ticks / 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 );
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);
//设定转速
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;
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 +182,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 +219,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的情况
@@ -289,16 +231,16 @@ float calculateMaxSpeed()
// 将最大电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比
float max_speed = (max_rpm * wheel_circumference) / (60.0f * gear_ratio);
return max_speed;
}
}
// 计算最大加速度
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 +317,6 @@ void distributeTorque(float rpm1, float rpm2, float total_torque, float* torque1
if ( fabs(*torque2) < min_torque) {
*torque2 = copysignf(min_torque, *torque2);
}
}
@@ -473,10 +414,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 +427,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
@@ -580,11 +521,32 @@ float mapRemoteControlSpeed(
static void diffProcess(void *signal_id)
{
(void)signal_id;
static float previous_time1 = 0.0f;
float time1 = (float)getCurrentTime();
float dt = (time1 - previous_time1) / PERIOD_TICK;
previous_time1 = time1;
// static float previous_time1 = 0.0f;
//
// float time1 = (float)getCurrentTime();
// float dt = (time1 - previous_time1) / PERIOD_TICK;
// previous_time1 = time1;
static uint32_t previous_time1 = 0;
uint32_t time1 = getCurrentTime();
uint32_t dt_ticks;
if(time1 < previous_time1)//如果当前时间小于上一次保存的时间，那表示溢出了。
{
dt_ticks = (0xFFFFFFFF - previous_time1) + time1 + 1;
}
else
{
dt_ticks = (time1 - previous_time1) ;
}
float dt = (float)dt_ticks / PERIOD_TICK; // 最后才转换为浮点
previous_time1 = time1;
// 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度
calculateCurrentState(dt);
@@ -621,7 +583,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 +633,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);
@@ -682,25 +643,29 @@ static void diffProcess(void *signal_id)
out_torque[1] = constrain(out_torque[1], -diff_data.max_Torq, diff_data.max_Torq);
out_torque[2] = constrain(out_torque[2], -diff_data.max_Torq, diff_data.max_Torq);
out_torque[3] = constrain(out_torque[3], -diff_data.max_Torq, diff_data.max_Torq);
// printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral);
// 设置电机输出
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);
}
publishMessage(&un_motor_output2, 1);
// }
// else
// {
// motorDisable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1);
// motorDisable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2);
// motorDisable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3);
// motorDisable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4);
//
// publishMessage(&un_motor_output1, 1);
// publishMessage(&un_motor_output2, 1);
// }
un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(diff_data.speed*10);
@@ -765,7 +730,7 @@ static void diffInput(void *signal_id)
diff_data.desired_speed = diff_data.desired_speed * 0.01f;
diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f;
// 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
// diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 1, 20, 5, 5, 0.5);
diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 1, 20, 5, 5, 0.5);
diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0.1, 2, 2, 1, 0.5);
diff_data.desired_curvature = -diff_data.desired_curvature;
@@ -802,40 +767,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) )// 处理第一个电机速度信号（左电机）
{
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;
}
else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右电机）
{
diff_data.right_front_motor_speed = (float)((int16_t)(un_motor_input2.bit_data.speed - 30000)); // 20250502 1号控制器增加反相
diff_data.right_rear_motor_speed = (float)((int16_t)(un_motor_input4.bit_data.speed - 30000));
diff_data.right_rear_motor_speed = - diff_data.right_rear_motor_speed;//20250708 增加反相
if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速
{
motor_speed_temp = diff_data.right_rear_motor_speed;
}
else
{
motor_speed_temp = diff_data.right_front_motor_speed;
}
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转换为转/分钟
diff_data.right_motor_speed = motor_speed_temp;
if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速
{
motor_speed_temp = diff_data.left_front_motor_speed;
}
else
{
motor_speed_temp = diff_data.left_rear_motor_speed;
}
diff_data.left_motor_speed = motor_speed_temp;//左侧轮速 //motor_speed_temp
if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速
{
motor_speed_temp = diff_data.right_rear_motor_speed;
}
else
{
motor_speed_temp = diff_data.right_front_motor_speed;
}
diff_data.right_motor_speed = motor_speed_temp;//右侧轮速
// 急停开关
diff_data.emergency_stop_state = (uint8_t)(diff_data.emergency_stop_switch == app_close() || diff_data.remote_emergency_stop == app_close());
@@ -853,6 +811,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 +907,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 +932,84 @@ static void diffInput(void *signal_id)
void preChargeFinish(void *signal_id)
{
(void)signal_id;
float out_torq[4] = {0.0f,0.0f,0.0f,0.0f};
setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr"));
// 档位
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;
publishMessage(&un_motor_output1, 1);
publishMessage(&un_motor_output2, 1);
publishMessage(&un_motor_output3, 1);
publishMessage(&un_motor_output4, 1);
// setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr"));
//-------------------------------------------------------------------------------------------------------------------------------
switch(diff_data.motor_init_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据
{
case 0:
// timerStart(&diff_app_timer3, 1000, 0); // 启动定时器，1s
diff_data.motor_init_state = 1;
break;
case 1:
if(diff_data.diff_cnt >= 10)//发送5次
{
diff_data.diff_cnt = 0;
diff_data.motor_init_state = 2;
}
else
{
diff_data.diff_cnt ++;
diff_data.motor_init_state = 1;
}
break;
case 2://模式设置
if(diff_data.diff_cnt >= 5)//发送5次
{
diff_data.diff_cnt = 0;
diff_data.motor_init_state = 3;
}
else
{
diff_data.diff_cnt ++;
diff_data.motor_init_state = 2;
setMotorMode(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1, CURRENT_MODE);
setMotorMode(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2, CURRENT_MODE);
setMotorMode(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3, CURRENT_MODE);
setMotorMode(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4, CURRENT_MODE);
publishMessage(&un_motor_output1, 1);
publishMessage(&un_motor_output2, 1);
// publishMessage(&un_motor_output3, 1);
// publishMessage(&un_motor_output4, 1);
}
break;
//------------------------------------------------------------------------------
case 3:
if(diff_data.diff_cnt >= 5)//发送5次
{
diff_data.diff_cnt = 0;
diff_data.motor_init_state = 4;
}
else
{
diff_data.diff_cnt ++;
diff_data.motor_init_state = 3;
motorEnable(MASTER_CANID, FRONT_LEFT_MOTOR_CANID, &un_motor_output1);
motorEnable(MASTER_CANID, FRONT_RIGHT_MOTOR_CANID, &un_motor_output2);
motorEnable(MASTER_CANID, REAR_LEFT_MOTOR_CANID, &un_motor_output3);
motorEnable(MASTER_CANID, REAR_RIGHT_MOTOR_CANID, &un_motor_output4);
publishMessage(&un_motor_output1, 1);
publishMessage(&un_motor_output2, 1);
// publishMessage(&un_motor_output3, 1);
// publishMessage(&un_motor_output4, 1);
}
break;
case 4:
diff_data.motor_init_state = 4;
publishMessage(&un_motor_output1, 1);//一直使能防止 ，防止can报故障
publishMessage(&un_motor_output2, 1);
break;
default:break;
}
}
@@ -915,7 +1024,7 @@ void diffParametersInit(void *signal_id)
getParam("Ospd_ki"),
getParam("Ospd_kd"),
getParam("Ospd_il"),
getParam("Ospd_ol")
getParam("Ospd_ol")
);
setPidParameters(&yaw_rate_pid,
@@ -963,9 +1072,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;
@@ -974,9 +1118,7 @@ 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,25 +1134,30 @@ 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);
subscribe(&un_remote_control_input, diffInput);
@@ -1063,6 +1210,9 @@ 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");
}

40
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.h
View File

@@ -19,13 +19,35 @@ 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; // 最小扭矩限制
@@ -79,7 +108,14 @@ typedef struct DiffData
float left_diff_touue; // 左侧扭矩差
float right_diff_touue; // 右侧扭矩差
float diff_dead_zone; // 差速速度死区
float diff_dead_zone; // 差速速度死区
float vehicle_mass; // 车重
float wheel_radius; // 轮胎直径
float gear_ratio; // 减速比
float wheel_base; // 轴距
float max_motor_rpm; // 电机最大速度
} DiffData;

2
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_frm_signal.h
View File

@@ -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 // 优先级层次

31
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_light.c
View File

@@ -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;
}
}

8
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_param_manage.h
View File

@@ -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)
// 定义一个包含所有参数名称的结构体

398
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_power.c
View File

@@ -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,111 +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_output1.bit_data.KGF13 = setPowerOn(); // 计算机
un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOn(); // 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_output1.bit_data.KGF13 = setPowerOn(); // 计算机
un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOn(); // 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_output1.bit_data.KGF13 = setPowerOn(); // 计算机
un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOn(); // 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_output1.bit_data.KGF13 = setPowerOn(); // 计算机
un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOn(); // 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_output1.bit_data.KGF13 = setPowerOn(); // 计算机
un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOn(); // 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_output1.bit_data.KGF13 = setPowerOn(); // 计算机
un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
un_inf_can_kgf_output1.bit_data.KGF01 = setPowerOn(); // 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:
@@ -225,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)
@@ -258,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)
@@ -286,7 +286,7 @@ static void powerTimerProcess(void *signal_id)
printf("Power: Transitioning from POWER_NEUTRAL to WORKING state\n");
}
else
{
{
power_data.neutral_cnt ++;
power_data.current_state = POWER_NEUTRAL; // 空挡
power_data.pre_charge_finish = 1; // 预充完成
@@ -294,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");
@@ -322,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); // 启动预充定时器
@@ -374,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) )// 遥控器断线，不更新数据
{
@@ -420,7 +425,22 @@ void powerAppInit(void)
power_data.old_high_voltage_switch = power_data.high_voltage_switch;
//恢复急停开关状态
power_data.emergency_stop = (uint8_t)getParam("stop_sw");
power_data.old_emergency_stop = power_data.emergency_stop;
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); // 急停开关、高压开关

403
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.c Normal file
View 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
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.h Normal file
View 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