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

Compare commits

base: LYL:117bdf6d12871f91bfa8e709ba9f3f9659e492b6
Branches Tags
LYL:main
..
compare: LYL:3a2da79a5e78e047876f18acb764e8fae3e6ae6a
Branches Tags
LYL:main

No commits in common. "117bdf6d12871f91bfa8e709ba9f3f9659e492b6" and "3a2da79a5e78e047876f18acb764e8fae3e6ae6a" have entirely different histories.
117bdf6d12 ... 3a2da79a5e

24 changed files with 313 additions and 673 deletions
Show Stats 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\\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"
"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"
],
"defines": [
"USE_HAL_DRIVER",

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

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

22
MDK-ARM/R2.uvoptx
View File

@ -158,7 +158,7 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>tar_angle</ItemText>
<ItemText>tar_angle,0x0A</ItemText>
</Ww>
<Ww>
<count>1</count>
@ -173,37 +173,27 @@
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>huart-&gt;ErrorCode</ItemText>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>chassis</ItemText>
<ItemText>qian_Nor_Vx</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
<ItemText>hou_Nor_Vx</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>SendBuffer</ItemText>
<ItemText>chassis,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>NUC_send</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>nuc_raw,0x0A</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>a,0x0A</ItemText>
<ItemText>cmd,0x0A</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.2.15.0</PackID>
<PackURL>http://www.keil.com/pack/</PackURL>
<PackID>Keil.STM32F4xx_DFP.3.0.0</PackID>
<PackURL>https://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
View File

@ -1,100 +0,0 @@
/*
视觉与电控通信协议
*/
#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(double x, double y, double *out_x, double *out_y) {
void normalize_vector(fp32 x, fp32 y, fp32 *out_x, fp32 *out_y) {
// 处理原点情况
if (x == 0.0 && y == 0.0) {
*out_x = 0.0;
@ -374,16 +374,32 @@ void normalize_vector(double x, double y, double *out_x, double *out_y) {
}
// 计算最大坐标绝对值和原始模长
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; // 缩放因子
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; // 缩放因子
// 应用缩放并保持方向
*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;
}
@ -405,19 +421,4 @@ 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(double x, double y, double *out_x, double *out_y) ;
void normalize_vector(fp32 x, fp32 y, fp32 *out_x, fp32 *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(double *vx, double *vy, double yaw) ;
void ChassisHeadlessMode(float *vx, float *vy, float yaw) ;
#endif

150
User/Module/Chassis.c
View File

@ -3,7 +3,6 @@
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
#include "nuc.h"
/*
机器人坐标系，向前x，右y，上yaw
@ -12,7 +11,9 @@
|
--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)
@ -21,11 +22,14 @@ 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;
@ -35,13 +39,13 @@ 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]+30) ; //有点误差，手动补偿
c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+26) ; //有点误差，手动补偿
c->sick_cali.sick_dis[2]=(fp32)(can->sickfed.raw_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];
// 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];
return CHASSIS_OK;
}
@ -67,33 +71,43 @@ 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, 50.0f);
LowPassFilter2p_Init(&(c->filled[2]), 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[3]), target_freq, 80.0f);
KalmanCreate(&c->extKalman[0],10,1);
KalmanCreate(&c->extKalman[1],10,1);
KalmanCreate(&c->extKalman[2],10,1);
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);
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) {
fp64 Nor_Vx, Nor_Vy;
normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
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);
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; // 左前
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; // 左前
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;
@ -104,28 +118,12 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
/*初始数据*/
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]);
// c->NUC_send.send[0] = 0;
// c->sick_cali.is_reach = 0;
switch (c->chassis_ctrl.ctrl) {
case ShootGame_Mode: //投球赛模式
c->to_nuc.send=0;
switch(c->chassis_ctrl.mode){
case ShootRst:
c->move_vec.Vw = ctrl->Vw * 8000;
c->move_vec.Vx = ctrl->Vy * 8000;
c->move_vec.Vy = ctrl->Vx * 8000;
break;
default:
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);
@ -141,40 +139,33 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->move_vec.Vx = ctrl->Vy * 8000;
c->move_vec.Vy = ctrl->Vx * 8000;
}else {
c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
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 REPOSITION:
c->to_nuc.send=1;
break;
case RCcontrol:
case RELAXED:
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 ;
break;
break;
default:
break;
}
// 电机速度限幅
abs_limit_fp(&c->move_vec.Vx, 6000.0f);
abs_limit_fp(&c->move_vec.Vy, 6000.0f);
@ -213,35 +204,26 @@ 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 = c->sick_cali.sick_dis[0];
fp32 sick1 = c->sick_cali.sick_dis[1];
fp32 sick2 = c->sick_cali.sick_dis[2];
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] );
const sickparam_t *param = &c->sick_cali.sickparam;
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));
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);
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)>param->w_error){
if(fabs(diff_yaw)>5){
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;
@ -265,21 +247,17 @@ 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))
{
c->ang_cail.ang_error=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
}
else {
c->ang_cail.ang_cur=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
c->ang_cail.ang_error=0;
cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
pian_angel=0;
}
c->ang_cail.out = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0);
pian_yaw = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0);
}

30
User/Module/Chassis.h
View File

@ -31,9 +31,7 @@
#include "can_use.h"
#include "cmd.h"
#include "filter.h"
#include "kalman.h"
#include "nuc.h"
#include <cmsis_os2.h>
#define CHASSIS_OK (0)
#define CHASSIS_ERR (-1)
#define CHASSIS_ERR_NULL (-2)
@ -104,6 +102,12 @@ typedef struct
fp32 Vw;
fp32 mul;//油门倍率
}ChassisMove_Vec;
typedef struct
{
fp32 send[4];
}NUC_send_t;
/**
* @brief
@ -168,30 +172,16 @@ 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 {
fp32 sick_dis[4]; //获取到的sick激光值
int32_t sick_dis[4]; //获取到的sick激光值
sickparam_t sickparam;
int is_reach;
}sick_cali;
struct {
fp32 send;
}to_nuc;
NUC_send_t NUC_send;
}Chassis_t;

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

49
User/Module/up.c
View File

@ -40,6 +40,7 @@ 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);
@ -192,7 +193,6 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
switch (c->CMD_CtrlType )
{
case ShootGame_Mode:
UP_AUTO_Control(u,out,c);
@ -213,7 +213,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
break ;
//
}
return 0;
@ -224,32 +224,26 @@ 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_GoStartReach&& is_GoSpeedReach){
if(is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f)&&
is_reached(u->motorfeedback.go_data.W,0,1.0f)){
//根据位置和速度判断是否到达初始位置
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(is_GoEndReach){ //检测go位置到达最上面，这里的检测条件可以更改
if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度，关闭
LaunchContext->LaunchState = Launch_TRIGGER;
@ -257,18 +251,19 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP
case Launch_TRIGGER:
if( is_HookDone ){ //当2006的总角度小于1，可以认为已经勾上,误差为1
if( u->motorfeedback .DJmotor_feedback [4].total_angle>-5){ //当2006的总角度小于5，可以认为已经勾上,误差为1
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_down_speed;
u->motor_target.go_shoot = EndPos ;
// if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f))
// LaunchContext->LaunchState = Launch_SHOOT_WAIT;
} break;
}
break;
case Launch_SHOOT_WAIT:
if(is_Shoot) //认为发射
if(u->motorfeedback.DJmotor_feedback[4].total_angle<-1) //认为发射
LaunchContext->LaunchState = Launch_DONE;
break;
default:break;
case Launch_DONE:
break ;
}
}
@ -324,10 +319,12 @@ 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:
@ -343,7 +340,7 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
break;
case PASS_START:
if(LaunchContext->LaunchState == Launch_TRIGGER){
if(LaunchContext->LaunchState == Launch_SHOOT_WAIT){
target->go_pull_speed=PassCfg->go_down_speed;
target->go_shoot = PassCfg->go_release_pos ;
}
@ -365,7 +362,7 @@ 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.25f,3.2,4.3,&u->MID360Context.Curve);
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)+0.15f,3.2,4.3,&u->MID360Context.Curve);
/* 函数，角度更新*/
if (u->MID360Context.Curve == CURVE_58) {
target->Pitch_angle = 58;
@ -390,12 +387,12 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
case Shooting://发射
target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;
break;
case ShootRst://重置发射
MID360Context->IsLaunch=0;
target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;
break ;

29
User/Module/up_utils.c
View File

@ -1,23 +1,19 @@
#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)
{
fp32 angle, delta;
int8_t cnt=0;
fp32 angle ,delta;
angle = f->ecd;
// 初始化阶段，记录 offset
if (f->init_cnt < 50) {
f->offset_ecd = (uint16_t)angle; // 记录初始偏移
f->orig_angle = angle - f->offset_ecd; // orig_angle 归零
f->last_angle = angle - f->offset_ecd;
f->orig_angle= angle;
f->last_angle = angle;
f->init_cnt++;
return 0;
}
// 使用 offset 修正
angle = angle - f->offset_ecd;
delta = angle - f->last_angle;
if (delta > 4096) {
@ -27,9 +23,10 @@ int8_t DJ_processdata(DJmotor_feedback_t *f, fp32 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)
{
@ -47,7 +44,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; // 限制位置
@ -68,13 +65,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 3.7028f * d * d + 11.2126f * d -142.9446f;
}
return 4.0310f * d * d + 8.9026f * d -139.5156;
}
// 计算58度曲线（偏下）
// 计算58度曲线（偏下）
static float curve_58(float d) {
return 0.9242f * d * d + 19.4246f * d - 154.9055f;
return -1.9776f * d * d + 42.8499f * d - 204.2442f;
}
/*
@ -82,10 +79,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,8 +33,6 @@ typedef struct
int32_t round_cnt;
int init_cnt;
fp32 total_angle;
uint16_t offset_ecd;
uint32_t msg_cnt;
}DJmotor_feedback_t;

39
User/bsp/protocol.h
View File

@ -14,13 +14,6 @@ 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)
@ -56,38 +49,6 @@ 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))
{
float status;
} Protocol_UpDataCMD_t;
/* 视觉 -> 电控 上层机构数据结构体*/
typedef struct __attribute__((packed)) {

91
User/device/cmd.c
View File

@ -136,73 +136,40 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
cmd->key_ctrl_l = rc->dr16.sw_l;
cmd->key_ctrl_r = rc->dr16.sw_r ;
// if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) {
// cmd->CMD_CtrlType =RELAXED;
// }
// else if(rc->dr16.sw_l==CMD_SW_UP)
// {
// cmd ->CMD_CtrlType =ShootGame_Mode;
// if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Shooting; //左上，右上，无模式
//
// if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
//
// if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =ShootRst; //左上，右上，无模式
//
//// if(rc->dr16.res > 3000) cmd ->CMD_mode =ShootRst;
//// if(rc->dr16.res == 660) cmd ->CMD_mode =Shooting;
// }
//
// else if(rc->dr16.sw_l==CMD_SW_MID)
// {
//
// cmd ->CMD_CtrlType =RCcontrol;
// if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左中，右中，雷达
//
// if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式
//
// if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左中，右下，视觉
//
// }
// else if(rc->dr16.sw_l==CMD_SW_DOWN)
// {
// cmd ->CMD_CtrlType =RCcontrol;
// if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，无模式
// if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = Normal; //左下，右中，无模式
// if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
// }
if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) {
cmd->CMD_CtrlType =RELAXED;
}
else if(rc->dr16.sw_l==CMD_SW_UP)
{
cmd ->CMD_CtrlType =ShootGame_Mode;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左上，右上，无模式
if ((rc->dr16.sw_r == CMD_SW_ERR) || (rc->dr16.sw_l == CMD_SW_ERR)) {
cmd->CMD_CtrlType = RELAXED;
}
// 右拨杆控制 CtrlType，左拨杆控制 Mode
else if (rc->dr16.sw_r == CMD_SW_UP)
{
cmd->CMD_CtrlType = ShootGame_Mode;
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
if (rc->dr16.sw_l == CMD_SW_UP) cmd->CMD_mode = Normal; // 右上，左上，
if (rc->dr16.sw_l == CMD_SW_MID) cmd->CMD_mode = ShootRst; // 右上，左中，
if (rc->dr16.sw_l == CMD_SW_DOWN) cmd->CMD_mode = Normal; // 右上，左下，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左上，右上，无模式
// 可选保留遥控旋钮控制
// if (rc->dr16.res > 3000) cmd->CMD_mode = ShootRst;
// if (rc->dr16.res == 660) cmd->CMD_mode = Shooting;
}
else if (rc->dr16.sw_r == CMD_SW_MID)
{
cmd->CMD_CtrlType = REPOSITION;
if(rc->dr16.res > 3000) cmd ->CMD_mode =ShootRst;
if(rc->dr16.res == 660) cmd ->CMD_mode =Shooting;
}
if (rc->dr16.sw_l == CMD_SW_UP) cmd->CMD_mode = Normal; // 右中，左上，
if (rc->dr16.sw_l == CMD_SW_MID) cmd->CMD_mode = Normal; // 右中，左中，无模式
if (rc->dr16.sw_l == CMD_SW_DOWN) cmd->CMD_mode = Normal; // 右中，左下，
}
else if (rc->dr16.sw_r == CMD_SW_DOWN)
{
cmd->CMD_CtrlType = RCcontrol;
else if(rc->dr16.sw_l==CMD_SW_MID)
{
if (rc->dr16.sw_l == CMD_SW_UP) cmd->CMD_mode = Normal; // 右下，左上，无模式
if (rc->dr16.sw_l == CMD_SW_MID) cmd->CMD_mode = Normal; // 右下，左中，无模式
if (rc->dr16.sw_l == CMD_SW_DOWN) cmd->CMD_mode = Normal; // 右下，左下，无模式
}
cmd ->CMD_CtrlType =RCcontrol;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左中，右中，雷达
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左中，右下，视觉
}
else if(rc->dr16.sw_l==CMD_SW_DOWN)
{
cmd ->CMD_CtrlType =RCcontrol;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，无模式
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = Normal; //左下，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
}
return 0;

3
User/device/cmd.h
View File

@ -13,10 +13,7 @@
typedef enum{
RCcontrol,//遥控器控制，左按键上，控制上层
REPOSITION, //重定位
AUTO,
PASS_BALL,//传球模式
ShootGame_Mode,//投球赛模式

83
User/device/nuc.c
View File

@ -40,6 +40,33 @@ 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));
@ -48,11 +75,11 @@ int8_t NUC_Restart(void)
}
bool_t NUC_WaitDmaCplt(void)
{
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 0) ==
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 20) ==
SIGNAL_NUC_RAW_REDY);
}
int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc)
int8_t NUC_RawParse(CMD_NUC_t *n)
{
if (n == NULL)
return DEVICE_ERR_NULL;
@ -138,7 +165,6 @@ int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc)
break;
}
nuc->unc_online = true; // 设置为在线状态
return DEVICE_OK;
}
error:
@ -147,57 +173,10 @@ error:
return DEVICE_ERR;
}
int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc)
int8_t NUC_HandleOffline(CMD_NUC_t *cmd)
{
if (cmd == NULL)
return DEVICE_ERR_NULL;
nuc->unc_online = false; // 设置为离线状态
memset(cmd, 0, sizeof(*cmd));
// 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.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)
/*仅发送cmd数据*/
if (HAL_UART_Transmit_DMA(
BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)&(nuc->to_nuc.cmd),
sizeof(nuc->to_nuc.cmd) ) == 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;
// }
}

