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

完成了LKmotor模块的重构，优化了DJIMotor的反馈计算

Browse Source
This commit is contained in:
NeoZng
2022-12-11 20:48:24 +08:00
parent 78cc27ee1a
commit 2f41e67de0
15 changed files with 274 additions and 245 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
modules/motor/HT04.c
View File

@@ -1,7 +1,7 @@
#include "HT04.h"
#include "memory.h"
joint_instance *joint_motor_info[HT_MOTOR_CNT];
HKMotor_Measure_t *joint_motor_info[HT_MOTOR_CNT];
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
@@ -36,15 +36,15 @@ static void DecodeJoint(CANInstance *motor_instance)
}
}
joint_instance *HTMotorInit(CAN_Init_Config_s config)
HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config)
{
static uint8_t idx;
joint_motor_info[idx] = (joint_instance *)malloc(sizeof(joint_instance));
joint_motor_info[idx] = (HKMotor_Measure_t *)malloc(sizeof(HKMotor_Measure_t));
joint_motor_info[idx]->motor_can_instace = CANRegister(&config);
return joint_motor_info[idx++];
}
void JointControl(joint_instance *_instance, float current)
void JointControl(HKMotor_Measure_t *_instance, float current)
{
uint16_t tmp;
LIMIT_MIN_MAX(current, T_MIN, T_MAX);
@@ -54,7 +54,7 @@ void JointControl(joint_instance *_instance, float current)
CANTransmit(_instance->motor_can_instace);
}
void SetJointMode(joint_mode cmd, joint_instance *_instance)
void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance)
{
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
buf[7] = (uint8_t)cmd;

8
modules/motor/HT04.h
View File

@@ -24,7 +24,7 @@ typedef struct // HT04
PIDInstance pid;
CANInstance *motor_can_instace;
} joint_instance;
} HKMotor_Measure_t;
typedef enum
{
@@ -33,10 +33,10 @@ typedef enum
CMD_ZERO_POSITION = 0xfe
} joint_mode;
joint_instance *HTMotorInit(CAN_Init_Config_s config);
HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config);
void JointControl(joint_instance *_instance, float current);
void JointControl(HKMotor_Measure_t *_instance, float current);
void SetJointMode(joint_mode cmd, joint_instance *_instance);
void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance);
#endif // !HT04_H#define HT04_H

90
modules/motor/LK9025.c
View File

@@ -1,47 +1,75 @@
#include "LK9025.h"
#include "stdlib.h"
static driven_instance *driven_motor_info[LK_MOTOR_CNT];
static uint8_t idx;
static LKMotorInstance* lkmotor_instance[LK_MOTOR_MX_CNT]={NULL};
static void DecodeDriven(CANInstance *_instance)
static void LKMotorDecode(CANInstance *_instance)
{
for (size_t i = 0; i < LK_MOTOR_CNT; i++)
static LKMotor_Measure_t* measure;
static uint8_t* rx_buff;
rx_buff=_instance->rx_buff;
measure=&((LKMotorInstance*)_instance)->measure;
measure->last_ecd=measure->ecd;
measure->ecd=(uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
measure->angle_single_round=ECD_ANGLE_COEF*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->real_current=(1-CURRENT_SMOOTH_COEF)*measure->real_current+
CURRENT_SMOOTH_COEF*(float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
measure->temperate=rx_buff[1];
//计算多圈角度
if (measure->ecd - measure->last_ecd > 32678)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -32678)
measure->total_round++;
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
}
void LKMotorControl()
{
for (size_t i = 0; i < idx; i++)
{
if (driven_motor_info[i]->motor_can_instance == _instance)
{
driven_motor_info[i]->last_ecd = driven_motor_info[i]->ecd;
driven_motor_info[i]->ecd = (uint16_t)((_instance->rx_buff[7] << 8) | _instance->rx_buff[6]);
driven_motor_info[i]->speed_rpm = (uint16_t)(_instance->rx_buff[5] << 8 | _instance->rx_buff[4]);
driven_motor_info[i]->real_current = (uint16_t)(_instance->rx_buff[3] << 8 | _instance->rx_buff[2]);
driven_motor_info[i]->temperate = _instance->rx_buff[1];
break;
}
}
}
driven_instance *LKMotroInit(CAN_Init_Config_s config)
LKMotorInstance *LKMotroInit(Motor_Init_Config_s *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);
return driven_motor_info[idx++];
lkmotor_instance[idx]=(LKMotorInstance*)malloc(sizeof(LKMotorInstance));
memset(lkmotor_instance[idx],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]->current_PID,&config->controller_param_init_config.current_PID);
PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_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;
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+0x240;
lkmotor_instance[idx]->motor_can_ins=CANRegister(&config->can_init_config);
LKMotorEnable(lkmotor_instance[idx]);
return lkmotor_instance[idx++];
}
void DrivenControl(int16_t motor1_current, int16_t motor2_current)
void LKMotorStop(LKMotorInstance *motor)
{
LIMIT_MIN_MAX(motor1_current, I_MIN, I_MAX);
LIMIT_MIN_MAX(motor2_current, I_MIN, I_MAX);
driven_motor_info[0]->motor_can_instance->tx_buff[0] = motor1_current;
driven_motor_info[0]->motor_can_instance->tx_buff[1] = motor1_current >> 8;
driven_motor_info[0]->motor_can_instance->tx_buff[2] = motor2_current;
driven_motor_info[0]->motor_can_instance->tx_buff[3] = motor2_current >> 8;
CANTransmit(driven_motor_info[0]->motor_can_instance);
motor->stop_flag=MOTOR_STOP;
}
void SetDrivenMode(driven_mode cmd, uint16_t motor_id)
void LKMotorEnable(LKMotorInstance *motor)
{
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
// code goes here ...
// CANTransmit(driven_mode)
motor->stop_flag=MOTOR_ENALBED;
}
void LKMotorSetRef(LKMotorInstance *motor, float ref)
{
motor->pid_ref=ref;
}

