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

云台跟随，连续发射

Browse Source
This commit is contained in:
chenfu
2024-05-20 22:22:24 +08:00
parent 4f80821435
commit 0c0d1201ea
12 changed files with 658 additions and 193 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.vscode/launch.json vendored
View File

@@ -43,7 +43,7 @@
"interface": "swd",
"svdFile": "STM32F407.svd",
"rtos": "FreeRTOS",
"preLaunchTask": "build task",//先运行Build任务,取消注释即可使用
//"preLaunchTask": "build task",//先运行Build任务,取消注释即可使用
"liveWatch": {
"enabled": true,
"samplesPerSecond": 4

5
.vscode/settings.json vendored
View File

@@ -5,7 +5,10 @@
"stdlib.h": "c",
"bsp_can.h": "c",
"math.h": "c",
"user_lib.h": "c"
"user_lib.h": "c",
"servo_motor.h": "c",
"main.h": "c",
"buzzer.h": "c"
},
"C_Cpp.errorSquiggles": "disabled"
}

73
application/chassis/balance.c
View File

@@ -19,18 +19,18 @@
#include "lqr_calc.h"
#include "speed_estimation.h"
#include "fly_detection.h"
#include "buzzer.h"
// 计时变量
static uint32_t balance_dwt_cnt;
static float del_t;
// 底盘拥有的实例模块
static INS_t *Chassis_IMU_data;
static RC_ctrl_t *rc_data; // 底盘单独调试用
static RC_ctrl_t *rc_data; // 底盘单独调试用
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘反馈数据
// 四个关节电机和两个驱动轮电机
static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
static LKMotorInstance *l_driven, *r_driven, *driven[2];
// 两个腿的参数,0为左腿,1为右腿
@@ -38,23 +38,35 @@ static LinkNPodParam l_side, r_side;
static ChassisParam chassis;
// 综合运动补偿的PID控制器
static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧, 不要积分(弹簧是无积分二阶系统), 增益不可过大否则抗外界冲击响应时太"硬"
static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平
static PIDInstance steer_p_pid, steer_v_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
static PIDInstance anti_crash_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上
static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧, 不要积分(弹簧是无积分二阶系统), 增益不可过大否则抗外界冲击响应时太"硬"
static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平
static PIDInstance steer_p_pid, steer_v_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
static PIDInstance anti_crash_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上
// 底盘状态
static Robot_Status_e chassis_status;
static referee_info_t* referee_data; // 用于获取裁判系统的数据
static referee_info_t *referee_data; // 用于获取裁判系统的数据
static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
static CANCommInstance *cmd_can_comm; // 底盘CAN通信实例
void BalanceInit()
{
rc_data = RemoteControlInit(&huart3);
Chassis_IMU_data = INS_Init();
referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
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),
};
cmd_can_comm = CANCommInit(&comm_conf);
// 关节电机
Motor_Init_Config_s joint_conf = {
// 写一个,剩下的修改方向和id即可
@@ -173,7 +185,7 @@ void BalanceInit()
// 状态初始化
l_side.target_len = r_side.target_len = 0.12;
chassis.vel_cov = 100; // 速度协方差初始化
chassis.vel_cov = 100; // 速度协方差初始化
chassis_status = ROBOT_READY;
DWT_GetDeltaT(&balance_dwt_cnt);
}
@@ -191,7 +203,7 @@ static uint8_t JointMotorIsLost()
{
for (uint8_t i = 0; i < JOINT_CNT; i++)
{
if(joint[i]->motor_daemon->temp_count == 0)
if (joint[i]->motor_daemon->temp_count == 0)
return 1;
}
@@ -201,9 +213,9 @@ static uint8_t JointMotorIsLost()
// 检查驱动轮电机是否离线
static uint8_t DrivenMotorIsLost()
{
for(uint8_t i = 0; i < DRIVEN_CNT; i++)
for (uint8_t i = 0; i < DRIVEN_CNT; i++)
{
if(driven[i]->daemon->temp_count == 0)
if (driven[i]->daemon->temp_count == 0)
return 1;
}
@@ -232,21 +244,22 @@ static void ControlSwitch()
}
else
{
chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后
chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后
chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
chassis_cmd_recv.delta_leglen = -0.0000005f * (float)rc_data[TEMP].rc.dial;
chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_;
}
}
else
chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停
{
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm);
}
}
/* 腿缩回复位,只允许驱动轮电机移动 */
static void ResetChassis()
{
EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
// 目标速度置0
chassis.target_v = 0;
@@ -278,7 +291,7 @@ static void ResetChassis()
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环
return; // 退出函数不再执行关节指令
return; // 退出函数不再执行关节指令
}
else
chassis_status = ROBOT_STOP;
@@ -291,7 +304,6 @@ static void ResetChassis()
}
}
// 工作状态设定
static void WokingStateSet()
{
@@ -334,8 +346,11 @@ static void WokingStateSet()
chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
// 角度输入
chassis.target_yaw = chassis_cmd_recv.offset_angle;
if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG)
{
chassis.target_yaw = chassis_cmd_recv.offset_angle;
}
chassis.target_yaw = chassis.yaw + chassis_cmd_recv.offset_angle*DEGREE_2_RAD;
// TODO 转向速度限幅
// TODO 最大dist误差限幅
@@ -343,7 +358,6 @@ static void WokingStateSet()
// TODO 最大速度误差限幅
}
/**
* @brief 将电机和imu的数据组装为LinkNPodParam结构体和chassisParam结构体
*
@@ -375,16 +389,11 @@ static void ParamAssemble()
r_side.w_ecd = -r_driven->measure.speed_rads;
}
static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
{
if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG)
{
// 双环控制
float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
PIDCalculate(&steer_v_pid, chassis.wz, p_ref);
}
float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
PIDCalculate(&steer_v_pid, chassis.wz, p_ref);
l_side.T_wheel -= steer_v_pid.Output;
r_side.T_wheel += steer_v_pid.Output;
@@ -396,7 +405,6 @@ static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
r_side.T_hip -= anti_crash_pid.Output - swerving_speed_ff;
}
static void LegControl() /* 腿长控制和Roll补偿 */
{
PIDCalculate(&roll_compensate_pid, chassis.roll, 0);
@@ -423,7 +431,7 @@ static void WattLimitSet() /* 设定运动模态的输出 */
void BalanceTask()
{
del_t = DWT_GetDeltaT(&balance_dwt_cnt);
BuzzerOn();
// 切换遥控器控制or云台板控制
ControlSwitch();
// 设置目标参数和工作模式
@@ -457,4 +465,5 @@ void BalanceTask()
// 运动模态,电机输出映射和限幅
WattLimitSet();
CANCommSend(cmd_can_comm, (void *)&chassis_feedback_data);
}

222
application/cmd/robot_cmd.c
View File

@@ -9,18 +9,238 @@
#include "general_def.h"
#include "dji_motor.h"
#include "bmi088.h"
#include "user_lib.h"
#include "can_comm.h"
// bsp
#include "bsp_dwt.h"
#include "bsp_log.h"
// 私有宏,自动将编码器转换成角度值
#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360
float yaw_align_angle = 0.0f, yaw_chassis_align_ecd = 2716;
static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者
static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者
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; // 机器人整体工作状态
static Work_Mode_e vision_work_mode; // 视觉工作模式
static CANCommInstance *cmd_can_comm; // 底盘CAN通信实例
void RobotCMDInit()
{
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));
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);
gimbal_cmd_send.pitch = 0;
gimbal_cmd_send.yaw = 0;
gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
robot_state = ROBOT_STOP; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
}
/**
* @brief 根据gimbal app传回的当前电机角度计算和零位的误差
* 单圈绝对角度的范围是0~360,说明文档中有图示
*
*/
static void CalcOffsetAngle()
{
// 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
static float angle;
yaw_align_angle = yaw_chassis_align_ecd * ECD_ANGLE_COEF_DJI; // 从底盘获取的yaw电机对齐角度
angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
if (yaw_chassis_align_ecd > 4096) // 如果大于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;
}
else
{ // 小于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;
}
}
void RobotCMDTask()
/**
* @brief 控制输入为遥控器(调试时)的模式和控制量设置
*
*/
static void RemoteControlSet()
{
shoot_cmd_send.bullet_speed = 30;
// // 云台参数,确定云台控制数据
// if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式
// {
// gimbal_cmd_send.yaw = ( vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw );
// gimbal_cmd_send.pitch =( vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch );
// }
// 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
chassis_cmd_send.vx = 0.0f;
if (abs(rc_data[TEMP].rc.rocker_r1) > 300)
{
chassis_cmd_send.vx = 0.003f * (float)rc_data[TEMP].rc.rocker_r1; // _水平方向
yaw_chassis_align_ecd = 2716;
}
else if (abs(rc_data[TEMP].rc.rocker_r_) > 300)
{
chassis_cmd_send.vx = 0.003f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向
yaw_chassis_align_ecd = 765;
}
gimbal_cmd_send.yaw -= 0.001f * (float)rc_data[TEMP].rc.rocker_l_;
gimbal_cmd_send.pitch -= 0.0004f * (float)rc_data[TEMP].rc.rocker_l1;
// 摇杆控制的软件限位
gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, -35.0f, 30.0f);
// gimbal_cmd_send.yaw = float_constrain(gimbal_cmd_send.yaw, -180.0f, 180.0f);
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
// 摩擦轮控制,拨轮向上打为负,向下为正
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 < -400)
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
else
shoot_cmd_send.load_mode = LOAD_STOP;
// // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
// if( rc_data[TEMP].rc.switch_left == 3)
// {
// shoot_cmd_send.friction_mode = FRICTION_ON;
// if(vision_recv_data->fire_mode ==AUTO_AIM)
// shoot_cmd_send.load_mode = LOAD_1_BULLET;
// else
// shoot_cmd_send.load_mode = LOAD_STOP;
// }
shoot_cmd_send.shoot_rate = 15;
}
/**
* @brief 输入为键鼠时模式和控制量设置
*
*/
static void MouseKeySet()
{
}
/**
* @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等
* 停止的阈值'300'待修改成合适的值,或改为开关控制.
*
* @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现
*
*/
static void EmergencyHandler()
{
if (switch_is_down(rc_data[TEMP].rc.switch_left) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
{
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;
gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步，避免恢复时疯转
gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
}
// 遥控器右侧开关为[上],恢复正常运行
if (switch_is_up(rc_data[TEMP].rc.switch_right))
{
robot_state = ROBOT_READY;
shoot_cmd_send.shoot_mode = SHOOT_ON;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
}
}
else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
{
switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_C] % 2) // ctrl+c 进入急停
{
case 0:
robot_state = ROBOT_READY;
shoot_cmd_send.shoot_mode = SHOOT_ON;
break;
default:
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;
gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步，避免恢复时疯转
gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
break;
}
}
}
/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
void RobotCMDTask()
{
chassis_fetch_data = *(Chassis_Upload_Data_s *)CANCommGet(cmd_can_comm);
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) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
RemoteControlSet();
if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
MouseKeySet();
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send);
PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
}

