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

简化了电机测量的命名,完成初版平衡底盘的功能编写,待测试方向和符号的正确性

Browse Source
This commit is contained in:
NeoZng
2023-04-14 22:26:33 +08:00
parent 9e364cbaaa
commit 1818edf117
14 changed files with 215 additions and 162 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
modules/algorithm/user_lib.c
View File

@@ -165,7 +165,7 @@ int float_rounding(float raw)
}
// 三维向量归一化
float* Norm3d(float* v)
float *Norm3d(float *v)
{
float len = Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
v[0] /= len;
@@ -181,7 +181,7 @@ float NormOf3d(float *v)
}
// 三维向量叉乘v1 x v2
void Cross3d(float* v1, float* v2,float* res)
void Cross3d(float *v1, float *v2, float *res)
{
res[0] = v1[1] * v2[2] - v1[2] * v2[1];
res[1] = v1[2] * v2[0] - v1[0] * v2[2];
@@ -189,7 +189,7 @@ void Cross3d(float* v1, float* v2,float* res)
}
// 三维向量点乘
float Dot3d(float* v1, float* v2)
float Dot3d(float *v1, float *v2)
{
return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}

10
modules/general_def.h
View File

@@ -3,15 +3,15 @@
// 一些module的通用数值型定义,注意条件macro兼容,一些宏可能在math.h中已经定义过了
#ifndef PI
#define PI 3.1415926535f
#endif // !PI
#endif
#define PI2 (PI * 2.0f) // 2 pi
#define RAD_2_ANGLE 57.2957795f // 180/pi
#define ANGLE_2_RAD 0.01745329252f // pi/180
#define RAD_2_DEGREE 57.2957795f // 180/pi
#define DEGREE_2_RAD 0.01745329252f // pi/180
#define RPM_2_ANGLE_PER_SEC 6.0f // ×360°/60sec
#define RPM_2_ANGLE_PER_SEC 6.0f // ×360°/60sec
#define RPM_2_RAD_PER_SEC 0.104719755f // ×2pi/60sec
#endif // !GENERAL_DEF_H

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

@@ -124,7 +124,7 @@ static void DecodeDJIMotor(CANInstance *_instance)
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
// _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
uint8_t *rxbuff = _instance->rx_buff;
DJI_Motor_Measure_s *measure = &(((DJIMotorInstance *)_instance->id)->motor_measure); // measure要多次使用,保存指针减小访存开销
DJI_Motor_Measure_s *measure = &(((DJIMotorInstance *)_instance->id)->measure); // measure要多次使用,保存指针减小访存开销
// 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
measure->last_ecd = measure->ecd;
@@ -223,7 +223,7 @@ void DJIMotorControl()
DJIMotorInstance *motor;
Motor_Control_Setting_s *motor_setting; // 电机控制参数
Motor_Controller_s *motor_controller; // 电机控制器
DJI_Motor_Measure_s *motor_measure; // 电机测量值
DJI_Motor_Measure_s *measure; // 电机测量值
float pid_measure, pid_ref; // 电机PID测量值和设定值
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
@@ -232,10 +232,10 @@ void DJIMotorControl()
motor = dji_motor_instance[i];
motor_setting = &motor->motor_settings;
motor_controller = &motor->motor_controller;
motor_measure = &motor->motor_measure;
measure = &motor->measure;
pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
pid_ref*= -1; // 设置反转
pid_ref *= -1; // 设置反转
// pid_ref会顺次通过被启用的闭环充当数据的载体
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP)
@@ -243,7 +243,7 @@ void DJIMotorControl()
if (motor_setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_angle_feedback_ptr;
else
pid_measure = motor_measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
pid_measure = measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
// 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref);
}
@@ -254,7 +254,7 @@ void DJIMotorControl()
if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_speed_feedback_ptr;
else // MOTOR_FEED
pid_measure = motor_measure->speed_aps;
pid_measure = measure->speed_aps;
// 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
}
@@ -262,12 +262,11 @@ void DJIMotorControl()
// 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
if (motor_setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PIDCalculate(&motor_controller->current_PID, motor_measure->real_current, pid_ref);
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
}
// 获取最终输出
set = (int16_t)pid_ref;
// 分组填入发送数据
group = motor->sender_group;

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

