LYL/25RC_ShootBallGame
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

协议更新

Browse Source
This commit is contained in:
ZHAISHUI04 2025-07-05 18:58:37 +08:00
parent 3a2da79a5e
commit 053e2cdea3
21 changed files with 610 additions and 299 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
MDK-ARM/.vscode/c_cpp_properties.json vendored
View File

@ -419,32 +419,32 @@
{
"name": "R2",
"includePath": [
"d:\\Desktop\\R2 _ball_game\\Core\\Inc",
"d:\\Desktop\\R2 _ball_game\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
"d:\\Desktop\\R2 _ball_game\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
"d:\\Desktop\\R2 _ball_game\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
"d:\\Desktop\\R2 _ball_game\\Drivers\\CMSIS\\Include",
"d:\\Desktop\\R2 _ball_game\\User",
"d:\\Desktop\\R2 _ball_game\\User\\bsp",
"d:\\Desktop\\R2 _ball_game\\User\\device",
"d:\\Desktop\\R2 _ball_game\\User\\task",
"d:\\Desktop\\R2 _ball_game\\User\\Algorithm",
"d:\\Desktop\\R2 _ball_game\\User\\Module",
"d:\\Desktop\\R2 _ball_game\\MDK-ARM",
"d:\\Desktop\\R2 _ball_game\\USB_DEVICE\\App",
"d:\\Desktop\\R2 _ball_game\\USB_DEVICE\\Target",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Inc",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Inc",
"d:\\Desktop\\R2 _ball_game\\Drivers\\CMSIS\\Lib\\ARM",
"d:\\Desktop\\R2 _ball_game\\Core\\Src",
"d:\\Desktop\\R2 _ball_game\\Drivers\\STM32F4xx_HAL_Driver\\Src",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Src",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Src"
"d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Inc",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Include",
"d:\\Desktop\\r2\\R2_NEW\\R2\\User",
"d:\\Desktop\\r2\\R2_NEW\\R2\\User\\bsp",
"d:\\Desktop\\r2\\R2_NEW\\R2\\User\\device",
"d:\\Desktop\\r2\\R2_NEW\\R2\\User\\task",
"d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Algorithm",
"d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Module",
"d:\\Desktop\\r2\\R2_NEW\\R2\\MDK-ARM",
"d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\App",
"d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\Target",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Inc",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Inc",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Lib\\ARM",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Src",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Src",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Src",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Src"
],
"defines": [
"USE_HAL_DRIVER",

6
MDK-ARM/.vscode/keil-assistant.log vendored
View File

@ -481,9 +481,7 @@
[info] Log at : 2025/6/28|01:22:24|GMT+0800
[info] Log at : 2025/6/29|03:38:10|GMT+0800
[info] Log at : 2025/6/30|18:03:06|GMT+0800
[info] Log at : 2025/6/30|15:21:11|GMT+0800
[info] Log at : 2025/6/30|18:03:49|GMT+0800
[info] Log at : 2025/7/1|14:01:22|GMT+0800

21
MDK-ARM/R2.uvoptx
View File

@ -158,7 +158,7 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>tar_angle,0x0A</ItemText>
<ItemText>tar_angle</ItemText>
</Ww>
<Ww>
<count>1</count>
@ -173,27 +173,12 @@
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
<ItemText>huart-&gt;ErrorCode</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>qian_Nor_Vx</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>hou_Nor_Vx</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>chassis,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>cmd,0x0A</ItemText>
<ItemText>chassis</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>

4
MDK-ARM/R2.uvprojx
View File

@ -17,8 +17,8 @@
<TargetCommonOption>
<Device>STM32F407IGHx</Device>
<Vendor>STMicroelectronics</Vendor>
<PackID>Keil.STM32F4xx_DFP.3.0.0</PackID>
<PackURL>https://www.keil.com/pack/</PackURL>
<PackID>Keil.STM32F4xx_DFP.2.15.0</PackID>
<PackURL>http://www.keil.com/pack/</PackURL>
<Cpu>IRAM(0x20000000-0x2001BFFF) IRAM2(0x2001C000-0x2001FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4") TZ</Cpu>
<FlashUtilSpec></FlashUtilSpec>
<StartupFile></StartupFile>

BIN
MDK-ARM/R2/R2.axf
View File

Binary file not shown.

100
Middlewares/Third_Party/Protocol/protocol.h vendored Normal file
View File

@ -0,0 +1,100 @@
/*
视觉与电控通信协议
*/
#pragma once
#include <stdint.h>
#include <stdbool.h>
#ifdef __cplusplus
extern "C"
{
#endif
#define NUC_HEAD (0xFF)
#define NUC_END (0xFE)
#define NUC_ID (0x09)
#define NUC_CTRL (0x01)
#define IMU_ID (0x01)
#define CMD_ID (0x02)
/* 电控 -> 视觉 IMU数据结构体*/
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float x;
float y;
float z;
} gyro; /* 陀螺仪数据 */
struct __attribute__((packed))
{
float x;
float y;
float z;
} accl; /* 四元数 */
struct __attribute__((packed))
{
float q0;
float q1;
float q2;
float q3;
} quat; /* 四元数 */
} Protocol_UpDataIMU_t;
/* 电控 -> 视觉 控制数据结构体*/
typedef struct __attribute__((packed))
{
int8_t cmd; /* 控制命令 */
} Protocol_UpDataCMD_t;
/* 视觉 -> 电控 数据结构体*/
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */
float distance; /* 距离（单位：米） */
float angle; /* 角度（单位：弧度） */
bool notice; /* 控制命令 */
} Protocol_DownData_t;
typedef struct __attribute__((packed))
{
uint8_t head;
uint8_t id; /* 数据包ID */
uint8_t ctrl; /* 控制字 */
Protocol_UpDataIMU_t data;
uint8_t end; /* 数据包结束符 */
} Protocol_UpPackageIMU_t;
typedef struct __attribute__((packed))
{
uint8_t head;
uint8_t id;
uint8_t ctrl;
Protocol_UpDataCMD_t data;
uint8_t end;
} Protocol_UpPackageCMD_t;
typedef struct __attribute__((packed))
{
Protocol_DownData_t data;
uint16_t crc16;
} Protocol_DownPackage_t;
#ifdef __cplusplus
}
#endif

