From bd2b992e9c3db420201b56a1af32587e8400ee5f Mon Sep 17 00:00:00 2001 From: NeoZng Date: Mon, 6 Mar 2023 18:52:24 +0800 Subject: [PATCH 01/12] =?UTF-8?q?=E5=88=A0=E9=99=A4=E4=BA=86message=20cent?= =?UTF-8?q?er=E7=9A=84=E6=8C=87=E9=92=88=E4=BC=A0=E9=80=92=E6=94=AF?= =?UTF-8?q?=E6=8C=81=EF=BC=8C=E5=A2=9E=E5=8A=A0=E4=BA=86=E5=AE=9A=E4=BD=8D?= =?UTF-8?q?bug=E7=9A=84=E6=8A=80=E5=B7=A7=E6=96=87=E6=A1=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Bug_Report.md | 4 ++ application/balance_chassis/balance.c | 27 +++++--- application/balance_chassis/gain_table.h | 3 +- application/balance_chassis/vmc_project.h | 14 ++++ application/gimbal/gimbal.c | 6 +- modules/message_center/message_center.c | 83 +++-------------------- modules/message_center/message_center.h | 27 +------- 如何定位bug.md | 73 +++++++++++++++++++- 8 files changed, 126 insertions(+), 111 deletions(-) create mode 100644 application/balance_chassis/vmc_project.h diff --git a/Bug_Report.md b/Bug_Report.md index 4223b3f..a517636 100644 --- a/Bug_Report.md +++ b/Bug_Report.md @@ -1,5 +1,7 @@ # 异常报告 +已知可能出现的bug将会列在此处，并指明修复期限和任务执行者。 + 使用中遇到的bug和错误放在此处。参照下列格式： ## 标题用简短的一句话描述 @@ -32,6 +34,8 @@ ## 使用LK电机并挂载在hcan2上时会出现HardFault +> 已修复此问题。修复日志请查看当前目录下的“如何定位bug.md”。 + 使用MF9025v2电机,并将其配置在CAN2上。经过一次LKMotorControl，第二次进入时hcan->instance会在HAL_CAN_Add_Tx_Message()结束时被未知的语句修改成奇怪的值，造成HardFault ### 尝试解决的方案 diff --git a/application/balance_chassis/balance.c b/application/balance_chassis/balance.c index 741c35b..d587381 100644 --- a/application/balance_chassis/balance.c +++ b/application/balance_chassis/balance.c @@ -1,4 +1,10 @@ +// app #include "balance.h" +#include "vmc_project.h" +#include "gain_table.h" +#include "robot_def.h" +#include "general_def.h" +// module #include "HT04.h" #include "LK9025.h" #include "bmi088.h" @@ -6,9 +12,8 @@ #include "super_cap.h" #include "controller.h" #include "can_comm.h" +// standard #include "stdint.h" -#include "robot_def.h" -#include "general_def.h" #include "arm_math.h" // 需要用到较多三角函数 /* 底盘拥有的模块实例 */ @@ -47,7 +52,7 @@ static float T_leg_left, T_leg_right; // 左右驱动电机力矩 * */ static void CalcLQR() -{ +{ } /** @@ -55,15 +60,15 @@ static void CalcLQR() * */ static void VMCProject() -{ +{ // 拟将功能封装到vmc_project.h中 } /** * @brief 腿部角度控制:转向和抗劈叉 * */ -static PIDInstance swerving_pid; -static PIDInstance anti_crash_pid; +static PIDInstance swerving_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量 +static PIDInstance anti_crash_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上 static void SynthesizeMotion() { @@ -73,15 +78,16 @@ static void SynthesizeMotion() * @brief 腿长控制:长度 + roll轴补偿(保持机体水平),用PD模拟弹簧的传递函数 * */ -static PIDInstance leg_length_pid; -static PIDInstance roll_compensate_pid; +static PIDInstance leg_length_pid; // 用PD模拟弹簧的传递函数,不需要积分项(弹簧是一个无积分的二阶系统),增益不可过大否则抗外界冲击响应时太"硬" +static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平 static void LegControl() { } /** - * @brief 离地监测和?跳跃控制 + * @brief 离地监测和?跳跃控制? + * 通过模型解算地面的支持力完成离地检测 * */ static void FlyDetect() @@ -89,7 +95,7 @@ static void FlyDetect() } /** - * @brief 功率限制 + * @brief 功率限制,一般不需要 * */ static void WattLimit() @@ -242,6 +248,7 @@ void BalanceInit() PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf); } +/* balanceTask可能需要以更高频率运行,以提高线性化的精确程度 */ void BalanceTask() { } diff --git a/application/balance_chassis/gain_table.h b/application/balance_chassis/gain_table.h index 1449e93..b2086bc 100644 --- a/application/balance_chassis/gain_table.h +++ b/application/balance_chassis/gain_table.h @@ -2,13 +2,14 @@ #pragma once #include "stdint.h" +#include "stm32f407xx.h" #include "arm_math.h" #include "math.h" #define GAIN_TABLE_SIZE 100 // 增益表大小 // K 2x6,6个状态变量2个输出(Tp关节电机和T驱动轮电机) -static float leglen2gain [GAIN_TABLE_SIZE][2][6] = {}; +static float leglen2gain [GAIN_TABLE_SIZE][2][6] = {0}; static interpolation_flag = 0; // 插值方式:1 线性插值 0 关闭插值 diff --git a/application/balance_chassis/vmc_project.h b/application/balance_chassis/vmc_project.h new file mode 100644 index 0000000..b74b239 --- /dev/null +++ b/application/balance_chassis/vmc_project.h @@ -0,0 +1,14 @@ +#ifndef VMC_PROJECT_H +#define VMC_PROJECT_H + +#include "stm32f407xx.h" +#include "arm_math.h" +#include "math.h" +#include "general_def.h" + +// 将五连杆和直杆的vmc映射放在此处,方便修改和调试,balance.c不会太长 +#endif // !VMC_PROJECT_H + + + + diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 5f23d3e..389ad64 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -4,7 +4,6 @@ #include "ins_task.h" #include "message_center.h" #include "general_def.h" - #include "bmi088.h" static attitude_t *gimba_IMU_data; // 云台IMU数据 @@ -136,6 +135,7 @@ void GimbalTask() // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); + // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref switch (gimbal_cmd_recv.gimbal_mode) { @@ -170,6 +170,10 @@ void GimbalTask() 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->motor_measure.angle_single_round; diff --git a/modules/message_center/message_center.c b/modules/message_center/message_center.c index 387c9fb..cc4e826 100644 --- a/modules/message_center/message_center.c +++ b/modules/message_center/message_center.c @@ -2,78 +2,11 @@ #include "stdlib.h" #include "string.h" -/* 消息初始化用 */ -static char pname[MAX_EVENT_COUNT][MAX_EVENT_NAME_LEN + 1]; -static char sname[MAX_EVENT_COUNT][MAX_EVENT_NAME_LEN + 1]; -static void *p_ptr[MAX_EVENT_COUNT] = {NULL}; -static void **s_pptr[MAX_EVENT_COUNT] = {NULL}; // 因为要修改指针,所以需要二重指针 - -/* ----------------------------------第三方指针传递版的实现,deprecated----------------------------------- */ - -void MessageInit() -{ - // pub必须唯一,即消息名称不能重复,不得有多个pub发布相同消息名称 - // 对每一个subscriber,寻找相同消息名称的publisher,可能有多个sub从相同pub获取消息 - for (size_t i = 0; i < MAX_EVENT_COUNT; ++i) - { - if (s_pptr[i] != NULL) - { - for (size_t j = 0; j < MAX_EVENT_COUNT; ++j) // 遍历publisher - { - if (p_ptr[j] != NULL) // 不为空 - { - if (strcmp(sname[i], pname[j]) == 0) // 比较消息名是否一致 - { - *s_pptr[i] = p_ptr[j]; // 将sub的指针指向pub的数据 - break; - } - } - else // 到结尾,退出 - { - while (1) - ; // 如果你卡在这里,说明没有找到消息发布者!请确认消息名称是否键入错误 - } - } - } - else // 说明已经遍历完所有的subs - { - break; - } - } -} - -/* 传入数据地址 */ -void PublisherRegister(char *name, void *data) -{ - static uint8_t idx; - for (size_t i = 0; i < idx; ++i) - { - if (strcmp(pname[i], name) == 0) - while (1) - ; // 运行至此说明pub的消息发布名称冲突 - } - strcpy(pname[idx], name); - p_ptr[idx++] = data; -} - -/* 注意传入的是指针的地址,传参时使用&对数据指针取地址 */ -void SubscribeEvent(char *name, void **data_ptr) -{ - static uint8_t idx; - strcpy(sname[idx], name); - s_pptr[idx++] = data_ptr; -} - - - -/* ----------------------------------链表-队列版的实现----------------------------------- */ - /* message_center是fake head node,是方便链表编写的技巧,这样就不需要处理链表头的特殊情况 */ static Publisher_t message_center = { .event_name = "Message_Manager", .first_subs = NULL, - .next_event_node = NULL - }; + .next_event_node = NULL}; static void CheckName(char *name) { @@ -112,18 +45,18 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len) { // 给消息队列的每一个元素分配空间,queue里保存的实际上是数据执指针,这样可以兼容不同的数据长度 ret->queue[i] = malloc(sizeof(data_len)); } - //如果是第一个订阅者,特殊处理一下 - if(node->first_subs==NULL) + // 如果是第一个订阅者,特殊处理一下 + if (node->first_subs == NULL) { - node->first_subs=ret; + node->first_subs = ret; return ret; } // 遍历订阅者链表,直到到达尾部 Subscriber_t *sub = node->first_subs; // 作为iterator - while (sub->next_subs_queue) // 遍历订阅了该事件的订阅者链表 + while (sub->next_subs_queue) // 遍历订阅了该事件的订阅者链表 { sub = sub->next_subs_queue; // 移动到下一个订阅者,遇到空指针停下,说明到了链表尾部 - } + } sub->next_subs_queue = ret; // 把刚刚创建的订阅者接上 return ret; } @@ -157,6 +90,7 @@ Publisher_t *PubRegister(char *name, uint8_t data_len) if (strcmp(node->event_name, name) == 0) // 如果已经注册了相同的事件,直接返回结点指针 { CheckLen(data_len, node->data_len); + node->pub_registered_flag = 1; return node; } } // 遍历完发现尚未创建name对应的事件 @@ -165,6 +99,7 @@ Publisher_t *PubRegister(char *name, uint8_t data_len) memset(node->next_event_node, 0, sizeof(Publisher_t)); node->next_event_node->data_len = data_len; strcpy(node->next_event_node->event_name, name); + node->pub_registered_flag = 1; return node->next_event_node; } @@ -189,7 +124,7 @@ uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr) while (iter) { if (iter->temp_size == QUEUE_SIZE) // 如果队列已满,则需要删除最老的数据(头部),再填入 - { + { // 队列头索引前移动,相当于抛弃前一个位置的数据,被抛弃的位置稍后会被写入新的数据 iter->front_idx = (iter->front_idx + 1) % QUEUE_SIZE; iter->temp_size--; // 相当于出队,size-1 diff --git a/modules/message_center/message_center.h b/modules/message_center/message_center.h index 44a3223..8335b4b 100644 --- a/modules/message_center/message_center.h +++ b/modules/message_center/message_center.h @@ -19,30 +19,6 @@ #define MAX_EVENT_COUNT 12 // 最多支持的事件数量 #define QUEUE_SIZE 1 -/** - * @brief 在所有应用初始化结束之后调用,当作app的"回调函数" - * - */ -void MessageInit(); - -/** - * @brief 注册成为消息发布者 - * - * @param name 消息标识名,注意不要超过MAX_EVENT_NAME_LEN - * @param data 发布者的数据地址 - */ -void PublisherRegister(char *name, void *data); - -/** - * @brief 订阅消息,成为消息订阅者 - * - * @param name 消息标识名 - * @param data 保存数据的指针的地址,注意这是一个二级指针,传入的时候对数据指针取地址(&) - */ -void SubscribeEvent(char *name, void **data); - -#endif // !PUBSUB_H - typedef struct mqt { /* 用数组模拟FIFO队列 */ @@ -69,6 +45,7 @@ typedef struct ent Subscriber_t *first_subs; /* 指向下一个Publisher的指针 */ struct ent *next_event_node; + uint8_t pub_registered_flag; // 用于标记该发布者是否已经注册 } Publisher_t; /** @@ -105,3 +82,5 @@ uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr); * @return uint8_t 新消息成功推送给几个订阅者 */ uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr); + +#endif // !PUBSUB_H \ No newline at end of file diff --git a/如何定位bug.md b/如何定位bug.md index 045e711..a2e521f 100644 --- a/如何定位bug.md +++ b/如何定位bug.md @@ -85,6 +85,77 @@ long long的范围比float小。无符号和有符号数直接转换可能变成 **宏只在当前文件生效**，如果宏放在.c那么对其他的文件是不可见的，这也一般称作私有宏。 + + ## 典型debug案例 -这是一个结合了软件和硬件且多模块耦合的异常。 \ No newline at end of file +这是一个结合了软件和硬件且有多模块耦合的异常。该bug发生在调试平衡步兵的底盘过程当中。 + +### 引发bug的原因 + +1. 指针在强制类型转换中变成了错误的类型,使得指向的内存地址被错误地修改 +2. CAN总线负载过大导致电机反馈消息丢失 + +这里是发生bug的代码片段: + +```c +static void LKMotorDecode(CANInstance *_instance) +{ + static LKMotor_Measure_t *measure; + static uint8_t *rx_buff; + rx_buff = _instance->rx_buff; + measure = &((LKMotorInstance *)_instance)->measure; // 通过caninstance保存的id获取对应的motorinstance + // 上面一行应为: measure = &(((LKMotorInstance *)_instance->id)->measure); + measure->last_ecd = measure->ecd; + measure->ecd = ... + + // .... + +} +``` + +这是问题1的出处。can instance中保存了父指针，即拥有该instance的LKMotorInstance。这里想通过强制类型转换将`void*`类型的`id`转换成电机的instance指针类型并访问其measure成员变量以从CAN反馈的报文中更新量测值。然而却直接将can instance转换成motor instance。 + +随后，更新之后的数据被覆写到can instance内部，使得其成员变量改变，包括hcan、txbuf、rxbuf、tx/rxlen等。hcan是HAL定义的can句柄类型，里面保存了指向can状态和控制寄存器的指针以及其他HAL状态信息，然而其值被电机反馈回来的值覆写，之后HAL的接口访问hcan时将引起异常。 + +第二个问题则不是显式存在的： + +```c +void MotorControlTask() +{ + DJIMotorControl(); + + HTMotorControl(); + + LKMotorControl(); + + ServeoMotorControl(); + + StepMotorControl(); +} +``` + +这是motortask的内容，此任务将以500hz的频率运行。在发生bug时，我们将4个HT04电机和2个LK MF9025电机全部连接到CAN1上。注意，HT04不支持多电机指令，因此占用的带宽较大。在`LKMotorControl()`完成参考值计算和CAN发送之后立刻会调用`HTMotorControl()`，后者需要连续发送4条报文。而HT和LK电机都会在接收到控制指令之后发送反馈信息报文。由于HT电机的控制在LK电机控制之后立刻执行，导致总线被占据，LK电机发送的反馈数据仲裁失败无法获得总线占有权，使得主机收不到反馈数据。 + + + +### bug的发现和定位的尝试 + +程序的大体情况如下，当时进行轮足式倒立摆机器人的测试，启用了balance.c，在其中注册了4个HT04电机（can1）和2个LK9025电机（can2）。控制报文的发送频率均为500Hz。 + +测试时发现，9025电机可以接收到mcu发送的控制指令并响应，但是mcu始终无法获得反馈值，`LKMotorInstance->measure`的所有成员变量一直是零。由于CAN是总线架构，电机能接收到数据说明通信正常。HT04电机也可以正常控制并收到反馈信息。在`LKMotorDecode()`函数中添加断点发现能够成功进入1~2次，随后便引发HardFault。 + +此时内心有些动摇，开始检查硬件连线。我们尝试把LK电机也挂载到CAN1总线上。开始单步调试，发现LK电机可以正常接收一次反馈报文，之后就进入`Hardfault_handler()`。HT和DJI电机均无此问题。进一步进行每条指令的调试，发现在成功接收到一次报文之后（接收报文指的是can发生中断并在处理函数中调用LK电机的解码函数，我们并没有查看measure值是否刷新，实际上这时候反馈值仍然为零），进入该电机的控制报文发送时，通过在`Hardfault_handler()`中添加汇编语句`asm("bx lr")`，即跳转到最后一次执行的指令，发现访问`hcan->state`会引起硬件错误。遇到这种情况，说明发生了越界访问或使用了野指针。检查hcan的值，发现是一个非常大的地址。因此怀疑hcan指针被其他的内存访问语句修改。 + +有了方向之后，进一步对每一个函数都进行单步进入调试，同时时刻监测hcan1的值。然而，这时候出现即使一开始就单步调试也无法进入LK电机解码函数的问题。于是，怀疑是CAN过滤器的配置问题，使得LK电机反馈报文被过滤，检查LK的接收id无误后，认为可能由于LK电机的发送和接收ID都比较大（0x140和0x280），CAN标准ID放不下。但是查阅CAN specification后发现standar ID可以容纳11位的值，应该不会有问题。于是把过滤器配置为mask模式，让bxCAN控制器接收所有报文（即不进行过滤）。然而还是不奏效，仍然无法收到数据。 + +这时候想起HT电机是不支持多电机控制指令的，因此500Hz的控制频率似乎有些过高，相当于2ms内要完成2x4+1+2=11次CAN报文的发送。计算1M波特率下最大通信速率，果然超出了负载。于是降低`MotorTask()`的频率为200Hz，果然能重新接收到数据了。 + +继续单步调试，终于发现在`LKMotorDecode()`中，通过强制类型转换获取LKMotorInstance的时候，用错了变量，使得反馈值被写入电机的`CANInstance`内，导致hcan指向随机的地址，最终造成访问时引发hardfault。 + +修改之后，将LK电机挂载到CAN2上，控制频率回到500Hz，程序正常运行。 + +### 解决方案 + +均衡总线负载，调节任务运行时间。 + From 87d0a5161cf48ccdca9f27aa66ae14dc73c5e448 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Sat, 18 Mar 2023 14:50:50 +0800 Subject: [PATCH 02/12] =?UTF-8?q?=E6=B3=A2=E7=89=B9=E7=8E=87=E6=8F=90?= =?UTF-8?q?=E5=8D=87=E4=B8=BA921600,=E4=BF=AE=E5=A4=8D=E4=BA=86=E5=92=8C?= =?UTF-8?q?=E8=A7=86=E8=A7=89=E9=80=9A=E4=BF=A1=E7=9A=84=E6=BA=A2=E5=87=BA?= =?UTF-8?q?=E9=94=99=E8=AF=AF=E4=BB=A5=E5=8F=8ADMA=E5=86=B2=E7=AA=81.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- HAL_N_Middlewares/Src/usart.c | 2 +- application/referee/referee.md | 3 +++ application/robot.c | 4 ++-- bsp/usart/bsp_usart.c | 26 +++++++++++------------- bsp/usart/bsp_usart.h | 15 ++++---------- modules/master_machine/master_process.c | 5 ++++- modules/master_machine/seasky_protocol.c | 2 +- modules/referee/referee.md | 3 +++ modules/referee/rm_referee.c | 2 +- 必须做&禁止做.md | 4 +++- 10 files changed, 34 insertions(+), 32 deletions(-) create mode 100644 application/referee/referee.md diff --git a/HAL_N_Middlewares/Src/usart.c b/HAL_N_Middlewares/Src/usart.c index 8595e32..5d513a2 100644 --- a/HAL_N_Middlewares/Src/usart.c +++ b/HAL_N_Middlewares/Src/usart.c @@ -46,7 +46,7 @@ void MX_USART1_UART_Init(void) /* USER CODE END USART1_Init 1 */ huart1.Instance = USART1; - huart1.Init.BaudRate = 115200; + huart1.Init.BaudRate = 921600; huart1.Init.WordLength = UART_WORDLENGTH_8B; huart1.Init.StopBits = UART_STOPBITS_1; huart1.Init.Parity = UART_PARITY_NONE; diff --git a/application/referee/referee.md b/application/referee/referee.md new file mode 100644 index 0000000..7e97160 --- /dev/null +++ b/application/referee/referee.md @@ -0,0 +1,3 @@ +# referee + +需要将此模块移动到module层，并新建一个rtos任务，以一定频率运行，用于ui刷新和多机通信 diff --git a/application/robot.c b/application/robot.c index 5e79041..6463b39 100644 --- a/application/robot.c +++ b/application/robot.c @@ -35,7 +35,7 @@ void RobotInit() #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - Referee_Interactive_init(); + // Referee_Interactive_init(); ChassisInit(); #endif // 初始化完成,开启中断 @@ -52,6 +52,6 @@ void RobotTask() #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) ChassisTask(); - Referee_Interactive_task(); + // Referee_Interactive_task(); #endif } diff --git a/bsp/usart/bsp_usart.c b/bsp/usart/bsp_usart.c index 0e26191..ba019d2 100644 --- a/bsp/usart/bsp_usart.c +++ b/bsp/usart/bsp_usart.c @@ -19,7 +19,7 @@ static USARTInstance *usart_instance[DEVICE_USART_CNT] = {NULL}; /** * @brief 启动串口服务,会在每个实例注册之后自动启用接收,当前实现为DMA接收,后续可能添加IT和BLOCKING接收 - * + * * @todo 串口服务会在每个实例注册之后自动启用接收,当前实现为DMA接收,后续可能添加IT和BLOCKING接收 * 可能还要将此函数修改为extern,使得module可以控制串口的启停 * @@ -52,24 +52,22 @@ USARTInstance *USARTRegister(USART_Init_Config_s *init_config) } /* @todo 当前仅进行了形式上的封装,后续要进一步考虑是否将module的行为与bsp完全分离 */ -void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size) -{ - HAL_UART_Transmit_DMA(_instance->usart_handle, send_buf, send_size); -} - -void USARTAbort(USARTInstance *_instance, USART_TRANSFER_MODE mode) +void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size, USART_TRANSFER_MODE mode) { switch (mode) { - case USART_TRANSFER_TX: - // if(_instance.work_mode == USART_TX_DMA) - HAL_UART_AbortTransmit_IT(_instance->usart_handle); + case USART_TRANSFER_BLOCKING: + HAL_UART_Transmit(_instance->usart_handle, send_buf, send_size, 100); break; - case USART_TRANSFER_RX: - // if(_instance.work_mode == USART_RX_DMA) - HAL_UART_AbortReceive_IT(_instance->usart_handle); + case USART_TRANSFER_IT: + HAL_UART_Transmit_IT(_instance->usart_handle, send_buf, send_size); break; - case USART_TRANSFER_NONE: + case USART_TRANSFER_DMA: + HAL_UART_Transmit_DMA(_instance->usart_handle, send_buf, send_size); + break; + default: + while (1) + ; // illegal mode! check your code context! break; } } diff --git a/bsp/usart/bsp_usart.h b/bsp/usart/bsp_usart.h index 34e74db..e7e9dcf 100644 --- a/bsp/usart/bsp_usart.h +++ b/bsp/usart/bsp_usart.h @@ -13,8 +13,9 @@ typedef void (*usart_module_callback)(); typedef enum { USART_TRANSFER_NONE=0, - USART_TRANSFER_TX, - USART_TRANSFER_RX, + USART_TRANSFER_BLOCKING, + USART_TRANSFER_IT, + USART_TRANSFER_DMA, } USART_TRANSFER_MODE; // 串口实例结构体,每个module都要包含一个实例. @@ -52,14 +53,6 @@ USARTInstance *USARTRegister(USART_Init_Config_s *init_config); * @param send_buf 待发送数据的buffer * @param send_size how many bytes to send */ -void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size); - -/** - * @brief 通过调用该函数终止串口的发送或者接收,通过传入mode参数来选择终止发送还是接收 - * - * @param _instance 串口实例 - * @param mode 选择终止发送还是接收的模式 - */ -void USARTAbort(USARTInstance *_instance,USART_TRANSFER_MODE mode); +void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size,USART_TRANSFER_MODE mode); #endif diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index 09cc659..3bed315 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -61,5 +61,8 @@ void VisionSend(Vision_Send_s *send) // 将数据转化为seasky协议的数据包 get_protocol_send_data(0x02, flag_register, &send->yaw, 3, send_buff, &tx_len); - USARTSend(vision_usart_instance, send_buff, tx_len); + USARTSend(vision_usart_instance, send_buff, tx_len,USART_TRANSFER_IT); // 和视觉通信使用IT,防止和接收使用的DMA冲突 + // 此处为HAL设计的缺陷,DMASTOP会停止发送和接收,导致再也无法进入接收中断. + // 也可在发送完成中断中重新启动DMA接收,但较为复杂.因此,此处使用IT发送. + } \ No newline at end of file diff --git a/modules/master_machine/seasky_protocol.c b/modules/master_machine/seasky_protocol.c index 105a6b8..a21b828 100644 --- a/modules/master_machine/seasky_protocol.c +++ b/modules/master_machine/seasky_protocol.c @@ -114,7 +114,7 @@ uint16_t get_protocol_info(uint8_t *rx_buf, // 接收到的原始数据 if (CRC16_Check_Sum(&rx_buf[0], date_length)) { *flags_register = (rx_buf[7] << 8) | rx_buf[6]; - memcpy(rx_data, rx_buf + 8, 4 * sizeof(pro.header.data_length - 2)); + memcpy(rx_data, rx_buf + 8, pro.header.data_length - 2); return pro.cmd_id; } } diff --git a/modules/referee/referee.md b/modules/referee/referee.md index e69de29..d818026 100644 --- a/modules/referee/referee.md +++ b/modules/referee/referee.md @@ -0,0 +1,3 @@ +# referee + +当前模块组织较为混乱，后续统一为多机通信+裁判系统信息接收+UI绘制。UI绘制和多机通信的发送部分在referee任务中以一定的频率运行，信息的接收通过中断完成。 \ No newline at end of file diff --git a/modules/referee/rm_referee.c b/modules/referee/rm_referee.c index e40297e..7d5e329 100644 --- a/modules/referee/rm_referee.c +++ b/modules/referee/rm_referee.c @@ -41,7 +41,7 @@ referee_info_t *RefereeInit(UART_HandleTypeDef *referee_usart_handle) */ void RefereeSend(uint8_t *send, uint16_t tx_len) { - USARTSend(referee_usart_instance, send, tx_len); + USARTSend(referee_usart_instance, send, tx_len,USART_TRANSFER_IT); /* syhtodo DMA请求过快会导致数据发送丢失，考虑数据尽可能打成一个整包以及队列发送，并且发送函数添加缓冲区 */ HAL_Delay(5); } diff --git a/必须做&禁止做.md b/必须做&禁止做.md index 60c2629..e3a9d0b 100644 --- a/必须做&禁止做.md +++ b/必须做&禁止做.md @@ -14,5 +14,7 @@ ## 请给你编写的bsp和module提供详细的文档和使用示例，并为接口增加安全检查 -“treat your user as idot！” +用于调试的条件编译和log输出也是必须的。 + +另外，“treat your user as idot！” From 6f7bf8e9d7936ffdce8ca139a5621fcdabeba2cb Mon Sep 17 00:00:00 2001 From: NeoZng Date: Sat, 18 Mar 2023 20:36:21 +0800 Subject: [PATCH 03/12] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86=E7=AD=89?= =?UTF-8?q?=E7=BA=A7=E6=97=A5=E5=BF=97=E5=92=8CRTT=20viewer=E7=9A=84?= =?UTF-8?q?=E6=94=AF=E6=8C=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/launch.json | 1 + .vscode/tasks.json | 7 +++++ VSCode+Ozone使用方法.md | 7 ++++- bsp/log/bsp_log.c | 5 ++-- bsp/log/bsp_log.h | 27 ++++++++++++++++---- bsp/usart/bsp_usart.c | 17 +++++++++++-- bsp/usart/bsp_usart.h | 25 +++++++++++++++--- modules/master_machine/master_process.c | 34 ++++++++++++++++++++++--- modules/motor/DJImotor/dji_motor.md | 6 ++++- 9 files changed, 110 insertions(+), 19 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 440a688..989a8f1 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -35,6 +35,7 @@ "servertype": "jlink", "interface": "swd", "svdFile": "STM32F407.svd", + "rtos": "FreeRTOS", // "preLaunchTask": "build task",//先运行Build任务,取消注释即可使用 }, ], diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 7ef9fdf..46dbedf 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -30,5 +30,12 @@ "isDefault": false, } }, + { + "label": "log", + "type": "shell", + "command":"JlinkRTTClient", + "args": [], + "problemMatcher": [], + } ] } \ No newline at end of file diff --git a/VSCode+Ozone使用方法.md b/VSCode+Ozone使用方法.md index a9f241c..17a8c1d 100644 --- a/VSCode+Ozone使用方法.md +++ b/VSCode+Ozone使用方法.md @@ -8,7 +8,6 @@ > > 1. 添加一键编译+启用ozone调试脚本，使得整个进一步流程自动化 > 2. 增加更多的背景知识介绍 -> 3. 增加VSCode下RTT viewer的支持 @@ -481,6 +480,12 @@ VSCode `ctrl+,`进入设置，通过`搜索`找到cortex-debug插件的设置。 +### RTT Viewer日志功能 + +本框架添加了vscode下Segger RTT client的支持。在`.vscode/task.json`中已经添加了启动rtt viewer client的任务。你也可以将此任务作为附加启动任务和调试一起启动，方便查看日志。要使用日志，请包含`bsp_log.h`。注意，需要将jlink的安装目录添加到环境变量中。 + + + ### 更好的编辑体验 建议安装以下插件： diff --git a/bsp/log/bsp_log.c b/bsp/log/bsp_log.c index 815a59b..630733e 100644 --- a/bsp/log/bsp_log.c +++ b/bsp/log/bsp_log.c @@ -15,7 +15,7 @@ int PrintLog(const char *fmt, ...) { va_list args; va_start(args, fmt); - int n = SEGGER_RTT_vprintf(BUFFER_INDEX, fmt, &args); + int n = SEGGER_RTT_vprintf(BUFFER_INDEX, fmt, &args); // 一次可以开启多个buffer(多个终端),我们只用一个 va_end(args); return n; } @@ -31,4 +31,5 @@ void Float2Str(char *str, float va) sprintf(str, "-%d.%d", head, point); else sprintf(str, "%d.%d", head, point); -} \ No newline at end of file +} + diff --git a/bsp/log/bsp_log.h b/bsp/log/bsp_log.h index b3d6e7a..d4fa43c 100644 --- a/bsp/log/bsp_log.h +++ b/bsp/log/bsp_log.h @@ -2,17 +2,34 @@ #define _BSP_LOG_H /** - * @brief 初始化日志功能,在操作系统启动之前调用 + * @brief 日志功能原型,供下面的LOGI,LOGW,LOGE等使用 * */ -void BSPLogInit(); +#define LOG_PROTO(type,color,format,...) \ + SEGGER_RTT_printf(BUFFER_INDEX," %s%s"format"\r\n%s", \ + color, \ + type, \ + ##__VA_ARGS__, \ + RTT_CTRL_RESET) + +/* 清屏 */ +#define LOG_CLEAR() SEGGER_RTT_WriteString(0, " "RTT_CTRL_CLEAR) + +/* 无颜色日志输出 */ +#define LOG(format,...) LOG_PROTO("","",format,##__VA_ARGS__) + +/* 有颜色格式日志输出 */ +#define LOGINFO(format,...) LOG_PROTO("I: ", RTT_CTRL_TEXT_BRIGHT_GREEN , format, ##__VA_ARGS__) +#define LOGWARNING(format,...) LOG_PROTO("W: ", RTT_CTRL_TEXT_BRIGHT_YELLOW, format, ##__VA_ARGS__) +#define LOGERROR(format,...) LOG_PROTO("E: ", RTT_CTRL_TEXT_BRIGHT_RED , format, ##__VA_ARGS__) + /** * @brief 通过segger RTT打印日志,支持格式化输出,格式化输出的实现参考printf * - * @param fmt - * @param ... - * @return int + * @param fmt 格式字符串 + * @param ... 参数列表 + * @return int 打印的log字符数 */ int PrintLog(const char *fmt, ...); diff --git a/bsp/usart/bsp_usart.c b/bsp/usart/bsp_usart.c index ba019d2..f1c0a97 100644 --- a/bsp/usart/bsp_usart.c +++ b/bsp/usart/bsp_usart.c @@ -25,7 +25,7 @@ static USARTInstance *usart_instance[DEVICE_USART_CNT] = {NULL}; * * @param _instance instance owned by module,模块拥有的串口实例 */ -static void USARTServiceInit(USARTInstance *_instance) +void USARTServiceInit(USARTInstance *_instance) { HAL_UARTEx_ReceiveToIdle_DMA(_instance->usart_handle, _instance->recv_buff, _instance->recv_buff_size); // 关闭dma half transfer中断防止两次进入HAL_UARTEx_RxEventCallback() @@ -67,11 +67,24 @@ void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size, break; default: while (1) - ; // illegal mode! check your code context! + ; // illegal mode! check your code context! 检查定义instance的代码上下文,可能出现指针越界 break; } } +/* 串口发送时,gstate会被设为BUSY_TX */ +uint8_t USARTIsReady(USARTInstance *_instance) +{ + if(_instance->usart_handle->gState | HAL_UART_STATE_BUSY_TX) + { + return 0; + } + else + { + return 1; + } +} + /** * @brief 每次dma/idle中断发生时，都会调用此函数.对于每个uart实例会调用对应的回调进行进一步的处理 * 例如:视觉协议解析/遥控器解析/裁判系统解析 diff --git a/bsp/usart/bsp_usart.h b/bsp/usart/bsp_usart.h index e7e9dcf..e6d5e4d 100644 --- a/bsp/usart/bsp_usart.h +++ b/bsp/usart/bsp_usart.h @@ -10,6 +10,7 @@ // 模块回调函数,用于解析协议 typedef void (*usart_module_callback)(); +/* 发送模式枚举 */ typedef enum { USART_TRANSFER_NONE=0, @@ -37,17 +38,25 @@ typedef struct } USART_Init_Config_s; /** - * @brief 注册一个串口实例. + * @brief 注册一个串口实例,返回一个串口实例指针 * * @param init_config 传入串口初始化结构体 */ USARTInstance *USARTRegister(USART_Init_Config_s *init_config); +/** + * @brief 启动串口服务,需要传入一个usart实例.一般用于lost callback的情况(使用串口的模块daemon) + * + * @param _instance + */ +void USARTServiceInit(USARTInstance *_instance); + + /** * @brief 通过调用该函数可以发送一帧数据,需要传入一个usart实例,发送buff以及这一帧的长度 - * 当前默认为DMA发送,后续会增加中断发送和阻塞发送模式的选择 - * @todo 目前只支持DMA发送,后续会增加中断发送和阻塞发送模式的选择 - * 在短时间内连续调用此接口会导致上一次的发送未完成而新的发送取消,后续会增加一个发送状态的判断以及消息队列以解决这个问题 + * @note 在短时间内连续调用此接口,若采用IT/DMA会导致上一次的发送未完成而新的发送取消. + * @note 若希望连续使用DMA/IT进行发送,请配合USARTIsReady()使用,或自行为你的module实现一个发送队列和任务. + * @todo 是否考虑为USARTInstance增加发送队列以进行连续发送? * * @param _instance 串口实例 * @param send_buf 待发送数据的buffer @@ -55,4 +64,12 @@ USARTInstance *USARTRegister(USART_Init_Config_s *init_config); */ void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size,USART_TRANSFER_MODE mode); +/** + * @brief 判断串口是否准备好,用于连续或异步的IT/DMA发送 + * + * @param _instance 要判断的串口实例 + * @return uint8_t ready 1, busy 0 + */ +uint8_t USARTIsReady(USARTInstance *_instance); + #endif diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index 3bed315..11044dd 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -12,12 +12,15 @@ #include "bsp_usart.h" #include "usart.h" #include "seasky_protocol.h" +#include "daemon.h" +#include "bsp_log.h" static Vision_Recv_s recv_data; // @todo:由于后续需要进行IMU-Cam的硬件触发采集控制,因此可能需要将发送设置为定时任务,或由IMU采集完成产生的中断唤醒的任务, // 后者显然更nice,使得时间戳对齐. 因此,在send_data中设定其他的标志位数据,让ins_task填充姿态值. // static Vision_Send_s send_data; static USARTInstance *vision_usart_instance; +static DaemonInstance *vision_daemon_instance; /** * @brief 接收解包回调函数,将在bsp_usart.c中被usart rx callback调用 @@ -27,8 +30,23 @@ static USARTInstance *vision_usart_instance; static void DecodeVision() { static uint16_t flag_register; + DaemonReload(vision_daemon_instance); // 喂狗 get_protocol_info(vision_usart_instance->recv_buff, &flag_register, (uint8_t *)&recv_data.pitch); // TODO: code to resolve flag_register; + PrintLog("decode vision"); + +} + +/** + * @brief 离线回调函数,将在daemon.c中被daemon task调用 + * @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法 + * 进入接收中断.通过daemon判断数据更新,重新调用服务启动函数以解决此问题. + * + * @param id vision_usart_instance的地址,此处没用. + */ +static void VisionOfflineCallback(void *id) +{ + USARTServiceInit(vision_usart_instance); } /* 视觉通信初始化 */ @@ -38,8 +56,16 @@ Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle) conf.module_callback = DecodeVision; conf.recv_buff_size = VISION_RECV_SIZE; conf.usart_handle = _handle; - vision_usart_instance = USARTRegister(&conf); + + // 为master process注册daemon,用于判断视觉通信是否离线 + Daemon_Init_Config_s daemon_conf = { + .callback = VisionOfflineCallback, // 离线时调用的回调函数,会重启串口接收 + .owner_id = vision_usart_instance, + .reload_count = 10, + }; + vision_daemon_instance = DaemonRegister(&daemon_conf); + return &recv_data; } @@ -58,11 +84,11 @@ void VisionSend(Vision_Send_s *send) static uint8_t send_buff[VISION_SEND_SIZE]; static uint16_t tx_len; // TODO: code to set flag_register - + // 将数据转化为seasky协议的数据包 get_protocol_send_data(0x02, flag_register, &send->yaw, 3, send_buff, &tx_len); - USARTSend(vision_usart_instance, send_buff, tx_len,USART_TRANSFER_IT); // 和视觉通信使用IT,防止和接收使用的DMA冲突 + USARTSend(vision_usart_instance, send_buff, tx_len, USART_TRANSFER_DMA); // 和视觉通信使用IT,防止和接收使用的DMA冲突 // 此处为HAL设计的缺陷,DMASTOP会停止发送和接收,导致再也无法进入接收中断. // 也可在发送完成中断中重新启动DMA接收,但较为复杂.因此,此处使用IT发送. - + // 若使用了daemon,则也可以使用DMA发送. } \ No newline at end of file diff --git a/modules/motor/DJImotor/dji_motor.md b/modules/motor/DJImotor/dji_motor.md index bed1d82..bec0ba5 100644 --- a/modules/motor/DJImotor/dji_motor.md +++ b/modules/motor/DJImotor/dji_motor.md @@ -7,6 +7,10 @@ > 1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏 > 2. 为M2006和M3508增加开环的零位校准函数 +--- + +> 建议将电机的反馈频率通过RoboMaster Assistant统一设置为500Hz。当前默认的`MotorTask()`执行频率为500Hz，若不修改电机反馈频率可能导致单条总线挂载的电机数量有限，且容易出现帧错误和仲裁失败的情况。 + ## 总览和封装说明 > 如果你不需要理解该模块的工作原理，你只需要查看这一小节。 @@ -19,7 +23,7 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详 2. ==速度环为角速度，单位为**度/每秒**（deg/sec）== -3. ==电流环为mA== +3. ==电流环为A== 4. ==GM6020的输入设定为**力矩**，待测量（-30000~30000）== From eefc0883ed15c3f2fa8ad795469ec11a4b8e665f Mon Sep 17 00:00:00 2001 From: NeoZng Date: Sun, 19 Mar 2023 11:11:40 +0800 Subject: [PATCH 04/12] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E4=B8=8D?= =?UTF-8?q?=E5=90=8C=E7=AD=89=E7=BA=A7=E7=9A=84=E6=97=A5=E5=BF=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/launch.json | 6 +++++- .vscode/tasks.json | 3 +++ application/chassis/chassis.c | 1 + application/cmd/robot_cmd.c | 2 ++ application/gimbal/gimbal.c | 1 + application/shoot/shoot.c | 1 + bsp/can/bsp_can.c | 1 + bsp/log/bsp_log.c | 1 - bsp/log/bsp_log.h | 30 ++++++++++++++++++++++++------ bsp/usb/bsp_usb.c | 7 ++++--- modules/motor/DJImotor/dji_motor.c | 26 +++++++++++--------------- 11 files changed, 53 insertions(+), 26 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 989a8f1..9247755 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -19,8 +19,10 @@ "configFiles": [ "openocd_dap.cfg", // 配置文件已经在根目录提供,若要修改以此类推,openocd的路径下的share/scripts中有各种写好的配置文件 ], - "runToEntryPoint": "main" // 调试时在main函数入口停下 + "runToEntryPoint": "main", // 调试时在main函数入口停下 //"preLaunchTask": "build task",//先运行Build任务编译项目,取消注释即可使用 + //"preLaunchTask": "log", // 调试时同时开启RTT viewer窗口 + // 若想要在调试前编译并且打开log,可只使用log的prelaunch task并为log任务添加depends on依赖 }, // 使用j-link进行调试时的参考配置 { @@ -37,6 +39,8 @@ "svdFile": "STM32F407.svd", "rtos": "FreeRTOS", // "preLaunchTask": "build task",//先运行Build任务,取消注释即可使用 + //"preLaunchTask": "log", // 调试时同时开启RTT viewer窗口 + // 若想要在调试前编译并且打开log,可只使用log的prelaunch task并为log任务添加depends on依赖 }, ], } \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 46dbedf..d5bf982 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -36,6 +36,9 @@ "command":"JlinkRTTClient", "args": [], "problemMatcher": [], + // "dependsOn":[ + // "build task", // 可以添加多个. + // ] } ] } \ No newline at end of file diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 30a23d8..283cb24 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -17,6 +17,7 @@ #include "super_cap.h" #include "message_center.h" +// referee需要移动到module层 ///////////////////////// #include "referee.h" #include "rm_referee.h" diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index ebfac6b..d4fdab2 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -1,5 +1,7 @@ +// app #include "robot_def.h" #include "robot_cmd.h" +// module #include "remote_control.h" #include "ins_task.h" #include "master_process.h" diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 389ad64..95f37f1 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -1,5 +1,6 @@ #include "gimbal.h" #include "robot_def.h" + #include "dji_motor.h" #include "ins_task.h" #include "message_center.h" diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index f2ff607..4f50b7d 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -1,5 +1,6 @@ #include "shoot.h" #include "robot_def.h" + #include "dji_motor.h" #include "message_center.h" #include "bsp_dwt.h" diff --git a/bsp/can/bsp_can.c b/bsp/can/bsp_can.c index 5d8c945..36066d1 100644 --- a/bsp/can/bsp_can.c +++ b/bsp/can/bsp_can.c @@ -6,6 +6,7 @@ /* can instance ptrs storage, used for recv callback */ // 在CAN产生接收中断会遍历数组,选出hcan和rxid与发生中断的实例相同的那个,调用其回调函数 +// @todo: 后续为每个CAN总线单独添加一个can_instance指针数组,提高回调查找的性能 static CANInstance *can_instance[CAN_MX_REGISTER_CNT] = {NULL}; static uint8_t idx; // 全局CAN实例索引,每次有新的模块注册会自增 diff --git a/bsp/log/bsp_log.c b/bsp/log/bsp_log.c index 630733e..da35f0d 100644 --- a/bsp/log/bsp_log.c +++ b/bsp/log/bsp_log.c @@ -4,7 +4,6 @@ #include "SEGGER_RTT_Conf.h" #include -#define BUFFER_INDEX 0 void BSPLogInit() { diff --git a/bsp/log/bsp_log.h b/bsp/log/bsp_log.h index d4fa43c..d5f0b2a 100644 --- a/bsp/log/bsp_log.h +++ b/bsp/log/bsp_log.h @@ -1,6 +1,20 @@ #ifndef _BSP_LOG_H #define _BSP_LOG_H +#include "SEGGER_RTT.h" +#include "SEGGER_RTT_Conf.h" +#include + +#define BUFFER_INDEX 0 + + + +/** + * @brief 日志系统初始化 + * + */ +void BSPLogInit(); + /** * @brief 日志功能原型,供下面的LOGI,LOGW,LOGE等使用 * @@ -12,20 +26,24 @@ ##__VA_ARGS__, \ RTT_CTRL_RESET) +/*------下面是日志输出的接口--------*/ + /* 清屏 */ #define LOG_CLEAR() SEGGER_RTT_WriteString(0, " "RTT_CTRL_CLEAR) /* 无颜色日志输出 */ #define LOG(format,...) LOG_PROTO("","",format,##__VA_ARGS__) -/* 有颜色格式日志输出 */ -#define LOGINFO(format,...) LOG_PROTO("I: ", RTT_CTRL_TEXT_BRIGHT_GREEN , format, ##__VA_ARGS__) -#define LOGWARNING(format,...) LOG_PROTO("W: ", RTT_CTRL_TEXT_BRIGHT_YELLOW, format, ##__VA_ARGS__) -#define LOGERROR(format,...) LOG_PROTO("E: ", RTT_CTRL_TEXT_BRIGHT_RED , format, ##__VA_ARGS__) - +/* 有颜色格式日志输出,建议使用这些宏来输出日志 */ +// information level +#define LOGINFO(format,...) LOG_PROTO("I", RTT_CTRL_TEXT_BRIGHT_GREEN , format, ##__VA_ARGS__) +// warning level +#define LOGWARNING(format,...) LOG_PROTO("W", RTT_CTRL_TEXT_BRIGHT_YELLOW, format, ##__VA_ARGS__) +// error level +#define LOGERROR(format,...) LOG_PROTO("E", RTT_CTRL_TEXT_BRIGHT_RED , format, ##__VA_ARGS__) /** - * @brief 通过segger RTT打印日志,支持格式化输出,格式化输出的实现参考printf + * @brief 通过segger RTT打印日志,支持格式化输出,格式化输出的实现参考printf. * * @param fmt 格式字符串 * @param ... 参数列表 diff --git a/bsp/usb/bsp_usb.c b/bsp/usb/bsp_usb.c index aade217..f19c69b 100644 --- a/bsp/usb/bsp_usb.c +++ b/bsp/usb/bsp_usb.c @@ -11,20 +11,21 @@ #include "bsp_usb.h" -static uint8_t *bsp_usb_rx_buffer; // 接收到的数据会被放在这里,buffer size为2028 +static uint8_t *bsp_usb_rx_buffer; // 接收到的数据会被放在这里,buffer size为2048 // 注意usb单个数据包(Full speed模式下)最大为64byte,超出可能会出现丢包情况 // 这是传输完成的回调函数,在usbd_cdc_if.c中被调用 __weak void USBTransmitCpltCallback(uint32_t len) { - // 本次发送的数据 + // 本次发送的数据长度 UNUSED(len); // 传输完成会调用此函数,to do something } +// 这是接收回调函数 __weak void USBReceiveCpltCallback(uint32_t len) { - // 本次接收的数据 + // 本次接收的数据长度 UNUSED(len); // 传输完成会调用此函数,to do something } diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index 6ca6fba..6127358 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -1,5 +1,6 @@ #include "dji_motor.h" #include "general_def.h" +#include "bsp_log.h" static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用 @@ -32,16 +33,6 @@ static CANInstance sender_assignment[6] = { */ static uint8_t sender_enable_flag[6] = {0}; -/** - * @brief 当注册的电机id冲突时,会进入这个函数并提示冲突的ID - * @todo 通过segger jlink 发送日志 - */ -static void IDcrash_Handler(uint8_t conflict_motor_idx, uint8_t temp_motor_idx) -{ - while (1) - ; -} - /** * @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID, * 并对电机进行分组以便处理多电机控制命令 @@ -79,7 +70,10 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf for (size_t i = 0; i < idx; ++i) { if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) - IDcrash_Handler(i, idx); + { + LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information."); // 后续可以把id和CAN打印出来 + while (1); // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是) + } } break; @@ -103,7 +97,10 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf for (size_t i = 0; i < idx; ++i) { if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) - IDcrash_Handler(i, idx); + { + LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information."); + while (1); // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) + } } break; @@ -139,7 +136,7 @@ static void DecodeDJIMotor(CANInstance *_instance) CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5])); measure->temperate = rxbuff[6]; - // 多圈角度计算,前提是两次采样间电机转过的角度小于180°,高速转动时可能会出现问题,自己画个图就清楚计算过程了 + // 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了 if (measure->ecd - measure->last_ecd > 4096) measure->total_round--; else if (measure->ecd - measure->last_ecd < -4096) @@ -191,8 +188,7 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback } else { - while (1) - ; // LOOP TYPE ERROR!!!检查是否传入了正确的LOOP类型,或发生了指针越界 + LOGERROR("[dji_motor] loop type error, check memory access and func param");// 检查是否传入了正确的LOOP类型,或发生了指针越界 } } From 58db6e5705b5a16b8921e4e4e251731f1edb9ec6 Mon Sep 17 00:00:00 2001 From: chenfu <2412777093@qq.com> Date: Wed, 22 Mar 2023 16:37:12 +0800 Subject: [PATCH 05/12] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E7=94=B5=E6=9C=BA?= =?UTF-8?q?=E5=8F=8D=E9=A6=88=E7=9A=84bug,=E9=80=9A=E4=BF=A1bug,ps2?= =?UTF-8?q?=E6=89=8B=E6=9F=84=E6=94=AF=E6=8C=81,=E8=88=B5=E6=9C=BA?= =?UTF-8?q?=E7=9A=84=E4=BF=AE=E5=A4=8D,cmdbug=E7=9A=84=E4=BF=AE=E5=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/launch.json | 10 ++-- .vscode/tasks.json | 4 +- HAL_N_Middlewares/Src/freertos.c | 4 +- HAL_N_Middlewares/Src/main.c | 2 +- HAL_N_Middlewares/Src/usart.c | 2 +- Makefile | 2 + VSCode+Ozone使用方法.md | 2 +- application/chassis/chassis.c | 40 +++++++------- application/cmd/robot_cmd.c | 44 ++++++++++++---- application/gimbal/gimbal.c | 66 +++++++++++++----------- application/robot.c | 2 +- application/robot_def.h | 14 +++-- application/shoot/shoot.c | 56 +++++++++++--------- bsp/usart/bsp_usart.c | 39 ++++++++------ bsp/usart/bsp_usart.h | 15 ++---- modules/algorithm/controller.h | 2 + modules/imu/ins_task.c | 11 +++- modules/master_machine/master_process.c | 10 +++- modules/master_machine/master_process.h | 2 +- modules/master_machine/seasky_protocol.c | 17 ++++-- modules/motor/DJImotor/dji_motor.c | 6 +-- modules/motor/DJImotor/dji_motor.md | 10 ++-- modules/motor/HTmotor/HT04.c | 2 +- modules/motor/LKmotor/LK9025.c | 2 +- modules/motor/motor_def.h | 12 +++-- 25 files changed, 231 insertions(+), 145 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 440a688..58fc8ea 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -6,7 +6,7 @@ { "name": "Debug-DAP", "cwd": "${workspaceRoot}", - "executable": "${workspaceRoot}\\build\\${workspaceFolderBasename}.elf", // 要下载到调试器的文件,花括号中的是vscode两个预定义的参数 + "executable": "${workspaceRoot}\\build\\basic_framework.elf", // 要下载到调试器的文件,花括号中的是vscode两个预定义的参数 "request": "launch", "type": "cortex-debug", //使用J-link GDB Server时必须;其他GBD Server时可选（有可能帮助自动选择SVD文件） @@ -20,13 +20,14 @@ "openocd_dap.cfg", // 配置文件已经在根目录提供,若要修改以此类推,openocd的路径下的share/scripts中有各种写好的配置文件 ], "runToEntryPoint": "main" // 调试时在main函数入口停下 - //"preLaunchTask": "build task",//先运行Build任务编译项目,取消注释即可使用 + //"preLaunchTask": "log", // 调试时同时开启RTT viewer窗口 + // 若想要在调试前编译并且打开log,可只使用log的prelaunch task并为log任务添加depends on依赖 }, // 使用j-link进行调试时的参考配置 { "name": "Debug-Jlink", "cwd": "${workspaceFolder}", - "executable": "${workspaceRoot}\\build\\${workspaceFolderBasename}.elf", + "executable": "${workspaceRoot}\\build\\basic_framework.elf", "request": "launch", "type": "cortex-debug", "device": "STM32F407IG", @@ -35,7 +36,8 @@ "servertype": "jlink", "interface": "swd", "svdFile": "STM32F407.svd", - // "preLaunchTask": "build task",//先运行Build任务,取消注释即可使用 + //"preLaunchTask": "log", // 调试时同时开启RTT viewer窗口 + // 若想要在调试前编译并且打开log,可只使用log的prelaunch task并为log任务添加depends on依赖 }, ], } \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 7ef9fdf..9ff5211 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -15,7 +15,7 @@ { "label": "download dap", "type": "shell", // 如果希望在下载前编译,可以把command换成下面的命令 - "command":"mingw32-make download_dap", // "mingw32-make -j24 && mingw32-make download_dap", + "command":"mingw32-make -j24 ; mingw32-make download_dap", // "mingw32-make -j24 && mingw32-make download_dap", "group": { // 如果没有修改代码,编译任务不会消耗时间,因此推荐使用上面的替换. "kind": "build", "isDefault": false, @@ -24,7 +24,7 @@ { "label": "download jlink", "type": "shell", - "command":"mingw32-make download_jlink", // "mingw32-make -j24 && mingw32-make download_dap" + "command":"mingw32-make -j24 ; mingw32-make download_jlink", // "mingw32-make -j24 && mingw32-make download_dap" "group": { "kind": "build", "isDefault": false, diff --git a/HAL_N_Middlewares/Src/freertos.c b/HAL_N_Middlewares/Src/freertos.c index 99e9a51..a7799d2 100644 --- a/HAL_N_Middlewares/Src/freertos.c +++ b/HAL_N_Middlewares/Src/freertos.c @@ -30,7 +30,7 @@ #include "led_task.h" #include "daemon.h" #include "robot.h" - +#include "ps_handle.h" /* USER CODE END Includes */ @@ -133,7 +133,7 @@ void MX_FREERTOS_Init(void) { /* USER CODE BEGIN RTOS_THREADS */ /* add threads, ... */ /* USER CODE END RTOS_THREADS */ - + } /* USER CODE BEGIN Header_StartDefaultTask */ diff --git a/HAL_N_Middlewares/Src/main.c b/HAL_N_Middlewares/Src/main.c index 535e8c4..df310d0 100644 --- a/HAL_N_Middlewares/Src/main.c +++ b/HAL_N_Middlewares/Src/main.c @@ -83,7 +83,7 @@ int main(void) /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ - HAL_Init(); + HAL_Init(); /* USER CODE BEGIN Init */ diff --git a/HAL_N_Middlewares/Src/usart.c b/HAL_N_Middlewares/Src/usart.c index 8595e32..5d513a2 100644 --- a/HAL_N_Middlewares/Src/usart.c +++ b/HAL_N_Middlewares/Src/usart.c @@ -46,7 +46,7 @@ void MX_USART1_UART_Init(void) /* USER CODE END USART1_Init 1 */ huart1.Instance = USART1; - huart1.Init.BaudRate = 115200; + huart1.Init.BaudRate = 921600; huart1.Init.WordLength = UART_WORDLENGTH_8B; huart1.Init.StopBits = UART_STOPBITS_1; huart1.Init.Parity = UART_PARITY_NONE; diff --git a/Makefile b/Makefile index dc73895..1139182 100644 --- a/Makefile +++ b/Makefile @@ -141,6 +141,7 @@ modules/can_comm/can_comm.c \ modules/message_center/message_center.c \ modules/daemon/daemon.c \ modules/vofa/vofa.c \ +modules/ps_handle/ps_handle.c \ application/gimbal/gimbal.c \ application/chassis/chassis.c \ application/shoot/shoot.c \ @@ -260,6 +261,7 @@ C_INCLUDES = \ -Imodules/message_center \ -Imodules/daemon \ -Imodules/vofa \ +-Imodules/ps_handle \ -Imodules # compile gcc flags diff --git a/VSCode+Ozone使用方法.md b/VSCode+Ozone使用方法.md index a9f241c..1f90a70 100644 --- a/VSCode+Ozone使用方法.md +++ b/VSCode+Ozone使用方法.md @@ -595,7 +595,7 @@ Project.SetOSPlugin(“plugin_name”) 2. 变量watch窗口，这里的变量不会实时更新，只有在暂停或遇到断点的时候才会更新。若希望实时查看，在这里右键选择需要动态查看的变量，选择Graph，他就会出现在**窗口8**的位置。 - 如果不需要可视化查看变量变化的趋势，但是想不赞同查看变量的值，请右键点击变量，选择一个合适的refresh rate： + 如果不需要可视化查看变量变化的趋势，但是想不暂停查看变量的值，请右键点击变量，选择一个合适的refresh rate： ![image-20221119173731119](assets/image-20221119173731119.png) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 62d2810..93e4a6e 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -62,16 +62,20 @@ void ChassisInit() .can_init_config.can_handle = &hcan1, .controller_param_init_config = { .speed_PID = { - .Kp = 10, - .Ki = 0, - .Kd = 0, - .MaxOut = 4000, + .Kp = 4.5,//9 + .Ki = 0,//0.02 + .Kd = 0.01,//0.01 + .IntegralLimit = 3000, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .MaxOut = 12000, }, .current_PID = { - .Kp = 1, - .Ki = 0, + .Kp = 0.4,//0.7 + .Ki = 0,//0.1 .Kd = 0, - .MaxOut = 4000, + .IntegralLimit = 3000, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .MaxOut = 15000, }, }, .controller_setting_init_config = { @@ -83,20 +87,20 @@ void ChassisInit() .motor_type = M3508, }; // @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改. - chassis_motor_config.can_init_config.tx_id = 4; - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 1; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_lf = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 3, - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 2; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_rf = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 1, - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 4; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_lb = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 2, - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 3; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_rb = DJIMotorInit(&chassis_motor_config); referee_data = RefereeInit(&huart6); // 裁判系统初始化 @@ -143,7 +147,7 @@ static void MecanumCalculate() { vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER; vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER; - vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER; + vt_lb = chassis_vx - chassis_vy -chassis_cmd_recv.wz * LB_CENTER; vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER; } @@ -210,10 +214,10 @@ void ChassisTask() chassis_cmd_recv.wz = 0; break; case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 - chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); + chassis_cmd_recv.wz = -1.5*chassis_cmd_recv.offset_angle*abs(chassis_cmd_recv.offset_angle); break; case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 - // chassis_cmd_recv.wz=sin(t) + chassis_cmd_recv.wz=4000; break; default: break; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index ebfac6b..da3cd06 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -66,6 +66,7 @@ void RobotCMDInit() }; cmd_can_comm = CANCommInit(&comm_conf); #endif // GIMBAL_BOARD +gimbal_cmd_send.pitch = 0; robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 } @@ -97,6 +98,17 @@ static void CalcOffsetAngle() #endif } +static void OffsetAnglePidCalc() +{ + // float pid_measure,pid_ref; + // static PIDInstance AngleCal = { + // .Kp = -1, + // .Ki = 0, + // .Kd = 0, + // .MaxOut = 10000, + // }; + // chassis_cmd_send.offset_angle = PIDCalculate(&AngleCal,chassis_cmd_send.offset_angle,0); +} /** * @brief 控制输入为遥控器(调试时)的模式和控制量设置 * @@ -105,9 +117,11 @@ static void RemoteControlSet() { // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台 - chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; + {chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;} else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 - chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; + {chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; + gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;} // 云台参数,确定云台控制数据 if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式 @@ -118,14 +132,23 @@ static void RemoteControlSet() // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制 if (switch_is_down(rc_data[TEMP].rc.switch_left) || vision_recv_data->target_state == NO_TARGET) { // 按照摇杆的输出大小进行角度增量,增益系数需调整 - gimbal_cmd_send.yaw += 0.0015f * (float)rc_data[TEMP].rc.rocker_l_; - gimbal_cmd_send.pitch += 0.002f * (float)rc_data[TEMP].rc.rocker_l1; - gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; + gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_; + gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1; + // if (gimbal_cmd_send.pitch <= 122) + // { + // gimbal_cmd_send.pitch =125; + // } + // else if (gimbal_cmd_send.pitch >= 175) + // { + // gimbal_cmd_send.pitch = 174; + // } + } // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向 chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向 + //chassis_cmd_send.wz = 300; // 发射参数 if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 @@ -144,7 +167,7 @@ static void RemoteControlSet() else shoot_cmd_send.load_mode = LOAD_STOP; // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频, - shoot_cmd_send.shoot_rate = 1; + shoot_cmd_send.shoot_rate = 8; } /** @@ -173,6 +196,8 @@ static void EmergencyHandler() 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; } // 遥控器右侧开关为[上],恢复正常运行 if (switch_is_up(rc_data[TEMP].rc.switch_right)) @@ -197,7 +222,6 @@ void RobotCMDTask() // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 CalcOffsetAngle(); - // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制 RemoteControlSet(); @@ -207,11 +231,11 @@ void RobotCMDTask() EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 // 设置视觉发送数据,还需增加加速度和角速度数据 - vision_send_data.bullet_speed = chassis_fetch_data.bullet_speed; - vision_send_data.enemy_color = chassis_fetch_data.enemy_color; + vision_send_data.bullet_speed = 15; + vision_send_data.enemy_color = 0; vision_send_data.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; vision_send_data.yaw = gimbal_fetch_data.gimbal_imu_data.Yaw; - vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll; + vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll;; // 推送消息,双板通信,视觉通信等 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 5798ce5..99769be 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -54,7 +54,7 @@ void GimbalInit() .cali_mode=BMI088_CALIBRATE_ONLINE_MODE, .work_mode=BMI088_BLOCK_PERIODIC_MODE, }; - // imu=BMI088Register(&imu_config); + // imu=BMI088Register(&imu_config); gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW Motor_Init_Config_s yaw_config = { @@ -64,60 +64,68 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 20, + .Kp = 8, //8 .Ki = 0, .Kd = 0, - .MaxOut = 2000, - .DeadBand = 0.3, + .DeadBand = 0.1, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .IntegralLimit = 100, + + .MaxOut = 500, }, .speed_PID = { - .Kp = 10, - .Ki = 0, + .Kp = 50,//40 + .Ki = 200, .Kd = 0, - .MaxOut = 4000, + .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement, + .IntegralLimit = 3000, + .MaxOut = 20000, }, .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - // .other_speed_feedback_ptr=&gimba_IMU_data.wz; + .other_speed_feedback_ptr=&gimba_IMU_data->Gyro[2], }, .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, + .angle_feedback_source = OTHER_FEED, + .speed_feedback_source = OTHER_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .reverse_flag = MOTOR_DIRECTION_NORMAL, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = GM6020}; // PITCH Motor_Init_Config_s pitch_config = { .can_init_config = { - .can_handle = &hcan1, + .can_handle = &hcan2, .tx_id = 2, }, .controller_param_init_config = { .angle_PID = { - .Kp = 30, + .Kp =10,//10 .Ki = 0, .Kd = 0, - .MaxOut = 4000, - .DeadBand = 0.3, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .IntegralLimit =100, + .MaxOut = 500, }, .speed_PID = { - .Kp = 10, - .Ki = 0, - .Kd = 0, - .MaxOut = 4000, + .Kp=50,//50 + .Ki =350,//350 + .Kd =0,//0.1 + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .IntegralLimit =2500, + .MaxOut = 20000, }, .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - // .other_speed_feedback_ptr=&gimba_IMU_data.wy, + .other_speed_feedback_ptr=(&gimba_IMU_data->Gyro[0]), }, .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, + .angle_feedback_source = OTHER_FEED, + .speed_feedback_source = OTHER_FEED, .outer_loop_type = ANGLE_LOOP, - .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .reverse_flag = MOTOR_DIRECTION_NORMAL, + .close_loop_type = SPEED_LOOP | ANGLE_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = GM6020, }; @@ -145,7 +153,7 @@ void GimbalTask() DJIMotorStop(pitch_motor); break; // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 - case GIMBAL_GYRO_MODE: // 后续只保留此模式 + case GIMBAL_GYRO_MODE: // 后续只保留此模式 DJIMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); @@ -159,10 +167,10 @@ void GimbalTask() case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) DJIMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, MOTOR_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, MOTOR_FEED); - DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, MOTOR_FEED); - DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, MOTOR_FEED); + 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; diff --git a/application/robot.c b/application/robot.c index 72b014c..fb24f9f 100644 --- a/application/robot.c +++ b/application/robot.c @@ -34,7 +34,7 @@ void RobotInit() #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - ChassisInit(); + ChassisInit(); #endif // 初始化完成,开启中断 __enable_irq(); diff --git a/application/robot_def.h b/application/robot_def.h index 2758616..f1532f7 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -34,21 +34,25 @@ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 -#define YAW_CHASSIS_ALIGN_ECD 4000 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 +#define YAW_CHASSIS_ALIGN_ECD 2711 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 -#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 +#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 // 发射参数 -#define ONE_BULLET_DELTA_ANGLE 0 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 +#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f -#define NUM_PER_CIRCLE 1 // 拨盘一圈的装载量 +#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量 // 机器人底盘修改的参数,单位为mm(毫米) -#define WHEEL_BASE 300 // 纵向轴距(前进后退方向) +#define WHEEL_BASE 350 // 纵向轴距(前进后退方向) #define TRACK_WIDTH 300 // 横向轮距(左右平移方向) #define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0 #define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0 #define RADIUS_WHEEL 60 // 轮子半径 #define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换 +#define GYRO2GIMBAL_DIR_YAW 1 //陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反 +#define GYRO2GIMBAL_DIR_PITCH 1 //陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反 +#define GYRO2GIMBAL_DIR_ROLL 1 //陀螺仪数据相较于云台的roll的方向,1为相同,-1为相反 + // 检查是否出现主控板定义冲突,只允许一个开发板定义存在,否则编译会自动报错 #if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \ (defined(ONE_BOARD) && defined(GIMBAL_BOARD)) || \ diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index f2ff607..c9cb2e3 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -25,20 +25,23 @@ void ShootInit() Motor_Init_Config_s friction_config = { .can_init_config = { .can_handle = &hcan2, - .tx_id = 6, }, .controller_param_init_config = { .speed_PID = { - .Kp = 10, - .Ki = 0, + .Kp = 0,//20 + .Ki = 0,//1 .Kd = 0, - .MaxOut = 2000, + .Improve = PID_Integral_Limit, + .IntegralLimit = 10000, + .MaxOut = 15000, }, .current_PID = { - .Kp = 1, - .Ki = 0, + .Kp = 0,//0.7 + .Ki = 0,//0.1 .Kd = 0, - .MaxOut = 2000, + .Improve = PID_Integral_Limit, + .IntegralLimit = 10000, + .MaxOut = 15000, }, }, .controller_setting_init_config = { @@ -47,47 +50,52 @@ void ShootInit() .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, - .reverse_flag = MOTOR_DIRECTION_NORMAL, + .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 = 5; // 右摩擦轮,改txid和方向就行 - friction_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + 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 = &hcan2, - .tx_id = 7, + .tx_id = 3, }, .controller_param_init_config = { .angle_PID = { // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降 - .Kp = 10, + .Kp = 0,//10 .Ki = 0, .Kd = 0, .MaxOut = 200, }, .speed_PID = { - .Kp = 1, - .Ki = 0, + .Kp = 0,//10 + .Ki = 0,//1 .Kd = 0, - .MaxOut = 2000, + .Improve = PID_Integral_Limit, + .IntegralLimit = 5000, + .MaxOut = 5000, }, .current_PID = { - .Kp = 1, - .Ki = 0, + .Kp = 0,//0.7 + .Ki = 0,//0.1 .Kd = 0, - .MaxOut = 3000, + .Improve = PID_Integral_Limit, + .IntegralLimit = 5000, + .MaxOut = 5000, }, }, .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转 - .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向 + .close_loop_type = CURRENT_LOOP | SPEED_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向 }, .motor_type = M2006 // 英雄使用m3508 }; @@ -119,8 +127,8 @@ void ShootTask() // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可 // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发) - if (hibernate_time + dead_time > DWT_GetTimeline_ms()) - return; + // if (hibernate_time + dead_time > DWT_GetTimeline_ms()) + // return; // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换 switch (shoot_cmd_recv.load_mode) @@ -181,8 +189,8 @@ void ShootTask() DJIMotorSetRef(friction_r, 0); break; default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速. - DJIMotorSetRef(friction_l, 1000); - DJIMotorSetRef(friction_r, 1000); + DJIMotorSetRef(friction_l, 30000); + DJIMotorSetRef(friction_r, 30000); break; } } diff --git a/bsp/usart/bsp_usart.c b/bsp/usart/bsp_usart.c index 0e26191..c9edaee 100644 --- a/bsp/usart/bsp_usart.c +++ b/bsp/usart/bsp_usart.c @@ -19,7 +19,7 @@ static USARTInstance *usart_instance[DEVICE_USART_CNT] = {NULL}; /** * @brief 启动串口服务,会在每个实例注册之后自动启用接收,当前实现为DMA接收,后续可能添加IT和BLOCKING接收 - * + * * @todo 串口服务会在每个实例注册之后自动启用接收,当前实现为DMA接收,后续可能添加IT和BLOCKING接收 * 可能还要将此函数修改为extern,使得module可以控制串口的启停 * @@ -52,28 +52,37 @@ USARTInstance *USARTRegister(USART_Init_Config_s *init_config) } /* @todo 当前仅进行了形式上的封装,后续要进一步考虑是否将module的行为与bsp完全分离 */ -void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size) -{ - HAL_UART_Transmit_DMA(_instance->usart_handle, send_buf, send_size); -} - -void USARTAbort(USARTInstance *_instance, USART_TRANSFER_MODE mode) +void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size, USART_TRANSFER_MODE mode) { switch (mode) { - case USART_TRANSFER_TX: - // if(_instance.work_mode == USART_TX_DMA) - HAL_UART_AbortTransmit_IT(_instance->usart_handle); + case USART_TRANSFER_BLOCKING: + HAL_UART_Transmit(_instance->usart_handle, send_buf, send_size, 100); break; - case USART_TRANSFER_RX: - // if(_instance.work_mode == USART_RX_DMA) - HAL_UART_AbortReceive_IT(_instance->usart_handle); + case USART_TRANSFER_IT: + HAL_UART_Transmit_IT(_instance->usart_handle, send_buf, send_size); break; - case USART_TRANSFER_NONE: + case USART_TRANSFER_DMA: + HAL_UART_Transmit_DMA(_instance->usart_handle, send_buf, send_size); + break; + default: + while (1) + ; // illegal mode! check your code context! break; } } - +/* 串口发送时,gstate会被设为BUSY_TX */ +uint8_t USARTIsReady(USARTInstance *_instance) +{ + if(_instance->usart_handle->gState | HAL_UART_STATE_BUSY_TX) + { + return 0; + } + else + { + return 1; + } +} /** * @brief 每次dma/idle中断发生时，都会调用此函数.对于每个uart实例会调用对应的回调进行进一步的处理 * 例如:视觉协议解析/遥控器解析/裁判系统解析 diff --git a/bsp/usart/bsp_usart.h b/bsp/usart/bsp_usart.h index 34e74db..e7e9dcf 100644 --- a/bsp/usart/bsp_usart.h +++ b/bsp/usart/bsp_usart.h @@ -13,8 +13,9 @@ typedef void (*usart_module_callback)(); typedef enum { USART_TRANSFER_NONE=0, - USART_TRANSFER_TX, - USART_TRANSFER_RX, + USART_TRANSFER_BLOCKING, + USART_TRANSFER_IT, + USART_TRANSFER_DMA, } USART_TRANSFER_MODE; // 串口实例结构体,每个module都要包含一个实例. @@ -52,14 +53,6 @@ USARTInstance *USARTRegister(USART_Init_Config_s *init_config); * @param send_buf 待发送数据的buffer * @param send_size how many bytes to send */ -void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size); - -/** - * @brief 通过调用该函数终止串口的发送或者接收,通过传入mode参数来选择终止发送还是接收 - * - * @param _instance 串口实例 - * @param mode 选择终止发送还是接收的模式 - */ -void USARTAbort(USARTInstance *_instance,USART_TRANSFER_MODE mode); +void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size,USART_TRANSFER_MODE mode); #endif diff --git a/modules/algorithm/controller.h b/modules/algorithm/controller.h index 38b7fb1..d9c860f 100644 --- a/modules/algorithm/controller.h +++ b/modules/algorithm/controller.h @@ -113,6 +113,8 @@ typedef struct // config parameter float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|recv_buff, &flag_register, (uint8_t *)&recv_data.pitch); + // TODO: code to resolve flag_register; } @@ -58,8 +59,13 @@ void VisionSend(Vision_Send_s *send) static uint8_t send_buff[VISION_SEND_SIZE]; static uint16_t tx_len; // TODO: code to set flag_register - + flag_register = 0x0001; // 将数据转化为seasky协议的数据包 get_protocol_send_data(0x02, flag_register, &send->yaw, 3, send_buff, &tx_len); - USARTSend(vision_usart_instance, send_buff, tx_len); + USARTSend(vision_usart_instance, send_buff, tx_len,USART_TRANSFER_IT); + // if(vision_usart_instance->usart_handle->ReceptionType == HAL_UART_RECEPTION_TOIDLE) + // {HAL_UARTEx_ReceiveToIdle_DMA(vision_usart_instance->usart_handle, vision_usart_instance->recv_buff, vision_usart_instance->recv_buff_size); + // __HAL_DMA_DISABLE_IT(vision_usart_instance->usart_handle->hdmarx, DMA_IT_HT); + + // } } \ No newline at end of file diff --git a/modules/master_machine/master_process.h b/modules/master_machine/master_process.h index 224faa3..e23cf7a 100644 --- a/modules/master_machine/master_process.h +++ b/modules/master_machine/master_process.h @@ -4,7 +4,7 @@ #include "bsp_usart.h" #include "seasky_protocol.h" -#define VISION_RECV_SIZE 36u // 当前为固定值,36字节 +#define VISION_RECV_SIZE 18u // 当前为固定值,36字节 #define VISION_SEND_SIZE 36u #pragma pack(1) diff --git a/modules/master_machine/seasky_protocol.c b/modules/master_machine/seasky_protocol.c index 105a6b8..18502f7 100644 --- a/modules/master_machine/seasky_protocol.c +++ b/modules/master_machine/seasky_protocol.c @@ -15,6 +15,11 @@ #include "crc16.h" #include "memory.h" +/*获取CRC8校验码*/ +uint8_t Get_CRC8_Check(uint8_t *pchMessage,uint16_t dwLength) +{ + return crc_8(pchMessage,dwLength); +} /*检验CRC8数据段*/ static uint8_t CRC8_Check_Sum(uint8_t *pchMessage, uint16_t dwLength) { @@ -25,6 +30,12 @@ static uint8_t CRC8_Check_Sum(uint8_t *pchMessage, uint16_t dwLength) return (ucExpected == pchMessage[dwLength - 1]); } +/*获取CRC16校验码*/ +uint16_t Get_CRC16_Check(uint8_t *pchMessage,uint32_t dwLength) +{ + return crc_16(pchMessage,dwLength); +} + /*检验CRC16数据段*/ static uint16_t CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength) { @@ -45,7 +56,7 @@ static uint8_t protocol_heade_Check(protocol_rm_struct *pro, uint8_t *rx_buf) pro->header.sof = rx_buf[0]; if (CRC8_Check_Sum(&rx_buf[0], 4)) { - pro->header.data_length = (rx_buf[2] << 8) | rx_buf[1]; + pro->header.data_length = ((rx_buf[2] << 8) | rx_buf[1]); pro->header.crc_check = rx_buf[3]; pro->cmd_id = (rx_buf[5] << 8) | rx_buf[4]; return 1; @@ -114,9 +125,9 @@ uint16_t get_protocol_info(uint8_t *rx_buf, // 接收到的原始数据 if (CRC16_Check_Sum(&rx_buf[0], date_length)) { *flags_register = (rx_buf[7] << 8) | rx_buf[6]; - memcpy(rx_data, rx_buf + 8, 4 * sizeof(pro.header.data_length - 2)); + memcpy(rx_data, rx_buf + 8, (pro.header.data_length - 2)); return pro.cmd_id; - } + } } return 0; } diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index 36b6641..06952d8 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -238,7 +238,8 @@ void DJIMotorControl() motor_controller = &motor->motor_controller; motor_measure = &motor->motor_measure; pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改 - + if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) + pid_ref*= -1; // 设置反转 // pid_ref会顺次通过被启用的闭环充当数据的载体 // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出 if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP) @@ -270,8 +271,7 @@ void DJIMotorControl() // 获取最终输出 set = (int16_t)pid_ref; - if (motor_setting->reverse_flag == MOTOR_DIRECTION_REVERSE) - set *= -1; // 设置反转 + // 分组填入发送数据 group = motor->sender_group; diff --git a/modules/motor/DJImotor/dji_motor.md b/modules/motor/DJImotor/dji_motor.md index bed1d82..7c936e7 100644 --- a/modules/motor/DJImotor/dji_motor.md +++ b/modules/motor/DJImotor/dji_motor.md @@ -138,7 +138,7 @@ Motor_Init_Config_s config = { .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, .speed_feedback_source = MOTOR_FEED, - .reverse_flag = MOTOR_DIRECTION_NORMAL}, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL}, // 电流环和速度环PID参数的设置,不采用计算优化则不需要传入Improve参数 // 不使用其他数据来源(如IMU),不需要传入反馈数据变量指针 .controller_param_init_config = {.current_PID = {.Improve = 0, @@ -241,7 +241,7 @@ typedef struct { Closeloop_Type_e outer_loop_type; Closeloop_Type_e close_loop_type; - Reverse_Flag_e reverse_flag; + Motor_Reverse_Flag_e motor_reverse_flag; Feedback_Source_e angle_feedback_source; Feedback_Source_e speed_feedback_source; } Motor_Control_Setting_s; @@ -268,14 +268,14 @@ typedef struct > 注意，务必分清串级控制（多环）和外层闭环的区别。前者是为了提高内环的性能，使得其能更好地跟随外环参考值；而后者描述的是系统真实的控制目标（闭环目标）。如3508，没有电流环仍然可以对速度完成闭环，对于高层的应用来说，它们本质上不关心电机内部是否还有电流环，它们只把外层闭环为速度的电机当作一个**速度伺服执行器**，**外层闭环**描述的就是真正的闭环目标。 - - 为了避开恼人的正负号，提高代码的可维护性，在初始化电机时设定`reverse_flag`使得所有电机都按照你想要的方向旋转，其定义如下： + - 为了避开恼人的正负号，提高代码的可维护性，在初始化电机时设定`motor_reverse_flag`使得所有电机都按照你想要的方向旋转，其定义如下： ```c typedef enum { MOTOR_DIRECTION_NORMAL = 0, MOTOR_DIRECTION_REVERSE = 1 - } Reverse_Flag_e; + } Motor_Reverse_Flag_e; ``` - `speed_feedback_source`以及`angle_feedback_source`是指示电机反馈来源的标志位。一般情况下电机使用自身的编码器作为控制反馈量。但在某些时候，如小陀螺模式，云台电机会使用IMU的姿态数据作为反馈数据来源。其定义如下： @@ -433,7 +433,7 @@ Motor_Init_Config_s config = { .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .speed_feedback_source = MOTOR_FEED, - .reverse_flag = MOTOR_DIRECTION_NORMAL + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL }, .controller_param_init_config = { .angle_PID = { diff --git a/modules/motor/HTmotor/HT04.c b/modules/motor/HTmotor/HT04.c index 13ec46a..38a00ea 100644 --- a/modules/motor/HTmotor/HT04.c +++ b/modules/motor/HTmotor/HT04.c @@ -137,7 +137,7 @@ void HTMotorControl() } set = pid_ref; - if (setting->reverse_flag == MOTOR_DIRECTION_REVERSE) + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) set *= -1; LIMIT_MIN_MAX(set, T_MIN, T_MAX); // 限幅,实际上这似乎和pid输出限幅重复了 diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c index 01af445..24e1795 100644 --- a/modules/motor/LKmotor/LK9025.c +++ b/modules/motor/LKmotor/LK9025.c @@ -110,7 +110,7 @@ void LKMotorControl() } set = pid_ref; - if (setting->reverse_flag == MOTOR_DIRECTION_REVERSE) + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) set *= -1; // 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t)); diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index ed16fec..e3cfa28 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -54,8 +54,13 @@ typedef enum { MOTOR_DIRECTION_NORMAL = 0, MOTOR_DIRECTION_REVERSE = 1 -} Reverse_Flag_e; - +} Motor_Reverse_Flag_e; +/* 反馈量正反标志 */ +typedef enum +{ + FEEDBACK_DIRECTION_NORMAL = 0, + FEEDBACK_DIRECTION_REVERSE = 1 +} Feedback_Reverse_Flag_e; typedef enum { MOTOR_STOP = 0, @@ -67,7 +72,8 @@ typedef struct { Closeloop_Type_e outer_loop_type; // 最外层的闭环,未设置时默认为最高级的闭环 Closeloop_Type_e close_loop_type; // 使用几个闭环(串级) - Reverse_Flag_e reverse_flag; // 是否反转 + Motor_Reverse_Flag_e motor_reverse_flag; // 是否反转 + Feedback_Reverse_Flag_e feedback_reverse_flag; // 反馈是否反向 Feedback_Source_e angle_feedback_source; // 角度反馈类型 Feedback_Source_e speed_feedback_source; // 速度反馈类型 Feedfoward_Type_e feedforward_flag; // 前馈标志 From 41d033e3f8c487abc1e6dfec38fdaf0b9bc6cc0d Mon Sep 17 00:00:00 2001 From: NeoZng Date: Thu, 23 Mar 2023 18:22:24 +0800 Subject: [PATCH 06/12] =?UTF-8?q?=E4=BF=AE=E5=A4=8DCAN=E4=B8=AD=E6=96=AD?= =?UTF-8?q?=E4=BC=98=E5=85=88=E7=BA=A7=E5=AF=BC=E8=87=B4=E5=87=BD=E6=95=B0?= =?UTF-8?q?=E9=87=8D=E5=85=A5=E8=AE=BF=E9=97=AEstatic=E5=8F=98=E9=87=8F?= =?UTF-8?q?=E7=9A=84=E9=97=AE=E9=A2=98=EF=BC=8C=E6=8F=90=E5=8D=87=E4=BA=86?= =?UTF-8?q?bsp=E5=92=8C=E9=83=A8=E5=88=86module=E7=9A=84=E6=80=A7=E8=83=BD?= =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Bug_Report.md | 20 +++++++++++++++- HAL_N_Middlewares/Src/can.c | 4 ++-- VSCode+Ozone使用方法.md | 2 +- basic_framework.ioc | 6 ++--- bsp/can/bsp_can.c | 4 ++-- bsp/gpio/bsp_gpio.c | 2 +- modules/can_comm/can_comm.c | 3 +-- modules/daemon/daemon.c | 2 +- modules/ist8310/ist8310.c | 5 ++-- modules/master_machine/master_process.c | 8 +++---- modules/motor/DJImotor/dji_motor.c | 32 ++++++++++++------------- modules/motor/HTmotor/HT04.c | 21 +++++++--------- modules/motor/LKmotor/LK9025.c | 14 +++++------ modules/motor/servo_motor/servo_motor.c | 2 +- modules/remote/remote_control.c | 12 +++++----- modules/super_cap/super_cap.c | 4 ++-- 16 files changed, 76 insertions(+), 65 deletions(-) diff --git a/Bug_Report.md b/Bug_Report.md index a517636..ee0936c 100644 --- a/Bug_Report.md +++ b/Bug_Report.md @@ -48,4 +48,22 @@ ### 紧急程度 -⭐⭐⭐⭐⭐ \ No newline at end of file +⭐⭐⭐⭐⭐ + +## 总线挂载多个电机后,pitch和yaw的GM6020电机出现编码器反馈值跳动 + +> 已修复，详细信息见“如何定位bug.md” + +CAN1总线挂载5个电机,4\*3508+1\*6020,控制报文发送频率为500Hz,电机的反馈频率皆为1kHz.云台在控制时会出现突然跳动.添加到Ozone graph查看发现ECD(编码器)值在静止状态下也会出现突然抖动,并且幅度超过4000.但不会出现超过编码器反馈值范围的值. + +### 尝试解决的方案 + +若使用单个6020电机,不会出现此问题. 曾认为是指针越界导致`motor_measure->ecd`值被修改, 需要进一步观察其他反馈值是否出现问题. 且反馈值始终在编码器范围之内. + +### 如何复现问题 + +同时启用CAN1和CAN2，并在单条CAN总线上挂载超过5个电机. + +### 紧急程度 + +⭐⭐⭐ \ No newline at end of file diff --git a/HAL_N_Middlewares/Src/can.c b/HAL_N_Middlewares/Src/can.c index c4c0948..292573f 100644 --- a/HAL_N_Middlewares/Src/can.c +++ b/HAL_N_Middlewares/Src/can.c @@ -122,7 +122,7 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle) HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); /* CAN1 interrupt Init */ - HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 6, 0); + HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0); HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 5, 0); HAL_NVIC_EnableIRQ(CAN1_RX1_IRQn); @@ -155,7 +155,7 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle) HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* CAN2 interrupt Init */ - HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 6, 0); + HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0); HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn); HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0); HAL_NVIC_EnableIRQ(CAN2_RX1_IRQn); diff --git a/VSCode+Ozone使用方法.md b/VSCode+Ozone使用方法.md index 17a8c1d..c3f35dd 100644 --- a/VSCode+Ozone使用方法.md +++ b/VSCode+Ozone使用方法.md @@ -659,7 +659,7 @@ Project.SetOSPlugin(“plugin_name”) 在Terminal窗口查看，还可以通过命令直接控制单片机的运行（不过不常用）。 -未打开窗口则在view-> terminal中打开。 +未打开窗口则在view-> terminal中打开。使用bsp_log打印的日志会输出到该窗口中. - **外设查看** diff --git a/basic_framework.ioc b/basic_framework.ioc index 6c9ab7e..28060eb 100644 --- a/basic_framework.ioc +++ b/basic_framework.ioc @@ -276,9 +276,9 @@ Mcu.UserName=STM32F407IGHx MxCube.Version=6.7.0 MxDb.Version=DB.6.0.70 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false -NVIC.CAN1_RX0_IRQn=true\:6\:0\:true\:false\:true\:true\:true\:true\:true +NVIC.CAN1_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true -NVIC.CAN2_RX0_IRQn=true\:6\:0\:true\:false\:true\:true\:true\:true\:true +NVIC.CAN2_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true NVIC.CAN2_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true NVIC.DMA1_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true @@ -432,7 +432,7 @@ PF0.Mode=I2C PF0.Signal=I2C2_SDA PF1.Mode=I2C PF1.Signal=I2C2_SCL -PF6.GPIOParameters=GPIO_Label,GPIO_Speed,GPIO_PuPd +PF6.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label PF6.GPIO_Label=IMU_TEMP PF6.GPIO_PuPd=GPIO_PULLUP PF6.GPIO_Speed=GPIO_SPEED_FREQ_HIGH diff --git a/bsp/can/bsp_can.c b/bsp/can/bsp_can.c index 36066d1..0b73035 100644 --- a/bsp/can/bsp_can.c +++ b/bsp/can/bsp_can.c @@ -131,9 +131,9 @@ void CANSetDLC(CANInstance *_instance, uint8_t length) */ static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox) { - static uint8_t can_rx_buff[8]; // 用于保存接收到的数据,static是为了减少栈空间占用,避免重复分配 static CAN_RxHeaderTypeDef rxconf; // 同上 - + uint8_t can_rx_buff[8]; + HAL_CAN_GetRxMessage(_hcan, fifox, &rxconf, can_rx_buff); // 从FIFO中获取数据 for (size_t i = 0; i < idx; ++i) { // 两者相等说明这是要找的实例 diff --git a/bsp/gpio/bsp_gpio.c b/bsp/gpio/bsp_gpio.c index a871709..970b81f 100644 --- a/bsp/gpio/bsp_gpio.c +++ b/bsp/gpio/bsp_gpio.c @@ -15,7 +15,7 @@ static GPIOInstance *gpio_instance[GPIO_MX_DEVICE_NUM] = {NULL}; void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { // 如有必要,可以根据pinstate和HAL_GPIO_ReadPin来判断是上升沿还是下降沿/rise&fall等 - static GPIOInstance *gpio; + GPIOInstance *gpio; for (size_t i = 0; i < idx; i++) { gpio = gpio_instance[i]; diff --git a/modules/can_comm/can_comm.c b/modules/can_comm/can_comm.c index 5e5d7b2..ad098b0 100644 --- a/modules/can_comm/can_comm.c +++ b/modules/can_comm/can_comm.c @@ -23,8 +23,7 @@ static void CANCommResetRx(CANCommInstance *ins) */ static void CANCommRxCallback(CANInstance *_instance) { - static CANCommInstance *comm; - comm = (CANCommInstance *)_instance->id; // 注意写法,将can instance的id强制转换为CANCommInstance*类型 + CANCommInstance *comm = (CANCommInstance *)_instance->id; // 注意写法,将can instance的id强制转换为CANCommInstance*类型 /* 接收状态判断 */ if (_instance->rx_buff[0] == CAN_COMM_HEADER && comm->recv_state == 0) // 之前尚未开始接收且此次包里第一个位置是帧头 diff --git a/modules/daemon/daemon.c b/modules/daemon/daemon.c index e71f057..783d0d5 100644 --- a/modules/daemon/daemon.c +++ b/modules/daemon/daemon.c @@ -33,7 +33,7 @@ uint8_t DaemonIsOnline(DaemonInstance *instance) void DaemonTask() { - static DaemonInstance *dins; // 提高可读性同时降低访存开销 + DaemonInstance *dins; // 提高可读性同时降低访存开销 for (size_t i = 0; i < idx; ++i) { dins = daemon_instances[i]; diff --git a/modules/ist8310/ist8310.c b/modules/ist8310/ist8310.c index 851416f..4fe4427 100644 --- a/modules/ist8310/ist8310.c +++ b/modules/ist8310/ist8310.c @@ -29,9 +29,8 @@ static uint8_t ist8310_write_reg_data_error[IST8310_WRITE_REG_NUM][3] = { */ static void IST8310Decode(IICInstance *iic) { - static int16_t temp[3]; // 用于存储解码后的数据 - static IST8310Instance *ist; // 用于存储IST8310实例的指针 - ist = (IST8310Instance *)(iic->id); // iic的id保存了IST8310实例的指针(父指针) + int16_t temp[3]; // 用于存储解码后的数据 + IST8310Instance *ist= (IST8310Instance *)(iic->id); // iic的id保存了IST8310实例的指针(父指针) memcpy(temp, ist->iic_buffer, 6 * sizeof(uint8_t)); // 不要强制转换,直接cpy for (uint8_t i = 0; i < 3; i++) diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index 11044dd..0b8b25a 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -29,7 +29,7 @@ static DaemonInstance *vision_daemon_instance; */ static void DecodeVision() { - static uint16_t flag_register; + uint16_t flag_register; DaemonReload(vision_daemon_instance); // 喂狗 get_protocol_info(vision_usart_instance->recv_buff, &flag_register, (uint8_t *)&recv_data.pitch); // TODO: code to resolve flag_register; @@ -80,9 +80,9 @@ Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle) */ void VisionSend(Vision_Send_s *send) { - static uint16_t flag_register; - static uint8_t send_buff[VISION_SEND_SIZE]; - static uint16_t tx_len; + uint16_t flag_register; + uint8_t send_buff[VISION_SEND_SIZE]; + uint16_t tx_len; // TODO: code to set flag_register // 将数据转化为seasky协议的数据包 diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index 6127358..13de756 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -72,7 +72,8 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) { LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information."); // 后续可以把id和CAN打印出来 - while (1); // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是) + while (1) + ; // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是) } } break; @@ -99,7 +100,8 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) { LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information."); - while (1); // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) + while (1) + ; // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) } } break; @@ -118,13 +120,10 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf */ static void DecodeDJIMotor(CANInstance *_instance) { - // 由于需要多次变址访存,直接将buff和measure地址保存在寄存器里避免多次存取 - static uint8_t *rxbuff; - static DJI_Motor_Measure_s *measure; - rxbuff = _instance->rx_buff; // 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址 // _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针 - measure = &(((DJIMotorInstance *)_instance->id)->motor_measure); // measure要多次使用,保存指针减小访存开销 + uint8_t *rxbuff = _instance->rx_buff; + DJI_Motor_Measure_s *measure = &(((DJIMotorInstance *)_instance->id)->motor_measure); // measure要多次使用,保存指针减小访存开销 // 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册 measure->last_ecd = measure->ecd; @@ -188,7 +187,7 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback } else { - LOGERROR("[dji_motor] loop type error, check memory access and func param");// 检查是否传入了正确的LOOP类型,或发生了指针越界 + LOGERROR("[dji_motor] loop type error, check memory access and func param"); // 检查是否传入了正确的LOOP类型,或发生了指针越界 } } @@ -217,15 +216,14 @@ void DJIMotorSetRef(DJIMotorInstance *motor, float ref) // 为所有电机实例计算三环PID,发送控制报文 void DJIMotorControl() { - // 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销 - // 同样可以提高可读性 - static uint8_t group, num; // 电机组号和组内编号 - static int16_t set; // 电机控制CAN发送设定值 - static DJIMotorInstance *motor; - static Motor_Control_Setting_s *motor_setting; // 电机控制参数 - static Motor_Controller_s *motor_controller; // 电机控制器 - static DJI_Motor_Measure_s *motor_measure; // 电机测量值 - static float pid_measure, pid_ref; // 电机PID测量值和设定值 + // 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性 + uint8_t group, num; // 电机组号和组内编号 + int16_t set; // 电机控制CAN发送设定值 + DJIMotorInstance *motor; + Motor_Control_Setting_s *motor_setting; // 电机控制参数 + Motor_Controller_s *motor_controller; // 电机控制器 + DJI_Motor_Measure_s *motor_measure; // 电机测量值 + float pid_measure, pid_ref; // 电机PID测量值和设定值 // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值 for (size_t i = 0; i < idx; ++i) diff --git a/modules/motor/HTmotor/HT04.c b/modules/motor/HTmotor/HT04.c index 13ec46a..4201dfc 100644 --- a/modules/motor/HTmotor/HT04.c +++ b/modules/motor/HTmotor/HT04.c @@ -40,12 +40,9 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits) */ static void HTMotorDecode(CANInstance *motor_can) { - static uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量 - static HTMotor_Measure_t *measure; - static uint8_t *rxbuff; - - rxbuff = motor_can->rx_buff; - measure = &((HTMotorInstance *)motor_can->id)->motor_measure; // 将can实例中保存的id转换成电机实例的指针 + uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量 + uint8_t *rxbuff = motor_can->rx_buff; + HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->motor_measure; // 将can实例中保存的id转换成电机实例的指针 measure->last_angle = measure->total_angle; @@ -89,12 +86,12 @@ void HTMotorSetRef(HTMotorInstance *motor, float ref) void HTMotorControl() { - static float set, pid_measure, pid_ref; - static uint16_t tmp; - static HTMotorInstance *motor; - static HTMotor_Measure_t *measure; - static Motor_Control_Setting_s *setting; - static CANInstance *motor_can; + float set, pid_measure, pid_ref; + uint16_t tmp; + HTMotorInstance *motor; + HTMotor_Measure_t *measure; + Motor_Control_Setting_s *setting; + CANInstance *motor_can; // 遍历所有电机实例,计算PID for (size_t i = 0; i < idx; i++) diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c index 8db73a7..6969aab 100644 --- a/modules/motor/LKmotor/LK9025.c +++ b/modules/motor/LKmotor/LK9025.c @@ -13,8 +13,8 @@ static CANInstance *sender_instance; // 多电机发送时使用的caninstance( */ static void LKMotorDecode(CANInstance *_instance) { - static LKMotor_Measure_t *measure; - static uint8_t *rx_buff; + LKMotor_Measure_t *measure; + uint8_t *rx_buff; rx_buff = _instance->rx_buff; measure = &(((LKMotorInstance *)_instance->id)->measure); // 通过caninstance保存的id获取对应的motorinstance @@ -72,11 +72,11 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config) /* 第一个电机的can instance用于发送数据,向其tx_buff填充数据 */ void LKMotorControl() { - static float pid_measure, pid_ref; - static int16_t set; - static LKMotorInstance *motor; - static LKMotor_Measure_t *measure; - static Motor_Control_Setting_s *setting; + float pid_measure, pid_ref; + int16_t set; + LKMotorInstance *motor; + LKMotor_Measure_t *measure; + Motor_Control_Setting_s *setting; for (size_t i = 0; i < idx; ++i) { diff --git a/modules/motor/servo_motor/servo_motor.c b/modules/motor/servo_motor/servo_motor.c index 41145d0..e5c4564 100644 --- a/modules/motor/servo_motor/servo_motor.c +++ b/modules/motor/servo_motor/servo_motor.c @@ -82,7 +82,7 @@ void Servo_Motor_Type_Select(ServoInstance *Servo_Motor, int16_t mode) */ void ServeoMotorControl() { - static ServoInstance *Servo_Motor; + ServoInstance *Servo_Motor; for (size_t i = 0; i < servo_idx; i++) { diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 5e9917b..4c601c1 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -92,12 +92,13 @@ static void RemoteControlRxCallback() } /** - * @brief + * @brief 遥控器离线的回调函数,注册到守护进程中,串口掉线时调用 * */ -static void RCLostCallback() +static void RCLostCallback(void *id) { // @todo 遥控器丢失的处理 + USARTServiceInit(rc_usart_instance); // 尝试重新启动接收 } RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle) @@ -111,9 +112,9 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle) // 进行守护进程的注册,用于定时检查遥控器是否正常工作 // @todo 当前守护进程直接在这里注册,后续考虑将其封装到遥控器的初始化函数中,即可以让用户决定reload_count的值(是否有必要?) Daemon_Init_Config_s daemon_conf = { - .reload_count = 100, // 100ms,遥控器的接收频率实际上是1000/14Hz(大约70) - .callback = NULL, // 后续考虑重新启动遥控器对应串口的传输 - .owner_id = NULL, // 只有1个遥控器,不需要owner_id + .reload_count = 10, // 100ms未收到数据视为离线,遥控器的接收频率实际上是1000/14Hz(大约70Hz) + .callback = RCLostCallback, + .owner_id = NULL, // 只有1个遥控器,不需要owner_id }; rc_daemon_instance = DaemonRegister(&daemon_conf); @@ -126,5 +127,4 @@ uint8_t RemotecontrolIsOnline() if (rc_init_flag) return DaemonIsOnline(rc_daemon_instance); return 0; - } \ No newline at end of file diff --git a/modules/super_cap/super_cap.c b/modules/super_cap/super_cap.c index 607a137..eb86749 100644 --- a/modules/super_cap/super_cap.c +++ b/modules/super_cap/super_cap.c @@ -13,8 +13,8 @@ static SuperCapInstance *super_cap_instance = NULL; // 可以由app保存此指 static void SuperCapRxCallback(CANInstance *_instance) { - static uint8_t *rxbuff; - static SuperCap_Msg_s *Msg; + uint8_t *rxbuff; + SuperCap_Msg_s *Msg; rxbuff = _instance->rx_buff; Msg = &super_cap_instance->cap_msg; Msg->vol = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]); From 59e21dc25d4c941c933b1575bbcb4c647d46a2a1 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Thu, 23 Mar 2023 18:57:54 +0800 Subject: [PATCH 07/12] Merge from captain chen --- .vscode/launch.json | 4 +- .vscode/tasks.json | 4 +- HAL_N_Middlewares/Src/freertos.c | 3 +- VSCode+Ozone使用方法.md | 2 +- application/chassis/chassis.c | 40 +++++++------- application/cmd/robot_cmd.c | 43 ++++++++------- application/gimbal/gimbal.c | 69 ++++++++++++++----------- application/robot_def.h | 14 +++-- application/shoot/shoot.c | 56 +++++++++++--------- bsp/usart/bsp_usart.h | 6 +-- modules/algorithm/controller.h | 9 ++-- modules/imu/ins_task.c | 3 +- modules/master_machine/master_process.c | 2 - modules/master_machine/master_process.h | 2 +- modules/motor/DJImotor/dji_motor.c | 6 +-- modules/motor/DJImotor/dji_motor.md | 10 ++-- modules/motor/HTmotor/HT04.c | 2 +- modules/motor/LKmotor/LK9025.c | 2 +- modules/motor/motor_def.h | 11 +++- 19 files changed, 161 insertions(+), 127 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 9247755..29cdabb 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -6,7 +6,7 @@ { "name": "Debug-DAP", "cwd": "${workspaceRoot}", - "executable": "${workspaceRoot}\\build\\${workspaceFolderBasename}.elf", // 要下载到调试器的文件,花括号中的是vscode两个预定义的参数 + "executable": "${workspaceRoot}\\build\\basic_framework.elf", // 要下载到调试器的文件,花括号中的是vscode两个预定义的参数 "request": "launch", "type": "cortex-debug", //使用J-link GDB Server时必须;其他GBD Server时可选（有可能帮助自动选择SVD文件） @@ -28,7 +28,7 @@ { "name": "Debug-Jlink", "cwd": "${workspaceFolder}", - "executable": "${workspaceRoot}\\build\\${workspaceFolderBasename}.elf", + "executable": "${workspaceRoot}\\build\\basic_framework.elf", "request": "launch", "type": "cortex-debug", "device": "STM32F407IG", diff --git a/.vscode/tasks.json b/.vscode/tasks.json index d5bf982..6291ce2 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -15,7 +15,7 @@ { "label": "download dap", "type": "shell", // 如果希望在下载前编译,可以把command换成下面的命令 - "command":"mingw32-make download_dap", // "mingw32-make -j24 && mingw32-make download_dap", + "command":"mingw32-make download_dap", // "mingw32-make -j24 ; mingw32-make download_dap", "group": { // 如果没有修改代码,编译任务不会消耗时间,因此推荐使用上面的替换. "kind": "build", "isDefault": false, @@ -24,7 +24,7 @@ { "label": "download jlink", "type": "shell", - "command":"mingw32-make download_jlink", // "mingw32-make -j24 && mingw32-make download_dap" + "command":"mingw32-make download_jlink", // "mingw32-make -j24 ; mingw32-make download_dap" "group": { "kind": "build", "isDefault": false, diff --git a/HAL_N_Middlewares/Src/freertos.c b/HAL_N_Middlewares/Src/freertos.c index 79c44b7..6229414 100644 --- a/HAL_N_Middlewares/Src/freertos.c +++ b/HAL_N_Middlewares/Src/freertos.c @@ -31,7 +31,6 @@ #include "daemon.h" #include "robot.h" - /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -133,7 +132,7 @@ void MX_FREERTOS_Init(void) { /* USER CODE BEGIN RTOS_THREADS */ /* add threads, ... */ /* USER CODE END RTOS_THREADS */ - + } /* USER CODE BEGIN Header_StartDefaultTask */ diff --git a/VSCode+Ozone使用方法.md b/VSCode+Ozone使用方法.md index c3f35dd..39ade18 100644 --- a/VSCode+Ozone使用方法.md +++ b/VSCode+Ozone使用方法.md @@ -600,7 +600,7 @@ Project.SetOSPlugin(“plugin_name”) 2. 变量watch窗口，这里的变量不会实时更新，只有在暂停或遇到断点的时候才会更新。若希望实时查看，在这里右键选择需要动态查看的变量，选择Graph，他就会出现在**窗口8**的位置。 - 如果不需要可视化查看变量变化的趋势，但是想不赞同查看变量的值，请右键点击变量，选择一个合适的refresh rate： + 如果不需要可视化查看变量变化的趋势，但是想不暂停查看变量的值，请右键点击变量，选择一个合适的refresh rate： ![image-20221119173731119](assets/image-20221119173731119.png) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 283cb24..1af1908 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -68,16 +68,20 @@ void ChassisInit() .can_init_config.can_handle = &hcan1, .controller_param_init_config = { .speed_PID = { - .Kp = 10, - .Ki = 0, - .Kd = 0, - .MaxOut = 4000, + .Kp = 4.5,//9 + .Ki = 0,//0.02 + .Kd = 0.01,//0.01 + .IntegralLimit = 3000, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .MaxOut = 12000, }, .current_PID = { - .Kp = 1, - .Ki = 0, + .Kp = 0.4,//0.7 + .Ki = 0,//0.1 .Kd = 0, - .MaxOut = 4000, + .IntegralLimit = 3000, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .MaxOut = 15000, }, }, .controller_setting_init_config = { @@ -89,20 +93,20 @@ void ChassisInit() .motor_type = M3508, }; // @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改. - chassis_motor_config.can_init_config.tx_id = 4; - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 1; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_lf = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 3, - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 2; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_rf = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 1, - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 4; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_lb = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 2, - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 3; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_rb = DJIMotorInit(&chassis_motor_config); // referee_data = RefereeInit(&huart6); // 裁判系统初始化 @@ -153,7 +157,7 @@ static void MecanumCalculate() { vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER; vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER; - vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER; + vt_lb = chassis_vx - chassis_vy -chassis_cmd_recv.wz * LB_CENTER; vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER; } @@ -220,10 +224,10 @@ void ChassisTask() chassis_cmd_recv.wz = 0; break; case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 - chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); + chassis_cmd_recv.wz = -1.5*chassis_cmd_recv.offset_angle*abs(chassis_cmd_recv.offset_angle); break; case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 - // chassis_cmd_recv.wz=sin(t) + chassis_cmd_recv.wz=4000; break; default: break; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index d4fdab2..3a6233c 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -21,7 +21,7 @@ static CANCommInstance *cmd_can_comm; // 双板通信 #ifdef ONE_BOARD static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者 static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者 -#endif // ONE_BOARD +#endif // ONE_BOARD static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 @@ -68,6 +68,7 @@ void RobotCMDInit() }; cmd_can_comm = CANCommInit(&comm_conf); #endif // GIMBAL_BOARD + gimbal_cmd_send.pitch = 0; robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 } @@ -80,9 +81,9 @@ void RobotCMDInit() static void CalcOffsetAngle() { // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数 - static float angle; + static float angle; angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度 -#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度 +#if YAW_ECD_GREATER_THAN_4096 // 如果大于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) @@ -107,9 +108,15 @@ static void RemoteControlSet() { // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台 - chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; + { + chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 + { chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; + gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; + } // 云台参数,确定云台控制数据 if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式 @@ -120,9 +127,8 @@ static void RemoteControlSet() // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制 if (switch_is_down(rc_data[TEMP].rc.switch_left) || vision_recv_data->target_state == NO_TARGET) { // 按照摇杆的输出大小进行角度增量,增益系数需调整 - gimbal_cmd_send.yaw += 0.0015f * (float)rc_data[TEMP].rc.rocker_l_; - gimbal_cmd_send.pitch += 0.002f * (float)rc_data[TEMP].rc.rocker_l1; - gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; + gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_; + gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1; } // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 @@ -131,7 +137,7 @@ static void RemoteControlSet() // 发射参数 if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 - ; // 弹舱舵机控制,待添加servo_motor模块,开启 + ; // 弹舱舵机控制,待添加servo_motor模块,开启 else ; // 弹舱舵机控制,待添加servo_motor模块,关闭 @@ -146,7 +152,7 @@ static void RemoteControlSet() else shoot_cmd_send.load_mode = LOAD_STOP; // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频, - shoot_cmd_send.shoot_rate = 1; + shoot_cmd_send.shoot_rate = 8; } /** @@ -162,7 +168,7 @@ static void MouseKeySet() /** * @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等 * 停止的阈值'300'待修改成合适的值,或改为开关控制. - * + * * @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现 * */ @@ -171,16 +177,16 @@ static void EmergencyHandler() // 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正 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; + 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; // 射击模式改为关闭 } // 遥控器右侧开关为[上],恢复正常运行 if (switch_is_up(rc_data[TEMP].rc.switch_right)) { - robot_state = ROBOT_READY; - shoot_cmd_send.shoot_mode = SHOOT_ON; + robot_state = ROBOT_READY; // 机器人状态改为准备就绪 + shoot_cmd_send.shoot_mode = SHOOT_ON; // 射击模式改为开启 } } @@ -199,7 +205,6 @@ void RobotCMDTask() // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 CalcOffsetAngle(); - // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制 RemoteControlSet(); @@ -209,8 +214,8 @@ void RobotCMDTask() EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 // 设置视觉发送数据,还需增加加速度和角速度数据 - vision_send_data.bullet_speed = chassis_fetch_data.bullet_speed; - vision_send_data.enemy_color = chassis_fetch_data.enemy_color; + vision_send_data.bullet_speed = 15; + vision_send_data.enemy_color = 0; vision_send_data.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; vision_send_data.yaw = gimbal_fetch_data.gimbal_imu_data.Yaw; vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll; diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 95f37f1..0df5633 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -1,6 +1,7 @@ +// #include "gimbal.h" #include "robot_def.h" - +// #include "dji_motor.h" #include "ins_task.h" #include "message_center.h" @@ -54,8 +55,8 @@ void GimbalInit() .cali_mode=BMI088_CALIBRATE_ONLINE_MODE, .work_mode=BMI088_BLOCK_PERIODIC_MODE, }; - // imu=BMI088Register(&imu_config); - // gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 + // imu=BMI088Register(&imu_config); + gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW Motor_Init_Config_s yaw_config = { .can_init_config = { @@ -64,60 +65,68 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 20, + .Kp = 8, //8 .Ki = 0, .Kd = 0, - .MaxOut = 2000, - .DeadBand = 0.3, + .DeadBand = 0.1, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .IntegralLimit = 100, + + .MaxOut = 500, }, .speed_PID = { - .Kp = 10, - .Ki = 0, + .Kp = 50,//40 + .Ki = 200, .Kd = 0, - .MaxOut = 4000, + .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement, + .IntegralLimit = 3000, + .MaxOut = 20000, }, .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - // .other_speed_feedback_ptr=&gimba_IMU_data.wz; + .other_speed_feedback_ptr=&gimba_IMU_data->Gyro[2], }, .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, + .angle_feedback_source = OTHER_FEED, + .speed_feedback_source = OTHER_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .reverse_flag = MOTOR_DIRECTION_NORMAL, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = GM6020}; // PITCH Motor_Init_Config_s pitch_config = { .can_init_config = { - .can_handle = &hcan1, + .can_handle = &hcan2, .tx_id = 2, }, .controller_param_init_config = { .angle_PID = { - .Kp = 30, + .Kp =10,//10 .Ki = 0, .Kd = 0, - .MaxOut = 4000, - .DeadBand = 0.3, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .IntegralLimit =100, + .MaxOut = 500, }, .speed_PID = { - .Kp = 10, - .Ki = 0, - .Kd = 0, - .MaxOut = 4000, + .Kp=50,//50 + .Ki =350,//350 + .Kd =0,//0.1 + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .IntegralLimit =2500, + .MaxOut = 20000, }, .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - // .other_speed_feedback_ptr=&gimba_IMU_data.wy, + .other_speed_feedback_ptr=(&gimba_IMU_data->Gyro[0]), }, .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, + .angle_feedback_source = OTHER_FEED, + .speed_feedback_source = OTHER_FEED, .outer_loop_type = ANGLE_LOOP, - .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .reverse_flag = MOTOR_DIRECTION_NORMAL, + .close_loop_type = SPEED_LOOP | ANGLE_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = GM6020, }; @@ -160,10 +169,10 @@ void GimbalTask() case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) DJIMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, MOTOR_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, MOTOR_FEED); - DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, MOTOR_FEED); - DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, MOTOR_FEED); + 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; diff --git a/application/robot_def.h b/application/robot_def.h index d81df22..f432ece 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -34,21 +34,25 @@ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 -#define YAW_CHASSIS_ALIGN_ECD 4000 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 +#define YAW_CHASSIS_ALIGN_ECD 2711 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 -#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 +#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 // 发射参数 -#define ONE_BULLET_DELTA_ANGLE 0 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 +#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f -#define NUM_PER_CIRCLE 1 // 拨盘一圈的装载量 +#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量 // 机器人底盘修改的参数,单位为mm(毫米) -#define WHEEL_BASE 300 // 纵向轴距(前进后退方向) +#define WHEEL_BASE 350 // 纵向轴距(前进后退方向) #define TRACK_WIDTH 300 // 横向轮距(左右平移方向) #define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0 #define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0 #define RADIUS_WHEEL 60 // 轮子半径 #define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换 +#define GYRO2GIMBAL_DIR_YAW 1 //陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反 +#define GYRO2GIMBAL_DIR_PITCH 1 //陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反 +#define GYRO2GIMBAL_DIR_ROLL 1 //陀螺仪数据相较于云台的roll的方向,1为相同,-1为相反 + // 检查是否出现主控板定义冲突,只允许一个开发板定义存在,否则编译会自动报错 #if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \ (defined(ONE_BOARD) && defined(GIMBAL_BOARD)) || \ diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 4f50b7d..14444be 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -26,20 +26,23 @@ void ShootInit() Motor_Init_Config_s friction_config = { .can_init_config = { .can_handle = &hcan2, - .tx_id = 6, }, .controller_param_init_config = { .speed_PID = { - .Kp = 10, - .Ki = 0, + .Kp = 0,//20 + .Ki = 0,//1 .Kd = 0, - .MaxOut = 2000, + .Improve = PID_Integral_Limit, + .IntegralLimit = 10000, + .MaxOut = 15000, }, .current_PID = { - .Kp = 1, - .Ki = 0, + .Kp = 0,//0.7 + .Ki = 0,//0.1 .Kd = 0, - .MaxOut = 2000, + .Improve = PID_Integral_Limit, + .IntegralLimit = 10000, + .MaxOut = 15000, }, }, .controller_setting_init_config = { @@ -48,47 +51,52 @@ void ShootInit() .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, - .reverse_flag = MOTOR_DIRECTION_NORMAL, + .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 = 5; // 右摩擦轮,改txid和方向就行 - friction_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + 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 = &hcan2, - .tx_id = 7, + .tx_id = 3, }, .controller_param_init_config = { .angle_PID = { // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降 - .Kp = 10, + .Kp = 0,//10 .Ki = 0, .Kd = 0, .MaxOut = 200, }, .speed_PID = { - .Kp = 1, - .Ki = 0, + .Kp = 0,//10 + .Ki = 0,//1 .Kd = 0, - .MaxOut = 2000, + .Improve = PID_Integral_Limit, + .IntegralLimit = 5000, + .MaxOut = 5000, }, .current_PID = { - .Kp = 1, - .Ki = 0, + .Kp = 0,//0.7 + .Ki = 0,//0.1 .Kd = 0, - .MaxOut = 3000, + .Improve = PID_Integral_Limit, + .IntegralLimit = 5000, + .MaxOut = 5000, }, }, .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转 - .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向 + .close_loop_type = CURRENT_LOOP | SPEED_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向 }, .motor_type = M2006 // 英雄使用m3508 }; @@ -120,8 +128,8 @@ void ShootTask() // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可 // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发) - if (hibernate_time + dead_time > DWT_GetTimeline_ms()) - return; + // if (hibernate_time + dead_time > DWT_GetTimeline_ms()) + // return; // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换 switch (shoot_cmd_recv.load_mode) @@ -182,8 +190,8 @@ void ShootTask() DJIMotorSetRef(friction_r, 0); break; default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速. - DJIMotorSetRef(friction_l, 1000); - DJIMotorSetRef(friction_r, 1000); + DJIMotorSetRef(friction_l, 30000); + DJIMotorSetRef(friction_r, 30000); break; } } diff --git a/bsp/usart/bsp_usart.h b/bsp/usart/bsp_usart.h index e6d5e4d..6cf0c00 100644 --- a/bsp/usart/bsp_usart.h +++ b/bsp/usart/bsp_usart.h @@ -46,8 +46,8 @@ USARTInstance *USARTRegister(USART_Init_Config_s *init_config); /** * @brief 启动串口服务,需要传入一个usart实例.一般用于lost callback的情况(使用串口的模块daemon) - * - * @param _instance + * + * @param _instance */ void USARTServiceInit(USARTInstance *_instance); @@ -66,7 +66,7 @@ void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size,U /** * @brief 判断串口是否准备好,用于连续或异步的IT/DMA发送 - * + * * @param _instance 要判断的串口实例 * @return uint8_t ready 1, busy 0 */ diff --git a/modules/algorithm/controller.h b/modules/algorithm/controller.h index 38b7fb1..dfe0e70 100644 --- a/modules/algorithm/controller.h +++ b/modules/algorithm/controller.h @@ -65,10 +65,10 @@ typedef struct // improve parameter PID_Improvement_e Improve; - float IntegralLimit; // 积分限幅 - float CoefA; // 变速积分 For Changing Integral - float CoefB; // 变速积分 ITerm = Err*((A-abs(err)+B)/A) when B<|err|recv_buff, &flag_register, (uint8_t *)&recv_data.pitch); // TODO: code to resolve flag_register; - PrintLog("decode vision"); - } /** diff --git a/modules/master_machine/master_process.h b/modules/master_machine/master_process.h index 224faa3..e23cf7a 100644 --- a/modules/master_machine/master_process.h +++ b/modules/master_machine/master_process.h @@ -4,7 +4,7 @@ #include "bsp_usart.h" #include "seasky_protocol.h" -#define VISION_RECV_SIZE 36u // 当前为固定值,36字节 +#define VISION_RECV_SIZE 18u // 当前为固定值,36字节 #define VISION_SEND_SIZE 36u #pragma pack(1) diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index 13de756..898b287 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -233,7 +233,8 @@ void DJIMotorControl() motor_controller = &motor->motor_controller; motor_measure = &motor->motor_measure; pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改 - + if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) + pid_ref*= -1; // 设置反转 // pid_ref会顺次通过被启用的闭环充当数据的载体 // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出 if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP) @@ -265,8 +266,7 @@ void DJIMotorControl() // 获取最终输出 set = (int16_t)pid_ref; - if (motor_setting->reverse_flag == MOTOR_DIRECTION_REVERSE) - set *= -1; // 设置反转 + // 分组填入发送数据 group = motor->sender_group; diff --git a/modules/motor/DJImotor/dji_motor.md b/modules/motor/DJImotor/dji_motor.md index bec0ba5..6e1dfda 100644 --- a/modules/motor/DJImotor/dji_motor.md +++ b/modules/motor/DJImotor/dji_motor.md @@ -142,7 +142,7 @@ Motor_Init_Config_s config = { .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, .speed_feedback_source = MOTOR_FEED, - .reverse_flag = MOTOR_DIRECTION_NORMAL}, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL}, // 电流环和速度环PID参数的设置,不采用计算优化则不需要传入Improve参数 // 不使用其他数据来源(如IMU),不需要传入反馈数据变量指针 .controller_param_init_config = {.current_PID = {.Improve = 0, @@ -245,7 +245,7 @@ typedef struct { Closeloop_Type_e outer_loop_type; Closeloop_Type_e close_loop_type; - Reverse_Flag_e reverse_flag; + Motor_Reverse_Flag_e motor_reverse_flag; Feedback_Source_e angle_feedback_source; Feedback_Source_e speed_feedback_source; } Motor_Control_Setting_s; @@ -272,14 +272,14 @@ typedef struct > 注意，务必分清串级控制（多环）和外层闭环的区别。前者是为了提高内环的性能，使得其能更好地跟随外环参考值；而后者描述的是系统真实的控制目标（闭环目标）。如3508，没有电流环仍然可以对速度完成闭环，对于高层的应用来说，它们本质上不关心电机内部是否还有电流环，它们只把外层闭环为速度的电机当作一个**速度伺服执行器**，**外层闭环**描述的就是真正的闭环目标。 - - 为了避开恼人的正负号，提高代码的可维护性，在初始化电机时设定`reverse_flag`使得所有电机都按照你想要的方向旋转，其定义如下： + - 为了避开恼人的正负号，提高代码的可维护性，在初始化电机时设定`motor_reverse_flag`使得所有电机都按照你想要的方向旋转，其定义如下： ```c typedef enum { MOTOR_DIRECTION_NORMAL = 0, MOTOR_DIRECTION_REVERSE = 1 - } Reverse_Flag_e; + } Motor_Reverse_Flag_e; ``` - `speed_feedback_source`以及`angle_feedback_source`是指示电机反馈来源的标志位。一般情况下电机使用自身的编码器作为控制反馈量。但在某些时候，如小陀螺模式，云台电机会使用IMU的姿态数据作为反馈数据来源。其定义如下： @@ -437,7 +437,7 @@ Motor_Init_Config_s config = { .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .speed_feedback_source = MOTOR_FEED, - .reverse_flag = MOTOR_DIRECTION_NORMAL + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL }, .controller_param_init_config = { .angle_PID = { diff --git a/modules/motor/HTmotor/HT04.c b/modules/motor/HTmotor/HT04.c index 4201dfc..ddbf913 100644 --- a/modules/motor/HTmotor/HT04.c +++ b/modules/motor/HTmotor/HT04.c @@ -134,7 +134,7 @@ void HTMotorControl() } set = pid_ref; - if (setting->reverse_flag == MOTOR_DIRECTION_REVERSE) + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) set *= -1; LIMIT_MIN_MAX(set, T_MIN, T_MAX); // 限幅,实际上这似乎和pid输出限幅重复了 diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c index 6969aab..4bdb70f 100644 --- a/modules/motor/LKmotor/LK9025.c +++ b/modules/motor/LKmotor/LK9025.c @@ -113,7 +113,7 @@ void LKMotorControl() } set = pid_ref; - if (setting->reverse_flag == MOTOR_DIRECTION_REVERSE) + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) set *= -1; // 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t)); diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index ed16fec..482e4bf 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -54,8 +54,14 @@ typedef enum { MOTOR_DIRECTION_NORMAL = 0, MOTOR_DIRECTION_REVERSE = 1 -} Reverse_Flag_e; +} Motor_Reverse_Flag_e; +/* 反馈量正反标志 */ +typedef enum +{ + FEEDBACK_DIRECTION_NORMAL = 0, + FEEDBACK_DIRECTION_REVERSE = 1 +} Feedback_Reverse_Flag_e; typedef enum { MOTOR_STOP = 0, @@ -67,7 +73,8 @@ typedef struct { Closeloop_Type_e outer_loop_type; // 最外层的闭环,未设置时默认为最高级的闭环 Closeloop_Type_e close_loop_type; // 使用几个闭环(串级) - Reverse_Flag_e reverse_flag; // 是否反转 + Motor_Reverse_Flag_e motor_reverse_flag; // 是否反转 + Feedback_Reverse_Flag_e feedback_reverse_flag; // 反馈是否反向 Feedback_Source_e angle_feedback_source; // 角度反馈类型 Feedback_Source_e speed_feedback_source; // 速度反馈类型 Feedfoward_Type_e feedforward_flag; // 前馈标志 From 462a5e065404facaa1c3125d651ea96da154ce9f Mon Sep 17 00:00:00 2001 From: NeoZng Date: Thu, 23 Mar 2023 19:40:36 +0800 Subject: [PATCH 08/12] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86CAN=E4=B8=AD?= =?UTF-8?q?=E6=96=AD=E4=BC=98=E5=85=88=E7=BA=A7=E9=94=99=E8=AF=AF=E5=BC=95?= =?UTF-8?q?=E8=B5=B7=E7=9A=84=E9=80=9A=E4=BF=A1debug=E6=A1=88=E4=BE=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Makefile | 2 -- modules/led/led.c | 2 +- 如何定位bug.md | 14 +++++++++++++- 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/Makefile b/Makefile index f6a50ab..7f97471 100644 --- a/Makefile +++ b/Makefile @@ -141,7 +141,6 @@ modules/can_comm/can_comm.c \ modules/message_center/message_center.c \ modules/daemon/daemon.c \ modules/vofa/vofa.c \ -modules/ps_handle/ps_handle.c \ application/gimbal/gimbal.c \ application/chassis/chassis.c \ application/shoot/shoot.c \ @@ -263,7 +262,6 @@ C_INCLUDES = \ -Imodules/message_center \ -Imodules/daemon \ -Imodules/vofa \ --Imodules/ps_handle \ -Imodules # compile gcc flags diff --git a/modules/led/led.c b/modules/led/led.c index 4b134ac..f3f4a38 100644 --- a/modules/led/led.c +++ b/modules/led/led.c @@ -10,7 +10,7 @@ LEDInstance *LEDRegister(LED_Init_Config_s *led_config) { LEDInstance *led_ins = (LEDInstance *)zero_malloc(sizeof(LEDInstance)); // 剩下的值暂时都被置零 - led_ins->led_pwm = GPIORegister(&led_config->pwm_config); + led_ins->led_pwm = PWMRegister(&led_config->pwm_config); led_ins->led_switch = led_config->init_swtich; bsp_led_ins[idx++] = led_ins; diff --git a/如何定位bug.md b/如何定位bug.md index a2e521f..f82395c 100644 --- a/如何定位bug.md +++ b/如何定位bug.md @@ -87,7 +87,7 @@ long long的范围比float小。无符号和有符号数直接转换可能变成 -## 典型debug案例 +## 典型debug案例一 这是一个结合了软件和硬件且有多模块耦合的异常。该bug发生在调试平衡步兵的底盘过程当中。 @@ -159,3 +159,15 @@ void MotorControlTask() 均衡总线负载，调节任务运行时间。 + + +# 典型debug案例二 + +这仍然是一个CAN总线引发的bug。使用的电机均为DJI电机。当多个电机接入时，会产生反馈值跳变的情况。起初认为**总线负载过高**，（控制频率为500Hz，反馈频率均为1kHz，计算之后得出CAN的负载率接近90%），但将电机减少为一半甚至更少时仍然出现此问题。**单独使用CAN1且仅挂载一个电机则问题消失**，同时使用CAN1和CAN2（不论单个总线挂载几个电机）则问题再次出现。 + +**单步调试发现反馈值并未因指针越界而被纂改**。仔细检查代码的计算发现并未出错，打开Ozone查看反馈值曲线，发现确实偶发跳变，但跳变值并未超出反馈值范围，即即使发生跳变值仍然在**正常范围内**，因此不像是总线负载过大导致数据帧错误或指针越界修改的随机值。加入多个电机同时查看反馈值，**发现反馈跳变之后会和另一电机的反馈值相同**，呈现“你跳到我我跳到你”的图景。怀疑CAN中断被**重入**，即一个中断未完成时另一个CAN报文到来，打断了当前的中断并执行了**相同的反馈解码函数**。但CAN1和CAN2的中断优先级均为5，因此不可能打断彼此。打开CubeMX查看初始化配置，发现两个CAN的FIFO0和FIFO1中断优先级不同，分别是5和6。则FIFO1的溢出中断会被FIFO0打断，且我们在电机的解码函数中使用了一些**静态变量**用于存储触发接收中断的电机报文的相关信息，故而新进入的中断覆写了之前中断的静态变量值，使得之前中断在恢复之后存储了前者的值，导致自身反馈错误。 + +将优先级统一设为5，编译之后重新运行，反馈值正常。 + +> “同时使用CAN1和CAN2（不论几个电机）则问题再次出现。” 导致此问题的原因是初始化CAN时按照rxid分配FIFO，因此注册的电机会被交替分配到不同的FIFO，故不论注册了几个电机（只要多于2）、注册到哪条总线都会出现FIFO1中断被FIFO0打断的情况。 + From 7e6a3673fb28560fbcd2a33b2af0ada96333bb2b Mon Sep 17 00:00:00 2001 From: chenfu <2412777093@qq.com> Date: Mon, 27 Mar 2023 22:03:35 +0800 Subject: [PATCH 09/12] =?UTF-8?q?=E6=9B=BE=E5=AE=B6=E9=94=AE=E9=BC=A0?= =?UTF-8?q?=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- HAL_N_Middlewares/Src/can.c | 4 +- application/chassis/chassis.c | 12 +- application/cmd/robot_cmd.c | 157 ++++++++++++++++++------ application/gimbal/gimbal.c | 13 +- application/robot_def.h | 5 +- modules/master_machine/master_process.c | 2 +- modules/remote/remote_control.c | 67 ++++++---- modules/remote/remote_control.h | 14 ++- 8 files changed, 193 insertions(+), 81 deletions(-) diff --git a/HAL_N_Middlewares/Src/can.c b/HAL_N_Middlewares/Src/can.c index c4c0948..292573f 100644 --- a/HAL_N_Middlewares/Src/can.c +++ b/HAL_N_Middlewares/Src/can.c @@ -122,7 +122,7 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle) HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); /* CAN1 interrupt Init */ - HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 6, 0); + HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0); HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 5, 0); HAL_NVIC_EnableIRQ(CAN1_RX1_IRQn); @@ -155,7 +155,7 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle) HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* CAN2 interrupt Init */ - HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 6, 0); + HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0); HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn); HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0); HAL_NVIC_EnableIRQ(CAN2_RX1_IRQn); diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 1af1908..34a66ef 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -19,7 +19,7 @@ // referee需要移动到module层 ///////////////////////// -#include "referee.h" + #include "rm_referee.h" ///////////////////////// @@ -68,16 +68,16 @@ void ChassisInit() .can_init_config.can_handle = &hcan1, .controller_param_init_config = { .speed_PID = { - .Kp = 4.5,//9 - .Ki = 0,//0.02 - .Kd = 0.01,//0.01 + .Kp = 10,//4.5 + .Ki = 0,//0 + .Kd = 0,//0 .IntegralLimit = 3000, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, .MaxOut = 12000, }, .current_PID = { - .Kp = 0.4,//0.7 - .Ki = 0,//0.1 + .Kp = 0.5,//0.4 + .Ki = 0,//0 .Kd = 0, .IntegralLimit = 3000, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 4883e04..5e4f787 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -21,11 +21,12 @@ static CANCommInstance *cmd_can_comm; // 双板通信 #ifdef ONE_BOARD static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者 static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者 -#endif // ONE_BOARD +#endif // ONE_BOARD 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; // 视觉发送数据 @@ -68,7 +69,7 @@ void RobotCMDInit() }; cmd_can_comm = CANCommInit(&comm_conf); #endif // GIMBAL_BOARD -gimbal_cmd_send.pitch = 0; + gimbal_cmd_send.pitch = 0; robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 } @@ -81,9 +82,9 @@ gimbal_cmd_send.pitch = 0; static void CalcOffsetAngle() { // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数 - static float angle; + static float angle; angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度 -#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度 +#if YAW_ECD_GREATER_THAN_4096 // 如果大于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) @@ -100,17 +101,6 @@ static void CalcOffsetAngle() #endif } -static void OffsetAnglePidCalc() -{ - // float pid_measure,pid_ref; - // static PIDInstance AngleCal = { - // .Kp = -1, - // .Ki = 0, - // .Kd = 0, - // .MaxOut = 10000, - // }; - // chassis_cmd_send.offset_angle = PIDCalculate(&AngleCal,chassis_cmd_send.offset_angle,0); -} /** * @brief 控制输入为遥控器(调试时)的模式和控制量设置 * @@ -119,11 +109,15 @@ static void RemoteControlSet() { // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台 - {chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;} + { + chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 - {chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; - gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;} + { + chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; + gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; + } // 云台参数,确定云台控制数据 if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式 @@ -136,25 +130,24 @@ static void RemoteControlSet() { // 按照摇杆的输出大小进行角度增量,增益系数需调整 gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_; gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1; - // if (gimbal_cmd_send.pitch <= 122) - // { - // gimbal_cmd_send.pitch =125; - // } - // else if (gimbal_cmd_send.pitch >= 175) - // { - // gimbal_cmd_send.pitch = 174; - // } - + // 摇杆控制的软件限位 + // if (gimbal_cmd_send.pitch <= PITCH_MIN_ECD) + // { + // gimbal_cmd_send.pitch = PITCH_MIN_ECD; + // } + // else if (gimbal_cmd_send.pitch >= PITCH_MAX_ECD) + // { + // gimbal_cmd_send.pitch = PITCH_MAX_ECD; + // } } // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向 chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向 - //chassis_cmd_send.wz = 300; // 发射参数 if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 - ; // 弹舱舵机控制,待添加servo_motor模块,开启 + ; // 弹舱舵机控制,待添加servo_motor模块,开启 else ; // 弹舱舵机控制,待添加servo_motor模块,关闭 @@ -163,7 +156,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 < -500) shoot_cmd_send.load_mode = LOAD_BURSTFIRE; else @@ -178,14 +171,89 @@ static void RemoteControlSet() */ static void MouseKeySet() { - // 待添加键鼠控制 - // ... + chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测 + chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300; + + gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测 + gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10; + + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速 + { + case 0: + shoot_cmd_send.bullet_speed = 15; + break; + case 1: + shoot_cmd_send.bullet_speed = 18; + break; + default: + shoot_cmd_send.bullet_speed = 30; + break; + } + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式 + { + case 0: + shoot_cmd_send.load_mode = LOAD_STOP; + break; + case 1: + shoot_cmd_send.load_mode = LOAD_1_BULLET; + break; + case 2: + shoot_cmd_send.load_mode = LOAD_3_BULLET; + break; + default: + shoot_cmd_send.load_mode = LOAD_BURSTFIRE; + break; + } + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱 + { + case 0: + shoot_cmd_send.lid_mode = LID_OPEN; + break; + default: + shoot_cmd_send.lid_mode = LID_CLOSE; + break; + } + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮 + { + case 0: + shoot_cmd_send.friction_mode = FRICTION_OFF; + break; + default: + shoot_cmd_send.friction_mode = FRICTION_ON; + break; + } + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度 + { + case 0: + chassis_cmd_send.chassis_speed_buff = 40; + break; + case 1: + chassis_cmd_send.chassis_speed_buff = 60; + break; + case 2: + chassis_cmd_send.chassis_speed_buff = 80; + break; + default: + chassis_cmd_send.chassis_speed_buff = 100; + break; + } + switch (rc_data[TEMP].key[KEY_PRESS].shift) //待添加 按shift允许超功率 消耗缓冲能量 + { + case 1: + + break; + + default: + + break; + } + } /** * @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等 * 停止的阈值'300'待修改成合适的值,或改为开关控制. - * + * * @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现 * */ @@ -204,9 +272,25 @@ static void EmergencyHandler() // 遥控器右侧开关为[上],恢复正常运行 if (switch_is_up(rc_data[TEMP].rc.switch_right)) { - robot_state = ROBOT_READY; + robot_state = ROBOT_READY; shoot_cmd_send.shoot_mode = SHOOT_ON; } + 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; + break; + } } /* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */ @@ -237,7 +321,8 @@ void RobotCMDTask() vision_send_data.enemy_color = 0; vision_send_data.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; vision_send_data.yaw = gimbal_fetch_data.gimbal_imu_data.Yaw; - vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll;; + vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll; + ; // 推送消息,双板通信,视觉通信等 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index f02ce9c..327fd79 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -74,8 +74,8 @@ void GimbalInit() .MaxOut = 500, }, .speed_PID = { - .Kp = 50,//40 - .Ki = 200, + .Kp = 50,//50 + .Ki = 200,//200 .Kd = 0, .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement, .IntegralLimit = 3000, @@ -111,7 +111,7 @@ void GimbalInit() .speed_PID = { .Kp=50,//50 .Ki =350,//350 - .Kd =0,//0.1 + .Kd =0,//0 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, .IntegralLimit =2500, .MaxOut = 20000, @@ -136,7 +136,7 @@ void GimbalInit() gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); } - +int aaaaaaa; /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ void GimbalTask() { @@ -178,7 +178,10 @@ void GimbalTask() default: break; } - +// if(yaw_motor->motor_measure.total_angle>120) +// { +// aaaaaaa++; +// } // 在合适的地方添加pitch重力补偿前馈力矩 // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩 // ... diff --git a/application/robot_def.h b/application/robot_def.h index f432ece..107eb7d 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -37,6 +37,8 @@ #define YAW_CHASSIS_ALIGN_ECD 2711 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 #define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 +#define PITCH_MAX_ANGLE 0 //云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) +#define PITCH_MIN_ANGLE 0 //云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) // 发射参数 #define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f @@ -132,6 +134,7 @@ typedef enum // 功率限制,从裁判系统获取 typedef struct { // 功率控制 + float chassis_power_mx; } Chassis_Power_Data_s; /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */ @@ -148,7 +151,7 @@ typedef struct float wz; // 旋转速度 float offset_angle; // 底盘和归中位置的夹角 chassis_mode_e chassis_mode; - + int chassis_speed_buff; // UI部分 // ... diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index 9cf70b4..7bb5282 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -84,7 +84,7 @@ void VisionSend(Vision_Send_s *send) static uint8_t send_buff[VISION_SEND_SIZE]; static uint16_t tx_len; // TODO: code to set flag_register - +flag_register = 30<<8|0b00000001; // 将数据转化为seasky协议的数据包 get_protocol_send_data(0x02, flag_register, &send->yaw, 3, send_buff, &tx_len); USARTSend(vision_usart_instance, send_buff, tx_len, USART_TRANSFER_DMA); // 和视觉通信使用IT,防止和接收使用的DMA冲突 diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 5e9917b..7e4361f 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -32,9 +32,10 @@ static void RectifyRCjoystick() * @param[out] rc_ctrl: remote control data struct point * @retval none */ +uint16_t aaaaa; static void sbus_to_rc(const uint8_t *sbus_buf) { - 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 @@ -53,32 +54,51 @@ static void sbus_to_rc(const uint8_t *sbus_buf) rc_ctrl[TEMP].mouse.press_l = sbus_buf[12]; //!< Mouse Left Is Press ? rc_ctrl[TEMP].mouse.press_r = sbus_buf[13]; //!< Mouse Right Is Press ? - // 按键值,每个键1bit,key_temp共16位;按键顺序在remote_control.h的宏定义中可见 - // 使用位域后不再需要这一中间操作 - rc_ctrl[TEMP].key_temp = sbus_buf[14] | (sbus_buf[15] << 8); //!< KeyBoard value + // 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位,msb在最后. 尚未测试 + *(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS] = (uint16_t)(sbus_buf[14] | (sbus_buf[15] << 8)); - // @todo 似乎可以直接用位域操作进行,把key_temp通过强制类型转换变成key类型? 位域方案在下面,尚未测试 - // 按键值解算,利用宏+循环减少代码长度 - for (uint16_t i = 0x0001, j = 0; i != 0x8000; i *= 2, j++) // 依次查看每一个键 + if (rc_ctrl[TEMP].key[KEY_PRESS].ctrl) + rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL] = rc_ctrl[TEMP].key[KEY_PRESS]; + else + memset(&rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL], 0, sizeof(Key_t)); + if (rc_ctrl[TEMP].key[KEY_PRESS].shift) + rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT] = rc_ctrl[TEMP].key[KEY_PRESS]; + else + memset(&rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT], 0, sizeof(Key_t)); + + for (uint32_t i = 0, j = 0x1; i < 16; j <<= 1, i++) { - // 如果键按下,对应键的key press状态置1,否则为0 - rc_ctrl[TEMP].key[KEY_PRESS][j] = rc_ctrl[TEMP].key_temp & i; - // 如果当前按下且上一次没按下,切换按键状态.一些模式要通过按键状态而不是按键是否按下来确定(实际上是大部分) - rc_ctrl[TEMP].key[KEY_STATE][j] = rc_ctrl[TEMP].key[KEY_PRESS][j] && !rc_ctrl[1].key[KEY_PRESS][j]; - // 检查是否有组合键按下 - if (rc_ctrl[TEMP].key_temp & 0x0001u << Key_Shift) // 按下ctrl - rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT][j] = rc_ctrl[TEMP].key_temp & i; - if (rc_ctrl[TEMP].key_temp & 0x0001u << Key_Ctrl) // 按下shift - rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL][j] = rc_ctrl[TEMP].key_temp & i; + if (((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS] & j) == j) && ((*(uint16_t *)&rc_ctrl[1].key[KEY_PRESS] & j) == 0) && ((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL] & j) != j) && ((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT] & j) != j)) + { + rc_ctrl[TEMP].key_count[KEY_PRESS][i]++; + + if (rc_ctrl[TEMP].key_count[KEY_PRESS][i] >= 240) + { + rc_ctrl[TEMP].key_count[KEY_PRESS][i] = 0; + } + + } + if (((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL] & j) == j) && ((*(uint16_t *)&rc_ctrl[1].key[KEY_PRESS_WITH_CTRL] & j) == 0)) + { + rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_CTRL][i]++; + + if (rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_CTRL][i] >= 240) + { + rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_CTRL][i] = 0; + } + } + if (((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT] & j) == j) && ((*(uint16_t *)&rc_ctrl[1].key[KEY_PRESS_WITH_SHIFT] & j) == 0)) + { + rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i]++; + + if (rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i] >= 240) + { + rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i] = 0; + } + } } - // 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位,msb在最后. 尚未测试 - // *(uint16_t *)&rc_ctrl[TEMP].key_test[KEY_PRESS] = (uint16_t)(sbus_buf[14] | (sbus_buf[15] << 8)); - // *(uint16_t *)&rc_ctrl[TEMP].key_test[KEY_STATE] = *(uint16_t *)&rc_ctrl[TEMP].key_test[KEY_PRESS] & ~(*(uint16_t *)&(rc_ctrl[1].key_test[KEY_PRESS])); - // if (rc_ctrl[TEMP].key_test[KEY_PRESS].ctrl) - // rc_ctrl[TEMP].key_test[KEY_PRESS_WITH_CTRL] = rc_ctrl[TEMP].key_test[KEY_PRESS]; - // if (rc_ctrl[TEMP].key_test[KEY_PRESS].shift) - // rc_ctrl[TEMP].key_test[Key_Shift] = rc_ctrl[TEMP].key_test[KEY_PRESS]; + memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断 } /** @@ -126,5 +146,4 @@ uint8_t RemotecontrolIsOnline() if (rc_init_flag) return DaemonIsOnline(rc_daemon_instance); return 0; - } \ No newline at end of file diff --git a/modules/remote/remote_control.h b/modules/remote/remote_control.h index d2c2187..8407ddd 100644 --- a/modules/remote/remote_control.h +++ b/modules/remote/remote_control.h @@ -24,8 +24,8 @@ // 获取按键操作 #define KEY_PRESS 0 #define KEY_STATE 1 -#define KEY_PRESS_WITH_CTRL 2 -#define KEY_PRESS_WITH_SHIFT 3 +#define KEY_PRESS_WITH_CTRL 1 +#define KEY_PRESS_WITH_SHIFT 2 // 检查接收值是否出错 #define RC_CH_VALUE_MIN ((uint16_t)364) @@ -42,6 +42,9 @@ #define switch_is_up(s) (s == RC_SW_UP) #define LEFT_SW 1 // 左侧开关 #define RIGHT_SW 0 // 右侧开关 +//键盘状态的宏 +#define key_is_press(s) (s == 1) +#define key_not_press(s) (s == 0) /* ----------------------- PC Key Definition-------------------------------- */ // 对应key[x][0~16],获取对应的键;例如通过key[KEY_PRESS][Key_W]获取W键是否按下,后续改为位域后删除 @@ -105,11 +108,10 @@ typedef struct uint8_t press_l; uint8_t press_r; } mouse; + + Key_t key[3]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍 - uint16_t key_temp; - uint8_t key[4][16]; // 当前使用的键盘索引 - // Key_t key_test[4]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍 - + uint8_t key_count[3][16]; } RC_ctrl_t; /* ------------------------- Internal Data ----------------------------------- */ From e949580221f91607a9827fdc36b4726178547680 Mon Sep 17 00:00:00 2001 From: chenfu <2412777093@qq.com> Date: Mon, 27 Mar 2023 22:05:54 +0800 Subject: [PATCH 10/12] =?UTF-8?q?=E6=9B=BE=E5=AE=B6=E9=94=AE=E9=BC=A0?= =?UTF-8?q?=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- modules/ps_handle/ps_handle.c | 115 ++++++++++++++++++++++++++++++++++ modules/ps_handle/ps_handle.h | 38 +++++++++++ 2 files changed, 153 insertions(+) create mode 100644 modules/ps_handle/ps_handle.c create mode 100644 modules/ps_handle/ps_handle.h diff --git a/modules/ps_handle/ps_handle.c b/modules/ps_handle/ps_handle.c new file mode 100644 index 0000000..8928512 --- /dev/null +++ b/modules/ps_handle/ps_handle.c @@ -0,0 +1,115 @@ +#include "ps_handle.h" +uint8_t PS2_RawData[9] = {0}; +PS2_Instance PS2_Data = {0}; +void PS2_CS(uint8_t Val) +{ + if (Val) + HAL_GPIO_WritePin(PS2_CS_GPIOx, PS2_CS_Pin, GPIO_PIN_SET); + else + HAL_GPIO_WritePin(PS2_CS_GPIOx, PS2_CS_Pin, GPIO_PIN_RESET); +} +void PS2_CLK(uint8_t Val) +{ + if (Val) + HAL_GPIO_WritePin(PS2_CLK_GPIOx, PS2_CLK_Pin, GPIO_PIN_SET); + else + HAL_GPIO_WritePin(PS2_CLK_GPIOx, PS2_CLK_Pin, GPIO_PIN_RESET); +} +void PS2_DO(uint8_t Val) +{ + if (Val) + HAL_GPIO_WritePin(PS2_DO_GPIOx, PS2_DO_Pin, GPIO_PIN_SET); + else + HAL_GPIO_WritePin(PS2_DO_GPIOx, PS2_DO_Pin, GPIO_PIN_RESET); +} +uint8_t PS2_Read_DI() +{ + return HAL_GPIO_ReadPin(PS2_DI_GPIOx, PS2_DI_Pin); +} +void PS2_Delay() +{ + for (int i = 0; i < 0xBf; i++) + __NOP(); +} +uint8_t PS2_ReadWrite_Byte(uint8_t TxData) +{ + uint8_t TX = TxData; + uint8_t RX = 0; + for (int i = 0; i < 8; i++) + { + if (TX & 0x01) + PS2_DO(1); + else + PS2_DO(0); + TX >>= 1; + PS2_CLK(1); + PS2_Delay(); + PS2_CLK(0); + RX >>= 1; + RX |= (PS2_Read_DI() << 7); + PS2_Delay(); + PS2_CLK(1); + PS2_Delay(); + } + return RX; +} + +static void PS2_Decode() +{ + if (PS2_RawData[2] == 0x5A) + { + PS2_Data.Key_Select = (~PS2_RawData[3] >> 0) & 0x01; //选择键 + PS2_Data.Key_Start = (~PS2_RawData[3] >> 3) & 0x01; //开始键 + + //左侧按键 + PS2_Data.Key_L_Up = (~PS2_RawData[3] >> 4) & 0x01; + PS2_Data.Key_L_Right = (~PS2_RawData[3] >> 5) & 0x01; + PS2_Data.Key_L_Down = (~PS2_RawData[3] >> 6) & 0x01; + PS2_Data.Key_L_Left = (~PS2_RawData[3] >> 7) & 0x01; + + //后侧按键 + PS2_Data.Key_L2 = (~PS2_RawData[4] >> 0) & 0x01; + PS2_Data.Key_R2 = (~PS2_RawData[4] >> 1) & 0x01; + PS2_Data.Key_L1 = (~PS2_RawData[4] >> 2) & 0x01; + PS2_Data.Key_R1 = (~PS2_RawData[4] >> 3) & 0x01; + + //右侧按键 + PS2_Data.Key_R_Up = (~PS2_RawData[4] >> 4) & 0x01; + PS2_Data.Key_R_Right = (~PS2_RawData[4] >> 5) & 0x01; + PS2_Data.Key_R_Down = (~PS2_RawData[4] >> 6) & 0x01; + PS2_Data.Key_R_Left = (~PS2_RawData[4] >> 7) & 0x01; + + if (PS2_RawData[1] == 0x41) + { //无灯模式(摇杆值八向) + PS2_Data.Rocker_LX = 127 * (PS2_Data.Key_L_Right - PS2_Data.Key_L_Left); + PS2_Data.Rocker_LY = 127 * (PS2_Data.Key_L_Up - PS2_Data.Key_L_Down); + + PS2_Data.Rocker_RX = 127 * (PS2_Data.Key_R_Right - PS2_Data.Key_R_Left); + PS2_Data.Rocker_RY = 127 * (PS2_Data.Key_R_Up - PS2_Data.Key_R_Down); + } + else if (PS2_RawData[1] == 0x73) + { //红灯模式(摇杆值模拟) + + //摇杆按键 + PS2_Data.Key_Rocker_Left = (~PS2_RawData[3] >> 1) & 0x01; + PS2_Data.Key_Rocker_Right = (~PS2_RawData[3] >> 2) & 0x01; + + //摇杆值 + PS2_Data.Rocker_LX = PS2_RawData[7] - 0x80; + PS2_Data.Rocker_LY = -1 - (PS2_RawData[8] - 0x80); + PS2_Data.Rocker_RX = PS2_RawData[5] - 0x80; + PS2_Data.Rocker_RY = -1 - (PS2_RawData[6] - 0x80); + } + } +} +void PS2_Read_Data(void) +{ + PS2_CS(0); + PS2_RawData[0] = PS2_ReadWrite_Byte(0x01); // 0 + PS2_RawData[1] = PS2_ReadWrite_Byte(0x42); // 1 + for (int i = 2; i < 9; i++) + PS2_RawData[i] = PS2_ReadWrite_Byte(0xff); + PS2_CS(1); + PS2_Decode(); +} + diff --git a/modules/ps_handle/ps_handle.h b/modules/ps_handle/ps_handle.h new file mode 100644 index 0000000..bd35311 --- /dev/null +++ b/modules/ps_handle/ps_handle.h @@ -0,0 +1,38 @@ +#ifndef PS_HANDLE_H +#define PS_HANDLE_H + +#include "bsp_spi.h" +#include "bsp_gpio.h" +#include "bsp_dwt.h" + +#define PS2_CS_GPIOx GPIOB +#define PS2_CS_Pin GPIO_PIN_12 + +#define PS2_CLK_GPIOx GPIOB +#define PS2_CLK_Pin GPIO_PIN_13 + +#define PS2_DO_GPIOx GPIOB +#define PS2_DO_Pin GPIO_PIN_15 + +#define PS2_DI_GPIOx GPIOB +#define PS2_DI_Pin GPIO_PIN_14 + +typedef struct +{ + uint8_t A_D; //模拟(红灯)为1 数字(无灯)为0 + int8_t Rocker_RX, Rocker_RY, Rocker_LX, Rocker_LY; //摇杆值(模拟状态为实际值0-0xFF)(数字态为等效的值0,0x80,0xFF) + //按键值0为未触发,1为触发态 + uint8_t Key_L1, Key_L2, Key_R1, Key_R2; //后侧大按键 + uint8_t Key_L_Right, Key_L_Left, Key_L_Up, Key_L_Down; //左侧按键 + uint8_t Key_R_Right, Key_R_Left, Key_R_Up, Key_R_Down; //右侧按键 + uint8_t Key_Select; //选择键 + uint8_t Key_Start; //开始键 + uint8_t Key_Rocker_Left, Key_Rocker_Right; //摇杆按键 + +} PS2_Instance; + + + + + +#endif // !PS_HANDLE_H#define PS_HANDLE_H From f3ad8040483b2afbcd1c29fd4c5cf7682648cda3 Mon Sep 17 00:00:00 2001 From: chenfu <2412777093@qq.com> Date: Mon, 27 Mar 2023 22:03:35 +0800 Subject: [PATCH 11/12] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E9=94=AE=E9=BC=A0?= =?UTF-8?q?=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- HAL_N_Middlewares/Src/can.c | 4 +- application/chassis/chassis.c | 12 +- application/cmd/robot_cmd.c | 157 ++++++++++++++++++------ application/gimbal/gimbal.c | 13 +- application/robot_def.h | 5 +- modules/master_machine/master_process.c | 2 +- modules/remote/remote_control.c | 67 ++++++---- modules/remote/remote_control.h | 14 ++- 8 files changed, 193 insertions(+), 81 deletions(-) diff --git a/HAL_N_Middlewares/Src/can.c b/HAL_N_Middlewares/Src/can.c index c4c0948..292573f 100644 --- a/HAL_N_Middlewares/Src/can.c +++ b/HAL_N_Middlewares/Src/can.c @@ -122,7 +122,7 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle) HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); /* CAN1 interrupt Init */ - HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 6, 0); + HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0); HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 5, 0); HAL_NVIC_EnableIRQ(CAN1_RX1_IRQn); @@ -155,7 +155,7 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle) HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* CAN2 interrupt Init */ - HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 6, 0); + HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0); HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn); HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0); HAL_NVIC_EnableIRQ(CAN2_RX1_IRQn); diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 1af1908..34a66ef 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -19,7 +19,7 @@ // referee需要移动到module层 ///////////////////////// -#include "referee.h" + #include "rm_referee.h" ///////////////////////// @@ -68,16 +68,16 @@ void ChassisInit() .can_init_config.can_handle = &hcan1, .controller_param_init_config = { .speed_PID = { - .Kp = 4.5,//9 - .Ki = 0,//0.02 - .Kd = 0.01,//0.01 + .Kp = 10,//4.5 + .Ki = 0,//0 + .Kd = 0,//0 .IntegralLimit = 3000, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, .MaxOut = 12000, }, .current_PID = { - .Kp = 0.4,//0.7 - .Ki = 0,//0.1 + .Kp = 0.5,//0.4 + .Ki = 0,//0 .Kd = 0, .IntegralLimit = 3000, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 4883e04..5e4f787 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -21,11 +21,12 @@ static CANCommInstance *cmd_can_comm; // 双板通信 #ifdef ONE_BOARD static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者 static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者 -#endif // ONE_BOARD +#endif // ONE_BOARD 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; // 视觉发送数据 @@ -68,7 +69,7 @@ void RobotCMDInit() }; cmd_can_comm = CANCommInit(&comm_conf); #endif // GIMBAL_BOARD -gimbal_cmd_send.pitch = 0; + gimbal_cmd_send.pitch = 0; robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 } @@ -81,9 +82,9 @@ gimbal_cmd_send.pitch = 0; static void CalcOffsetAngle() { // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数 - static float angle; + static float angle; angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度 -#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度 +#if YAW_ECD_GREATER_THAN_4096 // 如果大于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) @@ -100,17 +101,6 @@ static void CalcOffsetAngle() #endif } -static void OffsetAnglePidCalc() -{ - // float pid_measure,pid_ref; - // static PIDInstance AngleCal = { - // .Kp = -1, - // .Ki = 0, - // .Kd = 0, - // .MaxOut = 10000, - // }; - // chassis_cmd_send.offset_angle = PIDCalculate(&AngleCal,chassis_cmd_send.offset_angle,0); -} /** * @brief 控制输入为遥控器(调试时)的模式和控制量设置 * @@ -119,11 +109,15 @@ static void RemoteControlSet() { // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台 - {chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;} + { + chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 - {chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; - gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;} + { + chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; + gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; + } // 云台参数,确定云台控制数据 if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式 @@ -136,25 +130,24 @@ static void RemoteControlSet() { // 按照摇杆的输出大小进行角度增量,增益系数需调整 gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_; gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1; - // if (gimbal_cmd_send.pitch <= 122) - // { - // gimbal_cmd_send.pitch =125; - // } - // else if (gimbal_cmd_send.pitch >= 175) - // { - // gimbal_cmd_send.pitch = 174; - // } - + // 摇杆控制的软件限位 + // if (gimbal_cmd_send.pitch <= PITCH_MIN_ECD) + // { + // gimbal_cmd_send.pitch = PITCH_MIN_ECD; + // } + // else if (gimbal_cmd_send.pitch >= PITCH_MAX_ECD) + // { + // gimbal_cmd_send.pitch = PITCH_MAX_ECD; + // } } // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向 chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向 - //chassis_cmd_send.wz = 300; // 发射参数 if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 - ; // 弹舱舵机控制,待添加servo_motor模块,开启 + ; // 弹舱舵机控制,待添加servo_motor模块,开启 else ; // 弹舱舵机控制,待添加servo_motor模块,关闭 @@ -163,7 +156,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 < -500) shoot_cmd_send.load_mode = LOAD_BURSTFIRE; else @@ -178,14 +171,89 @@ static void RemoteControlSet() */ static void MouseKeySet() { - // 待添加键鼠控制 - // ... + chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测 + chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300; + + gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测 + gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10; + + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速 + { + case 0: + shoot_cmd_send.bullet_speed = 15; + break; + case 1: + shoot_cmd_send.bullet_speed = 18; + break; + default: + shoot_cmd_send.bullet_speed = 30; + break; + } + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式 + { + case 0: + shoot_cmd_send.load_mode = LOAD_STOP; + break; + case 1: + shoot_cmd_send.load_mode = LOAD_1_BULLET; + break; + case 2: + shoot_cmd_send.load_mode = LOAD_3_BULLET; + break; + default: + shoot_cmd_send.load_mode = LOAD_BURSTFIRE; + break; + } + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱 + { + case 0: + shoot_cmd_send.lid_mode = LID_OPEN; + break; + default: + shoot_cmd_send.lid_mode = LID_CLOSE; + break; + } + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮 + { + case 0: + shoot_cmd_send.friction_mode = FRICTION_OFF; + break; + default: + shoot_cmd_send.friction_mode = FRICTION_ON; + break; + } + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度 + { + case 0: + chassis_cmd_send.chassis_speed_buff = 40; + break; + case 1: + chassis_cmd_send.chassis_speed_buff = 60; + break; + case 2: + chassis_cmd_send.chassis_speed_buff = 80; + break; + default: + chassis_cmd_send.chassis_speed_buff = 100; + break; + } + switch (rc_data[TEMP].key[KEY_PRESS].shift) //待添加 按shift允许超功率 消耗缓冲能量 + { + case 1: + + break; + + default: + + break; + } + } /** * @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等 * 停止的阈值'300'待修改成合适的值,或改为开关控制. - * + * * @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现 * */ @@ -204,9 +272,25 @@ static void EmergencyHandler() // 遥控器右侧开关为[上],恢复正常运行 if (switch_is_up(rc_data[TEMP].rc.switch_right)) { - robot_state = ROBOT_READY; + robot_state = ROBOT_READY; shoot_cmd_send.shoot_mode = SHOOT_ON; } + 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; + break; + } } /* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */ @@ -237,7 +321,8 @@ void RobotCMDTask() vision_send_data.enemy_color = 0; vision_send_data.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; vision_send_data.yaw = gimbal_fetch_data.gimbal_imu_data.Yaw; - vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll;; + vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll; + ; // 推送消息,双板通信,视觉通信等 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index f02ce9c..327fd79 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -74,8 +74,8 @@ void GimbalInit() .MaxOut = 500, }, .speed_PID = { - .Kp = 50,//40 - .Ki = 200, + .Kp = 50,//50 + .Ki = 200,//200 .Kd = 0, .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement, .IntegralLimit = 3000, @@ -111,7 +111,7 @@ void GimbalInit() .speed_PID = { .Kp=50,//50 .Ki =350,//350 - .Kd =0,//0.1 + .Kd =0,//0 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, .IntegralLimit =2500, .MaxOut = 20000, @@ -136,7 +136,7 @@ void GimbalInit() gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); } - +int aaaaaaa; /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ void GimbalTask() { @@ -178,7 +178,10 @@ void GimbalTask() default: break; } - +// if(yaw_motor->motor_measure.total_angle>120) +// { +// aaaaaaa++; +// } // 在合适的地方添加pitch重力补偿前馈力矩 // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩 // ... diff --git a/application/robot_def.h b/application/robot_def.h index f432ece..107eb7d 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -37,6 +37,8 @@ #define YAW_CHASSIS_ALIGN_ECD 2711 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 #define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 +#define PITCH_MAX_ANGLE 0 //云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) +#define PITCH_MIN_ANGLE 0 //云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) // 发射参数 #define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f @@ -132,6 +134,7 @@ typedef enum // 功率限制,从裁判系统获取 typedef struct { // 功率控制 + float chassis_power_mx; } Chassis_Power_Data_s; /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */ @@ -148,7 +151,7 @@ typedef struct float wz; // 旋转速度 float offset_angle; // 底盘和归中位置的夹角 chassis_mode_e chassis_mode; - + int chassis_speed_buff; // UI部分 // ... diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index 9cf70b4..7bb5282 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -84,7 +84,7 @@ void VisionSend(Vision_Send_s *send) static uint8_t send_buff[VISION_SEND_SIZE]; static uint16_t tx_len; // TODO: code to set flag_register - +flag_register = 30<<8|0b00000001; // 将数据转化为seasky协议的数据包 get_protocol_send_data(0x02, flag_register, &send->yaw, 3, send_buff, &tx_len); USARTSend(vision_usart_instance, send_buff, tx_len, USART_TRANSFER_DMA); // 和视觉通信使用IT,防止和接收使用的DMA冲突 diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 5e9917b..7e4361f 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -32,9 +32,10 @@ static void RectifyRCjoystick() * @param[out] rc_ctrl: remote control data struct point * @retval none */ +uint16_t aaaaa; static void sbus_to_rc(const uint8_t *sbus_buf) { - 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 @@ -53,32 +54,51 @@ static void sbus_to_rc(const uint8_t *sbus_buf) rc_ctrl[TEMP].mouse.press_l = sbus_buf[12]; //!< Mouse Left Is Press ? rc_ctrl[TEMP].mouse.press_r = sbus_buf[13]; //!< Mouse Right Is Press ? - // 按键值,每个键1bit,key_temp共16位;按键顺序在remote_control.h的宏定义中可见 - // 使用位域后不再需要这一中间操作 - rc_ctrl[TEMP].key_temp = sbus_buf[14] | (sbus_buf[15] << 8); //!< KeyBoard value + // 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位,msb在最后. 尚未测试 + *(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS] = (uint16_t)(sbus_buf[14] | (sbus_buf[15] << 8)); - // @todo 似乎可以直接用位域操作进行,把key_temp通过强制类型转换变成key类型? 位域方案在下面,尚未测试 - // 按键值解算,利用宏+循环减少代码长度 - for (uint16_t i = 0x0001, j = 0; i != 0x8000; i *= 2, j++) // 依次查看每一个键 + if (rc_ctrl[TEMP].key[KEY_PRESS].ctrl) + rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL] = rc_ctrl[TEMP].key[KEY_PRESS]; + else + memset(&rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL], 0, sizeof(Key_t)); + if (rc_ctrl[TEMP].key[KEY_PRESS].shift) + rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT] = rc_ctrl[TEMP].key[KEY_PRESS]; + else + memset(&rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT], 0, sizeof(Key_t)); + + for (uint32_t i = 0, j = 0x1; i < 16; j <<= 1, i++) { - // 如果键按下,对应键的key press状态置1,否则为0 - rc_ctrl[TEMP].key[KEY_PRESS][j] = rc_ctrl[TEMP].key_temp & i; - // 如果当前按下且上一次没按下,切换按键状态.一些模式要通过按键状态而不是按键是否按下来确定(实际上是大部分) - rc_ctrl[TEMP].key[KEY_STATE][j] = rc_ctrl[TEMP].key[KEY_PRESS][j] && !rc_ctrl[1].key[KEY_PRESS][j]; - // 检查是否有组合键按下 - if (rc_ctrl[TEMP].key_temp & 0x0001u << Key_Shift) // 按下ctrl - rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT][j] = rc_ctrl[TEMP].key_temp & i; - if (rc_ctrl[TEMP].key_temp & 0x0001u << Key_Ctrl) // 按下shift - rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL][j] = rc_ctrl[TEMP].key_temp & i; + if (((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS] & j) == j) && ((*(uint16_t *)&rc_ctrl[1].key[KEY_PRESS] & j) == 0) && ((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL] & j) != j) && ((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT] & j) != j)) + { + rc_ctrl[TEMP].key_count[KEY_PRESS][i]++; + + if (rc_ctrl[TEMP].key_count[KEY_PRESS][i] >= 240) + { + rc_ctrl[TEMP].key_count[KEY_PRESS][i] = 0; + } + + } + if (((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL] & j) == j) && ((*(uint16_t *)&rc_ctrl[1].key[KEY_PRESS_WITH_CTRL] & j) == 0)) + { + rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_CTRL][i]++; + + if (rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_CTRL][i] >= 240) + { + rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_CTRL][i] = 0; + } + } + if (((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT] & j) == j) && ((*(uint16_t *)&rc_ctrl[1].key[KEY_PRESS_WITH_SHIFT] & j) == 0)) + { + rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i]++; + + if (rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i] >= 240) + { + rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i] = 0; + } + } } - // 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位,msb在最后. 尚未测试 - // *(uint16_t *)&rc_ctrl[TEMP].key_test[KEY_PRESS] = (uint16_t)(sbus_buf[14] | (sbus_buf[15] << 8)); - // *(uint16_t *)&rc_ctrl[TEMP].key_test[KEY_STATE] = *(uint16_t *)&rc_ctrl[TEMP].key_test[KEY_PRESS] & ~(*(uint16_t *)&(rc_ctrl[1].key_test[KEY_PRESS])); - // if (rc_ctrl[TEMP].key_test[KEY_PRESS].ctrl) - // rc_ctrl[TEMP].key_test[KEY_PRESS_WITH_CTRL] = rc_ctrl[TEMP].key_test[KEY_PRESS]; - // if (rc_ctrl[TEMP].key_test[KEY_PRESS].shift) - // rc_ctrl[TEMP].key_test[Key_Shift] = rc_ctrl[TEMP].key_test[KEY_PRESS]; + memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断 } /** @@ -126,5 +146,4 @@ uint8_t RemotecontrolIsOnline() if (rc_init_flag) return DaemonIsOnline(rc_daemon_instance); return 0; - } \ No newline at end of file diff --git a/modules/remote/remote_control.h b/modules/remote/remote_control.h index d2c2187..8407ddd 100644 --- a/modules/remote/remote_control.h +++ b/modules/remote/remote_control.h @@ -24,8 +24,8 @@ // 获取按键操作 #define KEY_PRESS 0 #define KEY_STATE 1 -#define KEY_PRESS_WITH_CTRL 2 -#define KEY_PRESS_WITH_SHIFT 3 +#define KEY_PRESS_WITH_CTRL 1 +#define KEY_PRESS_WITH_SHIFT 2 // 检查接收值是否出错 #define RC_CH_VALUE_MIN ((uint16_t)364) @@ -42,6 +42,9 @@ #define switch_is_up(s) (s == RC_SW_UP) #define LEFT_SW 1 // 左侧开关 #define RIGHT_SW 0 // 右侧开关 +//键盘状态的宏 +#define key_is_press(s) (s == 1) +#define key_not_press(s) (s == 0) /* ----------------------- PC Key Definition-------------------------------- */ // 对应key[x][0~16],获取对应的键;例如通过key[KEY_PRESS][Key_W]获取W键是否按下,后续改为位域后删除 @@ -105,11 +108,10 @@ typedef struct uint8_t press_l; uint8_t press_r; } mouse; + + Key_t key[3]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍 - uint16_t key_temp; - uint8_t key[4][16]; // 当前使用的键盘索引 - // Key_t key_test[4]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍 - + uint8_t key_count[3][16]; } RC_ctrl_t; /* ------------------------- Internal Data ----------------------------------- */ From 3f0325df28b919dc0b8cba9dc12adfa0f2b52b6c Mon Sep 17 00:00:00 2001 From: chenfu <2412777093@qq.com> Date: Mon, 27 Mar 2023 22:05:54 +0800 Subject: [PATCH 12/12] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E9=94=AE=E9=BC=A0?= =?UTF-8?q?=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- modules/ps_handle/ps_handle.c | 115 ++++++++++++++++++++++++++++++++++ modules/ps_handle/ps_handle.h | 38 +++++++++++ 2 files changed, 153 insertions(+) create mode 100644 modules/ps_handle/ps_handle.c create mode 100644 modules/ps_handle/ps_handle.h diff --git a/modules/ps_handle/ps_handle.c b/modules/ps_handle/ps_handle.c new file mode 100644 index 0000000..8928512 --- /dev/null +++ b/modules/ps_handle/ps_handle.c @@ -0,0 +1,115 @@ +#include "ps_handle.h" +uint8_t PS2_RawData[9] = {0}; +PS2_Instance PS2_Data = {0}; +void PS2_CS(uint8_t Val) +{ + if (Val) + HAL_GPIO_WritePin(PS2_CS_GPIOx, PS2_CS_Pin, GPIO_PIN_SET); + else + HAL_GPIO_WritePin(PS2_CS_GPIOx, PS2_CS_Pin, GPIO_PIN_RESET); +} +void PS2_CLK(uint8_t Val) +{ + if (Val) + HAL_GPIO_WritePin(PS2_CLK_GPIOx, PS2_CLK_Pin, GPIO_PIN_SET); + else + HAL_GPIO_WritePin(PS2_CLK_GPIOx, PS2_CLK_Pin, GPIO_PIN_RESET); +} +void PS2_DO(uint8_t Val) +{ + if (Val) + HAL_GPIO_WritePin(PS2_DO_GPIOx, PS2_DO_Pin, GPIO_PIN_SET); + else + HAL_GPIO_WritePin(PS2_DO_GPIOx, PS2_DO_Pin, GPIO_PIN_RESET); +} +uint8_t PS2_Read_DI() +{ + return HAL_GPIO_ReadPin(PS2_DI_GPIOx, PS2_DI_Pin); +} +void PS2_Delay() +{ + for (int i = 0; i < 0xBf; i++) + __NOP(); +} +uint8_t PS2_ReadWrite_Byte(uint8_t TxData) +{ + uint8_t TX = TxData; + uint8_t RX = 0; + for (int i = 0; i < 8; i++) + { + if (TX & 0x01) + PS2_DO(1); + else + PS2_DO(0); + TX >>= 1; + PS2_CLK(1); + PS2_Delay(); + PS2_CLK(0); + RX >>= 1; + RX |= (PS2_Read_DI() << 7); + PS2_Delay(); + PS2_CLK(1); + PS2_Delay(); + } + return RX; +} + +static void PS2_Decode() +{ + if (PS2_RawData[2] == 0x5A) + { + PS2_Data.Key_Select = (~PS2_RawData[3] >> 0) & 0x01; //选择键 + PS2_Data.Key_Start = (~PS2_RawData[3] >> 3) & 0x01; //开始键 + + //左侧按键 + PS2_Data.Key_L_Up = (~PS2_RawData[3] >> 4) & 0x01; + PS2_Data.Key_L_Right = (~PS2_RawData[3] >> 5) & 0x01; + PS2_Data.Key_L_Down = (~PS2_RawData[3] >> 6) & 0x01; + PS2_Data.Key_L_Left = (~PS2_RawData[3] >> 7) & 0x01; + + //后侧按键 + PS2_Data.Key_L2 = (~PS2_RawData[4] >> 0) & 0x01; + PS2_Data.Key_R2 = (~PS2_RawData[4] >> 1) & 0x01; + PS2_Data.Key_L1 = (~PS2_RawData[4] >> 2) & 0x01; + PS2_Data.Key_R1 = (~PS2_RawData[4] >> 3) & 0x01; + + //右侧按键 + PS2_Data.Key_R_Up = (~PS2_RawData[4] >> 4) & 0x01; + PS2_Data.Key_R_Right = (~PS2_RawData[4] >> 5) & 0x01; + PS2_Data.Key_R_Down = (~PS2_RawData[4] >> 6) & 0x01; + PS2_Data.Key_R_Left = (~PS2_RawData[4] >> 7) & 0x01; + + if (PS2_RawData[1] == 0x41) + { //无灯模式(摇杆值八向) + PS2_Data.Rocker_LX = 127 * (PS2_Data.Key_L_Right - PS2_Data.Key_L_Left); + PS2_Data.Rocker_LY = 127 * (PS2_Data.Key_L_Up - PS2_Data.Key_L_Down); + + PS2_Data.Rocker_RX = 127 * (PS2_Data.Key_R_Right - PS2_Data.Key_R_Left); + PS2_Data.Rocker_RY = 127 * (PS2_Data.Key_R_Up - PS2_Data.Key_R_Down); + } + else if (PS2_RawData[1] == 0x73) + { //红灯模式(摇杆值模拟) + + //摇杆按键 + PS2_Data.Key_Rocker_Left = (~PS2_RawData[3] >> 1) & 0x01; + PS2_Data.Key_Rocker_Right = (~PS2_RawData[3] >> 2) & 0x01; + + //摇杆值 + PS2_Data.Rocker_LX = PS2_RawData[7] - 0x80; + PS2_Data.Rocker_LY = -1 - (PS2_RawData[8] - 0x80); + PS2_Data.Rocker_RX = PS2_RawData[5] - 0x80; + PS2_Data.Rocker_RY = -1 - (PS2_RawData[6] - 0x80); + } + } +} +void PS2_Read_Data(void) +{ + PS2_CS(0); + PS2_RawData[0] = PS2_ReadWrite_Byte(0x01); // 0 + PS2_RawData[1] = PS2_ReadWrite_Byte(0x42); // 1 + for (int i = 2; i < 9; i++) + PS2_RawData[i] = PS2_ReadWrite_Byte(0xff); + PS2_CS(1); + PS2_Decode(); +} + diff --git a/modules/ps_handle/ps_handle.h b/modules/ps_handle/ps_handle.h new file mode 100644 index 0000000..bd35311 --- /dev/null +++ b/modules/ps_handle/ps_handle.h @@ -0,0 +1,38 @@ +#ifndef PS_HANDLE_H +#define PS_HANDLE_H + +#include "bsp_spi.h" +#include "bsp_gpio.h" +#include "bsp_dwt.h" + +#define PS2_CS_GPIOx GPIOB +#define PS2_CS_Pin GPIO_PIN_12 + +#define PS2_CLK_GPIOx GPIOB +#define PS2_CLK_Pin GPIO_PIN_13 + +#define PS2_DO_GPIOx GPIOB +#define PS2_DO_Pin GPIO_PIN_15 + +#define PS2_DI_GPIOx GPIOB +#define PS2_DI_Pin GPIO_PIN_14 + +typedef struct +{ + uint8_t A_D; //模拟(红灯)为1 数字(无灯)为0 + int8_t Rocker_RX, Rocker_RY, Rocker_LX, Rocker_LY; //摇杆值(模拟状态为实际值0-0xFF)(数字态为等效的值0,0x80,0xFF) + //按键值0为未触发,1为触发态 + uint8_t Key_L1, Key_L2, Key_R1, Key_R2; //后侧大按键 + uint8_t Key_L_Right, Key_L_Left, Key_L_Up, Key_L_Down; //左侧按键 + uint8_t Key_R_Right, Key_R_Left, Key_R_Up, Key_R_Down; //右侧按键 + uint8_t Key_Select; //选择键 + uint8_t Key_Start; //开始键 + uint8_t Key_Rocker_Left, Key_Rocker_Right; //摇杆按键 + +} PS2_Instance; + + + + + +#endif // !PS_HANDLE_H#define PS_HANDLE_H