From 48370d441152b13adfe73b1e29f82c70a831c735 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Tue, 3 Jan 2023 22:53:49 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86can=E7=9A=84?= =?UTF-8?q?=E5=8F=91=E9=80=81=E8=B6=85=E6=97=B6=E6=8E=A7=E5=88=B6,?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E6=89=80=E6=9C=89application?= =?UTF-8?q?=E5=B1=82=E7=9A=84=E6=96=87=E6=A1=A3=E5=92=8C=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/APP层应用编写指引.md | 45 ++++++++++++++++++++++++++++- application/chassis/chassis.md | 19 ++++++++++++ application/cmd/robot_cmd.md | 46 ++++++++++++++++++++++-------- application/gimbal/gimbal.md | 13 +++++++++ application/shoot/shoot.md | 15 ++++++++++ bsp/can/bsp_can.c | 8 ++++-- bsp/can/bsp_can.h | 5 ++-- bsp/can/bsp_can.md | 13 ++++++--- modules/can_comm/can_comm.c | 2 +- modules/motor/DJImotor/dji_motor.c | 2 +- modules/motor/HTmotor/HT04.c | 4 +-- modules/motor/LKmotor/LK9025.c | 2 +- modules/motor/motor_def.h | 7 ++--- modules/remote/remote.md | 6 ++-- modules/remote/remote_control.c | 11 ++++--- modules/super_cap/super_cap.c | 2 +- 16 files changed, 160 insertions(+), 40 deletions(-) diff --git a/application/APP层应用编写指引.md b/application/APP层应用编写指引.md index 3c0bcbf..9c3f0fc 100644 --- a/application/APP层应用编写指引.md +++ b/application/APP层应用编写指引.md @@ -2,6 +2,49 @@

neozng1@hnu.edu.cn