43
User/Algorithm/user_math.c
View File

@ -365,7 +365,7 @@ int read_flag_state(uint8_t flag) {
return state;
}
// 归一化函数，将正方形坐标映射到单位圆
void normalize_vector(fp32 x, fp32 y, fp32 *out_x, fp32 *out_y) {
void normalize_vector(double x, double y, double *out_x, double *out_y) {
// 处理原点情况
if (x == 0.0 && y == 0.0) {
*out_x = 0.0;
@ -374,32 +374,16 @@ void normalize_vector(fp32 x, fp32 y, fp32 *out_x, fp32 *out_y) {
}
// 计算最大坐标绝对值和原始模长
const fp32 abs_x = fabs(x);
const fp32 abs_y = fabs(y);
const fp32 s = fmax(abs_x, abs_y); // 最大坐标绝对值
const fp32 r = sqrt(x*x + y*y); // 原始向量模长
const fp32 scale = s / r; // 缩放因子
const double abs_x = fabs(x);
const double abs_y = fabs(y);
const double s = fmax(abs_x, abs_y); // 最大坐标绝对值
const double r = sqrt(x*x + y*y); // 原始向量模长
const double scale = s / r; // 缩放因子
// 应用缩放并保持方向
*out_x = x * scale;
*out_y = y * scale;
}
/**
* @brief 底盘无头模式转换函数
* @param vx 输入 X 方向速度（全局坐标系），输出转换后的底盘 X 速度
* @param vy 输入 Y 方向速度（全局坐标系），输出转换后的底盘 Y 速度
* @param vw 旋转速度（直接控制，不受影响）
* @param yaw 当前陀螺仪 yaw 角度（弧度制）
*/
void ChassisHeadlessMode(float *vx, float *vy, float yaw) {
float vx_global = *vx;
float vy_global = *vy;
// 标准二维旋转变换（逆时针为正）
*vx = vx_global * cosf(yaw) - vy_global * sinf(yaw);
*vy = vx_global * sinf(yaw) + vy_global * cosf(yaw);
}
bool is_reached(float current, float target, float mistake) {
return fabs(current - target) < mistake;
}
@ -421,4 +405,19 @@ bool is_reached_multiple(float current1, float current2, float current3, float t
count++; // 所有条件都满足，计数加 1
}
return false; // 未满足条件达到阈值，返回 0
}
/**
* @brief 底盘无头模式转换函数
* @param vx 输入 X 方向速度（全局坐标系），输出转换后的底盘 X 速度
* @param vy 输入 Y 方向速度（全局坐标系），输出转换后的底盘 Y 速度
* @param vw 旋转速度（直接控制，不受影响）
* @param yaw 当前陀螺仪 yaw 角度（弧度制）
*/
void ChassisHeadlessMode(double *vx, double *vy, double yaw) {
double vx_global = *vx;
double vy_global = *vy;
// 标准二维旋转变换（逆时针为正）
*vx = vx_global * cosf(yaw) - vy_global * sinf(yaw);
*vy = vx_global * sinf(yaw) + vy_global * cosf(yaw);
}

4
User/Algorithm/user_math.h
View File

@ -161,10 +161,10 @@ int abs_value(int num);
float expo_map(float value, float expo_factor) ;
int read_flag_state(uint8_t flag) ;
void normalize_vector(fp32 x, fp32 y, fp32 *out_x, fp32 *out_y) ;
void normalize_vector(double x, double y, double *out_x, double *out_y) ;
bool is_reached(float current, float target, float mistake) ;
bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ;
fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max);
void ChassisHeadlessMode(float *vx, float *vy, float yaw) ;
void ChassisHeadlessMode(double *vx, double *vy, double yaw) ;
#endif

201
User/Module/Chassis.c
View File

