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

增加了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

4
modules/can_comm/can_comm.c
View File

@@ -16,7 +16,7 @@ static void CANCommResetRx(CANCommInstance *ins)
}
/**
* @brief
* @brief cancomm的接收回调函数
*
* @param _instance
*/
@@ -66,7 +66,7 @@ static void CANCommRxCallback(CANInstance *_instance)
CANCommResetRx(comm);
return; // 重置状态然后返回
}
return; // 访问完一个can comm直接退出,一次中断只会也只可能会处理一个实例的回调
return; // 访问完一个can comm直接退出,一次中断只处理一个实例的回调
}
}
}

10
modules/can_comm/can_comm.h
View File

@@ -26,16 +26,16 @@ typedef struct
{
CANInstance *can_ins;
/* 发送部分 */
uint8_t send_data_len;
uint8_t send_buf_len;
uint8_t send_data_len; // 发送数据长度
uint8_t send_buf_len; // 发送缓冲区长度,为发送数据长度+帧头单包数据长度帧尾以及校验和(4)
uint8_t raw_sendbuf[CAN_COMM_MAX_BUFFSIZE + CAN_COMM_OFFSET_BYTES]; // 额外4个bytes保存帧头帧尾和校验和
/* 接收部分 */
uint8_t recv_data_len;
uint8_t recv_buf_len;
uint8_t recv_data_len; // 接收数据长度
uint8_t recv_buf_len; // 接收缓冲区长度,为接收数据长度+帧头单包数据长度帧尾以及校验和(4)
uint8_t raw_recvbuf[CAN_COMM_MAX_BUFFSIZE + CAN_COMM_OFFSET_BYTES]; // 额外4个bytes保存帧头帧尾和校验和
uint8_t unpacked_recv_data[CAN_COMM_MAX_BUFFSIZE]; // 解包后的数据,调用CANCommGet()后cast成对应的类型通过指针读取即可
/* 接收和更新标志位*/
uint8_t recv_state;
uint8_t recv_state; // 接收状态,
uint8_t cur_recv_len;
uint8_t update_flag;
} CANCommInstance;

34
modules/daemon/daemon.c
View File

@@ -1,21 +1,23 @@
#include "daemon.h"
#include "bsp_dwt.h" // 后续通过定时器来计时?
#include "stdlib.h"
#include "memory.h"
// 用于保存所有的daemon instance
static DaemonInstance *daemon_instances[DAEMON_MX_CNT] = {NULL};
static uint8_t idx;
static uint8_t idx; // 用于记录当前的daemon instance数量,配合回调使用
DaemonInstance *DaemonRegister(Daemon_Init_Config_s *config)
{
daemon_instances[idx] = (DaemonInstance *)malloc(sizeof(DaemonInstance));
DaemonInstance *instance = (DaemonInstance *)malloc(sizeof(DaemonInstance));
memset(instance, 0, sizeof(DaemonInstance));
daemon_instances[idx]->reload_count = config->reload_count;
daemon_instances[idx]->callback = config->callback;
daemon_instances[idx]->owner_id = config->id;
daemon_instances[idx]->temp_count = config->reload_count;
instance->owner_id = config->owner_id;
instance->reload_count = config->reload_count;
instance->callback = config->callback;
return daemon_instances[idx++];
daemon_instances[idx++] = instance;
return instance;
}
void DaemonReload(DaemonInstance *instance)
@@ -25,20 +27,20 @@ void DaemonReload(DaemonInstance *instance)
uint8_t DaemonIsOnline(DaemonInstance *instance)
{
return instance->temp_count>0;
return instance->temp_count > 0;
}
void DaemonTask()
{
static DaemonInstance* pins; //提高可读性同时降低访存开销
static DaemonInstance *dins; // 提高可读性同时降低访存开销
for (size_t i = 0; i < idx; ++i)
{
pins=daemon_instances[i];
if(pins->temp_count>0)
pins->temp_count--;
else if(pins->callback)
{ // 每个module根据自身的offline callback进行调用
pins->callback(pins->owner_id); // module将owner_id强制类型转换成自身类型
dins = daemon_instances[i];
if (dins->temp_count > 0)
dins->temp_count--;
else if (dins->callback) // 如果有callback
{
dins->callback(dins->owner_id); // module内可以将owner_id强制类型转换成自身类型从而调用自身的offline callback
}
}
}

18
modules/daemon/daemon.h
View File

@@ -11,11 +11,11 @@ typedef void (*offline_callback)(void *);
/* daemon结构体定义 */
typedef struct daemon_ins
{
uint16_t reload_count; // 重载值
uint16_t reload_count; // 重载值
offline_callback callback; // 异常处理函数,当模块发生异常时会被调用
uint16_t temp_count; //当前值,减为零说明模块离线或异常
void *owner_id; // daemon实例的地址,初始化的时候填入
uint16_t temp_count; // 当前值,减为零说明模块离线或异常
void *owner_id; // daemon实例的地址,初始化的时候填入
} DaemonInstance;
@@ -23,13 +23,13 @@ typedef struct daemon_ins
typedef struct
{
uint16_t reload_count; // 实际上这是app唯一需要设置的值?
offline_callback callback;
void *id; // id取拥有daemon的实例的地址,如DJIMotorInstance*,cast成void*类型
offline_callback callback;
void *owner_id; // id取拥有daemon的实例的地址,如DJIMotorInstance*,cast成void*类型
} Daemon_Init_Config_s;
/**
* @brief 注册一个daemon实例
*
*
* @param config 初始化配置
* @return DaemonInstance* 返回实例指针
*/
@@ -37,14 +37,14 @@ DaemonInstance *DaemonRegister(Daemon_Init_Config_s *config);
/**
* @brief 当模块收到新的数据或进行其他动作时,调用该函数重载temp_count,相当于"喂狗"
*
*
* @param instance daemon实例指针
*/
void DaemonReload(DaemonInstance *instance);
/**
* @brief 确认模块是否离线
*
*
* @param instance daemon实例指针
* @return uint8_t 若在线且工作正常,返回1;否则返回零. 后续根据异常类型和离线状态等进行优化.
*/
@@ -53,7 +53,7 @@ uint8_t DaemonIsOnline(DaemonInstance *instance);
/**
* @brief 放入rtos中,会给每个daemon实例的temp_count按频率进行递减操作.
* 模块成功接受数据或成功操作则会重载temp_count的值为reload_count.
*
*
*/
void DaemonTask();

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)

1
modules/super_cap/super_cap.c
View File

@@ -26,6 +26,7 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
{
super_cap_instance = (SuperCapInstance *)malloc(sizeof(SuperCapInstance));
memset(super_cap_instance, 0, sizeof(SuperCapInstance));
supercap_config->can_config.can_module_callback = SuperCapRxCallback;
super_cap_instance->can_ins = CANRegister(&supercap_config->can_config);
return super_cap_instance;

2
modules/super_cap/super_cap.h
View File

@@ -28,8 +28,6 @@ typedef struct
typedef struct
{
CAN_Init_Config_s can_config;
uint8_t send_data_len;
uint8_t recv_data_len;
} SuperCap_Init_Config_s;
SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config);