diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 0859405..559f45e 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -10,9 +10,10 @@ "UNICODE", "_UNICODE" ], + "compilerPath": "D:\\MinGW\\bin\\gcc.exe", "cStandard": "c17", - "cppStandard": "gnu++17", - "intelliSenseMode": "windows-gcc-arm", + "cppStandard": "gnu++14", + "intelliSenseMode": "windows-gcc-x86", "configurationProvider": "ms-vscode.makefile-tools" } ], diff --git a/.vscode/launch.json b/.vscode/launch.json index 0e9742c..704f635 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -21,7 +21,11 @@ ], "runToEntryPoint": "main", // 调试时在main函数入口停下 "rtos": "FreeRTOS", - //"preLaunchTask": "build task",//先运行Build任务编译项目,取消注释即可使用 + "preLaunchTask": "build task",//先运行Build任务编译项目,取消注释即可使用 + "liveWatch": { + "enabled": true, + "samplesPerSecond": 4 + } // dap若要使用log,请使用Jlink调试任务启动,之后再打开log任务 // 若想要在调试前编译并且打开log,可只使用log的prelaunch task并为log任务添加depends on依赖 }, @@ -39,7 +43,11 @@ "interface": "swd", "svdFile": "STM32F407.svd", "rtos": "FreeRTOS", - // "preLaunchTask": "build task",//先运行Build任务,取消注释即可使用 + "preLaunchTask": "build task",//先运行Build任务,取消注释即可使用 + "liveWatch": { + "enabled": true, + "samplesPerSecond": 4 + } //"preLaunchTask": "log", // 调试时同时开启RTT viewer窗口,若daplink使用jlinkGDBserver启动,需要先开始调试再打开log // 若想要在调试前编译并且打开log,可只使用log的prelaunch task并为log任务添加depends on依赖 }, diff --git a/.vscode/settings.json b/.vscode/settings.json index c84d887..7590390 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,68 +1,5 @@ { "files.associations": { - "robot_def.h": "c", - "bsp_dwt.h": "c", - "dji_motor.h": "c", - "message_center.h": "c", - "super_cap.h": "c", - "can_comm.h": "c", - "lqr.h": "c", - "math.h": "c", - "stdint.h": "c", - "general_def.h": "c", - "lk9025.h": "c", - "arm_math.h": "c", - "bmi088driver.h": "c", - "bmi088middleware.h": "c", - "bmi088_regndef.h": "c", - "bmi088reg.h": "c", - "balance.h": "c", - "stdlib.h": "c", - "memory.h": "c", - "bsp_usart.h": "c", - "compare": "c", - "limits": "c", - "*.tcc": "c", - "type_traits": "c", - "bsp_log.h": "c", - "segger_rtt.h": "c", - "referee.h": "c", - "referee_communication.h": "c", - "vmc_project.h": "c", - "user_lib.h": "c", - "quaternionekf.h": "c", - "bsp_usb.h": "c", - "robot.h": "c", - "rm_referee.h": "c", - "stdio.h": "c", - "crc.h": "c", - "bmi088.h": "c", - "cmath": "c", - "ht04.h": "c", - "gain_table.h": "c", - "referee_task.h": "c", - "task.h": "c", - "robot_task.h": "c", - "motor_task.h": "c", - "bsp_flash.h": "c", - "bsp_iic.h": "c", - "usbd_cdc_if.h": "c", - "kf.h": "c", - "none.h": "c", - "buzzer.h": "c", - "bsp_pwm.h": "c", - "main.h": "c", - "stm32f4xx_hal_conf.h": "c", - "master_process.h": "c", - "bsp_can.h": "c", - "can.h": "c", - "servo_motor.h": "c" - }, - // "clangd.arguments": [ - // "-query-driver=C:/msys64/mingw64/bin/arm-none-eabi-*.exe", - // ], - "cortex-debug.variableUseNaturalFormat": true, - "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", - // "C_Cpp.default.compilerPath": "D:\\Msys2\\mingw64\\bin\\arm-none-eabi-gcc.exe" - "makefile.compileCommandsPath": "build/compile_commands.json" + "stm32f4xx_hal_def.h": "c" + } } \ No newline at end of file diff --git a/Makefile b/Makefile index a79f07e..29bbfeb 100644 --- a/Makefile +++ b/Makefile @@ -151,7 +151,7 @@ modules/message_center/message_center.c \ modules/daemon/daemon.c \ modules/alarm/buzzer.c \ application/gimbal/gimbal.c \ -application/chassis/chassis.c \ +application/chassis/balance.c \ application/shoot/shoot.c \ application/cmd/robot_cmd.c \ application/robot.c diff --git a/application/chassis/balance.c b/application/chassis/balance.c new file mode 100644 index 0000000..2e98c8e --- /dev/null +++ b/application/chassis/balance.c @@ -0,0 +1,225 @@ +// app +#include "balance.h" +#include "robot_def.h" +#include "general_def.h" +#include "ins_task.h" +#include "HT04.h" +#include "LK9025.h" +#include "controller.h" +#include "can_comm.h" +#include "super_cap.h" +#include "user_lib.h" +#include "remote_control.h" +#include "referee_task.h" +#include "stdint.h" +#include "arm_math.h" // 需要用到较多三角函数 +#include "bsp_dwt.h" +#include "bsp_log.h" + + +static uint32_t balance_dwt_cnt; +static float del_t; + +/* 底盘拥有的模块实例 */ +static attitude_t *imu_data; +static RC_ctrl_t *rc_data; // 调试用 +static Referee_Interactive_info_t my_ui; +static referee_info_t *referee_data; +static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // syh +static Chassis_Upload_Data_s chassis_feed; +static CANCommInstance *ci; +static SuperCapInstance *cap; // syh +// 四个关节电机和两个驱动轮电机 +static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试 +static LKMotorInstance *l_driven, *r_driven, *driven[2]; +// 两个腿的参数,0为左腿,1为右腿 +static LinkNPodParam l_side, r_side; // syh phi5 +static ChassisParam chassis; +// 综合运动补偿的PID控制器 +static PIDInstance steer_p_pid, steer_v_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量 +static PIDInstance anti_crash_pid, phi5_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上 +static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧,不要积分(弹簧是无积分二阶系统),增益不可过大否则抗外界冲击响应时太"硬" +static PIDInstance legdot_pid_l, legdot_pid_r; +static PIDInstance roll_compensate_pid, rolldot_pid; // roll轴补偿,用于保持机体水平 + +static Robot_Status_e chassis_status; + +void BalanceInit() +{ + referee_data = UITaskInit(&huart6, &my_ui); + rc_data = RemoteControlInit(&huart3); + CANComm_Init_Config_s commconf = { + .can_config = { + .can_handle = &hcan1, + .tx_id = 0x40, + .rx_id = 0x41}, + .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s), + .send_data_len = sizeof(Chassis_Upload_Data_s)}; + ci = CANCommInit(&commconf); + imu_data = INS_Init(); + + SuperCap_Init_Config_s cap_conf = { + .can_config = { + .can_handle = &hcan1, + .tx_id = 0x302, // todo 电容id + .rx_id = 0x301}}; + cap = SuperCapInit(&cap_conf); + + Motor_Init_Config_s joint_conf = { + // 写一个,剩下的修改方向和id即可 + .can_init_config = { + .can_handle = &hcan1}, + .controller_param_init_config = { + .angle_PID = { + .Kp = 0.3, + .Kd = 0.1, + .Ki = 0, + .DeadBand = 0.0001, + .Improve = PID_DerivativeFilter, + .MaxOut = 4, + .Derivative_LPF_RC = 0.05, + }, // 仅用于复位腿 + }, + .controller_setting_init_config = { + .close_loop_type = ANGLE_LOOP, + .outer_loop_type = OPEN_LOOP, + .motor_reverse_flag = FEEDBACK_DIRECTION_NORMAL, + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + }, + .motor_type = HT04}; + joint_conf.can_init_config.tx_id = 4; + joint_conf.can_init_config.rx_id = 14; + joint[LF] = lf = HTMotorInit(&joint_conf); + joint_conf.can_init_config.tx_id = 3; + joint_conf.can_init_config.rx_id = 13; + joint[LB] = lb = HTMotorInit(&joint_conf); + joint_conf.can_init_config.tx_id = 2; + joint_conf.can_init_config.rx_id = 12; + joint[RF] = rf = HTMotorInit(&joint_conf); + joint_conf.can_init_config.tx_id = 1; + joint_conf.can_init_config.rx_id = 11; + joint[RB] = rb = HTMotorInit(&joint_conf); + + Motor_Init_Config_s driven_conf = { + // 写一个,剩下的修改方向和id即可 + .can_init_config.can_handle = &hcan2, + .controller_setting_init_config = { + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + .outer_loop_type = OPEN_LOOP, + .close_loop_type = OPEN_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, + }, + .motor_type = LK9025, + }; + driven_conf.can_init_config.tx_id = 1; + driven[LD] = l_driven = LKMotorInit(&driven_conf); + driven_conf.can_init_config.tx_id = 2; + driven[RD] = r_driven = LKMotorInit(&driven_conf); + + PID_Init_Config_s steer_p_pid_conf = { + .Kp = 2, + .Kd = 1, + .Ki = 0.0f, + .MaxOut = 4, + .DeadBand = 0.01f, + .Improve = PID_DerivativeFilter, + .Derivative_LPF_RC = 0.05, + }; + PIDInit(&steer_p_pid, &steer_p_pid_conf); + PID_Init_Config_s steer_v_pid_conf = { + .Kp = 2, + .Kd = 0.0f, + .Ki = 0.0f, + .MaxOut = 100, + .DeadBand = 0.0f, + .Improve = PID_DerivativeFilter | PID_Integral_Limit, + .Derivative_LPF_RC = 0.05, + .IntegralLimit = 2, + }; + PIDInit(&steer_v_pid, &steer_v_pid_conf); + + PID_Init_Config_s anti_crash_pid_conf = { + .Kp = 8, + .Kd = 2.5, + .Ki = 0.4, + .MaxOut = 45, + .DeadBand = 0.01f, + .Improve = PID_DerivativeFilter | PID_ChangingIntegrationRate | PID_Integral_Limit, + .Derivative_LPF_RC = 0.05, + .CoefA = 0.05, + .CoefB = 0.05, + .IntegralLimit = 2, + }; + PIDInit(&anti_crash_pid, &anti_crash_pid_conf); + + PID_Init_Config_s leg_length_pid_conf = { + .Kp = 450, + .Kd = 150, + .Ki = 5, + .MaxOut = 60, + .DeadBand = 0.0001f, + .Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement, + .CoefA = 0.01, + .CoefB = 0.02, + .Derivative_LPF_RC = 0.08, + }; + PIDInit(&leglen_pid_l, &leg_length_pid_conf); + PIDInit(&leglen_pid_r, &leg_length_pid_conf); + + PID_Init_Config_s roll_compensate_pid_conf = { + .Kp = 0.0008f, + .Kd = 0.00065f, + .Ki = 0.0f, + .MaxOut = 0.04, + .DeadBand = 0.005f, + .Improve = PID_DerivativeFilter, + .Derivative_LPF_RC = 0.05, + }; + PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf); + + l_side.target_len = r_side.target_len = 0.23; + chassis.vel_cov = 1000; // 初始化速度协方差 + chassis_status = ROBOT_READY; + DWT_GetDeltaT(&balance_dwt_cnt); +} + +static void EnableAllMotor() /* 打开所有电机 */ +{ + for (uint8_t i = 0; i < JOINT_CNT; i++) // 打开关节电机 + HTMotorEnable(joint[i]); + for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 打开驱动电机 + LKMotorEnable(driven[i]); +} + +/* 切换底盘遥控器控制和云台双板控制 */ +static void ControlSwitch() +{ // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令 + if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline()) + { + if (rc_data->rc.rocker_l1 < -600) + { + chassis_cmd_recv.chassis_mode = CHASSIS_RESET; + chassis_cmd_recv.vx = 0.5 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s + } + else // 设定值覆盖双板 + { + chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后 + chassis_cmd_recv.vx = 0.02 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s + chassis_cmd_recv.offset_angle = 0.001 * (float)rc_data[TEMP].rc.rocker_r_; // rotate? follow. + chassis_cmd_recv.delta_leglen = -0.0000015f * (float)rc_data[TEMP].rc.dial; + } + } + else if (CANCommIsOnline(ci) && !switch_is_down(rc_data->rc.switch_right)) + chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(ci); // 获取云台板指令 + else + chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停 +} + + +void BalanceTask() +{ + + +} \ No newline at end of file diff --git a/application/chassis/balance.h b/application/chassis/balance.h index e69de29..f652056 100644 --- a/application/chassis/balance.h +++ b/application/chassis/balance.h @@ -0,0 +1,90 @@ +#pragma once + +// 底盘参数 +#define CALF_LEN 0.245f // 小腿 +#define THIGH_LEN 0.14f // 大腿 +#define JOINT_DISTANCE 0.108f // 关节间距 +#define WHEEL_RADIUS 0.078f // 轮子半径 +#define LIMIT_LINK_RAD 0.15149458 // 初始限位角度,见ParamAssemble +#define WHEEL_DISTANCE 0.48f // 轮子间距 +#define BALANCE_GRAVITY_BIAS 0 +#define ROLL_GRAVITY_BIAS 0.03f +#define MAX_ACC_REF 0.7f +#define MAX_DIST_TRACK 0.1f +#define MAX_VEL_TRACK 0.5f + +#define CENTER_IMU_R 0.13f // IMU距离中心的距离 +#define CENTER_IMU_W 0.11f +#define CENTER_IMU_L 0.074f +#define CENTER_IMU_H 0.060f +#define CENTER_IMU_THETA 0.9768f + +#define VEL_PROCESS_NOISE 25 // 速度过程噪声 +#define VEL_MEASURE_NOISE 800 // 速度测量噪声 +// 同时估计加速度和速度时对加速度的噪声 +// 更好的方法是设置为动态,当有冲击时/加加速度大时更相信轮速 +#define ACC_PROCESS_NOISE 2000 // 加速度过程噪声 +#define ACC_MEASURE_NOISE 0.01 // 加速度测量噪声 + +// 用于循环枚举的宏,方便访问关节电机和驱动轮电机 +#define JOINT_CNT 4u +#define LF 0u +#define LB 1u +#define RF 2u +#define RB 3u + +#define DRIVEN_CNT 2u +#define LD 0u +#define RD 1u + +typedef struct +{ + // joint + float phi1_w, phi4_w, phi2_w, phi5_w; // phi2_w used for calc real wheel speed + float T_back, T_front; + // link angle, phi1-ph5, phi5 is pod angle + float phi1, phi2, phi3, phi4, phi5; + // wheel + float w_ecd; // 电机编码器速度 + float wheel_dist; // 单侧轮子的位移 + float wheel_w; // 单侧轮子的速度 + float body_v; // 髋关节速度 + float T_wheel; + // pod + float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一 + float leg_len, legd; + float height, height_v; + float F_leg, T_hip; + float target_len; + + float coord[6]; // xb yb xc yc xd yd + + float wheel_out[7]; + float hip_out[7]; +} LinkNPodParam; + +typedef struct +{ + float vel, target_v; // 底盘速度 + float vel_m; // 底盘速度测量值 + float vel_predict; // 底盘速度预测值 + float vel_cov; // 速度方差 + float acc, acc_m, acc_last; // 水平方向加速度,用于计算速度预测值 + + float dist, target_dist; // 底盘位移距离 + float yaw, wz, target_yaw; // yaw角度和底盘角速度 + float pitch, pitch_w; // 底盘俯仰角度和角速度 + float roll, roll_w; // 底盘横滚角度和角速度 +} ChassisParam; + +/** + * @brief 平衡底盘初始化 + * + */ +void BalanceInit(); + +/** + * @brief 平衡底盘任务 + * + */ +void BalanceTask(); diff --git a/application/chassis/balance.md b/application/chassis/balance.md new file mode 100644 index 0000000..e653b73 --- /dev/null +++ b/application/chassis/balance.md @@ -0,0 +1,32 @@ +# balance + +可以继续解耦,将VMC独立成模块. + +目前默认使用平衡底盘时为双板. + +## 工作流程 + +1. 获取控制信息和状态信息，组装到linkparam +2. 根据控制模式将控制指令转化为实际的参考输入 +3. 使用lqr得出的反馈增益,计算二阶倒立摆模型的控制输出;需要根据当前腿长查gain table,或预先拟合K=f(Leg)的函数 +4. 计算二阶倒立摆$[L0 phi0]$和轮腿[phi1 phi4]间的雅可比,根据VMC将lqr的输出[F Tp]映射成[T1 T2] ; 驱动轮不需要映射 +5. 进行综合运动补偿,即转向控制和抗劈叉 +6. 进行腿长控制计算,即长度控制和roll轴水平控制 +7. 进行离地检测判断是否要让腿保持垂直,后续再加入跳跃功能 +8. 根据裁判系统和超级电容的功率信息进行输出限幅 +9. 设置反馈信息,包括裁判系统的数据,并通过电机反馈和IMU数据计算底盘实际运动状态等 +10. 推送反馈信息 + +电机初始化为电流环即可,注意基于模型的控制需要正确设定单位 + +如果功率可能超限,需要判定降低功率输出后受影响最小的执行单元,并给予其较大的功率输出衰减(一般不会超功率) + +另外, 选择平衡底盘有枪口冷却增益, 注意将这一部分改变反馈给cmd, 以使得shoot有更好的表现 + +## 优化环节 + +为了控制系统有更好的效果，对工程上的细节有一些微小的优化如下： + +1. 腿长控制实际上对机体高度计算腿长闭环，使得云台能够保持恒定高度 +2. 静止时开启位置反馈，若有速度输入则不使用位置反馈，从而避免底盘打滑的情况 +3. 没有转向输入时使用轮式里程计计算位置x，有转向输入时使用imu的二重积分，从而避免平衡控制器和转向控制器冲突 \ No newline at end of file diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c deleted file mode 100644 index 61b43d8..0000000 --- a/application/chassis/chassis.c +++ /dev/null @@ -1,257 +0,0 @@ -/** - * @file chassis.c - * @author NeoZeng neozng1@hnu.edu.cn - * @brief 底盘应用,负责接收robot_cmd的控制命令并根据命令进行运动学解算,得到输出 - * 注意底盘采取右手系,对于平面视图,底盘纵向运动的正前方为x正方向;横向运动的右侧为y正方向 - * - * @version 0.1 - * @date 2022-12-04 - * - * @copyright Copyright (c) 2022 - * - */ - -#include "chassis.h" -#include "robot_def.h" -#include "dji_motor.h" -#include "super_cap.h" -#include "message_center.h" -#include "referee_task.h" - -#include "general_def.h" -#include "bsp_dwt.h" -#include "referee_UI.h" -#include "arm_math.h" - -/* 根据robot_def.h中的macro自动计算的参数 */ -#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距 -#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距 -#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长 - -/* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */ -#ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度 -#include "can_comm.h" -#include "ins_task.h" -static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm -attitude_t *Chassis_IMU_data; -#endif // CHASSIS_BOARD -#ifdef ONE_BOARD -static Publisher_t *chassis_pub; // 用于发布底盘的数据 -static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令 -#endif // !ONE_BOARD -static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令 -static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据 - -static referee_info_t* referee_data; // 用于获取裁判系统的数据 -static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI - -static SuperCapInstance *cap; // 超级电容 -static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back - -/* 用于自旋变速策略的时间变量 */ -// static float t; - -/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */ -static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘 -static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅 - -void ChassisInit() -{ - // 四个轮子的参数一样,改tx_id和反转标志位即可 - Motor_Init_Config_s chassis_motor_config = { - .can_init_config.can_handle = &hcan1, - .controller_param_init_config = { - .speed_PID = { - .Kp = 10, // 4.5 - .Ki = 0, // 0 - .Kd = 0, // 0 - .IntegralLimit = 3000, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .MaxOut = 12000, - }, - .current_PID = { - .Kp = 0.5, // 0.4 - .Ki = 0, // 0 - .Kd = 0, - .IntegralLimit = 3000, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .MaxOut = 15000, - }, - }, - .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, - .outer_loop_type = SPEED_LOOP, - .close_loop_type = SPEED_LOOP | CURRENT_LOOP, - }, - .motor_type = M3508, - }; - // @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改. - chassis_motor_config.can_init_config.tx_id = 1; - chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; - motor_lf = DJIMotorInit(&chassis_motor_config); - - chassis_motor_config.can_init_config.tx_id = 2; - chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; - motor_rf = DJIMotorInit(&chassis_motor_config); - - chassis_motor_config.can_init_config.tx_id = 4; - chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; - motor_lb = DJIMotorInit(&chassis_motor_config); - - chassis_motor_config.can_init_config.tx_id = 3; - chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; - motor_rb = DJIMotorInit(&chassis_motor_config); - - referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI - - SuperCap_Init_Config_s cap_conf = { - .can_config = { - .can_handle = &hcan2, - .tx_id = 0x302, // 超级电容默认接收id - .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的 - }}; - cap = SuperCapInit(&cap_conf); // 超级电容初始化 - - // 发布订阅初始化,如果为双板,则需要can comm来传递消息 -#ifdef CHASSIS_BOARD - Chassis_IMU_data = INS_Init(); // 底盘IMU初始化 - - CANComm_Init_Config_s comm_conf = { - .can_config = { - .can_handle = &hcan2, - .tx_id = 0x311, - .rx_id = 0x312, - }, - .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s), - .send_data_len = sizeof(Chassis_Upload_Data_s), - }; - chasiss_can_comm = CANCommInit(&comm_conf); // can comm初始化 -#endif // CHASSIS_BOARD - -#ifdef ONE_BOARD // 单板控制整车,则通过pubsub来传递消息 - chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s)); - chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s)); -#endif // ONE_BOARD -} - -#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD) -#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD) -#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD) -#define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD) -/** - * @brief 计算每个轮毂电机的输出,正运动学解算 - * 用宏进行预替换减小开销,运动解算具体过程参考教程 - */ -static void MecanumCalculate() -{ - vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER; - vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER; - vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER; - vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER; -} - -/** - * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值 - * - */ -static void LimitChassisOutput() -{ - // 功率限制待添加 - // referee_data->PowerHeatData.chassis_power; - // referee_data->PowerHeatData.chassis_power_buffer; - - // 完成功率限制后进行电机参考输入设定 - DJIMotorSetRef(motor_lf, vt_lf); - DJIMotorSetRef(motor_rf, vt_rf); - DJIMotorSetRef(motor_lb, vt_lb); - DJIMotorSetRef(motor_rb, vt_rb); -} - -/** - * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算 - * 对于双板的情况,考虑增加来自底盘板IMU的数据 - * - */ -static void EstimateSpeed() -{ - // 根据电机速度和陀螺仪的角速度进行解算,还可以利用加速度计判断是否打滑(如果有) - // chassis_feedback_data.vx vy wz = - // ... -} - -/* 机器人底盘控制核心任务 */ -void ChassisTask() -{ - // 后续增加没收到消息的处理(双板的情况) - // 获取新的控制信息 -#ifdef ONE_BOARD - SubGetMessage(chassis_sub, &chassis_cmd_recv); -#endif -#ifdef CHASSIS_BOARD - chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm); -#endif // CHASSIS_BOARD - - if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) - { // 如果出现重要模块离线或遥控器设置为急停,让电机停止 - DJIMotorStop(motor_lf); - DJIMotorStop(motor_rf); - DJIMotorStop(motor_lb); - DJIMotorStop(motor_rb); - } - else - { // 正常工作 - DJIMotorEnable(motor_lf); - DJIMotorEnable(motor_rf); - DJIMotorEnable(motor_lb); - DJIMotorEnable(motor_rb); - } - - // 根据控制模式设定旋转速度 - switch (chassis_cmd_recv.chassis_mode) - { - case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态 - chassis_cmd_recv.wz = 0; - break; - case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 - chassis_cmd_recv.wz = -1.5f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); - break; - case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 - chassis_cmd_recv.wz = 4000; - break; - default: - break; - } - - // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 - // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东) - static float sin_theta, cos_theta; - cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); - sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); - chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta; - chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta; - - // 根据控制模式进行正运动学解算,计算底盘输出 - MecanumCalculate(); - - // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值 - LimitChassisOutput(); - - // 根据电机的反馈速度和IMU(如果有)计算真实速度 - EstimateSpeed(); - - // // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 - // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red - // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0; - // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 - // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit; - // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0; - - // 推送反馈消息 -#ifdef ONE_BOARD - PubPushMessage(chassis_pub, (void *)&chassis_feedback_data); -#endif -#ifdef CHASSIS_BOARD - CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data); -#endif // CHASSIS_BOARD -} \ No newline at end of file diff --git a/application/chassis/chassis.h b/application/chassis/chassis.h deleted file mode 100644 index ea7c0b4..0000000 --- a/application/chassis/chassis.h +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef CHASSIS_H -#define CHASSIS_H - -/** - * @brief 底盘应用初始化,请在开启rtos之前调用(目前会被RobotInit()调用) - * - */ -void ChassisInit(); - -/** - * @brief 底盘应用任务,放入实时系统以一定频率运行 - * - */ -void ChassisTask(); - -#endif // CHASSIS_H \ No newline at end of file diff --git a/application/chassis/chassis.md b/application/chassis/chassis.md deleted file mode 100644 index b5e1b1b..0000000 --- a/application/chassis/chassis.md +++ /dev/null @@ -1,24 +0,0 @@ -# chassis - - -@Todo 使用条件编译，选择麦轮(全向轮),舵轮,平衡底盘 -## 工作流程 - -首先进行初始化，`ChasissInit()`会被`RobotInit()`调用，进行裁判系统、底盘电机的初始化。如果为双板模式，则还会初始化IMU，并且将消息订阅者和发布者的初始化改为`CANComm`的初始化。 - -操作系统启动后，工作顺序为： - -1. 从cmd模块获取数据（如果双板则从CANComm获取） -2. 判断当前控制数据的模式，如果为停止则停止所有电机 -3. 根据控制数据，计算底盘的旋转速度 -4. 根据控制数据中yaw电机的编码器值`angle_offset`，将控制数据映射到底盘坐标系下 -5. 进行麦克纳姆轮的运动学解算，得到每个电机的设定值 -6. 获取裁判系统的数据，并根据底盘功率限制对输出进行限幅 -7. 由电机的反馈数据和IMU（如果有），计算底盘当前的真实运动速度 -8. 设置底盘反馈数据，包括运动速度和裁判系统数据 -9. 将反馈数据推送到消息中心（如果双板则通过CANComm发送） - - -### 后续支持平衡底盘 - -新增一个app balance_chassis \ No newline at end of file diff --git a/application/chassis/mecanum.h b/application/chassis/mecanum.h deleted file mode 100644 index e69de29..0000000 diff --git a/application/chassis/steering.h b/application/chassis/steering.h deleted file mode 100644 index e69de29..0000000 diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 4b91006..54e6c36 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -13,345 +13,14 @@ #include "bsp_dwt.h" #include "bsp_log.h" -// 私有宏,自动将编码器转换成角度值 -#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 -/* 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 Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 -static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 - -static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回 -static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回 -static Vision_Send_s vision_send_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; // 机器人整体工作状态 - -BMI088Instance *bmi088_test; // 云台IMU -BMI088_Data_t bmi088_data; void RobotCMDInit() { - // BMI088_Init_Config_s bmi088_config = { - // .cali_mode = BMI088_CALIBRATE_ONLINE_MODE, - // .work_mode = BMI088_BLOCK_TRIGGER_MODE, - // .spi_acc_config = { - // .spi_handle = &hspi1, - // .GPIOx = GPIOA, - // .cs_pin = GPIO_PIN_4, - // .spi_work_mode = SPI_DMA_MODE, - // }, - // .acc_int_config = { - // .GPIOx = GPIOC, - // .GPIO_Pin = GPIO_PIN_4, - // .exti_mode = GPIO_EXTI_MODE_RISING, - // }, - // .spi_gyro_config = { - // .spi_handle = &hspi1, - // .GPIOx = GPIOB, - // .cs_pin = GPIO_PIN_0, - // .spi_work_mode = SPI_DMA_MODE, - // }, - // .gyro_int_config = { - // .GPIO_Pin = GPIO_PIN_5, - // .GPIOx = GPIOC, - // .exti_mode = GPIO_EXTI_MODE_RISING, - // }, - // .heat_pwm_config = { - // .htim = &htim10, - // .channel = TIM_CHANNEL_1, - // .period = 1, - // }, - // .heat_pid_config = { - // .Kp = 0.5, - // .Ki = 0, - // .Kd = 0, - // .DeadBand = 0.1, - // .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - // .IntegralLimit = 100, - // .MaxOut = 100, - // }, - // }; - //bmi088_test = BMI088Register(&bmi088_config); - rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 - vision_recv_data = VisionInit(&huart1); // 视觉通信串口 - 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)); - -#ifdef ONE_BOARD // 双板兼容 - chassis_cmd_pub = PubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s)); - chassis_feed_sub = SubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s)); -#endif // ONE_BOARD -#ifdef GIMBAL_BOARD - CANComm_Init_Config_s comm_conf = { - .can_config = { - .can_handle = &hcan1, - .tx_id = 0x312, - .rx_id = 0x311, - }, - .recv_data_len = sizeof(Chassis_Upload_Data_s), - .send_data_len = sizeof(Chassis_Ctrl_Cmd_s), - }; - cmd_can_comm = CANCommInit(&comm_conf); -#endif // GIMBAL_BOARD - gimbal_cmd_send.pitch = 0; - - robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 } -/** - * @brief 根据gimbal app传回的当前电机角度计算和零位的误差 - * 单圈绝对角度的范围是0~360,说明文档中有图示 - * - */ -static void CalcOffsetAngle() -{ - // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数 - static float angle; - angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度 -#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度 - if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE) - chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE; - else if (angle > 180.0f + YAW_ALIGN_ANGLE) - chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE - 360.0f; - else - chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE; -#else // 小于180度 - if (angle > YAW_ALIGN_ANGLE) - chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE; - else if (angle <= YAW_ALIGN_ANGLE && angle >= YAW_ALIGN_ANGLE - 180.0f) - chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE; - else - chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE + 360.0f; -#endif -} -/** - * @brief 控制输入为遥控器(调试时)的模式和控制量设置 - * - */ -static void RemoteControlSet() -{ - // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? - if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台 - { - chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - } - else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 - { - chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; - gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; - } - - // 云台参数,确定云台控制数据 - if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式 - { - // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制 - // ... - } - // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制 - if (switch_is_down(rc_data[TEMP].rc.switch_left) || vision_recv_data->target_state == NO_TARGET) - { // 按照摇杆的输出大小进行角度增量,增益系数需调整 - gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_; - gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1; - } - // 云台软件限位 - - // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 - chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向 - chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向 - - // 发射参数 - if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 - ; // 弹舱舵机控制,待添加servo_motor模块,开启 - else - ; // 弹舱舵机控制,待添加servo_motor模块,关闭 - - // 摩擦轮控制,拨轮向上打为负,向下为正 - if (rc_data[TEMP].rc.dial < -100) // 向上超过100,打开摩擦轮 - shoot_cmd_send.friction_mode = FRICTION_ON; - else - shoot_cmd_send.friction_mode = FRICTION_OFF; - // 拨弹控制,遥控器固定为一种拨弹模式,可自行选择 - if (rc_data[TEMP].rc.dial < -500) - shoot_cmd_send.load_mode = LOAD_BURSTFIRE; - else - shoot_cmd_send.load_mode = LOAD_STOP; - // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频, - shoot_cmd_send.shoot_rate = 8; -} - -/** - * @brief 输入为键鼠时模式和控制量设置 - * - */ -static void MouseKeySet() -{ - chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测 - chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300; - - gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测 - gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10; - - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速 - { - case 0: - shoot_cmd_send.bullet_speed = 15; - break; - case 1: - shoot_cmd_send.bullet_speed = 18; - break; - default: - shoot_cmd_send.bullet_speed = 30; - break; - } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式 - { - case 0: - shoot_cmd_send.load_mode = LOAD_STOP; - break; - case 1: - shoot_cmd_send.load_mode = LOAD_1_BULLET; - break; - case 2: - shoot_cmd_send.load_mode = LOAD_3_BULLET; - break; - default: - shoot_cmd_send.load_mode = LOAD_BURSTFIRE; - break; - } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱 - { - case 0: - shoot_cmd_send.lid_mode = LID_OPEN; - break; - default: - shoot_cmd_send.lid_mode = LID_CLOSE; - break; - } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮 - { - case 0: - shoot_cmd_send.friction_mode = FRICTION_OFF; - break; - default: - shoot_cmd_send.friction_mode = FRICTION_ON; - break; - } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度 - { - case 0: - chassis_cmd_send.chassis_speed_buff = 40; - break; - case 1: - chassis_cmd_send.chassis_speed_buff = 60; - break; - case 2: - chassis_cmd_send.chassis_speed_buff = 80; - break; - default: - chassis_cmd_send.chassis_speed_buff = 100; - break; - } - switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量 - { - case 1: - - break; - - default: - - break; - } -} - -/** - * @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等 - * 停止的阈值'300'待修改成合适的值,或改为开关控制. - * - * @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现 - * - */ -static void EmergencyHandler() -{ - // 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正 - if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断 - { - robot_state = ROBOT_STOP; - gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; - chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; - shoot_cmd_send.shoot_mode = SHOOT_OFF; - shoot_cmd_send.friction_mode = FRICTION_OFF; - shoot_cmd_send.load_mode = LOAD_STOP; - LOGERROR("[CMD] emergency stop!"); - } - // 遥控器右侧开关为[上],恢复正常运行 - if (switch_is_up(rc_data[TEMP].rc.switch_right)) - { - robot_state = ROBOT_READY; - shoot_cmd_send.shoot_mode = SHOOT_ON; - LOGINFO("[CMD] reinstate, robot ready"); - } -} - -/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */ void RobotCMDTask() { - // BMI088Acquire(bmi088_test,&bmi088_data) ; - // 从其他应用获取回传数据 -#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); - - // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 - CalcOffsetAngle(); - // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 - if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制 - RemoteControlSet(); - else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制 - MouseKeySet(); - - EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 - - // 设置视觉发送数据,还需增加加速度和角速度数据 - // VisionSetFlag(chassis_fetch_data.enemy_color,,chassis_fetch_data.bullet_speed) - - // 推送消息,双板通信,视觉通信等 - // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 -#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); - VisionSend(&vision_send_data); + } diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index b8a99a9..cb060fb 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -6,149 +6,14 @@ #include "general_def.h" #include "bmi088.h" -static attitude_t *gimba_IMU_data; // 云台IMU数据 -static DJIMotorInstance *yaw_motor, *pitch_motor; -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 BMI088Instance *bmi088; // 云台IMU void GimbalInit() { - gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 - // YAW - Motor_Init_Config_s yaw_config = { - .can_init_config = { - .can_handle = &hcan1, - .tx_id = 1, - }, - .controller_param_init_config = { - .angle_PID = { - .Kp = 8, // 8 - .Ki = 0, - .Kd = 0, - .DeadBand = 0.1, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 100, - .MaxOut = 500, - }, - .speed_PID = { - .Kp = 50, // 50 - .Ki = 200, // 200 - .Kd = 0, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 3000, - .MaxOut = 20000, - }, - .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, - // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2], - }, - .controller_setting_init_config = { - .angle_feedback_source = OTHER_FEED, - .speed_feedback_source = OTHER_FEED, - .outer_loop_type = ANGLE_LOOP, - .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, - }, - .motor_type = GM6020}; - // PITCH - Motor_Init_Config_s pitch_config = { - .can_init_config = { - .can_handle = &hcan2, - .tx_id = 2, - }, - .controller_param_init_config = { - .angle_PID = { - .Kp = 10, // 10 - .Ki = 0, - .Kd = 0, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 100, - .MaxOut = 500, - }, - .speed_PID = { - .Kp = 50, // 50 - .Ki = 350, // 350 - .Kd = 0, // 0 - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 2500, - .MaxOut = 20000, - }, - .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, - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, - }, - .motor_type = GM6020, - }; - // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 - yaw_motor = DJIMotorInit(&yaw_config); - pitch_motor = DJIMotorInit(&pitch_config); - - gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); - gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); } /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ void GimbalTask() { - // 获取云台控制数据 - // 后续增加未收到数据的处理 - SubGetMessage(gimbal_sub, &gimbal_cmd_recv); - - // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 - // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref - switch (gimbal_cmd_recv.gimbal_mode) - { - // 停止 - case GIMBAL_ZERO_FORCE: - DJIMotorStop(yaw_motor); - DJIMotorStop(pitch_motor); - break; - // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 - case GIMBAL_GYRO_MODE: // 后续只保留此模式 - DJIMotorEnable(yaw_motor); - DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 - DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); - break; - // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 - case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) - DJIMotorEnable(yaw_motor); - DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 - DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); - break; - default: - break; - } - - // 在合适的地方添加pitch重力补偿前馈力矩 - // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩 - // ... - - // 设置反馈数据,主要是imu和yaw的ecd - gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; - gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round; - - // 推送消息 - PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); + } \ No newline at end of file diff --git a/application/robot.c b/application/robot.c index 3102179..945a63b 100644 --- a/application/robot.c +++ b/application/robot.c @@ -10,7 +10,7 @@ #endif // !ROBOT_DEF_PARAM_WARNING #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) -#include "chassis.h" +#include "balance.h" #endif #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) @@ -36,7 +36,7 @@ void RobotInit() #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - ChassisInit(); + BalanceInit(); #endif OSTaskInit(); // 创建基础任务 @@ -54,7 +54,7 @@ void RobotTask() #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - ChassisTask(); + BalanceTask(); #endif } \ No newline at end of file diff --git a/application/robot_def.h b/application/robot_def.h index 3457352..6dec8d4 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -17,8 +17,8 @@ #include "stdint.h" /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ -#define ONE_BOARD // 单板控制整车 -// #define CHASSIS_BOARD //底盘板 +// #define ONE_BOARD // 单板控制整车 +#define CHASSIS_BOARD //底盘板 // #define GIMBAL_BOARD //云台板 #define VISION_USE_VCP // 使用虚拟串口发送视觉数据 @@ -31,17 +31,12 @@ #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 GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反 #define GYRO2GIMBAL_DIR_PITCH 1 // 陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反 @@ -84,8 +79,9 @@ typedef enum { CHASSIS_ZERO_FORCE = 0, // 电流零输入 CHASSIS_ROTATE, // 小陀螺模式 - CHASSIS_NO_FOLLOW, // 不跟随，允许全向平移 CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制 + CHASSIS_RESET, // 底盘重置,双腿缩回 + CHASSIS_FREE_DEBUG, // 底盘单独调试模式 } chassis_mode_e; // 云台模式设置 @@ -129,6 +125,12 @@ typedef struct float chassis_power_mx; } Chassis_Power_Data_s; +typedef enum +{ + CAHSSIS_ALIGN = 0, + CHASSIS_SIDLE +} chassis_direction_e; + /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */ /** * @brief 对于双板情况,遥控器和pc在云台,裁判系统在底盘 @@ -139,14 +141,18 @@ typedef struct { // 控制部分 float vx; // 前进方向速度 - float vy; // 横移方向速度 - float wz; // 旋转速度 + float delta_leglen; // 腿长 float offset_angle; // 底盘和归中位置的夹角 chassis_mode_e chassis_mode; - int chassis_speed_buff; - // UI部分 - // ... + chassis_direction_e direction; + // UI部分 + lid_mode_e lid_mode; + friction_mode_e friction_mode; + Target_State_e target_state; + loader_mode_e loader_mode; + + uint8_t ui_refresh_flag; } Chassis_Ctrl_Cmd_s; // cmd发布的云台控制数据,由gimbal订阅 @@ -187,6 +193,7 @@ typedef struct // float real_vy; // float real_wz; + float yaw_w; // 底盘当前转速 uint8_t rest_heat; // 剩余枪口热量 Bullet_Speed_e bullet_speed; // 弹速限制 Enemy_Color_e enemy_color; // 0 for blue, 1 for red diff --git a/application/robot_task.h b/application/robot_task.h index 561f316..466b430 100644 --- a/application/robot_task.h +++ b/application/robot_task.h @@ -119,9 +119,9 @@ __attribute__((noreturn)) void StartROBOTTASK(void const *argument) robot_start = DWT_GetTimeline_ms(); RobotTask(); robot_dt = DWT_GetTimeline_ms() - robot_start; - if (robot_dt > 5) + if (robot_dt > 1) LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt); - osDelay(5); + osDelay(1); } } diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index e19af5c..7def34a 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -6,208 +6,14 @@ #include "bsp_dwt.h" #include "general_def.h" -/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */ -static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机 -// static servo_instance *lid; 需要增加弹舱盖 - -static Publisher_t *shoot_pub; -static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自cmd的发射控制信息 -static Subscriber_t *shoot_sub; -static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息 - -// dwt定时,计算冷却用 -static float hibernate_time = 0, dead_time = 0; void ShootInit() { - // 左摩擦轮 - Motor_Init_Config_s friction_config = { - .can_init_config = { - .can_handle = &hcan2, - }, - .controller_param_init_config = { - .speed_PID = { - .Kp = 0, // 20 - .Ki = 0, // 1 - .Kd = 0, - .Improve = PID_Integral_Limit, - .IntegralLimit = 10000, - .MaxOut = 15000, - }, - .current_PID = { - .Kp = 0, // 0.7 - .Ki = 0, // 0.1 - .Kd = 0, - .Improve = PID_Integral_Limit, - .IntegralLimit = 10000, - .MaxOut = 15000, - }, - }, - .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, - - .outer_loop_type = SPEED_LOOP, - .close_loop_type = SPEED_LOOP | CURRENT_LOOP, - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, - }, - .motor_type = M3508}; - friction_config.can_init_config.tx_id = 1, - friction_l = DJIMotorInit(&friction_config); - - friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行 - friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; - friction_r = DJIMotorInit(&friction_config); - - // 拨盘电机 - Motor_Init_Config_s loader_config = { - .can_init_config = { - .can_handle = &hcan2, - .tx_id = 3, - }, - .controller_param_init_config = { - .angle_PID = { - // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降 - .Kp = 0, // 10 - .Ki = 0, - .Kd = 0, - .MaxOut = 200, - }, - .speed_PID = { - .Kp = 0, // 10 - .Ki = 0, // 1 - .Kd = 0, - .Improve = PID_Integral_Limit, - .IntegralLimit = 5000, - .MaxOut = 5000, - }, - .current_PID = { - .Kp = 0, // 0.7 - .Ki = 0, // 0.1 - .Kd = 0, - .Improve = PID_Integral_Limit, - .IntegralLimit = 5000, - .MaxOut = 5000, - }, - }, - .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, - .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转 - .close_loop_type = CURRENT_LOOP | SPEED_LOOP, - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向 - }, - .motor_type = M2006 // 英雄使用m3508 - }; - loader = DJIMotorInit(&loader_config); - - shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s)); - shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s)); + } /* 机器人发射机构控制核心任务 */ void ShootTask() { - // 从cmd获取控制数据 - SubGetMessage(shoot_sub, &shoot_cmd_recv); - - // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止) - if (shoot_cmd_recv.shoot_mode == SHOOT_OFF) - { - DJIMotorStop(friction_l); - DJIMotorStop(friction_r); - DJIMotorStop(loader); - } - else // 恢复运行 - { - DJIMotorEnable(friction_l); - DJIMotorEnable(friction_r); - DJIMotorEnable(loader); - } - - // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可 - // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发) - // if (hibernate_time + dead_time > DWT_GetTimeline_ms()) - // return; - - // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换 - switch (shoot_cmd_recv.load_mode) - { - // 停止拨盘 - case LOAD_STOP: - DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环 - DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快 - break; - // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射) - case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用. - DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环 - DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度 - hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 - dead_time = 150; // 完成1发弹丸发射的时间 - break; - // 三连发,如果不需要后续可能删除 - case LOAD_3_BULLET: - DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环 - DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发 - hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 - dead_time = 300; // 完成3发弹丸发射的时间 - break; - // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz - case LOAD_BURSTFIRE: - DJIMotorOuterLoop(loader, SPEED_LOOP); - DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8); - // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second) - break; - // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流) - // 也有可能需要从switch-case中独立出来 - case LOAD_REVERSE: - DJIMotorOuterLoop(loader, SPEED_LOOP); - // ... - break; - default: - while (1) - ; // 未知模式,停止运行,检查指针越界,内存溢出等问题 - } - - // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启) - if (shoot_cmd_recv.friction_mode == FRICTION_ON) - { - // 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入 - switch (shoot_cmd_recv.bullet_speed) - { - case SMALL_AMU_15: - DJIMotorSetRef(friction_l, 0); - DJIMotorSetRef(friction_r, 0); - break; - case SMALL_AMU_18: - DJIMotorSetRef(friction_l, 0); - DJIMotorSetRef(friction_r, 0); - break; - case SMALL_AMU_30: - DJIMotorSetRef(friction_l, 0); - DJIMotorSetRef(friction_r, 0); - break; - default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速. - DJIMotorSetRef(friction_l, 30000); - DJIMotorSetRef(friction_r, 30000); - break; - } - } - else // 关闭摩擦轮 - { - DJIMotorSetRef(friction_l, 0); - DJIMotorSetRef(friction_r, 0); - } - - // 开关弹舱盖 - if (shoot_cmd_recv.lid_mode == LID_CLOSE) - { - //... - } - else if (shoot_cmd_recv.lid_mode == LID_OPEN) - { - //... - } - - // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈 - PubPushMessage(shoot_pub, (void *)&shoot_feedback_data); + } \ No newline at end of file diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index a52980b..18e388d 100644 --- a/modules/motor/motor_task.c +++ b/modules/motor/motor_task.c @@ -10,7 +10,7 @@ void MotorControlTask() // static uint8_t cnt = 0; 设定不同电机的任务频率 // if(cnt%5==0) //200hz // if(cnt%10==0) //100hz - DJIMotorControl(); + // DJIMotorControl(); /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ LKMotorControl(); diff --git a/modules/referee/referee_task.c b/modules/referee/referee_task.c index 7971265..e35ada2 100644 --- a/modules/referee/referee_task.c +++ b/modules/referee/referee_task.c @@ -152,7 +152,6 @@ static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测 } case 2: { - _Interactive_data->chassis_mode = CHASSIS_NO_FOLLOW; _Interactive_data->gimbal_mode = GIMBAL_GYRO_MODE; _Interactive_data->shoot_mode = SHOOT_ON; _Interactive_data->friction_mode = FRICTION_ON; @@ -188,9 +187,6 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "rotate "); // 此处注意字数对齐问题，字数相同才能覆盖掉 break; - case CHASSIS_NO_FOLLOW: - UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "nofollow "); - break; case CHASSIS_FOLLOW_GIMBAL_YAW: UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow "); break;