@ -3,7 +3,6 @@
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
/*
机器人坐标系，向前x，右y，上yaw
不同于nuc (前x，左y，上yaw)
@ -11,9 +10,7 @@
|
--y
*/
static osThreadId_t thread_alert;
static int8_t Chassis_SetCtrl(Chassis_t *c, CMD_t *ctrl) {
if (c == NULL) return CHASSIS_ERR_NULL;
if (ctrl->CMD_CtrlType == c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode)
@ -22,14 +19,11 @@ static int8_t Chassis_SetCtrl(Chassis_t *c, CMD_t *ctrl) {
c->chassis_ctrl.mode = ctrl->CMD_mode;
return CHASSIS_OK;
}
//int8_t ShootGame_FlagSet(void)
//{
// osThreadFlagsSet(thread_alert, SIGNAL_NUC_RAW_REDY);
//
//}
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
if (c == NULL) return CHASSIS_ERR_NULL;
if (can == NULL) return CHASSIS_ERR_NULL;
for (uint8_t i = 0; i < 4; i++) {
c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed;
c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current;
@ -39,19 +33,19 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
// c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
// }
c->sick_cali.sick_dis[0]=(fp32)(can->sickfed.raw_dis[0]) ;
c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+26) ; //有点误差，手动补偿
c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+30) ; //有点误差，手动补偿
c->sick_cali.sick_dis[2]=(fp32)(can->sickfed.raw_dis[2] );
// c->vofa_send[7] =c->sick_cali.sick_dis[0]-c->sick_cali.sick_dis[1];
// c->vofa_send[6] = c->sick_cali.sick_dis[0];
// c->vofa_send[5] =c->sick_cali.sick_dis[1];
// c->vofa_send[4] =c->sick_cali.sick_dis[2];
c->vofa_send[7] =c->sick_cali.sick_dis[1]-c->sick_cali.sick_dis[2];
c->vofa_send[6] = c->sick_cali.sick_dis[0];
c->vofa_send[5] =c->sick_cali.sick_dis[1];
c->vofa_send[4] =c->sick_cali.sick_dis[2];
return CHASSIS_OK;
}
int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_freq) {
if (c == NULL) return CHASSIS_ERR_NULL;
if ((thread_alert = osThreadGetId()) == NULL)
if ((thread_alert = osThreadGetId()) == NULL)
return CHASSIS_ERR_NULL;
c->param = param;
@ -71,43 +65,33 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param));
LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); // y滤波
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 50.0f);
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[4]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[5]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[6]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[7]), target_freq, 80.0f);
KalmanCreate(&c->extKalman[0],10,1);
KalmanCreate(&c->extKalman[1],10,1);
KalmanCreate(&c->extKalman[2],10,1);
c->sick_cali .sickparam=c->param ->sickparam ;
c->ang_cail.is_open=1;
return CHASSIS_OK;
}
fp32 pian_yaw;
fp32 qian_Nor_Vx;
fp32 hou_Nor_Vx;
void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
fp32 Nor_Vx, Nor_Vy;
normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy); //速度归一，防止斜跑速度最快
qian_Nor_Vx=Nor_Vx;
ChassisHeadlessMode(&Nor_Vx,&Nor_Vy,c->pos088.imu_eulr.yaw);
hou_Nor_Vx=Nor_Vx;
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+pian_yaw; // 右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+pian_yaw; // 右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +pian_yaw; // 左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +pian_yaw; // 左前
fp64 Nor_Vx, Nor_Vy;
normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
ChassisHeadlessMode(&Nor_Vx,&Nor_Vy,c->pos088.imu_eulr.yaw);
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out; // 左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +c->ang_cail.out; // 左前
Chassis_AngleCompensate(c);
Chassis_AngleCompensate(c);
}
int i=0;
int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
if (c == NULL) return CHASSIS_ERR_NULL;
if (ctrl == NULL) return CHASSIS_ERR_NULL;
@ -118,12 +102,17 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
/*初始数据*/
// c->NUC_send.send[0] = 0;
// c->sick_cali.is_reach = 0;
c->NUC_send.send[0] = 0;
c->sick_cali.is_reach = 0;
c->vofa_send[0]= KalmanFilter(&c->extKalman[0] , c->sick_cali.sick_dis[0]);
c->vofa_send[1]= KalmanFilter(&c->extKalman[1] , c->sick_cali.sick_dis[1]);
c->vofa_send[2]= KalmanFilter(&c->extKalman[2] , c->sick_cali.sick_dis[2]);
switch (c->chassis_ctrl.ctrl) {
case ShootGame_Mode: //投球赛模式
i=(osThreadFlagsWait(SHOOT_GAME_FLAG, osFlagsWaitAny, 0)==SHOOT_GAME_FLAG);
if (ctrl->Vw != 0 || ctrl->Vy != 0 || ctrl->Vx != 0) {
// 有遥控器输出，设置标志位1
osThreadFlagsSet(thread_alert, SHOOT_GAME_FLAG);
@ -148,23 +137,74 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
}
break;
case RCcontrol:
c->move_vec.Vw = ctrl->Vw * 8000;
c->move_vec.Vx = ctrl->Vy * 8000;
c->move_vec.Vy = ctrl->Vx * 8000;
break;
default:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break;
case AUTO:
switch (c->chassis_ctrl.mode) {
case AUTO_MID360:
c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
// c->NUC_send.send[0] = 1;
break;
case AUTO_MID360_Pitch:
c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break;
case AUTO_MID360_Adjust:
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
break;
default:
break;
}
case PASS_BALL:
switch (c->chassis_ctrl.mode)
{
case PB_UP:
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
break ;
case PB_MID:
case PB_DOWN:
c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break ;
}
break;
case RELAXED:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break ;
break;
break;
default:
break;
}
// 电机速度限幅
abs_limit_fp(&c->move_vec.Vx, 6000.0f);
@ -204,26 +244,35 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
if (ctrl == NULL) return CHASSIS_ERR_NULL;
fp32 delta_w,delta_x,delta_y;
fp32 sick0 =LowPassFilter2p_Apply( &(c->filled[5]),c->sick_cali.sick_dis[0]) ;
fp32 sick1 =LowPassFilter2p_Apply( &(c->filled[6]), c->sick_cali.sick_dis[1]) ;
fp32 sick2 = LowPassFilter2p_Apply( &(c->filled[7]),c->sick_cali.sick_dis[2] );
fp32 sick0 = c->sick_cali.sick_dis[0];
fp32 sick1 = c->sick_cali.sick_dis[1];
fp32 sick2 = c->sick_cali.sick_dis[2];
const sickparam_t *param = &c->sick_cali.sickparam;
diff_yaw = -(fp32)(sick1 - sick2);
diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set);
diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set);
diff_yaw =LowPassFilter2p_Apply(&(c->filled[1]), -(fp32)(sick1 +10 - sick2));
diff_y = LowPassFilter2p_Apply(&(c->filled[2]),-(fp32)(sick1 - c->sick_cali.sickparam.y_set));
diff_x = LowPassFilter2p_Apply(&(c->filled[3]),(fp32)(sick0 - c->sick_cali.sickparam.x_set));
delta_w=PID_calc(&c->pid.SickCaliWzOutPID, diff_yaw, 0);
delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0);
delta_y=PID_calc(&c->pid.SickCaliVyOutPID, diff_y,0);
if(fabs(diff_yaw)>5){
if(fabs(diff_yaw)>param->w_error){
c->move_vec.Vw=PID_calc(&c->pid.SickCaliWzInPID,-delta_w,0);
// c->move_vec.Vw=delta_w;
// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVxInPID,delta_x,0);
// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVyInPID,delta_y,0);
}
// if(fabs(diff_y)>param->xy_error){
//
// c->move_vec.Vy=PID_calc(&c->pid.SickCaliVyInPID,delta_y,0);
// }
// if(fabs(diff_x)>param->xy_error){
//
// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVxInPID,-delta_x,0);
// }
static uint16_t reach_cnt = 0;
@ -247,17 +296,21 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
int8_t Chassis_AngleCompensate(Chassis_t *c)
{
if (c == NULL) return CHASSIS_ERR_NULL;
if(c->ang_cail.is_open==0) return 0;
static fp32 pian_angel,cur_angle;
if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0))
{
pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
c->ang_cail.ang_error=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
}
else {
cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
pian_angel=0;
c->ang_cail.ang_cur=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
c->ang_cail.ang_error=0;
}
pian_yaw = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0);
c->ang_cail.out = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0);
}

