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

修复了djimotor发送时的致命错误,编写了shoot app的框架

Browse Source
This commit is contained in:
NeoZng
2022-12-04 20:26:15 +08:00
parent 8e32fc0e6f
commit e94bb504b8
12 changed files with 254 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
modules/imu/ins_task.c
View File

@@ -19,17 +19,17 @@
#include "user_lib.h"
static INS_t INS;
IMU_Param_t IMU_Param;
PID_t TempCtrl = {0};
static IMU_Param_t IMU_Param;
static PID_t TempCtrl = {0};
const float xb[3] = {1, 0, 0};
const float yb[3] = {0, 1, 0};
const float zb[3] = {0, 0, 1};
// 用于获取两次采样之间的时间间隔
uint32_t INS_DWT_Count = 0;
static uint32_t INS_DWT_Count = 0;
static float dt = 0, t = 0;
float RefTemp = 40; // 恒温设定温度
static float RefTemp = 40; // 恒温设定温度
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);

4
modules/imu/ins_task.md
View File

@@ -8,3 +8,7 @@
## 算法解析
介绍EKF四元数姿态解算的教程在:[四元数EKF姿态更新算法](https://zhuanlan.zhihu.com/p/454155643)
## 模块移植
由于历史遗留问题,IMU模块耦合程度高.后续实现BSP_SPI,将bmi088 middleware删除.仅保留BMI088读取的协议和寄存器定义等,单独实现IMU模块.

4
modules/monitor/monitor.c
View File

@@ -1 +1,3 @@
#include "monitor.h"
#include "monitor.h"
#include "bsp_dwt.h"

54
modules/motor/dji_motor.c
View File

@@ -1,10 +1,9 @@
#include "dji_motor.h"
#define PI2 3.141592f
#define ECD_ANGLE_COEF 0.043945f // 360/8192
#define PI2 (3.141592f * 2) // 2pi
#define ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
static uint8_t stop_flag = 0;
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL};
@@ -196,6 +195,7 @@ void DJIMotorSetRef(dji_motor_instance *motor, float ref)
void DJIMotorControl()
{
// 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销
// 同样可以提高可读性
static uint8_t group, num;
static int16_t set;
static dji_motor_instance *motor;
@@ -213,40 +213,51 @@ void DJIMotorControl()
motor_controller = &motor->motor_controller;
motor_measure = &motor->motor_measure;
// pid_ref会顺次通过被启用的环充当数据的载体
if (motor_setting->close_loop_type & ANGLE_LOOP) // 计算位置环
// pid_ref会顺次通过被启用的闭环充当数据的载体
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP)
{
if (motor_setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_angle_feedback_ptr;
else // MOTOR_FEED
pid_measure = motor_measure->total_angle;
else // MOTOR_FEED
pid_measure = motor_measure->total_angle; // 对total angle闭环,防止在边界处出现突跃
// 更新pid_ref进入下一个环
motor_controller->pid_ref = PID_Calculate(&motor_controller->angle_PID, pid_measure, motor_controller->pid_ref);
}
if (motor_setting->close_loop_type & SPEED_LOOP) // 计算速度环
// 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type | (ANGLE_LOOP | SPEED_LOOP)))
{
if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_speed_feedback_ptr;
else
pid_measure = motor_measure->speed_rpm;
// 更新pid_ref进入下一个环
motor_controller->pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, motor_controller->pid_ref);
}
if (motor_setting->close_loop_type & CURRENT_LOOP) // 计算电流环
// 计算电流环,只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身的反馈
if (motor_setting->close_loop_type & CURRENT_LOOP)
{
motor_controller->pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->given_current, motor_controller->pid_ref);
}
set = (int16_t)motor_controller->pid_ref; // 获取最终输出
if (motor_setting->reverse_flag == MOTOR_DIRECTION_REVERSE) // 设置反转
set *= -1;
// 获取最终输出
set = (int16_t)motor_controller->pid_ref;
if (motor_setting->reverse_flag == MOTOR_DIRECTION_REVERSE)
set *= -1; // 设置反转
// 分组填入发送数据
group = motor->sender_group;
num = motor->message_num;
sender_assignment[group].tx_buff[num] = 0xff & set >> 8;
sender_assignment[group].tx_buff[2 * num] = 0xff & set >> 8;
sender_assignment[group].tx_buff[2 * num + 1] = 0xff & set;
// 电机是否停止运行
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将buff置零
memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
}
}
else // 遇到空指针说明所有遍历结束,退出循环
break;
@@ -257,21 +268,22 @@ void DJIMotorControl()
{
if (sender_enable_flag[i])
{
if (stop_flag) // 如果紧急停止,将所有发送信息置零
{
memset(sender_assignment[i].tx_buff, 0, 8);
}
CANTransmit(&sender_assignment[i]);
}
}
}
void DJIMotorStop()
void DJIMotorStop(dji_motor_instance *motor)
{
stop_flag = 1;
motor->stop_flag = MOTOR_STOP;
}
void DJIMotorEnable()
void DJIMotorEnable(dji_motor_instance *motor)
{
stop_flag = 0;
motor->stop_flag = MOTOR_ENALBED;
}
void DJIMotorOuterLoop(dji_motor_instance *motor, Closeloop_Type_e outer_loop)
{
motor->motor_settings.outer_loop_type = outer_loop;
}

