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

增加了电机前馈控制,更新readme，加快ins速度，删除了工程和平衡底盘相关的app

Browse Source
This commit is contained in:
NeoZng
2023-05-19 14:45:48 +08:00
parent ecb56ef935
commit a18a5091f4
27 changed files with 97 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
modules/BMI088/bmi088.c
View File

@@ -227,7 +227,7 @@ static void BMI088GyroINTCallback(GPIOInstance *gpio)
/**
* @brief
* @todo 现在要考虑一下数据返回的方式,指针还是结构体? 7个float数据有点费时,不然用DMA? or memcpy
* @todo 现在要考虑一下数据返回的方式,指针还是结构体?
*
* @param bmi088
* @return BMI088_Data_t

1
modules/BMI088/bmi088.h
View File

@@ -100,7 +100,6 @@ BMI088Instance *BMI088Register(BMI088_Init_Config_s *config);
/**
* @brief 读取BMI088数据
* @todo 7个float数据开销较大,后续考虑使用DMA或双缓冲区直接传递指针
* @param bmi088 BMI088实例指针
* @return BMI088_Data_t 读取到的数据
*/

5
modules/algorithm/algorithm.md
View File

@@ -3,11 +3,8 @@
<p align='right'>neozng1@hnu.edu.cn</p>
> TODO:
>
> 1. 实现麦轮和全向轮的速度解算
> 2. 实现LQR
> 3. 实现一些通用的滤波器，如指数平均，窗平均，低通等
> 4. 实现系统辨识
> 2. 实现一些通用的滤波器，如指数平均，窗平均，低通等
## 总览和使用

1
modules/algorithm/controller.c
View File

@@ -134,6 +134,7 @@ void PIDInit(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
DWT_GetDeltaT(&pid->DWT_CNT);
}
/**

2
modules/daemon/daemon.c
View File

@@ -13,7 +13,7 @@ DaemonInstance *DaemonRegister(Daemon_Init_Config_s *config)
memset(instance, 0, sizeof(DaemonInstance));
instance->owner_id = config->owner_id;
instance->reload_count = config->reload_count;
instance->reload_count = config->reload_count == 0 ? 100 : config->reload_count;
instance->callback = config->callback;
daemon_instances[idx++] = instance;

1
modules/master_machine/master_process.md
View File

@@ -94,7 +94,6 @@ typedef struct
float yaw;
float pitch;
float roll;
// uint32_t time_stamp; // @todo 用于和相机的时间戳对齐
} Vision_Send_s;
```

1
modules/message_center/message_center.h
View File

@@ -2,7 +2,6 @@
* @file message_center.h
* @author NeoZeng neozng1@hnu.edu.cn
* @brief 这是一个伪pubsub机制,仅对应用之间的通信进行了隔离
* @todo 实现基于链表-队列的pubsub机制
* @version 0.1
* @date 2022-11-30
*

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

@@ -160,6 +160,8 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
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;
instance->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr;
instance->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr;
// 后续增加电机前馈控制器(速度和电流)
// 电机分组,因为至多4个电机可以共用一帧CAN控制报文
@@ -251,6 +253,9 @@ void DJIMotorControl()
// 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
{
if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor_controller->speed_feedforward_ptr;
if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_speed_feedback_ptr;
else // MOTOR_FEED
@@ -260,6 +265,8 @@ void DJIMotorControl()
}
// 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor_controller->current_feedforward_ptr;
if (motor_setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);

4
modules/motor/DJImotor/dji_motor.h
View File

@@ -7,7 +7,6 @@
*
* @todo 1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏
2. 为M2006和M3508增加开环的零位校准函数,并在初始化时调用(根据用户配置决定是否调用)
3. 完成前馈功能（已经添加了前馈数据指针）
* @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
*
@@ -34,7 +33,7 @@ typedef struct
uint16_t last_ecd; // 上一次读取的编码器值
uint16_t ecd; // 0-8191,刻度总共有8192格
float angle_single_round; // 单圈角度
float speed_aps; // 角速度,单位为:度/秒 rpm:rounds per minute
float speed_aps; // 角速度,单位为:度/秒
int16_t real_current; // 实际电流
uint8_t temperate; // 温度 Celsius
@@ -100,7 +99,6 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback
/**
* @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
* @todo 增加前馈功能
*/
void DJIMotorControl();

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

