diff --git a/MDK-ARM/.vscode/c_cpp_properties.json b/MDK-ARM/.vscode/c_cpp_properties.json index 411c3e2..1efa7c0 100644 --- a/MDK-ARM/.vscode/c_cpp_properties.json +++ b/MDK-ARM/.vscode/c_cpp_properties.json @@ -419,32 +419,32 @@ { "name": "R2", "includePath": [ - "d:\\Desktop\\R2 _ball_game\\Core\\Inc", - "d:\\Desktop\\R2 _ball_game\\Drivers\\STM32F4xx_HAL_Driver\\Inc", - "d:\\Desktop\\R2 _ball_game\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy", - "d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include", - "d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2", - "d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F", - "d:\\Desktop\\R2 _ball_game\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include", - "d:\\Desktop\\R2 _ball_game\\Drivers\\CMSIS\\Include", - "d:\\Desktop\\R2 _ball_game\\User", - "d:\\Desktop\\R2 _ball_game\\User\\bsp", - "d:\\Desktop\\R2 _ball_game\\User\\device", - "d:\\Desktop\\R2 _ball_game\\User\\task", - "d:\\Desktop\\R2 _ball_game\\User\\Algorithm", - "d:\\Desktop\\R2 _ball_game\\User\\Module", - "d:\\Desktop\\R2 _ball_game\\MDK-ARM", - "d:\\Desktop\\R2 _ball_game\\USB_DEVICE\\App", - "d:\\Desktop\\R2 _ball_game\\USB_DEVICE\\Target", - "d:\\Desktop\\R2 _ball_game\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Inc", - "d:\\Desktop\\R2 _ball_game\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Inc", - "d:\\Desktop\\R2 _ball_game\\Drivers\\CMSIS\\Lib\\ARM", - "d:\\Desktop\\R2 _ball_game\\Core\\Src", - "d:\\Desktop\\R2 _ball_game\\Drivers\\STM32F4xx_HAL_Driver\\Src", - "d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source", - "d:\\Desktop\\R2 _ball_game\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang", - "d:\\Desktop\\R2 _ball_game\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Src", - "d:\\Desktop\\R2 _ball_game\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Src" + "d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Inc", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Include", + "d:\\Desktop\\r2\\R2_NEW\\R2\\User", + "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\bsp", + "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\device", + "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\task", + "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Algorithm", + "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Module", + "d:\\Desktop\\r2\\R2_NEW\\R2\\MDK-ARM", + "d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\App", + "d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\Target", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Inc", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Inc", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Lib\\ARM", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Src", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Src", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Src", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Src" ], "defines": [ "USE_HAL_DRIVER", diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log index 4a835d3..80658bb 100644 --- a/MDK-ARM/.vscode/keil-assistant.log +++ b/MDK-ARM/.vscode/keil-assistant.log @@ -481,9 +481,7 @@ [info] Log at : 2025/6/28|01:22:24|GMT+0800 -[info] Log at : 2025/6/29|03:38:10|GMT+0800 +[info] Log at : 2025/6/30|18:03:06|GMT+0800 -[info] Log at : 2025/6/30|15:21:11|GMT+0800 - -[info] Log at : 2025/6/30|18:03:49|GMT+0800 +[info] Log at : 2025/7/1|14:01:22|GMT+0800 diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx index d2632dd..d65d7cf 100644 --- a/MDK-ARM/R2.uvoptx +++ b/MDK-ARM/R2.uvoptx @@ -158,7 +158,7 @@ 0 1 - tar_angle,0x0A + tar_angle 1 @@ -173,27 +173,12 @@ 3 1 - UP,0x0A + huart->ErrorCode 4 1 - qian_Nor_Vx - - - 5 - 1 - hou_Nor_Vx - - - 6 - 1 - chassis,0x0A - - - 7 - 1 - cmd,0x0A + chassis diff --git a/MDK-ARM/R2.uvprojx b/MDK-ARM/R2.uvprojx index 893edd4..64c3412 100644 --- a/MDK-ARM/R2.uvprojx +++ b/MDK-ARM/R2.uvprojx @@ -17,8 +17,8 @@ STM32F407IGHx STMicroelectronics - Keil.STM32F4xx_DFP.3.0.0 - https://www.keil.com/pack/ + Keil.STM32F4xx_DFP.2.15.0 + http://www.keil.com/pack/ IRAM(0x20000000-0x2001BFFF) IRAM2(0x2001C000-0x2001FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4") TZ diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf index 71e7ac6..5494a3f 100644 Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ diff --git a/Middlewares/Third_Party/Protocol/protocol.h b/Middlewares/Third_Party/Protocol/protocol.h new file mode 100644 index 0000000..330770c --- /dev/null +++ b/Middlewares/Third_Party/Protocol/protocol.h @@ -0,0 +1,100 @@ +/* + 视觉与电控通信协议 +*/ + +#pragma once + +#include +#include + +#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 diff --git a/User/Algorithm/user_math.c b/User/Algorithm/user_math.c index 7ecc2fd..b16a7d3 100644 --- a/User/Algorithm/user_math.c +++ b/User/Algorithm/user_math.c @@ -365,7 +365,7 @@ int read_flag_state(uint8_t flag) { return state; } // 归一化函数，将正方形坐标映射到单位圆 -void normalize_vector(fp32 x, fp32 y, fp32 *out_x, fp32 *out_y) { +void normalize_vector(double x, double y, double *out_x, double *out_y) { // 处理原点情况 if (x == 0.0 && y == 0.0) { *out_x = 0.0; @@ -374,32 +374,16 @@ void normalize_vector(fp32 x, fp32 y, fp32 *out_x, fp32 *out_y) { } // 计算最大坐标绝对值和原始模长 - const fp32 abs_x = fabs(x); - const fp32 abs_y = fabs(y); - const fp32 s = fmax(abs_x, abs_y); // 最大坐标绝对值 - const fp32 r = sqrt(x*x + y*y); // 原始向量模长 - const fp32 scale = s / r; // 缩放因子 + const double abs_x = fabs(x); + const double abs_y = fabs(y); + const double s = fmax(abs_x, abs_y); // 最大坐标绝对值 + const double r = sqrt(x*x + y*y); // 原始向量模长 + const double scale = s / r; // 缩放因子 // 应用缩放并保持方向 *out_x = x * scale; *out_y = y * scale; } - -/** - * @brief 底盘无头模式转换函数 - * @param vx 输入 X 方向速度（全局坐标系），输出转换后的底盘 X 速度 - * @param vy 输入 Y 方向速度（全局坐标系），输出转换后的底盘 Y 速度 - * @param vw 旋转速度（直接控制，不受影响） - * @param yaw 当前陀螺仪 yaw 角度（弧度制） - */ -void ChassisHeadlessMode(float *vx, float *vy, float yaw) { - float vx_global = *vx; - float vy_global = *vy; - - // 标准二维旋转变换（逆时针为正） - *vx = vx_global * cosf(yaw) - vy_global * sinf(yaw); - *vy = vx_global * sinf(yaw) + vy_global * cosf(yaw); -} bool is_reached(float current, float target, float mistake) { return fabs(current - target) < mistake; } @@ -421,4 +405,19 @@ bool is_reached_multiple(float current1, float current2, float current3, float t count++; // 所有条件都满足，计数加 1 } return false; // 未满足条件达到阈值，返回 0 +} + /** + * @brief 底盘无头模式转换函数 + * @param vx 输入 X 方向速度（全局坐标系），输出转换后的底盘 X 速度 + * @param vy 输入 Y 方向速度（全局坐标系），输出转换后的底盘 Y 速度 + * @param vw 旋转速度（直接控制，不受影响） + * @param yaw 当前陀螺仪 yaw 角度（弧度制） + */ +void ChassisHeadlessMode(double *vx, double *vy, double yaw) { + double vx_global = *vx; + double vy_global = *vy; + + // 标准二维旋转变换（逆时针为正） + *vx = vx_global * cosf(yaw) - vy_global * sinf(yaw); + *vy = vx_global * sinf(yaw) + vy_global * cosf(yaw); } \ No newline at end of file diff --git a/User/Algorithm/user_math.h b/User/Algorithm/user_math.h index 9ced009..7464dd8 100644 --- a/User/Algorithm/user_math.h +++ b/User/Algorithm/user_math.h @@ -161,10 +161,10 @@ int abs_value(int num); float expo_map(float value, float expo_factor) ; int read_flag_state(uint8_t flag) ; -void normalize_vector(fp32 x, fp32 y, fp32 *out_x, fp32 *out_y) ; +void normalize_vector(double x, double y, double *out_x, double *out_y) ; bool is_reached(float current, float target, float mistake) ; bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ; fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max); -void ChassisHeadlessMode(float *vx, float *vy, float yaw) ; +void ChassisHeadlessMode(double *vx, double *vy, double yaw) ; #endif diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c index b55c130..576c55b 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -3,7 +3,6 @@ #include "user_math.h" #include "bsp_buzzer.h" #include "bsp_delay.h" - /* 机器人坐标系，向前x，右y，上yaw 不同于nuc (前x，左y，上yaw) @@ -11,9 +10,7 @@ | --y */ - static osThreadId_t thread_alert; - static int8_t Chassis_SetCtrl(Chassis_t *c, CMD_t *ctrl) { if (c == NULL) return CHASSIS_ERR_NULL; if (ctrl->CMD_CtrlType == c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) @@ -22,14 +19,11 @@ static int8_t Chassis_SetCtrl(Chassis_t *c, CMD_t *ctrl) { c->chassis_ctrl.mode = ctrl->CMD_mode; return CHASSIS_OK; } -//int8_t ShootGame_FlagSet(void) -//{ -// osThreadFlagsSet(thread_alert, SIGNAL_NUC_RAW_REDY); -// -//} + int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) { if (c == NULL) return CHASSIS_ERR_NULL; if (can == NULL) return CHASSIS_ERR_NULL; + for (uint8_t i = 0; i < 4; i++) { c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed; c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current; @@ -39,19 +33,19 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) { // c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值 // } c->sick_cali.sick_dis[0]=(fp32)(can->sickfed.raw_dis[0]) ; - c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+26) ; //有点误差，手动补偿 + c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+30) ; //有点误差，手动补偿 c->sick_cali.sick_dis[2]=(fp32)(can->sickfed.raw_dis[2] ); -// c->vofa_send[7] =c->sick_cali.sick_dis[0]-c->sick_cali.sick_dis[1]; -// c->vofa_send[6] = c->sick_cali.sick_dis[0]; -// c->vofa_send[5] =c->sick_cali.sick_dis[1]; -// c->vofa_send[4] =c->sick_cali.sick_dis[2]; + c->vofa_send[7] =c->sick_cali.sick_dis[1]-c->sick_cali.sick_dis[2]; + c->vofa_send[6] = c->sick_cali.sick_dis[0]; + c->vofa_send[5] =c->sick_cali.sick_dis[1]; + c->vofa_send[4] =c->sick_cali.sick_dis[2]; return CHASSIS_OK; } int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_freq) { if (c == NULL) return CHASSIS_ERR_NULL; - if ((thread_alert = osThreadGetId()) == NULL) + if ((thread_alert = osThreadGetId()) == NULL) return CHASSIS_ERR_NULL; c->param = param; @@ -71,43 +65,33 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param)); LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波 - LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波 - LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); // y滤波 + LowPassFilter2p_Init(&(c->filled[1]), target_freq, 50.0f); + LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f); - LowPassFilter2p_Init(&(c->filled[4]), target_freq, 80.0f); - LowPassFilter2p_Init(&(c->filled[5]), target_freq, 80.0f); - LowPassFilter2p_Init(&(c->filled[6]), target_freq, 80.0f); - LowPassFilter2p_Init(&(c->filled[7]), target_freq, 80.0f); + + KalmanCreate(&c->extKalman[0],10,1); + KalmanCreate(&c->extKalman[1],10,1); + KalmanCreate(&c->extKalman[2],10,1); + c->sick_cali .sickparam=c->param ->sickparam ; - + + c->ang_cail.is_open=1; return CHASSIS_OK; } -fp32 pian_yaw; - -fp32 qian_Nor_Vx; -fp32 hou_Nor_Vx; - void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) { - - fp32 Nor_Vx, Nor_Vy; - - normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy); //速度归一，防止斜跑速度最快 - qian_Nor_Vx=Nor_Vx; - ChassisHeadlessMode(&Nor_Vx,&Nor_Vy,c->pos088.imu_eulr.yaw); - hou_Nor_Vx=Nor_Vx; - - - c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+pian_yaw; // 右前 - c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+pian_yaw; // 右后 - c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +pian_yaw; // 左后 - c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +pian_yaw; // 左前 + fp64 Nor_Vx, Nor_Vy; + normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy); + ChassisHeadlessMode(&Nor_Vx,&Nor_Vy,c->pos088.imu_eulr.yaw); + c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前 + c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后 + c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out; // 左后 + c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +c->ang_cail.out; // 左前 - Chassis_AngleCompensate(c); + Chassis_AngleCompensate(c); } -int i=0; int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { if (c == NULL) return CHASSIS_ERR_NULL; if (ctrl == NULL) return CHASSIS_ERR_NULL; @@ -118,12 +102,17 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { /*初始数据*/ -// c->NUC_send.send[0] = 0; -// c->sick_cali.is_reach = 0; + c->NUC_send.send[0] = 0; + c->sick_cali.is_reach = 0; + + c->vofa_send[0]= KalmanFilter(&c->extKalman[0] , c->sick_cali.sick_dis[0]); + c->vofa_send[1]= KalmanFilter(&c->extKalman[1] , c->sick_cali.sick_dis[1]); + c->vofa_send[2]= KalmanFilter(&c->extKalman[2] , c->sick_cali.sick_dis[2]); + + switch (c->chassis_ctrl.ctrl) { case ShootGame_Mode: //投球赛模式 - i=(osThreadFlagsWait(SHOOT_GAME_FLAG, osFlagsWaitAny, 0)==SHOOT_GAME_FLAG); if (ctrl->Vw != 0 || ctrl->Vy != 0 || ctrl->Vx != 0) { // 有遥控器输出，设置标志位1 osThreadFlagsSet(thread_alert, SHOOT_GAME_FLAG); @@ -148,23 +137,74 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { } break; - - case RCcontrol: - - c->move_vec.Vw = ctrl->Vw * 8000; - c->move_vec.Vx = ctrl->Vy * 8000; - c->move_vec.Vy = ctrl->Vx * 8000; - - - break; - default: - c->move_vec.Vw = 0; - c->move_vec.Vx = 0; - c->move_vec.Vy = 0; - break; + case AUTO: + switch (c->chassis_ctrl.mode) { + case AUTO_MID360: + c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000; + c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000; + c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000; + abs_limit_fp(&c->move_vec.Vx, 5000.0f); + abs_limit_fp(&c->move_vec.Vy, 5000.0f); + abs_limit_fp(&c->move_vec.Vw, 5000.0f); + // c->NUC_send.send[0] = 1; + break; + case AUTO_MID360_Pitch: + c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000; + c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000; + c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000; + abs_limit_fp(&c->move_vec.Vx, 5000.0f); + abs_limit_fp(&c->move_vec.Vy, 5000.0f); + abs_limit_fp(&c->move_vec.Vw, 5000.0f); + break; + case AUTO_MID360_Adjust: + c->move_vec.Vw = ctrl->Vw * 6000; + c->move_vec.Vx = ctrl->Vy * 6000; + c->move_vec.Vy = ctrl->Vx * 6000; + + break; + default: + + break; + } + case PASS_BALL: + switch (c->chassis_ctrl.mode) + { + case PB_UP: + c->move_vec.Vw = ctrl->Vw * 6000; + c->move_vec.Vx = ctrl->Vy * 6000; + c->move_vec.Vy = ctrl->Vx * 6000; + break ; + case PB_MID: + case PB_DOWN: + c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000; + c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000; + c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000; + abs_limit_fp(&c->move_vec.Vx, 5000.0f); + abs_limit_fp(&c->move_vec.Vy, 5000.0f); + abs_limit_fp(&c->move_vec.Vw, 5000.0f); + break ; + + } + break; + case RELAXED: + c->move_vec.Vw = 0; + c->move_vec.Vx = 0; + c->move_vec.Vy = 0; + break ; + + + + + break; + break; + + default: + + break; } + // 电机速度限幅 abs_limit_fp(&c->move_vec.Vx, 6000.0f); @@ -204,26 +244,35 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) if (ctrl == NULL) return CHASSIS_ERR_NULL; fp32 delta_w,delta_x,delta_y; - fp32 sick0 =LowPassFilter2p_Apply( &(c->filled[5]),c->sick_cali.sick_dis[0]) ; - fp32 sick1 =LowPassFilter2p_Apply( &(c->filled[6]), c->sick_cali.sick_dis[1]) ; - fp32 sick2 = LowPassFilter2p_Apply( &(c->filled[7]),c->sick_cali.sick_dis[2] ); + fp32 sick0 = c->sick_cali.sick_dis[0]; + fp32 sick1 = c->sick_cali.sick_dis[1]; + fp32 sick2 = c->sick_cali.sick_dis[2]; + + const sickparam_t *param = &c->sick_cali.sickparam; - - diff_yaw = -(fp32)(sick1 - sick2); - diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set); - diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set); + + + diff_yaw =LowPassFilter2p_Apply(&(c->filled[1]), -(fp32)(sick1 +10 - sick2)); + diff_y = LowPassFilter2p_Apply(&(c->filled[2]),-(fp32)(sick1 - c->sick_cali.sickparam.y_set)); + + diff_x = LowPassFilter2p_Apply(&(c->filled[3]),(fp32)(sick0 - c->sick_cali.sickparam.x_set)); delta_w=PID_calc(&c->pid.SickCaliWzOutPID, diff_yaw, 0); delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0); delta_y=PID_calc(&c->pid.SickCaliVyOutPID, diff_y,0); - if(fabs(diff_yaw)>5){ + if(fabs(diff_yaw)>param->w_error){ + c->move_vec.Vw=PID_calc(&c->pid.SickCaliWzInPID,-delta_w,0); -// c->move_vec.Vw=delta_w; -// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVxInPID,delta_x,0); -// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVyInPID,delta_y,0); } - +// if(fabs(diff_y)>param->xy_error){ +// +// c->move_vec.Vy=PID_calc(&c->pid.SickCaliVyInPID,delta_y,0); +// } +// if(fabs(diff_x)>param->xy_error){ +// +// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVxInPID,-delta_x,0); +// } static uint16_t reach_cnt = 0; @@ -247,17 +296,21 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) int8_t Chassis_AngleCompensate(Chassis_t *c) { if (c == NULL) return CHASSIS_ERR_NULL; + if(c->ang_cail.is_open==0) return 0; + static fp32 pian_angel,cur_angle; + if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0)) { - pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw); + c->ang_cail.ang_error=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw); + } else { - cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw); - pian_angel=0; + c->ang_cail.ang_cur=AngleChange(DEGREE,c->pos088.imu_eulr.yaw); + c->ang_cail.ang_error=0; } - pian_yaw = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0); + c->ang_cail.out = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0); } diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h index a52863c..91d56cd 100644 --- a/User/Module/Chassis.h +++ b/User/Module/Chassis.h @@ -31,7 +31,8 @@ #include "can_use.h" #include "cmd.h" #include "filter.h" -#include +#include "kalman.h" + #define CHASSIS_OK (0) #define CHASSIS_ERR (-1) #define CHASSIS_ERR_NULL (-2) @@ -172,11 +173,23 @@ typedef struct{ }pid; fp32 vofa_send[8]; - + /*卡尔曼滤波*/ + extKalman_t extKalman[3]; LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */ + + /*角度偏转修正 */ + struct{ + + int is_open; + fp32 ang_error; + fp32 ang_cur; + fp32 out; + + + }ang_cail; struct { - int32_t sick_dis[4]; //获取到的sick激光值 + fp32 sick_dis[4]; //获取到的sick激光值 sickparam_t sickparam; int is_reach; }sick_cali; diff --git a/User/Module/config.c b/User/Module/config.c index e04ff7e..efd02a0 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -80,7 +80,7 @@ static const ConfigParam_t param ={ .go_pull_pos = -214.0f, // go上升去合并扳机，扳机位置 .go_up_speed = 12.0f, // 上升速度 .go_down_speed = 6.0f, // 下降速度 - .Pitch_angle = 66, //俯仰角 + .Pitch_angle = 58, //俯仰角 .go_init = -50 }, .PitchCfg = { @@ -100,7 +100,7 @@ static const ConfigParam_t param ={ .MID360Cfg={ .go_release_pos=-200, // GO电机发射位置 - .go_up_speed=15, // GO电机上升速度 + .go_up_speed=20, // GO电机上升速度 .go_down_speed=10, // GO电机下降速度 @@ -133,48 +133,99 @@ static const ConfigParam_t param ={ }, +// .SickCali_WInparam = { +// .p = 3.0f, +// .i = 0.000f, +// .d = 0.0f, +// .i_limit = 500.0f, +// .out_limit = 2000.0f, +// }, +// .SickCali_WOutparam = { +// .p = 2.6f, +// .i = 0.0f, +// .d = 0.0f, +// .i_limit = 0.0f, +// .out_limit = 1000.0f, +// }, +// .SickCali_YInparam = { +// .p = 1.0f, +// .i = 0.0f, +// .d = 0.0f, +// .i_limit = 0.0f, +// .out_limit = 5000.0f, +// }, +// .SickCali_YOutparam = { +// .p = 4.5f, +// .i = 0.0f, +// .d = 0.0f, +// .i_limit = 00.0f, +// .out_limit = 1000.0f, +// }, +// .SickCali_XInparam = { +// .p = 1.0f, +// .i = 0.0f, +// .d = 0.0f, +// .i_limit = 0.0f, +// .out_limit = 5000.0f, +// }, +// .SickCali_XOutparam = { +// .p = 4.5f, +// .i = 0.00f, +// .d = 0.0f, +// .i_limit = 500.0f, +// .out_limit = 2000.0f, +// }, + + + + + + + + .SickCali_WInparam = { .p = 3.0f, - .i = 0.005f, + .i = 0.000f, .d = 0.0f, .i_limit = 500.0f, .out_limit = 2000.0f, }, .SickCali_WOutparam = { - .p = 18.0f, + .p = 2.6f, .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, .out_limit = 1000.0f, }, .SickCali_YInparam = { - .p = 0.0f, + .p = 1.0f, .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, - .out_limit = 0.0f, + .out_limit = 5000.0f, }, .SickCali_YOutparam = { - .p = 2.9f, - .i = 0.005f, + .p = 4.5f, + .i = 0.0f, .d = 0.0f, - .i_limit = 500.0f, + .i_limit = 00.0f, .out_limit = 1000.0f, }, .SickCali_XInparam = { - .p = 0.0f, + .p = 1.0f, .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, - .out_limit = 0.0f, + .out_limit = 5000.0f, }, .SickCali_XOutparam = { - .p = 2.9f, - .i = 0.0065f, + .p = 4.5f, + .i = 0.00f, .d = 0.0f, .i_limit = 500.0f, - .out_limit = 3000.0f, + .out_limit = 2000.0f, }, + .Chassis_AngleAdjust_param={ .p = 10.0f, .i = 0.0f, @@ -184,10 +235,10 @@ static const ConfigParam_t param ={ }, .sickparam={ - .w_error=5, - .xy_error=5, - .x_set=600, - .y_set=100, + .w_error=2, + .xy_error=3, + .x_set=10000, + .y_set=2000, }, diff --git a/User/Module/up.c b/User/Module/up.c index dd0c224..85abdbe 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -40,7 +40,6 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq) PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param )); u->go_cmd =u->param ->go_cmd ; - LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f); @@ -119,7 +118,7 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed) u->final_out .final_VESC_5065_M1out =-u->motor_target .VESC_5065_M1_rpm; u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm; - return 0; + return 0; } @@ -213,7 +212,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) break ; -// + } return 0; @@ -224,26 +223,32 @@ return 0; //复用发射， int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){ + /*电机位置到达判断*/ + + bool is_GoStartReach=is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f); //go开始位置 + bool is_GoSpeedReach=is_reached(u->motorfeedback.go_data.W,0,1.0f) ; //go速度归零判断 + bool is_GoEndReach =(u->motorfeedback .go_data .Pos < -209); //go去上拉钩子位置，判断到达 + bool is_HookDone=(u->motorfeedback .DJmotor_feedback [4].total_angle>-10); //2006钩子放下判断 + bool is_Shoot=(u->motorfeedback.DJmotor_feedback[4].total_angle<-10);//是否发射判断 + + + + switch(LaunchContext->LaunchState){ case Launch_Stop: break; case Launch_PREPARE: u->motor_target.go_shoot = StartPos; - if(is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f)&& - is_reached(u->motorfeedback.go_data.W,0,1.0f)){ + if(is_GoStartReach&& is_GoSpeedReach){ //根据位置和速度判断是否到达初始位置 - u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_init ; - - if(u->motorfeedback .DJmotor_feedback [4].total_angle<-70){ LaunchContext->LaunchState = Launch_START; - - } + }break; case Launch_START: u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_up_speed; u->motor_target.go_shoot = u->LaunchContext.LaunchCfg.go_pull_pos; - if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改 + if(is_GoEndReach){ //检测go位置到达最上面，这里的检测条件可以更改 u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度，关闭 LaunchContext->LaunchState = Launch_TRIGGER; @@ -251,19 +256,18 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP case Launch_TRIGGER: - if( u->motorfeedback .DJmotor_feedback [4].total_angle>-5){ //当2006的总角度小于5，可以认为已经勾上,误差为1 + if( is_HookDone ){ //当2006的总角度小于1，可以认为已经勾上,误差为1 u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_down_speed; u->motor_target.go_shoot = EndPos ; - - } - break; +// if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f)) +// LaunchContext->LaunchState = Launch_SHOOT_WAIT; + } break; case Launch_SHOOT_WAIT: - if(u->motorfeedback.DJmotor_feedback[4].total_angle<-1) //认为发射 + if(is_Shoot) //认为发射 LaunchContext->LaunchState = Launch_DONE; break; - case Launch_DONE: - break ; + default:break; } } @@ -319,12 +323,10 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c) Pass_Sequence_Check(u,c); - if(c->pos) // - { + PassCfg ->go_release_pos = CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PassContext.Curve); - } switch (*state) { //遥控器按键进行状态切换 case PASS_STOP: @@ -334,13 +336,13 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c) break; case PASS_PREPARE: target->go_pull_speed=PassCfg->go_up_speed; - + Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out); break; case PASS_START: - if(LaunchContext->LaunchState == Launch_SHOOT_WAIT){ + if(LaunchContext->LaunchState == Launch_TRIGGER){ target->go_pull_speed=PassCfg->go_down_speed; target->go_shoot = PassCfg->go_release_pos ; } @@ -362,45 +364,36 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){ LaunchContext_t *LaunchContext = &u->LaunchContext; MID360Context_t *MID360Context=&u->MID360Context; MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg; - MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)+0.15f,3.2,4.3,&u->MID360Context.Curve); - /* 函数，角度更新*/ + MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.05,3.2,4.3,&u->MID360Context.Curve); if (u->MID360Context.Curve == CURVE_58) { target->Pitch_angle = 58; } else { target->Pitch_angle = 66; - /* 上下速度更改*/ } + } LaunchContext->LaunchCfg.go_up_speed=MID360Cfg->go_up_speed; LaunchContext->LaunchCfg.go_down_speed=MID360Cfg->go_down_speed; - /*检测重置*/ - if(MID360Context->IsLaunch==0){ + + switch(c-> CMD_mode) + { + + case AUTO_MID360_Pitch: + if(MID360Context->IsLaunch==0){ MID360Context->IsLaunch=1; LaunchContext->LaunchState = Launch_PREPARE; } - /*运行*/ Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out); - - switch(c-> CMD_mode) - { - - case Shooting://发射 - - target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init; - - break; - case ShootRst://重置发射 - MID360Context->IsLaunch=0; - - break ; - - - default: + case AUTO_MID360: + target->Shoot_M2006_angle= LaunchContext->LaunchCfg.m2006_init; + MID360Context->IsLaunch=0; + break ; + default: break; } - return 1; - } + + } \ No newline at end of file diff --git a/User/Module/up_utils.c b/User/Module/up_utils.c index f23fc3a..a1e48f0 100644 --- a/User/Module/up_utils.c +++ b/User/Module/up_utils.c @@ -1,32 +1,35 @@ #include "up_utils.h" #include "up.h" -int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle) +int8_t DJ_processdata(DJmotor_feedback_t *f, fp32 ecd_to_angle) { - int8_t cnt=0; - fp32 angle ,delta; - - angle = f->ecd; + fp32 angle, delta; + + angle = f->ecd; + + // 初始化阶段，记录 offset if (f->init_cnt < 50) { - f->orig_angle= angle; - f->last_angle = angle; - f->init_cnt++; - return 0; + f->offset_ecd = (uint16_t)angle; // 记录初始偏移 + f->orig_angle = angle - f->offset_ecd; // orig_angle 归零 + f->last_angle = angle - f->offset_ecd; + f->init_cnt++; + return 0; } - - - delta = angle - f->last_angle; + + // 使用 offset 修正 + angle = angle - f->offset_ecd; + + delta = angle - f->last_angle; if (delta > 4096) { f->round_cnt--; } else if (delta < -4096) { f->round_cnt++; } - f->last_angle = angle; - f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle; - + f->last_angle = angle; + f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle; } /*go电机控制*/ -fp32 a_pos; + int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, float limit) { @@ -44,7 +47,7 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo // 计算输出力矩 tau float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current); - a_pos=pos; + /*限制最大输入来限制最大输出*/ if (pos - q_current > limit) { go_cmd->Pos = q_current + limit; // 限制位置 @@ -65,13 +68,13 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo // 计算66度曲线（偏上） // 计算66度曲线（偏上） static float curve_66(float d) { - return 4.0310f * d * d + 8.9026f * d -139.5156; -} + return 3.7028f * d * d + 11.2126f * d -142.9446f; + } // 计算58度曲线（偏下） // 计算58度曲线（偏下） static float curve_58(float d) { - return -1.9776f * d * d + 42.8499f * d - 204.2442f; + return 0.9242f * d * d + 19.4246f * d - 154.9055f; } /* @@ -79,10 +82,10 @@ static float curve_58(float d) { 迟滞区x-y 曲线x重合区，根据当前函数和变化方向切换 */ - +int abdddd=0; float CurveChange(float d, float x, float y, CurveType *cs) { - + if (d<3.2) abdddd++; if (*cs == CURVE_66) { if (d > y) { *cs = CURVE_58; diff --git a/User/Module/up_utils.h b/User/Module/up_utils.h index c29ab78..436e403 100644 --- a/User/Module/up_utils.h +++ b/User/Module/up_utils.h @@ -33,6 +33,8 @@ typedef struct int32_t round_cnt; int init_cnt; fp32 total_angle; + uint16_t offset_ecd; + uint32_t msg_cnt; }DJmotor_feedback_t; diff --git a/User/bsp/protocol.h b/User/bsp/protocol.h index da06407..c0d7390 100644 --- a/User/bsp/protocol.h +++ b/User/bsp/protocol.h @@ -14,6 +14,11 @@ extern "C" { #define HEAD (0xFF) #define TAIL (0xFE) +#define IMU_ID (0x01) +#define CMD_ID (0x02) + +#define TYPE (0x09) + #define NAVI (0x05) #define PICK (0x06) @@ -49,6 +54,38 @@ typedef struct __attribute__((packed)) { } Protocol_DownDataChassis_t; + + /* 电控 -> 视觉 IMU数据结构体*/ + typedef struct __attribute__((packed)) + { + struct __attribute__((packed)) + { + float x; + float y; + float z; + } gyro; /* 陀螺仪数据 */ + + struct __attribute__((packed)) + { + float x; + float y; + float z; + } accl; /* 四元数 */ + struct __attribute__((packed)) + { + float q0; + float q1; + float q2; + float q3; + } quat; /* 四元数 */ + + } Protocol_UpDataIMU_t; + + typedef struct __attribute__((packed)) + { + uint8_t status; + } Protocol_UpDataCMD_t; + /* 视觉 -> 电控 上层机构数据结构体*/ typedef struct __attribute__((packed)) { diff --git a/User/device/nuc.c b/User/device/nuc.c index f7eb9f2..4d866f7 100644 --- a/User/device/nuc.c +++ b/User/device/nuc.c @@ -40,33 +40,6 @@ int8_t NUC_StartReceiving() return DEVICE_OK; return DEVICE_ERR; } -int8_t NUC_StartSending(fp32 *data) -{ - - union - { - float x[1]; - char data[4]; - } instance; - - // for (int i = 0; i < 1; i++) { - instance.x[0] = data[0]; - // } - - SendBuffer[0] = 0xFC; // 帧头 - SendBuffer[1] = 0x01; // 控制帧 - for (int i = 2; i < 6; i++) - { - SendBuffer[i] = instance.data[i - 2]; - } - SendBuffer[6] = 0xFD; // 帧尾 - - if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC), - (uint8_t *)SendBuffer, - sizeof(SendBuffer)) == HAL_OK) - return DEVICE_OK; - return DEVICE_ERR; -} int8_t NUC_Restart(void) { __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC)); @@ -75,11 +48,11 @@ int8_t NUC_Restart(void) } bool_t NUC_WaitDmaCplt(void) { - return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 20) == + return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 0) == SIGNAL_NUC_RAW_REDY); } -int8_t NUC_RawParse(CMD_NUC_t *n) +int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc) { if (n == NULL) return DEVICE_ERR_NULL; @@ -165,6 +138,7 @@ int8_t NUC_RawParse(CMD_NUC_t *n) break; } + nuc->unc_online = true; // 设置为在线状态 return DEVICE_OK; } error: @@ -173,10 +147,51 @@ error: return DEVICE_ERR; } -int8_t NUC_HandleOffline(CMD_NUC_t *cmd) +int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc) { if (cmd == NULL) return DEVICE_ERR_NULL; - // memset(cmd, 0, sizeof(*cmd)); + nuc->unc_online = false; // 设置为离线状态 + memset(cmd, 0, sizeof(*cmd)); return 0; } + +int8_t NUC_PackIMU(NUC_t *nuc, const AHRS_Quaternion_t *quat, const AHRS_Accl_t *accl, const AHRS_Gyro_t *gyro){ + nuc->to_nuc.imu.head = HEAD; + nuc->to_nuc.imu.id = IMU_ID; + nuc->to_nuc.imu.type = TYPE; + nuc->to_nuc.imu.end = TAIL; + memcpy((void *)&(nuc->to_nuc.imu.package.quat), (const void *)quat,sizeof(*quat)); + memcpy((void *)&(nuc->to_nuc.imu.package.gyro), (const void *)gyro,sizeof(*gyro)); + memcpy((void *)&(nuc->to_nuc.imu.package.accl), (const void *)accl,sizeof(*accl)); + return DEVICE_OK; +} + +int8_t NUC_PackCMD(NUC_t *nuc, const NUC_send_t *send){ + nuc->to_nuc.cmd.head = HEAD; + nuc->to_nuc.cmd.id = IMU_ID; + nuc->to_nuc.cmd.type = TYPE; + nuc->to_nuc.cmd.end = TAIL; + // memcpy((void *)&(nuc->to_nuc.cmd.package.status), (const void *)send,sizeof(*send)); + /*在这里添加你们的控制命令*/ + return DEVICE_OK; +} + +int8_t NUC_StartSend(NUC_t *nuc, bool cmd_update){ + if (cmd_update) { + if (HAL_UART_Transmit_DMA( + BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)&(nuc->to_nuc.cmd), + sizeof(nuc->to_nuc.cmd) + sizeof(nuc->to_nuc.imu)) == HAL_OK) + return DEVICE_OK; + else + return DEVICE_ERR; + } else { + if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC), + (uint8_t *)&(nuc->to_nuc.imu), + sizeof(nuc->to_nuc.imu)) == HAL_OK) + return DEVICE_OK; + else + return DEVICE_ERR; + } +} + diff --git a/User/device/nuc.h b/User/device/nuc.h index fad848d..b80178b 100644 --- a/User/device/nuc.h +++ b/User/device/nuc.h @@ -6,8 +6,8 @@ #include "bsp_usart.h" #include "cmd.h" #include "protocol.h" - - +#include "Algorithm/ahrs.h" +#include "Module/Chassis.h" typedef struct { @@ -16,11 +16,33 @@ typedef struct { } NUC_UpPackageMCU_t; typedef struct { -// osThreadId_t thread_alert; + uint8_t head; + uint8_t type; + uint8_t id; // 0x01 IMU帧 + Protocol_UpDataIMU_t package; + uint8_t end; +} NUC_UpPackageIMU_t; +typedef struct { + uint8_t head; + uint8_t type; // 0x01 控制帧 + uint8_t id; + Protocol_UpDataCMD_t package; + uint8_t end; +} NUC_UpPackageCMD_t; + + +typedef struct { +// osThreadId_t thread_alert; + bool unc_online; //是否在线 Protocol_DownPackageChassis_t Nucfor_chassis; //接收的数据协议 - NUC_UpPackageMCU_t to_nuc; //发送的数据协议 + // NUC_UpPackageMCU_t to_nuc; //发送的数据协议 + + struct { + NUC_UpPackageIMU_t imu; + NUC_UpPackageCMD_t cmd; + } to_nuc; uint8_t status;//控制状态 @@ -29,11 +51,14 @@ typedef struct { int8_t NUC_Init(NUC_t *nuc); int8_t NUC_StartReceiving(void); -int8_t NUC_StartSending(fp32 *data) ; bool_t NUC_WaitDmaCplt(void); -int8_t NUC_RawParse(CMD_NUC_t *n); -int8_t NUC_HandleOffline(CMD_NUC_t *cmd); +int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc); +int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc); int8_t NUC_Restart(void); +int8_t NUC_PackIMU(NUC_t *nuc, const AHRS_Quaternion_t *quat, const AHRS_Accl_t *accl, const AHRS_Gyro_t *gyro); +int8_t NUC_PackCMD(NUC_t *nuc, const NUC_send_t *send); +int8_t NUC_StartSend(NUC_t *nuc, bool cmd_update); + #endif diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c index d689212..ccda8a6 100644 --- a/User/task/atti_esti.c +++ b/User/task/atti_esti.c @@ -119,7 +119,7 @@ void Task_AttiEsti(void *argument) { AHRS_GetEulr(&imu_eulr, &gimbal_ahrs); detect_hook(IMU_TOE); - + osKernelUnlock(); osMessageQueueReset(task_runtime.msgq.imu.accl); osMessageQueuePut(task_runtime.msgq.imu.accl, &bmi088.accl, 0, 0); @@ -128,7 +128,13 @@ void Task_AttiEsti(void *argument) { osMessageQueueReset(task_runtime.msgq.imu.eulr); osMessageQueuePut(task_runtime.msgq.imu.eulr, &imu_eulr, 0, 0); - osKernelUnlock(); + + osMessageQueueReset(task_runtime.msgq.nuc.gyro); + osMessageQueuePut(task_runtime.msgq.nuc.gyro, &bmi088.gyro, 0, 0); + osMessageQueueReset(task_runtime.msgq.nuc.accl); + osMessageQueuePut(task_runtime.msgq.nuc.accl, &bmi088.accl, 0, 0); + osMessageQueueReset(task_runtime.msgq.nuc.quat); + osMessageQueuePut(task_runtime.msgq.nuc.quat, &gimbal_ahrs.quat, 0, 0); /* PID控制IMU温度，PWM输出 */ BSP_PWM_Set(BSP_PWM_IMU_HEAT, diff --git a/User/task/init.c b/User/task/init.c index 9b3e7af..ff67827 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -64,6 +64,13 @@ void Task_Init(void *argument) { task_runtime.msgq.can.output.up_dribble_3508 = osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL); + /*nuc*/ + task_runtime.msgq.nuc.quat = + osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL); + task_runtime.msgq.nuc.accl = + osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL); + task_runtime.msgq.nuc.gyro = + osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL); /* imu */ diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c index 29feee1..c66c9ce 100644 --- a/User/task/nuc_task.c +++ b/User/task/nuc_task.c @@ -5,16 +5,24 @@ NUC_t nuc_raw; CMD_NUC_t cmd_fromnuc; +AHRS_Quaternion_t quat; +AHRS_Accl_t accl; +AHRS_Gyro_t gyro; NUC_send_t NUC_send; #else static NUC_t nuc_raw; static CMD_NUC_t cmd_fromnuc; +static AHRS_Quaternion_t quat; +static AHRS_Accl_t accl; +static AHRS_Gyro_t gyro; NUC_send_t NUC_send; #endif fp32 send[4]={11.0f,45.0,1.f,4.0f}; int a=0; + + void Task_nuc(void *argument){ (void)argument; /**/ @@ -24,36 +32,47 @@ void Task_nuc(void *argument){ NUC_Init(&nuc_raw); uint32_t tick = osKernelGetTickCount(); - + uint32_t last_online_tick = tick; while (1) { #ifdef DEBUG - task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId()); - task_runtime .freq.nuc = TASK_FREQ_NUC; - task_runtime.last_up_time.nuc= tick; - + task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId()); #endif a++; NUC_StartReceiving(); - // NUC_StartSending(NUC_send.send); - // NUC_StartSending (send); if (NUC_WaitDmaCplt()){ - NUC_RawParse(&cmd_fromnuc); + NUC_RawParse(&cmd_fromnuc, &nuc_raw); + last_online_tick = tick; } else{ - NUC_HandleOffline(&cmd_fromnuc); + if (tick - last_online_tick > 300) NUC_HandleOffline(&cmd_fromnuc, &nuc_raw); } - osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc); - osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0); - osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0); - - + + if (nuc_raw.unc_online) { + osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc); + osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0); + } + + osMessageQueueGet(task_runtime.msgq.nuc.quat, &(quat), NULL, 0); + osMessageQueueGet(task_runtime.msgq.nuc.accl, &(accl), NULL, 0); + osMessageQueueGet(task_runtime.msgq.nuc.gyro, &(gyro), NULL, 0); + + // osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0); + + bool cmd_update = (osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, + &(NUC_send), NULL, 0) == osOK); + + NUC_PackIMU(&nuc_raw, &quat, &accl, &gyro); + if (cmd_update) { + NUC_PackCMD(&nuc_raw,&NUC_send); + } + + + // NUC_StartSend(&nuc_raw, cmd_update); + tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/ osDelayUntil(tick); - } - - - + } diff --git a/User/task/user_task.h b/User/task/user_task.h index 81c836e..9f2ba0f 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -18,7 +18,7 @@ #define TASK_FREQ_UP (900u) #define TASK_FREQ_CTRL_CMD (500u) -#define TASK_FREQ_NUC (500u) +#define TASK_FREQ_NUC (250u) #define TASK_FREQ_CAN (1000u) #define TASK_FREQ_RC (1000u) @@ -54,15 +54,20 @@ typedef struct { osMessageQueueId_t eulr; /* 姿态结算得到欧拉角 */ } imu; /* 控制指令 */ + struct { + osMessageQueueId_t quat; /* 姿态解算得到 */ + osMessageQueueId_t accl; /* IMU读取 */ + osMessageQueueId_t gyro; /* IMU读取 */ + } nuc; + struct { struct { osMessageQueueId_t rc; osMessageQueueId_t nuc; // osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/ osMessageQueueId_t nuc_send; //给nuc发 - }raw; - + /*控制分离*/ osMessageQueueId_t UP_cmd_ctrl_t; osMessageQueueId_t CHASSIS_cmd_ctrl_t;