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

取消can调试数据发送

Browse Source
This commit is contained in:
liumin
2025-11-07 09:52:24 +08:00
parent 9a71ca1edb
commit 565c3f8918
9 changed files with 829 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
app/app_differential_drive.c
View File

@@ -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;

48
app/app_param_manage.c
View File

@@ -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参数

12
app/app_power.c
View File

@@ -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(); // 网络交换机

92
app/app_temp.c
View File

@@ -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{}
}

166
app/app_ultrasonic.c
View File

@@ -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);
}

150
app/app_ultrasonic.h
View File

@@ -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 // ģ<><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>汾<EFBFBD><E6B1BE>(ʮ<><CAAE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ), <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: uint16_t
#define PROCESSED_DISTANCE_REG 0x0100 /*
* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ(mm), <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: uint16_t
* <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࣬<EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <20><>Ӧʱ<D3A6>䣺190-750ms(<28><><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD>)
*/
#define REALTIME_DISTANCE_REG 0x0101 /*
* ʵʱ<CAB5><CAB1><EFBFBD>β<EFBFBD><CEB2><EFBFBD>ֵ(mm), <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: uint16_t
* <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD>󴥷<EFBFBD><F3B4A5B7><EFBFBD><EFBFBD>β<EFBFBD><CEB2><EFBFBD>
* <20><>Ӧʱ<D3A6>䣺15-140ms(<28><><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD>)
*/
#define TEMPERATURE_REG 0x0102 /*
* <20>¶ȴ<C2B6><C8B4><EFBFBD><EFBFBD><EFBFBD>ֵ, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: int16_t
* <20><>λ<EFBFBD><CEBB>0.1<EFBFBD><EFBFBD> (ԭʼֵ<CABC><D6B5>/10), <20>ֱ<EFBFBD><D6B1>ʣ<EFBFBD>0.5<EFBFBD><EFBFBD>
* <20><>Ӧʱ<D3A6>䣺15-140ms(<28><><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD>)
*/
#define ECHO_TIME_REG 0x010A /*
* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD>ʱ<EFBFBD><CAB1>ԭʼֵ(<28><>s), <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: uint16_t
* <20><><EFBFBD>뻻<EFBFBD>㣺ֵ/5.75 = <20><><EFBFBD>׾<EFBFBD><D7BE><EFBFBD>
* <20><>Ӧʱ<D3A6>䣺15-140ms(<28><><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD>)
*/
#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 // ģ<><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>汾<EFBFBD><E6B1BE>(ʮ<><CAAE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ), <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: uint16_t
#define PROCESSED_DISTANCE_REG 0x0100 /*
* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ(mm), <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: uint16_t
* <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࣬<EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <20><>Ӧʱ<D3A6>䣺190-750ms(<28><><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD>)
*/
#define REALTIME_DISTANCE_REG 0x0101 /*
* ʵʱ<CAB5><CAB1><EFBFBD>β<EFBFBD><CEB2><EFBFBD>ֵ(mm), <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: uint16_t
* <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD>󴥷<EFBFBD><F3B4A5B7><EFBFBD><EFBFBD>β<EFBFBD><CEB2><EFBFBD>
* <20><>Ӧʱ<D3A6>䣺15-140ms(<28><><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD>)
*/
#define TEMPERATURE_REG 0x0102 /*
* <20>¶ȴ<C2B6><C8B4><EFBFBD><EFBFBD><EFBFBD>ֵ, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: int16_t
* <20><>λ<EFBFBD><CEBB>0.1<EFBFBD><EFBFBD> (ԭʼֵ<CABC><D6B5>/10), <20>ֱ<EFBFBD><D6B1>ʣ<EFBFBD>0.5<EFBFBD><EFBFBD>
* <20><>Ӧʱ<D3A6>䣺15-140ms(<28><><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD>)
*/
#define ECHO_TIME_REG 0x010A /*
* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD>ʱ<EFBFBD><CAB1>ԭʼֵ(<28><>s), <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: uint16_t
* <20><><EFBFBD>뻻<EFBFBD>㣺ֵ/5.75 = <20><><EFBFBD>׾<EFBFBD><D7BE><EFBFBD>
* <20><>Ӧʱ<D3A6>䣺15-140ms(<28><><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD>)
*/
#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

460
app_turntable.c Normal file
View File

@@ -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());
}

119
app_turntable.h Normal file
View File

@@ -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 //<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 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>ģʽ
#define CURRENT_MODE 3 // <20><><EFBFBD><EFBFBD>ģʽ
#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
#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 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; // <20><>ǰ<EFBFBD><C7B0>Դ״̬
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><CEBB>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><D2B5><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 max_ampere; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
} TurnableData;
void turnableInit();
#ifdef __cplusplus
}
#endif
#endif // TURNTAABLE_H

12
interface_can.c
View File

@@ -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<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
CAN_Send_Msg(&can_handle_1, 0x121, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
CAN_Send_Msg(&can_handle_2, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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_0, 0x120, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// CAN_Send_Msg(&can_handle_1, 0x121, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// CAN_Send_Msg(&can_handle_2, 0x122, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 17);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// 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_7, 0x127, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, CanData, 8, 16);//BMS<4D><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}