19
User/Module/Chassis.h
View File

@ -31,7 +31,8 @@
#include "can_use.h"
#include "cmd.h"
#include "filter.h"
#include <cmsis_os2.h>
#include "kalman.h"
#define CHASSIS_OK (0)
#define CHASSIS_ERR (-1)
#define CHASSIS_ERR_NULL (-2)
@ -172,11 +173,23 @@ typedef struct{
}pid;
fp32 vofa_send[8];
/*卡尔曼滤波*/
extKalman_t extKalman[3];
LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */
/*角度偏转修正 */
struct{
int is_open;
fp32 ang_error;
fp32 ang_cur;
fp32 out;
}ang_cail;
struct {
int32_t sick_dis[4]; //获取到的sick激光值
fp32 sick_dis[4]; //获取到的sick激光值
sickparam_t sickparam;
int is_reach;
}sick_cali;

87
User/Module/config.c
View File

@ -80,7 +80,7 @@ static const ConfigParam_t param ={
.go_pull_pos = -214.0f, // go上升去合并扳机，扳机位置
.go_up_speed = 12.0f, // 上升速度
.go_down_speed = 6.0f, // 下降速度
.Pitch_angle = 66, //俯仰角
.Pitch_angle = 58, //俯仰角
.go_init = -50
},
.PitchCfg = {
@ -100,7 +100,7 @@ static const ConfigParam_t param ={
.MID360Cfg={
.go_release_pos=-200, // GO电机发射位置
.go_up_speed=15, // GO电机上升速度
.go_up_speed=20, // GO电机上升速度
.go_down_speed=10, // GO电机下降速度
@ -133,48 +133,99 @@ static const ConfigParam_t param ={
},
// .SickCali_WInparam = {
// .p = 3.0f,
// .i = 0.000f,
// .d = 0.0f,
// .i_limit = 500.0f,
// .out_limit = 2000.0f,
// },
// .SickCali_WOutparam = {
// .p = 2.6f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 1000.0f,
// },
// .SickCali_YInparam = {
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 5000.0f,
// },
// .SickCali_YOutparam = {
// .p = 4.5f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 00.0f,
// .out_limit = 1000.0f,
// },
// .SickCali_XInparam = {
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 5000.0f,
// },
// .SickCali_XOutparam = {
// .p = 4.5f,
// .i = 0.00f,
// .d = 0.0f,
// .i_limit = 500.0f,
// .out_limit = 2000.0f,
// },
.SickCali_WInparam = {
.p = 3.0f,
.i = 0.005f,
.i = 0.000f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit = 2000.0f,
},
.SickCali_WOutparam = {
.p = 18.0f,
.p = 2.6f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 1000.0f,
},
.SickCali_YInparam = {
.p = 0.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 0.0f,
.out_limit = 5000.0f,
},
.SickCali_YOutparam = {
.p = 2.9f,
.i = 0.005f,
.p = 4.5f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 500.0f,
.i_limit = 00.0f,
.out_limit = 1000.0f,
},
.SickCali_XInparam = {
.p = 0.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 0.0f,
.out_limit = 5000.0f,
},
.SickCali_XOutparam = {
.p = 2.9f,
.i = 0.0065f,
.p = 4.5f,
.i = 0.00f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit = 3000.0f,
.out_limit = 2000.0f,
},
.Chassis_AngleAdjust_param={
.p = 10.0f,
.i = 0.0f,
@ -184,10 +235,10 @@ static const ConfigParam_t param ={
},
.sickparam={
.w_error=5,
.xy_error=5,
.x_set=600,
.y_set=100,
.w_error=2,
.xy_error=3,
.x_set=10000,
.y_set=2000,
},

87
User/Module/up.c
View File

@ -40,7 +40,6 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param ));
u->go_cmd =u->param ->go_cmd ;
LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);
@ -119,7 +118,7 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
u->final_out .final_VESC_5065_M1out =-u->motor_target .VESC_5065_M1_rpm;
u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm;
return 0;
return 0;
}
@ -213,7 +212,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
break ;
//
}
return 0;
@ -224,26 +223,32 @@ return 0;
//复用发射，
int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){
/*电机位置到达判断*/
bool is_GoStartReach=is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f); //go开始位置
bool is_GoSpeedReach=is_reached(u->motorfeedback.go_data.W,0,1.0f) ; //go速度归零判断
bool is_GoEndReach =(u->motorfeedback .go_data .Pos < -209); //go去上拉钩子位置，判断到达
bool is_HookDone=(u->motorfeedback .DJmotor_feedback [4].total_angle>-10); //2006钩子放下判断
bool is_Shoot=(u->motorfeedback.DJmotor_feedback[4].total_angle<-10);//是否发射判断
switch(LaunchContext->LaunchState){
case Launch_Stop: break;
case Launch_PREPARE:
u->motor_target.go_shoot = StartPos;
if(is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f)&&
is_reached(u->motorfeedback.go_data.W,0,1.0f)){
if(is_GoStartReach&& is_GoSpeedReach){
//根据位置和速度判断是否到达初始位置
u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_init ;
if(u->motorfeedback .DJmotor_feedback [4].total_angle<-70){
LaunchContext->LaunchState = Launch_START;
}
}break;
case Launch_START:
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_up_speed;
u->motor_target.go_shoot = u->LaunchContext.LaunchCfg.go_pull_pos;
if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
if(is_GoEndReach){ //检测go位置到达最上面，这里的检测条件可以更改
u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度，关闭
LaunchContext->LaunchState = Launch_TRIGGER;
@ -251,19 +256,18 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP
case Launch_TRIGGER:
if( u->motorfeedback .DJmotor_feedback [4].total_angle>-5){ //当2006的总角度小于5，可以认为已经勾上,误差为1
if( is_HookDone ){ //当2006的总角度小于1，可以认为已经勾上,误差为1
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_down_speed;
u->motor_target.go_shoot = EndPos ;
}
break;
// if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f))
// LaunchContext->LaunchState = Launch_SHOOT_WAIT;
} break;
case Launch_SHOOT_WAIT:
if(u->motorfeedback.DJmotor_feedback[4].total_angle<-1) //认为发射
if(is_Shoot) //认为发射
LaunchContext->LaunchState = Launch_DONE;
break;
case Launch_DONE:
break ;
default:break;
}
}
@ -319,12 +323,10 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
Pass_Sequence_Check(u,c);
if(c->pos) //
{
PassCfg ->go_release_pos =
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PassContext.Curve);
}
switch (*state) { //遥控器按键进行状态切换
case PASS_STOP:
@ -334,13 +336,13 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
break;
case PASS_PREPARE:
target->go_pull_speed=PassCfg->go_up_speed;
Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out);
break;
case PASS_START:
if(LaunchContext->LaunchState == Launch_SHOOT_WAIT){
if(LaunchContext->LaunchState == Launch_TRIGGER){
target->go_pull_speed=PassCfg->go_down_speed;
target->go_shoot = PassCfg->go_release_pos ;
}
@ -362,45 +364,36 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
LaunchContext_t *LaunchContext = &u->LaunchContext;
MID360Context_t *MID360Context=&u->MID360Context;
MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg;
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)+0.15f,3.2,4.3,&u->MID360Context.Curve);
/* 函数，角度更新*/
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.05,3.2,4.3,&u->MID360Context.Curve);
if (u->MID360Context.Curve == CURVE_58) {
target->Pitch_angle = 58;
} else {
target->Pitch_angle = 66;
/* 上下速度更改*/ }
}
LaunchContext->LaunchCfg.go_up_speed=MID360Cfg->go_up_speed;
LaunchContext->LaunchCfg.go_down_speed=MID360Cfg->go_down_speed;
/*检测重置*/
if(MID360Context->IsLaunch==0){
switch(c-> CMD_mode)
{
case AUTO_MID360_Pitch:
if(MID360Context->IsLaunch==0){
MID360Context->IsLaunch=1;
LaunchContext->LaunchState = Launch_PREPARE;
}
/*运行*/
Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out);
switch(c-> CMD_mode)
{
case Shooting://发射
target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;
break;
case ShootRst://重置发射
MID360Context->IsLaunch=0;
break ;
default:
case AUTO_MID360:
target->Shoot_M2006_angle= LaunchContext->LaunchCfg.m2006_init;
MID360Context->IsLaunch=0;
break ;
default:
break;
}
return 1;
}
}