@@ -1,6 +1,7 @@
#include "HT04.h"
#include "memory.h"
#include "general_def.h"
#include "bsp_dwt.h"
static uint8_t idx;
HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT];
@@ -43,19 +44,27 @@ static void HTMotorDecode(CANInstance *motor_can)
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
uint8_t *rxbuff = motor_can->rx_buff;
HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->measure; // 将can实例中保存的id转换成电机实例的指针
measure->feedback_dt = DWT_GetDeltaT(&measure->count);
measure->last_angle = measure->total_angle;
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16);
measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16) - measure->angle_bias;
tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
measure->speed_rads = SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) +
(1 - SPEED_SMOOTH_COEF) * measure->speed_rads;
measure->speed_rads = // SPEED_SMOOTH_COEF * measure->speed_rads + (1 - SPEED_SMOOTH_COEF) *
(measure->total_angle - measure->last_angle) / measure->feedback_dt;
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
(1 - CURRENT_SMOOTH_COEF) * measure->real_current;
measure->real_current = uint_to_float(((uint16_t)(rxbuff[3] << 4) | (uint16_t)(rxbuff[4] >> 4)), V_MIN, V_MAX, 12);
if (!measure->first_feedback_flag) // 初始化的时候设置偏置
{
measure->first_feedback_flag = 1;
measure->angle_bias = measure->total_angle;
measure->total_angle = 0;
measure->speed_rads = 0;
DWT_GetDeltaT(&measure->count);
}
// measure->real_current = uint_to_float((rxbuff[3] << 8) | rxbuff[4], I_MIN, I_MAX, 16);
}
HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
@@ -81,7 +90,7 @@ HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
void HTMotorSetRef(HTMotorInstance *motor, float ref)
{
motor->pid_ref = ref;
motor->pid_ref = ref * 0.285f;
}
void HTMotorControl()
@@ -107,7 +116,7 @@ void HTMotorControl()
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_angle_feedback_ptr;
else
pid_measure = measure->real_current;
pid_measure = measure->total_angle;
// measure单位是rad,ref是角度,统一到angle下计算,方便建模
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_DEGREE, pid_ref);
}
@@ -125,11 +134,10 @@ void HTMotorControl()
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_DEGREE, pid_ref);
}
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr;
if (setting->close_loop_type & CURRENT_LOOP)
{
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr;
pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
}
@@ -139,30 +147,34 @@ void HTMotorControl()
LIMIT_MIN_MAX(set, T_MIN, T_MAX); // 限幅,实际上这似乎和pid输出限幅重复了
tmp = float_to_uint(set, T_MIN, T_MAX, 12); // 数值最后在 -12~+12之间
if (motor->stop_flag == MOTOR_STOP)
tmp = float_to_uint(0, T_MIN, T_MAX, 12);
motor_can->tx_buff[6] = (tmp >> 8);
motor_can->tx_buff[7] = tmp & 0xff;
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将发送buff置零
memset(motor_can->tx_buff + 6, 0, sizeof(uint16_t));
}
CANTransmit(motor_can, 1);
}
}
void HTMotorStop(HTMotorInstance *motor)
{
if (motor->stop_flag == MOTOR_STOP)
return;
HTMotorSetMode(CMD_RESET_MODE, motor);
HTMotorSetMode(CMD_RESET_MODE, motor); // 发两次,确保电机停止
motor->stop_flag = MOTOR_STOP;
}
void HTMotorEnable(HTMotorInstance *motor)
{
if (motor->stop_flag == MOTOR_ENALBED)
return;
HTMotorSetMode(CMD_MOTOR_MODE, motor);
HTMotorSetMode(CMD_MOTOR_MODE, motor);
motor->stop_flag = MOTOR_ENALBED;
}
void HTMotorCalibEncoder(HTMotorInstance *motor)
{
HTMotorSetMode(CMD_ZERO_POSITION, motor);
}
// void HTMotorCalibEncoder(HTMotorInstance *motor)
// {
// HTMotorSetMode(CMD_ZERO_POSITION, motor);
// }

