461 lines
17 KiB
C
461 lines
17 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 "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());
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|