修复LK电机id计算错误，构建平衡底盘框架，增加通用通信模块，增加平衡底盘条件编译兼容，删除lqr
This commit is contained in:
NeoZng
parent
b9a7d87dfd
commit
a2a83f9fbf
19
Makefile
19
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 # 如果切换芯片可能需要修改此值
|
||||
|
||||
|
|
|
|||
12
TODO.md
12
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
|
||||
|
||||
- [ ] 增加步进电机模块
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
{
|
||||
}
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
#pragma once
|
||||
|
||||
/**
|
||||
* @brief 平衡底盘初始化
|
||||
*
|
||||
*/
|
||||
void BalanceInit();
|
||||
|
||||
/**
|
||||
* @brief 平衡底盘任务
|
||||
*
|
||||
*/
|
||||
void BalanceTask();
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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有更好的表现
|
||||
|
|
@ -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)
|
||||
{
|
||||
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -1 +1,3 @@
|
|||
抬升/横移机构
|
||||
|
||||
气缸和电磁阀似乎没有必要构建模块,但是也可以组合微动开关和编码器+继电器等传感器构成模块,降低lift的复杂度,完成解耦.
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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) // 超时
|
||||
{
|
||||
|
|
|
|||
|
|
@ -64,4 +64,3 @@ GPIO_PinState GPIORead(GPIOInstance *_instance)
|
|||
{
|
||||
return HAL_GPIO_ReadPin(_instance->GPIOx, _instance->GPIO_Pin);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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) // 回调函数不为空, 则调用回调函数
|
||||
{
|
||||
|
||||
|
|
|
|||
|
|
@ -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; // 传输工作模式
|
||||
|
|
|
|||
|
|
@ -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 +0,0 @@
|
|||
#include "LQR.h"
|
||||
|
|
@ -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
|
||||
|
|
@ -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,10 +143,10 @@ 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计算的时间间隔,用于积分和微分
|
||||
|
|
@ -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->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出
|
||||
|
||||
// 输出滤波
|
||||
if (pid->Improve & OutputFilter)
|
||||
if (pid->Improve & PID_OutputFilter)
|
||||
f_Output_Filter(pid);
|
||||
|
||||
// 输出限幅
|
||||
|
|
|
|||
|
|
@ -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_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 IntegralLimit; // 积分限幅
|
||||
float DeadBand; // 死区
|
||||
|
||||
// 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|<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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@
|
|||
******************************************************************************
|
||||
*/
|
||||
#include "stdlib.h"
|
||||
#include "string.h"
|
||||
#include "memory.h"
|
||||
#include "user_lib.h"
|
||||
#include "math.h"
|
||||
#include "main.h"
|
||||
|
|
@ -96,7 +96,6 @@ float float_deadband(float Value, float minValue, float maxValue)
|
|||
return Value;
|
||||
}
|
||||
|
||||
|
||||
// 限幅函数
|
||||
float float_constrain(float Value, float minValue, float maxValue)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
|
|
@ -64,7 +64,7 @@ attitude_t *INS_Init(void)
|
|||
.Ki = 20,
|
||||
.Kd = 0,
|
||||
.Improve = 0x01}; // enable integratiaon limit
|
||||
PID_Init(&TempCtrl, &config);
|
||||
PIDInit(&TempCtrl, &config);
|
||||
|
||||
// noise of accel is relatively big and of high freq,thus lpf is used
|
||||
INS.AccelLPF = 0.0085;
|
||||
|
|
|
|||
|
|
@ -1,4 +1,6 @@
|
|||
|
||||
# ist8310
|
||||
|
||||
## 使用示例
|
||||
|
||||
```c
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
#include "led.h"
|
||||
#include "stdlib.h"
|
||||
#include "string.h"
|
||||
#include "memory.h"
|
||||
#include "user_lib.h"
|
||||
|
||||
static uint8_t idx;
|
||||
|
|
@ -19,10 +19,8 @@ LEDInstance *LEDRegister(LED_Init_Config_s *led_config)
|
|||
|
||||
void LEDSet(LEDInstance *_led, uint8_t alpha, uint8_t color_value, uint8_t brightness)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
void LEDSwitch(LEDInstance *_led, uint8_t led_switch)
|
||||
{
|
||||
if (led_switch == 1)
|
||||
|
|
@ -36,9 +34,6 @@ void LEDSwitch(LEDInstance *_led,uint8_t led_switch)
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
void LEDShow()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
// 获取最终输出
|
||||
|
|
|
|||
|
|
@ -98,7 +98,7 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详
|
|||
float Derivative_LPF_RC;
|
||||
|
||||
PID_Improvement_e Improve; // 优化环节，定义在下一个代码块
|
||||
} PID_Init_config_s;
|
||||
} PIDInit_config_s;
|
||||
// 只有当你设启用了对应的优化环节，优化参数才会生效
|
||||
```
|
||||
|
||||
|
|
@ -123,12 +123,8 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详
|
|||
float *other_speed_feedback_ptr
|
||||
```
|
||||
|
||||
|
||||
|
||||
---
|
||||
|
||||
|
||||
|
||||
推荐的初始化参数编写格式如下：
|
||||
|
||||
```c
|
||||
|
|
@ -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文件内为私有函数和外部接口的定义。
|
||||
|
|
@ -372,8 +358,6 @@ void DJIMotorOuterLoop(dji_motor_instance *motor);
|
|||
|
||||
- `DJIMotorOuterLoop()`用于修改电机的外部闭环类型，即电机的真实闭环目标。
|
||||
|
||||
|
||||
|
||||
## 私有函数和变量
|
||||
|
||||
在.c文件内设为static的函数和变量
|
||||
|
|
|
|||
|
|
@ -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,7 +133,7 @@ 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;
|
||||
|
|
|
|||
|
|
@ -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,25 +99,25 @@ 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));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@
|
|||
typedef struct // 9025
|
||||
{
|
||||
uint16_t last_ecd; // 上一次读取的编码器值
|
||||
uint16_t ecd; //
|
||||
uint16_t ecd; // 当前编码器值
|
||||
float angle_single_round; // 单圈角度
|
||||
float speed_aps; // speed angle per sec(degree:°)
|
||||
int16_t real_current; // 实际电流
|
||||
|
|
@ -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;
|
||||
|
|
@ -50,18 +50,41 @@ typedef struct
|
|||
|
||||
} LKMotorInstance;
|
||||
|
||||
/**
|
||||
* @brief 初始化LK电机
|
||||
*
|
||||
* @param config 电机配置
|
||||
* @return LKMotorInstance* 返回实例指针
|
||||
*/
|
||||
LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config);
|
||||
|
||||
LKMotorInstance *LKMotroInit(Motor_Init_Config_s* config);
|
||||
|
||||
/**
|
||||
* @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,4 +2,4 @@ LK motor
|
|||
|
||||
这是瓴控电机的模块封装说明文档。关于LK电机的控制报文和反馈报文值，详见LK电机的说明文档。
|
||||
|
||||
注意LK电机在使用多电机发送的时候，只支持一条总线上至多4个电机，多电机模式下LK仅支持接收ID为0x280.
|
||||
注意LK电机在使用多电机发送的时候，只支持一条总线上至多4个电机，多电机模式下LK仅支持发送id 0x280为接收ID为0x140+id.
|
||||
|
|
@ -3,3 +3,11 @@
|
|||
当前oled支持不完整,api较为混乱. 需要使用bsp_iic进行实现的重构,并提供统一方便的接口
|
||||
|
||||
请使用字库软件制作自己的图标和不同大小的ascii码.
|
||||
|
||||
|
||||
> 后续尝试移植一些图形库使得功能更加丰富
|
||||
> oled主要作调试和log/错误显示等使用
|
||||
> 可以提供给视觉和机械的同学调试接口,方便他们通过显示屏进行简单的设置
|
||||
|
||||
|
||||
*可以引入RoboMaster oled,或额外增加一个编码器用于控制oled界面并设定一些功能.*
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
# univsersal communication
|
||||
|
||||
unicomm旨在为通信提供一套标准的协议接口，屏蔽底层的硬件差异，使得上层应用可以定制通信协议，包括包长度/可变帧长/帧头尾/校验方式等。
|
||||
|
||||
不论底层具体使用的是什么硬件接口，硬件的每一帧传输完将数据放在缓冲区里之后，就没有任何区别了。 此模块实际上就是对缓冲区的rawdata进行操作，包括查找帧头，计算包长度，校验错误等。
|
||||
|
||||
完成之后，可以将module/can_comm移除，把原使用了cancomm的应用迁移到此模块。
|
||||
Loading…
Reference in New Issue