58
modules/motor/LK9025.h
View File

@@ -6,32 +6,60 @@
#include "controller.h"
#include "motor_def.h"
#define LK_MOTOR_CNT 2
#define LK_MOTOR_MX_CNT 4
#define I_MIN -2000
#define I_MAX 2000
#define CURRENT_SMOOTH_COEF 0.9f
#define SPEED_SMOOTH_COEF 0.85f
#define REDUCTION_RATIO_DRIVEN 1
#define ECD_ANGLE_COEF (360.0f/65536.0f)
typedef struct // 9025
{
uint16_t last_ecd;
uint16_t ecd;
int16_t speed_rpm;
int16_t real_current;
uint8_t temperate;
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°
PIDInstance *pid;
CANInstance *motor_can_instance;
float total_angle; // 总角度
int32_t total_round; //总圈数
} driven_instance;
} LKMotor_Measure_t;
typedef enum
typedef struct
{
unused = 0,
} driven_mode;
LKMotor_Measure_t measure;
driven_instance *LKMotroInit(CAN_Init_Config_s config);
Motor_Control_Setting_s motor_settings;
void DrivenControl(int16_t motor1_current, int16_t motor2_current);
float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
float *other_speed_feedback_ptr;
PIDInstance current_PID;
PIDInstance speed_PID;
PIDInstance angle_PID;
float pid_ref;
Motor_Working_Type_e stop_flag; // 启停标志
CANInstance* motor_can_ins;
}LKMotorInstance;
LKMotorInstance *LKMotroInit(Motor_Init_Config_s* config);
void LKMotorSetRef(LKMotorInstance* motor,float ref);
void LKMotorControl();
void LKMotorStop(LKMotorInstance *motor);
void LKMotorEnable(LKMotorInstance *motor);
void LKMotorSetRef(LKMotorInstance *motor,float ref);
void SetDrivenMode(driven_mode cmd, uint16_t motor_id);
#endif // LK9025_H

44
modules/motor/dji_motor.c
View File

@@ -123,33 +123,26 @@ static void DecodeDJIMotor(CANInstance *_instance)
// 由于需要多次变址访存,直接将buff和measure地址保存在寄存器里避免多次存取
static uint8_t *rxbuff;
static DJI_Motor_Measure_s *measure;
rxbuff = _instance->rx_buff;
measure = &((DJIMotorInstance*)_instance->id)->motor_measure; // measure要多次使用,保存指针减小访存开销
for (size_t i = 0; i < idx; i++)
{
if (dji_motor_info[i]->motor_can_instance == _instance)
{
rxbuff = _instance->rx_buff;
measure = &dji_motor_info[i]->motor_measure; // measure要多次使用,保存指针减小访存开销
uint8_t nice;
// resolve data and apply filter to current and speed
measure->last_ecd = measure->ecd;
measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
measure->angle_single_round = ECD_ANGLE_COEF * (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->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];
uint8_t nice;
// resolve data and apply filter to current and speed
measure->last_ecd = measure->ecd;
measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
measure->angle_single_round = ECD_ANGLE_COEF * (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->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];
// multi rounds calc,计算的前提是两次采样间电机转过的角度小于180°
if (measure->ecd - measure->last_ecd > 4096)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -4096)
measure->total_round++;
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
break;
}
}
// multi rounds calc,计算的前提是两次采样间电机转过的角度小于180°
if (measure->ecd - measure->last_ecd > 4096)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -4096)
measure->total_round++;
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
}
// 电机初始化,返回一个电机实例
@@ -174,6 +167,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *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);
DJIMotorEnable(dji_motor_info[idx]);

