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

修复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

2
modules/BMI088/bmi088.h
View File

@@ -109,7 +109,7 @@ BMI088_Data_t BMI088Acquire(BMI088Instance *bmi088);
/**
* @brief 标定传感器.BMI088在初始化的时候会调用此函数. 提供接口方便标定离线数据
* @attention @todo 注意,当操作系统开始运行后,此函数会和INS_Task冲突.目前不允许在运行时调用此函数,后续加入标志位判断以提供运行时重新的标定功能
*
*
* @param _bmi088 待标定的实例
*/
void BMI088CalibrateIMU(BMI088Instance *_bmi088);

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

38
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优化环节的实现---------------------------- */
@@ -40,7 +40,7 @@ static void f_Integral_Limit(PIDInstance *pid)
static float temp_Output, temp_Iout;
temp_Iout = pid->Iout + pid->ITerm;
temp_Output = pid->Pout + pid->Iout + pid->Dout;
if (abs(temp_Output) > pid->MaxOut)
if (abs(temp_Output) > pid->MaxOut)
{
if (pid->Err * pid->Iout > 0) // 积分却还在累积
{
@@ -117,7 +117,6 @@ static void f_PID_ErrorHandle(PIDInstance *pid)
}
}
/* ---------------------------下面是PID的外部算法接口--------------------------- */
/**
@@ -126,8 +125,8 @@ 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;
@@ -160,33 +158,33 @@ float PID_Calculate(PIDInstance *pid, float measure, float ref)
// 如果在死区外,则计算PID
if (abs(pid->Err) > pid->DeadBand)
{
{
// 基本的pid计算,使用位置式
pid->Pout = pid->Kp * pid->Err;
pid->ITerm = pid->Ki * pid->Err * pid->dt;
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,10 +192,10 @@ float PID_Calculate(PIDInstance *pid, float measure, float ref)
}
else // 进入死区, 则清空积分和输出
{
pid->Output=0;
pid->ITerm=0;
pid->Output = 0;
pid->ITerm = 0;
}
// 保存当前数据,用于下次计算
pid->Last_Measure = pid->Measure;
pid->Last_Output = pid->Output;

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

20
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的频率运行此任务 */
@@ -76,7 +76,7 @@ void INS_Task(void)
{
static uint32_t count = 0;
const float gravity[3] = {0, 0, 9.81f};
dt = DWT_GetDeltaT(&INS_DWT_Count);
t += dt;

4
modules/ist8310/ist8310.md
View File

@@ -1,4 +1,6 @@
# ist8310
## 使用示例
```c
@@ -25,4 +27,4 @@ IST8310_Init_Config_s ist8310_conf = {
IST8310Instance *asdf = IST8310Init(&ist8310_conf);
// 随后数据会被放到asdf.mag[i]中
```
```

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

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.

10
modules/oled/oled.md
View File

@@ -2,4 +2,12 @@
当前oled支持不完整,api较为混乱. 需要使用bsp_iic进行实现的重构,并提供统一方便的接口
请使用字库软件制作自己的图标和不同大小的ascii码.
请使用字库软件制作自己的图标和不同大小的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的应用迁移到此模块。