遥控器
This commit is contained in:
zyx
parent
7070fb4a02
commit
1ea28f0dcb
|
|
@ -16,7 +16,7 @@
|
|||
#include "bsp_log.h"
|
||||
|
||||
// 私有宏,自动将编码器转换成角度值
|
||||
#define YAW_ALIGN_ANGLE 150.0F //(YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
|
||||
#define YAW_ALIGN_ANGLE 56.0F //(YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
|
||||
#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360
|
||||
|
||||
#define PITCH_SCAN -10.0f //扫描阶段PITCH固定角度
|
||||
|
|
@ -62,14 +62,14 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
|
|||
|
||||
static Robot_Status_e robot_state; // 机器人整体工作状态
|
||||
|
||||
static referee_info_t* referee_data; // 用于获取裁判系统的数据
|
||||
static referee_info_t *referee_data; // 用于获取裁判系统的数据
|
||||
static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
|
||||
|
||||
void RobotCMDInit() {
|
||||
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
|
||||
vision_recv_data = VisionInit(&huart1); // 视觉通信串口
|
||||
|
||||
referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
|
||||
referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
|
||||
|
||||
gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
|
||||
gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
|
||||
|
|
@ -176,7 +176,7 @@ static void RemoteControlSet() {
|
|||
|
||||
|
||||
}
|
||||
//左侧开关状态为[下],视觉模式
|
||||
//左侧开关状态为[下],视觉模式
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_left)) {
|
||||
gimbal_cmd_send.control_mode = TEST_CONTROL;
|
||||
trajectory_cal.v0 = 28; //弹速30
|
||||
|
|
@ -238,14 +238,14 @@ static void RemoteControlSet() {
|
|||
//gimbal_cmd_send.friction_mode = FRICTION_ON;
|
||||
else
|
||||
shoot_cmd_send.friction_mode = FRICTION_OFF;
|
||||
//gimbal_cmd_send.friction_mode = FRICTION_OFF;
|
||||
//gimbal_cmd_send.friction_mode = FRICTION_OFF;
|
||||
// 拨弹控制,遥控器固定为一种拨弹模式,可自行选择
|
||||
if (rc_data[TEMP].rc.dial < -500)
|
||||
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
|
||||
//gimbal_cmd_send.load_mode = LOAD_BURSTFIRE;
|
||||
else
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
//gimbal_cmd_send.load_mode = LOAD_STOP;
|
||||
//gimbal_cmd_send.load_mode = LOAD_STOP;
|
||||
|
||||
gimbal_cmd_send.shoot_mode = shoot_cmd_send.shoot_mode;//遥控发射指令同步发送给右边
|
||||
gimbal_cmd_send.load_mode = shoot_cmd_send.load_mode;//遥控发射指令同步发送给右边
|
||||
|
|
@ -322,7 +322,7 @@ static void AutoControlSet() {
|
|||
gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI;
|
||||
|
||||
float yaw_err = fabsf(gimbal_cmd_send.yaw - gimbal_fetch_data.gimbal_imu_data.Yaw);
|
||||
if (yaw_err <= 5) //3度
|
||||
if (yaw_err <= 5) //3度
|
||||
{
|
||||
aim_select.suggest_fire = 1;
|
||||
shoot_cmd_send.shoot_mode = SHOOT_ON;
|
||||
|
|
@ -371,6 +371,8 @@ static void EmergencyHandler() {
|
|||
if (switch_is_up(rc_data[TEMP].rc.switch_right)) {
|
||||
robot_state = ROBOT_READY;
|
||||
shoot_cmd_send.shoot_mode = SHOOT_ON;
|
||||
gimbal_cmd_send.yaw = gimbal_fetch_data.mini_yaw_encode_angle - 44;
|
||||
gimbal_cmd_send.big_yaw = gimbal_fetch_data.big_yaw_motor_angle;
|
||||
LOGINFO("[CMD] reinstate, robot ready");
|
||||
}
|
||||
}
|
||||
|
|
@ -391,35 +393,38 @@ void RobotCMDTask() {
|
|||
|
||||
// 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
|
||||
CalcOffsetAngle();
|
||||
|
||||
// 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
|
||||
|
||||
static control_mode_e last_gimbal_mode = 0;
|
||||
if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],自动模式
|
||||
{
|
||||
gimbal_cmd_send.control_mode = AUTO_CONTROL;
|
||||
AutoControlSet();
|
||||
}
|
||||
|
||||
else {
|
||||
if (last_gimbal_mode == AUTO_CONTROL) {
|
||||
gimbal_cmd_send.yaw = 0;
|
||||
gimbal_cmd_send.pitch = 0;
|
||||
|
||||
if (robot_state != ROBOT_STOP && rc_data[TEMP].rc.dial < 300) {
|
||||
if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],自动模式
|
||||
{
|
||||
gimbal_cmd_send.control_mode = AUTO_CONTROL;
|
||||
AutoControlSet();
|
||||
} else {
|
||||
if (last_gimbal_mode == AUTO_CONTROL) {
|
||||
gimbal_cmd_send.yaw = 0;
|
||||
gimbal_cmd_send.pitch = 0;
|
||||
}
|
||||
gimbal_cmd_send.control_mode = RC_CONTROL;
|
||||
RemoteControlSet();
|
||||
}
|
||||
gimbal_cmd_send.control_mode = RC_CONTROL;
|
||||
RemoteControlSet();
|
||||
}
|
||||
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
|
||||
|
||||
gimbal_cmd_send.enemy_color = !referee_data->referee_id.Robot_Color;
|
||||
gimbal_cmd_send.rest_heat = referee_data->PowerHeatData.shooter_17mm_2_barrel_heat;
|
||||
|
||||
|
||||
|
||||
last_gimbal_mode = gimbal_cmd_send.control_mode;
|
||||
|
||||
//热量控制
|
||||
if(referee_data->PowerHeatData.shooter_17mm_1_barrel_heat>350)
|
||||
if (referee_data->PowerHeatData.shooter_17mm_1_barrel_heat > 350)
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
|
||||
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
|
||||
|
||||
|
||||
// chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
|
||||
// // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
|
||||
|
|
@ -432,11 +437,10 @@ void RobotCMDTask() {
|
|||
|
||||
//sentry_state = sentry_behave();
|
||||
|
||||
if(referee_data->GameState.game_progress == 4)
|
||||
{
|
||||
if(referee_data->GameState.stage_remain_time >= (300-60))
|
||||
if (referee_data->GameState.game_progress == 4) {
|
||||
if (referee_data->GameState.stage_remain_time >= (300 - 60))
|
||||
sentry_state = 1;//前一分钟
|
||||
else if(referee_data->GameState.stage_remain_time <= 60)
|
||||
else if (referee_data->GameState.stage_remain_time <= 60)
|
||||
sentry_state = 3;//后一分钟
|
||||
else
|
||||
sentry_state = 2;//中间阶段
|
||||
|
|
@ -444,7 +448,7 @@ void RobotCMDTask() {
|
|||
|
||||
gimbal_cmd_send.game_state = sentry_state;
|
||||
|
||||
VisionSetFlag(!referee_data->referee_id.Robot_Color,sentry_state,referee_data->GameRobotState.current_HP);
|
||||
VisionSetFlag(!referee_data->referee_id.Robot_Color, sentry_state, referee_data->GameRobotState.current_HP);
|
||||
// 推送消息,双板通信,视觉通信等
|
||||
// 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
|
||||
#ifdef ONE_BOARD
|
||||
|
|
@ -460,45 +464,40 @@ void RobotCMDTask() {
|
|||
VisionSend(&vision_send_data);
|
||||
}
|
||||
|
||||
uint8_t sentry_behave()
|
||||
{
|
||||
uint8_t sentry_behave() {
|
||||
static uint8_t behave_flag = 0;
|
||||
static uint8_t last_behave_flag = 0;
|
||||
static uint8_t recover_cnt = 0;
|
||||
// 比赛阶段 0准备阶段 1前1分钟 2中间阶段 3后一分钟 4比赛结束 9补给区回血 10保留（无响应）
|
||||
if(referee_data->GameState.game_progress < 4)
|
||||
if (referee_data->GameState.game_progress < 4)
|
||||
behave_flag = 0;
|
||||
else if(referee_data->GameState.game_progress == 4)
|
||||
{
|
||||
if(referee_data->GameState.stage_remain_time >= (300-60))
|
||||
else if (referee_data->GameState.game_progress == 4) {
|
||||
if (referee_data->GameState.stage_remain_time >= (300 - 60))
|
||||
behave_flag = 1;//前一分钟
|
||||
else if(referee_data->GameState.stage_remain_time <= 60)
|
||||
else if (referee_data->GameState.stage_remain_time <= 60)
|
||||
behave_flag = 3;//后一分钟
|
||||
else
|
||||
behave_flag = 2;//中间阶段
|
||||
}
|
||||
else if (referee_data->GameState.game_progress == 5)
|
||||
} else if (referee_data->GameState.game_progress == 5)
|
||||
behave_flag = 4;
|
||||
|
||||
if(referee_data->GameRobotState.current_HP < 200 && referee_data->GameState.stage_remain_time >= 60 && recover_cnt <= 1)
|
||||
{
|
||||
if (referee_data->GameRobotState.current_HP < 200 && referee_data->GameState.stage_remain_time >= 60 &&
|
||||
recover_cnt <= 1) {
|
||||
behave_flag = 9;
|
||||
}
|
||||
|
||||
|
||||
|
||||
static uint16_t last_HP = 600;
|
||||
static uint16_t recover_start_T = 0;
|
||||
|
||||
if(behave_flag == 9 ) //回去回血了
|
||||
if (behave_flag == 9) //回去回血了
|
||||
{
|
||||
if(last_behave_flag != 9)
|
||||
{
|
||||
if (last_behave_flag != 9) {
|
||||
recover_start_T = referee_data->GameState.stage_remain_time;
|
||||
recover_cnt ++ ;
|
||||
recover_cnt++;
|
||||
}
|
||||
|
||||
if((referee_data->GameState.stage_remain_time - recover_start_T)>10)
|
||||
if ((referee_data->GameState.stage_remain_time - recover_start_T) > 10)
|
||||
behave_flag = 10;//随便给一个值
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -278,7 +278,7 @@ void GimbalTask()
|
|||
//loop_float_constrain(big_yaw_relative_angle,0,2*PI) * RAD_2_DEGREE;
|
||||
gimbal_feedback_data.mini_yaw_encode_angle = yaw_motor->measure.total_angle;
|
||||
gimbal_feedback_data.big_yaw_online = DMMotorIsOnline(big_yaw_motor);
|
||||
|
||||
gimbal_feedback_data.big_yaw_motor_angle = big_yaw_motor->measure.total_angle;
|
||||
// 推送消息
|
||||
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -224,6 +224,7 @@ typedef struct
|
|||
attitude_t gimbal_imu_data;
|
||||
float yaw_motor_single_round_angle;
|
||||
float mini_yaw_encode_angle;
|
||||
float big_yaw_motor_angle;
|
||||
uint8_t big_yaw_online;
|
||||
} Gimbal_Upload_Data_s;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue