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

增加了BSP PWM的支持,修改了bsp和module层的初始化提高可读性,新年快乐

Browse Source
This commit is contained in:
NeoZng
2023-01-01 12:45:07 +08:00
parent 08440ce116
commit 7e36f6a0cf
21 changed files with 469 additions and 424 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
modules/motor/HT04.c
View File

@@ -3,7 +3,7 @@
#include "general_def.h"
static uint8_t idx;
HTMotorInstance *ht_motor_info[HT_MOTOR_CNT];
HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT];
/**
* @brief
@@ -45,7 +45,7 @@ static void HTMotorDecode(CANInstance *motor_can)
static uint8_t *rxbuff;
rxbuff = motor_can->rx_buff;
measure = &((HTMotorInstance *)motor_can->id)->motor_measure;
measure = &((HTMotorInstance *)motor_can->id)->motor_measure; // 将can实例中保存的id转换成电机实例的指针
measure->last_angle = measure->total_angle;
@@ -63,23 +63,23 @@ static void HTMotorDecode(CANInstance *motor_can)
HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
{
HTMotorInstance *motor = (HTMotorInstance *)malloc(sizeof(HTMotorInstance));
memset(motor, 0, sizeof(HTMotorInstance));
ht_motor_info[idx] = (HTMotorInstance *)malloc(sizeof(HTMotorInstance));
memset(ht_motor_info[idx], 0, sizeof(HTMotorInstance));
ht_motor_info[idx]->motor_settings = config->controller_setting_init_config;
PID_Init(&ht_motor_info[idx]->current_PID, &config->controller_param_init_config.current_PID);
PID_Init(&ht_motor_info[idx]->speed_PID, &config->controller_param_init_config.speed_PID);
PID_Init(&ht_motor_info[idx]->angle_PID, &config->controller_param_init_config.angle_PID);
ht_motor_info[idx]->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
ht_motor_info[idx]->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
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);
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.can_module_callback = HTMotorDecode;
config->can_init_config.id = ht_motor_info[idx];
ht_motor_info[idx]->motor_can_instace = CANRegister(&config->can_init_config);
config->can_init_config.id = motor;
motor->motor_can_instace = CANRegister(&config->can_init_config);
HTMotorEnable(ht_motor_info[idx]);
return ht_motor_info[idx++];
HTMotorEnable(motor);
ht_motor_instance[idx++] = motor;
return motor;
}
void HTMotorSetRef(HTMotorInstance *motor, float ref)
@@ -99,7 +99,7 @@ void HTMotorControl()
// 遍历所有电机实例,计算PID
for (size_t i = 0; i < idx; i++)
{ // 先获取地址避免反复寻址
motor = ht_motor_info[i];
motor = ht_motor_instance[i];
measure = &motor->motor_measure;
setting = &motor->motor_settings;
motor_can = motor_can;

27
modules/motor/LK9025.c
View File

@@ -11,7 +11,7 @@ static void LKMotorDecode(CANInstance *_instance)
static LKMotor_Measure_t *measure;
static uint8_t *rx_buff;
rx_buff = _instance->rx_buff;
measure = &((LKMotorInstance *)_instance)->measure;
measure = &((LKMotorInstance *)_instance)->measure; // 通过caninstance保存的id获取对应的motorinstance
measure->last_ecd = measure->ecd;
measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
@@ -36,26 +36,27 @@ static void LKMotorDecode(CANInstance *_instance)
LKMotorInstance *LKMotroInit(Motor_Init_Config_s *config)
{
lkmotor_instance[idx] = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
memset(lkmotor_instance[idx], 0, sizeof(LKMotorInstance));
LKMotorInstance *motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
memset(motor, 0, sizeof(LKMotorInstance));
lkmotor_instance[idx]->motor_settings = config->controller_setting_init_config;
PID_Init(&lkmotor_instance[idx]->current_PID, &config->controller_param_init_config.current_PID);
PID_Init(&lkmotor_instance[idx]->speed_PID, &config->controller_param_init_config.speed_PID);
PID_Init(&lkmotor_instance[idx]->angle_PID, &config->controller_param_init_config.angle_PID);
lkmotor_instance[idx]->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
lkmotor_instance[idx]->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
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);
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 = lkmotor_instance[idx];
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;
lkmotor_instance[idx]->motor_can_ins = CANRegister(&config->can_init_config);
motor->motor_can_ins = CANRegister(&config->can_init_config);
if (idx == 0)
sender_instance = lkmotor_instance[idx]->motor_can_ins;
sender_instance = motor->motor_can_ins;
LKMotorEnable(lkmotor_instance[idx]);
LKMotorEnable(motor);
return lkmotor_instance[idx++];
}

172
modules/motor/dji_motor.c
View File

@@ -4,7 +4,7 @@
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
static DJIMotorInstance *dji_motor_info[DJI_MOTOR_CNT] = {NULL};
static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL};
/**
* @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送
@@ -54,7 +54,7 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config)
uint8_t motor_send_num;
uint8_t motor_grouping;
switch (dji_motor_info[idx]->motor_type)
switch (dji_motor_instance[idx]->motor_type)
{
case M2006:
case M3508:
@@ -72,13 +72,13 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config)
// 计算接收id并设置分组发送id
config->rx_id = 0x200 + motor_id + 1;
sender_enable_flag[motor_grouping] = 1;
dji_motor_info[idx]->message_num = motor_send_num;
dji_motor_info[idx]->sender_group = motor_grouping;
dji_motor_instance[idx]->message_num = motor_send_num;
dji_motor_instance[idx]->sender_group = motor_grouping;
// 检查是否发生id冲突
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_info[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance->rx_id == config->rx_id)
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
IDcrash_Handler(i, idx);
}
break;
@@ -97,12 +97,12 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config)
config->rx_id = 0x204 + motor_id + 1;
sender_enable_flag[motor_grouping] = 1;
dji_motor_info[idx]->message_num = motor_send_num;
dji_motor_info[idx]->sender_group = motor_grouping;
dji_motor_instance[idx]->message_num = motor_send_num;
dji_motor_instance[idx]->sender_group = motor_grouping;
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_info[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance->rx_id == config->rx_id)
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
IDcrash_Handler(i, idx);
}
break;
@@ -130,8 +130,8 @@ static void DecodeDJIMotor(CANInstance *_instance)
measure->last_ecd = measure->ecd;
measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd;
measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps +
RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float)((int16_t)(rxbuff[2] << 8 | rxbuff[3]));
measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps +
RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float)((int16_t)(rxbuff[2] << 8 | rxbuff[3]));
measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
measure->temperate = rxbuff[6];
@@ -147,42 +147,49 @@ static void DecodeDJIMotor(CANInstance *_instance)
// 电机初始化,返回一个电机实例
DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
{
dji_motor_info[idx] = (DJIMotorInstance *)malloc(sizeof(DJIMotorInstance));
memset(dji_motor_info[idx], 0, sizeof(DJIMotorInstance));
DJIMotorInstance *instance = (DJIMotorInstance *)malloc(sizeof(DJIMotorInstance));
memset(instance, 0, sizeof(DJIMotorInstance));
// motor basic setting
dji_motor_info[idx]->motor_type = config->motor_type;
dji_motor_info[idx]->motor_settings = config->controller_setting_init_config;
instance->motor_type = config->motor_type;
instance->motor_settings = config->controller_setting_init_config;
// motor controller init
PID_Init(&dji_motor_info[idx]->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
PID_Init(&dji_motor_info[idx]->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
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);
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;
// group motors, because 4 motors share the same CAN control message
MotorSenderGrouping(&config->can_init_config);
// register motor to CAN bus
config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
config->can_init_config.id = dji_motor_info[idx]; // set id,eq to address(it is identity)
dji_motor_info[idx]->motor_can_instance = CANRegister(&config->can_init_config);
config->can_init_config.id = instance; // set id,eq to address(it is identity)
instance->motor_can_instance = CANRegister(&config->can_init_config);
DJIMotorEnable(dji_motor_info[idx]);
return dji_motor_info[idx++];
DJIMotorEnable(instance);
dji_motor_instance[idx++] = instance;
return instance;
}
/* 电流只能通过电机自带传感器监测,后续考虑加入力矩传感器应变片等 */
void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type)
{
if (loop == ANGLE_LOOP)
{
motor->motor_settings.angle_feedback_source = type;
}
if (loop == SPEED_LOOP)
else if (loop == SPEED_LOOP)
{
motor->motor_settings.speed_feedback_source = type;
}
else
{
while (1)
; // LOOP TYPE ERROR!!!检查是否传入了正确的LOOP类型,或发生了指针越界
}
}
void DJIMotorStop(DJIMotorInstance *motor)
@@ -195,6 +202,7 @@ void DJIMotorEnable(DJIMotorInstance *motor)
motor->stop_flag = MOTOR_ENALBED;
}
/* 修改电机的实际闭环对象 */
void DJIMotorOuterLoop(DJIMotorInstance *motor, Closeloop_Type_e outer_loop)
{
motor->motor_settings.outer_loop_type = outer_loop;
@@ -211,70 +219,68 @@ void DJIMotorControl()
{
// 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销
// 同样可以提高可读性
static uint8_t group, num;
static int16_t set;
static uint8_t group, num; // 电机组号和组内编号
static int16_t set; // 电机控制CAN发送设定值
static DJIMotorInstance *motor;
static Motor_Control_Setting_s *motor_setting;
static Motor_Controller_s *motor_controller;
static DJI_Motor_Measure_s *motor_measure;
static float pid_measure, pid_ref;
static Motor_Control_Setting_s *motor_setting; // 电机控制参数
static Motor_Controller_s *motor_controller; // 电机控制器
static DJI_Motor_Measure_s *motor_measure; // 电机测量值
static float pid_measure, pid_ref; // 电机PID测量值和设定值
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_info[i])
{ // 减小访存开销,先保存指针引用
motor = dji_motor_instance[i];
motor_setting = &motor->motor_settings;
motor_controller = &motor->motor_controller;
motor_measure = &motor->motor_measure;
pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
// pid_ref会顺次通过被启用的闭环充当数据的载体
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP)
{
motor = dji_motor_info[i];
motor_setting = &motor->motor_settings;
motor_controller = &motor->motor_controller;
motor_measure = &motor->motor_measure;
pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
// 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; // 对total angle闭环,防止在边界处出现突跃
// 更新pid_ref进入下一个环
pid_ref = PID_Calculate(&motor_controller->angle_PID, pid_measure, pid_ref);
}
// 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
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 // MOTOR_FEED
pid_measure = motor_measure->speed_aps;
// 更新pid_ref进入下一个环
pid_ref = PID_Calculate(&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);
}
// 获取最终输出
set = (int16_t)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[2 * num] = (uint8_t)(set >> 8);
sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff);
// 电机是否停止运行
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将buff置零
memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
}
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_ref进入下一个环
pid_ref = PID_Calculate(&motor_controller->angle_PID, pid_measure, pid_ref);
}
// 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
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 // MOTOR_FEED
pid_measure = motor_measure->speed_aps;
// 更新pid_ref进入下一个环
pid_ref = PID_Calculate(&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);
}
// 获取最终输出
set = (int16_t)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[2 * num] = (uint8_t)(set >> 8);
sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff);
// 电机是否停止运行
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将buff置零
memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
}
}
// 遍历flag,检查是否要发送这一帧报文

31
modules/motor/servo_motor.c
View File

@@ -1,22 +1,26 @@
#include "servo_motor.h"
#include "stdlib.h"
#include "memory.h"
extern TIM_HandleTypeDef htim1;
/*第二版*/
static ServoInstance *servo_motor[SERVO_MOTOR_CNT] = {NULL};
static int16_t compare_value[SERVO_MOTOR_CNT]={0};
static ServoInstance *servo_motor_instance[SERVO_MOTOR_CNT] = {NULL};
static int16_t compare_value[SERVO_MOTOR_CNT] = {0};
static uint8_t servo_idx = 0; // register servo_idx,是该文件的全局舵机索引,在注册时使用
// 通过此函数注册一个舵机
ServoInstance *ServoInit(Servo_Init_Config_s *Servo_Init_Config)
{
servo_motor[servo_idx] = (ServoInstance *)malloc(sizeof(ServoInstance));
memset(servo_motor[servo_idx], 0, sizeof(ServoInstance));
servo_motor[servo_idx]->Servo_type = Servo_Init_Config->Servo_type;
servo_motor[servo_idx]->htim = Servo_Init_Config->htim;
servo_motor[servo_idx]->Channel = Servo_Init_Config->Channel;
HAL_TIM_Base_Start(Servo_Init_Config->htim);
ServoInstance *servo = (ServoInstance *)malloc(sizeof(ServoInstance));
memset(servo, 0, sizeof(ServoInstance));
servo->Servo_type = Servo_Init_Config->Servo_type;
servo->htim = Servo_Init_Config->htim;
servo->Channel = Servo_Init_Config->Channel;
HAL_TIM_PWM_Start(Servo_Init_Config->htim, Servo_Init_Config->Channel);
return servo_motor[servo_idx++];
servo_motor_instance[servo_idx++] = servo;
return servo;
}
/**
@@ -63,16 +67,15 @@ void Servo_Motor_StartSTOP_Angle_Set(ServoInstance *Servo_Motor, int16_t Start_a
}
/**
* @brief 舵机模式选择
*
*
* @param Servo_Motor 注册的舵机实例
* @param mode 需要选择的模式
*/
void Servo_Motor_Type_Select(ServoInstance *Servo_Motor,int16_t mode)
void Servo_Motor_Type_Select(ServoInstance *Servo_Motor, int16_t mode)
{
Servo_Motor->Servo_Angle_Type = mode;
}
/**
* @brief 舵机输出控制
*
@@ -84,9 +87,9 @@ void Servo_Motor_Control()
for (size_t i = 0; i < servo_idx; i++)
{
if (servo_motor[i])
if (servo_motor_instance[i])
{
Servo_Motor = servo_motor[i];
Servo_Motor = servo_motor_instance[i];
Servo_type = Servo_Motor->Servo_type;
switch (Servo_type)