TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 11:37:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files

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

78
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 // 三环全开
```
@@ -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()`。
前提是已经将`DJIMotorControl()`放入实时系统任务当中或以一定d。你也可以单独执行`DJIMotorControl()`。

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

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

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

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.