19
modules/motor/dji_motor.h
View File

@@ -61,6 +61,8 @@ typedef struct
Motor_Type_e motor_type;
Motor_Working_Type_e stop_flag;
} dji_motor_instance;
/**
@@ -106,16 +108,25 @@ void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedba
void DJIMotorControl();
/**
* @brief 停止所有电机,注意不是将设定值设为零,而是直接给电机发送的电流值置零
* @brief 停止电机,注意不是将设定值设为零,而是直接给电机发送的电流值置零
*
*/
void DJIMotorStop();
void DJIMotorStop(dji_motor_instance* motor);
/**
* @brief 启动所有电机,此时电机会响应设定值
* @brief 启动电机,此时电机会响应设定值
* 初始化时不需要此函数,因为stop_flag的默认值为0
*
*/
void DJIMotorEnable();
void DJIMotorEnable(dji_motor_instance* motor);
/**
* @brief 修改电机闭环目标(外层闭环)
*
* @param motor 要修改的电机实例指针
* @param outer_loop 外层闭环类型
*/
void DJIMotorOuterLoop(dji_motor_instance *motor, Closeloop_Type_e outer_loop);
#endif // !DJI_MOTOR_H

47
modules/motor/dji_motor.md
View File

@@ -11,7 +11,15 @@
> 如果你不需要理解该模块的工作原理，你只需要查看这一小节。
dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详尽的封装。你不再需要关心PID的计算以及CAN报文的发送和接收解析，你只需要专注于根据应用层的需求，设定合理的期望值，并通过`DJIMotorSetRef()`设置对应电机的输入参考即可。如果你希望更改电机的反馈来源，比如进入小陀螺模式（这时候你想要云台保持静止，使用IMU的yaw角度值作为反馈来源），只需要调用`DJIMotorChangeFeed()`，电机便可立刻切换反馈数据来源至IMU。
dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详尽的封装。你不再需要关心PID的计算以及CAN报文的发送和接收解析，你只需要专注于根据应用层的需求，设定合理的期望值，并通过`DJIMotorSetRef()`设置对应电机的输入参考即可。
**==设定值的单位==**
1. 位置环为角度，角度制。
2. 速度环为角速度，单位为度/每秒
3.
如果你希望更改电机的反馈来源，比如进入小陀螺模式/视觉模式（这时候你想要云台保持静止，使用IMU的yaw角度值作为反馈来源），只需要调用`DJIMotorChangeFeed()`，电机便可立刻切换反馈数据来源至IMU。
要获得一个电机，请通过`DJIMotorInit()`并传入一些参数，他就会返回一个电机的指针。你也不再需要查看这些电机和电调的说明书，**只需要设置其电机id**（6020为拨码开关值，2006和3508为电调的闪动次数），该模块会自动为你计算CAN发送和接收ID并搞定所有硬件层的琐事。
@@ -41,9 +49,9 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详
CURRENT_LOOP
SPEED_LOOP
ANGLE_LOOP
CURRENT_LOOP| SPEED_LOOP // 同时对电流和速度闭环
SPEED_LOOP | ANGLE_LOOP // 同时对速度和位置闭环
CURRENT_LOOP| SPEED_LOOP |ANGLE_LOOP // 三环全开
CURRENT_LOOP | SPEED_LOOP // 同时对电流和速度闭环
SPEED_LOOP | ANGLE_LOOP // 同时对速度和位置闭环
CURRENT_LOOP | SPEED_LOOP |ANGLE_LOOP // 三环全开
```
- 是否反转
@@ -120,9 +128,11 @@ Motor_Init_Config_s config = {
.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,
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
.outer_loop_type = SPEED_LOOP,
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
.speed_feedback_source = MOTOR_FEED,
.reverse_flag = MOTOR_DIRECTION_NORMAL},
// 电流环和速度环PID参数的设置,不采用计算优化则不需要传入Improve参数
@@ -218,7 +228,9 @@ typedef struct
/* sender assigment*/
uint8_t sender_group;
uint8_t message_num;
uint8_t stop_flag;
Motor_Type_e motor_type;
} dji_motor_instance;
```
@@ -233,6 +245,7 @@ typedef struct
```c
typedef struct /* 电机控制配置 */
{
Closeloop_Type_e outer_loop_type;
Closeloop_Type_e close_loop_type;
Reverse_Flag_e reverse_flag;
Feedback_Source_e angle_feedback_source;
@@ -257,6 +270,9 @@ typedef struct
```
以M3508为例，假设需要进行**速度闭环**和**电流闭环**，那么在初始化时就将这个变量的值设为`CURRENT_LOOP | SPEED_LOOP`。在`DJIMotorControl()`中，函数将会根据此标志位判断设定的参考值需要经过那些控制器的计算。
另外,你还需要设置当前电机的最外层闭环，即电机的闭环目标为什么类型的值。初始化时需要设置`outer_loop_type`。以M2006作为拨盘电机时为例，你希望它在单发/双发等固定发射数量的模式下对位置进行闭环（拨盘转过一定角度对应拨出一颗弹丸），但你也有可能希望在连发的时候让拨盘连续的转动，以一定的频率发射弹丸。我们提供了`DJIMotorOuterLoop()`用于修改电机的外层闭环，改变电机的闭环对象。
> 注意，务必分清串级控制（多环）和外层闭环的区别。前者是为了提高内环的性能，使得其能更好地跟随外环参考值；而后者描述的是系统真实的控制目标（闭环目标）。如3508，没有电流环仍然可以对速度完成闭环，对于高层的应用来说，它们本质上不关心电机内部是否还有电流环，它们只把外层闭环为速度的电机当作一个**速度伺服执行器**，**外层闭环**描述的就是真正的闭环目标。
- 为了避开恼人的正负号，提高代码的可维护性，在初始化电机时设定`reverse_flag`使得所有电机都按照你想要的方向旋转，其定义如下：
@@ -321,6 +337,12 @@ void DJIMotorChangeFeed(dji_motor_instance *motor,
Feedback_Source_e type);
void DJIMotorControl();
void DJIMotorStop(dji_motor_instance *motor);
void DJIMotorEnable(dji_motor_instance *motor);
void DJIMotorOuterLoop(dji_motor_instance *motor);
```
- `DJIMotorInit()`是用于初始化电机对象的接口，传入包括电机can配置、电机控制配置、电机控制器配置以及电机类型在内的初始化参数。**它将会返回一个电机实例指针**，你应当在应用层保存这个指针，这样才能操控这个电机。
@@ -337,6 +359,12 @@ void DJIMotorControl();
2. 根据反转标志位，确定是否将输出反转
3. 根据每个电机的发送分组，将最终输出值填入对应的分组buff
4. 检查每一个分组，若该分组有电机，发送报文
- `DJIMotorStop()`和`DJIMotorEnable()`用于控制电机的启动和停止。当电机被设为stop的时候，不会响应任何的参考输入。
- `DJIMotorOuterLoop()`用于修改电机的外部闭环类型，即电机的真实闭环目标。
## 私有函数和变量
@@ -350,7 +378,7 @@ static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL};
这是管理所有电机实例的入口。idx用于电机初始化。
```c
#define PI2 3.141592f
#define PI2 (3.141592f * 2)
#define ECD_ANGLE_COEF 3.835e-4 // ecd/8192*pi
```
@@ -404,6 +432,7 @@ Motor_Init_Config_s 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
@@ -436,4 +465,4 @@ dji_motor_instance *djimotor = DJIMotorInit(&config);
DJIMotorSetRef(djimotor, 10);
```
前提是已经将`MotorTask()`放入实时系统任务当中。你也可以单独执行`MotorControl()`。
前提是已经将`DJIMotorControl()`放入实时系统任务当中或以一定d。你也可以单独执行`DJIMotorControl()`。

