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

修改WDT以及增加转台

Browse Source
This commit is contained in:
liumin
2025-10-21 19:13:44 +08:00
parent 8242f618d1
commit 8cc4755d15
13 changed files with 988 additions and 329 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
app/app_differential_drive.c
View File

@@ -61,23 +61,6 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint
}
// 限制值在最小值和最大值之间
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)
{
@@ -533,17 +516,7 @@ static void diffProcess(void *signal_id)
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);
un_can_debug_output.bit_data.desired_speed = (uint8_t)(int8_t)(diff_data.desired_speed*10);
un_can_debug_output.bit_data.curvature = (uint8_t)(int8_t)(diff_data.yaw_rate*10);
un_can_debug_output.bit_data.desired_curvature = (uint8_t)(int8_t)(diff_data.desired_yaw_rate*10);
un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(diff_data.left_motor_speed);
un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_motor_speed);
publishMessage(&diff_data, 1);
}
@@ -579,9 +552,9 @@ int16_t Filter(int16_t *s,uint8_t Len)
static void diffInput(void *signal_id)
{
static float left_speed_fiter[SPEED_FITER_NUM] = {0};
// static float left_speed_fiter[SPEED_FITER_NUM] = {0};
// static uint8_t left_speed_cnt = 0;
static float right_speed_fiter[SPEED_FITER_NUM] = {0};
// static float right_speed_fiter[SPEED_FITER_NUM] = {0};
// static uint8_t right_speed_cnt = 0;
float motor_speed_temp = 0.0f;
@@ -599,7 +572,7 @@ static void diffInput(void *signal_id)
diff_data.desired_speed = (float)((int16_t)(un_remote_control_input.bit_data.speed));
diff_data.desired_curvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature));
// 单位转换
diff_data.desired_speed = diff_data.desired_speed * 0.01f;
diff_data.desired_speed = -diff_data.desired_speed * 0.01f;
diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f;
// 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5);
@@ -655,7 +628,7 @@ static void diffInput(void *signal_id)
// diff_data.right_motor_speed = LOWPASS_FILTER(motor_speed_temp,right_speed_fiter[0]);
diff_data.right_motor_speed = motor_speed_temp;
right_speed_fiter[0] = diff_data.right_motor_speed;
// right_speed_fiter[0] = diff_data.right_motor_speed;
// if(SPEED_FITER_NUM == right_speed_cnt)//取样4次后滤波
@@ -683,7 +656,7 @@ static void diffInput(void *signal_id)
diff_data.left_motor_speed = motor_speed_temp;
// diff_data.left_motor_speed = LOWPASS_FILTER(motor_speed_temp,left_speed_fiter[0]);//低通滤波器
left_speed_fiter[0] = diff_data.left_motor_speed;
// left_speed_fiter[0] = diff_data.left_motor_speed;
// left_speed_fiter[left_speed_cnt] = motor_speed_temp;
@@ -789,12 +762,12 @@ void diffParametersInit(void *signal_id)
diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩
printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
// printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
// printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
// printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
printf("left_motor_speed = %f\n",diff_data.left_motor_speed);
printf("right_motor_speed = %f\n",diff_data.right_motor_speed);
// printf("left_motor_speed = %f\n",diff_data.left_motor_speed);
// printf("right_motor_speed = %f\n",diff_data.right_motor_speed);
// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);
// printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", diff_data.out_torq[0], diff_data.out_torq[1], diff_data.out_torq[2], diff_data.out_torq[3]);
@@ -805,7 +778,7 @@ void diffParametersInit(void *signal_id)
deffspeed = deffspeed * 0.01f;
deffcurvature = deffcurvature * 0.0001f;
printf("remote speed = %f, remote curvature = %f\n", deffspeed, deffcurvature);
// printf("remote speed = %f, remote curvature = %f\n", deffspeed, deffcurvature);
timerStart(&diff_app_timer,1000,1);//1s调用一次
}

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

2
app/app_param_manage.h
View File