13
modules/motor/HTmotor/HT04.h
View File

@@ -19,10 +19,15 @@
typedef struct // HT04
{
float last_angle;
float total_angle; // 角度为多圈角度,范围是-95.5~95.5,单位为rad
float last_angle;
float speed_rads;
float real_current;
float angle_bias;
uint8_t first_feedback_flag;
float feedback_dt;
uint32_t count;
} HTMotor_Measure_t;
/* HT电机类型定义*/
@@ -91,11 +96,7 @@ void HTMotorStop(HTMotorInstance *motor);
void HTMotorEnable(HTMotorInstance *motor);
/**
* @brief 校准电机编码器
* @attention 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* @brief 校准电机编码器,设置的当前位置为0
*
* @param motor
*/

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

@@ -104,10 +104,10 @@ void LKMotorControl()
else
pid_measure = measure->speed_rads;
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr;
}
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr;
if (setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);

18
modules/motor/motor_def.h
View File

@@ -45,7 +45,7 @@ typedef enum
/* 反馈来源设定,若设为OTHER_FEED则需要指定数据来源指针,详见Motor_Controller_s*/
typedef enum
{
MOTOR_FEED=0,
MOTOR_FEED = 0,
OTHER_FEED,
} Feedback_Source_e;
@@ -71,13 +71,13 @@ typedef enum
/* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
typedef struct
{
Closeloop_Type_e outer_loop_type; // 最外层的闭环,未设置时默认为最高级的闭环
Closeloop_Type_e close_loop_type; // 使用几个闭环(串级)
Motor_Reverse_Flag_e motor_reverse_flag; // 是否反转
Closeloop_Type_e outer_loop_type; // 最外层的闭环,未设置时默认为最高级的闭环
Closeloop_Type_e close_loop_type; // 使用几个闭环(串级)
Motor_Reverse_Flag_e motor_reverse_flag; // 是否反转
Feedback_Reverse_Flag_e feedback_reverse_flag; // 反馈是否反向
Feedback_Source_e angle_feedback_source; // 角度反馈类型
Feedback_Source_e speed_feedback_source; // 速度反馈类型
Feedfoward_Type_e feedforward_flag; // 前馈标志
Feedback_Source_e angle_feedback_source; // 角度反馈类型
Feedback_Source_e speed_feedback_source; // 速度反馈类型
Feedfoward_Type_e feedforward_flag; // 前馈标志
} Motor_Control_Setting_s;
@@ -87,8 +87,8 @@ typedef struct
{
float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
float *other_speed_feedback_ptr;
// float *speed_foward_ptr;
// float *current_foward_ptr;
float *speed_feedforward_ptr;
float *current_feedforward_ptr;
PIDInstance current_PID;
PIDInstance speed_PID;

2
modules/remote/remote_control.c
View File

@@ -117,7 +117,6 @@ static void RemoteControlRxCallback()
*/
static void RCLostCallback(void *id)
{
// @todo 遥控器丢失的处理
USARTServiceInit(rc_usart_instance); // 尝试重新启动接收
}
@@ -130,7 +129,6 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle)
rc_usart_instance = USARTRegister(&conf);
// 进行守护进程的注册,用于定时检查遥控器是否正常工作
// @todo 当前守护进程直接在这里注册,后续考虑将其封装到遥控器的初始化函数中,即可以让用户决定reload_count的值(是否有必要?)
Daemon_Init_Config_s daemon_conf = {
.reload_count = 10, // 100ms未收到数据视为离线,遥控器的接收频率实际上是1000/14Hz(大约70Hz)
.callback = RCLostCallback,