HFUT_Firmament_EC
/
sentry_chassis_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修复LK电机id计算错误，构建平衡底盘框架，增加通用通信模块，增加平衡底盘条件编译兼容，删除lqr

This commit is contained in:
NeoZng 2023-02-16 15:46:04 +08:00
parent b9a7d87dfd
commit a2a83f9fbf
35 changed files with 554 additions and 242 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
Makefile
View File

@ -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 # 如果切换芯片可能需要修改此值

12
TODO.md
View File

@ -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
- [ ] 增加步进电机模块

229
application/balance_chassis/balance.c Normal file
View File

@ -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()
{
}

17
application/balance_chassis/balance.h Normal file
View File

@ -0,0 +1,17 @@
#pragma once
/**
* @brief 平衡底盘初始化
*
*/
void BalanceInit();
/**
* @brief 平衡底盘任务
*
*/
void BalanceTask();

24
application/balance_chassis/balance.md Normal file
View File

@ -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有更好的表现

24
application/balance_chassis/gain_table.h Normal file
View File

@ -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)
{
}

2
application/chassis/chassis.c
View File

@ -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);

2
application/lift/lift.md
View File

@ -1 +1,3 @@
抬升/横移机构
气缸和电磁阀似乎没有必要构建模块,但是也可以组合微动开关和编码器+继电器等传感器构成模块,降低lift的复杂度,完成解耦.

4
application/robot_def.h
View File

@ -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;

2
bsp/can/bsp_can.c
View File

@ -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) // 超时
{

33
bsp/gpio/bsp_gpio.c
View File

@ -3,7 +3,7 @@
#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,并调用模块回调函数(如果有)
@ -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));
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;
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);
}

18
bsp/spi/bsp_spi.c
View File

@ -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,10 +126,10 @@ 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) // 回调函数不为空, 则调用回调函数
{

6
bsp/spi/bsp_spi.h
View File

@ -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; // 传输工作模式

16
modules/BMI088/bmi088.md
View File

@ -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. 写入的数据

1
modules/algorithm/LQR.c
View File

@ -1 +0,0 @@
#include "LQR.h"

12
modules/algorithm/LQR.h
View File

@ -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

30
modules/algorithm/controller.c
View File

@ -9,7 +9,7 @@
* @copyrightCopyright (c) 2022 HNU YueLu EC all rights reserved
*/
#include "controller.h"
#include <memory.h>
#include "memory.h"
/* ----------------------------下面是pid优化环节的实现---------------------------- */
@ -117,7 +117,6 @@ static void f_PID_ErrorHandle(PIDInstance *pid)
}
}
/* ---------------------------下面是PID的外部算法接口--------------------------- */
/**
@ -126,7 +125,7 @@ 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;
@ -167,26 +165,26 @@ float PID_Calculate(PIDInstance *pid, float measure, float ref)
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,8 +192,8 @@ float PID_Calculate(PIDInstance *pid, float measure, float ref)
}
else // 进入死区, 则清空积分和输出
{
pid->Output=0;
pid->ITerm=0;
pid->Output = 0;
pid->ITerm = 0;
}
// 保存当前数据,用于下次计算

46
modules/algorithm/controller.h
View File

@ -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|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
PID_Improvement_e Improve;
//-----------------------------------
// for calculating
float Measure;
@ -96,31 +96,31 @@ typedef struct
} PIDInstance;
/* 用于PID初始化的结构体*/
typedef struct
typedef struct // config parameter
{
// config parameter
// basic parameter
float Kp;
float Ki;
float Kd;
float MaxOut; // 输出限幅
float DeadBand; // 死区
float MaxOut; // 输出限幅
// improve parameter
PID_Improvement_e Improve;
float IntegralLimit; // 积分限幅
float DeadBand; // 死区
float CoefA; // For Changing Integral
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
PID_Improvement_e Improve;
} PID_Init_Config_s;
/**
* @brief 初始化PID实例
*
* @todo 待修改为统一的PIDRegister风格
* @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);
/**
* @brief 计算PID输出
@ -130,6 +130,6 @@ void PID_Init(PIDInstance *pid, PID_Init_Config_s *config);
* @param ref 设定值
* @return float PID计算输出
*/
float PID_Calculate(PIDInstance *pid, float measure, float ref);
float PIDCalculate(PIDInstance *pid, float measure, float ref);
#endif

1
modules/algorithm/kalman_filter.h
View File

@ -37,6 +37,7 @@
#endif
#endif
// 若运算速度不够,可以使用q31代替f32,但是精度会降低
#define mat arm_matrix_instance_f32
#define Matrix_Init arm_mat_init_f32
#define Matrix_Add arm_mat_add_f32

9
modules/algorithm/user_lib.c
View File