@@ -52,6 +52,8 @@ extern "C"
X(Ocrv_il) \
X(Ocrv_ol) \
X(minTorq) \
X(minYpos) \
X(maxYpos) \
X(test)
// 定义一个包含所有参数名称的结构体

20
app/app_power.c
View File

@@ -43,7 +43,7 @@ static void handlePowerButton(void)
if (power_data.remote_power_switch == app_close())
{
power_button.state = BUTTON_STATE_SHORT_PRESS_DETECTED;
timerStart(&power_button.timer, 500, 0); // 启动短按定时器，500ms
timerStart(&power_button.timer, 500, 1); // 启动短按定时器，500ms
}
break;
@@ -53,7 +53,7 @@ static void handlePowerButton(void)
if (power_button.timer.active) // 定时器未到期,短按完成，启动等待长按定时器
{
power_button.state = BUTTON_STATE_WAIT_FOR_LONG_PRESS;
timerStart(&power_button.timer, 500, 0); // 启动等待长按定时器，500ms
timerStart(&power_button.timer, 500, 1); // 启动等待长按定时器，500ms
}
}
else if (!power_button.timer.active)// 短按定时器到期，按键仍被按下，视为无效，重置为初始状态
@@ -66,7 +66,7 @@ static void handlePowerButton(void)
if (power_data.remote_power_switch == app_close())// 检测是否在等待时间内进行长按
{
power_button.state = BUTTON_STATE_LONG_PRESS;
timerStart(&power_button.timer, 1000, 0); // 启动长按定时器，1000ms
timerStart(&power_button.timer, 1000, 1); // 启动长按定时器，1000ms
}
else if (!power_button.timer.active) // 等待长按超时，重置为初始状态
{
@@ -113,7 +113,9 @@ static void powerOutput(void *signal_id)
{
case POWER_PRE_CHARGE:
publishMessage(&power_data.pre_charge_finish, 1);//发布预充完成信号，100ms发送一次，直到预充完成
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOn(); // 预充继电器
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOn(); // 预充继电器
un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOn(); // 上装转台继电器
un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOn(); // 上装转台继电器
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器
@@ -133,6 +135,8 @@ static void powerOutput(void *signal_id)
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.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.KGF11 = setPowerOn(); // 低压继电器
@@ -152,6 +156,8 @@ static void powerOutput(void *signal_id)
case POWER_STANDBY:
// 初始状态,只开启基本设备
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOff(); // 上装转台继电器
un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOff(); // 上装转台继电器
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOff(); // 低压继电器
@@ -170,6 +176,8 @@ static void powerOutput(void *signal_id)
case POWER_WORKING:
// 工作状态,除预充继电器外所有设备开启
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOn(); // 上装转台继电器
un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOn(); // 上装转台继电器
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器
@@ -188,6 +196,8 @@ static void powerOutput(void *signal_id)
case POWER_EMERGENCY:
// 急停状态,断开高压
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOn(); // 上装转台继电器
un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOn(); // 上装转台继电器
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器
@@ -206,6 +216,8 @@ static void powerOutput(void *signal_id)
case POWER_SLEEP:
// 休眠状态,关闭所有设备
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
un_inf_can_kgf_output1.bit_data.KGF05 = setPowerOff(); // 上装转台继电器
un_inf_can_kgf_output1.bit_data.KGF06 = setPowerOff(); // 上装转台继电器
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOff(); // 低压继电器

705
app/app_turntable.c
View File

@@ -1,200 +1,513 @@
//#include "app_config.h"
//#include "app_dependence.h"
//#include "interface.h"
//
//#include "app_frm_monitor.h"
//#include "app_frm_signal.h"
//#include "app_frm_timer.h"
//
//#include "sdrv_vic.h"
//
//
//
///**
// * @brief 电机失能函数
// * @param motorIndex 电机索引 (0, 1, 2)
// * @return 0: 成功, -1: 失败
// */
//int motorDisable(uint8_t motorIndex)
//{
// // 检查电机索引有效性
// if (motorIndex > MAX_MOTOR_INDEX) {
// return -1;
// }
//
//
//
//
//
// // TODO: 添加电机失能的具体实现
// // 根据motorIndex执行对应电机的失能操作
//
// return 0;
//}
//
///**
// * @brief 电机使能函数
// * @param motorIndex 电机索引 (0, 1, 2)
// * @return 0: 成功, -1: 失败
// */
//int motorEnable(uint8_t motorIndex)
//{
// // 检查电机索引有效性
// if (motorIndex > MAX_MOTOR_INDEX) {
// return -1;
// }
//
// // TODO: 添加电机使能的具体实现
// // 根据motorIndex执行对应电机的使能操作
//
// return 0;
//}
//
///**
// * @brief 设置电机模式函数
// * @param motorIndex 电机索引 (0, 1, 2)
// * @param mode 电机模式参数
// * @return 0: 成功, -1: 失败
// */
//int setMotorMode(uint8_t motorIndex, uint8_t mode)
//{
// // 检查电机索引有效性
// if (motorIndex > MAX_MOTOR_INDEX) {
// return -1;
// }
//
// // TODO: 添加设置电机模式的具体实现
// // 根据motorIndex和mode参数设置对应电机的工作模式
//
// return 0;
//}
//
///**
// * @brief 设置电机速度函数
// * @param motorIndex 电机索引 (0, 1, 2)
// * @param speed 电机速度参数
// * @return 0: 成功, -1: 失败
// */
//int setMotorSpeed(uint8_t motorIndex, float speed)
//{
// // 检查电机索引有效性
// if (motorIndex > MAX_MOTOR_INDEX) {
// return -1;
// }
//
// // TODO: 添加设置电机速度的具体实现
// // 根据motorIndex和speed参数设置对应电机的速度
//
// return 0;
//}
//
///**
// * @brief 设置电机电流函数
// * @param motorIndex 电机索引 (0, 1, 2)
// * @param current 电机电流参数
// * @return 0: 成功, -1: 失败
// */
//int setMotorCurrent(uint8_t motorIndex, float current)
//{
// // 检查电机索引有效性
// if (motorIndex > MAX_MOTOR_INDEX) {
// return -1;
// }
//
// // TODO: 添加设置电机电流的具体实现
// // 根据motorIndex和current参数设置对应电机的电流
//
// return 0;
//}
//
///**
// * @brief 设置电机位置函数
// * @param motorIndex 电机索引 (0, 1, 2)
// * @param position 电机位置参数
// * @return 0: 成功, -1: 失败
// */
//int setMotorPosition(uint8_t motorIndex, float position)
//{
// // 检查电机索引有效性
// if (motorIndex > MAX_MOTOR_INDEX) {
// return -1;
// }
//
// switch(motorIndex)
#include "app_config.h"
#include "app_dependence.h"
#include "interface.h"
#include "app_turntable.h"
#include "app_pid.h"
#include "app_param_manage.h"
#include "app_frm_monitor.h"
#include "app_frm_signal.h"
#include "app_frm_timer.h"
#include "sdrv_vic.h"
PID_t turnable_speed_pid;
PID_t turnable_position_pid;
TurnableData turnable_data = {0};
/**
* @brief 带死区的原始数据到物理量转换函数（简单版）
* @param raw_value 原始16位无符号整数值 [0, 65535]
* @param min 物理量最小值（如 -10.0）
* @param max 物理量最大值（如 +10.0）
* @param deadzone 死区范围（物理量单位，如 1.0 表示 ±1.0 内为死区）
* @return 转换后的物理量值（若在死区内返回0，否则返回实际值）
*/
static float convertPhysical(uint16_t raw_value, float min, float max, float deadzone)
{
// 1. 计算实际物理量值
float physical_value = min + ((float)raw_value / 65535.0f) * (max - min);
// 2. 判断是否在死区内（绝对值 ≤ deadzone）
if (fabs(physical_value) <= deadzone)
{
return 0.0f; // 死区内返回0
}
else
{
return physical_value; // 死区外返回实际值
}
}
/**
* @brief 将浮点数转换为uint32_t（按小端序存储）
* @param num 输入的浮点数
* @return 转换后的uint32_t值（直接内存拷贝结果）
* @note 此函数通过内存直接拷贝实现转换，不进行数值计算，结果受平台字节序影响
*/
uint32_t floatToUint32(float num)
{
uint32_t result;
// 将浮点数的内存数据直接拷贝到uint32_t变量
memcpy(&result, &num, sizeof(num));
return result;
}
/**
* @brief 电机失能函数（停止电机运行）
* @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
* @param master_id 主控制器ID (用于标识发送方)
* @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
* @return 0: 成功, -1: 参数无效
* @note 此函数会修改unsdodata指向的结构体内容，调用后需及时发送CAN报文
*/
int8_t motorDisable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata)
{
/* 参数有效性检查 */
if (unsdodata == NULL) {
return -1;
}
/* 设置CAN报文ID域 */
unsdodata->tx_can_id.bits.mode = 3; /* 通信模式3：电机失能 */
unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */
unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */
unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */
/* 清零数据域 */
unsdodata->tx_can_data.bit_data.data = 0;
unsdodata->tx_can_data.bit_data.index = 0;
unsdodata->tx_can_data.bit_data.object_index = 0;
return 0;
}
/**
* @brief 电机使能函数（启动电机运行）
* @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
* @param master_id 主控制器ID (用于标识发送方)
* @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
* @return 0: 成功, -1: 参数无效
* @note 通信模式4：电机使能
*/
int8_t motorEnable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata)
{
/* 参数有效性检查 */
if (unsdodata == NULL) {
return -1;
}
/* 设置CAN报文ID域 */
unsdodata->tx_can_id.bits.mode = 3; /* 通信模式4：电机使能 */
unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */
unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */
unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */
/* 清零数据域 */
unsdodata->tx_can_data.bit_data.data = 0;
unsdodata->tx_can_data.bit_data.index = 0;
unsdodata->tx_can_data.bit_data.object_index = 0;
return 0;
}
/**
* @brief 设置电机运行模式
* @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
* @param master_id 主控制器ID (用于标识发送方)
* @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
* @param mode 要设置的模式值 (具体含义需参考电机协议文档)
* @return 0: 成功, -1: 参数无效
* @note RUM_MODE应为预定义的宏，表示运行模式索引
*/
int8_t setMotorMode(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint8_t mode)
{
/* 参数有效性检查 */
if (unsdodata == NULL) {
return -1;
}
/* 设置CAN报文ID域 */
unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */
unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */
unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */
unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */
/* 设置数据域 */
unsdodata->tx_can_data.bit_data.index = RUM_MODE; /* 运行模式索引 */
unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */
unsdodata->tx_can_data.bit_data.data = mode; /* 模式值 */
return 0;
}
/**
* @brief 写入电机参数
* @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
* @param master_id 主控制器ID (用于标识发送方)
* @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
* @param index 要写入的参数索引 (具体含义需参考电机协议文档)
* @param ref 要写入的参数值 (浮点数，会自动转换为uint32_t)
* @return 0: 成功, -1: 参数无效
* @note 使用floatToUint32函数转换浮点参数
*/
int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint16_t index, float ref)
{
/* 参数有效性检查 */
if (unsdodata == NULL) {
return -1;
}
/* 设置CAN报文ID域 */
unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */
unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */
unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */
unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */
/* 设置数据域 */
unsdodata->tx_can_data.bit_data.index = index; /* 参数索引 */
unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */
unsdodata->tx_can_data.bit_data.data = floatToUint32(ref); /* 转换并写入参数值 */
return 0;
}
/**
* @brief 动态斜率限制（支持变时间间隔）
* @param last_command 上一次的电流指令值
* @param target_current 本次目标电流指令
* @param delta_time 距离上一次调用的时间间隔 (s)
* @return 限制后的安全电流指令
*/
float dynamic_current_limit(float *last_command, float target_current, float delta_time)
{
// 计算期望的变化量
float desired_change = target_current - *last_command;
// 计算两种限制
float step_limit = MAX_STEP;
float time_limit = MAX_DI_DT * delta_time; // 动态计算时间限制
// 选择更严格的限制
float max_allowed_change = (step_limit < time_limit) ? step_limit : time_limit;
// 应用限制并返回新指令
float actual_change = constrain(desired_change, -max_allowed_change, max_allowed_change);
*last_command = *last_command + actual_change;//更新过去值
return *last_command + actual_change;
}
static void setTurnableMotorOutput()
{
static float previous_time2 = 0.0f;
float time1 = (float)getCurrentTime();
float dt = (time1 - previous_time2) / PERIOD_TICK;
previous_time2 = time1;
// turnable_data.out_pitch_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_pitch_motor_ampere_last,turnable_data.out_pitch_motor_ampere,dt);
turnable_data.out_right_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_right_motor_ampere_last,turnable_data.out_right_motor_ampere,dt);
turnable_data.out_left_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_left_motor_ampere_last ,turnable_data.out_left_motor_ampere ,dt);
setMotorWrite(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, LIMIT_SPEED_INDEX,turnable_data.out_pitch_motor_ampere);
setMotorWrite(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output4, LOC_REF_INDEX,turnable_data.desired_pitch_position);
setMotorWrite(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, IQ_REF_INDEX,turnable_data.out_right_motor_ampere_limit);
setMotorWrite(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3, IQ_REF_INDEX,turnable_data.out_left_motor_ampere_limit);
un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(turnable_data.speed*10);
un_can_debug_output.bit_data.desired_speed = (uint8_t)(int8_t)(turnable_data.desired_speed*10);
// un_can_debug_output.bit_data.curvature = (uint8_t)(int8_t)(diff_data.yaw_rate*10);
// un_can_debug_output.bit_data.desired_curvature = (uint8_t)(int8_t)(diff_data.desired_yaw_rate*10);
un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(turnable_data.out_left_motor_ampere_limit * 100);
un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(turnable_data.out_right_motor_ampere_limit*100);
publishMessage(&un_sdo_output1, 1);
publishMessage(&un_sdo_output2, 1);
publishMessage(&un_sdo_output3, 1);
publishMessage(&un_sdo_output4, 1);
}
// 转台
static void turnableProcess(void *signal_id)
{
// static float previous_time1 = 0.0f;
// float time1 = (float)getCurrentTime();
// float dt = (time1 - previous_time1) / PERIOD_TICK;
// previous_time1 = time1;
// if((turnable_data.current_state == POWER_WORKING))//高压上电才运行
// {
//
//
//
//
//
//
switch(turnable_data.turnable_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据
{
case 0:
timerStart(&turnable_data.turnable_timer, 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, CURRENT_MODE);
setMotorMode(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3, CURRENT_MODE);
publishMessage(&un_sdo_output1, 1);
publishMessage(&un_sdo_output2, 1);
publishMessage(&un_sdo_output3, 1);
}
break;
//------------------------------------------------------------------------------
case 3:
if(turnable_data.turnable_cnt >= 5)//发送5次
{
turnable_data.turnable_cnt = 0;
turnable_data.turnable_state = 4;
}
else
{
turnable_data.turnable_cnt ++;
turnable_data.turnable_state = 3;
motorEnable(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1);
motorEnable(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2);
motorEnable(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3);
publishMessage(&un_sdo_output1, 1);
publishMessage(&un_sdo_output2, 1);
publishMessage(&un_sdo_output3, 1);
}
break;
case 4:
turnable_data.turnable_cnt = 0;
turnable_data.turnable_state = 4;
setTurnableMotorOutput();//输出函数
break;
default:break;
}
// }
//
// un_sdo_output3.bit_data.index = 0x0;
// un_sdo_output3.bit_data.object_index = 0x0;
// un_sdo_output3.bit_data.data = 0x0;//
// publishMessage(&un_sdo_output3, 1);
//
//
//
// // TODO: 添加设置电机位置的具体实现
// // 根据motorIndex和position参数设置对应电机的位置
//
// return 0;
//}
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//void motorInit(void *signal_id)
//{
// if(turn_data.current_state == POWER_WORKING)
// else
// {
// un_sdo_output4.bit_data.index = RUM_MODE;
// un_sdo_output4.bit_data.object_index = 0x0;
// un_sdo_output4.bit_data.data = POSITION_MODE_CSP;//设定位置模式
// publishMessage(&un_sdo_output4, 1);
//
// un_sdo_output8.bit_data.index = RUM_MODE;
// un_sdo_output8.bit_data.object_index = 0x0;
// un_sdo_output8.bit_data.data = POSITION_MODE_CSP;//设定位置模式
// publishMessage(&un_sdo_output8, 1);
//
// un_sdo_output3.bit_data.index = 0x0;
// un_sdo_output3.bit_data.object_index = 0x0;
// un_sdo_output3.bit_data.data = 0x0;//
// publishMessage(&un_sdo_output3, 1);
//
// un_sdo_output7.bit_data.index = 0x0;
// un_sdo_output7.bit_data.object_index = 0x0;
// un_sdo_output7.bit_data.data = 0x0;//
// publishMessage(&un_sdo_output7, 1);
// turnable_data.turnable_cnt ++;
// turnable_data.turnable_state = 0;
// }
//// timerStart(&turn_timer2,1000,1);//1s调用一次
//}
//
//
//
//
//
//
//
//
//
//
//
//
//
}
void turnableParametersInit(void *signal_id)
{
(void)signal_id; // 标记变量为已使用，避免编译器警告
setPidParameters(&turnable_speed_pid,
getParam("spd_kp"),
getParam("spd_ki"),
getParam("spd_kd"),
getParam("spd_il"),
getParam("spd_ol")
);
// if(0 == un_right_intput.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
// {
// motorEnable(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output5);
// publishMessage(&un_sdo_output5, 1);
// }
//
// if(0 == un_turn_intput.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
// {
// motorEnable(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output5);
// publishMessage(&un_sdo_output5, 1);
// }
//
// if(0 == un_pitch_intput.rx_can_id.bits.mode_state)//判断状态是否为复位，如果复位就重新使能
// {
// motorEnable(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output5);
// publishMessage(&un_sdo_output5, 1);
// }
turnable_data.min_pitch_postion = getParam("minYpos"); //俯仰位置最小限制值
turnable_data.max_pitch_postion = getParam("maxYpos"); //俯仰位置最大限制值
printf( "turnable left 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);//1s调用一次
}
// 差速输入处理函数
static void turnableInput(void *signal_id)
{
if(signal_id == &power_data)//电机上电
{
turnable_data.current_state = power_data.current_state;
}
else if(signal_id == &un_computer_turnable_Input)
{
turnable_data.desired_speed = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_x) );
}
else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据
{
float x_axis_temp = (float)(un_remote_control_input.bit_data.x_axis) - REMOTE_ZERO;
if( ( x_axis_temp > 50 ) || ( x_axis_temp < -50 ) )
{
turnable_data.out_left_motor_ampere = 0.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{}
turnable_data.right_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_right_intput.rx_can_data.bit_data.current_velocity),ANGULAR_VELOCITY_MIN,ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE );
turnable_data.left_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_turn_intput.rx_can_data.bit_data.current_velocity) ,ANGULAR_VELOCITY_MIN,ANGULAR_VELOCITY_MAX, MOTOR_VELOCITY_DEADZONE );
turnable_data.speed = (turnable_data.right_motor_speed + turnable_data.left_motor_speed)/2.0f;
// 发布左右电机期望转速,电源在工作状态才能发送
if ( (power_data.current_state == POWER_STANDBY) || (power_data.current_state == POWER_SLEEP) )//这几种状态可以转转台
{
turnable_data.turnable_state = 0;//清空状态。保证每次上电都初始化
}
else
{
turnableProcess(signal_id);//处理映射
}
}
void turnableInit()
{
// 初始化速度 PID 控制器
initializePid(&turnable_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
// 设置速度 PID 控制器的参数
setPidParameters(&turnable_speed_pid,
getParam("spd_kp"),
getParam("spd_ki"),
getParam("spd_kd"),
getParam("spd_il"),
getParam("spd_ol")
);
subscribe(&un_computer_turnable_Input, turnableInput);
subscribe(&un_remote_control_input, turnableInput);
timerInit(&turnable_data.turnable_timer);
timerInit(&turnable_data.turnable_timer1);
subscribe(&turnable_data.turnable_timer1, turnableParametersInit);
timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次
turnable_data.turnable_state = 0;
un_right_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE;
un_right_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE;
un_right_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE;
un_turn_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE;
un_turn_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE;
un_turn_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE;
un_pitch_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE;
un_pitch_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE;
un_pitch_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE;
printf( "turnable: initial OK %d\n",getCurrentTime());
}

115
app/app_turntable.h
View File

@@ -1,21 +1,31 @@
#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 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>ģʽ
@@ -23,29 +33,86 @@ extern "C" {
#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
// canoeЭ<65><D0AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
typedef struct _StrSdoOutput
#define MOTOR_VELOCITY_DEADZONE 1.0f// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: 120 Nm
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD> (<28><>ӦByte4~5: <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>)
#define TORQUE_MIN -120.0f // <20><>С<EFBFBD><D0A1><EFBFBD><EFBFBD>: -120 Nm
#define TORQUE_MAX 120.0f // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: 120 Nm
#define ZERO_VAULE 0x0080 // 32768,<2C><>Ҫ<EFBFBD><D2AA>λ<EFBFBD><CEBB>ǰ
#define REMOTE_ZERO 1022
#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
{
//-----<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0x601----------------------------------------------
uint16_t index; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƼĴ<C6BC><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ
uint16_t object_index; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> Ϊ0x0000
uint32_t data; // <20><><EFBFBD><EFBFBD>
} StrRS04Output;
uint8_t turnable_state;
PowerState current_state; // <20><>ǰ<EFBFBD><C7B0>Դ״̬
typedef union _UnSdoOutput
{
StrSdoOutput bit_data; // ʹ<>ö<EFBFBD><C3B6><EFBFBD><EFBFBD>Ľṹ<C4BD><E1B9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
uint8_t arr[sizeof(StrSdoOutput)]; // ͨ<><CDA8><EFBFBD>ṹ<EFBFBD><E1B9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD>С
} UnRS04Output;
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><EFBFBD>z
float desired_speed; // <20><><EFBFBD><EFBFBD>ת̨<D7AA>ٶ<EFBFBD>
float desired_pitch_position; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
float desired_horizontal_position; // <20><><EFBFBD><EFBFBD>ˮƽλ<C6BD><CEBB>
float left_motor_speed; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
float right_motor_speed; // <20><>ǰ<EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>ٶ<EFBFBD>
float speed; // <20><>ǰת<C7B0><D7AA><EFBFBD>ٶ<EFBFBD>
float pitch_position; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
float horizontal_position; // <20><>ǰˮƽλ<C6BD><CEBB>
float max_speed; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
float out_left_motor_ampere; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float out_right_motor_ampere; // <20><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float out_pitch_motor_ampere; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float out_left_motor_ampere_last; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float out_right_motor_ampere_last; // <20><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float out_pitch_motor_ampere_last; // <20><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float out_left_motor_ampere_limit; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
float out_right_motor_ampere_limit; // <20><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
float out_pitch_motor_ampere_limit; // <20><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
Timer turnable_timer; // <20><>ʱ<EFBFBD><CAB1>
Timer turnable_timer1; // <20><>ʱ<EFBFBD><CAB1>
Timer turnable_timer2; // <20><>ʱ<EFBFBD><CAB1>
uint8_t turnable_cnt;
float min_pitch_postion; // λ<><CEBB><EFBFBD><EFBFBD>Ϣ
float max_pitch_postion; // λ<><CEBB><EFBFBD><EFBFBD>Ϣ
float max_ampere; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
} TurnableData;
void turnableInit();
#ifdef __cplusplus
}