From 4d1702b577c02366daeffe654e642cd93279dedc Mon Sep 17 00:00:00 2001 From: SkyLi Date: Fri, 29 May 2026 23:37:36 +0800 Subject: [PATCH] refactor: migrate from DJI motors to DM motors, simplify to gimbal-only control MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Rewrite DM motor driver based on DM_6220 reference protocol (MIT/PosVel/Velocity modes) - Use velocity control mode: MCU runs angle PID with IMU feedback, motor handles speed loop - Rewrite vision protocol with 3 frame types (0xA5 auto data, 0xA6 manual data, 0xA7 command) - Decouple serial parsing (opencv module) from mode switching (robot_cmd module) - Implement dual-loop architecture: vision PID outer loop → angle ref → motor PID inner loop - Add KF/PID reset on mode switch transitions - Merge motor_reverse_flag and feedback_reverse_flag into single output_reverse flag - Remove DMMotorEnable/DMMotorStop (motor always runs, controlled by angle ref) - Remove unused modules: DJI/LK/HT/step/servo motors, chassis, shoot, referee - Add ExitRun0Mode() for STM32H723 power supply configuration - Clean up CMakeLists: 115 → 76 C source files --- CMakeLists.txt | 39 +- Core/Src/fdcan.c | 4 +- application/cmd/robot_cmd.c | 991 +++++++++----------------------- application/cmd/robot_cmd.h | 8 +- application/gimbal/gimbal.c | 208 +++---- application/gimbal/gimbal.h | 6 +- application/robot.c | 43 +- application/robot.h | 6 +- application/robot_def.h | 190 +----- application/robot_task.h | 58 +- modules/algorithm/controller.c | 4 +- modules/motor/DMmotor/dmmotor.c | 316 +++++----- modules/motor/DMmotor/dmmotor.h | 133 +++-- modules/motor/motor_def.h | 36 +- modules/motor/motor_task.c | 22 +- modules/opencv/opencv.c | 138 ++--- modules/opencv/opencv.h | 73 ++- 17 files changed, 790 insertions(+), 1485 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 63044e7..80350d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,47 +75,35 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE bsp/log/bsp_log.c bsp/bsp_tools.c - # modules + # modules - algorithm modules/algorithm/controller.c modules/algorithm/kalman_filter.c modules/algorithm/QuaternionEKF.c modules/algorithm/crc8.c modules/algorithm/crc16.c modules/algorithm/user_lib.c - modules/bluetooth/HC05.c + # modules - sensors modules/BMI088/bmi088.c modules/imu/BMI088driver.c modules/imu/BMI088Middleware.c modules/imu/ins_task.c + # modules - communication modules/master_machine/master_process.c modules/master_machine/seasky_protocol.c - modules/motor/DJImotor/dji_motor.c - modules/motor/HTmotor/HT04.c - modules/motor/LKmotor/LK9025.c - modules/motor/DMmotor/dmmotor.c - modules/motor/step_motor/step_motor.c - modules/motor/step_motor/X_V2.c - modules/motor/servo_motor/servo_motor.c - modules/motor/motor_task.c - modules/oled/oled.c - modules/referee/crc_ref.c - modules/referee/rm_referee.c - modules/referee/referee_UI.c - modules/referee/referee_task.c modules/remote/remote_control.c - modules/super_cap/super_cap.c - modules/can_comm/can_comm.c + modules/timachine/board_comm.c + # modules - motor + modules/motor/DMmotor/dmmotor.c + modules/motor/motor_task.c + # modules - system modules/message_center/message_center.c modules/daemon/daemon.c modules/alarm/buzzer.c modules/vofa/vofa.c modules/opencv/opencv.c - modules/timachine/board_comm.c # application application/gimbal/gimbal.c - application/chassis/chassis.c - application/shoot/shoot.c application/cmd/robot_cmd.c application/robot.c ) @@ -144,21 +132,12 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE # modules modules/algorithm - modules/bluetooth modules/BMI088 modules/imu modules/master_machine - modules/motor/DJImotor - modules/motor/LKmotor - modules/motor/HTmotor - modules/motor/step_motor - modules/motor/servo_motor modules/motor/DMmotor modules/motor - modules/oled - modules/referee modules/remote - modules/super_cap modules/can_comm modules/message_center modules/daemon @@ -169,8 +148,6 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE modules # application - application/chassis - application/shoot application/gimbal application/cmd application diff --git a/Core/Src/fdcan.c b/Core/Src/fdcan.c index 469fd04..3a41870 100644 --- a/Core/Src/fdcan.c +++ b/Core/Src/fdcan.c @@ -46,7 +46,7 @@ void MX_FDCAN1_Init(void) hfdcan1.Init.TransmitPause = DISABLE; hfdcan1.Init.ProtocolException = ENABLE; hfdcan1.Init.NominalPrescaler = 3; - hfdcan1.Init.NominalSyncJumpWidth = 1; + hfdcan1.Init.NominalSyncJumpWidth = 10; hfdcan1.Init.NominalTimeSeg1 = 29; hfdcan1.Init.NominalTimeSeg2 = 10; hfdcan1.Init.DataPrescaler = 3; @@ -54,7 +54,7 @@ void MX_FDCAN1_Init(void) hfdcan1.Init.DataTimeSeg1 = 29; hfdcan1.Init.DataTimeSeg2 = 10; hfdcan1.Init.MessageRAMOffset = 0; - hfdcan1.Init.StdFiltersNbr = 14; + hfdcan1.Init.StdFiltersNbr = 1; hfdcan1.Init.ExtFiltersNbr = 0; hfdcan1.Init.RxFifo0ElmtsNbr = 4; hfdcan1.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 9572800..6e92988 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -1,801 +1,324 @@ -// app +/* app */ #include "robot_def.h" #include "robot_cmd.h" -// module +/* module */ #include "controller.h" #include "remote_control.h" #include "ins_task.h" -#include "master_process.h" #include "message_center.h" #include "general_def.h" -#include "dji_motor.h" -#include "bmi088.h" -#include "vofa.h" -#include "board_comm.h" -// bsp +#include "kalman_filter.h" +#include "opencv.h" +/* bsp */ #include "bsp_dwt.h" #include "bsp_log.h" -#include "opencv.h" -#include "step_motor.h" +/* ---- 模式 ---- */ +static Vision_Mode_e current_mode = VISION_MODE_MANUAL; -#include "kalman_filter.h" +/* ---- 视觉追踪 PID (外环) ---- */ +static PIDInstance vision_yaw_pid; +static PIDInstance vision_pitch_pid; -#include "arm_math.h" - -#define mapping(x, in_min, in_max, out_min, out_max) \ - ((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min) - -// 添加卡尔曼滤波器实例 +/* 视觉误差卡尔曼滤波器 */ static KalmanFilter_t yaw_error_kf; static KalmanFilter_t pitch_error_kf; -static float filtered_yaw_error = 0.0f; -static float filtered_pitch_error = 0.0f; +static float filtered_yaw_error; +static float filtered_pitch_error; -// 私有宏,自动将编码器转换成角度值 -#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360 -#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360 +/* 内环角度参考累计值 */ +static float gimbal_yaw_ref; +static float gimbal_pitch_ref; -#define yaw_control_kxmax 0.001f -#define yaw_control_kxmin 0.0005f -#define pitch_control_kxmax 0.001f -#define pitch_control_kxmin 0.0004f +/* ---- pub-sub ---- */ +static Publisher_t *gimbal_cmd_pub; +static Subscriber_t *gimbal_feed_sub; +static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; +static Gimbal_Upload_Data_s gimbal_fetch_data; -/* cmd应用包含的模块实例指针和交互信息存储*/ -#ifdef GIMBAL_BOARD // 对双板的兼容,条件编译 -#include "can_comm.h" -static CANCommInstance *cmd_can_comm; // 双板通信 -#endif -#ifdef ONE_BOARD -static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者 -static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者 -#endif // ONE_BOARD +/* ---- 遥控器 ---- */ +static RC_ctrl_t *rc_data; -static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 -static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 +/* ---- 机器人状态 ---- */ +static Robot_Status_e robot_state; -static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回 -static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回 -static Vision_Send_s vision_send_data; // 视觉发送数据 -static board_comm_data_t *board_comm_data; // 板间通信数据指针,初始化时返回 - -static Publisher_t *gimbal_cmd_pub; // 云台控制消息发布者 -static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者 -static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; // 传递给云台的控制信息 -static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息 - -static Publisher_t *shoot_cmd_pub; // 发射控制消息发布者 -static Subscriber_t *shoot_feed_sub; // 发射反馈信息订阅者 -static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息 -static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息 - -static Robot_Status_e robot_state; // 机器人整体工作状态 - -PIDInstance yaw_tracking_pid; // 云台yaw跟随pid -PIDInstance pitch_tracking_pid; // 云台pitch跟随pid -static float yaw_tracking_pid_output; // 云台yaw跟随pid输出 -static float pitch_tracking_pid_output; // 云台pitch跟随pid输出 - -static float vofa_debug[7]; - -// #define PITCH_HORIZON_ECD 6800 // 俯仰水平时的电机编码器 - -// 在RobotCMDInit()中添加卡尔曼滤波器初始化 -static void VisionKalmanFilterInit() -{ - // yaw误差卡尔曼滤波器初始化 - Kalman_Filter_Init(&yaw_error_kf, 2, 0, 1); // 状态2维(位置,速度), 控制0维, 观测1维 - - static float P_init_yaw[4] = { - 5.0f, 0.0f, // 减小初始不确定性 - 0.0f, 5.0f}; - memcpy(yaw_error_kf.P_data, P_init_yaw, sizeof(P_init_yaw)); - - // 状态转移矩阵F (dt = 1/200 = 0.005s, 200Hz控制频率) - static float F_init_yaw[4] = { - 1.0f, 0.005f, // 修正时间步长 - 0.0f, 1.0f}; - memcpy(yaw_error_kf.F_data, F_init_yaw, sizeof(F_init_yaw)); - - // 过程噪声协方差矩阵Q - static float Q_init_yaw[4] = { - 0.1f, 0.0f, // 增大位置过程噪声 - 0.0f, 1.0f // 增大速度过程噪声 - }; - memcpy(yaw_error_kf.Q_data, Q_init_yaw, sizeof(Q_init_yaw)); - - // 观测矩阵H - static uint8_t measurement_map_yaw[1] = {1}; // 观测第1个状态(位置) - static float measurement_degree_yaw[1] = {1.0f}; - static float mat_R_diagonal_yaw[1] = {1.0f}; // 减小观测噪声，增加对观测的信任 - static float state_min_variance_yaw[2] = {0.001f, 0.001f}; // 减小最小方差 - - yaw_error_kf.UseAutoAdjustment = 1; - memcpy(yaw_error_kf.MeasurementMap, measurement_map_yaw, sizeof(measurement_map_yaw)); - memcpy(yaw_error_kf.MeasurementDegree, measurement_degree_yaw, sizeof(measurement_degree_yaw)); - memcpy(yaw_error_kf.MatR_DiagonalElements, mat_R_diagonal_yaw, sizeof(mat_R_diagonal_yaw)); - memcpy(yaw_error_kf.StateMinVariance, state_min_variance_yaw, sizeof(state_min_variance_yaw)); - - // pitch误差卡尔曼滤波器初始化 - Kalman_Filter_Init(&pitch_error_kf, 2, 0, 1); // 状态2维(位置,速度), 控制0维, 观测1维 - static float P_init_pitch[4] = { - 5.0f, 0.0f, // 减小初始不确定性 - 0.0f, 5.0f}; - memcpy(pitch_error_kf.P_data, P_init_pitch, sizeof(P_init_pitch)); - // 状态转移矩阵F (dt = 1/200 = 0.005s, 200Hz控制频率) - static float F_init_pitch[4] = { - 1.0f, 0.005f, // 修正时间步长 - 0.0f, 1.0f}; - memcpy(pitch_error_kf.F_data, F_init_pitch, sizeof(F_init_pitch)); - // 过程噪声协方差矩阵Q - static float Q_init_pitch[4] = { - 0.1f, 0.0f, // 增大位置过程噪声 - 0.0f, 1.0f // 增大速度过程噪声 - }; - memcpy(pitch_error_kf.Q_data, Q_init_pitch, sizeof(Q_init_pitch)); - // 观测矩阵H - static uint8_t measurement_map_pitch[1] = {1}; // 观测第1个状态(位置) - static float measurement_degree_pitch[1] = {1.0f}; - static float mat_R_diagonal_pitch[1] = {1.0f}; // 减小观测噪声，增加对观测的信任 - static float state_min_variance_pitch[2] = {0.001f, 0.001f}; // 减小最小方差 - pitch_error_kf.UseAutoAdjustment = 1; - memcpy(pitch_error_kf.MeasurementMap, measurement_map_pitch, sizeof(measurement_map_pitch)); - memcpy(pitch_error_kf.MeasurementDegree, measurement_degree_pitch, sizeof(measurement_degree_pitch)); - memcpy(pitch_error_kf.MatR_DiagonalElements, mat_R_diagonal_pitch, sizeof(mat_R_diagonal_pitch)); - memcpy(pitch_error_kf.StateMinVariance, state_min_variance_pitch, sizeof(state_min_variance_pitch)); -} - -void RobotCMDInit() -{ - rc_data = RemoteControlInit(&huart5); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 - gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); - gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); - shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s)); - shoot_feed_sub = SubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s)); - chassis_cmd_pub = PubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s)); - chassis_feed_sub = SubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s)); - - static PID_Init_Config_s yaw_tracking_pid_config = { - .Kp = 1.5f, - .Ki = 3.0f, - .Kd = 0.07f, - .IntegralLimit = 100.0f, - .CoefB = 20.0f, - .CoefA = 20.0f, - // .Output_LPF_RC = 0.002f, // 100Hz - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate, - .MaxOut = 200.0f, - }; - static PID_Init_Config_s pitch_tracking_pid_config = { - .Kp = 1.0f, - .Ki = 2.0f, - .Kd = 0.05f, - .IntegralLimit = 100.0f, - .CoefB = 10.0f, - .CoefA = 30.0f, - // .Output_LPF_RC = 0.002f, // 100Hz - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate, - .MaxOut = 200.0f, - }; - - PIDInit(&yaw_tracking_pid, &yaw_tracking_pid_config); - PIDInit(&pitch_tracking_pid, &pitch_tracking_pid_config); - VisionKalmanFilterInit(); - Vision_Uart_Init(&huart10); - board_comm_data = BoardCommInit(&huart1); // 初始化板间通信数据指针 - robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 -} - -static uint8_t first_circle_flag = 1; -static float circle_progress = 0; // 画圆进度 100%为一圈 -static float circle_radius = 60.0f; // 画圆半径 -static float circle_angle = 0.0f; // 当前圆周角度 -static float target_x = 0.0f; // 圆周上目标点x坐标 -static float target_y = 0.0f; // 圆周上目标点y坐标 - -/** - * @brief 控制输入为遥控器(调试时)的模式和控制量设置 - * - */ -static void RemoteControlSet() -{ - chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - if (!switch_is_down(rc_data[TEMP].rc.switch_left)) - { - if (Vison_revc_flag && switch_is_mid(rc_data[TEMP].rc.switch_left)) - { - - LIMIT_MIN_MAX(opencv_Vision_data[0], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[1], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[2], 500.0f, 1560.0f); - yaw_error_kf.MeasuredVector[0] = opencv_Vision_data[0]; - float *yaw_filtered = Kalman_Filter_Update(&yaw_error_kf); - filtered_yaw_error = yaw_filtered[0]; - pitch_error_kf.MeasuredVector[0] = opencv_Vision_data[1]; - float *pitch_filtered = Kalman_Filter_Update(&pitch_error_kf); - filtered_pitch_error = pitch_filtered[0]; - - yaw_tracking_pid_output = PIDCalculate(&yaw_tracking_pid, filtered_yaw_error, 0); - pitch_tracking_pid_output = PIDCalculate(&pitch_tracking_pid, filtered_pitch_error, 0); - gimbal_cmd_send.yaw -= yaw_tracking_pid_output * mapping(opencv_Vision_data[2], 500.0f, 1560.0f, yaw_control_kxmax, yaw_control_kxmin); // yaw角度控制 - gimbal_cmd_send.pitch += 0.001f * pitch_tracking_pid_output; // pitch角度控制 - vofa_debug[0] = opencv_Vision_data[0]; // 调试用 - vofa_debug[1] = filtered_yaw_error; // 调试用 - vofa_debug[2] = yaw_tracking_pid.Err; // 调试用 - vofa_debug[3] = yaw_tracking_pid.Pout; // 调试用 - vofa_debug[4] = yaw_tracking_pid.Iout; // 调试用 - vofa_debug[5] = yaw_tracking_pid.Dout; // 调试用 - vofa_debug[6] = yaw_tracking_pid.Output; // 调试用 - vofa_justfloat_output(vofa_debug, 7, &huart7); - first_circle_flag = 1; // 重置画圆标志 - } - else if (switch_is_up(rc_data[TEMP].rc.switch_left)) - { - if (first_circle_flag) - { - circle_progress = 0; - circle_angle = 0.0f; - first_circle_flag = 0; - } - LIMIT_MIN_MAX(opencv_Vision_data[0], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[1], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[2], 500.0f, 1560.0f); - yaw_error_kf.MeasuredVector[0] = opencv_Vision_data[0]; - float *yaw_filtered = Kalman_Filter_Update(&yaw_error_kf); - filtered_yaw_error = yaw_filtered[0]; - pitch_error_kf.MeasuredVector[0] = opencv_Vision_data[1]; - float *pitch_filtered = Kalman_Filter_Update(&pitch_error_kf); - filtered_pitch_error = pitch_filtered[0]; - // 画圆模式 500hz 16s画完整圆 - circle_progress += 0.000125f; // 每次增加1/16 - if (circle_progress >= 1.0f) - { - circle_progress = 1.0f; // 限制在100% - } - circle_angle = 2.0f * PI * circle_progress; // 计算当前圆周角度 - target_x = circle_radius * cosf(circle_angle); // 计算圆周上目标点x坐标 - target_y = circle_radius * sinf(circle_angle); // 计算圆周上目标点y坐标 - yaw_tracking_pid_output = PIDCalculate(&yaw_tracking_pid, filtered_yaw_error, target_x); - pitch_tracking_pid_output = PIDCalculate(&pitch_tracking_pid, filtered_pitch_error, target_y); - gimbal_cmd_send.yaw -= yaw_tracking_pid_output * mapping(opencv_Vision_data[2], 500.0f, 1560.0f, yaw_control_kxmax, yaw_control_kxmin); // yaw角度控制 - gimbal_cmd_send.pitch += pitch_tracking_pid_output * mapping(opencv_Vision_data[2], 500.0f, 1560.0f, pitch_control_kxmax, pitch_control_kxmin); // pitch角度控制 - } - else - { - first_circle_flag = 1; // 重置画圆标志 - gimbal_cmd_send.yaw -= 0.0025f * (float)rc_data[TEMP].rc.rocker_r_; // yaw角度控制 - gimbal_cmd_send.pitch += 0.00125f * (float)rc_data[TEMP].rc.rocker_r1; // pitch角度控制 - } - } - else - { - gimbal_cmd_send.yaw -= 0.0025f * (float)rc_data[TEMP].rc.rocker_r_; // yaw角度控制 - gimbal_cmd_send.pitch += 0.00125f * (float)rc_data[TEMP].rc.rocker_r1; // pitch角度控制 - } - - if (gimbal_cmd_send.pitch > 30.0f) // 限制pitch角度 - gimbal_cmd_send.pitch = 30.0f; - else if (gimbal_cmd_send.pitch < -30.0f) - gimbal_cmd_send.pitch = -30.0f; -} - -void laser_on() +/* ---- 激光 ---- */ +static void laser_on(void) { HAL_GPIO_WritePin(POWER_5V_GPIO_Port, POWER_5V_Pin, GPIO_PIN_SET); } -void laser_off() +static void laser_off(void) { HAL_GPIO_WritePin(POWER_5V_GPIO_Port, POWER_5V_Pin, GPIO_PIN_RESET); } -enum YunTaiState_e + +/* ---- 视觉 PID 和卡尔曼滤波器重置 ---- */ +static void VisionPIDReset(void) { - YUNT_IDLE = 0, // 云台无任务 - YUNT_NO1, // 云台题号1准备 - YUNT_NO2_WAITING, - YUNT_NO2, - YUNT_NO3_WAITING, - YUNT_NO3_TURING, - YUNT_NO3_SEARCHING, - YUNT_NO3_TRACKING, - YUNT_NO4_1_WAITING, - YUNT_NO4_1_TURING, - YUNT_NO4_1_TRACKING, - YUNT_NO4_1_TRACKING_RUNNING, - YUNT_NO4_3_WAITING, - YUNT_NO4_3_TURING, - YUNT_NO4_3_TRACKING, - YUNT_NO4_3_TRACKING_RUNNING, -} YunTaiState = YUNT_IDLE; // 云台状态机 + /* 清零积分项和上次误差 */ + vision_yaw_pid.Iout = 0; + vision_yaw_pid.ITerm = 0; + vision_yaw_pid.Last_Err = 0; + vision_yaw_pid.Err = 0; + vision_yaw_pid.Pout = 0; + vision_yaw_pid.Dout = 0; + vision_yaw_pid.Output = 0; + vision_yaw_pid.Last_Dout = 0; -static float yuntai_no3_yawtarget = 0.0f; // 云台题号3目标yaw角度 -static float yuntai_no3_searching_yaw = 0.0f; // 云台题号3搜索yaw角度 -static float yuntai_no3_searching_yaw_max = 60.0f; // 云台yaw题号3搜索最大yaw角度 -static float yuntai_no3_searching_yaw_min = -60.0f; // 云台yaw题号3搜索最小yaw角度 -static float yuntai_no3_searching_step = 0.2f; // 云台题号3搜索步长,每次0.18度 -static uint8_t yuntai_no3_searching_dir = 0; // 云台题号3搜索方向, 0: -, 1:+ -static int16_t yuntai_no3_searching_count = 50; // 3计数器 + vision_pitch_pid.Iout = 0; + vision_pitch_pid.ITerm = 0; + vision_pitch_pid.Last_Err = 0; + vision_pitch_pid.Err = 0; + vision_pitch_pid.Pout = 0; + vision_pitch_pid.Dout = 0; + vision_pitch_pid.Output = 0; + vision_pitch_pid.Last_Dout = 0; -static uint8_t no4_turning_count = 0; // 题号4转向计数器 -static uint8_t no4_turning_status = 0; // 题号4转向状态 -static uint8_t no4_turning_feedforward = 0.05; + /* 重置滤波器状态: x = [0, 0], P = diag(5, 5) */ + memset(yaw_error_kf.xhat_data, 0, yaw_error_kf.xhatSize * sizeof(float)); + memset(yaw_error_kf.P_data, 0, yaw_error_kf.xhatSize * yaw_error_kf.xhatSize * sizeof(float)); + memset(pitch_error_kf.xhat_data, 0, pitch_error_kf.xhatSize * sizeof(float)); + memset(pitch_error_kf.P_data, 0, pitch_error_kf.xhatSize * pitch_error_kf.xhatSize * sizeof(float)); -static float timestart_timeline = 0.0f; // 题号开始时间 + float P_diag[4] = {5.0f, 0.0f, 0.0f, 5.0f}; + memcpy(yaw_error_kf.P_data, P_diag, sizeof(P_diag)); + memcpy(pitch_error_kf.P_data, P_diag, sizeof(P_diag)); -/** - * @brief 题号状态机 - * - */ -static void AutoSet() + filtered_yaw_error = 0; + filtered_pitch_error = 0; +} + +/* ---- 模式切换处理 ---- */ +static void SwitchToAutoMode(void) { + if (current_mode == VISION_MODE_AUTO) + return; - yaw_tracking_pid.Ki = 3.0f; + /* 同步角度参考值为当前 IMU 角度 */ + gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; + gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data.Pitch; - switch (board_comm_data->rx_int_data[0]) // 根据板间通信数据的第一个整数值来判断题号 + vision_data.manual_updated = 0; // 清除手动数据残留 + VisionPIDReset(); + current_mode = VISION_MODE_AUTO; + LOGINFO("[CMD] switched to AUTO mode"); +} + +static void SwitchToManualMode(void) +{ + if (current_mode == VISION_MODE_MANUAL) + return; + + gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; + gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data.Pitch; + + vision_data.auto_updated = 0; // 清除自动数据残留 + current_mode = VISION_MODE_MANUAL; + LOGINFO("[CMD] switched to MANUAL mode"); +} + +/* ---- 命令处理 ---- */ +static void ProcessCommand(void) +{ + if (!vision_data.cmd_pending) + return; + + vision_data.cmd_pending = 0; + + switch (vision_data.cmd) { - case 1: - YunTaiState = YUNT_NO1; + case VISION_CMD_SWITCH_AUTO: + SwitchToAutoMode(); break; - case 2: - if (board_comm_data->rx_int_data[1] == 0) - { - YunTaiState = YUNT_NO2_WAITING; - } - else - { - YunTaiState = YUNT_NO2; // 进入题号2状态 - } + case VISION_CMD_SWITCH_MANUAL: + SwitchToManualMode(); break; - case 3: - if (board_comm_data->rx_int_data[1] == 0) - { - YunTaiState = YUNT_NO3_WAITING; - } - else if (YunTaiState == YUNT_NO3_WAITING) - { - YunTaiState = YUNT_NO3_TURING; // 进入题号3转向状态 - } - break; - case 4: - case 5: - if (board_comm_data->rx_int_data[1] == 0) - { - YunTaiState = YUNT_NO4_1_WAITING; // 进入题号4-1等待状态 - } - else if (board_comm_data->rx_int_data[1] == 1 && YunTaiState == YUNT_NO4_1_WAITING) - { - YunTaiState = YUNT_NO4_1_TURING; // 进入题号4-1转向状态 - } - else if (board_comm_data->rx_int_data[1] == 2) - { - YunTaiState = YUNT_NO4_1_TRACKING_RUNNING; - } - break; - case 6: - if (board_comm_data->rx_int_data[1] == 0) - { - YunTaiState = YUNT_NO4_3_WAITING; // 进入题号4-3等待状态 - } - else if (board_comm_data->rx_int_data[1] == 1 && YunTaiState == YUNT_NO4_3_WAITING) - { - YunTaiState = YUNT_NO4_3_TURING; // 进入题号4-3状态 - } - else if (board_comm_data->rx_int_data[1] == 2) - { - YunTaiState = YUNT_NO4_3_TRACKING_RUNNING; - } - default: - break; - } - - switch (YunTaiState) - { - case YUNT_NO1: - gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; - chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; - gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; - gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; - laser_off(); - break; - case YUNT_NO2_WAITING: - gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; - gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; - gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; - timestart_timeline = DWT_GetTimeline_ms(); // 记录开始时间 - laser_off(); - break; - case YUNT_NO2: - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - if (DWT_GetTimeline_ms() - timestart_timeline >= 1500.0f) - { - laser_on(); - } - else - { - if (Vison_revc_flag) - { - LIMIT_MIN_MAX(opencv_Vision_data[0], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[1], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[2], 500.0f, 1560.0f); - yaw_error_kf.MeasuredVector[0] = opencv_Vision_data[0]; - float *yaw_filtered = Kalman_Filter_Update(&yaw_error_kf); - filtered_yaw_error = yaw_filtered[0]; - pitch_error_kf.MeasuredVector[0] = opencv_Vision_data[1]; - float *pitch_filtered = Kalman_Filter_Update(&pitch_error_kf); - filtered_pitch_error = pitch_filtered[0]; - yaw_tracking_pid_output = PIDCalculate(&yaw_tracking_pid, filtered_yaw_error, 0); - pitch_tracking_pid_output = PIDCalculate(&pitch_tracking_pid, filtered_pitch_error, 0); - gimbal_cmd_send.yaw -= yaw_tracking_pid_output * mapping(opencv_Vision_data[2], 500.0f, 1560.0f, yaw_control_kxmax, yaw_control_kxmin); // yaw角度控制 - gimbal_cmd_send.pitch += 0.001f * pitch_tracking_pid_output; - } - } - - break; - case YUNT_NO3_WAITING: - gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; - gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; - gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; - // 计算0度对应最接近的多圈yaw - yuntai_no3_yawtarget = roundf(gimbal_fetch_data.gimbal_imu_data.YawTotalAngle / 360.0f) * 360.0f; // 计算最近的多圈yaw - laser_off(); - timestart_timeline = DWT_GetTimeline_ms(); // 记录开始时间 - break; - case YUNT_NO3_TURING: - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - float yaw_diff = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle - yuntai_no3_yawtarget; // 计算当前yaw与目标yaw的差值 - float step_size = 0.8f; // 90度每秒, 500hz控制频率下每次0.18度 - if (fabsf(yaw_diff) > step_size) // 如果差值大于180度,则调整到最小角度 - { - if (yaw_diff > 0) - gimbal_cmd_send.yaw -= step_size; - else - gimbal_cmd_send.yaw += step_size; - } - if (fabsf(yaw_diff) <= 5.0f) // 如果差值小于5度,则认为转向完成 - { - if (Vison_revc_flag) - { - YunTaiState = YUNT_NO3_TRACKING; // 进入跟踪状态 - } - else - { - YunTaiState = YUNT_NO3_SEARCHING; // 进入搜索状态 - } - } - gimbal_cmd_send.pitch = 0; - break; - case YUNT_NO3_TRACKING: - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - if (DWT_GetTimeline_ms() - timestart_timeline >= 3500.0f) // 如果超过3.5秒,则认为跟踪完成 - { - laser_on(); - } - else - { - if (Vison_revc_flag && Vison_Frequency >= 20.0f) // 如果视觉数据可用且频率大于20Hz - { - LIMIT_MIN_MAX(opencv_Vision_data[0], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[1], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[2], 500.0f, 1560.0f); - yaw_error_kf.MeasuredVector[0] = opencv_Vision_data[0]; - float *yaw_filtered = Kalman_Filter_Update(&yaw_error_kf); - filtered_yaw_error = yaw_filtered[0]; - pitch_error_kf.MeasuredVector[0] = opencv_Vision_data[1]; - float *pitch_filtered = Kalman_Filter_Update(&pitch_error_kf); - filtered_pitch_error = pitch_filtered[0]; - - yaw_tracking_pid_output = PIDCalculate(&yaw_tracking_pid, filtered_yaw_error, 0); - pitch_tracking_pid_output = PIDCalculate(&pitch_tracking_pid, filtered_pitch_error, 0); - gimbal_cmd_send.yaw -= yaw_tracking_pid_output * mapping(opencv_Vision_data[2], 500.0f, 1560.0f, yaw_control_kxmax, yaw_control_kxmin); // yaw角度控制 - gimbal_cmd_send.pitch += 0.001f * pitch_tracking_pid_output; - } - else - { - YunTaiState = YUNT_NO3_SEARCHING; // 如果视觉数据不可用,则进入搜索状态 - } - } - - break; - case YUNT_NO3_SEARCHING: - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - if (yuntai_no3_searching_dir == 0) // 如果当前是向左搜索 - { - gimbal_cmd_send.yaw += yuntai_no3_searching_step; // 向左搜索 - if (gimbal_cmd_send.yaw > yuntai_no3_searching_yaw_max + yuntai_no3_yawtarget) // 如果超过最小值,则切换方向 - { - yuntai_no3_searching_dir = 1; // 切换到向右搜索 - } - } - else // 当前是向右搜索 - { - gimbal_cmd_send.yaw -= yuntai_no3_searching_step; // 向右搜索 - if (gimbal_cmd_send.yaw < yuntai_no3_searching_yaw_min + yuntai_no3_yawtarget) // 如果超过最大值,则切换方向 - { - yuntai_no3_searching_dir = 0; // 切换到向左搜索 - } - } - - gimbal_cmd_send.pitch = 0.0f; // 保持pitch角度为0 - if (Vison_revc_flag && Vison_Frequency >= 20.0f) - { - yuntai_no3_searching_count--; - } - else - { - yuntai_no3_searching_count = 100; // 如果视觉数据可用但频率低于20Hz,则重置计数器 - } - if (yuntai_no3_searching_count <= 0) - { - YunTaiState = YUNT_NO3_TRACKING; // 如果视觉数据可用,则进入跟踪状态 - } - break; - case YUNT_NO4_1_WAITING: - gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; - gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; - gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; - // 计算0度对应最接近的多圈yaw - yuntai_no3_yawtarget = roundf(gimbal_fetch_data.gimbal_imu_data.YawTotalAngle / 360.0f) * 360.0f; // 计算最近的多圈yaw - no4_turning_count = 0; // 题号4转向计数器 - no4_turning_status = 0; // 题号4转向状态 - timestart_timeline = DWT_GetTimeline_ms(); // 记录开始时间 - laser_off(); - break; - case YUNT_NO4_1_TURING: - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - gimbal_cmd_send.yaw = yuntai_no3_yawtarget - 45.0f; - gimbal_cmd_send.pitch = 0.0f; // 保持pitch角度为0 - if (fabsf(gimbal_fetch_data.gimbal_imu_data.YawTotalAngle - yuntai_no3_yawtarget + 45.0f) < 5.0f) - { - YunTaiState = YUNT_NO4_1_TRACKING; // 进入跟踪状态 - } - break; - case YUNT_NO4_1_TRACKING: - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - if (Vison_revc_flag) - { - LIMIT_MIN_MAX(opencv_Vision_data[0], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[1], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[2], 500.0f, 1560.0f); - yaw_error_kf.MeasuredVector[0] = opencv_Vision_data[0]; - float *yaw_filtered = Kalman_Filter_Update(&yaw_error_kf); - filtered_yaw_error = yaw_filtered[0]; - pitch_error_kf.MeasuredVector[0] = opencv_Vision_data[1]; - float *pitch_filtered = Kalman_Filter_Update(&pitch_error_kf); - filtered_pitch_error = pitch_filtered[0]; - - yaw_tracking_pid_output = PIDCalculate(&yaw_tracking_pid, filtered_yaw_error, 0); - pitch_tracking_pid_output = PIDCalculate(&pitch_tracking_pid, filtered_pitch_error, 0); - gimbal_cmd_send.yaw -= yaw_tracking_pid_output * mapping(opencv_Vision_data[2], 500.0f, 1560.0f, yaw_control_kxmax, yaw_control_kxmin); // yaw角度控制 - gimbal_cmd_send.pitch += 0.001f * pitch_tracking_pid_output; - } - break; - case YUNT_NO4_1_TRACKING_RUNNING: - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - if (board_comm_data->rx_float_data[1] >= 600.0f) - { - no4_turning_status = 1; // 题号4转向状态 - if (board_comm_data->rx_float_data[0] >= 0.0f && board_comm_data->rx_float_data[0] <= 12.5f) - { - no4_turning_count = 1; - } - else if (board_comm_data->rx_float_data[0] > 12.5f && board_comm_data->rx_float_data[0] <= 37.5f) - { - no4_turning_count = 2; - } - else if (board_comm_data->rx_float_data[0] > 37.5f && board_comm_data->rx_float_data[0] <= 62.5f) - { - no4_turning_count = 3; - } - else if (board_comm_data->rx_float_data[0] > 62.5f && board_comm_data->rx_float_data[0] <= 87.5f) - { - no4_turning_count = 4; - } - } - else - { - no4_turning_status = 0; // 题号4转向状态 - } - if (Vison_revc_flag) - { - LIMIT_MIN_MAX(opencv_Vision_data[0], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[1], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[2], 500.0f, 1560.0f); - yaw_error_kf.MeasuredVector[0] = opencv_Vision_data[0]; - float *yaw_filtered = Kalman_Filter_Update(&yaw_error_kf); - filtered_yaw_error = yaw_filtered[0]; - pitch_error_kf.MeasuredVector[0] = opencv_Vision_data[1]; - float *pitch_filtered = Kalman_Filter_Update(&pitch_error_kf); - filtered_pitch_error = pitch_filtered[0]; - yaw_tracking_pid_output = PIDCalculate(&yaw_tracking_pid, filtered_yaw_error, 0); - pitch_tracking_pid_output = PIDCalculate(&pitch_tracking_pid, filtered_pitch_error, 0); - gimbal_cmd_send.pitch += 0.001f * pitch_tracking_pid_output; - if (no4_turning_status == 1 && (no4_turning_count >=1 && no4_turning_count <=4)) // 如果题号4转向状态为1且转向计数器为1到4 - { - yaw_tracking_pid.Kp = 3.0f; // 关闭yaw比例 - yaw_tracking_pid.Ki = 0.0f; // 关闭yaw积分 - yaw_tracking_pid.Iout = 0.0f; // 清零yaw积分输出 - gimbal_cmd_send.yaw -= yaw_tracking_pid_output * mapping(opencv_Vision_data[2], 500.0f, 1560.0f, yaw_control_kxmax, yaw_control_kxmin); // yaw角度控制 - } - else - { - yaw_tracking_pid.Kp = 2.0f; // 关闭yaw比例 - yaw_tracking_pid.Ki = 4.0f; - gimbal_cmd_send.yaw -= yaw_tracking_pid_output * mapping(opencv_Vision_data[2], 500.0f, 1560.0f, yaw_control_kxmax, yaw_control_kxmin); // yaw角度控制 - } - laser_on(); - } - break; - case YUNT_NO4_3_WAITING: - gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; - gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; - gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; - no4_turning_count = 0; // 题号4转向计数器 - no4_turning_status = 0; // 题号4转向状态 - - // 计算0度对应最接近的多圈yaw - yuntai_no3_yawtarget = roundf(gimbal_fetch_data.gimbal_imu_data.YawTotalAngle / 360.0f) * 360.0f; // 计算最近的多圈yaw - timestart_timeline = DWT_GetTimeline_ms(); // 记录开始时间 - laser_off(); - break; - case YUNT_NO4_3_TURING: - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - gimbal_cmd_send.yaw = yuntai_no3_yawtarget - 45.0f; - gimbal_cmd_send.pitch = 0.0f; // 保持pitch角度为0 - if (fabsf(gimbal_fetch_data.gimbal_imu_data.YawTotalAngle - yuntai_no3_yawtarget + 45.0f) < 5.0f) - { - YunTaiState = YUNT_NO4_3_TRACKING; // 进入跟踪状态 - } - break; - case YUNT_NO4_3_TRACKING: - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - if (Vison_revc_flag) - { - LIMIT_MIN_MAX(opencv_Vision_data[0], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[1], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[2], 500.0f, 1560.0f); - yaw_error_kf.MeasuredVector[0] = opencv_Vision_data[0]; - float *yaw_filtered = Kalman_Filter_Update(&yaw_error_kf); - filtered_yaw_error = yaw_filtered[0]; - pitch_error_kf.MeasuredVector[0] = opencv_Vision_data[1]; - float *pitch_filtered = Kalman_Filter_Update(&pitch_error_kf); - filtered_pitch_error = pitch_filtered[0]; - - yaw_tracking_pid_output = PIDCalculate(&yaw_tracking_pid, filtered_yaw_error, 60.0f); // 题号4-3目标error为60 - pitch_tracking_pid_output = PIDCalculate(&pitch_tracking_pid, filtered_pitch_error, 0); - gimbal_cmd_send.yaw -= yaw_tracking_pid_output * mapping(opencv_Vision_data[2], 500.0f, 1560.0f, yaw_control_kxmax, yaw_control_kxmin); // yaw角度控制 - gimbal_cmd_send.pitch += 0.001f * pitch_tracking_pid_output; - } - break; - case YUNT_NO4_3_TRACKING_RUNNING: - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - if (board_comm_data->rx_float_data[1] >= 600.0f) - { - no4_turning_status = 1; // 题号4转向状态 - if (board_comm_data->rx_float_data[0] >= 0.0f && board_comm_data->rx_float_data[0] <= 12.5f) - { - no4_turning_count = 1; - } - else if (board_comm_data->rx_float_data[0] > 12.5f && board_comm_data->rx_float_data[0] <= 37.5f) - { - no4_turning_count = 2; - } - else if (board_comm_data->rx_float_data[0] > 37.5f && board_comm_data->rx_float_data[0] <= 62.5f) - { - no4_turning_count = 3; - } - else if (board_comm_data->rx_float_data[0] > 62.5f && board_comm_data->rx_float_data[0] <= 87.5f) - { - no4_turning_count = 4; - } - } - else - { - no4_turning_status = 0; // 题号4转向状态 - } - if (Vison_revc_flag) - { - LIMIT_MIN_MAX(opencv_Vision_data[0], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[1], -100.0f, 100.0f); - LIMIT_MIN_MAX(opencv_Vision_data[2], 500.0f, 1560.0f); - yaw_error_kf.MeasuredVector[0] = opencv_Vision_data[0]; - float *yaw_filtered = Kalman_Filter_Update(&yaw_error_kf); - filtered_yaw_error = yaw_filtered[0]; - pitch_error_kf.MeasuredVector[0] = opencv_Vision_data[1]; - float *pitch_filtered = Kalman_Filter_Update(&pitch_error_kf); - filtered_pitch_error = pitch_filtered[0]; - // 画圆模式 500hz 16s画完整圆 - circle_progress = board_comm_data->rx_float_data[0] / 100.0f; // 画圆进度,从板间通信数据获取 - if (circle_progress >= 1.0f) - { - circle_progress = 1.0f; // 限制在100% - } - circle_angle = 2.0f * PI * circle_progress; // 计算当前圆周角度 - target_x = circle_radius * cosf(circle_angle); // 计算圆周上目标点x坐标 - target_y = circle_radius * sinf(circle_angle); // 计算圆周上目标点y坐标 - yaw_tracking_pid_output = PIDCalculate(&yaw_tracking_pid, filtered_yaw_error, target_x); - pitch_tracking_pid_output = PIDCalculate(&pitch_tracking_pid, filtered_pitch_error, target_y); - gimbal_cmd_send.pitch += pitch_tracking_pid_output * mapping(opencv_Vision_data[2], 500.0f, 1560.0f, pitch_control_kxmax, pitch_control_kxmin); // pitch角度控制 - if (no4_turning_status == 1 && (no4_turning_count == 2 || no4_turning_count == 3)) // 如果题号4转向状态为1且转向计数器为2或3 - { - yaw_tracking_pid.Kp = 3.0f; // 关闭yaw比例 - yaw_tracking_pid.Ki = 0.0f; // 关闭yaw积分 - yaw_tracking_pid.Iout = 0.0f; // 清零yaw积分输出 - gimbal_cmd_send.yaw -= yaw_tracking_pid_output * mapping(opencv_Vision_data[2], 500.0f, 1560.0f, yaw_control_kxmax, yaw_control_kxmin); // yaw角度控制 - } - else - { - yaw_tracking_pid.Kp = 1.5f; // 关闭yaw比例 - yaw_tracking_pid.Ki = 3.0f; - gimbal_cmd_send.yaw -= yaw_tracking_pid_output * mapping(opencv_Vision_data[2], 500.0f, 1560.0f, yaw_control_kxmax, yaw_control_kxmin); // yaw角度控制 - } - } + case VISION_CMD_LASER_ON: laser_on(); break; - + case VISION_CMD_LASER_OFF: + laser_off(); + break; default: break; } } -/** - * @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等 - * 停止的阈值'300'待修改成合适的值,或改为开关控制. - * - * @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现 - * - */ +/* ---- KF + PID 初始化 ---- */ +static void VisionKalmanFilterInit() +{ + Kalman_Filter_Init(&yaw_error_kf, 2, 0, 1); + static float P_init[4] = {5.0f, 0.0f, 0.0f, 5.0f}; + memcpy(yaw_error_kf.P_data, P_init, sizeof(P_init)); + static float F_init[4] = {1.0f, 0.005f, 0.0f, 1.0f}; + memcpy(yaw_error_kf.F_data, F_init, sizeof(F_init)); + static float Q_init[4] = {0.1f, 0.0f, 0.0f, 1.0f}; + memcpy(yaw_error_kf.Q_data, Q_init, sizeof(Q_init)); + static uint8_t measurement_map[1] = {1}; + static float measurement_degree[1] = {1.0f}; + static float mat_R_diagonal[1] = {1.0f}; + static float state_min_variance[2] = {0.001f, 0.001f}; + yaw_error_kf.UseAutoAdjustment = 1; + memcpy(yaw_error_kf.MeasurementMap, measurement_map, sizeof(measurement_map)); + memcpy(yaw_error_kf.MeasurementDegree, measurement_degree, sizeof(measurement_degree)); + memcpy(yaw_error_kf.MatR_DiagonalElements, mat_R_diagonal, sizeof(mat_R_diagonal)); + memcpy(yaw_error_kf.StateMinVariance, state_min_variance, sizeof(state_min_variance)); + + Kalman_Filter_Init(&pitch_error_kf, 2, 0, 1); + memcpy(pitch_error_kf.P_data, P_init, sizeof(P_init)); + memcpy(pitch_error_kf.F_data, F_init, sizeof(F_init)); + memcpy(pitch_error_kf.Q_data, Q_init, sizeof(Q_init)); + pitch_error_kf.UseAutoAdjustment = 1; + memcpy(pitch_error_kf.MeasurementMap, measurement_map, sizeof(measurement_map)); + memcpy(pitch_error_kf.MeasurementDegree, measurement_degree, sizeof(measurement_degree)); + memcpy(pitch_error_kf.MatR_DiagonalElements, mat_R_diagonal, sizeof(mat_R_diagonal)); + memcpy(pitch_error_kf.StateMinVariance, state_min_variance, sizeof(state_min_variance)); +} + +void RobotCMDInit() +{ + rc_data = RemoteControlInit(&huart5); + + gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); + gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); + + static PID_Init_Config_s yaw_vpid_config = { + .Kp = 1.5f, .Ki = 3.0f, .Kd = 0.07f, + .IntegralLimit = 100.0f, + .CoefA = 20.0f, .CoefB = 20.0f, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | + PID_Derivative_On_Measurement | PID_ChangingIntegrationRate, + .MaxOut = 200.0f, + }; + static PID_Init_Config_s pitch_vpid_config = { + .Kp = 1.0f, .Ki = 2.0f, .Kd = 0.05f, + .IntegralLimit = 100.0f, + .CoefA = 10.0f, .CoefB = 30.0f, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | + PID_Derivative_On_Measurement | PID_ChangingIntegrationRate, + .MaxOut = 200.0f, + }; + PIDInit(&vision_yaw_pid, &yaw_vpid_config); + PIDInit(&vision_pitch_pid, &pitch_vpid_config); + VisionKalmanFilterInit(); + + Vision_Uart_Init(&huart10); + + robot_state = ROBOT_READY; + laser_off(); +} + +/* ---- 自动 / 视觉追踪模式 ---- */ +static void VisionTrackingSet() +{ + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + + if (vision_data.auto_updated) + { + vision_data.auto_updated = 0; + + yaw_error_kf.MeasuredVector[0] = vision_data.error_x; + Kalman_Filter_Update(&yaw_error_kf); + filtered_yaw_error = yaw_error_kf.FilteredValue[0]; + + pitch_error_kf.MeasuredVector[0] = vision_data.error_y; + Kalman_Filter_Update(&pitch_error_kf); + filtered_pitch_error = pitch_error_kf.FilteredValue[0]; + + float yaw_increment = PIDCalculate(&vision_yaw_pid, filtered_yaw_error, 0); + float pitch_increment = PIDCalculate(&vision_pitch_pid, filtered_pitch_error, 0); + + gimbal_yaw_ref += 0.001f * yaw_increment; + gimbal_pitch_ref += 0.001f * pitch_increment; + } + + if (gimbal_pitch_ref > 30.0f) + gimbal_pitch_ref = 30.0f; + else if (gimbal_pitch_ref < -30.0f) + gimbal_pitch_ref = -30.0f; + + gimbal_cmd_send.yaw = gimbal_yaw_ref; + gimbal_cmd_send.pitch = gimbal_pitch_ref; +} + +/* ---- 手动模式 ---- */ +static void ManualModeSet() +{ + if (vision_data.manual_updated && vision_data.zero_force) + { + /* 零力子模式 */ + vision_data.manual_updated = 0; + gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; + gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; + gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data.Pitch; + } + else if (vision_data.manual_updated) + { + /* 上位机角度参考 */ + vision_data.manual_updated = 0; + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + gimbal_yaw_ref = vision_data.yaw_ref; + gimbal_pitch_ref = vision_data.pitch_ref; + } + else + { + /* 遥控器 */ + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + gimbal_yaw_ref -= 0.0025f * (float)rc_data[TEMP].rc.rocker_r_; + gimbal_pitch_ref += 0.00125f * (float)rc_data[TEMP].rc.rocker_r1; + } + + if (gimbal_pitch_ref > 30.0f) + gimbal_pitch_ref = 30.0f; + else if (gimbal_pitch_ref < -30.0f) + gimbal_pitch_ref = -30.0f; + + gimbal_cmd_send.yaw = gimbal_yaw_ref; + gimbal_cmd_send.pitch = gimbal_pitch_ref; +} + +/* ---- 急停 ---- */ static void EmergencyHandler() { - // 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正 - if (switch_is_down(rc_data[TEMP].rc.switch_right) || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断 + if (switch_is_down(rc_data[TEMP].rc.switch_right)) { robot_state = ROBOT_STOP; gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; - chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; - gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; + gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; - LOGERROR("[CMD] emergency stop!"); + laser_off(); } - // 遥控器右侧开关为[上],恢复正常运行 + if (!switch_is_down(rc_data[TEMP].rc.switch_right) && robot_state == ROBOT_STOP) { robot_state = ROBOT_READY; + gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; + gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data.Pitch; LOGINFO("[CMD] reinstate, robot ready"); } } -/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */ void RobotCMDTask() { - // 从其他应用获取回传数据 -#ifdef ONE_BOARD - SubGetMessage(chassis_feed_sub, (void *)&chassis_fetch_data); -#endif // ONE_BOARD -#ifdef GIMBAL_BOARD - chassis_fetch_data = *(Chassis_Upload_Data_s *)CANCommGet(cmd_can_comm); -#endif // GIMBAL_BOARD - SubGetMessage(shoot_feed_sub, &shoot_fetch_data); SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data); - // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 - if (switch_is_mid(rc_data[TEMP].rc.switch_right)) + + /* 优先处理串口命令 (模式切换 / 激光等) */ + ProcessCommand(); + + EmergencyHandler(); + + if (robot_state == ROBOT_STOP) { - RemoteControlSet(); - } - else if (switch_is_up(rc_data[TEMP].rc.switch_right) || !RemoteControlIsOnline()) - { - AutoSet(); + PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send); + return; } - // AutoSet(); + /* 根据当前模式执行控制 */ + if (current_mode == VISION_MODE_AUTO) + VisionTrackingSet(); + else + ManualModeSet(); - EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 -#ifdef ONE_BOARD - PubPushMessage(chassis_cmd_pub, (void *)&chassis_cmd_send); -#endif // ONE_BOARD -#ifdef GIMBAL_BOARD - CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send); -#endif // GIMBAL_BOARD - PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send); PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send); } diff --git a/application/cmd/robot_cmd.h b/application/cmd/robot_cmd.h index 74b0d32..0f2584e 100644 --- a/application/cmd/robot_cmd.h +++ b/application/cmd/robot_cmd.h @@ -1,18 +1,14 @@ #ifndef ROBOT_CMD_H #define ROBOT_CMD_H - /** * @brief 机器人核心控制任务初始化,会被RobotInit()调用 - * */ void RobotCMDInit(); /** - * @brief 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) - * + * @brief 机器人核心控制任务,500Hz频率运行 */ void RobotCMDTask(); - -#endif // !ROBOT_CMD_H \ No newline at end of file +#endif // ROBOT_CMD_H diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index f51f606..8583e58 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -1,165 +1,113 @@ #include "gimbal.h" #include "robot_def.h" -#include "dji_motor.h" #include "dmmotor.h" #include "ins_task.h" #include "message_center.h" #include "general_def.h" -#include "bmi088.h" -#include "vofa.h" -static attitude_t *gimba_IMU_data; // 云台IMU数据 +static attitude_t *gimbal_IMU_data; -static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd) -static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 -static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 -static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 +static Publisher_t *gimbal_pub; +static Subscriber_t *gimbal_sub; +static Gimbal_Upload_Data_s gimbal_feedback_data; +static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; -static DJIMotorInstance *yaw_motor; // 云台yaw电机实例 -static DMMotorInstance *pitch_motor; // 云台pitch电机实例 +static DMMotorInstance *yaw_motor; // DM-G6220 +static DMMotorInstance *pitch_motor; // DM-J4310-2EC -static float yaw_speed_feedforward = 0.0f; // yaw电机的速度前馈 -static float yaw_current_feedforward_abs = 500.0f; -static float yaw_current_feedforward = 0.0f; // yaw电机的电流前馈 - -static float yaw_last_ref = 0.0f; // 上一次yaw电机的设定值,用于计算前馈 - -static BMI088Instance *bmi088; // 云台IMU void GimbalInit() { - HAL_GPIO_WritePin(POWER_24V_1_GPIO_Port, POWER_24V_1_Pin, GPIO_PIN_SET); - gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 - HAL_GPIO_WritePin(POWER_24V_1_GPIO_Port, POWER_24V_1_Pin, GPIO_PIN_RESET); + gimbal_IMU_data = INS_Init(); + gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); - // 初始化yaw电机 - Motor_Init_Config_s yaw_motor_config = { - .can_init_config.can_handle = &hcan1, - .can_init_config.tx_id = 0x01, - .controller_param_init_config = { - .angle_PID = { - .Kp = 0.3, - .Ki = 0.8, - .Kd = 0.005, - .IntegralLimit = 20, - .CoefA = 15.0f, - .CoefB = 1.0f, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate, - .MaxOut = 20, + /* ---- Yaw: DM-G6220, CAN1, TX=0x01, RX=0xF1, Velocity模式 ---- */ + DM_Motor_Init_Config_s yaw_config = { + .motor_config = { + .can_init_config = { + .can_handle = &hcan1, + .tx_id = 0x01, + .rx_id = 0xF1, }, - .speed_PID = { - .Kp = 1200, - .Ki = 0, - .Kd = 3, - .IntegralLimit = 16000, - .Output_LPF_RC = 0.001, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_OutputFilter, - .MaxOut = 16000, + .controller_param_init_config = { + .angle_PID = { + .Kp = 30, .Ki = 80, .Kd = 0.2f, + .IntegralLimit = 1000, + .CoefA = 20, .CoefB = 10, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | + PID_Derivative_On_Measurement | PID_ChangingIntegrationRate | + PID_DerivativeFilter | PID_OutputFilter, + .MaxOut = 2000, + }, + .other_angle_feedback_ptr = &gimbal_IMU_data->YawTotalAngle, + .other_speed_feedback_ptr = &gimbal_IMU_data->Gyro[2], }, - .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, - .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2], - .speed_feedforward_ptr = &yaw_speed_feedforward, // 速度前馈指针 - + .controller_setting_init_config = { + .angle_feedback_source = OTHER_FEED, + .speed_feedback_source = OTHER_FEED, + .outer_loop_type = ANGLE_LOOP, + .close_loop_type = ANGLE_LOOP, + .output_reverse = OUTPUT_DIRECTION_NORMAL, + }, + .motor_type = DM_MOTOR, }, - .controller_setting_init_config = { - .angle_feedback_source = OTHER_FEED, .speed_feedback_source = OTHER_FEED, - .outer_loop_type = ANGLE_LOOP, // 设置为开环，电机设定值由下面的功率控制设定，不走普通的pid - .close_loop_type = SPEED_LOOP | ANGLE_LOOP, // 角度环和速度环 - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, - .feedforward_flag = SPEED_FEEDFORWARD | CURRENT_FEEDFORWARD, // 开启速度前馈 - }, - .motor_type = GM6020_NEW, + .ctrl_mode = DM_MODE_VELOCITY, + .mit_kp = 0, .mit_kd = 0, }; - yaw_motor = DJIMotorInit(&yaw_motor_config); - // 初始化PITCH电机 - Motor_Init_Config_s pitch_config = { - .can_init_config = { - .can_handle = &hcan1, - .tx_id = 0x06, - .rx_id = 0x16, - }, - .controller_param_init_config = { - .angle_PID = { - .Kp = 1, - .Ki = 0, - .Kd = 0.005, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_OutputFilter | PID_DerivativeFilter, - .IntegralLimit = 10, - .MaxOut = 10, + yaw_motor = DMMotorInit(&yaw_config); + + /* ---- Pitch: DM-J4310-2EC, CAN1, TX=0x02, RX=0xF2, Velocity模式 ---- */ + DM_Motor_Init_Config_s pitch_config = { + .motor_config = { + .can_init_config = { + .can_handle = &hcan1, + .tx_id = 0x02, + .rx_id = 0xF2, }, - .speed_PID = { - .Kp = 0.8, - .Ki = 0, - .Kd = 0, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 0.1, - .MaxOut = 1, + .controller_param_init_config = { + .angle_PID = { + .Kp = 20, .Ki = 50, .Kd = 0.1f, + .IntegralLimit = 500, + .CoefA = 15, .CoefB = 5, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | + PID_Derivative_On_Measurement | PID_ChangingIntegrationRate, + .MaxOut = 1000, + }, + .other_angle_feedback_ptr = &gimbal_IMU_data->Pitch, + .other_speed_feedback_ptr = &gimbal_IMU_data->Gyro[0], }, - .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, - // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]), - }, - .controller_setting_init_config = { - .angle_feedback_source = OTHER_FEED, - .speed_feedback_source = OTHER_FEED, - .outer_loop_type = ANGLE_LOOP, - .close_loop_type = SPEED_LOOP | ANGLE_LOOP, - .feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL, - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, + .controller_setting_init_config = { + .angle_feedback_source = OTHER_FEED, + .speed_feedback_source = OTHER_FEED, + .outer_loop_type = ANGLE_LOOP, + .close_loop_type = ANGLE_LOOP, + .output_reverse = OUTPUT_DIRECTION_NORMAL, + }, + .motor_type = DM_MOTOR, }, + .ctrl_mode = DM_MODE_VELOCITY, + .mit_kp = 0, .mit_kd = 0, }; pitch_motor = DMMotorInit(&pitch_config); } -/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ void GimbalTask() { - // 获取云台控制数据 - // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); - switch (gimbal_cmd_recv.gimbal_mode) + if (gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE) { - // 停止 - case GIMBAL_ZERO_FORCE: - DJIMotorStop(yaw_motor); - DMMotorStop(pitch_motor); - - yaw_speed_feedforward = 0.0f; // 停止时清零前馈 - yaw_last_ref = 0.0f; // 停止时清零上一次参考值 - break; - // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 - case GIMBAL_GYRO_MODE: // 后续只保留此模式 - DJIMotorEnable(yaw_motor); - DMMotorEnable(pitch_motor); - - yaw_speed_feedforward = DEGREE_2_RAD * (gimbal_cmd_recv.yaw - yaw_last_ref) * 500.0f; - LIMIT_MIN_MAX(yaw_speed_feedforward, -10.0f, 10.0f); // 限制前馈值 - yaw_last_ref = gimbal_cmd_recv.yaw; // 更新上一次的参考值 - DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 + /* 零力模式: 保持当前IMU角度, 电机不主动出力 */ + DMMotorSetRef(yaw_motor, gimbal_IMU_data->YawTotalAngle); + DMMotorSetRef(pitch_motor, gimbal_IMU_data->Pitch); + } + else + { + DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); - if (yaw_motor->motor_controller.speed_PID.Ref >= 0.5f) - { - yaw_current_feedforward = 500.0f; // 前馈电流 - } - else if (yaw_motor->motor_controller.speed_PID.Ref <= -0.5f) - { - yaw_current_feedforward = -500.0f; // 前馈电流 - } - else - { - yaw_current_feedforward = yaw_current_feedforward_abs * yaw_motor->motor_controller.speed_PID.Ref / 0.5f; // 前馈电流 - } - - break; - default: - break; } - // 设置反馈数据,主要是imu和yaw的ecd - gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; - - // 推送消息 + gimbal_feedback_data.gimbal_imu_data = *gimbal_IMU_data; PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); -} \ No newline at end of file +} diff --git a/application/gimbal/gimbal.h b/application/gimbal/gimbal.h index 0bcc37e..7458d2e 100644 --- a/application/gimbal/gimbal.h +++ b/application/gimbal/gimbal.h @@ -3,14 +3,12 @@ /** * @brief 初始化云台,会被RobotInit()调用 - * */ void GimbalInit(); /** - * @brief 云台任务 - * + * @brief 云台任务,从gimbal_cmd话题获取控制指令并执行闭环控制 */ void GimbalTask(); -#endif // GIMBAL_H \ No newline at end of file +#endif // GIMBAL_H diff --git a/application/robot.c b/application/robot.c index 1fdc6bd..087d144 100644 --- a/application/robot.c +++ b/application/robot.c @@ -3,58 +3,23 @@ #include "robot_def.h" #include "robot_task.h" -// 编译warning,提醒开发者修改机器人参数 -#ifndef ROBOT_DEF_PARAM_WARNING -#define ROBOT_DEF_PARAM_WARNING -#pragma message "check if you have configured the parameters in robot_def.h, IF NOT, please refer to the comments AND DO IT, otherwise the robot will have FATAL ERRORS!!!" -#endif // !ROBOT_DEF_PARAM_WARNING - -#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) -#include "chassis.h" -#endif - -#if defined(ONE_BOARD) || defined(GIMBAL_BOARD) #include "gimbal.h" -#include "shoot.h" #include "robot_cmd.h" -#endif - void RobotInit() -{ - // 关闭中断,防止在初始化过程中发生中断 - // 请不要在初始化过程中使用中断和延时函数！ - // 若必须,则只允许使用DWT_Delay() +{ __disable_irq(); - - BSPInit(); -#if defined(ONE_BOARD) || defined(GIMBAL_BOARD) + BSPInit(); RobotCMDInit(); GimbalInit(); - // ShootInit(); -#endif + OSTaskInit(); -#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - //ChassisInit(); -#endif - - OSTaskInit(); // 创建基础任务 - - // 初始化完成,开启中断 __enable_irq(); } void RobotTask() { -#if defined(ONE_BOARD) || defined(GIMBAL_BOARD) RobotCMDTask(); GimbalTask(); - // ShootTask(); -#endif - -#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - //ChassisTask(); -#endif - -} \ No newline at end of file +} diff --git a/application/robot.h b/application/robot.h index 36f9809..e2c457d 100644 --- a/application/robot.h +++ b/application/robot.h @@ -1,18 +1,14 @@ #ifndef ROBOT_H #define ROBOT_H -/* Robot利用robot_def.h中的宏对不同的机器人进行了大量的兼容,同时兼容了两个开发板(云台板和底盘板)的配置 */ - /** * @brief 机器人初始化,请在开启rtos之前调用.这也是唯一需要放入main函数的函数 - * */ void RobotInit(); /** * @brief 机器人任务,放入实时系统以一定频率运行,内部会调用各个应用的任务 - * */ void RobotTask(); -#endif \ No newline at end of file +#endif diff --git a/application/robot_def.h b/application/robot_def.h index 45960c9..435a2b2 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -1,14 +1,4 @@ -/** - * @file robot_def.h - * @author NeoZeng neozng1@hnu.edu.cn - * @author Even - * @version 0.1 - * @date 2022-12-02 - * - * @copyright Copyright (c) HNU YueLu EC 2022 all rights reserved - * - */ -#pragma once // 可以用#pragma once代替#ifndef ROBOT_DEF_H(header guard) +#pragma once #ifndef ROBOT_DEF_H #define ROBOT_DEF_H @@ -16,58 +6,33 @@ #include "master_process.h" #include "stdint.h" -/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ +/* 开发板类型定义 */ #define ONE_BOARD // 单板控制整车 -// #define CHASSIS_BOARD //底盘板 -// #define GIMBAL_BOARD //云台板 //#define VISION_USE_VCP // 使用虚拟串口发送视觉数据 #define VISION_USE_UART // 使用串口发送视觉数据 -/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ -// 云台参数 -#define YAW_CHASSIS_ALIGN_ECD 2711 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 -#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 -#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 -#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) -#define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) -// 发射参数 -#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 -#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f -#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量 -// 机器人底盘修改的参数,单位为mm(毫米) -#define WHEEL_BASE 350 // 纵向轴距(前进后退方向) -#define TRACK_WIDTH 300 // 横向轮距(左右平移方向) -#define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0 -#define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0 -#define RADIUS_WHEEL 60 // 轮子半径 -#define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换 +/* 云台参数 */ +#define YAW_CHASSIS_ALIGN_ECD 2711 +#define PITCH_HORIZON_ECD 3412 +#define PITCH_MAX_ANGLE 0 +#define PITCH_MIN_ANGLE 0 -#define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反 -#define GYRO2GIMBAL_DIR_PITCH 1 // 陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反 -#define GYRO2GIMBAL_DIR_ROLL 1 // 陀螺仪数据相较于云台的roll的方向,1为相同,-1为相反 +/* IMU->云台方向 */ +#define GYRO2GIMBAL_DIR_YAW 1 +#define GYRO2GIMBAL_DIR_PITCH 1 +#define GYRO2GIMBAL_DIR_ROLL 1 -// 检查是否出现主控板定义冲突,只允许一个开发板定义存在,否则编译会自动报错 -#if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \ - (defined(ONE_BOARD) && defined(GIMBAL_BOARD)) || \ - (defined(CHASSIS_BOARD) && defined(GIMBAL_BOARD)) -#error Conflict board definition! You can only define one board type. -#endif +#pragma pack(1) -#pragma pack(1) // 压缩结构体,取消字节对齐,下面的数据都可能被传输 -/* -------------------------基本控制模式和数据类型定义-------------------------*/ -/** - * @brief 这些枚举类型和结构体会作为CMD控制数据和各应用的反馈数据的一部分 - * - */ -// 机器人状态 +/* 机器人状态 */ typedef enum { ROBOT_STOP = 0, ROBOT_READY, } Robot_Status_e; -// 应用状态 +/* 应用状态 */ typedef enum { APP_OFFLINE = 0, @@ -75,125 +40,22 @@ typedef enum APP_ERROR, } App_Status_e; -// 底盘模式设置 -/** - * @brief 后续考虑修改为云台跟随底盘,而不是让底盘去追云台,云台的惯量比底盘小. - * - */ +/* 云台模式 */ typedef enum { - CHASSIS_ZERO_FORCE = 0, // 电流零输入 - CHASSIS_ROTATE, // 小陀螺模式 - CHASSIS_NO_FOLLOW, // 不跟随，允许全向平移 - CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制 -} chassis_mode_e; - -// 云台模式设置 -typedef enum -{ - GIMBAL_ZERO_FORCE = 0, // 电流零输入 - GIMBAL_FREE_MODE, // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle;似乎可以改为全部用IMU数据? - GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式 + GIMBAL_ZERO_FORCE = 0, + GIMBAL_GYRO_MODE, } gimbal_mode_e; -// 发射模式设置 -typedef enum -{ - SHOOT_OFF = 0, - SHOOT_ON, -} shoot_mode_e; -typedef enum -{ - FRICTION_OFF = 0, // 摩擦轮关闭 - FRICTION_ON, // 摩擦轮开启 -} friction_mode_e; - -typedef enum -{ - LID_OPEN = 0, // 弹舱盖打开 - LID_CLOSE, // 弹舱盖关闭 -} lid_mode_e; - -typedef enum -{ - LOAD_STOP = 0, // 停止发射 - LOAD_REVERSE, // 反转 - LOAD_1_BULLET, // 单发 - LOAD_3_BULLET, // 三发 - LOAD_BURSTFIRE, // 连发 -} loader_mode_e; - -// 功率限制,从裁判系统获取,是否有必要保留? -typedef struct -{ // 功率控制 - float chassis_power_mx; -} Chassis_Power_Data_s; - -/* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */ -/** - * @brief 对于双板情况,遥控器和pc在云台,裁判系统在底盘 - * - */ -// cmd发布的底盘控制数据,由chassis订阅 +/* ---- CMD 发布给 gimbal 的控制数据 ---- */ typedef struct { - // 控制部分 - float vx; // 前进方向速度 - float vy; // 横移方向速度 - float wz; // 旋转速度 - float offset_angle; // 底盘和归中位置的夹角 - chassis_mode_e chassis_mode; - int chassis_speed_buff; - // UI部分 - // ... - -} Chassis_Ctrl_Cmd_s; - -// cmd发布的云台控制数据,由gimbal订阅 -typedef struct -{ // 云台角度控制 - float yaw; - float pitch; - float chassis_rotate_wz; - + float yaw; // yaw 目标角度 (degrees, multi-turn) + float pitch; // pitch 目标角度 (degrees) gimbal_mode_e gimbal_mode; } Gimbal_Ctrl_Cmd_s; -// cmd发布的发射控制数据,由shoot订阅 -typedef struct -{ - shoot_mode_e shoot_mode; - loader_mode_e load_mode; - lid_mode_e lid_mode; - friction_mode_e friction_mode; - Bullet_Speed_e bullet_speed; // 弹速枚举 - uint8_t rest_heat; - float shoot_rate; // 连续发射的射频,unit per s,发/秒 -} Shoot_Ctrl_Cmd_s; - -/* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/ -/** - * @brief 由cmd订阅,其他应用也可以根据需要获取. - * - */ - -typedef struct -{ -#if defined(CHASSIS_BOARD) || defined(GIMBAL_BOARD) // 非单板的时候底盘还将imu数据回传(若有必要) - // attitude_t chassis_imu_data; -#endif - // 后续增加底盘的真实速度 - // float real_vx; - // float real_vy; - // float real_wz; - - uint8_t rest_heat; // 剩余枪口热量 - Bullet_Speed_e bullet_speed; // 弹速限制 - Enemy_Color_e enemy_color; // 0 for blue, 1 for red - -} Chassis_Upload_Data_s; - - +/* ---- gimbal 发布给 CMD 的反馈数据 ---- */ typedef struct { attitude_t gimbal_imu_data; @@ -201,12 +63,6 @@ typedef struct uint8_t gimbal_power; } Gimbal_Upload_Data_s; -typedef struct -{ - // code to go here - // ... -} Shoot_Upload_Data_s; +#pragma pack() -#pragma pack() // 开启字节对齐,结束前面的#pragma pack(1) - -#endif // !ROBOT_DEF_H \ No newline at end of file +#endif // !ROBOT_DEF_H diff --git a/application/robot_task.h b/application/robot_task.h index 4093715..19cb46c 100644 --- a/application/robot_task.h +++ b/application/robot_task.h @@ -1,4 +1,3 @@ -/* 注意该文件应只用于任务初始化,只能被robot.c包含*/ #pragma once #include "FreeRTOS.h" @@ -9,12 +8,8 @@ #include "robot.h" #include "ins_task.h" #include "motor_task.h" -#include "referee_task.h" -#include "master_process.h" #include "daemon.h" -#include "HT04.h" #include "buzzer.h" -#include "dmmotor.h" #include "bsp_log.h" @@ -22,23 +17,16 @@ osThreadId insTaskHandle; osThreadId robotTaskHandle; osThreadId motorTaskHandle; osThreadId daemonTaskHandle; -osThreadId uiTaskHandle; void StartINSTASK(void const *argument); void StartMOTORTASK(void const *argument); void StartDAEMONTASK(void const *argument); void StartROBOTTASK(void const *argument); -void StartUITASK(void const *argument); -/** - * @brief 初始化机器人任务,所有持续运行的任务都在这里初始化 - * - */ void OSTaskInit() { osThreadDef(instask, StartINSTASK, osPriorityAboveNormal, 0, 1024); - insTaskHandle = osThreadCreate(osThread(instask), NULL); // 由于是阻塞读取传感器,为姿态解算设置较高优先级,确保以1khz的频率执行 - // // 后续修改为读取传感器数据准备好的中断处理, + insTaskHandle = osThreadCreate(osThread(instask), NULL); osThreadDef(motortask, StartMOTORTASK, osPriorityNormal, 0, 256); motorTaskHandle = osThreadCreate(osThread(motortask), NULL); @@ -48,36 +36,27 @@ void OSTaskInit() osThreadDef(robottask, StartROBOTTASK, osPriorityNormal, 0, 1024); robotTaskHandle = osThreadCreate(osThread(robottask), NULL); - - // osThreadDef(uitask, StartUITASK, osPriorityNormal, 0, 512); - // uiTaskHandle = osThreadCreate(osThread(uitask), NULL); - DMMotorControlInit(); // 初始化大电机 - - } __attribute__((noreturn)) void StartINSTASK(void const *argument) { - static float ins_start; - static float ins_dt; - INS_Init(); // 确保BMI088被正确初始化. + static float ins_start, ins_dt; + INS_Init(); LOGINFO("[freeRTOS] INS Task Start"); for (;;) { - // 1kHz ins_start = DWT_GetTimeline_ms(); INS_Task(); ins_dt = DWT_GetTimeline_ms() - ins_start; if (ins_dt > 1) - LOGERROR("[freeRTOS] INS Task is being DELAY! dt = [%f]", &ins_dt); + LOGERROR("[freeRTOS] INS Task DELAY dt=%.3f", ins_dt); osDelay(1); } } __attribute__((noreturn)) void StartMOTORTASK(void const *argument) { - static float motor_dt; - static float motor_start; + static float motor_start, motor_dt; LOGINFO("[freeRTOS] MOTOR Task Start"); for (;;) { @@ -85,56 +64,39 @@ __attribute__((noreturn)) void StartMOTORTASK(void const *argument) MotorControlTask(); motor_dt = DWT_GetTimeline_ms() - motor_start; if (motor_dt > 1) - LOGERROR("[freeRTOS] MOTOR Task is being DELAY! dt = [%f]", &motor_dt); + LOGERROR("[freeRTOS] MOTOR Task DELAY dt=%.3f", motor_dt); osDelay(1); } } __attribute__((noreturn)) void StartDAEMONTASK(void const *argument) { - static float daemon_dt; - static float daemon_start; + static float daemon_start, daemon_dt; BuzzerInit(); LOGINFO("[freeRTOS] Daemon Task Start"); for (;;) { - // 100Hz daemon_start = DWT_GetTimeline_ms(); DaemonTask(); BuzzerTask(); daemon_dt = DWT_GetTimeline_ms() - daemon_start; if (daemon_dt > 10) - LOGERROR("[freeRTOS] Daemon Task is being DELAY! dt = [%f]", &daemon_dt); + LOGERROR("[freeRTOS] Daemon Task DELAY dt=%.3f", daemon_dt); osDelay(10); } } __attribute__((noreturn)) void StartROBOTTASK(void const *argument) { - static float robot_dt; - static float robot_start; + static float robot_start, robot_dt; LOGINFO("[freeRTOS] ROBOT core Task Start"); - // 200Hz-500Hz,若有额外的控制任务如平衡步兵可能需要提升至1kHz for (;;) { robot_start = DWT_GetTimeline_ms(); RobotTask(); robot_dt = DWT_GetTimeline_ms() - robot_start; if (robot_dt > 5) - LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt); + LOGERROR("[freeRTOS] ROBOT core Task DELAY dt=%.3f", robot_dt); osDelay(2); } } - -__attribute__((noreturn)) void StartUITASK(void const *argument) -{ - LOGINFO("[freeRTOS] UI Task Start"); - MyUIInit(); - LOGINFO("[freeRTOS] UI Init Done, communication with ref has established"); - for (;;) - { - // 每给裁判系统发送一包数据会挂起一次,详见UITask函数的refereeSend() - UITask(); - osDelay(1); // 即使没有任何UI需要刷新,也挂起一次,防止卡在UITask中无法切换 - } -} diff --git a/modules/algorithm/controller.c b/modules/algorithm/controller.c index f044549..fd6ddb3 100644 --- a/modules/algorithm/controller.c +++ b/modules/algorithm/controller.c @@ -28,8 +28,8 @@ static void f_Changing_Integration_Rate(PIDInstance *pid) // 积分呈累积趋势 if (abs(pid->Err) <= pid->CoefB) return; // Full integral - if (abs(pid->Err) <= (pid->CoefA + pid->CoefB)) - pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA; + if (abs(pid->Err) <= (pid->CoefB + pid->CoefA)) + pid->ITerm *= (pid->CoefB + pid->CoefA - abs(pid->Err)) / pid->CoefA; else // 最大阈值,不使用积分 pid->ITerm = 0; } diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c index 26eb9d5..6797567 100644 --- a/modules/motor/DMmotor/dmmotor.c +++ b/modules/motor/DMmotor/dmmotor.c @@ -1,23 +1,23 @@ #include "dmmotor.h" -#include "memory.h" #include "general_def.h" -#include "user_lib.h" -#include "cmsis_os.h" #include "string.h" #include "daemon.h" #include "stdlib.h" #include "bsp_log.h" +#include "bsp_dwt.h" static uint8_t idx; static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT]; -static osThreadId dm_task_handle[DM_MOTOR_CNT]; -/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */ + +/* ---- float <-> uint conversion for MIT mode ---- */ + static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits) { float span = x_max - x_min; float offset = x_min; return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span); } + static float uint_to_float(int x_int, float x_min, float x_max, int bits) { float span = x_max - x_min; @@ -25,74 +25,155 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits) return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset; } +/* ---- Low-level CAN send functions (all three modes) ---- */ + +/* MIT mode: CAN ID = motor_id */ +void DMMotorSendMIT(CANInstance *can, float pos, float vel, float kp, float kd, float torq) +{ + uint16_t pos_uint = float_to_uint(pos, DM_P_MIN, DM_P_MAX, 16); + uint16_t vel_uint = float_to_uint(vel, DM_V_MIN, DM_V_MAX, 12); + uint16_t kp_uint = float_to_uint(kp, DM_KP_MIN, DM_KP_MAX, 12); + uint16_t kd_uint = float_to_uint(kd, DM_KD_MIN, DM_KD_MAX, 12); + uint16_t torq_uint = float_to_uint(torq, DM_T_MIN, DM_T_MAX, 12); + + uint8_t *buf = can->tx_buff; + buf[0] = (uint8_t)(pos_uint >> 8); + buf[1] = (uint8_t)(pos_uint); + buf[2] = (uint8_t)(vel_uint >> 4); + buf[3] = (uint8_t)(((vel_uint & 0xF) << 4) | (kp_uint >> 8)); + buf[4] = (uint8_t)(kp_uint); + buf[5] = (uint8_t)(kd_uint >> 4); + buf[6] = (uint8_t)(((kd_uint & 0xF) << 4) | (torq_uint >> 8)); + buf[7] = (uint8_t)(torq_uint); + + CANTransmit(can, 1); +} + +/* Position-Velocity mode: CAN ID = motor_id + 0x100 */ +void DMMotorSendPosVel(CANInstance *can, float pos, float vel) +{ + uint8_t *buf = can->tx_buff; + uint8_t *pos_bytes = (uint8_t *)&pos; + uint8_t *vel_bytes = (uint8_t *)&vel; + + buf[0] = pos_bytes[0]; + buf[1] = pos_bytes[1]; + buf[2] = pos_bytes[2]; + buf[3] = pos_bytes[3]; + buf[4] = vel_bytes[0]; + buf[5] = vel_bytes[1]; + buf[6] = vel_bytes[2]; + buf[7] = vel_bytes[3]; + + /* Temporarily change tx_id to motor_id + 0x100 for this transmission */ + uint32_t saved_id = can->tx_id; + can->tx_id = saved_id + 0x100; + CANTransmit(can, 1); + can->tx_id = saved_id; +} + +/* Velocity mode: CAN ID = motor_id + 0x200 */ +void DMMotorSendVelocity(CANInstance *can, float vel) +{ + uint8_t *buf = can->tx_buff; + uint8_t *vel_bytes = (uint8_t *)&vel; + + buf[0] = vel_bytes[0]; + buf[1] = vel_bytes[1]; + buf[2] = vel_bytes[2]; + buf[3] = vel_bytes[3]; + buf[4] = 0; + buf[5] = 0; + buf[6] = 0; + buf[7] = 0; + + uint32_t saved_id = can->tx_id; + can->tx_id = saved_id + 0x200; + CANTransmit(can, 1); + can->tx_id = saved_id; +} + +/* ---- Motor commands (enable/disable/zero/clear error) ---- */ + void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor) { - memset(motor->motor_can_instace->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff - motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id - CANTransmit(motor->motor_can_instace, 1); + memset(motor->motor_can_instance->tx_buff, 0xFF, 7); + motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd; + CANTransmit(motor->motor_can_instance, 1); } +/* ---- Feedback decode: CAN RX interrupt callback ---- */ + static void DMMotorDecode(CANInstance *motor_can) { - uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量 uint8_t *rxbuff = motor_can->rx_buff; DMMotorInstance *motor = (DMMotorInstance *)motor_can->id; - DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针 + DM_Motor_Measure_s *measure = &motor->measure; + uint16_t tmp; DaemonReload(motor->motor_daemon); - measure->id = rxbuff[0] & 0x0F; // 低 4 位是控制器 ID - measure->state = (rxbuff[0] >> 4) & 0x0F; // ERR 存储在 state 字段 + + measure->id = rxbuff[0] & 0x0F; + measure->state = (rxbuff[0] >> 4) & 0x0F; measure->last_position = measure->position; + tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]); measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16); - tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4); + tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4)); measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12); - tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]); + tmp = (uint16_t)(((rxbuff[4] & 0x0F) << 8) | rxbuff[5]); measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12); - measure->T_Mos = (float)rxbuff[6]; + measure->T_Mos = (float)rxbuff[6]; measure->T_Rotor = (float)rxbuff[7]; - // 多圈角度计算 - if (measure->position - measure->last_position > 12.5) - measure->total_round--; - else if (measure->position - measure->last_position < -12.5) - measure->total_round++; - measure->total_angle = measure->position + measure->total_round * 25; - measure->total_angle = measure->total_angle * RAD_2_DEGREE; - measure->velocity = measure->velocity * RAD_2_DEGREE; + /* multi-turn tracking */ + if (measure->position - measure->last_position > 10.0f) + measure->total_round--; + else if (measure->position - measure->last_position < -10.0f) + measure->total_round++; + + measure->total_angle = (measure->position + measure->total_round * 25.1327f) * RAD_2_DEGREE; + measure->velocity = measure->velocity * RAD_2_DEGREE; } static void DMMotorLostCallback(void *motor_ptr) { DMMotorInstance *motor = (DMMotorInstance *)motor_ptr; - uint16_t can_bus = motor->motor_can_instace->can_handle == &hcan1 ? 1 : 2; - LOGWARNING("[DMmotor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instace->tx_id); + LOGWARNING("[DMmotor] Motor lost, id [%d]", motor->motor_can_instance->tx_id); } -void DMMotorCaliEncoder(DMMotorInstance *motor) -{ - DMMotorSetMode(DM_CMD_ZERO_POSITION, motor); - DWT_Delay(0.1); -} -DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config) + +/* ---- Init ---- */ + +DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config) { DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance)); memset(motor, 0, sizeof(DMMotorInstance)); - motor->motor_settings = config->controller_setting_init_config; - PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID); - PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID); - PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID); - motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; - motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; + motor->motor_settings = config->motor_config.controller_setting_init_config; + motor->ctrl_mode = config->ctrl_mode; + motor->mit_kp = config->mit_kp; + motor->mit_kd = config->mit_kd; - config->can_init_config.can_module_callback = DMMotorDecode; - config->can_init_config.id = motor; - motor->motor_can_instace = CANRegister(&config->can_init_config); + /* init PID controllers */ + PIDInit(&motor->motor_controller.angle_PID, &config->motor_config.controller_param_init_config.angle_PID); + PIDInit(&motor->motor_controller.speed_PID, &config->motor_config.controller_param_init_config.speed_PID); + PIDInit(&motor->motor_controller.current_PID, &config->motor_config.controller_param_init_config.current_PID); + motor->motor_controller.other_angle_feedback_ptr = config->motor_config.controller_param_init_config.other_angle_feedback_ptr; + motor->motor_controller.other_speed_feedback_ptr = config->motor_config.controller_param_init_config.other_speed_feedback_ptr; + motor->motor_controller.speed_feedforward_ptr = config->motor_config.controller_param_init_config.speed_feedforward_ptr; + motor->motor_controller.current_feedforward_ptr = config->motor_config.controller_param_init_config.current_feedforward_ptr; + + /* register CAN instance */ + config->motor_config.can_init_config.can_module_callback = DMMotorDecode; + config->motor_config.can_init_config.id = motor; + motor->motor_can_instance = CANRegister(&config->motor_config.can_init_config); + + /* register daemon */ Daemon_Init_Config_s conf = { .callback = DMMotorLostCallback, .owner_id = motor, @@ -100,133 +181,110 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config) }; motor->motor_daemon = DaemonRegister(&conf); - DMMotorEnable(motor); + /* enable motor */ DMMotorSetMode(DM_CMD_MOTOR_MODE, motor); - // DWT_Delay(0.1); - // DMMotorCaliEncoder(motor); - // DWT_Delay(0.1); + dm_motor_instance[idx++] = motor; return motor; } +/* ---- Runtime control ---- */ + void DMMotorSetRef(DMMotorInstance *motor, float ref) { motor->pid_ref = ref; } -void DMMotorEnable(DMMotorInstance *motor) -{ - motor->stop_flag = MOTOR_ENALBED; -} - -void DMMotorStop(DMMotorInstance *motor) // 不使用使能模式是因为需要收到反馈 -{ - motor->stop_flag = MOTOR_STOP; -} - void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type) { motor->motor_settings.outer_loop_type = type; } -//@Todo: 目前只实现了力控，更多位控PID等请自行添加 -void DMMotorTask(void const *argument) +void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode) { - float pid_ref, set; - DMMotorInstance *motor = (DMMotorInstance *)argument; - DM_Motor_Measure_s *measure = &motor->measure; - Motor_Control_Setting_s *setting = &motor->motor_settings; - // CANInstance *motor_can = motor->motor_can_instace; - // uint16_t tmp; - DMMotor_Send_s motor_send_mailbox; - float pid_measure; // 电机PID测量值和设定值 - while (1) + motor->ctrl_mode = mode; +} + +/* ---- Main control: called from Motor Task at 1kHz ---- */ + +void DMMotorControl(void) +{ + for (uint8_t i = 0; i < idx; i++) { - if(motor->measure.state == 0x00) - { - DMMotorSetMode(DM_CMD_MOTOR_MODE, motor); - osDelay(20); - } - if(motor->measure.state != 0x00&&motor->measure.state != 0x01) + DMMotorInstance *motor = dm_motor_instance[i]; + DM_Motor_Measure_s *measure = &motor->measure; + Motor_Control_Setting_s *setting = &motor->motor_settings; + Motor_Controller_s *ctrl = &motor->motor_controller; + CANInstance *can = motor->motor_can_instance; + + /* handle motor error states */ + if (measure->state != 0x00 && measure->state != 0x01) { DMMotorSetMode(DM_CMD_CLEAR_ERROR, motor); - osDelay(20); + DWT_Delay(0.001); DMMotorSetMode(DM_CMD_MOTOR_MODE, motor); - osDelay(20); + continue; } - pid_ref = motor->pid_ref; - if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) - pid_ref *= -1; // 设置反转 + + float pid_ref = motor->pid_ref; + + /* ---- Angle loop (outermost) ---- */ if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP) { + float pid_measure; if (setting->angle_feedback_source == OTHER_FEED) - { - pid_measure = *motor->other_angle_feedback_ptr; - // char buff[20]; - // Float2Str(buff, pid_measure); - // LOGINFO("pid_measure: %s", buff); - } + pid_measure = *ctrl->other_angle_feedback_ptr; else pid_measure = measure->total_angle; - // 更新pid_ref进入下一个环 - pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref); + + pid_ref = PIDCalculate(&ctrl->angle_PID, pid_measure, pid_ref); } + + /* ---- Speed loop (inner) ---- */ if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))) { if (setting->feedforward_flag & SPEED_FEEDFORWARD) - pid_ref += *motor->speed_feedforward_ptr; + pid_ref += *ctrl->speed_feedforward_ptr; + float pid_measure; if (setting->speed_feedback_source == OTHER_FEED) - pid_measure = *motor->other_speed_feedback_ptr; - else // MOTOR_FEED + pid_measure = *ctrl->other_speed_feedback_ptr; + else pid_measure = measure->velocity; - // 更新pid_ref进入下一个环 - pid_ref = PIDCalculate(&motor->speed_PID, pid_measure, pid_ref); + + pid_ref = PIDCalculate(&ctrl->speed_PID, pid_measure, pid_ref); } - if (setting->feedforward_flag == 1) - pid_ref += 0.8; - - if (setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) + + /* output reverse */ + if (setting->output_reverse == OUTPUT_DIRECTION_REVERSE) pid_ref *= -1; - set = pid_ref; + /* ---- Send via selected control mode ---- */ + float vel_cmd = pid_ref * DEGREE_2_RAD; // deg/s → rad/s + LIMIT_MIN_MAX(vel_cmd, DM_V_MIN, DM_V_MAX); - LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX); - motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16); - motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12); - motor_send_mailbox.torque_des = float_to_uint(set, DM_T_MIN, DM_T_MAX, 12); - motor_send_mailbox.Kp = 0; - motor_send_mailbox.Kd = 0; + switch (motor->ctrl_mode) + { + case DM_MODE_VELOCITY: + /* Velocity mode: MCU angle PID output → motor velocity ref */ + DMMotorSendVelocity(can, vel_cmd); + break; - if (motor->stop_flag == MOTOR_STOP) - motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12); + case DM_MODE_MIT: { + /* MIT mode: MCU angle PID output → motor position ref, motor handles position-velocity internally */ + float pos_cmd = pid_ref * DEGREE_2_RAD; + LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX); + DMMotorSendMIT(can, pos_cmd, 0.0f, motor->mit_kp, motor->mit_kd, 0.0f); + break; + } - motor->motor_can_instace->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8); - motor->motor_can_instace->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des); - motor->motor_can_instace->tx_buff[2] = (uint8_t)(motor_send_mailbox.velocity_des >> 4); - motor->motor_can_instace->tx_buff[3] = (uint8_t)(((motor_send_mailbox.velocity_des & 0xF) << 4) | (motor_send_mailbox.Kp >> 8)); - motor->motor_can_instace->tx_buff[4] = (uint8_t)(motor_send_mailbox.Kp); - motor->motor_can_instace->tx_buff[5] = (uint8_t)(motor_send_mailbox.Kd >> 4); - motor->motor_can_instace->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8)); - motor->motor_can_instace->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des); - - CANTransmit(motor->motor_can_instace, 1); - - osDelay(1); + case DM_MODE_POS_VEL: { + /* Position-Velocity mode: MCU angle PID output → motor position ref + velocity ff */ + float pos_cmd = pid_ref * DEGREE_2_RAD; + LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX); + DMMotorSendPosVel(can, pos_cmd, vel_cmd); + break; + } + } } } -void DMMotorControlInit() -{ - char dm_task_name[5] = "dm"; - // 遍历所有电机实例,创建任务 - if (!idx) - return; - for (size_t i = 0; i < idx; i++) - { - char dm_id_buff[2] = {0}; - __itoa(i, dm_id_buff, 10); - strcat(dm_task_name, dm_id_buff); - osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128); - dm_task_handle[i] = osThreadCreate(osThread(dm_task_name), dm_motor_instance[i]); - } -} \ No newline at end of file diff --git a/modules/motor/DMmotor/dmmotor.h b/modules/motor/DMmotor/dmmotor.h index 4dee2b9..3e15caa 100644 --- a/modules/motor/DMmotor/dmmotor.h +++ b/modules/motor/DMmotor/dmmotor.h @@ -1,5 +1,6 @@ #ifndef DMMOTOR_H #define DMMOTOR_H + #include #include "bsp_can.h" #include "controller.h" @@ -8,73 +9,99 @@ #define DM_MOTOR_CNT 4 -#define DM_P_MIN (-12.56637f) -#define DM_P_MAX 12.56637f +/* DM motor parameter ranges (from reference DM_6220 driver) */ +#define DM_P_MIN (-12.5f) +#define DM_P_MAX 12.5f #define DM_V_MIN (-45.0f) #define DM_V_MAX 45.0f -#define DM_T_MIN (-18.0f) -#define DM_T_MAX 18.0f +#define DM_T_MIN (-10.0f) +#define DM_T_MAX 10.0f +#define DM_KP_MIN 0.0f +#define DM_KP_MAX 500.0f +#define DM_KD_MIN 0.0f +#define DM_KD_MAX 5.0f -typedef struct +/* DM motor control modes */ +typedef enum { - uint8_t id; - uint8_t state; - float velocity; - float last_position; - float position; - float torque; - float T_Mos; - float T_Rotor; - int32_t total_round; - float total_angle; -}DM_Motor_Measure_s; + DM_MODE_MIT = 0, // MIT mode: CAN ID, packed pos/vel/KP/KD/torque + DM_MODE_POS_VEL = 1, // Position-Velocity mode: CAN ID + 0x100, float pos + float vel + DM_MODE_VELOCITY = 2, // Velocity mode: CAN ID + 0x200, float velocity only +} DM_Motor_Mode_e; +/* DM motor measurement data (parsed from feedback CAN frame) */ typedef struct { - uint16_t position_des; - uint16_t velocity_des; - uint16_t torque_des; - uint16_t Kp; - uint16_t Kd; -}DMMotor_Send_s; -typedef struct + uint8_t id; // motor ID from feedback byte[0:3] + uint8_t state; // motor state from feedback byte[7:4]: 0=reset, 1=run + float position; // current position (rad, range ±12.5) + float velocity; // current velocity (rad/s, range ±45) + float torque; // current torque (N·m, range ±10) + float T_Mos; // MOS temperature (Celsius) + float T_Rotor; // rotor temperature (Celsius) + float last_position; // previous position for multi-turn calculation + int32_t total_round; // accumulated full revolutions + float total_angle; // multi-turn angle (degrees) +} DM_Motor_Measure_s; + +/* DM motor instance (runtime state) */ +typedef struct { DM_Motor_Measure_s measure; Motor_Control_Setting_s motor_settings; - PIDInstance current_PID; - PIDInstance speed_PID; - PIDInstance angle_PID; - float *other_angle_feedback_ptr; - float *other_speed_feedback_ptr; - float *speed_feedforward_ptr; - float *current_feedforward_ptr; - float pid_ref; - Motor_Working_Type_e stop_flag; - CANInstance *motor_can_instace; - DaemonInstance* motor_daemon; - uint32_t lost_cnt; - uint8_t lost_status; - uint8_t last_lost_status; -}DMMotorInstance; + Motor_Controller_s motor_controller; + CANInstance *motor_can_instance; // CAN communication instance + DaemonInstance *motor_daemon; // daemon watchdog + DM_Motor_Mode_e ctrl_mode; // current control mode (MIT / Pos-Vel / Velocity) + float mit_kp; // MIT mode KP (motor internal position gain, 0~500) + float mit_kd; // MIT mode KD (motor internal velocity gain, 0~5) + + float pid_ref; // reference input from application +} DMMotorInstance; + +/* DM motor init config */ +typedef struct +{ + Motor_Init_Config_s motor_config; // generic motor PID and setting config + DM_Motor_Mode_e ctrl_mode; // default control mode + float mit_kp; // MIT mode KP (only for MIT mode) + float mit_kd; // MIT mode KD (only for MIT mode) +} DM_Motor_Init_Config_s; + +/* DM motor command codes */ typedef enum { - DM_CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令 - DM_CMD_RESET_MODE = 0xfd, // 停止 - DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位 - DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误 -}DMMotor_Mode_e; + DM_CMD_MOTOR_MODE = 0xFC, // enable motor + DM_CMD_RESET_MODE = 0xFD, // disable motor + DM_CMD_ZERO_POSITION = 0xFE, // set current position as encoder zero + DM_CMD_CLEAR_ERROR = 0xFB, // clear motor error +} DMMotor_Mode_e; -DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config); +/* ---- API ---- */ + +DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config); void DMMotorSetRef(DMMotorInstance *motor, float ref); - -void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type); - -void DMMotorEnable(DMMotorInstance *motor); - -void DMMotorStop(DMMotorInstance *motor); -void DMMotorCaliEncoder(DMMotorInstance *motor); -void DMMotorControlInit(); +void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type); +void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode); void DMMotorSetMode(DMMotor_Mode_e mode, DMMotorInstance *motor); -#endif // !DMMOTOR \ No newline at end of file + +/** + * @brief Main control function called from Motor Task at 1kHz. + * Iterates all registered motors, runs PID cascade, sends CAN frames. + */ +void DMMotorControl(void); + +/* ---- Low-level CAN send functions (all three modes) ---- */ + +/* MIT mode: send position + velocity + KP + KD + torque in packed 8-byte frame */ +void DMMotorSendMIT(CANInstance *can, float pos, float vel, float kp, float kd, float torq); + +/* Position-Velocity mode: send float position + float velocity */ +void DMMotorSendPosVel(CANInstance *can, float pos, float vel); + +/* Velocity mode: send float velocity only */ +void DMMotorSendVelocity(CANInstance *can, float vel); + +#endif // DMMOTOR_H diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index add26b3..f136326 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -49,19 +49,13 @@ typedef enum OTHER_FEED, } Feedback_Source_e; -/* 电机正反转标志 */ +/* 输出反向标志: 取反最终PID输出 */ typedef enum { - MOTOR_DIRECTION_NORMAL = 0, - MOTOR_DIRECTION_REVERSE = 1 -} Motor_Reverse_Flag_e; + OUTPUT_DIRECTION_NORMAL = 0, + OUTPUT_DIRECTION_REVERSE = 1, +} Output_Reverse_Flag_e; -/* 反馈量正反标志 */ -typedef enum -{ - FEEDBACK_DIRECTION_NORMAL = 0, - FEEDBACK_DIRECTION_REVERSE = 1 -} Feedback_Reverse_Flag_e; typedef enum { MOTOR_STOP = 0, @@ -77,14 +71,13 @@ typedef enum /* 电机控制设置,包括闭环类型,反转标志和反馈来源 */ typedef struct { - Closeloop_Type_e outer_loop_type; // 最外层的闭环,未设置时默认为最高级的闭环 - Closeloop_Type_e close_loop_type; // 使用几个闭环(串级) - Motor_Reverse_Flag_e motor_reverse_flag; // 是否反转 - Feedback_Reverse_Flag_e feedback_reverse_flag; // 反馈是否反向 - Feedback_Source_e angle_feedback_source; // 角度反馈类型 - Feedback_Source_e speed_feedback_source; // 速度反馈类型 - uint8_t feedforward_flag; // 前馈标志 - MOTOR_SEND_MODE_e send_mode; // 发送模式 + Closeloop_Type_e outer_loop_type; // 最外层的闭环 + Closeloop_Type_e close_loop_type; // 使用几个闭环(串级) + Output_Reverse_Flag_e output_reverse; // 输出反向 + Feedback_Source_e angle_feedback_source; // 角度反馈类型 + Feedback_Source_e speed_feedback_source; // 速度反馈类型 + uint8_t feedforward_flag; // 前馈标志 + MOTOR_SEND_MODE_e send_mode; // 发送模式 } Motor_Control_Setting_s; @@ -111,12 +104,7 @@ typedef struct typedef enum { MOTOR_TYPE_NONE = 0, - GM6020, - M3508, - M2006, - LK9025, - HT04, - GM6020_NEW //新版固件6020 + DM_MOTOR, } Motor_Type_e; /** diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index a52980b..9422c50 100644 --- a/modules/motor/motor_task.c +++ b/modules/motor/motor_task.c @@ -1,25 +1,7 @@ #include "motor_task.h" -#include "LK9025.h" -#include "HT04.h" -#include "dji_motor.h" -#include "step_motor.h" -#include "servo_motor.h" +#include "dmmotor.h" void MotorControlTask() { - // static uint8_t cnt = 0; 设定不同电机的任务频率 - // if(cnt%5==0) //200hz - // if(cnt%10==0) //100hz - DJIMotorControl(); - - /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ - LKMotorControl(); - - // legacy support - // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞 - // 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换 - // HTMotorControl(); - // 将所有的CAN设备集中在一处发送,最高反馈频率仅能达到500Hz,为了更好的控制效果,应使用新的HTMotorControlInit()接口 - - // StepMotorControl(); + DMMotorControl(); } diff --git a/modules/opencv/opencv.c b/modules/opencv/opencv.c index d1213ce..58deda4 100644 --- a/modules/opencv/opencv.c +++ b/modules/opencv/opencv.c @@ -1,99 +1,75 @@ #include "opencv.h" -#include "daemon.h" -#define FRAME_SOF 0xA5 -#define FRAME_PAYLOAD_LEN (2+2+4*12) // 52 -#define FRAME_FULL_LEN (1+1+FRAME_PAYLOAD_LEN) //54 -float opencv_Vision_data[12]; +VisionData_t vision_data; -uint8_t Vison_revc_flag = 0; // 视觉数据接收标志位 -uint8_t vision_used_flag = 0; // 视觉数据使用标志位,用于判断是否需要使用视觉数据 - - - -float Vison_Frequency; // 视觉数据接收频率,单位Hz static USARTInstance *cv_usart_instance; -static DaemonInstance *OPENCV_vision_daemon_instance; +static DaemonInstance *vision_daemon_instance; -/** - * @brief 接收解包回调函数,将在bsp_usart.c中被usart rx callback调用 - * @todo 1.提高可读性,将get_protocol_info的第四个参数增加一个float类型buffer - * 2.添加标志位解码 - */ -static void DecodeVision() +static uint8_t calc_checksum(uint8_t *buf, uint8_t len) { - DaemonReload(OPENCV_vision_daemon_instance); // 喂狗,防止离线 - static uint32_t counter; - float dt = DWT_GetDeltaT(&counter); - Vison_Frequency = 1.0f / dt; // 计算频率 - const uint16_t len = cv_usart_instance->recv_buff_size; - uint16_t flag_register; - uint8_t *buf = cv_usart_instance->recv_buff; - if (len != FRAME_FULL_LEN) { - // 长度不符 - LOGERROR("Vision frame length error"); - return; - } - if (buf[0] != FRAME_SOF) { - // SOF 错误 - LOGERROR("Vision frame SOF error"); - return; - } - uint8_t payload_len = buf[1]; - if (payload_len != FRAME_PAYLOAD_LEN) { - // Payload 长度不符 - LOGERROR("Vision frame payload length error"); - return; - } - // 解析命令号和 flags（小端） - uint16_t cmd_id = buf[2] | (buf[3] << 8); - uint16_t flags = buf[4] | (buf[5] << 8); - - // 解析 12 个 float - for (int i = 0; i < 12; i++) { - // 从 buf[6 + 4*i] 连续读 4 字节 - memcpy(&opencv_Vision_data[i], buf + 6 + 4*i, sizeof(float)); - } - // // 解析任务标志位(拟合最接近的int) - // int last_task_cmd = Vison_task_cmd; - // Vison_task_cmd = (int)(Vison_data[10] + 0.5f); - // if (Vison_task_cmd != last_task_cmd) { - // Vison_task_changed = 1; // 任务命令变化 - // } - - Vison_revc_flag = 1; // 设置接收标志位 - // vision_used_flag = 1; // 设置视觉数据使用标志位 - //解析暂停标志位 - // run = (int)(Vison_data[11] + 0.5f); - - // 处理... - // printf("Got frame: cmd=0x%04X flags=0x%04X\n", cmd_id, flags); - // for (int i = 0; i < 12; i += 2) { - // printf(" coord %d: (%.3f, %.3f)\n", - // i/2, data[i], data[i+1]); - // float a = data[0]; - - // TODO: code to resolve flag_register; + uint8_t sum = 0; + for (uint8_t i = 0; i < len - 1; i++) + sum += buf[i]; + return sum; } -void VisionOfflineCallback(void *owner_id) +static void DecodeVision() { - Vison_revc_flag = 0; // 清除接收标志位 + DaemonReload(vision_daemon_instance); + + static uint32_t counter; + float dt = DWT_GetDeltaT(&counter); + vision_data.frequency = 1.0f / dt; + + uint8_t *buf = cv_usart_instance->recv_buff; + uint16_t len = cv_usart_instance->recv_buff_size; + uint8_t sof = buf[0]; + + if (sof == VISION_SOF_AUTO && len == VISION_AUTO_LEN) + { + if (calc_checksum(buf, VISION_AUTO_LEN) != buf[VISION_AUTO_LEN - 1]) + return; + memcpy(&vision_data.error_x, buf + 1, 4); + memcpy(&vision_data.error_y, buf + 5, 4); + vision_data.auto_updated = 1; + } + else if (sof == VISION_SOF_MANUAL && len == VISION_MANUAL_LEN) + { + if (calc_checksum(buf, VISION_MANUAL_LEN) != buf[VISION_MANUAL_LEN - 1]) + return; + vision_data.zero_force = buf[1]; + memcpy(&vision_data.yaw_ref, buf + 2, 4); + memcpy(&vision_data.pitch_ref, buf + 6, 4); + vision_data.manual_updated = 1; + } + else if (sof == VISION_SOF_CMD && len == VISION_CMD_LEN) + { + if (calc_checksum(buf, VISION_CMD_LEN) != buf[VISION_CMD_LEN - 1]) + return; + vision_data.cmd = buf[1]; + vision_data.cmd_pending = 1; + } +} + +static void VisionOfflineCallback(void *id) +{ + (void)id; + USARTServiceInit(cv_usart_instance); } void Vision_Uart_Init(UART_HandleTypeDef *_handle) { - USART_Init_Config_s conf; - conf.module_callback = DecodeVision; - conf.recv_buff_size = 54; - conf.usart_handle = _handle; + USART_Init_Config_s conf = { + .module_callback = DecodeVision, + .recv_buff_size = 64, + .usart_handle = _handle, + }; cv_usart_instance = USARTRegister(&conf); - // 为master process注册daemon,用于判断视觉通信是否离线 Daemon_Init_Config_s daemon_conf = { - .callback = VisionOfflineCallback, // 离线时调用的回调函数,会重启串口接收 - .owner_id = NULL, + .callback = NULL, + .owner_id = NULL, .reload_count = 10, }; - OPENCV_vision_daemon_instance = DaemonRegister(&daemon_conf); -} \ No newline at end of file + vision_daemon_instance = DaemonRegister(&daemon_conf); +} diff --git a/modules/opencv/opencv.h b/modules/opencv/opencv.h index ebefd1a..5c76bf5 100644 --- a/modules/opencv/opencv.h +++ b/modules/opencv/opencv.h @@ -1,22 +1,75 @@ #ifndef OPENCV_H #define OPENCV_H + #include "bsp_usart.h" -#include "opencv.h" -#include "bsp_log.h" #include "bsp_dwt.h" +#include "bsp_log.h" #include "usart.h" -#include "robot_def.h" +#include "daemon.h" #include -#include +/* + * 上位机串口协议 — 三种帧类型, 通过帧头区分 + * + * ---- 自动/视觉追踪数据帧 0xA5 (10 字节) ---- + * [SOF:0xA5][error_x:4][error_y:4][checksum:1] + * error_x/y: float, 归一化误差, 视觉PID外环反馈值 + * + * ---- 手动控制数据帧 0xA6 (11 字节) ---- + * [SOF:0xA6][zero_force:1][yaw_ref:4][pitch_ref:4][checksum:1] + * zero_force: 0=正常角度控制, 1=零力/保持当前位置 + * yaw_ref/pitch_ref: float, 角度参考值 (degrees) + * + * ---- 命令帧 0xA7 (3 字节) ---- + * [SOF:0xA7][cmd:1][checksum:1] + * cmd: 0x01=切换到自动模式, 0x02=切换到手动模式, 0x03=激光开, 0x04=激光关 + * + * checksum: 帧头到数据末尾前一字节累加和取低8位 + */ +#define VISION_SOF_AUTO 0xA5 +#define VISION_SOF_MANUAL 0xA6 +#define VISION_SOF_CMD 0xA7 +#define VISION_AUTO_LEN 10 +#define VISION_MANUAL_LEN 11 +#define VISION_CMD_LEN 3 + +/* 命令码 */ +#define VISION_CMD_SWITCH_AUTO 0x01 +#define VISION_CMD_SWITCH_MANUAL 0x02 +#define VISION_CMD_LASER_ON 0x03 +#define VISION_CMD_LASER_OFF 0x04 + +/* 工作模式 */ +typedef enum +{ + VISION_MODE_AUTO = 0, // 自动/视觉追踪 + VISION_MODE_MANUAL = 1, // 手动 +} Vision_Mode_e; + +typedef struct +{ + /* 自动模式数据 */ + float error_x; + float error_y; + uint8_t auto_updated; // 收到新的自动数据帧标志 + + /* 手动模式数据 */ + uint8_t zero_force; + float yaw_ref; + float pitch_ref; + uint8_t manual_updated; // 收到新的手动数据帧标志 + + /* 命令 */ + uint8_t cmd; // 最新命令码 + uint8_t cmd_pending; // 有待处理命令标志 + + float frequency; // 接收频率 (Hz) +} VisionData_t; + +extern VisionData_t vision_data; void Vision_Uart_Init(UART_HandleTypeDef *_handle); -extern float opencv_Vision_data[12]; // 视觉数据,12个float -extern uint8_t Vison_revc_flag; // 视觉数据接收标志位 -extern uint8_t vision_used_flag; // 视觉数据使用标志位,用于判断是否需要使用视觉数据 -extern float Vison_Frequency; // 视觉数据频率 - -#endif // !OPENCV_H +#endif // OPENCV_H