修改WDT以及增加转台
This commit is contained in:
@@ -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调用一次
|
||||
}
|
||||
|
||||
@@ -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 // 优先级层次
|
||||
|
||||
|
||||
@@ -52,6 +52,8 @@ extern "C"
|
||||
X(Ocrv_il) \
|
||||
X(Ocrv_ol) \
|
||||
X(minTorq) \
|
||||
X(minYpos) \
|
||||
X(maxYpos) \
|
||||
X(test)
|
||||
|
||||
// 定义一个包含所有参数名称的结构体
|
||||
|
||||
@@ -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(); // 低压继电器
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
}
|
||||
|
||||
40
interface.c
40
interface.c
@@ -41,13 +41,16 @@ UnUltrasonicOutput un_ultrasonic_output1 ;//超声波传感
|
||||
|
||||
|
||||
|
||||
//UnSdoOutput un_sdo_output1 ;//设定转向电机位置
|
||||
//UnSdoOutput un_sdo_output2 ;//设定转向电机速度
|
||||
//UnSdoOutput un_sdo_output3 ;//使能向下一个点
|
||||
//UnSdoOutput un_sdo_output4 ;//设定电机使能
|
||||
//UnSdoOutput un_sdo_output5 ;//设置电机模式
|
||||
//UnSdoOutput un_sdo_output6 ;//接收数据
|
||||
//UnSdoOutput un_sdo_output7 ;//发送使能数据
|
||||
StrTxCanFrame un_sdo_output1 ;//电机1输出
|
||||
StrTxCanFrame un_sdo_output2 ;//电机2输出
|
||||
StrTxCanFrame un_sdo_output3 ;//电机3输出
|
||||
StrTxCanFrame un_sdo_output4 ;//电机3速度输出
|
||||
|
||||
StrTxCanFrame un_sdo_output5 ;//电机使能输出
|
||||
|
||||
StrRxCanFrame un_pitch_intput ;//电机输入
|
||||
StrRxCanFrame un_right_intput ;//电机输入
|
||||
StrRxCanFrame un_turn_intput ;//电机输入
|
||||
|
||||
|
||||
//IO口
|
||||
@@ -59,7 +62,13 @@ UnAutoComputerInput un_auto_computer_input ;//自主计算机
|
||||
UnManualComputerInput un_manual_computer_input ;//自主计算机手动数据
|
||||
UnRequestFrame un_request_frame ;//请求帧
|
||||
|
||||
UnComputerTurnableInput un_computer_turnable_Input ;//转台以太网输入
|
||||
|
||||
|
||||
UnComputerOutput un_computer_output ;//输出给自主计算机
|
||||
|
||||
|
||||
|
||||
|
||||
//输出给上位机
|
||||
UnVehicleInfoOutput un_vehicle_Info_output ;// 车辆信息，输出给上位机
|
||||
@@ -77,5 +86,20 @@ UnCanDebugOutput un_can_debug_output;//调试输出
|
||||
|
||||
|
||||
|
||||
|
||||
// 限制值在最小值和最大值之间
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
218
interface.h
218
interface.h
@@ -195,7 +195,28 @@ typedef union _UnManualComputerInput
|
||||
} UnManualComputerInput;
|
||||
|
||||
|
||||
// 接收转台指令输入
|
||||
typedef struct _StrComputerTurnableInput
|
||||
{
|
||||
// 多字节数据，高位在前，低位在后
|
||||
unsigned int frame_header : 16; // 帧头 固定值0xFFCC
|
||||
unsigned int frame_type : 16; // 帧类型 固定值0x0001
|
||||
unsigned int frame_length : 8; // 帧长 固定值0x19
|
||||
unsigned int heartbeat : 8; // 心跳 按帧累加
|
||||
|
||||
// --- 坐标数据部分 ---
|
||||
int32_t position_x; // X轴坐标
|
||||
int32_t position_y; // Y轴坐标
|
||||
int32_t position_z; // Z轴坐标
|
||||
|
||||
unsigned int crc : 8; // CRC 按字节累加之和，溢出取低8位
|
||||
} StrComputerTurnableInput;
|
||||
|
||||
typedef union _UnComputerTurnableInput
|
||||
{
|
||||
StrComputerTurnableInput bit_data; // 使用定义的结构体变量名
|
||||
uint8_t arr[sizeof(StrComputerTurnableInput)]; // 通过结构体类型确定大小
|
||||
} UnComputerTurnableInput;
|
||||
|
||||
|
||||
|
||||
@@ -237,6 +258,11 @@ typedef struct _StrRemoteControlInput
|
||||
unsigned int reserve3 : 1; // 保留
|
||||
|
||||
unsigned int enable : 8; // 使能
|
||||
|
||||
uint16_t y_axis;
|
||||
uint16_t x_axis;
|
||||
uint16_t reserve4;
|
||||
uint16_t reserve5;
|
||||
} StrRemoteControlInput;
|
||||
|
||||
typedef union _UnRemoteControlInput
|
||||
@@ -359,26 +385,172 @@ typedef union _UnGatherOutput
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// canoe协议输出
|
||||
typedef struct _StrSdoOutput
|
||||
// CAN ID 解析（联合体形式，支持位域和32位直接访问）
|
||||
typedef union _UnCanIdInfo
|
||||
{
|
||||
//-----发送数据0x601----------------------------------------------
|
||||
unsigned int cmd : 8; // 命令
|
||||
unsigned int object_index : 16; // 索引
|
||||
unsigned int sub_index : 8; // 从索引
|
||||
unsigned int data : 32; // 数据
|
||||
} StrSdoOutput;
|
||||
uint32_t raw; // 32位整型，直接读写整个CAN ID
|
||||
struct
|
||||
{
|
||||
uint32_t motor_id : 8; // 0-7 bit (电机ID)
|
||||
uint32_t data : 16; // 8-23 bit (数据字段)
|
||||
uint32_t mode : 5; // 24-28 bit (模式)
|
||||
uint32_t res : 3; // 29-31 bit (保留位)
|
||||
} bits;
|
||||
|
||||
typedef union _UnSdoOutput
|
||||
} UnCanIdInfo;
|
||||
|
||||
|
||||
// 输出can数据
|
||||
typedef struct _StrTxCanOutput
|
||||
{
|
||||
StrSdoOutput bit_data; // 使用定义的结构体变量名
|
||||
uint8_t arr[sizeof(StrSdoOutput)]; // 通过结构体类型确定大小
|
||||
} UnSdoOutput;
|
||||
uint16_t index; // 索引（类似寄存器地址）
|
||||
uint16_t object_index; // 子索引（通常为0x0000）
|
||||
uint32_t data; // 数据字段
|
||||
} StrTxCanOutput;
|
||||
|
||||
|
||||
// CAN数据区联合体 (8字节，严格遵循图片协议，大端序数据)
|
||||
typedef union _UnTxCanData {
|
||||
StrTxCanOutput bit_data; // 结构化访问
|
||||
uint8_t arr[sizeof(StrTxCanOutput)]; // 字节数组形式（用于原始数据读写）
|
||||
} UnTxCanData;
|
||||
|
||||
|
||||
// 接收CAN帧结构体
|
||||
typedef struct _StrTxCanFrame
|
||||
{
|
||||
UnCanIdInfo tx_can_id; // 接收到的29位CAN ID
|
||||
UnTxCanData tx_can_data; // 接收到的8字节数据区
|
||||
} StrTxCanFrame;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//// CAN ID 解析（联合体形式，支持位域和32位直接访问）
|
||||
//typedef union _UnCanIdInfo
|
||||
//{
|
||||
// struct
|
||||
// {
|
||||
// uint32_t motor_id : 8; // 0-7 bit (电机ID)
|
||||
// uint32_t data : 16; // 8-23 bit (数据字段)
|
||||
// uint32_t mode : 5; // 24-28 bit (模式)
|
||||
// uint32_t res : 3; // 29-31 bit (保留位)
|
||||
// } bits;
|
||||
// uint32_t raw; // 32位整型，直接读写整个CAN ID
|
||||
//} UnCanIdInfo;
|
||||
//
|
||||
//// CANoe协议输出（主结构体）
|
||||
//typedef struct _StrSdoOutput
|
||||
//{
|
||||
// uint16_t index; // 索引（类似寄存器地址）
|
||||
// uint16_t object_index; // 子索引（通常为0x0000）
|
||||
// uint32_t data; // 数据字段
|
||||
// UnCanIdInfo rx_can_id; // CAN ID（联合体形式）
|
||||
//} StrSdoOutput;
|
||||
//
|
||||
//// 主联合体（支持结构体和字节数组访问）
|
||||
//typedef union _UnSdoOutput
|
||||
//{
|
||||
// StrSdoIntput bit_data; // 结构化访问
|
||||
// uint8_t arr[sizeof(StrSdoIntput)]; // 字节数组形式（用于原始数据读写）
|
||||
//} UnSdoIntput;
|
||||
|
||||
|
||||
|
||||
|
||||
// CAN ID 解析联合体 (29位扩展帧，严格遵循图片协议，小端序适配)
|
||||
typedef union _UnRxCanIdInfo {
|
||||
uint32_t raw; // 完整的32位值
|
||||
|
||||
struct {
|
||||
// 注意：小端序下，位域布局从低位到高位（Bit0到Bit31）
|
||||
// 编译器通常从低位开始分配位域
|
||||
|
||||
// 主机CAN_ID (Bit7~Bit0) - 8位 - 最低字节
|
||||
uint32_t host_id : 8; // Bit7~0: 主机CAN_ID
|
||||
|
||||
// 电机状态与故障信息域 (Bit23~Bit8) - 16位
|
||||
uint32_t motor_can_id : 8; // Bit15~8: 当前电机CAN ID
|
||||
uint32_t undervoltage : 1; // Bit16: 欠压故障 (0无1有)
|
||||
uint32_t overcurrent : 1; // Bit17: 过流 (0无1有)
|
||||
uint32_t overtemperature : 1; // Bit18: 过温 (0无1有)
|
||||
uint32_t mag_encoder_fault : 1; // Bit19: 磁编码故障 (0无1有)
|
||||
uint32_t hall_fault : 1; // Bit20: HALL编码故障 (0无1有)
|
||||
uint32_t uncalibrated : 1; // Bit21: 未标定 (0无1有)
|
||||
uint32_t mode_state : 2; // Bit23~22: 模式状态 (0:Reset,1:Cali,2:Motor)
|
||||
|
||||
// 协议标识 (Bit28~Bit24) - 5位
|
||||
uint32_t protocol_id : 5; // Bit28~24: 协议标识（图中为2）
|
||||
|
||||
// 保留位 (Bit31~29) - 3位（图片中未使用）
|
||||
uint32_t reserved : 3; // Bit31~29: 保留位，应设置为0
|
||||
} bits;
|
||||
} UnRxCanIdInfo;
|
||||
|
||||
// 输出can数据
|
||||
typedef struct _StrRxCanOutput
|
||||
{
|
||||
uint16_t current_angle; // Byte0~1: 当前角度 [0~65535]对应(-4π~4π)
|
||||
uint16_t current_velocity; // Byte2~3: 当前角速度 [0~65535]对应(-15rad/s~15rad/s)
|
||||
uint16_t current_torque; // Byte4~5: 当前力矩 [0~65535]对应(-120Nm~120Nm)
|
||||
uint16_t temperature; // Byte6~7: 当前温度: Temp(摄氏度)*10
|
||||
} StrRxCanOutput;
|
||||
|
||||
// CAN数据区联合体 (8字节，严格遵循图片协议，大端序数据)
|
||||
typedef union _UnRxCanData
|
||||
{
|
||||
StrRxCanOutput bit_data; // 结构化访问
|
||||
uint8_t arr[sizeof(StrRxCanOutput)]; // 字节数组形式（用于原始数据读写）
|
||||
} UnRxCanData;
|
||||
|
||||
// 接收CAN帧结构体
|
||||
typedef struct _StrRxCanFrame
|
||||
{
|
||||
UnRxCanIdInfo rx_can_id; // 接收到的29位CAN ID
|
||||
UnRxCanData rx_can_data; // 接收到的8字节数据区
|
||||
} StrRxCanFrame;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//// ID 解析输出
|
||||
//typedef struct _StrCanIdInfo
|
||||
//{
|
||||
////-----发送数据0x601----------------------------------------------
|
||||
// uint32_t motor_id:8;
|
||||
// uint32_t data:16;
|
||||
// uint32_t mode:5;
|
||||
// uint32_t res:3;
|
||||
//} StrCanIdInfo;
|
||||
//
|
||||
//
|
||||
//
|
||||
//// canoe协议输出
|
||||
//typedef struct _StrSdoOutput
|
||||
//{
|
||||
////-----发送数据0x601----------------------------------------------
|
||||
// uint16_t index; //索引，类似寄存器地址
|
||||
// uint16_t object_index; // 从索引 为0x0000
|
||||
// uint32_t data; // 数据
|
||||
// StrCanIdInfo rx_can_id;
|
||||
//} StrSdoOutput;
|
||||
//
|
||||
//typedef union _UnSdoOutput
|
||||
//{
|
||||
// StrSdoOutput bit_data; // 使用定义的结构体变量名
|
||||
// uint8_t arr[sizeof(StrSdoOutput)]; // 通过结构体类型确定大小
|
||||
//} UnSdoOutput;
|
||||
|
||||
|
||||
// 超声波数据发送
|
||||
typedef struct _StrUltrasonicOutput
|
||||
@@ -925,6 +1097,17 @@ extern UnTempModuleInput un_temp_module_input;//温度采集模块
|
||||
extern UnAutoComputerInput un_auto_computer_input;//自主计算机自动数据
|
||||
extern UnManualComputerInput un_manual_computer_input;//自主计算机手动数据
|
||||
|
||||
extern StrTxCanFrame un_sdo_output1 ;//电机1输出
|
||||
extern StrTxCanFrame un_sdo_output2 ;//电机2输出
|
||||
extern StrTxCanFrame un_sdo_output3 ;//电机3输出
|
||||
extern StrTxCanFrame un_sdo_output4 ;//电机3速度输出
|
||||
extern StrTxCanFrame un_sdo_output5 ;//电机使能输出
|
||||
|
||||
extern StrRxCanFrame un_pitch_intput ;//电机输入
|
||||
extern StrRxCanFrame un_right_intput ;//电机输入
|
||||
extern StrRxCanFrame un_turn_intput ;//电机输入
|
||||
|
||||
|
||||
extern UnUltrasonicInput un_ultrasonic_input1;//超声波传感器输入1
|
||||
|
||||
extern UnUltrasonicOutput un_ultrasonic_output1;//超声波传感器输出
|
||||
@@ -958,6 +1141,8 @@ extern UnRequestFrame un_request_frame; //请求帧
|
||||
|
||||
extern UnComputerOutput un_computer_output; //输出给自主计算机
|
||||
|
||||
extern UnComputerTurnableInput un_computer_turnable_Input ;//转台以太网输入
|
||||
|
||||
//输出给上位机
|
||||
extern UnVehicleInfoOutput un_vehicle_Info_output; // 车辆信息，输出给上位机
|
||||
extern UnMotorStatusOutput un_motor_status_output; // 电机状态信息，输出给上位机
|
||||
@@ -966,7 +1151,6 @@ extern UnAnalogSignalOutput un_analog_signal_output; // 模拟信号输
|
||||
extern UnRemoteControlOutput un_remote_control_output;// 遥控器数据输出，给上位机
|
||||
extern UnManualControlOutput un_manual_control_output;// 手动控制数据，返回给请求者
|
||||
extern UnAutoControlOutput un_auto_control_output; // 自动控制数据输出，返回给请求者
|
||||
extern UnSdoOutput un_sdo_output ;//转向电机输出
|
||||
|
||||
|
||||
//变量
|
||||
@@ -978,6 +1162,8 @@ extern UnCanDebugOutput un_can_debug_output;//调试输出
|
||||
//函数
|
||||
void canSendAll(void *signal_id);
|
||||
void ethernetSendAll(void *signal_id);
|
||||
float constrain(float value, float min_val, float max_val);
|
||||
|
||||
|
||||
#pragma pack()
|
||||
|
||||
|
||||
@@ -84,7 +84,7 @@ void bootmian(void *signal_id)
|
||||
}
|
||||
}
|
||||
|
||||
timerStart(&boot_timer_interface, 100,0);
|
||||
timerStart(&boot_timer_interface, 100,1);
|
||||
|
||||
// printf("bootAPP spend time:%d\n",getCurrentTime() - time_boot);//<2F><><EFBFBD><EFBFBD>app<70><70><EFBFBD>˶ʱ<E0B3A4><CAB1>
|
||||
}
|
||||
@@ -114,7 +114,7 @@ void bootInterfaceInit(void)
|
||||
// <20><><EFBFBD>Ķ<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>źţ<C5BA><C5A3><EFBFBD><EFBFBD>ڶ<EFBFBD>ʱ<EFBFBD>ɼ<EFBFBD>
|
||||
subscribe(&boot_timer_interface, bootmian);
|
||||
|
||||
timerStart(&boot_timer_interface, 100,0); //100ms
|
||||
timerStart(&boot_timer_interface, 100,1); //100ms
|
||||
|
||||
feedWatchdog();//ι<><CEB9>,<2C><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ιһ<CEB9><D2BB>
|
||||
|
||||
|
||||
111
interface_can.c
111
interface_can.c
@@ -9,7 +9,7 @@
|
||||
#include "app/app_differential_drive.h"
|
||||
#include "app/app_param_manage.h"
|
||||
#include "app/app_ultrasonic.h"
|
||||
|
||||
#include "app/app_turntable.h"
|
||||
|
||||
|
||||
uint32_t OTA_CANTxID = 0x02;//Ĭ<>Ϸ<EFBFBD><CFB7><EFBFBD>IDΪ0x02
|
||||
@@ -533,6 +533,8 @@ void flexcan_Receive_callback_5(flexcan_handle_t *handle,
|
||||
flexcan_status_e status, uint32_t result,
|
||||
void *userData)
|
||||
{
|
||||
flexcan_frame_t *buf = (flexcan_frame_t *)userData;
|
||||
uint8_t i = 0;
|
||||
//--------------------------------------------------------------
|
||||
switch (status)
|
||||
{
|
||||
@@ -540,7 +542,33 @@ void flexcan_Receive_callback_5(flexcan_handle_t *handle,
|
||||
break;
|
||||
|
||||
case FLEXCAN_RX_FIFO_IDLE:
|
||||
|
||||
if( PITCH_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) )
|
||||
{
|
||||
un_pitch_intput.rx_can_id.raw = (buf->id);
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_pitch_intput.rx_can_data.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
}
|
||||
else if( RIGHT_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) )
|
||||
{
|
||||
un_right_intput.rx_can_id.raw = (buf->id);
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_right_intput.rx_can_data.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
|
||||
}
|
||||
else if( TURN_MOTOR_RxCANID == ( (buf->id) & MOTOR_RxCAN_Mask) )
|
||||
{
|
||||
un_turn_intput.rx_can_id.raw = (buf->id);
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_turn_intput.rx_can_data.arr[i] = buf->dataBuffer[i];
|
||||
}
|
||||
|
||||
}
|
||||
else{}
|
||||
break;
|
||||
|
||||
case FLEXCAN_TX_IDLE:
|
||||
@@ -584,6 +612,7 @@ void flexcan_Receive_callback_6(flexcan_handle_t *handle,
|
||||
break;
|
||||
|
||||
case FLEXCAN_RX_FIFO_IDLE:
|
||||
|
||||
if(OTA_CANRxID == (buf->id))//IDΪ1<CEAA><31> boot<6F><74><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
{
|
||||
boot_can_flag = true;
|
||||
@@ -605,7 +634,8 @@ void flexcan_Receive_callback_6(flexcan_handle_t *handle,
|
||||
}
|
||||
publishMessage(&un_ultrasonic_input1, 1);
|
||||
}
|
||||
else{}
|
||||
|
||||
else{}
|
||||
break;
|
||||
|
||||
case FLEXCAN_TX_IDLE:
|
||||
@@ -664,6 +694,13 @@ void flexcan_Receive_callback_7(flexcan_handle_t *handle,
|
||||
|
||||
// printf("ori_remote_stop: %d\n", un_remote_control_input.bit_data.switch_b);
|
||||
}
|
||||
else if(REMOTE_ID_1 == (buf->id))
|
||||
{
|
||||
for(i = 0; i < (buf->length); i++)
|
||||
{
|
||||
un_remote_control_input.arr[i+8] = buf->dataBuffer[i];
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
}
|
||||
@@ -1014,24 +1051,35 @@ void canTimerProcess(void *signal_id)
|
||||
}
|
||||
|
||||
|
||||
//static void processSdoOutput1(void *signal_id)
|
||||
//{
|
||||
// (void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output1, 8, 15);//
|
||||
//}
|
||||
//
|
||||
//static void processSdoOutput2(void *signal_id)
|
||||
//{
|
||||
// (void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output2, 8, 16);
|
||||
//}
|
||||
//
|
||||
//static void processSdoOutput3(void *signal_id)
|
||||
//{
|
||||
// (void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_3, 0x601, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output3, 8, 17);
|
||||
//}
|
||||
//
|
||||
static void processSdoOutput1(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_5, un_sdo_output1.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output1.tx_can_data.arr[0], 8, 15);//
|
||||
}
|
||||
|
||||
static void processSdoOutput2(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_6, un_sdo_output2.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output2.tx_can_data.arr[0], 8, 16);
|
||||
}
|
||||
|
||||
static void processSdoOutput3(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_5, un_sdo_output3.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output3.tx_can_data.arr[0], 8, 17);
|
||||
}
|
||||
|
||||
static void processSdoOutput4(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_5, un_sdo_output4.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output4.tx_can_data.arr[0], 8, 18);
|
||||
}
|
||||
|
||||
static void processSdoOutput5(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_5, un_sdo_output5.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output5.tx_can_data.arr[0], 8, 19);
|
||||
}
|
||||
//static void processSdoOutput4(void *signal_id)
|
||||
//{
|
||||
// (void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
@@ -1147,7 +1195,7 @@ static void processUnGatherOutput(void *signal_id)
|
||||
static void processUltrasonicOutput(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_6, ULTRASONIC_ID_1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_ultrasonic_output1, 8, 15);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_6, ULTRASONIC_ID_1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_ultrasonic_output1, 8, 18);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
|
||||
//static void processHBridgeOutput2(void *signal_id)
|
||||
@@ -1254,7 +1302,7 @@ void canSendAll(void *signal_id)
|
||||
// CAN_Send_Msg(&can_handle_3, 0x123, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_4, 0x124, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_5, 0x125, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
CAN_Send_Msg(&can_handle_6, 0x126, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, &un_can_debug_output.arr[0], 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_6, 0x126, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, &un_can_debug_output.arr[0], 8, 19);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// CAN_Send_Msg(&can_handle_7, 0x127, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
}
|
||||
//
|
||||
@@ -1373,20 +1421,17 @@ void canInterfaceInit(void)
|
||||
subscribe(&un_sw_sample, canInterfaceInput); // <20><>ͣ<EFBFBD><CDA3><EFBFBD>ء<EFBFBD><D8A1><EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>ѱ<EFBFBD>־
|
||||
subscribe(&un_ultrasonic_output1, processUltrasonicOutput); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// subscribe(&un_h_bridge_output2, processHBridgeOutput2);
|
||||
// subscribe(&un_lifter_output, processLifterOutput);
|
||||
|
||||
// subscribe(&un_sdo_output1, processSdoOutput1);
|
||||
// subscribe(&un_sdo_output2, processSdoOutput2);
|
||||
// subscribe(&un_sdo_output3, processSdoOutput3);
|
||||
// subscribe(&un_sdo_output4, processSdoOutput4);
|
||||
subscribe(&un_sdo_output1, processSdoOutput1);
|
||||
subscribe(&un_sdo_output2, processSdoOutput2);
|
||||
subscribe(&un_sdo_output3, processSdoOutput3);
|
||||
subscribe(&un_sdo_output4, processSdoOutput4);
|
||||
subscribe(&un_sdo_output5, processSdoOutput5);
|
||||
|
||||
|
||||
// subscribe(&un_sdo_output5, processSdoOutput5);
|
||||
// subscribe(&un_sdo_output7, processSdoOutput7);
|
||||
|
||||
|
||||
@@ -35,9 +35,9 @@
|
||||
//#define MOTOR_INPUT_ID_4 0x10F94708//<2F>Һ<EFBFBD>
|
||||
//
|
||||
//
|
||||
#define MOTOR_INPUT_ID_5 0x10F81708//<2F><>ǰ <20><>λ
|
||||
#define MOTOR_INPUT_ID_6 0x10F82708//<2F><>ǰ
|
||||
#define MOTOR_INPUT_ID_7 0x10F83708//<2F><><EFBFBD><EFBFBD>
|
||||
//#define MOTOR_INPUT_ID_1 0x10F81708//<2F><>ǰ <20><>λ
|
||||
//#define MOTOR_INPUT_ID_2 0x10F82708//<2F><>ǰ
|
||||
//#define MOTOR_INPUT_ID_3 0x10F83708//<2F><><EFBFBD><EFBFBD>
|
||||
#define MOTOR_INPUT_ID_8 0x10F84708//<2F>Һ<EFBFBD>
|
||||
|
||||
|
||||
@@ -46,6 +46,7 @@
|
||||
#define BMS_INPUT_ID1 0x100
|
||||
#define BMS_INPUT_ID2 0x101
|
||||
#define REMOTE_ID 0x12000023
|
||||
#define REMOTE_ID_1 0x12000024
|
||||
#define TEMP_MODULE_INPUT_ID_1 0x15000003
|
||||
|
||||
|
||||
|
||||
@@ -207,7 +207,16 @@ void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_ad
|
||||
uint16_t i = 0;
|
||||
uint16_t len = 0;
|
||||
uint16_t udp_temp = 0;
|
||||
//------------------------------------------------------------------------------
|
||||
//------------------------------------------------------------------------------
|
||||
// uint8_t ip_addr[4] = {0,0,0,0};
|
||||
//
|
||||
// ip_addr[0] = addr->addr & 0xff; /* IADDR4 */
|
||||
// ip_addr[1] = (addr->addr >> 8) & 0xff; /* IADDR3 */
|
||||
// ip_addr[2] = (addr->addr >> 16) & 0xff; /* IADDR2 */
|
||||
// ip_addr[3] = (addr->addr >> 24) & 0xff; /* IADDR1 */
|
||||
|
||||
|
||||
|
||||
|
||||
if( (0xFF == buf[0] ) && ( 0xBB == buf[1] ) )//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֶ<EFBFBD>ң<EFBFBD><D2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6>ң<EFBFBD><D2A3><EFBFBD><EFBFBD>
|
||||
{
|
||||
@@ -273,9 +282,35 @@ void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_ad
|
||||
// printf("Autorecive len:%d\n",len);
|
||||
// udp_sendto(upcb, p, addr, port);
|
||||
}
|
||||
else
|
||||
else if( (0xFF == buf[0] ) && ( 0x12 == buf[1] ) )
|
||||
{
|
||||
}
|
||||
|
||||
if( (p->len) >= sizeof(un_computer_turnable_Input) )//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD>ȳ<EFBFBD><C8B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1>С<EFBFBD><D0A1><EFBFBD><EFBFBD>
|
||||
{
|
||||
len = sizeof(un_computer_turnable_Input);
|
||||
}
|
||||
else
|
||||
{
|
||||
len = p->len;
|
||||
}
|
||||
|
||||
|
||||
for(i = 0; i < len; i++)
|
||||
{
|
||||
un_computer_turnable_Input.arr[i] = buf[i];
|
||||
// printf("%d", un_computer_turnable_Input.arr[i]); // <20><>16<31><36><EFBFBD>ƴ<EFBFBD>ӡ<EFBFBD><D3A1><EFBFBD>ʺ϶<CABA><CFB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD>
|
||||
}
|
||||
publishMessage(&un_computer_turnable_Input, 1);
|
||||
|
||||
// // <20><>ӡPITCH<43><48><EFBFBD><EFBFBD>CAN<41><4E><EFBFBD><EFBFBD>
|
||||
// printf("un_computer_turnable_Input - ID: 0x%08X, Data: ", buf->id);
|
||||
// for(i = 0; i < 8; i++) {
|
||||
// printf("%02X ", buf->dataBuffer[i]);
|
||||
// }
|
||||
// printf("\n");
|
||||
|
||||
}
|
||||
else{}
|
||||
|
||||
pbuf_free(p);
|
||||
}
|
||||
|
||||
3
main.c
3
main.c
@@ -31,6 +31,7 @@
|
||||
#include <app/app_light.h>
|
||||
#include "app/app_request.h"
|
||||
#include "app/app_ultrasonic.h"
|
||||
#include "app/app_turntable.h"
|
||||
|
||||
void testAppInit(void);
|
||||
|
||||
@@ -129,11 +130,11 @@ int main(void)
|
||||
requestAppInit();
|
||||
canInterfaceInit();
|
||||
bootInterfaceInit();
|
||||
turnableInit();
|
||||
// ultrasonicAppInit();
|
||||
|
||||
printf("All init OK ------ %d\n",getCurrentTime());
|
||||
|
||||
sdrv_gpio_set_pin_output_level(GPIO_B9, 1); //测量时间
|
||||
for (;;)
|
||||
{
|
||||
// 处理信号
|
||||
|
||||
Reference in New Issue
Block a user