+## 通信机制 + +**应用之间不应该有任何包含关系，它们必须是平行工作的。**而这通过pub-sub的机制实现。module层提供了`message_center`模块，支持发布订阅者的消息订阅机制。以传统的框架为例，负责整车控制的应用和其他应用（或任务）是从属的树状结构，或不同的任务和应用之间通过全局变量传递消息（**请不要使用全局变量！**），而此框架下的不同应用是并行的关系。 + +如果一个应用希望获取另一个应用的数据，那么他应该**订阅**由此此应用发布的话题（事件）。一个应用要把自己希望共享的数据，注册到消息中心，即**发布**。为了区别不同的消息来源（你希望订阅谁的消息？哪一个消息？），可以通过**话题名**进行订阅。也就是说，消息中心作为第三方，管理所有的消息发布者和订阅者，它像报刊亭一样对消息进行中转，使得不同的应用之间不需要包含彼此，更不用全局变量也能共享消息。 + +> 更多关于发布-订阅的实现，请参考`modules/message_center`下的文档。 + + + ## robot_cmd -机器人命令模块是对整个机器人的抽象，对于单板控制整车的情况， \ No newline at end of file +机器人命令模块是对整个机器人的抽象，对于单板控制整车的情况，该应用应该包含接收控制指令的模块，例如遥控器、视觉通信模块。该模块会处理接收到的控制数据，并将其转化为**具体的、定量的**控制信息，发送给其他模块。 + +如从遥控器获知当前右侧摇杆拨向上方，则将遥控器发来的数值转化为底盘前进的速度值，然后发送给其他应用。同时，robot_cmd还要从其他应用获取反馈信息，做出其他决策。可以将其视为整个机器人的**大脑**。 + + + +## gimbal + +以步兵为例，云台应用应当包含两个电机，分别用于驱动yaw和pitch轴，还有一个imu（开发板一般放在云台上）。gimbal模块会接收robot_cmd发来的控制信息（云台的角度、转速等），并通过电机提供的接口完成电机的参考值设定。gimbal还要把imu的数据反馈给cmd，用于和视觉的通信以及云台状态的判断。 + + + +## shoot + +还是以步兵为例，发射应用应当包括摩擦轮电机、拨盘电机和弹舱盖。根据cmd应用发来的控制信息，决定当前的发射模式（单发、双发、连发），弹舱盖的开合，以及射速（15？18？30？）等。 + + + +## chassis + +以步兵为例，底盘应该包括4个电机。根据cmd应用发来的控制信息，进行麦克纳姆轮的运动学解算，从而获知四个电机需要的设定值，然后调用电机提供的接口进行设定。chassis还要根据电机的反馈数据以及imu信息（如果有imu的话，即双板的情况，云台一个底盘一个），计算底盘的实际运动状态，反馈给robot_cmd应用。 + + + +## lift + +以工程机器人为例，抬升机构应该包含用于抬升的执行单元（可能是气缸、电磁阀、电机、点推杆等），根据cmd发来的数据控制执行单元运行到特定的高度，并进行必要的反馈。 + + + +## 双板兼容 + +此框架对单开发板/双开发板/多开发板的情况都提供了支持（多板一般只在工程机器人上出现，需要自己编写），目前通过条件编译实现了对单双板的切换。使用双板时，主控板在云台上，连接遥控器和上位机；副板在底盘上，负责底盘的运动控制和与裁判系统的通信。 + diff --git a/application/chassis/chassis.md b/application/chassis/chassis.md index e69de29..e5256f2 100644 --- a/application/chassis/chassis.md +++ b/application/chassis/chassis.md @@ -0,0 +1,19 @@ +# chassis + + + +## 工作流程 + +首先进行初始化，`ChasissInit()`会被`RobotInit()`调用，进行裁判系统、底盘电机的初始化。如果为双板模式，则还会初始化IMU，并且将消息订阅者和发布者的初始化改为`CANComm`的初始化。 + +操作系统启动后，工作顺序为： + +1. 从cmd模块获取数据（如果双板则从CANComm获取） +2. 判断当前控制数据的模式，如果为停止则停止所有电机 +3. 根据控制数据，计算底盘的旋转速度 +4. 根据控制数据中yaw电机的编码器值`angle_offset`，将控制数据映射到底盘坐标系下 +5. 进行麦克纳姆轮的运动学解算，得到每个电机的设定值 +6. 获取裁判系统的数据，并根据底盘功率限制对输出进行限幅 +7. 由电机的反馈数据和IMU（如果有），计算底盘当前的真实运动速度 +8. 设置底盘反馈数据，包括运动速度和裁判系统数据 +9. 将反馈数据推送到消息中心（如果双板则通过CANComm发送） \ No newline at end of file diff --git a/application/cmd/robot_cmd.md b/application/cmd/robot_cmd.md index 1a9075d..c8c9d16 100644 --- a/application/cmd/robot_cmd.md +++ b/application/cmd/robot_cmd.md @@ -4,21 +4,43 @@ ## 运行流程 -运行流程可以很直观的从`RobotCMDTask()`中看出。首先通过消息订阅机制，获取其他应用的反馈信息，然后使用`CalcOffsetAngle()`计算底盘和云台的offset angle（使得底盘始终获知当前的正方向），接着根据当前是通过键鼠or遥控器控制，调用对应的函数，将控制指令量化为具体的控制信息；得到控制信息之后，先不急着发布，而是检测重要的模块和应用是否掉线或出现异常，以及遥控器是否进入紧急停止模式，如果以上情况发生，那么将所有的控制信息都置零，即让电机和其他执行单元保持静止。最后还是通过pubsub机制，把具体的控制信息发布到对应话题，让其他应用获取。 +运行流程可以很直观的从`RobotCMDTask()`中看出。 + +1. 首先通过消息订阅机制，获取其他应用的反馈信息 +2. 使用`CalcOffsetAngle()`计算底盘和云台的offset angle（使得底盘始终获知当前的正方向） +3. 接着根据当前是通过键鼠or遥控器控制，调用对应的函数，将控制指令量化为具体的控制信息 +4. 得到控制信息之后，先不急着发布，而是检测重要的模块和应用是否掉线或出现异常，以及遥控器是否进入紧急停止模式，如果以上情况发生，那么将所有的控制信息都置零，即让电机和其他执行单元保持静止。 +5. 最后通过pubsub机制，把具体的控制信息发布到对应话题，让其他应用获取。若为双板，则将原本要推送给底盘的信息通过CANComm进行发送。 -## 外部接口 - - - - - - - - - -## 私有函数和变量 +### 遥控器控制模式: + +拨轮向下打到底进入紧急停止模式(后续改为关闭遥控器停止,利用daemon);拨轮向上打开启摩擦轮,超过一半开始发射(速度环,连发) + +左侧开关: +- 上:键鼠控制 +- 中:视觉控制(没有识别到目标的时候仍然可以使用遥控器控制云台) +- 下:遥控器控制 + +右侧开关: +- 上:弹舱开 +- 中:底盘云台分离(底盘不旋转,全向移动) +- 下:底盘跟随云台 + +### 键鼠控制模式: + +遥控器左侧开关拨到最上方,进入键鼠控制模式,此时不会响应遥控器遥感和拨轮的输入. + +前后左右:WSAD + +开关弹舱盖:R + +小陀螺:Q + +发射:鼠标左键 + +自瞄:鼠标右键 diff --git a/application/gimbal/gimbal.md b/application/gimbal/gimbal.md index e69de29..d83ca3c 100644 --- a/application/gimbal/gimbal.md +++ b/application/gimbal/gimbal.md @@ -0,0 +1,13 @@ +# gimbal + + + +## 工作流程 + +初始化pitch和yaw电机以及一个imu。订阅gimbal_cmd消息（来自robot_cmd）并发布gimbal_feed话题。 + +1. 从消息中心获取gimbal_cmd话题的消息 +2. 根据消息中的控制模式进行模式切换，如果急停则关闭所有电机 +3. 由设定的模式，进行电机反馈数据来源的切换，修改反馈数据指针，设置前馈控制数据指针等。 +4. 设置反馈数据，包括yaw电机的绝对角度和imu数据 +5. 推送反馈数据到gimbal_feed话题下 \ No newline at end of file diff --git a/application/shoot/shoot.md b/application/shoot/shoot.md index e69de29..8608033 100644 --- a/application/shoot/shoot.md +++ b/application/shoot/shoot.md @@ -0,0 +1,15 @@ +# shoot + + + +## 工作流程 + +初始化3个电机和一个舵机，包括发射的2个m3508，拨盘的m2006和弹仓盖上的舵机（双开门舵机可能要换成2个）。m2006初始化时设置为速度闭环，防止上电乱转。订阅shoot_cmd话题（robot_cmd发布的）并发布shoot_feed话题（robot_cmd会订阅）。 + +1. 从shoot_cmd获取消息 +2. 根据工作模式确定是否急停 +3. 如果之前是单发模式或3发模式并且冷却时间没到，直接结束本次任务，等待下一次进入 +4. 如果已经冷却完成，根据发来的拨盘模式，设定m2006的闭环类型和参考值 +5. 根据发来的弹速数据，设定摩擦轮的参考值 +6. 根据发来的弹舱数据进行开合 +7. 设定反馈数据，推送到shoot_feed话题 \ No newline at end of file diff --git a/bsp/can/bsp_can.c b/bsp/can/bsp_can.c index 85a4f3a..45272d5 100644 --- a/bsp/can/bsp_can.c +++ b/bsp/can/bsp_can.c @@ -2,6 +2,7 @@ #include "main.h" #include "memory.h" #include "stdlib.h" +#include "bsp_dwt.h" /* can instance ptrs storage, used for recv callback */ // 在CAN产生接收中断会遍历数组,选出hcan和rxid与发生中断的实例相同的那个,调用其回调函数 @@ -88,12 +89,15 @@ CANInstance *CANRegister(CAN_Init_Config_s *config) /* @todo 目前似乎封装过度,应该添加一个指向tx_buff的指针,tx_buff不应该由CAN instance保存 */ /* 如果让CANinstance保存txbuff,会增加一次复制的开销 */ -void CANTransmit(CANInstance *_instance) +uint8_t CANTransmit(CANInstance *_instance,uint8_t timeout) { + float dwt_start = DWT_GetTimeline_ms(); while (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0) // 等待邮箱空闲 - ; + if (DWT_GetTimeline_ms() - dwt_start > timeout) // 超时 + return 0; // tx_mailbox会保存实际填入了这一帧消息的邮箱,但是知道是哪个邮箱发的似乎也没啥用 HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox); + return 1; // 发送成功 } void CANSetDLC(CANInstance *_instance, uint8_t length) diff --git a/bsp/can/bsp_can.h b/bsp/can/bsp_can.h index 90d098f..c24817a 100644 --- a/bsp/can/bsp_can.h +++ b/bsp/can/bsp_can.h @@ -56,9 +56,10 @@ void CANSetDLC(CANInstance *_instance, uint8_t length); /** * @brief transmit mesg through CAN device,通过can实例发送消息 * 发送前需要向CAN实例的tx_buff写入发送数据 - * + * + * @param timeout 超时时间,单位为ms;后续改为us,获得更精确的控制 * @param _instance* can instance owned by module */ -void CANTransmit(CANInstance *_instance); +uint8_t CANTransmit(CANInstance *_instance,uint8_t timeout); #endif diff --git a/bsp/can/bsp_can.md b/bsp/can/bsp_can.md index dafd7eb..39c5cab 100644 --- a/bsp/can/bsp_can.md +++ b/bsp/can/bsp_can.md @@ -4,8 +4,7 @@ > TODO: > -> 1. 增加数据帧的长度定义，使得收发更加灵活，而不是固定的8 bytes -> 2. 增加自动检测ID冲突的log输出。 +> 1. 增加自动检测ID冲突的log输出。 ## 使用说明 @@ -65,7 +64,7 @@ typedef void (*can_callback)(can_instance*); ```c void CANRegister(can_instance* instance, can_instance_config config); -void CANTransmit(can_instance* _instance); +uint8_t CANTransmit(can_instance* _instance, uint8_t timeout); ``` `CANRegister`是用于初始化CAN实例的接口，module层的模块对象（也应当为一个结构体）内要包含一个`usart_instance`。调用时传入实例指针，以及用于初始化的config。`CANRegister`应当在module的初始化函数内被调用，推荐config采用以下的方式定义，更加直观明了： @@ -103,4 +102,10 @@ void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) - `HAL_CAN_RxFifo0MsgPendingCallback()`和`HAL_CAN_RxFifo1MsgPendingCallback()`都是对HAL的CAN回调函数的重定义（原本的callback是`__week`修饰的弱定义），当发生FIFO0或FIFO1有新消息到达的时候，对应的callback会被调用。`CANFIFOxCallback()`随后被前两者调用，并根据接收id和硬件中断来源（哪一个CAN硬件，CAN1还是CAN2）调用对应的instance的回调函数进行协议解析。 -- 当有一个模块注册了多个can实例时，通过`CANInstance.id`,使用强制类型转换将其转换成对应模块的实例指针，就可以对不同的模块实例进行回调处理了。 \ No newline at end of file +- 当有一个模块注册了多个can实例时，通过`CANInstance.id`,使用强制类型转换将其转换成对应模块的实例指针，就可以对不同的模块实例进行回调处理了。 + +## 注意事项 + +由于CAN总线自带发送检测，如果总线上没有挂载目标设备（接收id和发送报文相同的设备），那么CAN邮箱会被占满而无法发送。在`CANTransmit()`中会对CAN邮箱是否已满进行`while(1)`检查。当超出`timeout`之后函数会返回零，说明发送失败。 + +由于卡在`while(1)`处不断检查邮箱是否空闲，调用`CANTransmit()`的任务可能无法按时挂起，导致任务定时不精确。建议在没有连接CAN进行调试时，按需注释掉有关CAN发送的代码部分，或设定一个较小的`timeout`值，防止对其他需要精确定时的任务产生影响。 \ No newline at end of file diff --git a/modules/can_comm/can_comm.c b/modules/can_comm/can_comm.c index cb6500e..5e5d7b2 100644 --- a/modules/can_comm/can_comm.c +++ b/modules/can_comm/can_comm.c @@ -109,7 +109,7 @@ void CANCommSend(CANCommInstance *instance, uint8_t *data) send_len = instance->send_buf_len - i >= 8 ? 8 : instance->send_buf_len - i; CANSetDLC(instance->can_ins, send_len); memcpy(instance->can_ins->tx_buff, instance->raw_sendbuf + i, send_len); - CANTransmit(instance->can_ins); + CANTransmit(instance->can_ins,1); } } diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index aff5330..f9c5af3 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -288,7 +288,7 @@ void DJIMotorControl() { if (sender_enable_flag[i]) { - CANTransmit(&sender_assignment[i]); + CANTransmit(&sender_assignment[i],1); } } } diff --git a/modules/motor/HTmotor/HT04.c b/modules/motor/HTmotor/HT04.c index 51381b8..691cb36 100644 --- a/modules/motor/HTmotor/HT04.c +++ b/modules/motor/HTmotor/HT04.c @@ -16,7 +16,7 @@ static void HTMotorSetMode(HTMotor_Mode_t cmd, HTMotorInstance *motor) static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00}; buf[7] = (uint8_t)cmd; memcpy(motor->motor_can_instace->tx_buff, buf, sizeof(buf)); - CANTransmit(motor->motor_can_instace); + CANTransmit(motor->motor_can_instace,1); } /* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */ @@ -144,7 +144,7 @@ void HTMotorControl() { // 若该电机处于停止状态,直接将发送buff置零 memset(motor_can->tx_buff + 6, 0, sizeof(uint16_t)); } - CANTransmit(motor_can); + CANTransmit(motor_can,1); } } diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c index decfda7..c417436 100644 --- a/modules/motor/LKmotor/LK9025.c +++ b/modules/motor/LKmotor/LK9025.c @@ -117,7 +117,7 @@ void LKMotorControl() if (idx) // 如果有电机注册了 { - CANTransmit(sender_instance); + CANTransmit(sender_instance,1); } } diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index eb276ee..c650a22 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -29,9 +29,9 @@ typedef enum ANGLE_LOOP = 0b0100, // only for checking - _ = 0b0011, - __ = 0b0110, - ___ = 0b0111 + SPEED_AND_CURRENT_LOOP = 0b0011, + ANGLE_AND_SPEED_LOOP = 0b0110, + ALL_THREE_LOOP = 0b0111 } Closeloop_Type_e; typedef enum @@ -80,7 +80,6 @@ typedef struct { float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针 float *other_speed_feedback_ptr; - // float *angle_foward_ptr; //前馈数据指针 // float *speed_foward_ptr; // float *current_foward_ptr; diff --git a/modules/remote/remote.md b/modules/remote/remote.md index dba2a7d..e561463 100644 --- a/modules/remote/remote.md +++ b/modules/remote/remote.md @@ -71,14 +71,14 @@ remote_control -拨轮向下打到底进入紧急停止模式;拨轮向上打开启摩擦轮,超过一半开始发射(速度环,连发) +拨轮向下打到底进入紧急停止模式(后续改为关闭遥控器停止,利用daemon);拨轮向上打开启摩擦轮,超过一半开始发射(速度环,连发) -左侧开关`s[1]`: +左侧开关: - 上:键鼠控制 - 中:视觉控制(没有识别到目标的时候仍然可以使用遥控器控制云台) - 下:遥控器控制 -右侧开关`s[0]`: +右侧开关: - 上:弹舱开 - 中:底盘云台分离(底盘不旋转,全向移动) - 下:底盘跟随云台 diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 3a4e347..f0df420 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -11,7 +11,7 @@ static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据TEMP,[1]:上一次的数据LAST. static USARTInstance *rc_usart_instance; /** - * @brief 矫正遥控器摇杆的值 + * @brief 矫正遥控器摇杆的值,超过660或者小于-660的值都认为是无效值,置0 * */ static void RectifyRCjoystick() @@ -31,7 +31,7 @@ static void RectifyRCjoystick() */ static void sbus_to_rc(const uint8_t *sbus_buf) { - memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据 + memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断 // 摇杆,直接解算时减去偏置 rc_ctrl[TEMP].rc.rocker_r_ = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 0 rc_ctrl[TEMP].rc.rocker_r1 = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 1 @@ -40,8 +40,8 @@ static void sbus_to_rc(const uint8_t *sbus_buf) rc_ctrl[TEMP].rc.dial = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF) - RC_CH_VALUE_OFFSET; // 左侧拨轮 RectifyRCjoystick(); // 开关,0左1右 - rc_ctrl[TEMP].rc.switch_right = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left - rc_ctrl[TEMP].rc.switch_left = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right + rc_ctrl[TEMP].rc.switch_right = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch right + rc_ctrl[TEMP].rc.switch_left = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch left // 鼠标解析 rc_ctrl[TEMP].mouse.x = sbus_buf[6] | (sbus_buf[7] << 8); //!< Mouse X axis @@ -81,8 +81,7 @@ static void sbus_to_rc(const uint8_t *sbus_buf) /** * @brief protocol resolve callback * this func would be called when usart3 idle interrupt happens - * 对sbus_to_rc的简单封装 - * + * 对sbus_to_rc的简单封装,用于注册到bsp_usart的回调函数中 */ static void RemoteControlRxCallback() { diff --git a/modules/super_cap/super_cap.c b/modules/super_cap/super_cap.c index fecbf84..7271298 100644 --- a/modules/super_cap/super_cap.c +++ b/modules/super_cap/super_cap.c @@ -35,7 +35,7 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config) void SuperCapSend(SuperCapInstance *instance, uint8_t *data) { memcpy(instance->can_ins->tx_buff, data, 8); - CANTransmit(instance->can_ins); + CANTransmit(instance->can_ins,1); } SuperCap_Msg_s SuperCapGet(SuperCapInstance *instance)