22
modules/motor/motor_def.h
View File

@@ -17,7 +17,6 @@
#define LIMIT_MIN_MAX(x, min, max) (x) = (((x) <= (min)) ? (min) : (((x) >= (max)) ? (max) : (x)))
/**
* @brief 闭环类型,如果需要多个闭环,则使用或运算
* 例如需要速度环和电流环: CURRENT_LOOP|SPEED_LOOP
@@ -49,13 +48,20 @@ typedef enum
MOTOR_DIRECTION_REVERSE = 1
} Reverse_Flag_e;
typedef enum
{
MOTOR_STOP = 0,
MOTOR_ENALBED = 1,
} Motor_Working_Type_e;
/* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
typedef struct
{
Closeloop_Type_e close_loop_type;
Reverse_Flag_e reverse_flag;
Feedback_Source_e angle_feedback_source;
Feedback_Source_e speed_feedback_source;
Closeloop_Type_e outer_loop_type; // 最外层的闭环,未设置时默认为最高级的闭环
Closeloop_Type_e close_loop_type; // 使用几个闭环(串级)
Reverse_Flag_e reverse_flag; // 是否反转
Feedback_Source_e angle_feedback_source; // 角度反馈类型
Feedback_Source_e speed_feedback_source; // 速度反馈类型
} Motor_Control_Setting_s;
@@ -63,9 +69,9 @@ typedef struct
// 后续增加前馈数据指针
typedef struct
{
float *other_angle_feedback_ptr;
float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
float *other_speed_feedback_ptr;
// float *angle_foward_ptr;
// float *angle_foward_ptr; //前馈数据指针
// float *speed_foward_ptr;
// float *current_foward_ptr;
@@ -83,7 +89,7 @@ typedef enum
M3508 = 1,
M2006 = 2,
LK9025 = 3,
HT04 = 4
HT04 = 4,
} Motor_Type_e;
/**