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

Compare commits

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

2 Commits
3a2da79a5e ... 117bdf6d12

Author SHA1 Message Date
ZHAISHUI04
117bdf6d12 增加重定位，已测试 2025-07-06 03:09:43 +08:00
ZHAISHUI04
053e2cdea3 协议更新 2025-07-05 18:58:37 +08:00
24 changed files with 673 additions and 313 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", "name": "R2",
"includePath": [ "includePath": [
"d:\\Desktop\\R2 _ball_game\\Core\\Inc", "d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Inc",
"d:\\Desktop\\R2 _ball_game\\Drivers\\STM32F4xx_HAL_Driver\\Inc", "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
"d:\\Desktop\\R2 _ball_game\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy", "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include", "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2", "d:\\Desktop\\r2\\R2_NEW\\R2\\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\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
"d:\\Desktop\\R2 _ball_game\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include", "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
"d:\\Desktop\\R2 _ball_game\\Drivers\\CMSIS\\Include", "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Include",
"d:\\Desktop\\R2 _ball_game\\User", "d:\\Desktop\\r2\\R2_NEW\\R2\\User",
"d:\\Desktop\\R2 _ball_game\\User\\bsp", "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\bsp",
"d:\\Desktop\\R2 _ball_game\\User\\device", "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\device",
"d:\\Desktop\\R2 _ball_game\\User\\task", "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\task",
"d:\\Desktop\\R2 _ball_game\\User\\Algorithm", "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Algorithm",
"d:\\Desktop\\R2 _ball_game\\User\\Module", "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Module",
"d:\\Desktop\\R2 _ball_game\\MDK-ARM", "d:\\Desktop\\r2\\R2_NEW\\R2\\MDK-ARM",
"d:\\Desktop\\R2 _ball_game\\USB_DEVICE\\App", "d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\App",
"d:\\Desktop\\R2 _ball_game\\USB_DEVICE\\Target", "d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\Target",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Inc", "d:\\Desktop\\r2\\R2_NEW\\R2\\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\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Inc",
"d:\\Desktop\\R2 _ball_game\\Drivers\\CMSIS\\Lib\\ARM", "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Lib\\ARM",
"d:\\Desktop\\R2 _ball_game\\Core\\Src", "d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Src",
"d:\\Desktop\\R2 _ball_game\\Drivers\\STM32F4xx_HAL_Driver\\Src", "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Src",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source", "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang", "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang",
"d:\\Desktop\\R2 _ball_game\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Src", "d:\\Desktop\\r2\\R2_NEW\\R2\\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\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Src"
], ],
"defines": [ "defines": [
"USE_HAL_DRIVER", "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/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/7/1|14:01:22|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> <Ww>
<count>0</count> <count>0</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>tar_angle,0x0A</ItemText> <ItemText>tar_angle</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>1</count> <count>1</count>
@ -173,27 +173,37 @@
<Ww> <Ww>
<count>3</count> <count>3</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText> <ItemText>huart-&gt;ErrorCode</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>4</count> <count>4</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>qian_Nor_Vx</ItemText> <ItemText>chassis</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>5</count> <count>5</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>hou_Nor_Vx</ItemText> <ItemText>UP,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>6</count> <count>6</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>chassis,0x0A</ItemText> <ItemText>SendBuffer</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>7</count> <count>7</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>cmd,0x0A</ItemText> <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>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>

4
MDK-ARM/R2.uvprojx
View File

@ -17,8 +17,8 @@
<TargetCommonOption> <TargetCommonOption>
<Device>STM32F407IGHx</Device> <Device>STM32F407IGHx</Device>
<Vendor>STMicroelectronics</Vendor> <Vendor>STMicroelectronics</Vendor>
<PackID>Keil.STM32F4xx_DFP.3.0.0</PackID> <PackID>Keil.STM32F4xx_DFP.2.15.0</PackID>
<PackURL>https://www.keil.com/pack/</PackURL> <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> <Cpu>IRAM(0x20000000-0x2001BFFF) IRAM2(0x2001C000-0x2001FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4") TZ</Cpu>
<FlashUtilSpec></FlashUtilSpec> <FlashUtilSpec></FlashUtilSpec>
<StartupFile></StartupFile> <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; 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) { if (x == 0.0 && y == 0.0) {
*out_x = 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 double abs_x = fabs(x);
const fp32 abs_y = fabs(y); const double abs_y = fabs(y);
const fp32 s = fmax(abs_x, abs_y); // 最大坐标绝对值 const double s = fmax(abs_x, abs_y); // 最大坐标绝对值
const fp32 r = sqrt(x*x + y*y); // 原始向量模长 const double r = sqrt(x*x + y*y); // 原始向量模长
const fp32 scale = s / r; // 缩放因子 const double scale = s / r; // 缩放因子
// 应用缩放并保持方向 // 应用缩放并保持方向
*out_x = x * scale; *out_x = x * scale;
*out_y = y * 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) { bool is_reached(float current, float target, float mistake) {
return fabs(current - target) < mistake; return fabs(current - target) < mistake;
} }
@ -421,4 +405,19 @@ bool is_reached_multiple(float current1, float current2, float current3, float t
count++; // 所有条件都满足，计数加 1 count++; // 所有条件都满足，计数加 1
} }
return false; // 未满足条件达到阈值，返回 0 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) ; float expo_map(float value, float expo_factor) ;
int read_flag_state(uint8_t flag) ; 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(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) ; 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); 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 #endif

