TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-23 19:25:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

balance init

Browse Source
This commit is contained in:
kai
2024-01-14 21:22:34 +08:00
parent 9dd0057563
commit 6f22c0ac30
20 changed files with 395 additions and 1056 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
.vscode/c_cpp_properties.json vendored
View File

@@ -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"
}
],

12
.vscode/launch.json vendored
View File

@@ -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依赖
},

67
.vscode/settings.json vendored
View File

@@ -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"
}
}

2
Makefile
View File

@@ -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

225
application/chassis/balance.c Normal file
View File

@@ -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()
{
}

90
application/chassis/balance.h
View File

@@ -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();

32
application/chassis/balance.md Normal file
View File

@@ -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的二重积分，从而避免平衡控制器和转向控制器冲突

257
application/chassis/chassis.c
View File

@@ -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
}

16
application/chassis/chassis.h
View File

@@ -1,16 +0,0 @@
#ifndef CHASSIS_H
#define CHASSIS_H
/**
* @brief 底盘应用初始化,请在开启rtos之前调用(目前会被RobotInit()调用)
*
*/
void ChassisInit();
/**
* @brief 底盘应用任务,放入实时系统以一定频率运行
*
*/
void ChassisTask();
#endif // CHASSIS_H

24
application/chassis/chassis.md
View File

@@ -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

0
application/chassis/mecanum.h
View File

0
application/chassis/steering.h
View File

333
application/cmd/robot_cmd.c
View File

@@ -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);
}

137
application/gimbal/gimbal.c
View File

@@ -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);
}

6
application/robot.c
View File

@@ -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
}

37
application/robot_def.h
View File

@@ -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

4
application/robot_task.h
View File

@@ -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);
}
}

198
application/shoot/shoot.c
View File

@@ -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);
}

2
modules/motor/motor_task.c
View File

@@ -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();

4
modules/referee/referee_task.c
View File

@@ -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;