From df356cf6751d65415a6d45885ae74834adfe176c Mon Sep 17 00:00:00 2001 From: NeoZng Date: Tue, 6 Dec 2022 22:58:42 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0robot=5Fcmd=E7=9A=84=E6=80=BB?= =?UTF-8?q?=E6=8E=A7=E5=88=B6=E5=92=8C=E6=80=A5=E5=81=9C=E5=A4=84=E7=90=86?= =?UTF-8?q?,=E6=B7=BB=E5=8A=A0=E9=81=A5=E6=8E=A7=E5=99=A8=E4=BD=8D?= =?UTF-8?q?=E5=9F=9F=E5=A4=84=E7=90=86,=E5=BE=85=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/cmd/robot_cmd.c | 28 ++++++++++++++++++++ application/robot_def.h | 7 +++++ modules/remote/remote_control.c | 16 +++++++++--- modules/remote/remote_control.h | 45 ++++++++++++++++++--------------- 4 files changed, 72 insertions(+), 24 deletions(-) diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index dc51218..552fb1b 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -36,6 +36,8 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息, static Subscriber_t *chassis_feed_sub; static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 +static Robot_Status_e robot_state; + void GimbalCMDInit() { remote_control_data = RemoteControlInit(&huart3); // 修改为对应串口,注意dbus协议串口需加反相器 @@ -81,6 +83,8 @@ static void CalcOffsetAngle() */ static void RemoteControlSetMode() { + + } /** @@ -91,6 +95,28 @@ static void MouseKeySetMode() { } +/** + * @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等 + * '300'待修改成合适的值,或改为开关控制 + * + */ +static void EmergencyHandler() +{ + if(remote_control_data[0].joy_stick.ch[4]<-300)//还需添加重要应用和模块离线的判断 + { + 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_STOP; + return; + } + // if(remote_control_data[0].joy_stick.ch[4]>300 && 各个模块正常) + // { + // //恢复运行 + // //... + // } +} + void GimbalCMDTask() { // 从其他应用获取回传数据 @@ -105,6 +131,8 @@ void GimbalCMDTask() RemoteControlSetMode(); else if (0) // 键盘控制 MouseKeySetMode(); + + EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 // 设置视觉发送数据,work_mode在前一部分设置 vision_send_data.bullet_speed = chassis_fetch_data.bullet_speed; diff --git a/application/robot_def.h b/application/robot_def.h index bbe34bf..7dc9619 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -52,6 +52,13 @@ * @brief 这些枚举类型和结构体会作为CMD控制数据和各应用的反馈数据的一部分 * */ +//机器人状态 +typedef enum +{ + ROBOT_STOP, + ROBOT_WORKING, +} Robot_Status_e; + // 应用状态 typedef enum diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 78da028..0f9c3a4 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -36,9 +36,10 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf) rc_ctrl[0].mouse.press_r = sbus_buf[13]; //!< Mouse Right Is Press ? // 按键值,每个键1bit,key_temp共16位;按键顺序在remote_control.h的宏定义中可见 + // 使用位域后不再需要这一中间操作 rc_ctrl[0].key_temp = sbus_buf[14] | (sbus_buf[15] << 8); //!< KeyBoard value - // @todo 似乎可以直接用位域操作进行,把key_temp通过强制类型转换变成key类型? 方案见remote_control.md + // @todo 似乎可以直接用位域操作进行,把key_temp通过强制类型转换变成key类型? 位域方案在下面,尚未测试 // 按键值解算,利用宏+循环减少代码长度 for (uint16_t i = 0x0001, j = 0; i < 0x8001; i *= 2, j++) // 依次查看每一个键 { @@ -53,6 +54,15 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf) rc_ctrl[0].key[KEY_PRESS_WITH_CTRL][j] = rc_ctrl[0].key_temp & i; } + // 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位 + *(uint16_t*)&rc_ctrl[0].key_test[KEY_PRESS]= (uint16_t)(sbus_buf[14] | (sbus_buf[15] << 8)); + *(uint16_t*)&rc_ctrl[0].key_test[KEY_STATE]=*(uint16_t*)&rc_ctrl[0].key_test[KEY_PRESS] & ~(*(uint16_t*)&(rc_ctrl[1].key_test[KEY_PRESS])); + if(rc_ctrl[0].key_test[KEY_PRESS].ctrl) + rc_ctrl[0].key_test[KEY_PRESS_WITH_CTRL]=rc_ctrl[0].key_test[KEY_PRESS]; + if(rc_ctrl[0].key_test[KEY_PRESS].shift) + rc_ctrl[0].key_test[Key_Shift]=rc_ctrl[0].key_test[KEY_PRESS]; + + // 减去偏置值 rc_ctrl[0].joy_stick.ch[0] -= RC_CH_VALUE_OFFSET; rc_ctrl[0].joy_stick.ch[1] -= RC_CH_VALUE_OFFSET; @@ -67,7 +77,7 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf) * 对sbus_to_rc的简单封装 * */ -static void RefereeRxCallback() +static void RemoteControlRxCallback() { sbus_to_rc(rc_usart_instance->recv_buff); } @@ -75,7 +85,7 @@ static void RefereeRxCallback() RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle) { USART_Init_Config_s conf; - conf.module_callback = RefereeRxCallback; + conf.module_callback = RemoteControlRxCallback; conf.usart_handle = rc_usart_handle; conf.recv_buff_size = REMOTE_CONTROL_FRAME_SIZE; rc_usart_instance = USARTRegister(&conf); diff --git a/modules/remote/remote_control.h b/modules/remote/remote_control.h index 556e99b..b0cd2a2 100644 --- a/modules/remote/remote_control.h +++ b/modules/remote/remote_control.h @@ -55,27 +55,6 @@ #define Key_B 15 /* ----------------------- Data Struct ------------------------------------- */ -typedef struct -{ - struct - { - int16_t ch[5]; // 右|0 ,右-1 ,左-2 ,左|3 ,拨轮4 - uint8_t s[2]; //[0]:left [1]:right - } joy_stick; - struct - { - int16_t x; - int16_t y; - int16_t z; - uint8_t press_l; - uint8_t press_r; - } mouse; - - uint16_t key_temp; - uint8_t key[4][16]; - -} RC_ctrl_t; - // 待测试的位域结构体,可以极大提升解析速度 typedef struct { @@ -97,6 +76,30 @@ typedef struct uint16_t b : 1; } Key_t; +typedef struct +{ + struct + { + int16_t ch[5]; // 右|0 ,右-1 ,左-2 ,左|3 ,拨轮4 + uint8_t s[2]; //[0]:left [1]:right + } joy_stick; + struct + { + int16_t x; + int16_t y; + int16_t z; + uint8_t press_l; + uint8_t press_r; + } mouse; + + uint16_t key_temp; + uint8_t key[4][16]; // 当前使用的键盘索引 + Key_t key_test[4]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍 + +} RC_ctrl_t; + + + /* ------------------------- Internal Data ----------------------------------- */ /**