41
User/device/nuc.h
View File

@ -6,47 +6,21 @@
#include "bsp_usart.h"
#include "cmd.h"
#include "protocol.h"
#include "Algorithm/ahrs.h"
#include "Module/Chassis.h"
typedef struct
{
fp32 send;
}NUC_send_t;
typedef struct {
uint8_t id;
Protocol_UpPackageMCU_t package;
} NUC_UpPackageMCU_t;
typedef struct {
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 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; //发送的数据协议
struct {
NUC_UpPackageIMU_t imu;
NUC_UpPackageCMD_t cmd;
} to_nuc;
NUC_UpPackageMCU_t to_nuc; //发送的数据协议
uint8_t status;//控制状态
@ -55,14 +29,11 @@ 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, NUC_t *nuc);
int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc);
int8_t NUC_RawParse(CMD_NUC_t *n);
int8_t NUC_HandleOffline(CMD_NUC_t *cmd);
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,13 +128,7 @@ void Task_AttiEsti(void *argument) {
osMessageQueueReset(task_runtime.msgq.imu.eulr);
osMessageQueuePut(task_runtime.msgq.imu.eulr, &imu_eulr, 0, 0);
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);
osKernelUnlock();
/* PID控制IMU温度，PWM输出 */
BSP_PWM_Set(BSP_PWM_IMU_HEAT,

2
User/task/chassis_task.c
View File

@ -88,7 +88,7 @@ void Task_Chassis(void *argument)
osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列
osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0); //发送数据
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc_send);//清空队列
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc_send, &chassis.to_nuc, 0, 0); //发送数据
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc_send, &chassis.NUC_send, 0, 0); //发送数据
vofa_send[0] = chassis.vofa_send[0];
vofa_send[1] = chassis.vofa_send[1];
vofa_send[2] = chassis.vofa_send[2];

7
User/task/init.c
View File

@ -64,13 +64,6 @@ 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 */

43
User/task/nuc_task.c
View File

@ -5,24 +5,16 @@
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; /**/
@ -32,47 +24,36 @@ 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;
#endif
a++;
NUC_StartReceiving();
// NUC_StartSending(NUC_send.send);
// NUC_StartSending (send);
if (NUC_WaitDmaCplt()){
NUC_RawParse(&cmd_fromnuc, &nuc_raw);
last_online_tick = tick;
NUC_RawParse(&cmd_fromnuc);
}
else{
if (tick - last_online_tick > 300) NUC_HandleOffline(&cmd_fromnuc, &nuc_raw);
NUC_HandleOffline(&cmd_fromnuc);
}
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.cmd.raw.nuc_send, &NUC_send, NULL, 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);
}
}

9
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 (250u)
#define TASK_FREQ_NUC (500u)
#define TASK_FREQ_CAN (1000u)
#define TASK_FREQ_RC (1000u)
@ -54,18 +54,13 @@ 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;
/*控制分离*/