174
User/Module/Chassis.c
View File

@ -3,6 +3,7 @@
#include "user_math.h" #include "user_math.h"
#include "bsp_buzzer.h" #include "bsp_buzzer.h"
#include "bsp_delay.h" #include "bsp_delay.h"
#include "nuc.h"
/* /*
机器人坐标系，向前x，右y，上yaw 机器人坐标系，向前x，右y，上yaw
@ -11,9 +12,7 @@
| |
--y --y
*/ */
static osThreadId_t thread_alert; static osThreadId_t thread_alert;
static int8_t Chassis_SetCtrl(Chassis_t *c, CMD_t *ctrl) { static int8_t Chassis_SetCtrl(Chassis_t *c, CMD_t *ctrl) {
if (c == NULL) return CHASSIS_ERR_NULL; if (c == NULL) return CHASSIS_ERR_NULL;
if (ctrl->CMD_CtrlType == c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) if (ctrl->CMD_CtrlType == c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode)
@ -22,14 +21,11 @@ static int8_t Chassis_SetCtrl(Chassis_t *c, CMD_t *ctrl) {
c->chassis_ctrl.mode = ctrl->CMD_mode; c->chassis_ctrl.mode = ctrl->CMD_mode;
return CHASSIS_OK; 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) { int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
if (c == NULL) return CHASSIS_ERR_NULL; if (c == NULL) return CHASSIS_ERR_NULL;
if (can == NULL) return CHASSIS_ERR_NULL; if (can == NULL) return CHASSIS_ERR_NULL;
for (uint8_t i = 0; i < 4; i++) { 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_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; c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current;
@ -39,19 +35,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[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
// } // }
c->sick_cali.sick_dis[0]=(fp32)(can->sickfed.raw_dis[0]) ; 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->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[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[6] = c->sick_cali.sick_dis[0];
// c->vofa_send[5] =c->sick_cali.sick_dis[1]; c->vofa_send[5] =c->sick_cali.sick_dis[1];
// c->vofa_send[4] =c->sick_cali.sick_dis[2]; c->vofa_send[4] =c->sick_cali.sick_dis[2];
return CHASSIS_OK; return CHASSIS_OK;
} }
int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_freq) { int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_freq) {
if (c == NULL) return CHASSIS_ERR_NULL; if (c == NULL) return CHASSIS_ERR_NULL;
if ((thread_alert = osThreadGetId()) == NULL) if ((thread_alert = osThreadGetId()) == NULL)
return CHASSIS_ERR_NULL; return CHASSIS_ERR_NULL;
c->param = param; c->param = param;
@ -71,43 +67,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)); 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[0]), target_freq, 80.0f); // 角加速度滤波
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波 LowPassFilter2p_Init(&(c->filled[1]), target_freq, 50.0f);
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); // y滤波 LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[3]), 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); KalmanCreate(&c->extKalman[0],10,1);
LowPassFilter2p_Init(&(c->filled[6]), target_freq, 80.0f); KalmanCreate(&c->extKalman[1],10,1);
LowPassFilter2p_Init(&(c->filled[7]), target_freq, 80.0f); KalmanCreate(&c->extKalman[2],10,1);
c->sick_cali .sickparam=c->param ->sickparam ; c->sick_cali .sickparam=c->param ->sickparam ;
c->ang_cail.is_open=1;
return CHASSIS_OK; 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) { void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
fp64 Nor_Vx, Nor_Vy;
fp32 Nor_Vx, Nor_Vy; normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
ChassisHeadlessMode(&Nor_Vx,&Nor_Vy,c->pos088.imu_eulr.yaw);
normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy); //速度归一，防止斜跑速度最快 c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前
qian_Nor_Vx=Nor_Vx; c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后
ChassisHeadlessMode(&Nor_Vx,&Nor_Vy,c->pos088.imu_eulr.yaw); c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out; // 左后
hou_Nor_Vx=Nor_Vx; c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +c->ang_cail.out; // 左前
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); Chassis_AngleCompensate(c);
} }
int i=0;
int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
if (c == NULL) return CHASSIS_ERR_NULL; if (c == NULL) return CHASSIS_ERR_NULL;
if (ctrl == NULL) return CHASSIS_ERR_NULL; if (ctrl == NULL) return CHASSIS_ERR_NULL;
@ -117,13 +103,29 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z); c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
/*初始数据*/ /*初始数据*/
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) { switch (c->chassis_ctrl.ctrl) {
case ShootGame_Mode: //投球赛模式 case ShootGame_Mode: //投球赛模式
i=(osThreadFlagsWait(SHOOT_GAME_FLAG, osFlagsWaitAny, 0)==SHOOT_GAME_FLAG); 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:
if (ctrl->Vw != 0 || ctrl->Vy != 0 || ctrl->Vx != 0) { if (ctrl->Vw != 0 || ctrl->Vy != 0 || ctrl->Vx != 0) {
// 有遥控器输出，设置标志位1 // 有遥控器输出，设置标志位1
osThreadFlagsSet(thread_alert, SHOOT_GAME_FLAG); osThreadFlagsSet(thread_alert, SHOOT_GAME_FLAG);
@ -139,32 +141,39 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->move_vec.Vx = ctrl->Vy * 8000; c->move_vec.Vx = ctrl->Vy * 8000;
c->move_vec.Vy = ctrl->Vx * 8000; c->move_vec.Vy = ctrl->Vx * 8000;
}else { }else {
c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000; c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000; c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 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.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f); abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f); abs_limit_fp(&c->move_vec.Vw, 5000.0f);
} }
break;
}
case RCcontrol: break;
c->move_vec.Vw = ctrl->Vw * 8000; case REPOSITION:
c->move_vec.Vx = ctrl->Vy * 8000; c->to_nuc.send=1;
c->move_vec.Vy = ctrl->Vx * 8000; break;
case RELAXED:
break; c->move_vec.Vw = 0;
default: c->move_vec.Vx = 0;
c->move_vec.Vw = 0; c->move_vec.Vy = 0;
c->move_vec.Vx = 0; break ;
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.Vx, 6000.0f);
@ -204,26 +213,35 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
if (ctrl == NULL) return CHASSIS_ERR_NULL; if (ctrl == NULL) return CHASSIS_ERR_NULL;
fp32 delta_w,delta_x,delta_y; fp32 delta_w,delta_x,delta_y;
fp32 sick0 =LowPassFilter2p_Apply( &(c->filled[5]),c->sick_cali.sick_dis[0]) ; fp32 sick0 = c->sick_cali.sick_dis[0];
fp32 sick1 =LowPassFilter2p_Apply( &(c->filled[6]), c->sick_cali.sick_dis[1]) ; fp32 sick1 = c->sick_cali.sick_dis[1];
fp32 sick2 = LowPassFilter2p_Apply( &(c->filled[7]),c->sick_cali.sick_dis[2] ); fp32 sick2 = c->sick_cali.sick_dis[2];
const sickparam_t *param = &c->sick_cali.sickparam; const sickparam_t *param = &c->sick_cali.sickparam;
diff_yaw = -(fp32)(sick1 - sick2);
diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set); diff_yaw =LowPassFilter2p_Apply(&(c->filled[1]), -(fp32)(sick1 +10 - sick2));
diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set); 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_w=PID_calc(&c->pid.SickCaliWzOutPID, diff_yaw, 0);
delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0); delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0);
delta_y=PID_calc(&c->pid.SickCaliVyOutPID, diff_y,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=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; static uint16_t reach_cnt = 0;
@ -247,17 +265,21 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
int8_t Chassis_AngleCompensate(Chassis_t *c) int8_t Chassis_AngleCompensate(Chassis_t *c)
{ {
if (c == NULL) return CHASSIS_ERR_NULL; if (c == NULL) return CHASSIS_ERR_NULL;
if(c->ang_cail.is_open==0) return 0;
static fp32 pian_angel,cur_angle; static fp32 pian_angel,cur_angle;
if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0)) 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 { else {
cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw); c->ang_cail.ang_cur=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
pian_angel=0; 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);
} }