47
User/Module/up_utils.c
View File

@ -1,32 +1,35 @@
#include "up_utils.h"
#include "up.h"
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle)
int8_t DJ_processdata(DJmotor_feedback_t *f, fp32 ecd_to_angle)
{
int8_t cnt=0;
fp32 angle ,delta;
angle = f->ecd;
fp32 angle, delta;
angle = f->ecd;
// 初始化阶段，记录 offset
if (f->init_cnt < 50) {
f->orig_angle= angle;
f->last_angle = angle;
f->init_cnt++;
return 0;
f->offset_ecd = (uint16_t)angle; // 记录初始偏移
f->orig_angle = angle - f->offset_ecd; // orig_angle 归零
f->last_angle = angle - f->offset_ecd;
f->init_cnt++;
return 0;
}
delta = angle - f->last_angle;
// 使用 offset 修正
angle = angle - f->offset_ecd;
delta = angle - f->last_angle;
if (delta > 4096) {
f->round_cnt--;
} else if (delta < -4096) {
f->round_cnt++;
}
f->last_angle = angle;
f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
f->last_angle = angle;
f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
}
/*go电机控制*/
fp32 a_pos;
int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, float limit)
{
@ -44,7 +47,7 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
// 计算输出力矩 tau
float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);
a_pos=pos;
/*限制最大输入来限制最大输出*/
if (pos - q_current > limit) {
go_cmd->Pos = q_current + limit; // 限制位置
@ -65,13 +68,13 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
// 计算66度曲线（偏上）
// 计算66度曲线（偏上）
static float curve_66(float d) {
return 4.0310f * d * d + 8.9026f * d -139.5156;
}
return 3.7028f * d * d + 11.2126f * d -142.9446f;
}
// 计算58度曲线（偏下）
// 计算58度曲线（偏下）
static float curve_58(float d) {
return -1.9776f * d * d + 42.8499f * d - 204.2442f;
return 0.9242f * d * d + 19.4246f * d - 154.9055f;
}
/*
@ -79,10 +82,10 @@ static float curve_58(float d) {
迟滞区x-y
曲线x重合区，根据当前函数和变化方向切换
*/
int abdddd=0;
float CurveChange(float d, float x, float y, CurveType *cs)
{
if (d<3.2) abdddd++;
if (*cs == CURVE_66) {
if (d > y) {
*cs = CURVE_58;

2
User/Module/up_utils.h
View File

@ -33,6 +33,8 @@ typedef struct
int32_t round_cnt;
int init_cnt;
fp32 total_angle;
uint16_t offset_ecd;
uint32_t msg_cnt;
}DJmotor_feedback_t;

37
User/bsp/protocol.h
View File

@ -14,6 +14,11 @@ extern "C" {
#define HEAD (0xFF)
#define TAIL (0xFE)
#define IMU_ID (0x01)
#define CMD_ID (0x02)
#define TYPE (0x09)
#define NAVI (0x05)
#define PICK (0x06)
@ -49,6 +54,38 @@ typedef struct __attribute__((packed)) {
} Protocol_DownDataChassis_t;
/* 电控 -> 视觉 IMU数据结构体*/
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float x;
float y;
float z;
} gyro; /* 陀螺仪数据 */
struct __attribute__((packed))
{
float x;
float y;
float z;
} accl; /* 四元数 */
struct __attribute__((packed))
{
float q0;
float q1;
float q2;
float q3;
} quat; /* 四元数 */
} Protocol_UpDataIMU_t;
typedef struct __attribute__((packed))
{
uint8_t status;
} Protocol_UpDataCMD_t;
/* 视觉 -> 电控 上层机构数据结构体*/
typedef struct __attribute__((packed)) {

77
User/device/nuc.c
View File

@ -40,33 +40,6 @@ int8_t NUC_StartReceiving()
return DEVICE_OK;
return DEVICE_ERR;
}
int8_t NUC_StartSending(fp32 *data)
{
union
{
float x[1];
char data[4];
} instance;
// for (int i = 0; i < 1; i++) {
instance.x[0] = data[0];
// }
SendBuffer[0] = 0xFC; // 帧头
SendBuffer[1] = 0x01; // 控制帧
for (int i = 2; i < 6; i++)
{
SendBuffer[i] = instance.data[i - 2];
}
SendBuffer[6] = 0xFD; // 帧尾
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
(uint8_t *)SendBuffer,
sizeof(SendBuffer)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
int8_t NUC_Restart(void)
{
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC));
@ -75,11 +48,11 @@ int8_t NUC_Restart(void)
}
bool_t NUC_WaitDmaCplt(void)
{
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 20) ==
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 0) ==
SIGNAL_NUC_RAW_REDY);
}
int8_t NUC_RawParse(CMD_NUC_t *n)
int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc)
{
if (n == NULL)
return DEVICE_ERR_NULL;
@ -165,6 +138,7 @@ int8_t NUC_RawParse(CMD_NUC_t *n)
break;
}
nuc->unc_online = true; // 设置为在线状态
return DEVICE_OK;
}
error:
@ -173,10 +147,51 @@ error:
return DEVICE_ERR;
}
int8_t NUC_HandleOffline(CMD_NUC_t *cmd)
int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc)
{
if (cmd == NULL)
return DEVICE_ERR_NULL;
// memset(cmd, 0, sizeof(*cmd));
nuc->unc_online = false; // 设置为离线状态
memset(cmd, 0, sizeof(*cmd));
return 0;
}
int8_t NUC_PackIMU(NUC_t *nuc, const AHRS_Quaternion_t *quat, const AHRS_Accl_t *accl, const AHRS_Gyro_t *gyro){
nuc->to_nuc.imu.head = HEAD;
nuc->to_nuc.imu.id = IMU_ID;
nuc->to_nuc.imu.type = TYPE;
nuc->to_nuc.imu.end = TAIL;
memcpy((void *)&(nuc->to_nuc.imu.package.quat), (const void *)quat,sizeof(*quat));
memcpy((void *)&(nuc->to_nuc.imu.package.gyro), (const void *)gyro,sizeof(*gyro));
memcpy((void *)&(nuc->to_nuc.imu.package.accl), (const void *)accl,sizeof(*accl));
return DEVICE_OK;
}
int8_t NUC_PackCMD(NUC_t *nuc, const NUC_send_t *send){
nuc->to_nuc.cmd.head = HEAD;
nuc->to_nuc.cmd.id = IMU_ID;
nuc->to_nuc.cmd.type = TYPE;
nuc->to_nuc.cmd.end = TAIL;
// memcpy((void *)&(nuc->to_nuc.cmd.package.status), (const void *)send,sizeof(*send));
/*在这里添加你们的控制命令*/
return DEVICE_OK;
}
int8_t NUC_StartSend(NUC_t *nuc, bool cmd_update){
if (cmd_update) {
if (HAL_UART_Transmit_DMA(
BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)&(nuc->to_nuc.cmd),
sizeof(nuc->to_nuc.cmd) + sizeof(nuc->to_nuc.imu)) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
} else {
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
(uint8_t *)&(nuc->to_nuc.imu),
sizeof(nuc->to_nuc.imu)) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
}
}

