diff --git a/app/app_differential_drive.c b/app/app_differential_drive.c index 87a3b2a..3af0cc9 100644 --- a/app/app_differential_drive.c +++ b/app/app_differential_drive.c @@ -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调用一次 } diff --git a/app/app_frm_signal.h b/app/app_frm_signal.h index f598bfa..cac6043 100644 --- a/app/app_frm_signal.h +++ b/app/app_frm_signal.h @@ -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 // 优先级层次 diff --git a/app/app_param_manage.h b/app/app_param_manage.h index d583f96..695cec8 100644 --- a/app/app_param_manage.h +++ b/app/app_param_manage.h @@ -52,6 +52,8 @@ extern "C" X(Ocrv_il) \ X(Ocrv_ol) \ X(minTorq) \ + X(minYpos) \ + X(maxYpos) \ X(test) // 定义一个包含所有参数名称的结构体 diff --git a/app/app_power.c b/app/app_power.c index aa68289..16629f6 100644 --- a/app/app_power.c +++ b/app/app_power.c @@ -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(); // 低压继电器 diff --git a/app/app_turntable.c b/app/app_turntable.c index afc50dc..347f035 100644 --- a/app/app_turntable.c +++ b/app/app_turntable.c @@ -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()); +} + + + + + + + + + + + + + diff --git a/app/app_turntable.h b/app/app_turntable.h index 1aa6101..814f16b 100644 --- a/app/app_turntable.h +++ b/app/app_turntable.h @@ -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 //CAN Ƴλ - - - #define LIMIT_SPEED_INDEX 0x7017//CSPٶ -#define LOC_REF_INDEX 0x7016//CSPλ +#define LOC_REF_INDEX 0x7016//CSPλ +#define IQ_REF_INDEX 0x7006//ģʽ + #define RUM_MODE 0x7005//modeģʽ - - - #define OPERATION_MODE 0 // ˿ģʽ #define POSITION_MODE_PP 1 // λģʽ (PP - Profile Position) #define VELOCITY_MODE 2 // ٶģʽ @@ -23,29 +33,86 @@ extern "C" { #define POSITION_MODE_CSP 5 // λģʽ (CSP - Cyclic Synchronous Position) +// ĵ仯 (A) +#define MAX_STEP 1.0f // 仯 (A)5Aʼ +#define MAX_DI_DT 1000.0f // 仯 (A/s)5000A/sʼ + + +// ѧ +#define PI 3.14159f // ߾Ȧֵ +// Ƕ̶ (ӦByte0~1: ǰǶ) +#define ANGLE_RANGE_MIN (-4.0f * PI) // СǶ: -4 +#define ANGLE_RANGE_MAX (4.0f * PI) // Ƕ: 4 + +// ٶ̶ (ӦByte2~3: ǰٶ) +#define ANGULAR_VELOCITY_MIN -15.0f // Сٶ: -15 rad/s +#define ANGULAR_VELOCITY_MAX 15.0f // ٶ: 15 rad/s + -// canoeЭ -typedef struct _StrSdoOutput +#define MOTOR_VELOCITY_DEADZONE 1.0f// : 120 Nm + +// ̶ (ӦByte4~5: ǰ) +#define TORQUE_MIN -120.0f // С: -120 Nm +#define TORQUE_MAX 120.0f // : 120 Nm + +#define ZERO_VAULE 0x0080 // 32768,Ҫλǰ + +#define REMOTE_ZERO 1022 + + +#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 { -//-----0x601---------------------------------------------- - uint16_t index; //ƼĴַ - uint16_t object_index; // Ϊ0x0000 - uint32_t data; // -} StrRS04Output; + uint8_t turnable_state; + PowerState current_state; // ǰԴ״̬ -typedef union _UnSdoOutput -{ - StrSdoOutput bit_data; // ʹöĽṹ - uint8_t arr[sizeof(StrSdoOutput)]; // ͨṹȷС -} UnRS04Output; + float position_x; //ת̨λx + float position_y; //ת̨λy + float position_z; //ת̨λz + + float desired_speed; // ת̨ٶ + float desired_pitch_position; // λ + float desired_horizontal_position; // ˮƽλ + + float left_motor_speed; // ǰٶ + float right_motor_speed; // ǰҵٶ + float speed; // ǰתٶ + float pitch_position; // ǰλ + float horizontal_position; // ǰˮƽλ + + float max_speed; // ٶ + float out_left_motor_ampere; // + float out_right_motor_ampere; // ҵ + float out_pitch_motor_ampere; // + + float out_left_motor_ampere_last; // + float out_right_motor_ampere_last; // ҵ + float out_pitch_motor_ampere_last; // ҵ + + float out_left_motor_ampere_limit; // ֵ + float out_right_motor_ampere_limit; // ҵֵ + float out_pitch_motor_ampere_limit; // ҵֵ + + Timer turnable_timer; // ʱ + Timer turnable_timer1; // ʱ + Timer turnable_timer2; // ʱ + uint8_t turnable_cnt; + + float min_pitch_postion; // λϢ + float max_pitch_postion; // λϢ + + float max_ampere; // +} TurnableData; + - - - - - +void turnableInit(); + #ifdef __cplusplus } diff --git a/interface.c b/interface.c index 5660176..ed3ea37 100644 --- a/interface.c +++ b/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; + } +} diff --git a/interface.h b/interface.h index bf8f381..7bbbc8d 100644 --- a/interface.h +++ b/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() diff --git a/interface_boot.c b/interface_boot.c index 37a4183..a4b30e5 100644 --- a/interface_boot.c +++ b/interface_boot.c @@ -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);//app˶೤ʱ } @@ -114,7 +114,7 @@ void bootInterfaceInit(void) // Ķʱźţڶʱɼ subscribe(&boot_timer_interface, bootmian); - timerStart(&boot_timer_interface, 100,0); //100ms + timerStart(&boot_timer_interface, 100,1); //100ms feedWatchdog();//ι,ʼιһ diff --git a/interface_can.c b/interface_can.c index 0e0cac5..0182b2e 100644 --- a/interface_can.c +++ b/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;//ĬϷ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 boot { 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; // DZΪʹã -// 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; // DZΪʹã -// 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; // DZΪʹã -// 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; // DZΪʹã +// 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; // DZΪʹã + 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; // DZΪʹã +// 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; // DZΪʹã +// 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; // DZΪʹã +// 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; // DZΪʹã @@ -1147,7 +1195,7 @@ static void processUnGatherOutput(void *signal_id) static void processUltrasonicOutput(void *signal_id) { (void)signal_id; // DZΪʹã - CAN_Send_Msg(&can_handle_6, ULTRASONIC_ID_1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_ultrasonic_output1, 8, 15);// + CAN_Send_Msg(&can_handle_6, ULTRASONIC_ID_1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_ultrasonic_output1, 8, 18);// } //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 // CAN_Send_Msg(&can_handle_4, 0x124, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS // CAN_Send_Msg(&can_handle_5, 0x125, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS - CAN_Send_Msg(&can_handle_6, 0x126, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, &un_can_debug_output.arr[0], 8, 16);//BMS +// CAN_Send_Msg(&can_handle_6, 0x126, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, &un_can_debug_output.arr[0], 8, 19);//BMS // CAN_Send_Msg(&can_handle_7, 0x127, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS } // @@ -1373,20 +1421,17 @@ void canInterfaceInit(void) subscribe(&un_sw_sample, canInterfaceInput); // ͣءѹ ѱ־ subscribe(&un_ultrasonic_output1, processUltrasonicOutput); // - - - - - - // 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); diff --git a/interface_can.h b/interface_can.h index 7b66dd5..577c9a8 100644 --- a/interface_can.h +++ b/interface_can.h @@ -35,9 +35,9 @@ //#define MOTOR_INPUT_ID_4 0x10F94708//Һ // // -#define MOTOR_INPUT_ID_5 0x10F81708//ǰ λ -#define MOTOR_INPUT_ID_6 0x10F82708//ǰ -#define MOTOR_INPUT_ID_7 0x10F83708// +//#define MOTOR_INPUT_ID_1 0x10F81708//ǰ λ +//#define MOTOR_INPUT_ID_2 0x10F82708//ǰ +//#define MOTOR_INPUT_ID_3 0x10F83708// #define MOTOR_INPUT_ID_8 0x10F84708//Һ @@ -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 diff --git a/interface_ethernet.c b/interface_ethernet.c index 6cc8c78..262a980 100644 --- a/interface_ethernet.c +++ b/interface_ethernet.c @@ -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] ) )//ֶңԶң { @@ -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) )//ȡȳȡС + { + 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]); // 16ƴӡʺ϶ݣ + } + publishMessage(&un_computer_turnable_Input, 1); + +// // ӡPITCHCAN +// 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); } diff --git a/main.c b/main.c index a7938aa..f70f56d 100644 --- a/main.c +++ b/main.c @@ -31,6 +31,7 @@ #include #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 (;;) { // 处理信号