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

增加了LK电机和HT电机的基本支持,待编写控制

Browse Source
This commit is contained in:
NeoZng
2022-12-13 19:40:03 +08:00
parent 2f41e67de0
commit 11329aaddb
21 changed files with 280 additions and 171 deletions
Download Patch File Download Diff File Expand all files Collapse all files

119
modules/motor/HT04.c
View File

@@ -1,15 +1,31 @@
#include "HT04.h"
#include "memory.h"
#include "general_def.h"
HKMotor_Measure_t *joint_motor_info[HT_MOTOR_CNT];
static uint8_t idx;
HTMotorInstance *ht_motor_info[HT_MOTOR_CNT];
/**
* @brief
*
* @param cmd
* @param motor
*/
static void HTMotorSetMode(HTMotor_Mode_t cmd, HTMotorInstance *motor)
{
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
buf[7] = (uint8_t)cmd;
memcpy(motor->motor_can_instace->tx_buff, buf, sizeof(buf));
CANTransmit(motor->motor_can_instace);
}
/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
float span = x_max - x_min;
float offset = x_min;
return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
}
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
float span = x_max - x_min;
@@ -17,47 +33,84 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits)
return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}
static void DecodeJoint(CANInstance *motor_instance)
/**
* @brief 解析电机反馈值
*
* @param motor_can 收到
*/
static void HTMotorDecode(CANInstance *motor_can)
{
uint16_t tmp;
for (size_t i = 0; i < HT_MOTOR_CNT; i++)
static uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
static HTMotor_Measure_t *measure;
static uint8_t *rxbuff;
rxbuff = motor_can->rx_buff;
measure = &((HTMotorInstance *)motor_can->id)->motor_measure;
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_MAX, P_MIN, 16);
tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
measure->speed_aps = RAD_2_ANGLE * SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MAX, V_MIN, 12) +
(1 - SPEED_SMOOTH_COEF) * measure->speed_aps;
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MAX, T_MIN, 12) +
(1 - CURRENT_SMOOTH_COEF) * measure->real_current;
}
HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
{
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;
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);
HTMotorEnable(ht_motor_info[idx]);
return ht_motor_info[idx++];
}
void HTMotorSetRef(HTMotorInstance *motor, float ref)
{
motor->pid_ref = ref;
}
void HTMotorControl()
{
static uint16_t tmp;
for (size_t i = 0; i < idx; i++)
{
if (joint_motor_info[i]->motor_can_instace == motor_instance)
{
tmp = (motor_instance->rx_buff[1] << 8) | motor_instance->rx_buff[2];
joint_motor_info[i]->last_ecd = joint_motor_info[i]->ecd;
joint_motor_info[i]->ecd = uint_to_float(tmp, P_MAX, P_MIN, 16);
tmp = (motor_instance->rx_buff[3] << 4) | (motor_instance->rx_buff[4] >> 4);
joint_motor_info[i]->speed_rpm = uint_to_float(tmp, V_MAX, V_MIN, 12);
tmp = ((motor_instance->rx_buff[4] & 0xf) << 8) | motor_instance->rx_buff[5];
joint_motor_info[i]->real_current = uint_to_float(tmp, T_MAX, T_MIN, 12);
break;
}
// tmp=float_to_uint()
// _instance->motor_can_instace->rx_buff[6] = tmp >> 8;
// _instance->motor_can_instace->rx_buff[7] = tmp & 0xff;
// CANTransmit(_instance->motor_can_instace);
}
}
HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config)
void HTMotorStop(HTMotorInstance *motor)
{
static uint8_t idx;
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++];
HTMotorSetMode(CMD_RESET_MODE, motor);
}
void JointControl(HKMotor_Measure_t *_instance, float current)
void HTMotorEnable(HTMotorInstance *motor)
{
uint16_t tmp;
LIMIT_MIN_MAX(current, T_MIN, T_MAX);
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);
HTMotorSetMode(CMD_MOTOR_MODE, motor);
}
void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance)
void HTMotorCalibEncoder(HTMotorInstance *motor)
{
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);
HTMotorSetMode(CMD_ZERO_POSITION, motor);
}