添加均值滤波,修改HT电机控制方式,添加了一些文档
This commit is contained in:
NeoZng
parent
7403943814
commit
ff5028036a
|
|
@ -32,6 +32,7 @@
|
|||
#include "master_process.h"
|
||||
#include "daemon.h"
|
||||
#include "robot.h"
|
||||
#include "HT04.h"
|
||||
/* USER CODE END Includes */
|
||||
|
||||
/* Private typedef -----------------------------------------------------------*/
|
||||
|
|
@ -73,13 +74,13 @@ void StartROBOTTASK(void const *argument);
|
|||
void StartUITASK(void const *argument);
|
||||
/* USER CODE END FunctionPrototypes */
|
||||
|
||||
void StartDefaultTask(void const * argument);
|
||||
void StartDefaultTask(void const *argument);
|
||||
|
||||
extern void MX_USB_DEVICE_Init(void);
|
||||
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
|
||||
|
||||
/* GetIdleTaskMemory prototype (linked to static allocation support) */
|
||||
void vApplicationGetIdleTaskMemory( StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize );
|
||||
void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize);
|
||||
|
||||
/* USER CODE BEGIN GET_IDLE_TASK_MEMORY */
|
||||
static StaticTask_t xIdleTaskTCBBuffer;
|
||||
|
|
@ -95,11 +96,12 @@ void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer, StackTyp
|
|||
/* USER CODE END GET_IDLE_TASK_MEMORY */
|
||||
|
||||
/**
|
||||
* @brief FreeRTOS initialization
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
void MX_FREERTOS_Init(void) {
|
||||
* @brief FreeRTOS initialization
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
void MX_FREERTOS_Init(void)
|
||||
{
|
||||
/* USER CODE BEGIN Init */
|
||||
|
||||
/* USER CODE END Init */
|
||||
|
|
@ -127,8 +129,9 @@ void MX_FREERTOS_Init(void) {
|
|||
|
||||
/* USER CODE BEGIN RTOS_THREADS */
|
||||
/* add threads, ... */
|
||||
osThreadDef(instask, StartINSTASK, osPriorityNormal, 0, 1024);
|
||||
insTaskHandle = osThreadCreate(osThread(instask), NULL);
|
||||
osThreadDef(instask, StartINSTASK, osPriorityAboveNormal, 0, 1024);
|
||||
insTaskHandle = osThreadCreate(osThread(instask), NULL); // 由于是阻塞读取传感器,为姿态解算设置较高优先级,确保以1khz的频率执行
|
||||
// 后续修改为读取传感器数据准备好的中断处理,
|
||||
|
||||
osThreadDef(motortask, StartMOTORTASK, osPriorityNormal, 0, 256);
|
||||
motorTaskHandle = osThreadCreate(osThread(motortask), NULL);
|
||||
|
|
@ -143,7 +146,6 @@ void MX_FREERTOS_Init(void) {
|
|||
defaultTaskHandle = osThreadCreate(osThread(uitask), NULL);
|
||||
|
||||
/* USER CODE END RTOS_THREADS */
|
||||
|
||||
}
|
||||
|
||||
/* USER CODE BEGIN Header_StartDefaultTask */
|
||||
|
|
@ -153,12 +155,12 @@ void MX_FREERTOS_Init(void) {
|
|||
* @retval None
|
||||
*/
|
||||
/* USER CODE END Header_StartDefaultTask */
|
||||
void StartDefaultTask(void const * argument)
|
||||
void StartDefaultTask(void const *argument)
|
||||
{
|
||||
/* init code for USB_DEVICE */
|
||||
MX_USB_DEVICE_Init();
|
||||
/* USER CODE BEGIN StartDefaultTask */
|
||||
vTaskDelete( NULL );
|
||||
vTaskDelete(NULL);
|
||||
/* USER CODE END StartDefaultTask */
|
||||
}
|
||||
|
||||
|
|
@ -170,13 +172,15 @@ void StartINSTASK(void const *argument)
|
|||
{
|
||||
// 1kHz
|
||||
INS_Task();
|
||||
VisionSend(); // 解算完æˆ<EFBFBD>å<EFBFBD>Žå<EFBFBD>‘é€<EFBFBD>视觉数æ<EFBFBD>?
|
||||
VisionSend(); // 解算完成后发送视觉数<EFBFBD>?
|
||||
osDelay(1);
|
||||
}
|
||||
}
|
||||
|
||||
void StartMOTORTASK(void const *argument)
|
||||
{
|
||||
// 若使用HT电机则取消本行注释
|
||||
HTMotorControlInit();
|
||||
while (1)
|
||||
{
|
||||
// 500Hz
|
||||
|
|
|
|||
|
|
@ -21,11 +21,14 @@ static osMutexId DWT_MUTEX;
|
|||
/**
|
||||
* @brief 私有函数,用于检查DWT CYCCNT寄存器是否溢出,并更新CYCCNT_RountCount
|
||||
* @attention 此函数假设两次调用之间的时间间隔不超过一次溢出
|
||||
*
|
||||
* @todo 更好的方案是为dwt的时间更新单独设置一个任务
|
||||
* 不过,使用dwt的初衷是定时不被中断/任务等因素影响,因此该实现仍然有其存在的意义
|
||||
*
|
||||
*/
|
||||
static void DWT_CNT_Update(void)
|
||||
{
|
||||
if (__get_CONTROL()) // 不在中断中,使用互斥锁;在中断则直接执行即可
|
||||
if (__get_CONTROL()) // 不在中断中,使用互斥锁;在中断则直接执行即可,本框架将所有中断优先级设置为相同,故不会被其他中断重入
|
||||
if (osOK != osMutexWait(DWT_MUTEX, 0))
|
||||
return;
|
||||
|
||||
|
|
|
|||
|
|
@ -192,4 +192,18 @@ void Cross3d(float *v1, float *v2, float *res)
|
|||
float Dot3d(float *v1, float *v2)
|
||||
{
|
||||
return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
|
||||
}
|
||||
}
|
||||
|
||||
// 均值滤波,删除buffer中的最后一个元素,填入新的元素并求平均值
|
||||
float AverageFilter(float new_data, float *buf, uint8_t len)
|
||||
{
|
||||
float sum = 0;
|
||||
for (uint8_t i = 0; i < len - 1; i++)
|
||||
{
|
||||
buf[i] = buf[i + 1];
|
||||
sum += buf[i];
|
||||
}
|
||||
buf[len - 1] = new_data;
|
||||
sum += new_data;
|
||||
return sum / len;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -12,20 +12,16 @@
|
|||
*/
|
||||
#ifndef _USER_LIB_H
|
||||
#define _USER_LIB_H
|
||||
|
||||
|
||||
#include "stdint.h"
|
||||
#include "main.h"
|
||||
#include "cmsis_os.h"
|
||||
|
||||
enum
|
||||
{
|
||||
CHASSIS_DEBUG = 1,
|
||||
GIMBAL_DEBUG,
|
||||
INS_DEBUG,
|
||||
RC_DEBUG,
|
||||
IMU_HEAT_DEBUG,
|
||||
SHOOT_DEBUG,
|
||||
AIMASSIST_DEBUG,
|
||||
};
|
||||
|
||||
#define msin(x) (arm_sin_f32(x))
|
||||
#define mcos(x) (arm_cos_f32(x))
|
||||
|
||||
|
||||
extern uint8_t GlobalDebugMode;
|
||||
|
||||
|
|
@ -89,40 +85,41 @@ extern uint8_t GlobalDebugMode;
|
|||
|
||||
/**
|
||||
* @brief 返回一块干净的内<EFBFBD>?,不过仍然需要强制转<EFBFBD>?为你需要的类型
|
||||
*
|
||||
*
|
||||
* @param size 分配大小
|
||||
* @return void*
|
||||
* @return void*
|
||||
*/
|
||||
void* zero_malloc(size_t size);
|
||||
void *zero_malloc(size_t size);
|
||||
|
||||
//<EFBFBD><EFBFBD><EFBFBD>ٿ<EFBFBD><EFBFBD><EFBFBD>
|
||||
// <EFBFBD><EFBFBD><EFBFBD>ٿ<EFBFBD><EFBFBD><EFBFBD>
|
||||
float Sqrt(float x);
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
float abs_limit(float num, float Limit);
|
||||
//<EFBFBD>жϷ<EFBFBD><EFBFBD><EFBFBD>λ
|
||||
// <EFBFBD>жϷ<EFBFBD><EFBFBD><EFBFBD>λ
|
||||
float sign(float value);
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
float float_deadband(float Value, float minValue, float maxValue);
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
float float_constrain(float Value, float minValue, float maxValue);
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue);
|
||||
//ѭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// ѭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
float loop_float_constrain(float Input, float minValue, float maxValue);
|
||||
//<EFBFBD>Ƕ<EFBFBD> <20><><EFBFBD><EFBFBD> 180 ~ -180
|
||||
// <EFBFBD>Ƕ<EFBFBD> <20><><EFBFBD><EFBFBD> 180 ~ -180
|
||||
float theta_format(float Ang);
|
||||
|
||||
int float_rounding(float raw);
|
||||
|
||||
float* Norm3d(float* v);
|
||||
float *Norm3d(float *v);
|
||||
|
||||
float NormOf3d(float* v);
|
||||
float NormOf3d(float *v);
|
||||
|
||||
void Cross3d(float* v1, float* v2, float* res);
|
||||
void Cross3d(float *v1, float *v2, float *res);
|
||||
|
||||
float Dot3d(float* v1, float* v2);
|
||||
float Dot3d(float *v1, float *v2);
|
||||
|
||||
float AverageFilter(float new_data, float *buf, uint8_t len);
|
||||
|
||||
//<2F><><EFBFBD>ȸ<EFBFBD>ʽ<EFBFBD><CABD>Ϊ-PI~PI
|
||||
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -99,7 +99,7 @@ attitude_t *INS_Init(void)
|
|||
|
||||
// noise of accel is relatively big and of high freq,thus lpf is used
|
||||
INS.AccelLPF = 0.0085;
|
||||
DWT_GetDeltaT64(&INS_DWT_Count);
|
||||
DWT_GetDeltaT(&INS_DWT_Count);
|
||||
return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@ typedef struct
|
|||
uint8_t led_alpha; // 透明度,通过pwm频率改变
|
||||
uint8_t led_brightness; // 亮度,通过电压改变(如果可以,使用dac)
|
||||
uint8_t led_color; // rgb value,0-255
|
||||
uint8_t led_switch // 开关,on1 off0
|
||||
uint8_t led_switch; // 开关,on1 off0
|
||||
// void (*action_callback)(void); // led动作回调函数
|
||||
} LEDInstance;
|
||||
|
||||
|
|
|
|||
|
|
@ -134,7 +134,7 @@ static void DecodeDJIMotor(CANInstance *_instance)
|
|||
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];
|
||||
measure->temperature = rxbuff[6];
|
||||
|
||||
// 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
|
||||
if (measure->ecd - measure->last_ecd > 4096)
|
||||
|
|
@ -238,6 +238,7 @@ void DJIMotorControl()
|
|||
pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
|
||||
if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
|
||||
pid_ref *= -1; // 设置反转
|
||||
|
||||
// pid_ref会顺次通过被启用的闭环充当数据的载体
|
||||
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
|
||||
if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP)
|
||||
|
|
@ -272,6 +273,9 @@ void DJIMotorControl()
|
|||
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
|
||||
}
|
||||
|
||||
if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
|
||||
pid_ref *= -1;
|
||||
|
||||
// 获取最终输出
|
||||
set = (int16_t)pid_ref;
|
||||
|
||||
|
|
|
|||
|
|
@ -35,7 +35,7 @@ typedef struct
|
|||
float angle_single_round; // 单圈角度
|
||||
float speed_aps; // 角速度,单位为:度/秒
|
||||
int16_t real_current; // 实际电流
|
||||
uint8_t temperate; // 温度 Celsius
|
||||
uint8_t temperature; // 温度 Celsius
|
||||
|
||||
float total_angle; // 总角度,注意方向
|
||||
int32_t total_round; // 总圈数,注意方向
|
||||
|
|
@ -48,7 +48,7 @@ typedef struct
|
|||
typedef struct
|
||||
{
|
||||
|
||||
DJI_Motor_Measure_s measure; // 电机测量值
|
||||
DJI_Motor_Measure_s measure; // 电机测量值
|
||||
Motor_Control_Setting_s motor_settings; // 电机设置
|
||||
Motor_Controller_s motor_controller; // 电机控制器
|
||||
|
||||
|
|
|
|||
|
|
@ -1,10 +1,16 @@
|
|||
#include "HT04.h"
|
||||
#include "memory.h"
|
||||
#include "general_def.h"
|
||||
#include "bsp_dwt.h"
|
||||
#include "user_lib.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "string.h"
|
||||
#include "daemon.h"
|
||||
#include "stdlib.h"
|
||||
|
||||
static uint8_t idx;
|
||||
HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT];
|
||||
static HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT];
|
||||
static osThreadId ht_task_handle[HT_MOTOR_CNT];
|
||||
static uint8_t zero_buff[6] = {0};
|
||||
|
||||
/**
|
||||
* @brief 设置电机模式,报文内容[0xff,0xff,0xff,0xff,0xff,0xff,0xff,cmd]
|
||||
|
|
@ -17,9 +23,8 @@ static void HTMotorSetMode(HTMotor_Mode_t cmd, HTMotorInstance *motor)
|
|||
memset(motor->motor_can_instace->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff
|
||||
motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
|
||||
CANTransmit(motor->motor_can_instace, 1);
|
||||
memset(motor->motor_can_instace->tx_buff, 0, 6); // 发送控制指令的时候前面6bytes都是0
|
||||
memcpy(motor->motor_can_instace->tx_buff, zero_buff, 6);
|
||||
}
|
||||
|
||||
/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
|
||||
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
|
||||
{
|
||||
|
|
@ -43,28 +48,60 @@ static void HTMotorDecode(CANInstance *motor_can)
|
|||
{
|
||||
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
|
||||
uint8_t *rxbuff = motor_can->rx_buff;
|
||||
HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->measure; // 将can实例中保存的id转换成电机实例的指针
|
||||
measure->feedback_dt = DWT_GetDeltaT(&measure->count);
|
||||
HTMotorInstance *motor = (HTMotorInstance *)motor_can->id;
|
||||
HTMotor_Measure_t *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
|
||||
|
||||
DaemonReload(motor->motor_daemon);
|
||||
measure->feed_dt = DWT_GetDeltaT(&measure->feed_cnt);
|
||||
|
||||
measure->last_angle = measure->total_angle;
|
||||
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
|
||||
measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16) - measure->angle_bias;
|
||||
measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16);
|
||||
|
||||
measure->speed_rads = // SPEED_SMOOTH_COEF * measure->speed_rads + (1 - SPEED_SMOOTH_COEF) *
|
||||
(measure->total_angle - measure->last_angle) / measure->feedback_dt;
|
||||
tmp = (uint16_t)(rxbuff[3] << 4) | (rxbuff[4] >> 4);
|
||||
measure->speed_rads = AverageFilter((uint_to_float(tmp, V_MIN, V_MAX, 12) - HT_SPEED_BIAS), measure->speed_buff, SPEED_BUFFER_SIZE);
|
||||
|
||||
measure->real_current = uint_to_float(((uint16_t)(rxbuff[3] << 4) | (uint16_t)(rxbuff[4] >> 4)), V_MIN, V_MAX, 12);
|
||||
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
|
||||
measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
|
||||
(1 - CURRENT_SMOOTH_COEF) * measure->real_current;
|
||||
|
||||
|
||||
}
|
||||
|
||||
if (!measure->first_feedback_flag) // 初始化的时候设置偏置
|
||||
{
|
||||
measure->first_feedback_flag = 1;
|
||||
measure->angle_bias = measure->total_angle;
|
||||
measure->total_angle = 0;
|
||||
measure->speed_rads = 0;
|
||||
DWT_GetDeltaT(&measure->count);
|
||||
}
|
||||
static void HTMotorLostCallback(void *motor_ptr)
|
||||
{
|
||||
HTMotorInstance *motor = (HTMotorInstance *)motor_ptr;
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
return;
|
||||
if (++motor->lost_cnt % 10 != 0)
|
||||
HTMotorSetMode(CMD_MOTOR_MODE, motor); // 若不在停止模式,尝试重新让电机进入控制模式
|
||||
}
|
||||
|
||||
// measure->real_current = uint_to_float((rxbuff[3] << 8) | rxbuff[4], I_MIN, I_MAX, 16);
|
||||
/* 海泰电机一生黑,什么垃圾协议! */
|
||||
void HTMotorCalibEncoder(HTMotorInstance *motor)
|
||||
{
|
||||
uint16_t p, v, kp, kd, t;
|
||||
p = float_to_uint(0, P_MIN, P_MAX, 16);
|
||||
v = float_to_uint(0, V_MIN, V_MAX, 12);
|
||||
kp = float_to_uint(0, KP_MIN, KP_MAX, 12);
|
||||
kd = float_to_uint(0, KD_MIN, KD_MAX, 12);
|
||||
t = float_to_uint(0, T_MIN, T_MAX, 12);
|
||||
|
||||
uint8_t *buf = motor->motor_can_instace->tx_buff;
|
||||
buf[0] = p >> 8;
|
||||
buf[1] = p & 0xFF;
|
||||
buf[2] = v >> 4;
|
||||
buf[3] = ((v & 0xF) << 4) | (kp >> 8);
|
||||
buf[4] = kp & 0xFF;
|
||||
buf[5] = kd >> 4;
|
||||
buf[6] = ((kd & 0xF) << 4) | (t >> 8);
|
||||
buf[7] = t & 0xff;
|
||||
memcpy(zero_buff, buf, 6); // 初始化的时候至少调用一次,故将其他指令为0时发送的报文保存一下,详见ht04电机说明
|
||||
CANTransmit(motor->motor_can_instace, 1);
|
||||
DWT_Delay(0.005);
|
||||
HTMotorSetMode(CMD_ZERO_POSITION, motor);
|
||||
DWT_Delay(0.005);
|
||||
// HTMotorSetMode(CMD_MOTOR_MODE, motor);
|
||||
}
|
||||
|
||||
HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
|
||||
|
|
@ -83,34 +120,43 @@ HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
|
|||
config->can_init_config.id = motor;
|
||||
motor->motor_can_instace = CANRegister(&config->can_init_config);
|
||||
|
||||
Daemon_Init_Config_s conf = {
|
||||
.callback = HTMotorLostCallback,
|
||||
.owner_id = motor,
|
||||
.reload_count = 5, // 20ms
|
||||
};
|
||||
motor->motor_daemon = DaemonRegister(&conf);
|
||||
|
||||
HTMotorEnable(motor);
|
||||
HTMotorSetMode(CMD_MOTOR_MODE, motor); // 确保电机已经上电并执行电机模式
|
||||
DWT_Delay(0.05);
|
||||
HTMotorCalibEncoder(motor); // 将当前编码器位置作为零位
|
||||
DWT_Delay(0.05); // 保证下一个电机发送时CAN是空闲的,注意应用在初始化模块的时候不应该进入中断
|
||||
ht_motor_instance[idx++] = motor;
|
||||
return motor;
|
||||
}
|
||||
|
||||
void HTMotorSetRef(HTMotorInstance *motor, float ref)
|
||||
{
|
||||
motor->pid_ref = ref * 0.285f;
|
||||
motor->pid_ref = ref;
|
||||
}
|
||||
|
||||
void HTMotorControl()
|
||||
/**
|
||||
* @brief 为了避免总线堵塞,为每个电机创建一个发送任务
|
||||
* @param argument 传入的电机指针
|
||||
*/
|
||||
void HTMotorTask(void const *argument)
|
||||
{
|
||||
float set, pid_measure, pid_ref;
|
||||
HTMotorInstance *motor = (HTMotorInstance *)argument;
|
||||
HTMotor_Measure_t *measure = &motor->measure;
|
||||
Motor_Control_Setting_s *setting = &motor->motor_settings;
|
||||
CANInstance *motor_can = motor->motor_can_instace;
|
||||
uint16_t tmp;
|
||||
HTMotorInstance *motor;
|
||||
HTMotor_Measure_t *measure;
|
||||
Motor_Control_Setting_s *setting;
|
||||
CANInstance *motor_can;
|
||||
|
||||
// 遍历所有电机实例,计算PID
|
||||
for (size_t i = 0; i < idx; i++)
|
||||
{ // 先获取地址避免反复寻址
|
||||
motor = ht_motor_instance[i];
|
||||
measure = &motor->measure;
|
||||
setting = &motor->motor_settings;
|
||||
motor_can = motor->motor_can_instace;
|
||||
while (1)
|
||||
{
|
||||
pid_ref = motor->pid_ref;
|
||||
|
||||
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
|
||||
{
|
||||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
|
|
@ -145,36 +191,47 @@ void HTMotorControl()
|
|||
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
|
||||
set *= -1;
|
||||
|
||||
LIMIT_MIN_MAX(set, T_MIN, T_MAX); // 限幅,实际上这似乎和pid输出限幅重复了
|
||||
tmp = float_to_uint(set, T_MIN, T_MAX, 12); // 数值最后在 -12~+12之间
|
||||
LIMIT_MIN_MAX(set, T_MIN, T_MAX);
|
||||
tmp = float_to_uint(set, T_MIN, T_MAX, 12);
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
tmp = float_to_uint(0, T_MIN, T_MAX, 12);
|
||||
motor_can->tx_buff[6] = (tmp >> 8);
|
||||
motor_can->tx_buff[7] = tmp & 0xff;
|
||||
|
||||
CANTransmit(motor_can, 1);
|
||||
CANTransmit(motor_can, 0.5);
|
||||
|
||||
osDelay(1);
|
||||
}
|
||||
}
|
||||
|
||||
void HTMotorControlInit()
|
||||
{
|
||||
char ht_task_name[5] = "ht";
|
||||
// 遍历所有电机实例,创建任务
|
||||
if (!idx)
|
||||
return;
|
||||
for (size_t i = 0; i < idx; i++)
|
||||
{
|
||||
char ht_id_buff[2] = {0};
|
||||
__itoa(i, ht_id_buff, 10);
|
||||
strcat(ht_task_name, ht_id_buff); // 似乎没什么吊用,osthreaddef会把第一个变量当作宏字符串传入,作为任务名
|
||||
// todo 还需要一个更优雅的方案来区分不同的电机任务
|
||||
osThreadDef(ht_task_name, HTMotorTask, osPriorityNormal, 0, 128);
|
||||
ht_task_handle[i] = osThreadCreate(osThread(ht_task_name), ht_motor_instance[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void HTMotorStop(HTMotorInstance *motor)
|
||||
{
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
return;
|
||||
HTMotorSetMode(CMD_RESET_MODE, motor);
|
||||
HTMotorSetMode(CMD_RESET_MODE, motor); // 发两次,确保电机停止
|
||||
motor->stop_flag = MOTOR_STOP;
|
||||
}
|
||||
|
||||
void HTMotorEnable(HTMotorInstance *motor)
|
||||
{
|
||||
if (motor->stop_flag == MOTOR_ENALBED)
|
||||
return;
|
||||
HTMotorSetMode(CMD_MOTOR_MODE, motor);
|
||||
HTMotorSetMode(CMD_MOTOR_MODE, motor);
|
||||
motor->stop_flag = MOTOR_ENALBED;
|
||||
}
|
||||
|
||||
// void HTMotorCalibEncoder(HTMotorInstance *motor)
|
||||
// {
|
||||
// HTMotorSetMode(CMD_ZERO_POSITION, motor);
|
||||
// }
|
||||
void HTMotorOuterLoop(HTMotorInstance *motor, Closeloop_Type_e type)
|
||||
{
|
||||
motor->motor_settings.outer_loop_type = type;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,10 +5,12 @@
|
|||
#include "bsp_can.h"
|
||||
#include "controller.h"
|
||||
#include "motor_def.h"
|
||||
#include "daemon.h"
|
||||
|
||||
#define HT_MOTOR_CNT 4
|
||||
#define CURRENT_SMOOTH_COEF 0.9f
|
||||
#define SPEED_SMOOTH_COEF 0.85f
|
||||
#define SPEED_BUFFER_SIZE 5
|
||||
#define HT_SPEED_BIAS -0.0109901428f // 电机速度偏差,单位rad/s
|
||||
|
||||
#define P_MIN -95.5f // Radians
|
||||
#define P_MAX 95.5f
|
||||
|
|
@ -16,18 +18,23 @@
|
|||
#define V_MAX 45.0f
|
||||
#define T_MIN -18.0f // N·m
|
||||
#define T_MAX 18.0f
|
||||
#define KP_MIN 0.0f // N-m/rad
|
||||
#define KP_MAX 500.0f
|
||||
#define KD_MIN 0.0f // N-m/rad/s
|
||||
#define KD_MAX 5.0f
|
||||
|
||||
typedef struct // HT04
|
||||
{
|
||||
float total_angle; // 角度为多圈角度,范围是-95.5~95.5,单位为rad
|
||||
float last_angle;
|
||||
float speed_rads;
|
||||
float real_current;
|
||||
float angle_bias;
|
||||
uint8_t first_feedback_flag;
|
||||
|
||||
float feedback_dt;
|
||||
uint32_t count;
|
||||
float speed_rads;
|
||||
float speed_buff[SPEED_BUFFER_SIZE];
|
||||
|
||||
float real_current;
|
||||
|
||||
float feed_dt;
|
||||
uint32_t feed_cnt;
|
||||
} HTMotor_Measure_t;
|
||||
|
||||
/* HT电机类型定义*/
|
||||
|
|
@ -49,6 +56,9 @@ typedef struct
|
|||
Motor_Working_Type_e stop_flag; // 启停标志
|
||||
|
||||
CANInstance *motor_can_instace;
|
||||
|
||||
DaemonInstance *motor_daemon;
|
||||
uint32_t lost_cnt;
|
||||
} HTMotorInstance;
|
||||
|
||||
/* HT电机模式,初始化时自动进入CMD_MOTOR_MODE*/
|
||||
|
|
@ -76,10 +86,10 @@ HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config);
|
|||
void HTMotorSetRef(HTMotorInstance *motor, float ref);
|
||||
|
||||
/**
|
||||
* @brief 给所有的HT电机发送控制指令
|
||||
* @brief 初始化电机任务,若要使用,需要在motortask的死循环前调用
|
||||
*
|
||||
*/
|
||||
void HTMotorControl();
|
||||
void HTMotorControlInit();
|
||||
|
||||
/**
|
||||
* @brief 停止电机,之后电机不会响应HTMotorSetRef设定的值
|
||||
|
|
@ -96,7 +106,15 @@ void HTMotorStop(HTMotorInstance *motor);
|
|||
void HTMotorEnable(HTMotorInstance *motor);
|
||||
|
||||
/**
|
||||
* @brief 校准电机编码器,设置的当前位置为0
|
||||
* @brief 修改电机闭环对象
|
||||
*
|
||||
* @param motor
|
||||
* @param type
|
||||
*/
|
||||
void HTMotorOuterLoop(HTMotorInstance *motor, Closeloop_Type_e type);
|
||||
|
||||
/**
|
||||
* @brief 将当前编码器位置置零
|
||||
*
|
||||
* @param motor
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -9,8 +9,34 @@
|
|||
|
||||
请注意发送和反馈数据的**单位**。
|
||||
|
||||
> ~~HT的电机协议做的真不行，纯纯直接抄mit还抄不明白，之后全部换成LK！~~
|
||||
|
||||
另一种上电时校准编码器的方法:
|
||||
|
||||
```c
|
||||
static void HTMotorDecode(CANInstance *motor_can)
|
||||
{
|
||||
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
|
||||
uint8_t *rxbuff = motor_can->rx_buff;
|
||||
HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->measure; // 将can实例中保存的id转换成电机实例的指针
|
||||
|
||||
measure->last_angle = measure->total_angle;
|
||||
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
|
||||
measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16) - measure->angle_bias;
|
||||
|
||||
> ~~HT的电机协议做的真不行，纯纯直接抄mit还抄不明白，之后全部换成LK！~~
|
||||
tmp = (uint16_t)(rxbuff[3] << 4) | (rxbuff[4] >> 4);
|
||||
measure->speed_rads = AverageFilter((uint_to_float(tmp, V_MIN, V_MAX, 12) - HT_SPEED_BIAS), measure->speed_buff, SPEED_BUFFER_SIZE);
|
||||
|
||||
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
|
||||
measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
|
||||
(1 - CURRENT_SMOOTH_COEF) * measure->real_current;
|
||||
|
||||
if (!measure->first_feedback_flag) // 初始化的时候设置偏置
|
||||
{
|
||||
measure->first_feedback_flag = 1;
|
||||
measure->angle_bias = measure->total_angle;
|
||||
measure->total_angle = 0;
|
||||
measure->speed_rads = 0;
|
||||
}
|
||||
}
|
||||
```
|
||||
|
|
|
|||
|
|
@ -1,6 +1,8 @@
|
|||
#include "LK9025.h"
|
||||
#include "stdlib.h"
|
||||
#include "general_def.h"
|
||||
#include "daemon.h"
|
||||
#include "bsp_dwt.h"
|
||||
|
||||
static uint8_t idx;
|
||||
static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
|
||||
|
|
@ -14,10 +16,12 @@ static CANInstance *sender_instance; // 多电机发送时使用的caninstance(
|
|||
*/
|
||||
static void LKMotorDecode(CANInstance *_instance)
|
||||
{
|
||||
LKMotor_Measure_t *measure;
|
||||
uint8_t *rx_buff;
|
||||
rx_buff = _instance->rx_buff;
|
||||
measure = &(((LKMotorInstance *)_instance->id)->measure); // 通过caninstance保存的id获取对应的motorinstance
|
||||
LKMotorInstance *motor = (LKMotorInstance *)_instance->id; // 通过caninstance保存的father id获取对应的motorinstance
|
||||
LKMotor_Measure_t *measure = &motor->measure;
|
||||
uint8_t *rx_buff = _instance->rx_buff;
|
||||
|
||||
DaemonReload(motor->daemon); // 喂狗
|
||||
measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt);
|
||||
|
||||
measure->last_ecd = measure->ecd;
|
||||
measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
|
||||
|
|
@ -30,12 +34,11 @@ static void LKMotorDecode(CANInstance *_instance)
|
|||
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];
|
||||
measure->temperature = rx_buff[1];
|
||||
|
||||
// 计算多圈角度
|
||||
if (measure->ecd - measure->last_ecd > 32678)
|
||||
if (measure->ecd - measure->last_ecd > 32768)
|
||||
measure->total_round--;
|
||||
else if (measure->ecd - measure->last_ecd < -32678)
|
||||
else if (measure->ecd - measure->last_ecd < -32768)
|
||||
measure->total_round++;
|
||||
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
|
||||
}
|
||||
|
|
@ -66,7 +69,16 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
|
|||
}
|
||||
|
||||
LKMotorEnable(motor);
|
||||
DWT_GetDeltaT(&motor->measure.feed_dwt_cnt);
|
||||
lkmotor_instance[idx++] = motor;
|
||||
|
||||
Daemon_Init_Config_s daemon_config = {
|
||||
.callback = NULL,
|
||||
.owner_id = motor,
|
||||
.reload_count = 5, // 0.05秒
|
||||
};
|
||||
motor->daemon = DaemonRegister(&daemon_config);
|
||||
|
||||
return motor;
|
||||
}
|
||||
|
||||
|
|
@ -91,7 +103,7 @@ void LKMotorControl()
|
|||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor->other_angle_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->total_angle;
|
||||
pid_measure = measure->real_current;
|
||||
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
|
||||
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
|
||||
pid_ref += *motor->speed_feedforward_ptr;
|
||||
|
|
@ -104,10 +116,10 @@ void LKMotorControl()
|
|||
else
|
||||
pid_measure = measure->speed_rads;
|
||||
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
|
||||
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
|
||||
pid_ref += *motor->current_feedforward_ptr;
|
||||
}
|
||||
|
||||
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
|
||||
pid_ref += *motor->current_feedforward_ptr;
|
||||
if (setting->close_loop_type & CURRENT_LOOP)
|
||||
{
|
||||
pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
|
||||
|
|
@ -126,9 +138,7 @@ void LKMotorControl()
|
|||
}
|
||||
|
||||
if (idx) // 如果有电机注册了
|
||||
{
|
||||
CANTransmit(sender_instance, 1);
|
||||
}
|
||||
CANTransmit(sender_instance, 0.2);
|
||||
}
|
||||
|
||||
void LKMotorStop(LKMotorInstance *motor)
|
||||
|
|
@ -145,3 +155,8 @@ void LKMotorSetRef(LKMotorInstance *motor, float ref)
|
|||
{
|
||||
motor->pid_ref = ref;
|
||||
}
|
||||
|
||||
uint8_t LKMotorIsOnline(LKMotorInstance *motor)
|
||||
{
|
||||
return DaemonIsOnline(motor->daemon);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,6 +5,7 @@
|
|||
#include "bsp_can.h"
|
||||
#include "controller.h"
|
||||
#include "motor_def.h"
|
||||
#include "daemon.h"
|
||||
|
||||
#define LK_MOTOR_MX_CNT 4 // 最多允许4个LK电机使用多电机指令,挂载在一条总线上
|
||||
|
||||
|
|
@ -23,11 +24,13 @@ typedef struct // 9025
|
|||
float angle_single_round; // 单圈角度
|
||||
float speed_rads; // speed rad/s
|
||||
int16_t real_current; // 实际电流
|
||||
uint8_t temperate; // 温度,C°
|
||||
uint8_t temperature; // 温度,C°
|
||||
|
||||
float total_angle; // 总角度
|
||||
int32_t total_round; // 总圈数
|
||||
|
||||
float feed_dt;
|
||||
uint32_t feed_dwt_cnt;
|
||||
} LKMotor_Measure_t;
|
||||
|
||||
typedef struct
|
||||
|
|
@ -49,6 +52,8 @@ typedef struct
|
|||
|
||||
CANInstance *motor_can_ins;
|
||||
|
||||
DaemonInstance *daemon;
|
||||
|
||||
} LKMotorInstance;
|
||||
|
||||
/**
|
||||
|
|
@ -88,4 +93,6 @@ void LKMotorStop(LKMotorInstance *motor);
|
|||
*/
|
||||
void LKMotorEnable(LKMotorInstance *motor);
|
||||
|
||||
uint8_t LKMotorIsOnline(LKMotorInstance *motor);
|
||||
|
||||
#endif // LK9025_H
|
||||
|
|
|
|||
|
|
@ -5,10 +5,9 @@
|
|||
#include "step_motor.h"
|
||||
#include "servo_motor.h"
|
||||
|
||||
|
||||
void MotorControlTask()
|
||||
{
|
||||
// static uint8_t cnt = 0; 设定任务频率
|
||||
// static uint8_t cnt = 0; 设定不同电机的任务频率
|
||||
// if(cnt%5==0) //200hz
|
||||
// if(cnt%10==0) //100hz
|
||||
DJIMotorControl();
|
||||
|
|
@ -16,10 +15,13 @@ void MotorControlTask()
|
|||
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
|
||||
LKMotorControl();
|
||||
|
||||
HTMotorControl();
|
||||
// legacy support
|
||||
// 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
|
||||
// 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换
|
||||
// HTMotorControl();
|
||||
// 将所有的CAN设备集中在一处发送,最高反馈频率仅能达到500Hz,为了更好的控制效果,应使用新的HTMotorControlInit()接口
|
||||
|
||||
ServeoMotorControl();
|
||||
|
||||
//StepMotorControl();
|
||||
// StepMotorControl();
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue