From a2a83f9fbfa43fbb89c67fb4bdf6ee966c53bd6d Mon Sep 17 00:00:00 2001 From: NeoZng Date: Thu, 16 Feb 2023 15:46:04 +0800 Subject: [PATCH] =?UTF-8?q?=20=E4=BF=AE=E5=A4=8DLK=E7=94=B5=E6=9C=BAid?= =?UTF-8?q?=E8=AE=A1=E7=AE=97=E9=94=99=E8=AF=AF=EF=BC=8C=E6=9E=84=E5=BB=BA?= =?UTF-8?q?=E5=B9=B3=E8=A1=A1=E5=BA=95=E7=9B=98=E6=A1=86=E6=9E=B6=EF=BC=8C?= =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E9=80=9A=E7=94=A8=E9=80=9A=E4=BF=A1=E6=A8=A1?= =?UTF-8?q?=E5=9D=97=EF=BC=8C=E5=A2=9E=E5=8A=A0=E5=B9=B3=E8=A1=A1=E5=BA=95?= =?UTF-8?q?=E7=9B=98=E6=9D=A1=E4=BB=B6=E7=BC=96=E8=AF=91=E5=85=BC=E5=AE=B9?= =?UTF-8?q?=EF=BC=8C=E5=88=A0=E9=99=A4lqr?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Makefile | 19 +- TODO.md | 12 ++ application/balance_chassis/balance.c | 229 +++++++++++++++++++++++ application/balance_chassis/balance.h | 17 ++ application/balance_chassis/balance.md | 24 +++ application/balance_chassis/gain_table.h | 24 +++ application/chassis/chassis.c | 2 +- application/grab/grab.md | 2 +- application/lift/lift.md | 4 +- application/robot_def.h | 4 +- bsp/can/bsp_can.c | 2 +- bsp/gpio/bsp_gpio.c | 37 ++-- bsp/spi/bsp_spi.c | 20 +- bsp/spi/bsp_spi.h | 8 +- modules/BMI088/bmi088.h | 2 +- modules/BMI088/bmi088.md | 16 +- modules/algorithm/LQR.c | 1 - modules/algorithm/LQR.h | 12 -- modules/algorithm/controller.c | 38 ++-- modules/algorithm/controller.h | 46 ++--- modules/algorithm/kalman_filter.h | 1 + modules/algorithm/user_lib.c | 9 +- modules/imu/ins_task.c | 20 +- modules/ist8310/ist8310.md | 4 +- modules/led/led.c | 25 +-- modules/motor/DJImotor/dji_motor.c | 12 +- modules/motor/DJImotor/dji_motor.md | 78 +++----- modules/motor/HTmotor/HT04.c | 18 +- modules/motor/LKmotor/LK9025.c | 32 ++-- modules/motor/LKmotor/LK9025.h | 59 ++++-- modules/motor/LKmotor/LK_motor.md | 2 +- modules/oled/oled.md | 10 +- modules/unicomm/unicomm.c | 0 modules/unicomm/unicomm.h | 0 modules/unicomm/unicomm.md | 7 + 35 files changed, 554 insertions(+), 242 deletions(-) create mode 100644 application/balance_chassis/balance.c create mode 100644 application/balance_chassis/balance.h create mode 100644 application/balance_chassis/balance.md create mode 100644 application/balance_chassis/gain_table.h delete mode 100644 modules/algorithm/LQR.c delete mode 100644 modules/algorithm/LQR.h create mode 100644 modules/unicomm/unicomm.c create mode 100644 modules/unicomm/unicomm.h create mode 100644 modules/unicomm/unicomm.md diff --git a/Makefile b/Makefile index d18bc88..5b2dd4e 100644 --- a/Makefile +++ b/Makefile @@ -1,14 +1,3 @@ -########################################################################################################################## -# File automatically-generated by tool: [projectgenerator] version: [3.17.1] date: [Thu Oct 20 16:31:27 CST 2022] -########################################################################################################################## - -# ------------------------------------------------ -# Generic Makefile (based on gcc) -# -# ChangeLog : -# 2017-02-10 - Several enhancements + project update mode -# 2015-07-22 - first version -# ------------------------------------------------ ###################################### # target @@ -24,7 +13,7 @@ SHELL = cmd.exe DEBUG = 1 # optimization OPT = -O0 # O0避免没有使用到的变量被优化,如果没有特殊的调试需求请修改成-Og. - # 为了更高的性能,正式上车不需要调试时修改成-O3 + # 为了更高的性能,正式上车不需要调试时修改成-O3/-Ofast ####################################### @@ -156,6 +145,7 @@ application/gimbal/gimbal.c \ application/chassis/chassis.c \ application/shoot/shoot.c \ application/cmd/robot_cmd.c \ +application/balance_chassis/balance.c \ application/robot.c @@ -236,6 +226,7 @@ C_INCLUDES = \ -Iapplication/shoot \ -Iapplication/gimbal \ -Iapplication/cmd \ +-Iapplication/balance_chassis \ -Iapplication \ -Ibsp/dwt \ -Ibsp/can \ @@ -269,7 +260,7 @@ C_INCLUDES = \ -Imodules/message_center \ -Imodules/daemon \ -Imodules/vofa \ --Imodules/ +-Imodules # compile gcc flags ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections @@ -341,7 +332,7 @@ clean: # linux: rm -rf $(BUILD_DIR) ####################################### -# download without debugging +# download directl without debugging ####################################### OPENOCD_FLASH_START = 0x08000000 # 如果切换芯片可能需要修改此值 diff --git a/TODO.md b/TODO.md index fbbf138..379dbd4 100644 --- a/TODO.md +++ b/TODO.md @@ -141,6 +141,7 @@ #### controller - [x] 增加前馈数据 +- [ ] 将PID的初始化改写为PIDRegister的形式,在controller统一分配内存. #### user_lib @@ -150,11 +151,22 @@ - [ ] 增加3508和2006的开环零位校准函数 - [ ] 为实例增加低通滤波系数变量，使不同电机有不同的配置 +- [ ] 正反转标志位设置,需要修改反馈量和pid计算 +#### LKmotor +- [ ] 正反转标志位设置,需要修改反馈量和pid计算 + +#### HTmotor + +- [ ] 正反转标志位设置,需要修改反馈量和pid计算 ### 待添加 +#### unicomm + +- [ ] 完成初版构建 + #### step_motor - [ ] 增加步进电机模块 diff --git a/application/balance_chassis/balance.c b/application/balance_chassis/balance.c new file mode 100644 index 0000000..1d27e57 --- /dev/null +++ b/application/balance_chassis/balance.c @@ -0,0 +1,229 @@ +#include "balance.h" +#include "HT04.h" +#include "LK9025.h" +#include "bmi088.h" +#include "referee.h" +#include "super_cap.h" +#include "controller.h" +#include "can_comm.h" +#include "stdint.h" +#include "robot_def.h" +#include "general_def.h" +#include "arm_math.h" // 需要用到较多三角函数 + +/* 底盘拥有的模块实例 */ +static BMI088Instance *imu; +static SuperCapInstance *super_cap; +static referee_info_t *referee_data; // 裁判系统的数据会被不断更新 + +static HTMotorInstance *lf; +static HTMotorInstance *rf; +static HTMotorInstance *lb; +static HTMotorInstance *rb; +static LKMotorInstance *l_driven; +static LKMotorInstance *r_driven; + +static CANCommInstance *chassis_comm; // 底盘板和云台板通信 +static Chassis_Ctrl_Cmd_s chassis_cmd_recv; +static Chassis_Upload_Data_s chassis_feed_send; +// 由于采用多板架构,即使使用C板也有空余串口,可以使用串口通信以获得更高的通信速率 + +/* 方便函数间无开销传递参数的中间变量 */ +// 若将下面的封装函数取缔,则可以将这些变量放入BalanceTask函数体内. +// static ... +static float leg_len_l, leg_len_r; // 左右腿长(虚拟) +static float leg_angle_l, leg_angle_r; // 左右腿角度(虚拟) +// 倒立摆的虚拟力和虚拟力矩 +static float F_virtual_left, T_virtual_left, F_virtual_right, T_virtual_right; +// 左前,左后,右前,右后关节力矩 +static float T_joint_lf, T_joint_lr, T_joint_rf, T_joint_rb; +static float T_leg_left, T_leg_right; // 左右驱动电机力矩 + +/* ↓↓↓分割出这些函数是为了提高可读性,使得阅读者的逻辑更加顺畅;但实际上这些函数都不长,可以以注释的形式直接加到BalanceTask里↓↓↓*/ + +/** + * @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出 + * 由于反馈矩阵和控制矩阵都比较稀疏,故不使用矩阵库,避免非零项计算 + * + */ +static void CalcLQR() +{ +} + +/** + * @brief 将LQR的输出映射到关节和驱动电机的输出 + * + */ +static void VMCProject() +{ +} + +/** + * @brief 腿部角度控制:转向和抗劈叉 + * + */ +static PIDInstance swerving_pid; +static PIDInstance anti_crash_pid; + +static void SynthesizeMotion() +{ +} + +/** + * @brief 腿长控制:长度 + roll轴补偿(保持机体水平),用PD模拟弹簧的传递函数 + * + */ +static PIDInstance leg_length_pid; +static PIDInstance roll_compensate_pid; + +static void LegControl() +{ +} + +/** + * @brief 离地监测和?跳跃控制 + * + */ +static void FlyDetect() +{ +} + +/** + * @brief 功率限制 + * + */ +static void WattLimit() +{ +} + +void BalanceInit() +{ + BMI088_Init_Config_s imu_config = { + // IMU初始化 + .spi_acc_config = { + .GPIOx = GPIOC, + .cs_pin = GPIO_PIN_4, + .spi_handle = &hspi1, + }, + .spi_gyro_config = { + .GPIOx = GPIOC, + .cs_pin = GPIO_PIN_4, + .spi_handle = &hspi1, + }, + .acc_int_config = { + .exti_mode = EXTI_TRIGGER_FALLING, + .GPIO_Pin = GPIO_PIN_10, + .GPIOx = GPIOA, + }, + .gyro_int_config = { + .exti_mode = EXTI_TRIGGER_FALLING, + .GPIO_Pin = GPIO_PIN_11, + .GPIOx = GPIOA, + }, + .heat_pid_config = { + .Kp = 0.0f, + .Kd = 0.0f, + .Ki = 0.0f, + .MaxOut = 0.0f, + .DeadBand = 0.0f, + }, + .heat_pwm_config = { + .channel = TIM_CHANNEL_1, + .htim = &htim1, + }, + .cali_mode = BMI088_CALIBRATE_ONLINE_MODE, + .work_mode = BMI088_BLOCK_PERIODIC_MODE, + }; + imu = BMI088Register(&imu_config); + + SuperCap_Init_Config_s cap_conf = { + // 超级电容初始化 + .can_config.can_handle = &hcan1, + .can_config.rx_id = 0x311, + .can_config.tx_id = 0x312, + }; + super_cap = SuperCapInit(&cap_conf); + + // ↓↓↓---------------关节电机初始化----------------↓↓↓ + + Motor_Init_Config_s joint_conf = { + // 写一个,剩下的修改方向和id即可 + + }; + lf = HTMotorInit(&joint_conf); + + rf = HTMotorInit(&joint_conf); + + lb = HTMotorInit(&joint_conf); + + rb = HTMotorInit(&joint_conf); + + // ↓↓↓---------------驱动电机初始化----------------↓↓↓ + + Motor_Init_Config_s driven_conf = { + // 写一个,剩下的修改方向和id即可 + + }; + l_driven = LKMotorInit(&driven_conf); + + r_driven = LKMotorInit(&driven_conf); + + CANComm_Init_Config_s chassis_comm_conf = { + // 底盘板和云台板通信 + .can_config = { + .can_handle = &hcan1, + .rx_id = 0x201, + .tx_id = 0x200, + }, + .send_data_len = sizeof(Chassis_Upload_Data_s), + .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s), + }; + chassis_comm = CANCommInit(&chassis_comm_conf); + + referee_data = RefereeInit(&huart6); // 裁判系统串口 + + // ↓↓↓---------------综合运动控制----------------↓↓↓ + PID_Init_Config_s swerving_pid_conf = { + .Kp = 0.0f, + .Kd = 0.0f, + .Ki = 0.0f, + .MaxOut = 0.0f, + .DeadBand = 0.0f, + .Improve = PID_IMPROVE_NONE, + }; + PIDInit(&swerving_pid, &swerving_pid_conf); + + PID_Init_Config_s anti_crash_pid_conf = { + .Kp = 0.0f, + .Kd = 0.0f, + .Ki = 0.0f, + .MaxOut = 0.0f, + .DeadBand = 0.0f, + .Improve = PID_IMPROVE_NONE, + }; + PIDInit(&swerving_pid, &swerving_pid_conf); + + PID_Init_Config_s leg_length_pid_conf = { + .Kp = 0.0f, + .Kd = 0.0f, + .Ki = 0.0f, + .MaxOut = 0.0f, + .DeadBand = 0.0f, + .Improve = PID_IMPROVE_NONE, + }; + PIDInit(&leg_length_pid, &leg_length_pid_conf); + + PID_Init_Config_s roll_compensate_pid_conf = { + .Kp = 0.0f, + .Kd = 0.0f, + .Ki = 0.0f, + .MaxOut = 0.0f, + .DeadBand = 0.0f, + .Improve = PID_IMPROVE_NONE, + }; + PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf); +} + +void BalanceTask() +{ +} diff --git a/application/balance_chassis/balance.h b/application/balance_chassis/balance.h new file mode 100644 index 0000000..4e63883 --- /dev/null +++ b/application/balance_chassis/balance.h @@ -0,0 +1,17 @@ +#pragma once + +/** + * @brief 平衡底盘初始化 + * + */ +void BalanceInit(); + +/** + * @brief 平衡底盘任务 + * + */ +void BalanceTask(); + + + + diff --git a/application/balance_chassis/balance.md b/application/balance_chassis/balance.md new file mode 100644 index 0000000..6c5392d --- /dev/null +++ b/application/balance_chassis/balance.md @@ -0,0 +1,24 @@ +# balance + +可以继续解耦,将VMC独立成模块. + +目前默认使用平衡底盘时为双板. + +## 工作流程 + +1. 获取控制信息 +2. 根据控制模式将控制指令转化为实际的参考输入 +3. 使用lqr得出的反馈增益,计算二阶倒立摆模型的控制输出;需要根据当前腿长查gain table,或预先拟合K=f(Leg)的函数 +4. 计算二阶倒立摆$[L0 phi0]$和轮腿[phi1 phi4]间的雅可比,根据VMC将lqr的输出[F Tp]映射成[T1 T2] ; 驱动轮不需要映射 +5. 进行综合运动补偿,即转向控制和抗劈叉 +6. 进行腿长控制计算,即长度控制和roll轴水平控制 +7. 进行离地检测判断是否要让腿保持垂直,后续再加入跳跃功能 +8. 根据裁判系统和超级电容的功率信息进行输出限幅 +9. 设置反馈信息,包括裁判系统的数据,并通过电机反馈和IMU数据计算底盘实际运动状态等 +10. 推送反馈信息 + +电机初始化为电流环即可,注意基于模型的控制需要正确设定单位 + +如果功率可能超限,需要判定降低功率输出后受影响最小的执行单元,并给予其较大的功率输出衰减(一般不会超功率) + +另外, 选择平衡底盘有枪口冷却增益, 注意将这一部分改变反馈给cmd, 以使得shoot有更好的表现 diff --git a/application/balance_chassis/gain_table.h b/application/balance_chassis/gain_table.h new file mode 100644 index 0000000..1449e93 --- /dev/null +++ b/application/balance_chassis/gain_table.h @@ -0,0 +1,24 @@ +/* 平衡底盘lqr反馈增益和腿长的关系表,可以选择查找精度和插值 */ + +#pragma once +#include "stdint.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 interpolation_flag = 0; // 插值方式:1 线性插值 0 关闭插值 + +void EnalbeInterpolation(void) +{ + interpolation_flag = 1; +} + +/* 默认关闭插值,向下取整 */ +float LookUpKgain(float leg_length) +{ + +} \ No newline at end of file diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 55625a1..62d2810 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -82,7 +82,7 @@ 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; motor_lf = DJIMotorInit(&chassis_motor_config); diff --git a/application/grab/grab.md b/application/grab/grab.md index b09acba..47ab839 100644 --- a/application/grab/grab.md +++ b/application/grab/grab.md @@ -1 +1 @@ -工程机器人夹爪 \ No newline at end of file +工程机器人夹爪 \ No newline at end of file diff --git a/application/lift/lift.md b/application/lift/lift.md index 60a2ab0..98507b2 100644 --- a/application/lift/lift.md +++ b/application/lift/lift.md @@ -1 +1,3 @@ -抬升/横移机构 \ No newline at end of file +抬升/横移机构 + +气缸和电磁阀似乎没有必要构建模块,但是也可以组合微动开关和编码器+继电器等传感器构成模块,降低lift的复杂度,完成解耦. \ No newline at end of file diff --git a/application/robot_def.h b/application/robot_def.h index 56bbe00..2758616 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -16,11 +16,11 @@ #include "master_process.h" #include "stdint.h" - /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ #define ONE_BOARD // 单板控制整车 // #define CHASSIS_BOARD //底盘板 // #define GIMBAL_BOARD //云台板 +// #define BALANCE_BOARD //启用平衡底盘,则默认双板且当前板位底盘,目前不支持!请勿使用! // @todo: 增加机器人类型定义,后续是否要兼容所有机器人?(只兼容步兵英雄哨兵似乎就够了) // 通过该宏,你可以直接将所有机器人的参数保存在一处,然后每次只需要修改这个宏就可以替换所有参数 @@ -179,6 +179,7 @@ typedef struct * */ +/* @todo : 对于平衡底盘,需要新增控制模式和控制数据 */ typedef struct { #ifdef CHASSIS_BOARD @@ -198,6 +199,7 @@ typedef struct } Chassis_Upload_Data_s; +/* @todo : 对于平衡底盘,需要不同的反馈数据 */ typedef struct { attitude_t gimbal_imu_data; diff --git a/bsp/can/bsp_can.c b/bsp/can/bsp_can.c index 50f28ed..30200af 100644 --- a/bsp/can/bsp_can.c +++ b/bsp/can/bsp_can.c @@ -92,7 +92,7 @@ CANInstance *CANRegister(CAN_Init_Config_s *config) uint8_t CANTransmit(CANInstance *_instance,uint8_t timeout) { float dwt_start = DWT_GetTimeline_ms(); - while (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0);// 等待邮箱空闲 + while (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0) // 等待邮箱空闲 { if (DWT_GetTimeline_ms() - dwt_start > timeout) // 超时 { diff --git a/bsp/gpio/bsp_gpio.c b/bsp/gpio/bsp_gpio.c index 62ac154..a871709 100644 --- a/bsp/gpio/bsp_gpio.c +++ b/bsp/gpio/bsp_gpio.c @@ -3,11 +3,11 @@ #include "stdlib.h" static uint8_t idx; -static GPIOInstance* gpio_instance[GPIO_MX_DEVICE_NUM] = {NULL}; +static GPIOInstance *gpio_instance[GPIO_MX_DEVICE_NUM] = {NULL}; /** * @brief EXTI中断回调函数,根据GPIO_Pin找到对应的GPIOInstance,并调用模块回调函数(如果有) - * @note 如何判断具体是哪一个GPIO的引脚连接到这个EXTI中断线上? + * @note 如何判断具体是哪一个GPIO的引脚连接到这个EXTI中断线上? * 一个EXTI中断线只能连接一个GPIO引脚,因此可以通过GPIO_Pin来判断,PinX对应EXTIX * 一个Pin号只会对应一个EXTI,详情见gpio.md * @param GPIO_Pin 发生中断的GPIO_Pin @@ -18,8 +18,8 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) static GPIOInstance *gpio; for (size_t i = 0; i < idx; i++) { - gpio=gpio_instance[i]; - if(gpio->GPIO_Pin==GPIO_Pin && gpio->gpio_model_callback!=NULL) + gpio = gpio_instance[i]; + if (gpio->GPIO_Pin == GPIO_Pin && gpio->gpio_model_callback != NULL) { gpio->gpio_model_callback(gpio); return; @@ -29,16 +29,16 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) GPIOInstance *GPIORegister(GPIO_Init_Config_s *GPIO_config) { - GPIOInstance *ins=(GPIOInstance*)malloc(sizeof(GPIOInstance)); - memset(ins,0,sizeof(GPIOInstance)); - - ins->GPIOx=GPIO_config->GPIOx; - ins->GPIO_Pin=GPIO_config->GPIO_Pin; - ins->pin_state=GPIO_config->pin_state; - ins->exti_mode=GPIO_config->exti_mode; - ins->id=GPIO_config->id; - ins->gpio_model_callback=GPIO_config->gpio_model_callback; - gpio_instance[idx++]=ins; + GPIOInstance *ins = (GPIOInstance *)malloc(sizeof(GPIOInstance)); + memset(ins, 0, sizeof(GPIOInstance)); + + ins->GPIOx = GPIO_config->GPIOx; + ins->GPIO_Pin = GPIO_config->GPIO_Pin; + ins->pin_state = GPIO_config->pin_state; + ins->exti_mode = GPIO_config->exti_mode; + ins->id = GPIO_config->id; + ins->gpio_model_callback = GPIO_config->gpio_model_callback; + gpio_instance[idx++] = ins; return ins; } @@ -47,21 +47,20 @@ GPIOInstance *GPIORegister(GPIO_Init_Config_s *GPIO_config) void GPIOToggel(GPIOInstance *_instance) { - HAL_GPIO_TogglePin(_instance->GPIOx,_instance->GPIO_Pin); + HAL_GPIO_TogglePin(_instance->GPIOx, _instance->GPIO_Pin); } void GPIOSet(GPIOInstance *_instance) { - HAL_GPIO_WritePin(_instance->GPIOx,_instance->GPIO_Pin,GPIO_PIN_SET); + HAL_GPIO_WritePin(_instance->GPIOx, _instance->GPIO_Pin, GPIO_PIN_SET); } void GPIOReset(GPIOInstance *_instance) { - HAL_GPIO_WritePin(_instance->GPIOx,_instance->GPIO_Pin,GPIO_PIN_RESET); + HAL_GPIO_WritePin(_instance->GPIOx, _instance->GPIO_Pin, GPIO_PIN_RESET); } GPIO_PinState GPIORead(GPIOInstance *_instance) { - return HAL_GPIO_ReadPin(_instance->GPIOx,_instance->GPIO_Pin); + return HAL_GPIO_ReadPin(_instance->GPIOx, _instance->GPIO_Pin); } - diff --git a/bsp/spi/bsp_spi.c b/bsp/spi/bsp_spi.c index 97c2ec4..aad7d0d 100644 --- a/bsp/spi/bsp_spi.c +++ b/bsp/spi/bsp_spi.c @@ -15,7 +15,7 @@ SPIInstance *SPIRegister(SPI_Init_Config_s *conf) memset(instance, 0, sizeof(SPIInstance)); instance->spi_handle = conf->spi_handle; - instance->GPIO_cs = conf->GPIO_cs; + instance->GPIOx = conf->GPIOx; instance->cs_pin = conf->cs_pin; instance->spi_work_mode = conf->spi_work_mode; instance->callback = conf->callback; @@ -28,7 +28,7 @@ SPIInstance *SPIRegister(SPI_Init_Config_s *conf) void SPITransmit(SPIInstance *spi_ins, uint8_t *ptr_data, uint8_t len) { // 拉低片选,开始传输(选中从机) - HAL_GPIO_WritePin(spi_ins->GPIO_cs, spi_ins->cs_pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(spi_ins->GPIOx, spi_ins->cs_pin, GPIO_PIN_RESET); switch (spi_ins->spi_work_mode) { case SPI_DMA_MODE: @@ -40,7 +40,7 @@ void SPITransmit(SPIInstance *spi_ins, uint8_t *ptr_data, uint8_t len) case SPI_BLOCK_MODE: HAL_SPI_Transmit(spi_ins->spi_handle, ptr_data, len, 50); // 默认50ms超时 // 阻塞模式不会调用回调函数,传输完成后直接拉高片选结束 - HAL_GPIO_WritePin(spi_ins->GPIO_cs, spi_ins->cs_pin, GPIO_PIN_SET); + HAL_GPIO_WritePin(spi_ins->GPIOx, spi_ins->cs_pin, GPIO_PIN_SET); break; default: while (1) @@ -55,7 +55,7 @@ void SPIRecv(SPIInstance *spi_ins, uint8_t *ptr_data, uint8_t len) spi_ins->rx_size = len; spi_ins->rx_buffer = ptr_data; // 拉低片选,开始传输 - HAL_GPIO_WritePin(spi_ins->GPIO_cs, spi_ins->cs_pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(spi_ins->GPIOx, spi_ins->cs_pin, GPIO_PIN_RESET); switch (spi_ins->spi_work_mode) { case SPI_DMA_MODE: @@ -67,7 +67,7 @@ void SPIRecv(SPIInstance *spi_ins, uint8_t *ptr_data, uint8_t len) case SPI_BLOCK_MODE: HAL_SPI_Receive(spi_ins->spi_handle, ptr_data, len, 10); // 阻塞模式不会调用回调函数,传输完成后直接拉高片选结束 - HAL_GPIO_WritePin(spi_ins->GPIO_cs, spi_ins->cs_pin, GPIO_PIN_SET); + HAL_GPIO_WritePin(spi_ins->GPIOx, spi_ins->cs_pin, GPIO_PIN_SET); break; default: while (1) @@ -82,7 +82,7 @@ void SPITransRecv(SPIInstance *spi_ins, uint8_t *ptr_data_rx, uint8_t *ptr_data_ spi_ins->rx_size = len; spi_ins->rx_buffer = ptr_data_rx; // 拉低片选,开始传输 - HAL_GPIO_WritePin(spi_ins->GPIO_cs, spi_ins->cs_pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(spi_ins->GPIOx, spi_ins->cs_pin, GPIO_PIN_RESET); switch (spi_ins->spi_work_mode) { case SPI_DMA_MODE: @@ -94,7 +94,7 @@ void SPITransRecv(SPIInstance *spi_ins, uint8_t *ptr_data_rx, uint8_t *ptr_data_ case SPI_BLOCK_MODE: HAL_SPI_TransmitReceive(spi_ins->spi_handle, ptr_data_tx, ptr_data_rx, len, 50); // 默认50ms超时 // 阻塞模式不会调用回调函数,传输完成后直接拉高片选结束 - HAL_GPIO_WritePin(spi_ins->GPIO_cs, spi_ins->cs_pin, GPIO_PIN_SET); + HAL_GPIO_WritePin(spi_ins->GPIOx, spi_ins->cs_pin, GPIO_PIN_SET); break; default: while (1) @@ -126,13 +126,13 @@ void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) { // 如果是当前spi硬件发出的complete,且cs_pin为低电平(说明正在传输),则尝试调用回调函数 if (spi_instance[i]->spi_handle == hspi && // 显然同一时间一条总线只能有一个从机在接收数据 - HAL_GPIO_ReadPin(spi_instance[i]->GPIO_cs, spi_instance[i]->cs_pin) == GPIO_PIN_RESET) + HAL_GPIO_ReadPin(spi_instance[i]->GPIOx, spi_instance[i]->cs_pin) == GPIO_PIN_RESET) { // 先拉高片选,结束传输,在判断是否有回调函数,如果有则调用回调函数 - HAL_GPIO_WritePin(spi_instance[i]->GPIO_cs, spi_instance[i]->cs_pin, GPIO_PIN_SET); + HAL_GPIO_WritePin(spi_instance[i]->GPIOx, spi_instance[i]->cs_pin, GPIO_PIN_SET); if (spi_instance[i]->callback != NULL) // 回调函数不为空, 则调用回调函数 { - + spi_instance[i]->callback(spi_instance[i]); } return; diff --git a/bsp/spi/bsp_spi.h b/bsp/spi/bsp_spi.h index 14f0cf3..e065b76 100644 --- a/bsp/spi/bsp_spi.h +++ b/bsp/spi/bsp_spi.h @@ -18,7 +18,7 @@ typedef enum typedef struct spi_ins_temp { SPI_HandleTypeDef *spi_handle; // SPI外设handle - GPIO_TypeDef *GPIO_cs; // 片选信号对应的GPIO,如GPIOA,GPIOB等等 + GPIO_TypeDef *GPIOx; // 片选信号对应的GPIO,如GPIOA,GPIOB等等 uint16_t cs_pin; // 片选信号对应的引脚号,GPIO_PIN_1,GPIO_PIN_2等等 SPI_TXRX_MODE_e spi_work_mode; // 传输工作模式 @@ -32,12 +32,12 @@ typedef struct spi_ins_temp /* 接收回调函数定义,包含SPI的module按照此格式构建回调函数 */ typedef void (*spi_rx_callback)(SPIInstance *); -// @todo: 这里可以将GPIO_TypeDef *GPIO_cs; uint16_t cs_pin合并为bsp_gpio以简化代码实现 +// @todo: 这里可以将GPIO_TypeDef *GPIOx; uint16_t cs_pin合并为bsp_gpio以简化代码实现 /* SPI初始化配置,其实基本和SPIIstance一模一样,为了代码风格统一因此再次定义 */ typedef struct { SPI_HandleTypeDef *spi_handle; // SPI外设handle - GPIO_TypeDef *GPIO_cs; // 片选信号对应的GPIO,如GPIOA,GPIOB等等 + GPIO_TypeDef *GPIOx; // 片选信号对应的GPIO,如GPIOA,GPIOB等等 uint16_t cs_pin; // 片选信号对应的引脚号,GPIO_PIN_1,GPIO_PIN_2等等 SPI_TXRX_MODE_e spi_work_mode; // 传输工作模式 @@ -76,7 +76,7 @@ void SPIRecv(SPIInstance *spi_ins, uint8_t *ptr_data, uint8_t len); /** * @brief 通过spi利用移位寄存器同时收发数据 * @todo 后续加入阻塞模式下的timeout参数 - * + * * @param spi_ins spi实例指针 * @param ptr_data_rx 接收数据地址 * @param ptr_data_tx 发送数据地址 diff --git a/modules/BMI088/bmi088.h b/modules/BMI088/bmi088.h index 8d6ef53..1b10f5f 100644 --- a/modules/BMI088/bmi088.h +++ b/modules/BMI088/bmi088.h @@ -109,7 +109,7 @@ BMI088_Data_t BMI088Acquire(BMI088Instance *bmi088); /** * @brief 标定传感器.BMI088在初始化的时候会调用此函数. 提供接口方便标定离线数据 * @attention @todo 注意,当操作系统开始运行后,此函数会和INS_Task冲突.目前不允许在运行时调用此函数,后续加入标志位判断以提供运行时重新的标定功能 - * + * * @param _bmi088 待标定的实例 */ void BMI088CalibrateIMU(BMI088Instance *_bmi088); diff --git a/modules/BMI088/bmi088.md b/modules/BMI088/bmi088.md index d2925f8..d226a1b 100644 --- a/modules/BMI088/bmi088.md +++ b/modules/BMI088/bmi088.md @@ -2,19 +2,18 @@ **注意,此模块待测试** - ## 示例 ```c BMI088_Init_Config_s imu_config = { .spi_acc_config={ - .GPIO_cs=GPIOC, - .GPIO_cs=GPIO_PIN_4, + .GPIOx=GPIOC, + .GPIOx=GPIO_PIN_4, .spi_handle=&hspi1, }, .spi_gyro_config={ - .GPIO_cs=GPIOC, - .GPIO_cs=GPIO_PIN_4, + .GPIOx=GPIOC, + .GPIOx=GPIO_PIN_4, .spi_handle=&hspi1, }, .acc_int_config={ @@ -63,9 +62,11 @@ BMI088Instance* imu=BMI088Register(&imu_config); `__HAL_GPIO_EXTI_GENERATE_SWIT()` `HAL_EXTI_GENERATE_SWI()` 可以触发软件中断 of course,两者的数据更新实际上可以异步进行,这里为了方便起见当两者数据都准备好以后再行融合 + ## 数据读写规则(so called 16-bit protocol) 加速度计读取read: + 1. bit 0 :1 bit 1-7: reg address 2. dummy read,加速度计此时返回的数据无效 3. 真正的数据从第三个字节开始. @@ -76,6 +77,7 @@ byte2: 没用 byte3: 读取到的数据 write写入: + 1. bit 0: 0 bit1-7: reg address 2. 要写入寄存器的数据(注意没有dummy byte) @@ -84,6 +86,7 @@ write写入: **注意,陀螺仪和加速度计的读取不同** 陀螺仪gyro读取read: + 1. bit 0 :1 bit1-7: reg address 2. 读回的数据 @@ -92,7 +95,6 @@ byte1: 1(读)+7位寄存器地址 byte2: 读取到的数据 write写入: + 1. bit0 : 0 bit1-7 : reg address 2. 写入的数据 - - diff --git a/modules/algorithm/LQR.c b/modules/algorithm/LQR.c deleted file mode 100644 index a59e79e..0000000 --- a/modules/algorithm/LQR.c +++ /dev/null @@ -1 +0,0 @@ -#include "LQR.h" diff --git a/modules/algorithm/LQR.h b/modules/algorithm/LQR.h deleted file mode 100644 index 0a1675b..0000000 --- a/modules/algorithm/LQR.h +++ /dev/null @@ -1,12 +0,0 @@ -/** - * @file LQR.h - * @author your name (you@domain.com) - * @brief 利用arm math库实现矩阵运算功能 - * @version 0.1 - * @date 2023-02-14 - * - * @copyright Copyright (c) 2023 - * - */ - -#pragma once \ No newline at end of file diff --git a/modules/algorithm/controller.c b/modules/algorithm/controller.c index b0fafba..bd2350a 100644 --- a/modules/algorithm/controller.c +++ b/modules/algorithm/controller.c @@ -9,7 +9,7 @@ * @copyrightCopyright (c) 2022 HNU YueLu EC all rights reserved */ #include "controller.h" -#include +#include "memory.h" /* ----------------------------下面是pid优化环节的实现---------------------------- */ @@ -40,7 +40,7 @@ static void f_Integral_Limit(PIDInstance *pid) static float temp_Output, temp_Iout; temp_Iout = pid->Iout + pid->ITerm; temp_Output = pid->Pout + pid->Iout + pid->Dout; - if (abs(temp_Output) > pid->MaxOut) + if (abs(temp_Output) > pid->MaxOut) { if (pid->Err * pid->Iout > 0) // 积分却还在累积 { @@ -117,7 +117,6 @@ static void f_PID_ErrorHandle(PIDInstance *pid) } } - /* ---------------------------下面是PID的外部算法接口--------------------------- */ /** @@ -126,8 +125,8 @@ static void f_PID_ErrorHandle(PIDInstance *pid) * @param pid PID实例 * @param config PID初始化设置 */ -void PID_Init(PIDInstance *pid, PID_Init_Config_s *config) -{ +void PIDInit(PIDInstance *pid, PID_Init_Config_s *config) +{ // config的数据和pid的部分数据是连续且相同的的,所以可以直接用memcpy // @todo: 不建议这样做,可扩展性差,不知道的开发者可能会误以为pid和config是同一个结构体 // 后续修改为逐个赋值 @@ -135,7 +134,6 @@ void PID_Init(PIDInstance *pid, PID_Init_Config_s *config) // utilize the quality of struct that its memeory is continuous memcpy(pid, config, sizeof(PID_Init_Config_s)); // set rest of memory to 0 - } /** @@ -145,13 +143,13 @@ void PID_Init(PIDInstance *pid, PID_Init_Config_s *config) * @param[in] 期望值 * @retval 返回空 */ -float PID_Calculate(PIDInstance *pid, float measure, float ref) +float PIDCalculate(PIDInstance *pid, float measure, float ref) { // 堵转检测 - if (pid->Improve & ErrorHandle) + if (pid->Improve & PID_ErrorHandle) f_PID_ErrorHandle(pid); - pid->dt = DWT_GetDeltaT((void *)&pid->DWT_CNT); //获取两次pid计算的时间间隔,用于积分和微分 + pid->dt = DWT_GetDeltaT((void *)&pid->DWT_CNT); // 获取两次pid计算的时间间隔,用于积分和微分 // 保存上次的测量值和误差,计算当前error pid->Measure = measure; @@ -160,33 +158,33 @@ float PID_Calculate(PIDInstance *pid, float measure, float ref) // 如果在死区外,则计算PID if (abs(pid->Err) > pid->DeadBand) - { + { // 基本的pid计算,使用位置式 pid->Pout = pid->Kp * pid->Err; pid->ITerm = pid->Ki * pid->Err * pid->dt; pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt; // 梯形积分 - if (pid->Improve & Trapezoid_Intergral) + if (pid->Improve & PID_Trapezoid_Intergral) f_Trapezoid_Intergral(pid); // 变速积分 - if (pid->Improve & ChangingIntegrationRate) + if (pid->Improve & PID_ChangingIntegrationRate) f_Changing_Integration_Rate(pid); // 微分先行 - if (pid->Improve & Derivative_On_Measurement) + if (pid->Improve & PID_Derivative_On_Measurement) f_Derivative_On_Measurement(pid); // 微分滤波器 - if (pid->Improve & DerivativeFilter) + if (pid->Improve & PID_DerivativeFilter) f_Derivative_Filter(pid); // 积分限幅 - if (pid->Improve & Integral_Limit) + if (pid->Improve & PID_Integral_Limit) f_Integral_Limit(pid); - pid->Iout += pid->ITerm; // 累加积分 + pid->Iout += pid->ITerm; // 累加积分 pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出 // 输出滤波 - if (pid->Improve & OutputFilter) + if (pid->Improve & PID_OutputFilter) f_Output_Filter(pid); // 输出限幅 @@ -194,10 +192,10 @@ float PID_Calculate(PIDInstance *pid, float measure, float ref) } else // 进入死区, 则清空积分和输出 { - pid->Output=0; - pid->ITerm=0; + pid->Output = 0; + pid->ITerm = 0; } - + // 保存当前数据,用于下次计算 pid->Last_Measure = pid->Measure; pid->Last_Output = pid->Output; diff --git a/modules/algorithm/controller.h b/modules/algorithm/controller.h index 2122239..bb8137e 100644 --- a/modules/algorithm/controller.h +++ b/modules/algorithm/controller.h @@ -15,7 +15,7 @@ #include "main.h" #include "stdint.h" -#include "string.h" +#include "memory.h" #include "stdlib.h" #include "bsp_dwt.h" #include "arm_math.h" @@ -25,18 +25,18 @@ #define abs(x) ((x > 0) ? x : -x) #endif -// PID 优化环节使能标志位 +// PID 优化环节使能标志位,通过位与可以判断启用的优化环节;也可以改成位域的形式 typedef enum { - PID_IMPROVE_NONE = 0b00000000, // 0000 0000 - Integral_Limit = 0b00000001, // 0000 0001 - Derivative_On_Measurement = 0b00000010, // 0000 0010 - Trapezoid_Intergral = 0b00000100, // 0000 0100 - Proportional_On_Measurement = 0b00001000, // 0000 1000 - OutputFilter = 0b00010000, // 0001 0000 - ChangingIntegrationRate = 0b00100000, // 0010 0000 - DerivativeFilter = 0b01000000, // 0100 0000 - ErrorHandle = 0b10000000, // 1000 0000 + PID_IMPROVE_NONE = 0b00000000, // 0000 0000 + PID_Integral_Limit = 0b00000001, // 0000 0001 + PID_Derivative_On_Measurement = 0b00000010, // 0000 0010 + PID_Trapezoid_Intergral = 0b00000100, // 0000 0100 + PID_Proportional_On_Measurement = 0b00001000, // 0000 1000 + PID_OutputFilter = 0b00010000, // 0001 0000 + PID_ChangingIntegrationRate = 0b00100000, // 0010 0000 + PID_DerivativeFilter = 0b01000000, // 0100 0000 + PID_ErrorHandle = 0b10000000, // 1000 0000 } PID_Improvement_e; /* PID 报错类型枚举*/ @@ -60,16 +60,16 @@ typedef struct float Kp; float Ki; float Kd; - float MaxOut; - float IntegralLimit; float DeadBand; + + PID_Improvement_e Improve; + float IntegralLimit; float CoefA; // For Changing Integral float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|led_pwm=GPIORegister(&led_config->pwm_config); - led_ins->led_switch=led_config->init_swtich; - + led_ins->led_pwm = GPIORegister(&led_config->pwm_config); + led_ins->led_switch = led_config->init_swtich; + bsp_led_ins[idx++] = led_ins; return led_ins; } -void LEDSet(LEDInstance *_led,uint8_t alpha,uint8_t color_value,uint8_t brightness) +void LEDSet(LEDInstance *_led, uint8_t alpha, uint8_t color_value, uint8_t brightness) { - } - -void LEDSwitch(LEDInstance *_led,uint8_t led_switch) +void LEDSwitch(LEDInstance *_led, uint8_t led_switch) { - if(led_switch==1) + if (led_switch == 1) { - _led->led_switch=1; + _led->led_switch = 1; } else { - _led->led_switch=0; + _led->led_switch = 0; // PWMSetPeriod(_led,0); } } - void LEDShow() { - } - diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index 3ea3982..36b6641 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -157,9 +157,9 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等 // motor controller init 电机控制器初始化 - PID_Init(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID); - PID_Init(&instance->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID); - PID_Init(&instance->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID); + PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID); + PIDInit(&instance->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID); + PIDInit(&instance->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID); instance->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; instance->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; // 后续增加电机前馈控制器(速度和电流) @@ -248,7 +248,7 @@ void DJIMotorControl() else pid_measure = motor_measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃 // 更新pid_ref进入下一个环 - pid_ref = PID_Calculate(&motor_controller->angle_PID, pid_measure, pid_ref); + pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref); } // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环 @@ -259,13 +259,13 @@ void DJIMotorControl() else // MOTOR_FEED pid_measure = motor_measure->speed_aps; // 更新pid_ref进入下一个环 - pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, pid_ref); + pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref); } // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈 if (motor_setting->close_loop_type & CURRENT_LOOP) { - pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->real_current, pid_ref); + pid_ref = PIDCalculate(&motor_controller->current_PID, motor_measure->real_current, pid_ref); } // 获取最终输出 diff --git a/modules/motor/DJImotor/dji_motor.md b/modules/motor/DJImotor/dji_motor.md index 49dff38..bed1d82 100644 --- a/modules/motor/DJImotor/dji_motor.md +++ b/modules/motor/DJImotor/dji_motor.md @@ -57,8 +57,8 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详 CURRENT_LOOP SPEED_LOOP ANGLE_LOOP - CURRENT_LOOP | SPEED_LOOP // 同时对电流和速度闭环 - SPEED_LOOP | ANGLE_LOOP // 同时对速度和位置闭环 + CURRENT_LOOP | SPEED_LOOP // 同时对电流和速度闭环 + SPEED_LOOP | ANGLE_LOOP // 同时对速度和位置闭环 CURRENT_LOOP | SPEED_LOOP |ANGLE_LOOP // 三环全开 ``` @@ -87,7 +87,7 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详 float Kp; float Ki; float Kd; - + float MaxOut; // 输出限幅 // 以下是优化参数 float IntegralLimit; // 积分限幅 @@ -98,22 +98,22 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详 float Derivative_LPF_RC; PID_Improvement_e Improve; // 优化环节，定义在下一个代码块 - } PID_Init_config_s; + } PIDInit_config_s; // 只有当你设启用了对应的优化环节，优化参数才会生效 ``` ```c typedef enum { - NONE = 0b00000000, - Integral_Limit = 0b00000001, + NONE = 0b00000000, + Integral_Limit = 0b00000001, Derivative_On_Measurement = 0b00000010, - Trapezoid_Intergral = 0b00000100, + Trapezoid_Intergral = 0b00000100, Proportional_On_Measurement = 0b00001000, - OutputFilter = 0b00010000, - ChangingIntegrationRate = 0b00100000, - DerivativeFilter = 0b01000000, - ErrorHandle = 0b10000000, + OutputFilter = 0b00010000, + ChangingIntegrationRate = 0b00100000, + DerivativeFilter = 0b01000000, + ErrorHandle = 0b10000000, } PID_Improvement_e; // 若希望使用多个环节的优化，这样就行：Integral_Limit |Trapezoid_Intergral|...|... ``` @@ -123,35 +123,31 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详 float *other_speed_feedback_ptr ``` - - --- - - 推荐的初始化参数编写格式如下： ```c Motor_Init_Config_s config = { - .motor_type = M3508, // 要注册的电机为3508电机 - .can_init_config = {.can_handle = &hcan1, // 挂载在CAN1 - .tx_id = 1}, // C620每隔一段时间闪动1次,设置为1 - // 采用电机编码器角度与速度反馈,启用速度环和电流环,不反转,最外层闭环为速度环 + .motor_type = M3508, // 要注册的电机为3508电机 + .can_init_config = {.can_handle = &hcan1, // 挂载在CAN1 + .tx_id = 1}, // C620每隔一段时间闪动1次,设置为1 + // 采用电机编码器角度与速度反馈,启用速度环和电流环,不反转,最外层闭环为速度环 .controller_setting_init_config = {.angle_feedback_source = MOTOR_FEED, - + .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, - .speed_feedback_source = MOTOR_FEED, - .reverse_flag = MOTOR_DIRECTION_NORMAL}, - // 电流环和速度环PID参数的设置,不采用计算优化则不需要传入Improve参数 + .speed_feedback_source = MOTOR_FEED, + .reverse_flag = MOTOR_DIRECTION_NORMAL}, + // 电流环和速度环PID参数的设置,不采用计算优化则不需要传入Improve参数 // 不使用其他数据来源(如IMU),不需要传入反馈数据变量指针 - .controller_param_init_config = {.current_PID = {.Improve = 0, + .controller_param_init_config = {.current_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}, - .speed_PID = {.Improve = 0, + .speed_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, @@ -161,12 +157,8 @@ Motor_Init_Config_s config = { dji_motor_instance *djimotor = DJIMotorInit(config); // 设置好参数后进行初始化并保留返回的指针 ``` - - --- - - 要控制一个DJI电机，我们提供了2个接口： ```c @@ -189,16 +181,10 @@ float speed=LeftForwardMotor->motor_measure->speed_rpm; ... ``` - - ***现在，忘记PID的计算和发送、接收以及协议解析，专注于模块之间的逻辑交互吧。*** - - --- - - ## 代码结构 .h文件内包括了外部接口和类型定义,以及模块对应的宏。c文件内为私有函数和外部接口的定义。 @@ -236,8 +222,8 @@ typedef struct /* sender assigment*/ uint8_t sender_group; uint8_t message_num; - - uint8_t stop_flag; + + uint8_t stop_flag; Motor_Type_e motor_type; } dji_motor_instance; @@ -372,8 +358,6 @@ void DJIMotorOuterLoop(dji_motor_instance *motor); - `DJIMotorOuterLoop()`用于修改电机的外部闭环类型，即电机的真实闭环目标。 - - ## 私有函数和变量 在.c文件内设为static的函数和变量 @@ -401,7 +385,7 @@ static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL}; static can_instance sender_assignment[6] = { [0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, - ... + ... ... }; @@ -439,19 +423,19 @@ static void DecodeDJIMotor(can_instance *_instance) ```c //初始化设置 Motor_Init_Config_s config = { - .motor_type = GM6020, - .can_init_config = { - .can_handle = &hcan1, - .tx_id = 6 + .motor_type = GM6020, + .can_init_config = { + .can_handle = &hcan1, + .tx_id = 6 }, - .controller_setting_init_config = { + .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .speed_feedback_source = MOTOR_FEED, .reverse_flag = MOTOR_DIRECTION_NORMAL }, - .controller_param_init_config = { + .controller_param_init_config = { .angle_PID = { .Improve = 0, .Kp = 1, @@ -479,4 +463,4 @@ dji_motor_instance *djimotor = DJIMotorInit(&config); DJIMotorSetRef(djimotor, 10); ``` -前提是已经将`DJIMotorControl()`放入实时系统任务当中或以一定d。你也可以单独执行`DJIMotorControl()`。 \ No newline at end of file +前提是已经将`DJIMotorControl()`放入实时系统任务当中或以一定d。你也可以单独执行`DJIMotorControl()`。 diff --git a/modules/motor/HTmotor/HT04.c b/modules/motor/HTmotor/HT04.c index c07934a..13ec46a 100644 --- a/modules/motor/HTmotor/HT04.c +++ b/modules/motor/HTmotor/HT04.c @@ -12,7 +12,7 @@ HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT]; * @param motor */ static void HTMotorSetMode(HTMotor_Mode_t cmd, HTMotorInstance *motor) -{ +{ memset(motor->motor_can_instace->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id CANTransmit(motor->motor_can_instace, 1); @@ -67,9 +67,9 @@ HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config) memset(motor, 0, sizeof(HTMotorInstance)); motor->motor_settings = config->controller_setting_init_config; - PID_Init(&motor->current_PID, &config->controller_param_init_config.current_PID); - PID_Init(&motor->speed_PID, &config->controller_param_init_config.speed_PID); - PID_Init(&motor->angle_PID, &config->controller_param_init_config.angle_PID); + PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID); + PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID); + PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID); motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; @@ -112,7 +112,7 @@ void HTMotorControl() else pid_measure = measure->real_current; // measure单位是rad,ref是角度,统一到angle下计算,方便建模 - pid_ref = PID_Calculate(&motor->angle_PID, pid_measure*RAD_2_ANGLE, pid_ref); + pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_ANGLE, pid_ref); } if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)) @@ -125,7 +125,7 @@ void HTMotorControl() else pid_measure = measure->speed_aps; // measure单位是rad / s ,ref是angle per sec,统一到angle下计算 - pid_ref = PID_Calculate(&motor->speed_PID, pid_measure*RAD_2_ANGLE, pid_ref); + pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_ANGLE, pid_ref); } if (setting->close_loop_type & CURRENT_LOOP) @@ -133,15 +133,15 @@ void HTMotorControl() if (setting->feedforward_flag & CURRENT_FEEDFORWARD) pid_ref += *motor->current_feedforward_ptr; - pid_ref = PID_Calculate(&motor->current_PID, measure->real_current, pid_ref); + pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref); } set = pid_ref; if (setting->reverse_flag == MOTOR_DIRECTION_REVERSE) set *= -1; - LIMIT_MIN_MAX(set, T_MIN, T_MAX); // 限幅,实际上这似乎和pid输出限幅重复了 - tmp = float_to_uint(set, T_MIN, T_MAX, 12); // 数值最后在 -12~+12之间 + LIMIT_MIN_MAX(set, T_MIN, T_MAX); // 限幅,实际上这似乎和pid输出限幅重复了 + tmp = float_to_uint(set, T_MIN, T_MAX, 12); // 数值最后在 -12~+12之间 motor_can->tx_buff[6] = (tmp >> 8); motor_can->tx_buff[7] = tmp & 0xff; diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c index c417436..01af445 100644 --- a/modules/motor/LKmotor/LK9025.c +++ b/modules/motor/LKmotor/LK9025.c @@ -6,6 +6,11 @@ static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL}; static CANInstance *sender_instance; // 多电机发送时使用的caninstance(当前保存的是注册的第一个电机的caninstance) // 后续考虑兼容单电机和多电机指令. +/** + * @brief 电机反馈报文解析 + * + * @param _instance 发生中断的caninstance + */ static void LKMotorDecode(CANInstance *_instance) { static LKMotor_Measure_t *measure; @@ -34,30 +39,31 @@ static void LKMotorDecode(CANInstance *_instance) measure->total_angle = measure->total_round * 360 + measure->angle_single_round; } -LKMotorInstance *LKMotroInit(Motor_Init_Config_s *config) +LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config) { LKMotorInstance *motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance)); motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance)); memset(motor, 0, sizeof(LKMotorInstance)); motor->motor_settings = config->controller_setting_init_config; - PID_Init(&motor->current_PID, &config->controller_param_init_config.current_PID); - PID_Init(&motor->speed_PID, &config->controller_param_init_config.speed_PID); - PID_Init(&motor->angle_PID, &config->controller_param_init_config.angle_PID); + PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID); + PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID); + PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID); motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; config->can_init_config.id = motor; config->can_init_config.can_module_callback = LKMotorDecode; config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id; - config->can_init_config.tx_id = config->can_init_config.tx_id + 0x280; + config->can_init_config.tx_id = config->can_init_config.tx_id + 0x280 - 1; // 这样在发送写入buffer的时候更方便,因为下标从0开始,LK多电机发送id为0x280 motor->motor_can_ins = CANRegister(&config->can_init_config); - if (idx == 0) + if (idx == 0) // 用第一个电机的can instance发送数据 sender_instance = motor->motor_can_ins; LKMotorEnable(motor); - return lkmotor_instance[idx++]; + lkmotor_instance[idx++] = motor; + return motor; } /* 第一个电机的can instance用于发送数据,向其tx_buff填充数据 */ @@ -82,7 +88,7 @@ void LKMotorControl() pid_measure = *motor->other_angle_feedback_ptr; else pid_measure = measure->real_current; - pid_ref = PID_Calculate(&motor->angle_PID, pid_measure, pid_ref); + pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref); if (setting->feedforward_flag & SPEED_FEEDFORWARD) pid_ref += *motor->speed_feedforward_ptr; } @@ -93,31 +99,31 @@ void LKMotorControl() pid_measure = *motor->other_speed_feedback_ptr; else pid_measure = measure->speed_aps; - pid_ref = PID_Calculate(&motor->angle_PID, pid_measure, pid_ref); + pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref); if (setting->feedforward_flag & CURRENT_FEEDFORWARD) pid_ref += *motor->current_feedforward_ptr; } if (setting->close_loop_type & CURRENT_LOOP) { - pid_ref = PID_Calculate(&motor->current_PID, measure->real_current, pid_ref); + pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref); } set = pid_ref; if (setting->reverse_flag == MOTOR_DIRECTION_REVERSE) set *= -1; // 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance - memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280 - 1) * 2, &set, sizeof(uint16_t)); + memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t)); if (motor->stop_flag == MOTOR_STOP) { // 若该电机处于停止状态,直接将发送buff置零 - memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280 - 1) * 2, 0, sizeof(uint16_t)); + memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, 0, sizeof(uint16_t)); } } if (idx) // 如果有电机注册了 { - CANTransmit(sender_instance,1); + CANTransmit(sender_instance, 1); } } diff --git a/modules/motor/LKmotor/LK9025.h b/modules/motor/LKmotor/LK9025.h index e5e46b6..02974a9 100644 --- a/modules/motor/LKmotor/LK9025.h +++ b/modules/motor/LKmotor/LK9025.h @@ -13,19 +13,19 @@ #define CURRENT_SMOOTH_COEF 0.9f #define SPEED_SMOOTH_COEF 0.85f #define REDUCTION_RATIO_DRIVEN 1 -#define ECD_ANGLE_COEF_LK (360.0f/65536.0f) +#define ECD_ANGLE_COEF_LK (360.0f / 65536.0f) typedef struct // 9025 { - uint16_t last_ecd;// 上一次读取的编码器值 - uint16_t ecd; // + uint16_t last_ecd; // 上一次读取的编码器值 + uint16_t ecd; // 当前编码器值 float angle_single_round; // 单圈角度 - float speed_aps; // speed angle per sec(degree:°) - int16_t real_current; // 实际电流 - uint8_t temperate; //温度,C° + float speed_aps; // speed angle per sec(degree:°) + int16_t real_current; // 实际电流 + uint8_t temperate; // 温度,C° - float total_angle; // 总角度 - int32_t total_round; //总圈数 + float total_angle; // 总角度 + int32_t total_round; // 总圈数 } LKMotor_Measure_t; @@ -37,8 +37,8 @@ typedef struct float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针 float *other_speed_feedback_ptr; - float *speed_feedforward_ptr; - float *current_feedforward_ptr; + float *speed_feedforward_ptr; // 速度前馈数据指针,可以通过此指针设置速度前馈值,或LQR等时作为速度状态变量的输入 + float *current_feedforward_ptr; // 电流前馈指针 PIDInstance current_PID; PIDInstance speed_PID; PIDInstance angle_PID; @@ -46,22 +46,45 @@ typedef struct Motor_Working_Type_e stop_flag; // 启停标志 - CANInstance* motor_can_ins; - -}LKMotorInstance; + CANInstance *motor_can_ins; +} LKMotorInstance; -LKMotorInstance *LKMotroInit(Motor_Init_Config_s* config); +/** + * @brief 初始化LK电机 + * + * @param config 电机配置 + * @return LKMotorInstance* 返回实例指针 + */ +LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config); -void LKMotorSetRef(LKMotorInstance* motor,float ref); +/** + * @brief 设置参考值 + * @attention 注意此函数设定的ref是最外层闭环的输入,若要设定内层闭环的值请通过前馈数据指针设置 + * + * @param motor 要设置的电机 + * @param ref 设定值 + */ +void LKMotorSetRef(LKMotorInstance *motor, float ref); +/** + * @brief 为所有LK电机计算pid/反转/模式控制,并通过bspcan发送电流值(发送CAN报文) + * + */ void LKMotorControl(); +/** + * @brief 停止LK电机,之后电机不会响应任何指令 + * + * @param motor + */ void LKMotorStop(LKMotorInstance *motor); +/** + * @brief 启动LK电机 + * + * @param motor + */ void LKMotorEnable(LKMotorInstance *motor); -void LKMotorSetRef(LKMotorInstance *motor,float ref); - - #endif // LK9025_H diff --git a/modules/motor/LKmotor/LK_motor.md b/modules/motor/LKmotor/LK_motor.md index 657aa13..fec4fc7 100644 --- a/modules/motor/LKmotor/LK_motor.md +++ b/modules/motor/LKmotor/LK_motor.md @@ -2,4 +2,4 @@ LK motor 这是瓴控电机的模块封装说明文档。关于LK电机的控制报文和反馈报文值，详见LK电机的说明文档。 -注意LK电机在使用多电机发送的时候，只支持一条总线上至多4个电机，多电机模式下LK仅支持接收ID为0x280. \ No newline at end of file +注意LK电机在使用多电机发送的时候，只支持一条总线上至多4个电机，多电机模式下LK仅支持发送id 0x280为接收ID为0x140+id. \ No newline at end of file diff --git a/modules/oled/oled.md b/modules/oled/oled.md index 42fe703..cf95f6b 100644 --- a/modules/oled/oled.md +++ b/modules/oled/oled.md @@ -2,4 +2,12 @@ 当前oled支持不完整,api较为混乱. 需要使用bsp_iic进行实现的重构,并提供统一方便的接口 -请使用字库软件制作自己的图标和不同大小的ascii码. \ No newline at end of file +请使用字库软件制作自己的图标和不同大小的ascii码. + + +> 后续尝试移植一些图形库使得功能更加丰富 +> oled主要作调试和log/错误显示等使用 +> 可以提供给视觉和机械的同学调试接口,方便他们通过显示屏进行简单的设置 + + +*可以引入RoboMaster oled,或额外增加一个编码器用于控制oled界面并设定一些功能.* \ No newline at end of file diff --git a/modules/unicomm/unicomm.c b/modules/unicomm/unicomm.c new file mode 100644 index 0000000..e69de29 diff --git a/modules/unicomm/unicomm.h b/modules/unicomm/unicomm.h new file mode 100644 index 0000000..e69de29 diff --git a/modules/unicomm/unicomm.md b/modules/unicomm/unicomm.md new file mode 100644 index 0000000..21ee4fa --- /dev/null +++ b/modules/unicomm/unicomm.md @@ -0,0 +1,7 @@ +# univsersal communication + +unicomm旨在为通信提供一套标准的协议接口，屏蔽底层的硬件差异，使得上层应用可以定制通信协议，包括包长度/可变帧长/帧头尾/校验方式等。 + +不论底层具体使用的是什么硬件接口，硬件的每一帧传输完将数据放在缓冲区里之后，就没有任何区别了。 此模块实际上就是对缓冲区的rawdata进行操作，包括查找帧头，计算包长度，校验错误等。 + +完成之后，可以将module/can_comm移除，把原使用了cancomm的应用迁移到此模块。 \ No newline at end of file