From 41d033e3f8c487abc1e6dfec38fdaf0b9bc6cc0d Mon Sep 17 00:00:00 2001 From: NeoZng Date: Thu, 23 Mar 2023 18:22:24 +0800 Subject: [PATCH 1/3] =?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 2/3] 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 3/3] =?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打断的情况。 +