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

finish the beta version of dji_motor

Browse Source
This commit is contained in:
NeoZng
2022-10-31 20:20:16 +08:00
parent 3dd4f1066c
commit c113ca81e0
20 changed files with 618 additions and 1137 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
modules/motor/HT04.c
View File

@@ -40,7 +40,7 @@ joint_instance* HTMotorInit(can_instance_config config)
{
static uint8_t idx;
joint_motor_info[idx]=(joint_instance*)malloc(sizeof(joint_instance));
joint_motor_info[idx++]->motor_can_instace=CANRegister(config);
CANRegister(&joint_motor_info[idx++]->motor_can_instace,config);
}
void JointControl(joint_instance* _instance,float current)
@@ -50,7 +50,7 @@ void JointControl(joint_instance* _instance,float current)
tmp = float_to_uint(current, T_MIN, T_MAX, 12);
_instance->motor_can_instace->rx_buff[6] = tmp>>8;
_instance->motor_can_instace->rx_buff[7] = tmp&0xff;
CANTransmit(&_instance->motor_can_instace);
CANTransmit(_instance->motor_can_instace);
}
void SetJointMode(joint_mode cmd,joint_instance* _instance)
@@ -58,5 +58,5 @@ void SetJointMode(joint_mode cmd,joint_instance* _instance)
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
buf[7]=(uint8_t)cmd;
memcpy(_instance->motor_can_instace->rx_buff,buf,8*sizeof(uint8_t));
CANTransmit(&_instance->motor_can_instace);
CANTransmit(_instance->motor_can_instace);
}

2
modules/motor/LK9025.c
View File

@@ -23,7 +23,7 @@ driven_instance* LKMotroInit(can_instance_config config)
static uint8_t idx;
driven_motor_info[idx]=(driven_instance*)malloc(sizeof(driven_instance));
config.can_module_callback=DecodeDriven;
driven_motor_info[idx++]->motor_can_instance=CANRegister(config);
CANRegister(driven_motor_info[idx++]->motor_can_instance,config);
}
void DrivenControl(int16_t motor1_current,int16_t motor2_current)

220
modules/motor/dji_motor.c
View File