4
modules/motor/dji_motor.h
View File

@@ -22,8 +22,8 @@
#define DJI_MOTOR_CNT 12
/* 滤波系数设置为1的时候即关闭滤波 */
#define SPEED_SMOOTH_COEF 0.9f // better to be greater than 0.85
#define CURRENT_SMOOTH_COEF 0.98f // this coef *must* be greater than 0.95
#define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.85
#define CURRENT_SMOOTH_COEF 0.9f // this coef *must* be greater than 0.9
#define ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制
/* DJI电机CAN反馈信息*/

25
modules/remote/remote_control.c
View File

@@ -9,22 +9,19 @@ static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据,[1]:上一次的数据.用于按
// 遥控器拥有的串口实例
static USARTInstance *rc_usart_instance;
/**
* @brief 矫正遥控器摇杆的值
*
*
*/
static void RectifyRCjoystick()
{
for (uint8_t i = 0; i < 5; i++)
{
if(rc_ctrl[TEMP].rc.joystick[i]>660 || rc_ctrl[TEMP].rc.joystick[i]<-660)
rc_ctrl[TEMP].rc.joystick[i]=0;
if (*(&rc_ctrl[TEMP].rc.rocker_l_+i) > 660 || *(&rc_ctrl[TEMP].rc.rocker_l_+i) < -660)
*(&rc_ctrl[TEMP].rc.rocker_l_+i) = 0;
}
}
/**
* @brief remote control protocol resolution
* @param[in] sbus_buf: raw data point
@@ -35,15 +32,15 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf)
{
memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据
// 摇杆,直接解算时减去偏置
rc_ctrl[TEMP].rc.joystick[0] = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 0
rc_ctrl[TEMP].rc.joystick[1] = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 1
rc_ctrl[TEMP].rc.joystick[2] = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 2
rc_ctrl[TEMP].rc.joystick[3] = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 3
rc_ctrl[TEMP].rc.joystick[4] = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF)- RC_CH_VALUE_OFFSET; // 左侧拨轮
rc_ctrl[TEMP].rc.rocker_r_ = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 0
rc_ctrl[TEMP].rc.rocker_r1 = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 1
rc_ctrl[TEMP].rc.rocker_l_ = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2
rc_ctrl[TEMP].rc.rocker_l1 = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 3
rc_ctrl[TEMP].rc.dial = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF) - RC_CH_VALUE_OFFSET; // 左侧拨轮
RectifyRCjoystick();
// 开关,0左1右
rc_ctrl[TEMP].rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left
rc_ctrl[TEMP].rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right
rc_ctrl[TEMP].rc.switch_right = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left
rc_ctrl[TEMP].rc.switch_left = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right
// 鼠标解析
rc_ctrl[TEMP].mouse.x = sbus_buf[6] | (sbus_buf[7] << 8); //!< Mouse X axis
@@ -98,5 +95,5 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle)
conf.usart_handle = rc_usart_handle;
conf.recv_buff_size = REMOTE_CONTROL_FRAME_SIZE;
rc_usart_instance = USARTRegister(&conf);
return (RC_ctrl_t*)&rc_ctrl;
return (RC_ctrl_t *)&rc_ctrl;
}

18
modules/remote/remote_control.h
View File

@@ -17,7 +17,7 @@
#include "main.h"
#include "usart.h"
//
//
#define LAST 1
#define TEMP 0
@@ -27,7 +27,7 @@
#define KEY_PRESS_WITH_CTRL 2
#define KEY_PRESS_WITH_SHIFT 3
// 检查接受值是否出错
// 检查接收值是否出错
#define RC_CH_VALUE_MIN ((uint16_t)364)
#define RC_CH_VALUE_OFFSET ((uint16_t)1024)
#define RC_CH_VALUE_MAX ((uint16_t)1684)
@@ -87,8 +87,14 @@ typedef struct
{
struct
{
int16_t joystick[5]; // 右|0 ,右-1 ,左-2 ,左|3 ,拨轮4
uint8_t s[2]; //[0]:left [1]:right
int16_t rocker_l_; // 左水平
int16_t rocker_l1; // 左竖直
int16_t rocker_r_; // 右水平
int16_t rocker_r1; // 右竖直
int16_t dial; // 侧边拨轮
uint8_t switch_left; // 左侧开关
uint8_t switch_right; // 右侧开关
} rc;
struct
{
@@ -101,12 +107,10 @@ typedef struct
uint16_t key_temp;
uint8_t key[4][16]; // 当前使用的键盘索引
Key_t key_test[4]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍
// Key_t key_test[4]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍
} RC_ctrl_t;
/* ------------------------- Internal Data ----------------------------------- */
/**