30
User/Module/Chassis.h
View File

@ -31,7 +31,9 @@
#include "can_use.h" #include "can_use.h"
#include "cmd.h" #include "cmd.h"
#include "filter.h" #include "filter.h"
#include <cmsis_os2.h> #include "kalman.h"
#include "nuc.h"
#define CHASSIS_OK (0) #define CHASSIS_OK (0)
#define CHASSIS_ERR (-1) #define CHASSIS_ERR (-1)
#define CHASSIS_ERR_NULL (-2) #define CHASSIS_ERR_NULL (-2)
@ -102,12 +104,6 @@ typedef struct
fp32 Vw; fp32 Vw;
fp32 mul;//油门倍率 fp32 mul;//油门倍率
}ChassisMove_Vec; }ChassisMove_Vec;
typedef struct
{
fp32 send[4];
}NUC_send_t;
/** /**
* @brief * @brief
@ -172,16 +168,30 @@ typedef struct{
}pid; }pid;
fp32 vofa_send[8]; fp32 vofa_send[8];
/*卡尔曼滤波*/
extKalman_t extKalman[3];
LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */ LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */
/*角度偏转修正 */
struct{
int is_open;
fp32 ang_error;
fp32 ang_cur;
fp32 out;
}ang_cail;
struct { struct {
int32_t sick_dis[4]; //获取到的sick激光值 fp32 sick_dis[4]; //获取到的sick激光值
sickparam_t sickparam; sickparam_t sickparam;
int is_reach; int is_reach;
}sick_cali; }sick_cali;
NUC_send_t NUC_send; struct {
fp32 send;
}to_nuc;
}Chassis_t; }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_pull_pos = -214.0f, // go上升去合并扳机，扳机位置
.go_up_speed = 12.0f, // 上升速度 .go_up_speed = 12.0f, // 上升速度
.go_down_speed = 6.0f, // 下降速度 .go_down_speed = 6.0f, // 下降速度
.Pitch_angle = 66, //俯仰角 .Pitch_angle = 58, //俯仰角
.go_init = -50 .go_init = -50
}, },
.PitchCfg = { .PitchCfg = {
@ -100,7 +100,7 @@ static const ConfigParam_t param ={
.MID360Cfg={ .MID360Cfg={
.go_release_pos=-200, // GO电机发射位置 .go_release_pos=-200, // GO电机发射位置
.go_up_speed=15, // GO电机上升速度 .go_up_speed=20, // GO电机上升速度
.go_down_speed=10, // 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 = { .SickCali_WInparam = {
.p = 3.0f, .p = 3.0f,
.i = 0.005f, .i = 0.000f,
.d = 0.0f, .d = 0.0f,
.i_limit = 500.0f, .i_limit = 500.0f,
.out_limit = 2000.0f, .out_limit = 2000.0f,
}, },
.SickCali_WOutparam = { .SickCali_WOutparam = {
.p = 18.0f, .p = 2.6f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit = 1000.0f, .out_limit = 1000.0f,
}, },
.SickCali_YInparam = { .SickCali_YInparam = {
.p = 0.0f, .p = 1.0f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit = 0.0f, .out_limit = 5000.0f,
}, },
.SickCali_YOutparam = { .SickCali_YOutparam = {
.p = 2.9f, .p = 4.5f,
.i = 0.005f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 500.0f, .i_limit = 00.0f,
.out_limit = 1000.0f, .out_limit = 1000.0f,
}, },
.SickCali_XInparam = { .SickCali_XInparam = {
.p = 0.0f, .p = 1.0f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit = 0.0f, .out_limit = 5000.0f,
}, },
.SickCali_XOutparam = { .SickCali_XOutparam = {
.p = 2.9f, .p = 4.5f,
.i = 0.0065f, .i = 0.00f,
.d = 0.0f, .d = 0.0f,
.i_limit = 500.0f, .i_limit = 500.0f,
.out_limit = 3000.0f, .out_limit = 2000.0f,
}, },
.Chassis_AngleAdjust_param={ .Chassis_AngleAdjust_param={
.p = 10.0f, .p = 10.0f,
.i = 0.0f, .i = 0.0f,
@ -184,10 +235,10 @@ static const ConfigParam_t param ={
}, },
.sickparam={ .sickparam={
.w_error=5, .w_error=2,
.xy_error=5, .xy_error=3,
.x_set=600, .x_set=10000,
.y_set=100, .y_set=2000,
}, },

59
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 )); PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param ));
u->go_cmd =u->param ->go_cmd ; u->go_cmd =u->param ->go_cmd ;
LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f); 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_M1out =-u->motor_target .VESC_5065_M1_rpm;
u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm; u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm;
return 0; return 0;
} }
@ -193,6 +192,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
switch (c->CMD_CtrlType ) switch (c->CMD_CtrlType )
{ {
case ShootGame_Mode: case ShootGame_Mode:
UP_AUTO_Control(u,out,c); 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 ; break ;
//
} }
return 0; return 0;
@ -224,26 +224,32 @@ return 0;
//复用发射， //复用发射，
int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){ 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){ switch(LaunchContext->LaunchState){
case Launch_Stop: break; case Launch_Stop: break;
case Launch_PREPARE: case Launch_PREPARE:
u->motor_target.go_shoot = StartPos; u->motor_target.go_shoot = StartPos;
if(is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f)&& if(is_GoStartReach&& is_GoSpeedReach){
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; LaunchContext->LaunchState = Launch_START;
}
}break; }break;
case Launch_START: case Launch_START:
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_up_speed; u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_up_speed;
u->motor_target.go_shoot = u->LaunchContext.LaunchCfg.go_pull_pos; 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角度，关闭 u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度，关闭
LaunchContext->LaunchState = Launch_TRIGGER; LaunchContext->LaunchState = Launch_TRIGGER;
@ -251,19 +257,18 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP
case Launch_TRIGGER: 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_pull_speed=LaunchContext->LaunchCfg.go_down_speed;
u->motor_target.go_shoot = EndPos ; 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: case Launch_SHOOT_WAIT:
if(u->motorfeedback.DJmotor_feedback[4].total_angle<-1) //认为发射 if(is_Shoot) //认为发射
LaunchContext->LaunchState = Launch_DONE; LaunchContext->LaunchState = Launch_DONE;
break; break;
case Launch_DONE: default:break;
break ;
} }
} }
@ -319,12 +324,10 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
Pass_Sequence_Check(u,c); Pass_Sequence_Check(u,c);
if(c->pos) //
{
PassCfg ->go_release_pos = PassCfg ->go_release_pos =
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PassContext.Curve); CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PassContext.Curve);
}
switch (*state) { //遥控器按键进行状态切换 switch (*state) { //遥控器按键进行状态切换
case PASS_STOP: case PASS_STOP:
@ -334,13 +337,13 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
break; break;
case PASS_PREPARE: case PASS_PREPARE:
target->go_pull_speed=PassCfg->go_up_speed; target->go_pull_speed=PassCfg->go_up_speed;
Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out); Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out);
break; break;
case PASS_START: case PASS_START:
if(LaunchContext->LaunchState == Launch_SHOOT_WAIT){ if(LaunchContext->LaunchState == Launch_TRIGGER){
target->go_pull_speed=PassCfg->go_down_speed; target->go_pull_speed=PassCfg->go_down_speed;
target->go_shoot = PassCfg->go_release_pos ; target->go_shoot = PassCfg->go_release_pos ;
} }
@ -362,7 +365,7 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
LaunchContext_t *LaunchContext = &u->LaunchContext; LaunchContext_t *LaunchContext = &u->LaunchContext;
MID360Context_t *MID360Context=&u->MID360Context; MID360Context_t *MID360Context=&u->MID360Context;
MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg; 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.25f,3.2,4.3,&u->MID360Context.Curve);
/* 函数，角度更新*/ /* 函数，角度更新*/
if (u->MID360Context.Curve == CURVE_58) { if (u->MID360Context.Curve == CURVE_58) {
target->Pitch_angle = 58; target->Pitch_angle = 58;
@ -386,13 +389,13 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
{ {
case Shooting://发射 case Shooting://发射
target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;
break; break;
case ShootRst://重置发射 case ShootRst://重置发射
MID360Context->IsLaunch=0; MID360Context->IsLaunch=0;
target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;
break ; break ;
@ -403,4 +406,4 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
} }
return 1; return 1;
} }

