diff --git a/app/app_differential_drive.c b/app/app_differential_drive.c index dd93c24..b3cb677 100644 --- a/app/app_differential_drive.c +++ b/app/app_differential_drive.c @@ -625,9 +625,7 @@ static void diffProcess(void *signal_id) float out_torque[4] = {0,0,0,0}; // 使用差速车辆动力学模型计算左右电机的期望速度 computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque); - - - + if( fabs(diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数 { diff_data.left_speed_diff = diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed; diff --git a/app/app_param_manage.c b/app/app_param_manage.c index 661a2d0..542e18e 100644 --- a/app/app_param_manage.c +++ b/app/app_param_manage.c @@ -432,36 +432,36 @@ void paramAppInit(void) // 上电读取所有参数 memset(param_manager.arr, 0, sizeof(param_manager.arr)); accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION); -// whl_bas 轮胎直径 -// max_rpm 最大转速 -// whl_dia 轴距 -// max_acc 最大加速度 -// spd_kp 速度P -// spd_ki 速度I -// spd_kd 速度D -// spd_il 速度积分限制 -// spd_ol 速度PID输出限制 -// crv_kp 转弯P -// crv_ki 转弯I -// crv_kd 转弯D -// crv_il 转弯积分限制 -// crv_ol 转弯PID输出限制 -// brk_on 刹车刹紧时间 -// brk_off 刹车释放时间 -// maxTorq 最大扭矩 -// feedPwr 最大馈电功率 -// dispPwr 最大放电功率 -// VehMass 车重 +// whl_bas 轮胎直径<单位：m> +// max_rpm 最大转速<单位：rpm/min> +// whl_dia 轴距<单位：m> +// max_acc 最大加速度 +// spd_kp 遥控速度P +// spd_ki 遥控速度I +// spd_kd 遥控速度D +// spd_il 遥控速度积分限制 +// spd_ol 遥控速度PID输出限制 +// crv_kp 遥控转弯P +// crv_ki 遥控转弯I +// crv_kd 遥控转弯D +// crv_il 遥控转弯积分限制 +// crv_ol 遥控转弯PID输出限制 +// brk_on 刹车刹紧时间 单位：ms +// brk_off 刹车释放时间 单位：ms +// maxTorq 最大扭矩 单位：n*m +// feedPwr 最大馈电功率 单位：w +// dispPwr 最大放电功率 单位：w +// VehMass 车重 单位：kg // gRatio 减速比 -// prCTime 预充电时间 +// prCTime 预充电时间 单位：s // brk_pos 刹车位置 // pwr_sta 电源状态 // high_sw 高压开关状态 // stop_sw 急停开关状态 // lightSt 灯光状态 // pwr_btn 电源开关状态 -// sleepTm 休眠时间 -// wakeTm 唤醒时间 +// sleepTm 休眠时间 单位：min +// wakeTm 唤醒时间 单位：min // Ospd_kp 自主速度P // Ospd_ki 自主速度I // Ospd_kd 自主速度D @@ -472,7 +472,7 @@ void paramAppInit(void) // Ocrv_kd 自主转弯D // Ocrv_il 自主转弯积分限制 // Ocrv_ol 自主转弯PID输出限制 -// minTorq 输出扭矩死区 +// minTorq 输出扭矩死区 单位：n*m // brk_rev 刹车方向 // mot_kp 同侧扭矩P参数 // mot_ki 同侧扭矩I参数 diff --git a/app/app_power.c b/app/app_power.c index 5a158c9..f15c045 100644 --- a/app/app_power.c +++ b/app/app_power.c @@ -120,6 +120,8 @@ static void powerOutput(void *signal_id) un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 计算机 + un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 @@ -136,6 +138,8 @@ static void powerOutput(void *signal_id) un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 计算机 + un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 @@ -153,6 +157,8 @@ static void powerOutput(void *signal_id) un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器 un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 计算机 + un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 @@ -169,6 +175,8 @@ static void powerOutput(void *signal_id) un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 计算机 + un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 @@ -185,6 +193,8 @@ static void powerOutput(void *signal_id) un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 计算机 + un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 @@ -201,6 +211,8 @@ static void powerOutput(void *signal_id) un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器 un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 计算机 + un_inf_can_kgf_output1.bit_data.KGF14 = setPowerOn(); // 计算机 un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 diff --git a/app/app_temp.c b/app/app_temp.c index 971db69..db6b6c8 100644 --- a/app/app_temp.c +++ b/app/app_temp.c @@ -11,7 +11,6 @@ // 声明 temp_data 变量 TempSystem temp_data; - static void handleTemperatureAlarm(int16_t current_temp, float alarm_temp, float critical_temp, float threshold_temp, TempState *state) @@ -89,23 +88,6 @@ static void tempOutput(void *signal_id) // 电机1风扇 左前 switch (temp_data.state[0]) - { - case TEMP_NORMAL: - un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_01 = 0; - break; - case TEMP_WARNING: - un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_01 = 5; - break; - case TEMP_CRITICAL: - un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_01 = 0; - break; - } - - // 电机2风扇 右前 - switch (temp_data.state[1]) { case TEMP_NORMAL: un_inf_can_kgf_output1.bit_data.KGF02 = setFanOff();//电机控制器风扇 @@ -119,28 +101,28 @@ static void tempOutput(void *signal_id) un_inf_can_kgf_output1.bit_data.KGF02 = setFanOn();//电机控制器风扇 un_inf_can_kgf_output1.bit_data.pwm_02 = 0; break; + } + + // 电机2风扇 右前 + switch (temp_data.state[1]) + { + case TEMP_NORMAL: + un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_01 = 0; + break; + case TEMP_WARNING: + un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_01 = 5; + break; + case TEMP_CRITICAL: + un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_01 = 0; + break; } // 电机3风扇 左后 switch (temp_data.state[2]) - { - case TEMP_NORMAL: - un_inf_can_kgf_output2.bit_data.KGF07 = setFanOff();//电机控制器风扇 - un_inf_can_kgf_output2.bit_data.pwm_07 = 0; - break; - case TEMP_WARNING: - un_inf_can_kgf_output2.bit_data.KGF07 = setFanOn();//电机控制器风扇 - un_inf_can_kgf_output2.bit_data.pwm_07 = 5; - break; - case TEMP_CRITICAL: - un_inf_can_kgf_output2.bit_data.KGF07 = setFanOn();//电机控制器风扇 - un_inf_can_kgf_output2.bit_data.pwm_07 = 0; - break; - } - - // 电机4风扇 右后 - switch (temp_data.state[3]) { case TEMP_NORMAL: un_inf_can_kgf_output2.bit_data.KGF08 = setFanOff();//电机控制器风扇 @@ -149,11 +131,28 @@ static void tempOutput(void *signal_id) case TEMP_WARNING: un_inf_can_kgf_output2.bit_data.KGF08 = setFanOn();//电机控制器风扇 un_inf_can_kgf_output2.bit_data.pwm_08 = 5; - break; - case TEMP_CRITICAL: + break; + case TEMP_CRITICAL: un_inf_can_kgf_output2.bit_data.KGF08 = setFanOn();//电机控制器风扇 un_inf_can_kgf_output2.bit_data.pwm_08 = 0; break; + } + + // 电机4风扇 右后 + switch (temp_data.state[3]) + { + case TEMP_NORMAL: + un_inf_can_kgf_output2.bit_data.KGF07 = setFanOff();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_07 = 0; + break; + case TEMP_WARNING: + un_inf_can_kgf_output2.bit_data.KGF07 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_07 = 5; + break; + case TEMP_CRITICAL: + un_inf_can_kgf_output2.bit_data.KGF07 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_07 = 0; + break; } // // 电机3风扇 @@ -174,26 +173,33 @@ static void tempOutput(void *signal_id) // } publishMessage(&un_inf_can_kgf_output1, 1); + publishMessage(&un_inf_can_kgf_output2, 1); } // 温度状态处理函数 static void tempProcess(void *signal_id) { (void)signal_id; - int16_t max_temp[4] = {0,0}; + int16_t max_temp[4] = {0,0,0,0}; + // 调用按钮处理函数 max_temp[0] = temp_data.current_temp[0]; max_temp[1] = temp_data.current_temp[1]; - -// printf("motor1 temp: %d, motor2 temp: %d\n", max_temp[0], max_temp[1]); + max_temp[2] = temp_data.current_temp[2]; + max_temp[3] = temp_data.current_temp[3]; + + // printf("motor1 temp: %d, motor2 temp: %d\n", max_temp[0], max_temp[1]); handleTemperatureAlarm(max_temp[0], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[0]); - + handleTemperatureAlarm(max_temp[1], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[1]); handleTemperatureAlarm(max_temp[2], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[2]); - + handleTemperatureAlarm(max_temp[3], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[3]); + + + // if (max_temp[0] >= 60) // 假设60度为危险温度 @@ -252,7 +258,7 @@ static void tempInput(void *signal_id) else if(signal_id == &un_motor_temp4) { temp_data.current_temp[3] = ( (int16_t)(un_motor_temp4.bit_data.controller_temp) - 40); - } + } else{} } diff --git a/app/app_ultrasonic.c b/app/app_ultrasonic.c index bbc96a6..29a4d5e 100644 --- a/app/app_ultrasonic.c +++ b/app/app_ultrasonic.c @@ -1,83 +1,83 @@ -#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" -#include "app_ultrasonic.h" - - -// 定时器结构体 -Timer ultrasonic_timer; -Timer ultrasonic_timer1; - -uint16_t fornt_distance = 0;//mm - -// 定时器信号处理函数 -void ultrasonicTimer1Process(void *signal_id) -{ - (void)signal_id; // 标记变量为已使用，避免编译器警告 - - printf("fornt distance = %d\n",fornt_distance); - - - timerStart(&ultrasonic_timer1, 1000,1); -} - - - - - -// 定时器信号处理函数 -void ultrasonicTimerProcess(void *signal_id) -{ - (void)signal_id; // 标记变量为已使用，避免编译器警告 - - un_ultrasonic_output1.bit_data.node = ULTRASONIC_FRONT_NODE; - un_ultrasonic_output1.bit_data.function_code = ULTRASONIC_READ; - un_ultrasonic_output1.bit_data.register_address = REALTIME_DISTANCE_REG; - un_ultrasonic_output1.bit_data.data = 0x100; - publishMessage(&un_ultrasonic_output1, 1); - - timerStart(&ultrasonic_timer, 200,1); -} - -void ultrasonicInput(void *signal_id) -{ - if(signal_id == &un_ultrasonic_input1) - { - fornt_distance = SWAP_ENDIAN_16(un_ultrasonic_input1.bit_data.data);//高低字节交换 - } -} - - - - -// APP模块的初始化 -void ultrasonicAppInit(void) -{ - // 初始化定时器 - timerInit(&ultrasonic_timer); - timerInit(&ultrasonic_timer1); - // 订阅定时器信号 - subscribe(&ultrasonic_timer, ultrasonicTimerProcess); - subscribe(&ultrasonic_timer1, ultrasonicTimer1Process); - - subscribe(&un_ultrasonic_input1, ultrasonicInput); - printf("ultrasonicAPP: initial OK %d\n",getCurrentTime()); - timerStart(&ultrasonic_timer, 200,1); - timerStart(&ultrasonic_timer1, 1000,1); -} - - - - - - - - - - +#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" +#include "app_ultrasonic.h" + + +// 定时器结构体 +Timer ultrasonic_timer; +Timer ultrasonic_timer1; + +uint16_t fornt_distance = 0;//mm + +// 定时器信号处理函数 +void ultrasonicTimer1Process(void *signal_id) +{ + (void)signal_id; // 标记变量为已使用，避免编译器警告 + + printf("fornt distance = %d\n",fornt_distance); + + + timerStart(&ultrasonic_timer1, 1000,1); +} + + + + + +// 定时器信号处理函数 +void ultrasonicTimerProcess(void *signal_id) +{ + (void)signal_id; // 标记变量为已使用，避免编译器警告 + + un_ultrasonic_output1.bit_data.node = ULTRASONIC_FRONT_NODE; + un_ultrasonic_output1.bit_data.function_code = ULTRASONIC_READ; + un_ultrasonic_output1.bit_data.register_address = REALTIME_DISTANCE_REG; + un_ultrasonic_output1.bit_data.data = 0x100; + publishMessage(&un_ultrasonic_output1, 1); + + timerStart(&ultrasonic_timer, 200,1); +} + +void ultrasonicInput(void *signal_id) +{ + if(signal_id == &un_ultrasonic_input1) + { + fornt_distance = SWAP_ENDIAN_16(un_ultrasonic_input1.bit_data.data);//高低字节交换 + } +} + + + + +// APP模块的初始化 +void ultrasonicAppInit(void) +{ + // 初始化定时器 + timerInit(&ultrasonic_timer); + timerInit(&ultrasonic_timer1); + // 订阅定时器信号 + subscribe(&ultrasonic_timer, ultrasonicTimerProcess); + subscribe(&ultrasonic_timer1, ultrasonicTimer1Process); + + subscribe(&un_ultrasonic_input1, ultrasonicInput); + printf("ultrasonicAPP: initial OK %d\n",getCurrentTime()); + timerStart(&ultrasonic_timer, 200,1); + timerStart(&ultrasonic_timer1, 1000,1); +} + + + + + + + + + + diff --git a/app/app_ultrasonic.h b/app/app_ultrasonic.h index 00691f7..924c13a 100644 --- a/app/app_ultrasonic.h +++ b/app/app_ultrasonic.h @@ -1,75 +1,75 @@ -#ifndef ULTRASONIC_H -#define ULTRASONIC_H - -#ifdef __cplusplus -extern "C" { -#endif - - -#define ULTRASONIC_FRONT_NODE 0x1 -#define ULTRASONIC_BACK_NODE 0x2 - -#define ULTRASONIC_ID_1 0x520 + ULTRASONIC_FRONT_NODE -#define ULTRASONIC_ID_2 0x520 + ULTRASONIC_BACK_NODE - -#define ULTRASONIC_READ 0x03 -#define ULTRASONIC_WRITE 0x06 - - - - -#define SOFTWARE_VERSION_REG 0x0000 // ģ汾(ʮֵ), : uint16_t - - -#define PROCESSED_DISTANCE_REG 0x0100 /* - * ľֵ(mm), : uint16_t - * ָ࣬㷨 - * Ӧʱ䣺190-750ms(̶) - */ - -#define REALTIME_DISTANCE_REG 0x0101 /* - * ʵʱβֵ(mm), : uint16_t - * ָ󴥷β - * Ӧʱ䣺15-140ms(̶) - */ - -#define TEMPERATURE_REG 0x0102 /* - * ¶ȴֵ, : int16_t - * λ0.1 (ԭʼֵ/10), ֱʣ0.5 - * Ӧʱ䣺15-140ms(̶) - */ - -#define ECHO_TIME_REG 0x010A /* - * زʱԭʼֵ(s), : uint16_t - * 뻻㣺ֵ/5.75 = ׾ - * Ӧʱ䣺15-140ms(̶) - */ - - - -#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)) - - - - - - - - - - - - void ultrasonicAppInit(void); - - - - - - - -#ifdef __cplusplus -} -#endif - -#endif // ULTRASONIC_H +#ifndef ULTRASONIC_H +#define ULTRASONIC_H + +#ifdef __cplusplus +extern "C" { +#endif + + +#define ULTRASONIC_FRONT_NODE 0x1 +#define ULTRASONIC_BACK_NODE 0x2 + +#define ULTRASONIC_ID_1 0x520 + ULTRASONIC_FRONT_NODE +#define ULTRASONIC_ID_2 0x520 + ULTRASONIC_BACK_NODE + +#define ULTRASONIC_READ 0x03 +#define ULTRASONIC_WRITE 0x06 + + + + +#define SOFTWARE_VERSION_REG 0x0000 // ģ汾(ʮֵ), : uint16_t + + +#define PROCESSED_DISTANCE_REG 0x0100 /* + * ľֵ(mm), : uint16_t + * ָ࣬㷨 + * Ӧʱ䣺190-750ms(̶) + */ + +#define REALTIME_DISTANCE_REG 0x0101 /* + * ʵʱβֵ(mm), : uint16_t + * ָ󴥷β + * Ӧʱ䣺15-140ms(̶) + */ + +#define TEMPERATURE_REG 0x0102 /* + * ¶ȴֵ, : int16_t + * λ0.1 (ԭʼֵ/10), ֱʣ0.5 + * Ӧʱ䣺15-140ms(̶) + */ + +#define ECHO_TIME_REG 0x010A /* + * زʱԭʼֵ(s), : uint16_t + * 뻻㣺ֵ/5.75 = ׾ + * Ӧʱ䣺15-140ms(̶) + */ + + + +#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)) + + + + + + + + + + + + void ultrasonicAppInit(void); + + + + + + + +#ifdef __cplusplus +} +#endif + +#endif // ULTRASONIC_H diff --git a/app_turntable.c b/app_turntable.c new file mode 100644 index 0000000..d4c4236 --- /dev/null +++ b/app_turntable.c @@ -0,0 +1,460 @@ +#include "app_config.h" +#include "app_dependence.h" +#include "interface.h" +#include "app_turntable.h" +#include "app_pid.h" +#include "app_param_manage.h" + +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" + +#include "sdrv_vic.h" + +PID_t turnable_speed_pid; +PID_t turnable_position_pid; + +TurnableData turnable_data = {0}; + + + + + +/** + * @brief 带死区的原始数据到物理量转换函数（简单版） + * @param raw_value 原始16位无符号整数值 [0, 65535] + * @param min 物理量最小值（如 -10.0） + * @param max 物理量最大值（如 +10.0） + * @param deadzone 死区范围（物理量单位，如 1.0 表示 ±1.0 内为死区） + * @return 转换后的物理量值（若在死区内返回0，否则返回实际值） + */ +static float convertPhysical(uint16_t raw_value, float min, float max, float deadzone) +{ + // 1. 计算实际物理量值 + float physical_value = min + ((float)raw_value / 65535.0f) * (max - min); + + // 2. 判断是否在死区内（绝对值 ≤ deadzone） + if (fabs(physical_value) <= deadzone) + { + return 0.0f; // 死区内返回0 + } + else + { + return physical_value; // 死区外返回实际值 + } +} + + + +/** + * @brief 将浮点数转换为uint32_t（按小端序存储） + * @param num 输入的浮点数 + * @return 转换后的uint32_t值（直接内存拷贝结果） + * @note 此函数通过内存直接拷贝实现转换，不进行数值计算，结果受平台字节序影响 + */ +uint32_t floatToUint32(float num) +{ + uint32_t result; + // 将浮点数的内存数据直接拷贝到uint32_t变量 + memcpy(&result, &num, sizeof(num)); + return result; +} + + +/** + * @brief 电机失能函数（停止电机运行） + * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) + * @param master_id 主控制器ID (用于标识发送方) + * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 + * @return 0: 成功, -1: 参数无效 + * @note 此函数会修改unsdodata指向的结构体内容，调用后需及时发送CAN报文 + */ +int8_t motorDisable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata) +{ + /* 参数有效性检查 */ + if (unsdodata == NULL) { + return -1; + } + + /* 设置CAN报文ID域 */ + unsdodata->tx_can_id.bits.mode = 3; /* 通信模式3：电机失能 */ + unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ + unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ + unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ + + /* 清零数据域 */ + unsdodata->tx_can_data.bit_data.data = 0; + unsdodata->tx_can_data.bit_data.index = 0; + unsdodata->tx_can_data.bit_data.object_index = 0; + + return 0; +} + +/** + * @brief 电机使能函数（启动电机运行） + * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) + * @param master_id 主控制器ID (用于标识发送方) + * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 + * @return 0: 成功, -1: 参数无效 + * @note 通信模式4：电机使能 + */ +int8_t motorEnable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata) +{ + /* 参数有效性检查 */ + if (unsdodata == NULL) { + return -1; + } + + /* 设置CAN报文ID域 */ + unsdodata->tx_can_id.bits.mode = 3; /* 通信模式4：电机使能 */ + unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ + unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ + unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ + + /* 清零数据域 */ + unsdodata->tx_can_data.bit_data.data = 0; + unsdodata->tx_can_data.bit_data.index = 0; + unsdodata->tx_can_data.bit_data.object_index = 0; + + return 0; +} + +/** + * @brief 设置电机运行模式 + * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) + * @param master_id 主控制器ID (用于标识发送方) + * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 + * @param mode 要设置的模式值 (具体含义需参考电机协议文档) + * @return 0: 成功, -1: 参数无效 + * @note RUM_MODE应为预定义的宏，表示运行模式索引 + */ +int8_t setMotorMode(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint8_t mode) +{ + /* 参数有效性检查 */ + if (unsdodata == NULL) { + return -1; + } + + /* 设置CAN报文ID域 */ + unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */ + unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ + unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ + unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ + + /* 设置数据域 */ + unsdodata->tx_can_data.bit_data.index = RUM_MODE; /* 运行模式索引 */ + unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */ + unsdodata->tx_can_data.bit_data.data = mode; /* 模式值 */ + + return 0; +} + +/** + * @brief 写入电机参数 + * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255) + * @param master_id 主控制器ID (用于标识发送方) + * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据 + * @param index 要写入的参数索引 (具体含义需参考电机协议文档) + * @param ref 要写入的参数值 (浮点数，会自动转换为uint32_t) + * @return 0: 成功, -1: 参数无效 + * @note 使用floatToUint32函数转换浮点参数 + */ +int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint16_t index, float ref) +{ + /* 参数有效性检查 */ + if (unsdodata == NULL) { + return -1; + } + + /* 设置CAN报文ID域 */ + unsdodata->tx_can_id.bits.mode = 0x12; /* 通信模式0x12：参数写入 */ + unsdodata->tx_can_id.bits.motor_id = motor_id; /* 目标电机ID */ + unsdodata->tx_can_id.bits.res = 0; /* 保留位清零 */ + unsdodata->tx_can_id.bits.data = master_id; /* 主控制器ID */ + + /* 设置数据域 */ + unsdodata->tx_can_data.bit_data.index = index; /* 参数索引 */ + unsdodata->tx_can_data.bit_data.object_index = 0; /* 子索引通常为0 */ + unsdodata->tx_can_data.bit_data.data = floatToUint32(ref); /* 转换并写入参数值 */ + + return 0; +} + + +/** + * @brief 动态斜率限制（支持变时间间隔） + * @param last_command 上一次的电流指令值 + * @param target_current 本次目标电流指令 + * @param delta_time 距离上一次调用的时间间隔 (s) + * @return 限制后的安全电流指令 + */ +float dynamic_current_limit(float *last_command, float target_current, float delta_time) +{ + // 计算期望的变化量 + float desired_change = target_current - *last_command; + + // 计算两种限制 + float step_limit = MAX_STEP; + float time_limit = MAX_DI_DT * delta_time; // 动态计算时间限制 + + // 选择更严格的限制 + float max_allowed_change = (step_limit < time_limit) ? step_limit : time_limit; + + // 应用限制并返回新指令 + float actual_change = constrain(desired_change, -max_allowed_change, max_allowed_change); + *last_command = *last_command + actual_change;//更新过去值 + + return *last_command + actual_change; +} + +static void setTurnableMotorOutput() +{ + static float previous_time2 = 0.0f; + + float time1 = (float)getCurrentTime(); + float dt = (time1 - previous_time2) / PERIOD_TICK; + previous_time2 = time1; + + turnable_data.out_pitch_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_pitch_motor_ampere_last,turnable_data.out_pitch_motor_ampere,dt); + turnable_data.out_right_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_right_motor_ampere_last,turnable_data.out_right_motor_ampere,dt); + turnable_data.out_left_motor_ampere_limit = dynamic_current_limit(&turnable_data.out_left_motor_ampere_last ,turnable_data.out_left_motor_ampere ,dt); + + + + + setMotorWrite(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, IQ_REF_INDEX,turnable_data.out_pitch_motor_ampere_limit); + setMotorWrite(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, IQ_REF_INDEX,turnable_data.out_right_motor_ampere_limit); + setMotorWrite(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3, IQ_REF_INDEX,turnable_data.out_left_motor_ampere_limit); + + + un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(turnable_data.speed*10); + un_can_debug_output.bit_data.desired_speed = (uint8_t)(int8_t)(turnable_data.desired_speed*10); + +// un_can_debug_output.bit_data.curvature = (uint8_t)(int8_t)(diff_data.yaw_rate*10); +// un_can_debug_output.bit_data.desired_curvature = (uint8_t)(int8_t)(diff_data.desired_yaw_rate*10); + + un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(turnable_data.out_left_motor_ampere_limit * 100); + un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(turnable_data.out_right_motor_ampere_limit*100); + + + + + + publishMessage(&un_sdo_output1, 1); + publishMessage(&un_sdo_output2, 1); + publishMessage(&un_sdo_output3, 1); +} + + + + + +// 转台 +static void turnableProcess(void *signal_id) +{ + + static float previous_time1 = 0.0f; + + float time1 = (float)getCurrentTime(); + float dt = (time1 - previous_time1) / PERIOD_TICK; + previous_time1 = time1; +// if((turnable_data.current_state == POWER_WORKING))//高压上电才运行 +// { + switch(turnable_data.turnable_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据 + { + case 0: + timerStart(&turnable_data.turnable_timer, 1000, 0); // 启动定时器，1s + turnable_data.turnable_state = 1; + break; + + case 1: + if (!turnable_data.turnable_timer.active)// 1s定时 + { + turnable_data.turnable_state = 2; + } + else + { + turnable_data.turnable_state = 1; + } + break; + + case 2://模式设置 + if(turnable_data.turnable_cnt >= 5)//发送5次 + { + turnable_data.turnable_cnt = 0; + turnable_data.turnable_state = 3; + } + else + { + turnable_data.turnable_cnt ++; + turnable_data.turnable_state = 2; + + setMotorMode(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1, POSITION_MODE_CSP); + setMotorMode(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2, CURRENT_MODE); + setMotorMode(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3, CURRENT_MODE); + + publishMessage(&un_sdo_output1, 1); + publishMessage(&un_sdo_output2, 1); + publishMessage(&un_sdo_output3, 1); + + } + break; + +//------------------------------------------------------------------------------ + case 3: + if(turnable_data.turnable_cnt >= 5)//发送5次 + { + turnable_data.turnable_cnt = 0; + turnable_data.turnable_state = 4; + } + else + { + turnable_data.turnable_cnt ++; + turnable_data.turnable_state = 3; + + motorEnable(MASTER_CANID, PITCH_MOTOR_CANID, &un_sdo_output1); + motorEnable(MASTER_CANID, RIGHT_MOTOR_CANID, &un_sdo_output2); + motorEnable(MASTER_CANID, TURN_MOTOR_CANID, &un_sdo_output3); + + publishMessage(&un_sdo_output1, 1); + publishMessage(&un_sdo_output2, 1); + publishMessage(&un_sdo_output3, 1); + } + break; + + case 4: + turnable_data.out_left_motor_ampere = calculatePidOutput(&turnable_speed_pid, turnable_data.desired_speed, turnable_data.speed, 0.0f, dt); + + turnable_data.turnable_cnt = 0; + turnable_data.turnable_state = 4; + + setTurnableMotorOutput();//输出函数 + break; + + default:break; + } +// } +// else +// { +// turnable_data.turnable_cnt ++; +// turnable_data.turnable_state = 0; +// } +} + + + + +void turnableParametersInit(void *signal_id) +{ + (void)signal_id; // 标记变量为已使用，避免编译器警告 + + setPidParameters(&turnable_speed_pid, + getParam("spd_kp"), + getParam("spd_ki"), + getParam("spd_kd"), + getParam("spd_il"), + getParam("spd_ol") + ); + + printf( "turnable left A %f\n",turnable_data.out_left_motor_ampere); + printf( "turnable right A %f\n",turnable_data.out_right_motor_ampere); + printf( "turnable pitch A %f\n",turnable_data.out_pitch_motor_ampere); + printf( "desired speed %f\n",turnable_data.desired_speed); + printf( "speed %f\n",turnable_data.speed); + printf( "turnable state %d\n",turnable_data.turnable_state); + + timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次 + + +} + + + + + + + + + + + + + +// 差速输入处理函数 +static void turnableInput(void *signal_id) +{ + if(signal_id == &power_data)//电机上电 + { + turnable_data.current_state = power_data.current_state; + } + else if(signal_id == &un_computer_turnable_Input) + { + turnable_data.desired_speed = (float)( SWAP_ENDIAN_32(un_computer_turnable_Input.bit_data.position_x) ); + + } + else{} + + turnable_data.right_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_right_intput.rx_can_data.bit_data.current_velocity),ANGULAR_VELOCITY_MIN,ANGULAR_VELOCITY_MAX,MOTOR_VELOCITY_DEADZONE ); + turnable_data.left_motor_speed = convertPhysical( SWAP_ENDIAN_16(un_turn_intput.rx_can_data.bit_data.current_velocity) ,ANGULAR_VELOCITY_MIN,ANGULAR_VELOCITY_MAX, MOTOR_VELOCITY_DEADZONE ); + turnable_data.speed = (turnable_data.right_motor_speed + turnable_data.left_motor_speed)/2.0f; + + turnableProcess(signal_id);//处理映射 + timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次 +} + + +void turnableInit() +{ + // 初始化速度 PID 控制器 + initializePid(&turnable_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + + // 设置速度 PID 控制器的参数 + setPidParameters(&turnable_speed_pid, + getParam("spd_kp"), + getParam("spd_ki"), + getParam("spd_kd"), + getParam("spd_il"), + getParam("spd_ol") + ); + + subscribe(&un_computer_turnable_Input, turnableInput); + + timerInit(&turnable_data.turnable_timer); + timerInit(&turnable_data.turnable_timer1); + + timerInit(&turnable_data.turnable_timer2); + subscribe(&turnable_data.turnable_timer2, turnableInput); + + timerStart(&turnable_data.turnable_timer2,100,1);//100ms调用一次 + + subscribe(&turnable_data.turnable_timer1, turnableParametersInit); + timerStart(&turnable_data.turnable_timer1,1000,1);//100ms调用一次 + + turnable_data.turnable_state = 0; + un_right_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE; + un_right_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE; + un_right_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE; + + un_turn_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE; + un_turn_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE; + un_turn_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE; + + un_pitch_intput.rx_can_data.bit_data.current_velocity = ZERO_VAULE; + un_pitch_intput.rx_can_data.bit_data.current_angle = ZERO_VAULE; + un_pitch_intput.rx_can_data.bit_data.current_torque = ZERO_VAULE; + + printf( "turnable: initial OK %d\n",getCurrentTime()); +} + + + + + + + + + + + + + diff --git a/app_turntable.h b/app_turntable.h new file mode 100644 index 0000000..5a2093b --- /dev/null +++ b/app_turntable.h @@ -0,0 +1,119 @@ +#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 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 // ٶģʽ +#define CURRENT_MODE 3 // ģʽ +#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 + + +#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 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 +{ + uint8_t turnable_state; + PowerState current_state; // ǰԴ״̬ + + 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 max_ampere; // +} TurnableData; + + + +void turnableInit(); + + +#ifdef __cplusplus +} +#endif + +#endif // TURNTAABLE_H diff --git a/interface_can.c b/interface_can.c index 5c78e5d..132eb2d 100644 --- a/interface_can.c +++ b/interface_can.c @@ -1275,12 +1275,12 @@ void canSendAll(void *signal_id) processWheelSpeedOutput(CanData); - CAN_Send_Msg(&can_handle_0, 0x120, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS�������� - CAN_Send_Msg(&can_handle_1, 0x121, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS�������� - CAN_Send_Msg(&can_handle_2, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS�������� - 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_0, 0x120, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS�������� +// CAN_Send_Msg(&can_handle_1, 0x121, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS�������� +// CAN_Send_Msg(&can_handle_2, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS�������� +// 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_7, 0x127, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS�������� }