@@ -49,7 +49,7 @@ typedef struct
typedef struct
{
DJI_Motor_Measure_s motor_measure; // 电机测量值
DJI_Motor_Measure_s measure; // 电机测量值
Motor_Control_Setting_s motor_settings; // 电机设置
Motor_Controller_s motor_controller; // 电机控制器

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

@@ -42,16 +42,16 @@ 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)->motor_measure; // 将can实例中保存的id转换成电机实例的指针
HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->measure; // 将can实例中保存的id转换成电机实例的指针
measure->last_angle = measure->total_angle;
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
measure->total_angle = RAD_2_ANGLE * uint_to_float(tmp, P_MIN, P_MAX, 16);
measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16);
tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
measure->speed_aps = SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) +
(1 - SPEED_SMOOTH_COEF) * measure->speed_aps;
measure->speed_rads = SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) +
(1 - SPEED_SMOOTH_COEF) * measure->speed_rads;
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
@@ -97,7 +97,7 @@ void HTMotorControl()
for (size_t i = 0; i < idx; i++)
{ // 先获取地址避免反复寻址
motor = ht_motor_instance[i];
measure = &motor->motor_measure;
measure = &motor->measure;
setting = &motor->motor_settings;
motor_can = motor->motor_can_instace;
pid_ref = motor->pid_ref;
@@ -109,7 +109,7 @@ void HTMotorControl()
else
pid_measure = measure->real_current;
// measure单位是rad,ref是角度,统一到angle下计算,方便建模
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_ANGLE, pid_ref);
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_DEGREE, pid_ref);
}
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
@@ -120,9 +120,9 @@ void HTMotorControl()
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else
pid_measure = measure->speed_aps;
pid_measure = measure->speed_rads;
// measure单位是rad / s ,ref是angle per sec,统一到angle下计算
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_ANGLE, pid_ref);
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_DEGREE, pid_ref);
}
if (setting->close_loop_type & CURRENT_LOOP)

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

@@ -18,17 +18,17 @@
#define T_MAX 18.0f
typedef struct // HT04
{
float last_angle;
{
float last_angle;
float total_angle; // 角度为多圈角度,范围是-95.5~95.5,单位为rad
float speed_aps;
float speed_rads;
float real_current;
} HTMotor_Measure_t;
/* HT电机类型定义*/
typedef struct
{
HTMotor_Measure_t motor_measure;
HTMotor_Measure_t measure;
Motor_Control_Setting_s motor_settings;
@@ -42,7 +42,7 @@ typedef struct
float pid_ref;
Motor_Working_Type_e stop_flag; // 启停标志
CANInstance *motor_can_instace;
} HTMotorInstance;
@@ -55,16 +55,16 @@ typedef enum
} HTMotor_Mode_t;
/**
* @brief
*
* @param config
* @return HTMotorInstance*
* @brief
*
* @param config
* @return HTMotorInstance*
*/
HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config);
/**
* @brief 设定电机的参考值
*
*
* @param motor 要设定的电机
* @param current 设定值
*/
@@ -72,20 +72,20 @@ void HTMotorSetRef(HTMotorInstance *motor, float ref);
/**
* @brief 给所有的HT电机发送控制指令
*
*
*/
void HTMotorControl();
/**
* @brief 停止电机,之后电机不会响应HTMotorSetRef设定的值
*
* @param motor
*
* @param motor
*/
void HTMotorStop(HTMotorInstance *motor);
/**
* @brief 启动电机
*
*
* @param motor 要启动的电机
*/
void HTMotorEnable(HTMotorInstance *motor);
@@ -96,8 +96,8 @@ void HTMotorEnable(HTMotorInstance *motor);
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
*
* @param motor
*
* @param motor
*/
void HTMotorCalibEncoder(HTMotorInstance *motor);

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

@@ -1,5 +1,6 @@
#include "LK9025.h"
#include "stdlib.h"
#include "general_def.h"
static uint8_t idx;
static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
@@ -23,8 +24,8 @@ static void LKMotorDecode(CANInstance *_instance)
measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd;
measure->speed_aps = (1 - SPEED_SMOOTH_COEF) * measure->speed_aps +
SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
measure->speed_rads = (1 - SPEED_SMOOTH_COEF) * measure->speed_rads +
DEGREE_2_RAD * SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
measure->real_current = (1 - CURRENT_SMOOTH_COEF) * measure->real_current +
CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
@@ -101,7 +102,7 @@ void LKMotorControl()
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else
pid_measure = measure->speed_aps;
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;

3
modules/motor/LKmotor/LK9025.h
View File

@@ -14,13 +14,14 @@
#define SPEED_SMOOTH_COEF 0.85f
#define REDUCTION_RATIO_DRIVEN 1
#define ECD_ANGLE_COEF_LK (360.0f / 65536.0f)
#define CURRENT_TORQUE_COEF_LK 0.003645f // 电流设定值转换成扭矩的系数,算出来的设定值除以这个系数就是扭矩值
typedef struct // 9025
{
uint16_t last_ecd; // 上一次读取的编码器值
uint16_t ecd; // 当前编码器值
float angle_single_round; // 单圈角度
float speed_aps; // speed angle per sec(degree:°)
float speed_rads; // speed rad/s
int16_t real_current; // 实际电流
uint8_t temperate; // 温度,C°