39
User/device/nuc.h
View File

@ -6,8 +6,8 @@
#include "bsp_usart.h"
#include "cmd.h"
#include "protocol.h"
#include "Algorithm/ahrs.h"
#include "Module/Chassis.h"
typedef struct {
@ -16,11 +16,33 @@ typedef struct {
} NUC_UpPackageMCU_t;
typedef struct {
// osThreadId_t thread_alert;
uint8_t head;
uint8_t type;
uint8_t id; // 0x01 IMU帧
Protocol_UpDataIMU_t package;
uint8_t end;
} NUC_UpPackageIMU_t;
typedef struct {
uint8_t head;
uint8_t type; // 0x01 控制帧
uint8_t id;
Protocol_UpDataCMD_t package;
uint8_t end;
} NUC_UpPackageCMD_t;
typedef struct {
// osThreadId_t thread_alert;
bool unc_online; //是否在线
Protocol_DownPackageChassis_t Nucfor_chassis; //接收的数据协议
NUC_UpPackageMCU_t to_nuc; //发送的数据协议
// NUC_UpPackageMCU_t to_nuc; //发送的数据协议
struct {
NUC_UpPackageIMU_t imu;
NUC_UpPackageCMD_t cmd;
} to_nuc;
uint8_t status;//控制状态
@ -29,11 +51,14 @@ typedef struct {
int8_t NUC_Init(NUC_t *nuc);
int8_t NUC_StartReceiving(void);
int8_t NUC_StartSending(fp32 *data) ;
bool_t NUC_WaitDmaCplt(void);
int8_t NUC_RawParse(CMD_NUC_t *n);
int8_t NUC_HandleOffline(CMD_NUC_t *cmd);
int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc);
int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc);
int8_t NUC_Restart(void);
int8_t NUC_PackIMU(NUC_t *nuc, const AHRS_Quaternion_t *quat, const AHRS_Accl_t *accl, const AHRS_Gyro_t *gyro);
int8_t NUC_PackCMD(NUC_t *nuc, const NUC_send_t *send);
int8_t NUC_StartSend(NUC_t *nuc, bool cmd_update);
#endif

10
User/task/atti_esti.c
View File

@ -119,7 +119,7 @@ void Task_AttiEsti(void *argument) {
AHRS_GetEulr(&imu_eulr, &gimbal_ahrs);
detect_hook(IMU_TOE);
osKernelUnlock();
osMessageQueueReset(task_runtime.msgq.imu.accl);
osMessageQueuePut(task_runtime.msgq.imu.accl, &bmi088.accl, 0, 0);
@ -128,7 +128,13 @@ void Task_AttiEsti(void *argument) {
osMessageQueueReset(task_runtime.msgq.imu.eulr);
osMessageQueuePut(task_runtime.msgq.imu.eulr, &imu_eulr, 0, 0);
osKernelUnlock();
osMessageQueueReset(task_runtime.msgq.nuc.gyro);
osMessageQueuePut(task_runtime.msgq.nuc.gyro, &bmi088.gyro, 0, 0);
osMessageQueueReset(task_runtime.msgq.nuc.accl);
osMessageQueuePut(task_runtime.msgq.nuc.accl, &bmi088.accl, 0, 0);
osMessageQueueReset(task_runtime.msgq.nuc.quat);
osMessageQueuePut(task_runtime.msgq.nuc.quat, &gimbal_ahrs.quat, 0, 0);
/* PID控制IMU温度，PWM输出 */
BSP_PWM_Set(BSP_PWM_IMU_HEAT,

7
User/task/init.c
View File

@ -64,6 +64,13 @@ void Task_Init(void *argument) {
task_runtime.msgq.can.output.up_dribble_3508 =
osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL);
/*nuc*/
task_runtime.msgq.nuc.quat =
osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
task_runtime.msgq.nuc.accl =
osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
task_runtime.msgq.nuc.gyro =
osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
/* imu */

55
User/task/nuc_task.c
View File

@ -5,16 +5,24 @@
NUC_t nuc_raw;
CMD_NUC_t cmd_fromnuc;
AHRS_Quaternion_t quat;
AHRS_Accl_t accl;
AHRS_Gyro_t gyro;
NUC_send_t NUC_send;
#else
static NUC_t nuc_raw;
static CMD_NUC_t cmd_fromnuc;
static AHRS_Quaternion_t quat;
static AHRS_Accl_t accl;
static AHRS_Gyro_t gyro;
NUC_send_t NUC_send;
#endif
fp32 send[4]={11.0f,45.0,1.f,4.0f};
int a=0;
void Task_nuc(void *argument){
(void)argument; /**/
@ -24,36 +32,47 @@ void Task_nuc(void *argument){
NUC_Init(&nuc_raw);
uint32_t tick = osKernelGetTickCount();
uint32_t last_online_tick = tick;
while (1)
{
#ifdef DEBUG
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
task_runtime .freq.nuc = TASK_FREQ_NUC;
task_runtime.last_up_time.nuc= tick;
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
#endif
a++;
NUC_StartReceiving();
// NUC_StartSending(NUC_send.send);
// NUC_StartSending (send);
if (NUC_WaitDmaCplt()){
NUC_RawParse(&cmd_fromnuc);
NUC_RawParse(&cmd_fromnuc, &nuc_raw);
last_online_tick = tick;
}
else{
NUC_HandleOffline(&cmd_fromnuc);
if (tick - last_online_tick > 300) NUC_HandleOffline(&cmd_fromnuc, &nuc_raw);
}
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0);
osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0);
if (nuc_raw.unc_online) {
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0);
}
osMessageQueueGet(task_runtime.msgq.nuc.quat, &(quat), NULL, 0);
osMessageQueueGet(task_runtime.msgq.nuc.accl, &(accl), NULL, 0);
osMessageQueueGet(task_runtime.msgq.nuc.gyro, &(gyro), NULL, 0);
// osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0);
bool cmd_update = (osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send,
&(NUC_send), NULL, 0) == osOK);
NUC_PackIMU(&nuc_raw, &quat, &accl, &gyro);
if (cmd_update) {
NUC_PackCMD(&nuc_raw,&NUC_send);
}
// NUC_StartSend(&nuc_raw, cmd_update);
tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
osDelayUntil(tick);
}
}

11
User/task/user_task.h
View File

@ -18,7 +18,7 @@
#define TASK_FREQ_UP (900u)
#define TASK_FREQ_CTRL_CMD (500u)
#define TASK_FREQ_NUC (500u)
#define TASK_FREQ_NUC (250u)
#define TASK_FREQ_CAN (1000u)
#define TASK_FREQ_RC (1000u)
@ -54,15 +54,20 @@ typedef struct {
osMessageQueueId_t eulr; /* 姿态结算得到欧拉角 */
} imu;
/* 控制指令 */
struct {
osMessageQueueId_t quat; /* 姿态解算得到 */
osMessageQueueId_t accl; /* IMU读取 */
osMessageQueueId_t gyro; /* IMU读取 */
} nuc;
struct {
struct {
osMessageQueueId_t rc;
osMessageQueueId_t nuc; //
osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
osMessageQueueId_t nuc_send; //给nuc发
}raw;
/*控制分离*/
osMessageQueueId_t UP_cmd_ctrl_t;
osMessageQueueId_t CHASSIS_cmd_ctrl_t;