404 lines
14 KiB
C
404 lines
14 KiB
C
#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());
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|