153
application/gimbal/gimbal.c
View File

@@ -4,16 +4,169 @@
#include "ins_task.h"
#include "message_center.h"
#include "general_def.h"
#include "buzzer.h"
#include "bmi088.h"
static INS_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的控制信息
void GimbalInit()
{
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
// YAW
Motor_Init_Config_s yaw_config = {
.can_init_config = {
.can_handle = &hcan1,
.tx_id = 4,
},
.controller_param_init_config = {
.angle_PID = {
.Kp = 0.5, //0.55
.Ki = 0.0,//0.05
.Kd = 0.0,//0.02
.CoefA =0.0,//0.5
.CoefB = 0.0,//0.6
.Output_LPF_RC = 0,
.DeadBand = 0.0,//0.02
.Derivative_LPF_RC=0.0,//0.008
.Improve = PID_Trapezoid_Intergral |PID_ChangingIntegrationRate| PID_Integral_Limit |PID_Derivative_On_Measurement | PID_OutputFilter |PID_DerivativeFilter,
.IntegralLimit = 0.0,
.MaxOut = 400,
},
.speed_PID = {
.Kp = 15000,//22000
.Ki = 0,//
.Kd =0,
// .CoefA = 0.8,
// .CoefB = 0.1,
.Output_LPF_RC = 0.0,//0.002
.Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement | PID_OutputFilter,
.IntegralLimit = 0,
.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,
.feedforward_flag = SPEED_FEEDFORWARD,
},
.motor_type = GM6020};
// PITCH
Motor_Init_Config_s pitch_config = {
.can_init_config = {
.can_handle = &hcan2,
.tx_id = 3,
},
.controller_param_init_config = {
.angle_PID = {
.Kp =0.4,//0.4
.Ki = 0.0,//0.15
.Kd = 0.0,//0.006
.CoefA = 0.0,//0.5
.CoefB = 0.0,//0.6
.DeadBand = 0.0,//0.005
.Output_LPF_RC = 0.0,//0.01
.Improve = PID_Trapezoid_Intergral |PID_ChangingIntegrationRate| PID_Integral_Limit| PID_OutputFilter|PID_DerivativeFilter,
.IntegralLimit =1,//1
.MaxOut = 600,//600
},
.speed_PID = {
.Kp=10000,//14000
.Ki =0,//0
.Kd =0.0,//0.0005
.CoefA =0,//1500
.CoefB =0,//2000
.Output_LPF_RC = 0.0,//0.005
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_OutputFilter,
.IntegralLimit =3000,//3000
.MaxOut = 20000,//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 = ANGLE_LOOP | SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
.feedback_reverse_flag = FEEDBACK_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()
{
BuzzerOn();
// 获取云台控制数据
// 后续增加未收到数据的处理
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: // 后续只保留此模式
// DJIMotorSetFeedfoward(yaw_motor,SPEED_FEEDFORWARD);
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);
}

10
application/robot_def.h
View File

@@ -19,14 +19,14 @@
/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
// #define ONE_BOARD // 单板控制整车
#define CHASSIS_BOARD //底盘板
// #define GIMBAL_BOARD //云台板
//#define GIMBAL_BOARD //云台板
#define VISION_USE_VCP // 使用虚拟串口发送视觉数据
// #define VISION_USE_UART // 使用串口发送视觉数据
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
// 云台参数
#define YAW_CHASSIS_ALIGN_ECD 2711 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_CHASSIS_ALIGN_ECD 2716 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
@@ -35,8 +35,8 @@
// 发射参数
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量
#define NUM_PER_CIRCLE 12 // 拨盘一圈的装载量
#define LOAD_RATIO 2.5f //中心供弹需要机械减速比
#define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
#define GYRO2GIMBAL_DIR_PITCH 1 // 陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反
@@ -203,7 +203,7 @@ typedef struct
typedef struct
{
attitude_t gimbal_imu_data;
INS_t gimbal_imu_data;
uint16_t yaw_motor_single_round_angle;
} Gimbal_Upload_Data_s;

2
application/robot_task.h
View File

@@ -93,14 +93,12 @@ __attribute__((noreturn)) void StartDAEMONTASK(void const *argument)
{
static float daemon_dt;
static float daemon_start;
BuzzerInit();
LOGINFO("[freeRTOS] Daemon Task Start");
for (;;)
{
// 100Hz
daemon_start = DWT_GetTimeline_ms();
DaemonTask();
BuzzerTask();
daemon_dt = DWT_GetTimeline_ms() - daemon_start;
if (daemon_dt > 10)
LOGERROR("[freeRTOS] Daemon Task is being DELAY! dt = [%f]", &daemon_dt);

188
application/shoot/shoot.c
View File

@@ -5,15 +5,203 @@
#include "message_center.h"
#include "bsp_dwt.h"
#include "general_def.h"
#include "servo_motor.h"
/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
static DJIMotorInstance *friction_l; // 左摩擦轮
static DJIMotorInstance *friction_r; // 右摩擦轮
static DJIMotorInstance *loader; // 拨盘电机
static ServoInstance *lid_L; //需要增加弹舱盖
static ServoInstance *lid_R; //需要增加弹舱盖
static Publisher_t *shoot_pub;
static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自gimbal_cmd的发射控制信息
static Subscriber_t *shoot_sub;
static Shoot_Upload_Data_s shoot_feedback_data; // 来自gimbal_cmd的发射控制信息
// dwt定时,计算冷却用
static float hibernate_time = 0, dead_time = 0;
static float load_angle_set = 0;
void ShootInit()
{
// 左摩擦轮
Motor_Init_Config_s friction_config = {
.can_init_config = {
.can_handle = &hcan2,
},
.controller_param_init_config = {
.speed_PID = {
.Kp = 1,//20
.Ki = 0,//1
.Kd = 0,
.Derivative_LPF_RC = 0.02,
.Improve = PID_Integral_Limit| PID_Trapezoid_Intergral | PID_DerivativeFilter,
.IntegralLimit = 10000,
.MaxOut = 15000,
},
.current_PID = {
.Kp = 1.5,//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 = &hcan1,
.tx_id = 3,
},
.controller_param_init_config = {
.angle_PID = {
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
.Kp = 10,//10
.Ki = 0,
.Kd = 0,
.MaxOut = 20000,
},
.speed_PID = {
.Kp = 0.5,//0
.Ki = 0.1,//0
.Kd = 0.002,
.Improve = PID_Integral_Limit,
.IntegralLimit = 200,
.MaxOut = 10000,
},
.current_PID = {
.Kp = 1.5,//0
.Ki = 0,//0
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 5000,
.MaxOut = 10000,
},
},
.controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
.close_loop_type = 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);
}
if (shoot_cmd_recv.friction_mode == FRICTION_ON)
{
// 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
switch (shoot_cmd_recv.bullet_speed)
{
case SMALL_AMU_15:
DJIMotorSetRef(friction_l, 26800);
DJIMotorSetRef(friction_r, 26800);
break;
case SMALL_AMU_18:
DJIMotorSetRef(friction_l, 30000);
DJIMotorSetRef(friction_r, 30000);
break;
case SMALL_AMU_30:
DJIMotorSetRef(friction_l, 46500);
DJIMotorSetRef(friction_r, 46500);
break;
default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
DJIMotorSetRef(friction_l, 24500);
DJIMotorSetRef(friction_r, 24500);
break;
}
}
else // 关闭摩擦轮
{
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
}
// 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
// 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
if (hibernate_time + dead_time > DWT_GetTimeline_ms())
return;
switch (shoot_cmd_recv.load_mode)
{
case LOAD_STOP:
DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
DJIMotorSetRef(loader, 0);
break;
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
load_angle_set = loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE * 36;
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
DJIMotorSetRef(loader, load_angle_set); // 控制量增加一发弹丸的角度
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 500; // 完成1发弹丸发射的时间
break;
// 三连发,如果不需要后续可能删除
case LOAD_3_BULLET:
load_angle_set = loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE * 36 *3 ;
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
DJIMotorSetRef(loader, load_angle_set); // 增加3发
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 1800; // 完成3发弹丸发射的时间
break;
// 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
case LOAD_BURSTFIRE:
DJIMotorOuterLoop(loader, SPEED_LOOP);
DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER * LOAD_RATIO/ NUM_PER_CIRCLE);
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
break;
// 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
// 也有可能需要从switch-case中独立出来
case LOAD_REVERSE:
DJIMotorOuterLoop(loader, SPEED_LOOP);
// ...
break;
default:
while (1)
; // 未知模式,停止运行,检查指针越界,内存溢出等问题
}
// 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
}

3
bsp/bsp_init.h
View File

@@ -5,7 +5,7 @@
#include "bsp_log.h"
#include "bsp_dwt.h"
#include "bsp_usb.h"
#include "buzzer.h"
/**
* @brief bsp层初始化统一入口,这里仅初始化必须的bsp组件,其他组件的初始化在各自的模块中进行
* 需在实时系统启动前调用,目前由RobotoInit()调用
@@ -17,6 +17,7 @@ void BSPInit()
{
DWT_Init(168);
BSPLogInit();
BuzzerInit();
}

102
modules/alarm/buzzer.c
View File

@@ -1,93 +1,31 @@
#include "bsp_pwm.h"
#include "buzzer.h"
#include "bsp_dwt.h"
#include "string.h"
#include "main.h"
static PWMInstance *buzzer;
// static uint8_t idx;
static BuzzzerInstance *buzzer_list[BUZZER_DEVICE_CNT] = {0};
extern TIM_HandleTypeDef htim4;
static uint8_t tmp_warning_level = 0;
/**
* @brief 蜂鸣器初始化
*
*/
void BuzzerInit()
{
PWM_Init_Config_s buzzer_config = {
.htim = &htim4,
.channel = TIM_CHANNEL_3,
.dutyratio = 0,
.period = 0.001,
};
buzzer = PWMRegister(&buzzer_config);
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, 0);
}
BuzzzerInstance *BuzzerRegister(Buzzer_config_s *config)
void BuzzerOn( )
{
if (config->alarm_level > BUZZER_DEVICE_CNT) // 超过最大实例数,考虑增加或查看是否有内存泄漏
while (1)
;
BuzzzerInstance *buzzer_temp = (BuzzzerInstance *)malloc(sizeof(BuzzzerInstance));
memset(buzzer_temp, 0, sizeof(BuzzzerInstance));
buzzer_temp->alarm_level = config->alarm_level;
buzzer_temp->loudness = config->loudness;
buzzer_temp->octave = config->octave;
buzzer_temp->alarm_state = ALARM_OFF;
buzzer_list[config->alarm_level] = buzzer_temp;
return buzzer_temp;
}
void AlarmSetStatus(BuzzzerInstance *buzzer, AlarmState_e state)
{
buzzer->alarm_state = state;
}
void BuzzerTask()
{
BuzzzerInstance *buzz;
for (size_t i = 0; i < BUZZER_DEVICE_CNT; ++i)
static int16_t temp = 4000 ;
if(temp < 1000)
{
buzz = buzzer_list[i];
if (buzz->alarm_level > ALARM_LEVEL_LOW)
{
continue;
}
if (buzz->alarm_state == ALARM_OFF)
{
PWMSetDutyRatio(buzzer, 0);
}
else
{
PWMSetDutyRatio(buzzer, buzz->loudness);
switch (buzz->octave)
{
case OCTAVE_1:
PWMSetPeriod(buzzer, (float)1 / DoFreq);
break;
case OCTAVE_2:
PWMSetPeriod(buzzer, (float)1 / ReFreq);
break;
case OCTAVE_3:
PWMSetPeriod(buzzer, (float)1 / MiFreq);
break;
case OCTAVE_4:
PWMSetPeriod(buzzer, (float)1 / FaFreq);
break;
case OCTAVE_5:
PWMSetPeriod(buzzer, (float)1 / SoFreq);
break;
case OCTAVE_6:
PWMSetPeriod(buzzer, (float)1 / LaFreq);
break;
case OCTAVE_7:
PWMSetPeriod(buzzer, (float)1 / SiFreq);
break;
default:
break;
}
break;
}
BuzzerOff();
return;
}
__HAL_TIM_PRESCALER(&htim4,(int)(temp/1000));
__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, 10000);
temp -= 5 ;
}
void BuzzerOff(void)
{
__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, 0);
tmp_warning_level = 0;
}

64
modules/alarm/buzzer.h
View File

@@ -1,60 +1,10 @@
#ifndef BUZZER_H
#define BUZZER_H
#include "bsp_pwm.h"
#define BUZZER_DEVICE_CNT 5
#define DoFreq 523
#define ReFreq 587
#define MiFreq 659
#define FaFreq 698
#define SoFreq 784
#define LaFreq 880
#define SiFreq 988
typedef enum
{
OCTAVE_1 = 0,
OCTAVE_2,
OCTAVE_3,
OCTAVE_4,
OCTAVE_5,
OCTAVE_6,
OCTAVE_7,
OCTAVE_8,
}octave_e;
typedef enum
{
ALARM_LEVEL_HIGH = 0,
ALARM_LEVEL_ABOVE_MEDIUM,
ALARM_LEVEL_MEDIUM,
ALARM_LEVEL_BELOW_MEDIUM,
ALARM_LEVEL_LOW,
}AlarmLevel_e;
typedef enum
{
ALARM_OFF = 0,
ALARM_ON,
}AlarmState_e;
typedef struct
{
AlarmLevel_e alarm_level;
octave_e octave;
float loudness;
}Buzzer_config_s;
typedef struct
{
float loudness;
octave_e octave;
AlarmLevel_e alarm_level;
AlarmState_e alarm_state;
}BuzzzerInstance;
#ifndef BSP_BUZZER_H
#define BSP_BUZZER_H
#include <stdint.h>
void BuzzerInit();
void BuzzerTask();
BuzzzerInstance *BuzzerRegister(Buzzer_config_s *config);
void AlarmSetStatus(BuzzzerInstance *buzzer, AlarmState_e state);
#endif // !BUZZER_H
extern void BuzzerOn();
extern void BuzzerOff(void);
#endif

13
modules/motor/motor_task.c
View File

@@ -4,16 +4,21 @@
#include "dji_motor.h"
#include "step_motor.h"
#include "servo_motor.h"
#include "dji_motor.h"
#include "robot_def.h"
void MotorControlTask()
{
// static uint8_t cnt = 0; 设定不同电机的任务频率
// if(cnt%5==0) //200hz
// if(cnt%10==0) //100hz
// DJIMotorControl();
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
#ifdef GIMBAL_BOARD
DJIMotorControl();
#endif // DEBUG
#ifdef CHASSIS_BOARD
LKMotorControl();
#endif // DEBUG
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
//LKMotorControl();
// legacy support
// 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