47
User/Module/up_utils.c
View File

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

2
User/Module/up_utils.h
View File

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

39
User/bsp/protocol.h
View File

@ -14,6 +14,13 @@ extern "C" {
#define HEAD (0xFF) #define HEAD (0xFF)
#define TAIL (0xFE) #define TAIL (0xFE)
#define IMU_ID (0x01)
#define CMD_ID (0x02)
#define TYPE (0x09)
#define NAVI (0x05) #define NAVI (0x05)
#define PICK (0x06) #define PICK (0x06)
@ -49,6 +56,38 @@ typedef struct __attribute__((packed)) {
} Protocol_DownDataChassis_t; } 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)) { typedef struct __attribute__((packed)) {

103
User/device/cmd.c
View File

@ -136,41 +136,74 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
cmd->key_ctrl_l = rc->dr16.sw_l; cmd->key_ctrl_l = rc->dr16.sw_l;
cmd->key_ctrl_r = rc->dr16.sw_r ; cmd->key_ctrl_r = rc->dr16.sw_r ;
if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) { // if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) {
cmd->CMD_CtrlType =RELAXED; // cmd->CMD_CtrlType =RELAXED;
} // }
else if(rc->dr16.sw_l==CMD_SW_UP) // else if(rc->dr16.sw_l==CMD_SW_UP)
{ // {
cmd ->CMD_CtrlType =ShootGame_Mode; // cmd ->CMD_CtrlType =ShootGame_Mode;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左上，右上，无模式 // 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_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
//
if(rc->dr16.sw_r ==CMD_SW_DOWN) 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 > 3000) cmd ->CMD_mode =ShootRst;
if(rc->dr16.res == 660) cmd ->CMD_mode =Shooting; //// if(rc->dr16.res == 660) cmd ->CMD_mode =Shooting;
} // }
//
else if(rc->dr16.sw_l==CMD_SW_MID) // else if(rc->dr16.sw_l==CMD_SW_MID)
{ // {
//
cmd ->CMD_CtrlType =RCcontrol; // cmd ->CMD_CtrlType =RCcontrol;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左中，右中，雷达 // 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_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式
//
if(rc->dr16.sw_r ==CMD_SW_DOWN) 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) // else if(rc->dr16.sw_l==CMD_SW_DOWN)
{ // {
cmd ->CMD_CtrlType =RCcontrol; // cmd ->CMD_CtrlType =RCcontrol;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，无模式 // 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_MID) cmd ->CMD_mode = Normal; //左下，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式 // if(rc->dr16.sw_r ==CMD_SW_DOWN) 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_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.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.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;
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; // 右下，左下，无模式
}
return 0; return 0;
} }