@@ -1,6 +1,7 @@
#include "dji_motor.h"
static uint8_t idx = 0; // register idx
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL};
@@ -29,60 +30,8 @@ static can_instance sender_assignment[6] =
static uint8_t sender_enable_flag[6] = {0};
/**
* @brief 根据电调/拨码开关上的ID,计算发送ID和接收ID,并对电机进行分组以便处理多电机控制命令
*
* @param config
*/
static void MotorSenderGrouping(can_instance_config *config)
{
uint8_t motor_id = config->tx_id - 1;
uint8_t motor_rx_id;
uint8_t motor_send_num;
uint8_t motor_grouping;
switch (dji_motor_info[idx]->motor_type)
{
case M2006:
case M3508:
if (motor_id < 4)
{
dji_motor_info[idx]->message_num = motor_id;
dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 1 : 4;
}
else
{
dji_motor_info[idx]->message_num = motor_id - 4;
dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 0 : 3;
}
config->rx_id = 0x200 + motor_id;
sender_enable_flag[dji_motor_info[idx]->sender_group] = 1;
break;
case GM6020:
if (motor_id < 4)
{
dji_motor_info[idx]->message_num = motor_id;
dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 0 : 3;
}
else
{
dji_motor_info[idx]->message_num = motor_id - 4;
dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 2 : 5;
}
config->rx_id = 0x204 + motor_id;
sender_enable_flag[dji_motor_info[idx]->sender_group] = 1;
break;
// other motors should not be registered here
default:
break;
}
}
/**
* @todo 待添加此功能.
*
* @brief 当注册的电机id冲突时,会进入这个函数并提示冲突的ID
*
* @todo 通过segger jlink 发送日志
*/
static void IDcrash_Handler()
{
@@ -91,17 +40,86 @@ static void IDcrash_Handler()
};
}
/**
* @brief 根据电调/拨码开关上的ID,计算发送ID和接收ID,并对电机进行分组以便处理多电机控制命令
*
* @param config
*/
static void MotorSenderGrouping(can_instance_config *config)
{
uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值
uint8_t motor_send_num;
uint8_t motor_grouping;
switch (dji_motor_info[idx]->motor_type)
{
case M2006:
case M3508:
if (motor_id < 4) // 根据ID分组
{
motor_send_num = motor_id;
motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
}
// 计算接收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;
// 检查是否发生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)
IDcrash_Handler();
}
break;
case GM6020:
if (motor_id < 4)
{
motor_send_num = motor_id;
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
}
config->rx_id = 0x204 + motor_id;
sender_enable_flag[motor_grouping] = 1;
dji_motor_info[idx]->message_num = motor_send_num;
dji_motor_info[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)
IDcrash_Handler();
}
break;
default: // other motors should not be registered here
break;
}
}
/**
* @brief 根据返回的can_instance对反馈报文进行解析
*
* @param _instance 收到数据的instance,通过遍历与所有电机进行对比
* @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
*/
static void DecodeDJIMotor(can_instance *_instance)
{
uint8_t *rxbuff = _instance->rx_buff;
for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
{
if (dji_motor_info[i]->motor_can_instance == _instance)
if (&dji_motor_info[i]->motor_can_instance == _instance)
{
dji_motor_info[i]->motor_measure.last_ecd = dji_motor_info[i]->motor_measure.ecd;
dji_motor_info[i]->motor_measure.ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]);
@@ -113,52 +131,108 @@ static void DecodeDJIMotor(can_instance *_instance)
}
}
dji_motor_instance *DJIMotorInit(can_instance_config config,
// 电机初始化,返回一个电机实例
dji_motor_instance *DJIMotorInit(can_instance_config can_config,
Motor_Control_Setting_s motor_setting,
Motor_Controller_Init_s controller_init,
Motor_Type_e type)
{
dji_motor_info[idx] = (dji_motor_instance *)malloc(sizeof(dji_motor_instance));
// motor setting
// motor basic setting
dji_motor_info[idx]->motor_type = type;
dji_motor_info[idx]->motor_settings = motor_setting;
// motor controller init
// @todo : PID init
PID_Init(&dji_motor_info[idx]->motor_controller.current_PID, &controller_init.current_PID);
PID_Init(&dji_motor_info[idx]->motor_controller.speed_PID, &controller_init.speed_PID);
PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &controller_init.angle_PID);
dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = controller_init.other_angle_feedback_ptr;
dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = controller_init.other_speed_feedback_ptr;
// group motors, because 4 motors share the same CAN control message
MotorSenderGrouping(&config);
MotorSenderGrouping(&can_config);
// register motor to CAN bus
config.can_module_callback = DecodeDJIMotor;
dji_motor_info[idx]->motor_can_instance = CANRegister(config);
can_config.can_module_callback = DecodeDJIMotor; // set callback
CANRegister(&dji_motor_info[idx]->motor_can_instance, can_config);
return dji_motor_info[idx++];
}
// 改变反馈来源
void DJIMotorChangeFeed(dji_motor_instance *motor,Closeloop_Type_e loop,Feedback_Source_e type)
{
if(loop==ANGLE_LOOP)
{
motor->motor_settings.angle_feedback_source=type;
}
if(loop==SPEED_LOOP)
{
motor->motor_settings.speed_feedback_source=type;
}
}
// 设置参考值
void DJIMotorSetRef(dji_motor_instance *motor, float ref)
{
motor->motor_controller.pid_ref = ref;
}
void DJIMotorControl()
{
static uint8_t group, num, set;
{
// 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销
static uint8_t group, num;
static int16_t set;
static dji_motor_instance* motor;
static Motor_Control_Setting_s *motor_setting;
static Motor_Controller_s *motor_controller;
static dji_motor_measure *motor_measure;
static float pid_measure;
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
{
if (dji_motor_info[i])
{
// @todo: 计算PID
// calculate pid output
// ...
group = dji_motor_info[i]->sender_group;
num = dji_motor_info[i]->message_num;
set = (int16_t)dji_motor_info[i]->motor_controller.pid_output;
// sender_assignment[group].rx_buff[num]= 0xff & PIDoutPIDoutput>>8;
// sender_assignment[group].rx_buff[num]= 0xff & PIDoutput;
motor=dji_motor_info[i];
motor_setting=&motor->motor_settings;
motor_controller=&motor->motor_controller;
motor_measure=&motor->motor_measure;
if (motor_setting->close_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;
// 更新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->speed_feedback_source == OTHER_FEED)
pid_measure=*motor_controller->other_speed_feedback_ptr;
else
pid_measure=motor_measure->speed_rpm;
motor_controller->pid_ref=PID_Calculate(&motor_controller->speed_PID,pid_measure,motor_controller->pid_ref);
}
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;
// 分组填入发送数据
group = motor->sender_group;
num = motor->message_num;
sender_assignment[group].tx_buff[num] = 0xff & set >> 8;
sender_assignment[group].tx_buff[num+1] = 0xff & set;
}
else
else // 遇到空指针说明所有遍历结束,退出循环
break;
}

43
modules/motor/dji_motor.h
View File

@@ -7,6 +7,18 @@
#include "controller.h"
#include "motor_def.h"
/* DJI电机CAN反馈信息*/
typedef struct
{
uint16_t ecd;
uint16_t last_ecd;
int16_t speed_rpm;
int16_t given_current;
uint8_t temperate;
int16_t total_round;
int32_t total_angle;
} dji_motor_measure;
/**
* @brief DJI intelligent motor typedef
*
@@ -14,16 +26,7 @@
typedef struct
{
/* motor measurement recv from CAN feedback */
struct
{
uint16_t ecd;
uint16_t last_ecd;
int16_t speed_rpm;
int16_t given_current;
uint8_t temperate;
int16_t total_round;
int32_t total_angle;
} motor_measure;
dji_motor_measure motor_measure;
/* basic config of a motor*/
Motor_Control_Setting_s motor_settings;
@@ -32,7 +35,7 @@ typedef struct
Motor_Controller_s motor_controller;
/* the CAN instance own by motor instance*/
can_instance *motor_can_instance;
can_instance motor_can_instance;
/* sender assigment*/
uint8_t sender_group;
@@ -44,8 +47,6 @@ typedef struct
/**
* @todo 加入ID冲突的检查机制,如果发现注册的ID冲突,进入IDcrash_Handler()的死循环中
*
* @brief 调用此函数注册一个DJI智能电机,需要传递较多的初始化参数,请在application初始化的时候调用此函数
* 推荐传参时像标准库一样构造initStructure然后传入此函数.
* recommend: type xxxinitStructure = {
@@ -55,8 +56,8 @@ typedef struct
* 请注意不要在一条总线上挂载过多的电机(超过6个),若一定要这么做,请降低每个电机的反馈频率(设为500Hz),
* 并减小DJIMotorControl()任务的运行频率.
*
* @attention 当前并没有对电机的ID冲突进行检查,请保证在注册电机的时候,他们的反馈ID不会产生冲突!
* M3508和M2006的反馈报文都是0x200+id,而GM6020的反馈是0x204+id,请注意前两者和后者的id不要冲突.
* @attention M3508和M2006的反馈报文都是0x200+id,而GM6020的反馈是0x204+id,请注意前两者和后者的id不要冲突.
* 如果产生冲突,在初始化电机的时候会进入IDcrash_Handler(),可以通过debug来判断是否出现冲突.
*
* @param config 电机can设置,对于DJI电机仅需要将tx_id设置为电调闪动次数(c620,615,610)或拨码开关的值(GM6020)
* 你不需要自己查表计算发送和接收id,我们会帮你处理好!
@@ -87,8 +88,18 @@ void DJIMotorSetRef(dji_motor_instance *motor, float ref);
/**
* @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
* @brief 切换反馈的目标来源,如将角速度和角度的来源换为IMU(小陀螺模式常用)
*
* @param motor 要切换反馈数据来源的电机
* @param loop 要切换反馈数据来源的控制闭环
* @param type 目标反馈模式
*/
void DJIMotorChangeFeed(dji_motor_instance *motor,Closeloop_Type_e loop,Feedback_Source_e type);
/**
* @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
* @todo 增加前馈功能
*/
void DJIMotorControl();

85
modules/motor/motor_def.h
View File

@@ -1,27 +1,31 @@
#ifndef MOTOR_DEF_H
#define MOTOR_DEF_H
typedef enum
typedef enum
{
CURRENT=0,
SPEED=1,
ANGLE=2
CURRENT_LOOP = 0b0001,
SPEED_LOOP = 0b0010,
ANGLE_LOOP = 0b0100,
// only for check
_ = 0b0011,
__ = 0b0110,
___ = 0b0111
} Closeloop_Type_e;
typedef enum
{
MOTOR=0,
OTHER=1
MOTOR_FEED = 0,
OTHER_FEED = 1
} Feedback_Source_e;
typedef enum
{
CLOCKWISE=0,
COUNTER_CLOCKWISE=1
MOTOR_DIRECTION_NORMAL = 0,
MOTOR_DIRECTION_REVERSE = 1
} Reverse_Flag_e;
typedef struct
typedef struct
{
Closeloop_Type_e close_loop_type;
Reverse_Flag_e reverse_flag;
@@ -30,46 +34,37 @@ typedef struct
} Motor_Control_Setting_s;
typedef struct
{
float *other_angle_feedback_ptr;
float *other_speed_feedback_ptr;
PID_t current_PID;
PID_t speed_PID;
PID_t angle_PID;
// 将会作为每个环的输入和输出
float pid_ref;
} Motor_Controller_s;
typedef enum
{
GM6020 = 0,
M3508 = 1,
M2006 = 2,
LK9025 = 3,
HT04 = 4
} Motor_Type_e;
typedef struct
{
float* other_angle_feedback_ptr;
float* other_speed_feedback_ptr;
float *other_angle_feedback_ptr;
float *other_speed_feedback_ptr;
PID_t* current_PID;
PID_t* speed_PID;
PID_t* angle_PID;
PID_Init_config_s current_PID;
PID_Init_config_s speed_PID;
PID_Init_config_s angle_PID;
float pid_ref;
float pid_output;
} Motor_Controller_s;
typedef enum
{
GM6020=0,
M3508=1,
M2006=2,
LK9025=3,
HT04=4
} Motor_Type_e;
typedef struct
{
/* data */
} PID_config_s;
typedef struct
{
float* other_angle_feedback_ptr;
float* other_speed_feedback_ptr;
PID_config_s current_PID;
PID_config_s speed_PID;
PID_config_s angle_PID;
} Motor_Controller_Init_s;
#endif // !MOTOR_DEF_H

31
modules/motor/motor_task.c
View File

@@ -1,32 +1,11 @@
#include "motor_task.h"
static dji_motor_instance* dji_motor_info[DJI_MOTOR_CNT];
static joint_instance* joint_motor_info[HT_MOTOR_CNT];
static driven_instance* driven_motor_info[LK_MOTOR_CNT];
void RegisterMotor(Motor_Type_e motor_type,void* motor_instance)
{
static uint8_t dji_idx,joint_idx,driven_idx;
switch (motor_type)
{
case GM6020:
case M3508:
case M2006:
dji_motor_info[dji_idx++]=(dji_motor_instance*)motor_instance;
break;
case LK9025:
driven_motor_info[driven_idx++]=(driven_instance*)motor_instance;
break;
case HT04:
joint_motor_info[joint_idx++]=(joint_instance*)motor_instance;
break;
}
}
void MotorControlTask()
{
DJIMotorControl();
//LKMotorControl();
//HTMotorControl();
}

5
modules/motor/motor_task.h
View File

@@ -4,13 +4,8 @@
#include "LK9025.h"
#include "HT04.h"
#include "dji_motor.h"
#include "motor_def.h"
void MotorControlTask();
void RegisterMotor(Motor_Type_e motor_type,void* motor_instance);
#endif // !MOTOR_TASK_H