mirror of
https://gitee.com/dlmu-cone/bf_original_balance_chassis
synced 2026-07-23 19:25:09 +08:00
小陀螺时移动速度置0
This commit is contained in:
@@ -20,6 +20,7 @@
|
||||
#include "speed_estimation.h"
|
||||
#include "fly_detection.h"
|
||||
#include "buzzer.h"
|
||||
|
||||
// 计时变量
|
||||
static uint32_t balance_dwt_cnt;
|
||||
static float del_t;
|
||||
@@ -29,6 +30,7 @@ static INS_t *Chassis_IMU_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 LKMotorInstance *l_driven, *r_driven, *driven[2];
|
||||
@@ -68,6 +70,7 @@ void BalanceInit()
|
||||
.send_data_len = sizeof(Chassis_Upload_Data_s),
|
||||
};
|
||||
cmd_can_comm = CANCommInit(&comm_conf);
|
||||
|
||||
// 关节电机
|
||||
Motor_Init_Config_s joint_conf = {
|
||||
// 写一个,剩下的修改方向和id即可
|
||||
@@ -256,7 +259,6 @@ static void ControlSwitch()
|
||||
// chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm);
|
||||
// }
|
||||
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm);
|
||||
|
||||
}
|
||||
|
||||
/* 腿缩回复位,只允许驱动轮电机移动 */
|
||||
@@ -353,7 +355,8 @@ static void WokingStateSet()
|
||||
{
|
||||
chassis.target_yaw = chassis_cmd_recv.offset_angle;
|
||||
}
|
||||
chassis.target_yaw = chassis.yaw + chassis_cmd_recv.offset_angle*DEGREE_2_RAD;
|
||||
chassis.target_yaw = chassis.yaw + chassis_cmd_recv.offset_angle * DEGREE_2_RAD;
|
||||
|
||||
// TODO 转向速度限幅
|
||||
|
||||
// TODO 最大dist误差限幅
|
||||
@@ -395,12 +398,12 @@ static void ParamAssemble()
|
||||
static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
|
||||
{
|
||||
if(chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG ||
|
||||
chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW)
|
||||
chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) // 底盘跟随
|
||||
{
|
||||
float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
|
||||
PIDCalculate(&steer_v_pid, chassis.wz, p_ref);
|
||||
}
|
||||
else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE)
|
||||
else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 小陀螺
|
||||
{
|
||||
PIDCalculate(&steer_v_pid, chassis.wz, 6);
|
||||
}
|
||||
@@ -441,6 +444,7 @@ static void WattLimitSet() /* 设定运动模态的输出 */
|
||||
void BalanceTask()
|
||||
{
|
||||
del_t = DWT_GetDeltaT(&balance_dwt_cnt);
|
||||
|
||||
BuzzerOn();
|
||||
// 切换遥控器控制or云台板控制
|
||||
ControlSwitch();
|
||||
|
||||
@@ -14,10 +14,7 @@
|
||||
// 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绘制相关
|
||||
@@ -39,11 +36,10 @@ 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通信实例
|
||||
|
||||
static chassis_direction_e chassis_direction; // 底盘方向
|
||||
|
||||
void RobotCMDInit()
|
||||
{
|
||||
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
|
||||
@@ -84,8 +80,9 @@ void RobotCMDInit()
|
||||
*/
|
||||
static void CalcOffsetAngle()
|
||||
{
|
||||
//@todo:相差一整圈时会出问题,待修复
|
||||
// @todo:相差一整圈时会出问题,待修复
|
||||
// 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
|
||||
uint16_t yaw_chassis_align_ecd;
|
||||
if(chassis_direction == CHASSIS_ALIGN)
|
||||
{
|
||||
yaw_chassis_align_ecd = 2716;
|
||||
@@ -94,9 +91,11 @@ static void CalcOffsetAngle()
|
||||
{
|
||||
yaw_chassis_align_ecd = 765;
|
||||
}
|
||||
static float angle;
|
||||
|
||||
static float angle, yaw_align_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)
|
||||
@@ -107,7 +106,7 @@ static void CalcOffsetAngle()
|
||||
chassis_cmd_send.offset_angle = angle - yaw_align_angle + 360.0f;
|
||||
}
|
||||
else
|
||||
{ // 小于180度
|
||||
{ // 小于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)
|
||||
@@ -123,16 +122,22 @@ static void CalcOffsetAngle()
|
||||
*/
|
||||
static void RemoteControlSet()
|
||||
{
|
||||
shoot_cmd_send.bullet_speed = 30;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
// // 云台参数,确定云台控制数据
|
||||
// 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 );
|
||||
// }
|
||||
// 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
|
||||
|
||||
// 云台参数,确定云台控制数据
|
||||
gimbal_cmd_send.yaw -= 0.001f * (float)rc_data[TEMP].rc.rocker_l_;
|
||||
gimbal_cmd_send.pitch -= 0.0006f * (float)rc_data[TEMP].rc.rocker_l1;
|
||||
|
||||
// 摇杆控制的软件限位
|
||||
gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
|
||||
|
||||
if (switch_is_down(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 );
|
||||
}
|
||||
|
||||
// 底盘参数
|
||||
if (abs(rc_data[TEMP].rc.rocker_r1) > 500)
|
||||
{
|
||||
chassis_direction = CHASSIS_ALIGN;
|
||||
@@ -142,24 +147,23 @@ static void RemoteControlSet()
|
||||
chassis_direction = CHASSIS_SIDLE;
|
||||
}
|
||||
|
||||
// 右拨杆拨下去,底盘旋转
|
||||
// 右侧开关状态为[下],底盘旋转
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_right))
|
||||
{
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
|
||||
chassis_cmd_send.vx = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
chassis_cmd_send.vx = 0.003f * (-(float)rc_data[TEMP].rc.rocker_r_ + (float)rc_data[TEMP].rc.rocker_r1);
|
||||
}
|
||||
|
||||
// 左侧开关状态为[中],腿长控制
|
||||
if (switch_is_mid(rc_data[TEMP].rc.switch_left))
|
||||
{
|
||||
chassis_cmd_send.delta_leglen = -0.000001f * (abs(rc_data[TEMP].rc.dial) > 100 ? (float)rc_data[TEMP].rc.dial : 0);
|
||||
}
|
||||
chassis_cmd_send.vx = 0.003f * ((float)rc_data[TEMP].rc.rocker_r_ + (float)rc_data[TEMP].rc.rocker_r1);
|
||||
|
||||
gimbal_cmd_send.yaw -= 0.001f * (float)rc_data[TEMP].rc.rocker_l_;
|
||||
gimbal_cmd_send.pitch -= 0.0006f * (float)rc_data[TEMP].rc.rocker_l1;
|
||||
// 摇杆控制的软件限位
|
||||
gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
|
||||
// gimbal_cmd_send.yaw = float_constrain(gimbal_cmd_send.yaw, -90.0f, 90+.0f);
|
||||
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
|
||||
|
||||
// 摩擦轮控制,拨轮向上打为负,向下为正
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_left))
|
||||
@@ -168,6 +172,7 @@ static void RemoteControlSet()
|
||||
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;
|
||||
@@ -175,15 +180,8 @@ static void RemoteControlSet()
|
||||
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.bullet_speed = 30;
|
||||
shoot_cmd_send.shoot_rate = 15;
|
||||
}
|
||||
|
||||
@@ -200,7 +198,15 @@ static void MouseKeySet()
|
||||
|
||||
gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
|
||||
|
||||
chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s - rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d);
|
||||
if(chassis_cmd_send.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW)
|
||||
{
|
||||
chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s
|
||||
- rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d);
|
||||
}
|
||||
else if (chassis_cmd_send.chassis_mode == CHASSIS_ROTATE)
|
||||
{
|
||||
chassis_cmd_send.vx = 0;
|
||||
}
|
||||
|
||||
chassis_cmd_send.delta_leglen = (float)(rc_data[TEMP].key[KEY_PRESS].e - rc_data[TEMP].key[KEY_PRESS].c) * 0.001f;
|
||||
|
||||
@@ -233,6 +239,16 @@ static void MouseKeySet()
|
||||
break;
|
||||
}
|
||||
|
||||
if(shoot_cmd_send.friction_mode == FRICTION_ON)
|
||||
{
|
||||
if (rc_data[TEMP].mouse.press_l)
|
||||
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
|
||||
else
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
}
|
||||
else
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
|
||||
if (rc_data[TEMP].mouse.press_r)
|
||||
{
|
||||
gimbal_cmd_send.yaw = (vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw);
|
||||
@@ -249,7 +265,6 @@ static void MouseKeySet()
|
||||
*/
|
||||
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 && switch_is_down(rc_data[TEMP].rc.switch_left)) || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
|
||||
@@ -259,6 +274,7 @@ static void EmergencyHandler()
|
||||
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;
|
||||
@@ -270,7 +286,6 @@ static void EmergencyHandler()
|
||||
shoot_cmd_send.shoot_mode = SHOOT_ON;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
chassis_cmd_send.direction = CHASSIS_ALIGN;
|
||||
}
|
||||
}
|
||||
else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
|
||||
@@ -291,7 +306,6 @@ static void EmergencyHandler()
|
||||
shoot_cmd_send.shoot_mode = SHOOT_OFF;
|
||||
shoot_cmd_send.friction_mode = FRICTION_OFF;
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
chassis_cmd_send.direction = CHASSIS_ALIGN;
|
||||
|
||||
gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步,避免恢复时疯转
|
||||
gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
|
||||
|
||||
@@ -36,7 +36,6 @@ void GimbalInit()
|
||||
.Derivative_LPF_RC=0,//0.01
|
||||
.Improve = PID_Trapezoid_Intergral |PID_ChangingIntegrationRate| PID_Integral_Limit |PID_Derivative_On_Measurement | PID_OutputFilter |PID_DerivativeFilter,
|
||||
.IntegralLimit = 4.0,
|
||||
|
||||
.MaxOut = 20,
|
||||
},
|
||||
.speed_PID = {
|
||||
@@ -61,7 +60,6 @@ void GimbalInit()
|
||||
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
.feedforward_flag = SPEED_FEEDFORWARD,
|
||||
|
||||
},
|
||||
.motor_type = GM6020};
|
||||
// PITCH
|
||||
|
||||
@@ -18,8 +18,8 @@
|
||||
|
||||
/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
|
||||
// #define ONE_BOARD // 单板控制整车
|
||||
#define CHASSIS_BOARD //底盘板
|
||||
// #define GIMBAL_BOARD //云台板
|
||||
// #define CHASSIS_BOARD //底盘板
|
||||
#define GIMBAL_BOARD //云台板
|
||||
|
||||
#define VISION_USE_VCP // 使用虚拟串口发送视觉数据
|
||||
// #define VISION_USE_UART // 使用串口发送视觉数据
|
||||
@@ -34,10 +34,10 @@
|
||||
#define PITCH_MIN_ANGLE (-30.0f) // 云台竖直方向最小角度 (注意反馈如果是陀螺仪,则填写陀螺仪的角度)
|
||||
|
||||
// 发射参数
|
||||
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
|
||||
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
|
||||
#define ONE_BULLET_DELTA_ANGLE 75 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
|
||||
#define REDUCTION_RATIO_LOADER 36.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
|
||||
#define NUM_PER_CIRCLE 12 // 拨盘一圈的装载量
|
||||
#define LOAD_RATIO 2.5f //中心供弹需要机械减速比
|
||||
#define LOAD_RATIO 2.5f // 中心供弹需要机械减速比
|
||||
|
||||
#define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
|
||||
#define GYRO2GIMBAL_DIR_PITCH 1 // 陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反
|
||||
|
||||
@@ -1,17 +1,15 @@
|
||||
#include "shoot.h"
|
||||
#include "robot_def.h"
|
||||
|
||||
#include "dji_motor.h"
|
||||
#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的发射控制信息
|
||||
@@ -21,6 +19,7 @@ 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()
|
||||
{
|
||||
// 左摩擦轮
|
||||
@@ -46,16 +45,13 @@ void ShootInit()
|
||||
.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,
|
||||
@@ -157,50 +153,50 @@ void ShootTask()
|
||||
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)
|
||||
; // 未知模式,停止运行,检查指针越界,内存溢出等问题
|
||||
}
|
||||
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;
|
||||
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 * 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);
|
||||
|
||||
Reference in New Issue
Block a user