@ -12,7 +12,7 @@
******************************************************************************
*/
#include "stdlib.h"
#include "string.h"
#include "memory.h"
#include "user_lib.h"
#include "math.h"
#include "main.h"
@ -25,10 +25,10 @@
uint8_t GlobalDebugMode = 7;
void* zero_malloc(size_t size)
void *zero_malloc(size_t size)
{
void* ptr=malloc(size);
memset(ptr,0,size);
void *ptr = malloc(size);
memset(ptr, 0, size);
return ptr;
}
@ -96,7 +96,6 @@ float float_deadband(float Value, float minValue, float maxValue)
return Value;
}
// 限幅函数
float float_constrain(float Value, float minValue, float maxValue)
{

18
modules/imu/ins_task.c
View File

@ -39,7 +39,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[
*/
static void IMU_Temperature_Ctrl(void)
{
PID_Calculate(&TempCtrl, BMI088.Temperature, RefTemp);
PIDCalculate(&TempCtrl, BMI088.Temperature, RefTemp);
IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
}
@ -58,17 +58,17 @@ attitude_t *INS_Init(void)
IMU_QuaternionEKF_Init(10, 0.001, 10000000, 1, 0);
// imu heat init
PID_Init_Config_s config = {.MaxOut = 2000,
.IntegralLimit = 300,
.DeadBand = 0,
.Kp = 1000,
.Ki = 20,
.Kd = 0,
.Improve = 0x01}; // enable integratiaon limit
PID_Init(&TempCtrl, &config);
.IntegralLimit = 300,
.DeadBand = 0,
.Kp = 1000,
.Ki = 20,
.Kd = 0,
.Improve = 0x01}; // enable integratiaon limit
PIDInit(&TempCtrl, &config);
// noise of accel is relatively big and of high freq,thus lpf is used
INS.AccelLPF = 0.0085;
return (attitude_t*)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
}
/* 注意以1kHz的频率运行此任务 */

2
modules/ist8310/ist8310.md
View File

@ -1,4 +1,6 @@
# ist8310
## 使用示例
```c

23
modules/led/led.c
View File

@ -1,44 +1,39 @@
#include "led.h"
#include "stdlib.h"
#include "string.h"
#include "memory.h"
#include "user_lib.h"
static uint8_t idx;
static LEDInstance* bsp_led_ins[LED_MAX_NUM] = {NULL};
static LEDInstance *bsp_led_ins[LED_MAX_NUM] = {NULL};
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_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()
{
}

12
modules/motor/DJImotor/dji_motor.c
View File

@ -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);
}
// 获取最终输出

70
modules/motor/DJImotor/dji_motor.md
View File

@ -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 // 三环全开
```
@ -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文件内为私有函数和外部接口的定义。
@ -237,7 +223,7 @@ typedef struct
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,

16
modules/motor/HTmotor/HT04.c
View File

@ -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;

32
modules/motor/LKmotor/LK9025.c
View File

@ -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);
}
}

59
modules/motor/LKmotor/LK9025.h
View File

@ -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;
CANInstance *motor_can_ins;
}LKMotorInstance;
} LKMotorInstance;
/**
* @brief 初始化LK电机
*
* @param config 电机配置
* @return LKMotorInstance* 返回实例指针
*/
LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config);
LKMotorInstance *LKMotroInit(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

2
modules/motor/LKmotor/LK_motor.md
View File

@ -2,4 +2,4 @@ LK motor
这是瓴控电机的模块封装说明文档。关于LK电机的控制报文和反馈报文值，详见LK电机的说明文档。
注意LK电机在使用多电机发送的时候，只支持一条总线上至多4个电机，多电机模式下LK仅支持接收ID为0x280.
注意LK电机在使用多电机发送的时候，只支持一条总线上至多4个电机，多电机模式下LK仅支持发送id 0x280为接收ID为0x140+id.

8
modules/oled/oled.md
View File

@ -3,3 +3,11 @@
当前oled支持不完整,api较为混乱. 需要使用bsp_iic进行实现的重构,并提供统一方便的接口
请使用字库软件制作自己的图标和不同大小的ascii码.
> 后续尝试移植一些图形库使得功能更加丰富
> oled主要作调试和log/错误显示等使用
> 可以提供给视觉和机械的同学调试接口,方便他们通过显示屏进行简单的设置
*可以引入RoboMaster oled,或额外增加一个编码器用于控制oled界面并设定一些功能.*

0
modules/unicomm/unicomm.c Normal file
View File

0
modules/unicomm/unicomm.h Normal file
View File

7
modules/unicomm/unicomm.md Normal file
View File

@ -0,0 +1,7 @@
# univsersal communication
unicomm旨在为通信提供一套标准的协议接口，屏蔽底层的硬件差异，使得上层应用可以定制通信协议，包括包长度/可变帧长/帧头尾/校验方式等。
不论底层具体使用的是什么硬件接口，硬件的每一帧传输完将数据放在缓冲区里之后，就没有任何区别了。 此模块实际上就是对缓冲区的rawdata进行操作，包括查找帧头，计算包长度，校验错误等。
完成之后，可以将module/can_comm移除，把原使用了cancomm的应用迁移到此模块。