3
User/device/cmd.h
View File

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

83
User/device/nuc.c
View File

@ -40,33 +40,6 @@ int8_t NUC_StartReceiving()
return DEVICE_OK; return DEVICE_OK;
return DEVICE_ERR; 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) int8_t NUC_Restart(void)
{ {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC)); __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC));
@ -75,11 +48,11 @@ int8_t NUC_Restart(void)
} }
bool_t NUC_WaitDmaCplt(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); 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) if (n == NULL)
return DEVICE_ERR_NULL; return DEVICE_ERR_NULL;
@ -165,6 +138,7 @@ int8_t NUC_RawParse(CMD_NUC_t *n)
break; break;
} }
nuc->unc_online = true; // 设置为在线状态
return DEVICE_OK; return DEVICE_OK;
} }
error: error:
@ -173,10 +147,57 @@ error:
return DEVICE_ERR; 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) if (cmd == NULL)
return DEVICE_ERR_NULL; return DEVICE_ERR_NULL;
// memset(cmd, 0, sizeof(*cmd)); nuc->unc_online = false; // 设置为离线状态
memset(cmd, 0, sizeof(*cmd));
return 0; 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;
// }
}

43
User/device/nuc.h
View File

@ -6,9 +6,14 @@
#include "bsp_usart.h" #include "bsp_usart.h"
#include "cmd.h" #include "cmd.h"
#include "protocol.h" #include "protocol.h"
#include "Algorithm/ahrs.h"
#include "Module/Chassis.h"
typedef struct
{
fp32 send;
}NUC_send_t;
typedef struct { typedef struct {
uint8_t id; uint8_t id;
@ -16,11 +21,32 @@ typedef struct {
} NUC_UpPackageMCU_t; } NUC_UpPackageMCU_t;
typedef struct { 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 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; //接收的数据协议 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;//控制状态 uint8_t status;//控制状态
@ -29,11 +55,14 @@ typedef struct {
int8_t NUC_Init(NUC_t *nuc); int8_t NUC_Init(NUC_t *nuc);
int8_t NUC_StartReceiving(void); int8_t NUC_StartReceiving(void);
int8_t NUC_StartSending(fp32 *data) ;
bool_t NUC_WaitDmaCplt(void); bool_t NUC_WaitDmaCplt(void);
int8_t NUC_RawParse(CMD_NUC_t *n); int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc);
int8_t NUC_HandleOffline(CMD_NUC_t *cmd); int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc);
int8_t NUC_Restart(void); 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 #endif

10
User/task/atti_esti.c
View File

@ -119,7 +119,7 @@ void Task_AttiEsti(void *argument) {
AHRS_GetEulr(&imu_eulr, &gimbal_ahrs); AHRS_GetEulr(&imu_eulr, &gimbal_ahrs);
detect_hook(IMU_TOE); detect_hook(IMU_TOE);
osKernelUnlock();
osMessageQueueReset(task_runtime.msgq.imu.accl); osMessageQueueReset(task_runtime.msgq.imu.accl);
osMessageQueuePut(task_runtime.msgq.imu.accl, &bmi088.accl, 0, 0); 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); osMessageQueueReset(task_runtime.msgq.imu.eulr);
osMessageQueuePut(task_runtime.msgq.imu.eulr, &imu_eulr, 0, 0); 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输出 */ /* PID控制IMU温度，PWM输出 */
BSP_PWM_Set(BSP_PWM_IMU_HEAT, 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);//清空队列 osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列
osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0); //发送数据 osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0); //发送数据
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc_send);//清空队列 osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc_send);//清空队列
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc_send, &chassis.NUC_send, 0, 0); //发送数据 osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc_send, &chassis.to_nuc, 0, 0); //发送数据
vofa_send[0] = chassis.vofa_send[0]; vofa_send[0] = chassis.vofa_send[0];
vofa_send[1] = chassis.vofa_send[1]; vofa_send[1] = chassis.vofa_send[1];
vofa_send[2] = chassis.vofa_send[2]; vofa_send[2] = chassis.vofa_send[2];

7
User/task/init.c
View File

@ -64,6 +64,13 @@ void Task_Init(void *argument) {
task_runtime.msgq.can.output.up_dribble_3508 = task_runtime.msgq.can.output.up_dribble_3508 =
osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL); 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 */ /* imu */

55
User/task/nuc_task.c
View File

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

11
User/task/user_task.h
View File

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