HFUT_Firmament_EC
/
sentry_gimbal_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

增加了电机前馈控制,更新readme，加快ins速度，删除了工程和平衡底盘相关的app

This commit is contained in:
NeoZng 2023-05-19 14:45:48 +08:00
parent ecb56ef935
commit a18a5091f4
27 changed files with 97 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.vscode/tasks.json vendored
View File

@ -5,7 +5,7 @@
{
"label": "build task", // 任务标签
"type": "shell", // 任务类型,因为要调用mingw32-make,是在终端(CMD)里运行的,所以是shell任务
"command": "mingw32-make -j24 -l12",// 任务命令,线程数可以根据自己的电脑修改,建议为核数的4~8倍
"command": "mingw32-make -j24 -l12",// 任务命令,线程数可以根据自己的电脑修改,建议与cpu核数相同
"problemMatcher": [],
"group": {
"kind": "build",

2
Makefile
View File

@ -156,7 +156,6 @@ application/gimbal/gimbal.c \
application/chassis/chassis.c \
application/shoot/shoot.c \
application/cmd/robot_cmd.c \
application/balance_chassis/balance.c \
application/robot.c
# ASM sources
@ -235,7 +234,6 @@ C_INCLUDES = \
-Iapplication/shoot \
-Iapplication/gimbal \
-Iapplication/cmd \
-Iapplication/balance_chassis \
-Iapplication \
-Ibsp/dwt \
-Ibsp/can \

2
Src/freertos.c
View File

@ -170,7 +170,7 @@ void StartINSTASK(void const *argument)
{
// 1kHz
INS_Task();
VisionSend(); // 解算完成后发送视觉数据
VisionSend(); // 解算完æˆ<EFBFBD>å<EFBFBD>Žå<EFBFBD>‘é€<EFBFBD>视觉数æ<EFBFBD>?
osDelay(1);
}
}

2
Src/spi.c
View File

@ -49,7 +49,7 @@ void MX_SPI1_Init(void)
hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
hspi1.Init.NSS = SPI_NSS_SOFT;
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256;
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;

1
application/grab/grab.md
View File

@ -1 +0,0 @@
工程机器人夹爪

3
application/lift/lift.md
View File

@ -1,3 +0,0 @@
抬升/横移机构
气缸和电磁阀似乎没有必要构建模块,但是也可以组合微动开关和编码器+继电器等传感器构成模块,降低lift的复杂度,完成解耦.

1
application/manipulator/manipulator.md
View File

@ -1 +0,0 @@
机械臂(不包含末端执行机构)

13
application/robot_def.h
View File

@ -25,16 +25,6 @@
#define VISION_USE_VCP // 使用虚拟串口发送视觉数据
// #define VISION_USE_UART // 使用串口发送视觉数据
// @todo: 增加机器人类型定义,后续是否要兼容所有机器人?(只兼容步兵英雄哨兵似乎就够了)
// 通过该宏,你可以直接将所有机器人的参数保存在一处,然后每次只需要修改这个宏就可以替换所有参数
/* 机器人类型定义 */
// #define ROBOT_HERO 1 // 英雄机器人
// #define ROBOT_ENINEER 2 // 工程机器人
#define ROBOT_INFANTRY 3 // 步兵机器人3
// #define ROBOT_INFANTRY 4 // 步兵机器人4
// #define ROBOT_INFANTRY 5 // 步兵机器人5
// #define ROBOT_SENTRY 6 // 哨兵机器人
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
// 云台参数
#define YAW_CHASSIS_ALIGN_ECD 2711 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
@ -188,7 +178,6 @@ typedef struct
*
*/
/* @todo : 对于平衡底盘,需要新增控制模式和控制数据 */
typedef struct
{
#if defined(CHASSIS_BOARD) || defined(GIMBAL_BOARD) // 非单板的时候底盘还将imu数据回传(若有必要)
@ -205,7 +194,7 @@ typedef struct
} Chassis_Upload_Data_s;
/* @todo : 对于平衡底盘,需要不同的反馈数据 */
typedef struct
{
attitude_t gimbal_imu_data;

6
basic_framework.ioc
View File

@ -520,7 +520,7 @@ ProjectManager.StackSize=0x4000
ProjectManager.TargetToolchain=Makefile
ProjectManager.ToolChainLocation=
ProjectManager.UnderRoot=false
ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_CAN1_Init-CAN1-false-HAL-true,6-MX_CAN2_Init-CAN2-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true,8-MX_TIM4_Init-TIM4-false-HAL-true,9-MX_TIM5_Init-TIM5-false-HAL-true,10-MX_USART3_UART_Init-USART3-false-HAL-true,11-MX_RNG_Init-RNG-false-HAL-true,12-MX_RTC_Init-RTC-false-HAL-true,13-MX_TIM1_Init-TIM1-false-HAL-true,14-MX_TIM10_Init-TIM10-false-HAL-true,15-MX_USART1_UART_Init-USART1-false-HAL-true,16-MX_USART6_UART_Init-USART6-false-HAL-true,17-MX_TIM8_Init-TIM8-false-HAL-true,18-MX_I2C2_Init-I2C2-false-HAL-true,19-MX_I2C3_Init-I2C3-false-HAL-true,20-MX_SPI2_Init-SPI2-false-HAL-true,21-MX_CRC_Init-CRC-false-HAL-true,22-MX_DAC_Init-DAC-false-HAL-true,23-MX_USB_OTG_FS_USB_Init-USB_OTG_FS-false-HAL-true
ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_CAN1_Init-CAN1-false-HAL-true,6-MX_CAN2_Init-CAN2-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true,8-MX_TIM4_Init-TIM4-false-HAL-true,9-MX_TIM5_Init-TIM5-false-HAL-true,10-MX_USART3_UART_Init-USART3-false-HAL-true,11-MX_RNG_Init-RNG-false-HAL-true,12-MX_RTC_Init-RTC-false-HAL-true,13-MX_TIM1_Init-TIM1-false-HAL-true,14-MX_TIM10_Init-TIM10-false-HAL-true,15-MX_USART1_UART_Init-USART1-false-HAL-true,16-MX_USART6_UART_Init-USART6-false-HAL-true,17-MX_TIM8_Init-TIM8-false-HAL-true,18-MX_I2C2_Init-I2C2-false-HAL-true,19-MX_I2C3_Init-I2C3-false-HAL-true,20-MX_SPI2_Init-SPI2-false-HAL-true,21-MX_CRC_Init-CRC-false-HAL-true,22-MX_DAC_Init-DAC-false-HAL-true,23-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-false
RCC.48MHZClocksFreq_Value=48000000
RCC.AHBFreq_Value=168000000
RCC.APB1CLKDivider=RCC_HCLK_DIV4
@ -591,10 +591,10 @@ SH.S_TIM8_CH2.0=TIM8_CH2,Output Compare2 CH2
SH.S_TIM8_CH2.ConfNb=1
SH.S_TIM8_CH3.0=TIM8_CH3,Output Compare3 CH3
SH.S_TIM8_CH3.ConfNb=1
SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_256
SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_8
SPI1.CLKPhase=SPI_PHASE_2EDGE
SPI1.CLKPolarity=SPI_POLARITY_HIGH
SPI1.CalculateBaudRate=328.125 KBits/s
SPI1.CalculateBaudRate=10.5 MBits/s
SPI1.Direction=SPI_DIRECTION_2LINES
SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler,CLKPolarity,CLKPhase
SPI1.Mode=SPI_MODE_MASTER

1
bsp/bsp_init.c
View File

@ -11,7 +11,6 @@ void BSPInit()
{
DWT_Init(168);
BSPLogInit();
// USBInit(); // 务必在进入操作系统之前执行USBInit
// legacy support，待删除,将在实现了led/tempctrl/buzzer的module之后移动到app层进行XXXRegister()

43
bsp/can/bsp_can.c
View File

@ -3,6 +3,7 @@
#include "memory.h"
#include "stdlib.h"
#include "bsp_dwt.h"
#include "bsp_log.h"
/* can instance ptrs storage, used for recv callback */
// 在CAN产生接收中断会遍历数组,选出hcan和rxid与发生中断的实例相同的那个,调用其回调函数
@ -30,14 +31,14 @@ static void CANAddFilter(CANInstance *_instance)
CAN_FilterTypeDef can_filter_conf;
static uint8_t can1_filter_idx = 0, can2_filter_idx = 14; // 0-13给can1用,14-27给can2用
can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST; //使用id list模式,即只有将rxid添加到过滤器中才会接收到,其他报文会被过滤
can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT; //使用16位id模式,即只有低16位有效
can_filter_conf.FilterFIFOAssignment = (_instance->tx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1; //奇数id的模块会被分配到FIFO0,偶数id的模块会被分配到FIFO1
can_filter_conf.SlaveStartFilterBank = 14; // 从第14个过滤器开始配置从机过滤器(在STM32的BxCAN控制器中CAN2是CAN1的从机)
can_filter_conf.FilterIdLow = _instance->rx_id << 5; // 过滤器寄存器的低16位,因为使用STDID,所以只有低11位有效,高5位要填0
can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST; // 使用id list模式,即只有将rxid添加到过滤器中才会接收到,其他报文会被过滤
can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT; // 使用16位id模式,即只有低16位有效
can_filter_conf.FilterFIFOAssignment = (_instance->tx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1; // 奇数id的模块会被分配到FIFO0,偶数id的模块会被分配到FIFO1
can_filter_conf.SlaveStartFilterBank = 14; // 从第14个过滤器开始配置从机过滤器(在STM32的BxCAN控制器中CAN2是CAN1的从机)
can_filter_conf.FilterIdLow = _instance->rx_id << 5; // 过滤器寄存器的低16位,因为使用STDID,所以只有低11位有效,高5位要填0
can_filter_conf.FilterBank = _instance->can_handle == &hcan1 ? (can1_filter_idx++) : (can2_filter_idx++); // 根据can_handle判断是CAN1还是CAN2,然后自增
can_filter_conf.FilterActivation = CAN_FILTER_ENABLE; // 启用过滤器
can_filter_conf.FilterActivation = CAN_FILTER_ENABLE; // 启用过滤器
HAL_CAN_ConfigFilter(_instance->can_handle, &can_filter_conf);
}
@ -69,12 +70,12 @@ CANInstance *CANRegister(CAN_Init_Config_s *config)
while (1)
;
for (size_t i = 0; i < idx; i++)
{ // 重复注册 | id重复
if (can_instance[i]->rx_id == config->rx_id && can_instance[i]->can_handle == config->can_handle)
{ // 重复注册 | id重复
if (can_instance[i]->rx_id == config->rx_id && can_instance[i]->can_handle == config->can_handle)
while (1)
;
}
CANInstance *instance = (CANInstance *)malloc(sizeof(CANInstance)); // 分配空间
memset(instance, 0, sizeof(CANInstance)); // 分配的空间未必是0,所以要先清空
// 进行发送报文的配置
@ -97,18 +98,28 @@ CANInstance *CANRegister(CAN_Init_Config_s *config)
/* @todo 目前似乎封装过度,应该添加一个指向tx_buff的指针,tx_buff不应该由CAN instance保存 */
/* 如果让CANinstance保存txbuff,会增加一次复制的开销 */
uint8_t CANTransmit(CANInstance *_instance,uint8_t timeout)
uint8_t CANTransmit(CANInstance *_instance, float timeout)
{
static uint32_t busy_count;
static float wait_time;
float dwt_start = DWT_GetTimeline_ms();
while (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0) // 等待邮箱空闲
{
{
if (DWT_GetTimeline_ms() - dwt_start > timeout) // 超时
{
LOGWARNING("CAN BUSY sending! cnt:%d", busy_count);
busy_count++;
return 0;
}
}
wait_time = DWT_GetTimeline_ms() - dwt_start;
// tx_mailbox会保存实际填入了这一帧消息的邮箱,但是知道是哪个邮箱发的似乎也没啥用
HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox);
if (HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox))
{
LOGWARNING("CAN BUSY bus! cnt:%d", busy_count);
busy_count++;
return 0;
}
return 1; // 发送成功
}
@ -132,8 +143,8 @@ void CANSetDLC(CANInstance *_instance, uint8_t length)
static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox)
{
static CAN_RxHeaderTypeDef rxconf; // 同上
uint8_t can_rx_buff[8];
uint8_t can_rx_buff[8];
HAL_CAN_GetRxMessage(_hcan, fifox, &rxconf, can_rx_buff); // 从FIFO中获取数据
for (size_t i = 0; i < idx; ++i)
{ // 两者相等说明这是要找的实例
@ -177,6 +188,4 @@ void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan)
CANFIFOxCallback(hcan, CAN_RX_FIFO1); // 调用我们自己写的函数来处理消息
}
// 人工智能会毁灭人类吗？请在补全回答

2
bsp/can/bsp_can.h
View File

@ -63,6 +63,6 @@ void CANSetDLC(CANInstance *_instance, uint8_t length);
* @param timeout 超时时间,单位为ms;后续改为us,获得更精确的控制
* @param _instance* can instance owned by module
*/
uint8_t CANTransmit(CANInstance *_instance,uint8_t timeout);
uint8_t CANTransmit(CANInstance *_instance,float timeout);
#endif

1
bsp/usb/bsp_usb.h
View File

@ -2,7 +2,6 @@
* @file bsp_usb.h
* @author your name (you@domain.com)
* @brief 提供对usb vpc(virtal com port)的操作接口,hid和msf考虑后续添加
* @todo 与usart的接口统一完成兼容
* @attention 这一版usb修改了usbd_cdc_if.c中的CDC_Receive_FS函数,若使用cube生成后会被覆盖.后续需要由usbcdciftemplate创建一套自己的模板
* @version 0.1
* @date 2023-02-09

2
modules/BMI088/bmi088.c
View File

@ -227,7 +227,7 @@ static void BMI088GyroINTCallback(GPIOInstance *gpio)
/**
* @brief
* @todo 现在要考虑一下数据返回的方式,指针还是结构体? 7个float数据有点费时,不然用DMA? or memcpy
* @todo 现在要考虑一下数据返回的方式,指针还是结构体?
*
* @param bmi088
* @return BMI088_Data_t

1
modules/BMI088/bmi088.h
View File

@ -100,7 +100,6 @@ BMI088Instance *BMI088Register(BMI088_Init_Config_s *config);
/**
* @brief 读取BMI088数据
* @todo 7个float数据开销较大,后续考虑使用DMA或双缓冲区直接传递指针
* @param bmi088 BMI088实例指针
* @return BMI088_Data_t 读取到的数据
*/

5
modules/algorithm/algorithm.md
View File

@ -3,11 +3,8 @@
<p align='right'>neozng1@hnu.edu.cn</p>
> TODO:
>
> 1. 实现麦轮和全向轮的速度解算
> 2. 实现LQR
> 3. 实现一些通用的滤波器，如指数平均，窗平均，低通等
> 4. 实现系统辨识
> 2. 实现一些通用的滤波器，如指数平均，窗平均，低通等
## 总览和使用

1
modules/algorithm/controller.c
View File

@ -134,6 +134,7 @@ void PIDInit(PIDInstance *pid, PID_Init_Config_s *config)
// utilize the quality of struct that its memeory is continuous
memcpy(pid, config, sizeof(PID_Init_Config_s));
// set rest of memory to 0
DWT_GetDeltaT(&pid->DWT_CNT);
}
/**

2
modules/daemon/daemon.c
View File

@ -13,7 +13,7 @@ DaemonInstance *DaemonRegister(Daemon_Init_Config_s *config)
memset(instance, 0, sizeof(DaemonInstance));
instance->owner_id = config->owner_id;
instance->reload_count = config->reload_count;
instance->reload_count = config->reload_count == 0 ? 100 : config->reload_count;
instance->callback = config->callback;
daemon_instances[idx++] = instance;

1
modules/master_machine/master_process.md
View File

@ -94,7 +94,6 @@ typedef struct
float yaw;
float pitch;
float roll;
// uint32_t time_stamp; // @todo 用于和相机的时间戳对齐
} Vision_Send_s;
```

1
modules/message_center/message_center.h
View File

@ -2,7 +2,6 @@
* @file message_center.h
* @author NeoZeng neozng1@hnu.edu.cn
* @brief 这是一个伪pubsub机制,仅对应用之间的通信进行了隔离
* @todo 实现基于链表-队列的pubsub机制
* @version 0.1
* @date 2022-11-30
*

7
modules/motor/DJImotor/dji_motor.c
View File

@ -160,6 +160,8 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
PIDInit(&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;
instance->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr;
instance->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr;
// 后续增加电机前馈控制器(速度和电流)
// 电机分组,因为至多4个电机可以共用一帧CAN控制报文
@ -251,6 +253,9 @@ void DJIMotorControl()
// 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
{
if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor_controller->speed_feedforward_ptr;
if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_speed_feedback_ptr;
else // MOTOR_FEED
@ -260,6 +265,8 @@ void DJIMotorControl()
}
// 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor_controller->current_feedforward_ptr;
if (motor_setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);

4
modules/motor/DJImotor/dji_motor.h
View File

@ -7,7 +7,6 @@
*
* @todo 1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏
2. 为M2006和M3508增加开环的零位校准函数,并在初始化时调用(根据用户配置决定是否调用)
3. 完成前馈功能（已经添加了前馈数据指针）
* @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
*
@ -34,7 +33,7 @@ typedef struct
uint16_t last_ecd; // 上一次读取的编码器值
uint16_t ecd; // 0-8191,刻度总共有8192格
float angle_single_round; // 单圈角度
float speed_aps; // 角速度,单位为:度/秒 rpm:rounds per minute
float speed_aps; // 角速度,单位为:度/秒
int16_t real_current; // 实际电流
uint8_t temperate; // 温度 Celsius
@ -100,7 +99,6 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback
/**
* @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
* @todo 增加前馈功能
*/
void DJIMotorControl();

54
modules/motor/HTmotor/HT04.c
View File

@ -1,6 +1,7 @@
#include "HT04.h"
#include "memory.h"
#include "general_def.h"
#include "bsp_dwt.h"
static uint8_t idx;
HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT];
@ -43,19 +44,27 @@ 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);
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->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16) - measure->angle_bias;
tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
measure->speed_rads = SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) +
(1 - SPEED_SMOOTH_COEF) * measure->speed_rads;
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[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;
measure->real_current = uint_to_float(((uint16_t)(rxbuff[3] << 4) | (uint16_t)(rxbuff[4] >> 4)), V_MIN, V_MAX, 12);
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);
}
// measure->real_current = uint_to_float((rxbuff[3] << 8) | rxbuff[4], I_MIN, I_MAX, 16);
}
HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
@ -81,7 +90,7 @@ HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
void HTMotorSetRef(HTMotorInstance *motor, float ref)
{
motor->pid_ref = ref;
motor->pid_ref = ref * 0.285f;
}
void HTMotorControl()
@ -107,7 +116,7 @@ void HTMotorControl()
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_angle_feedback_ptr;
else
pid_measure = measure->real_current;
pid_measure = measure->total_angle;
// measure单位是rad,ref是角度,统一到angle下计算,方便建模
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_DEGREE, pid_ref);
}
@ -125,11 +134,10 @@ void HTMotorControl()
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_DEGREE, pid_ref);
}
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr;
if (setting->close_loop_type & CURRENT_LOOP)
{
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr;
pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
}
@ -139,30 +147,34 @@ void HTMotorControl()
LIMIT_MIN_MAX(set, T_MIN, T_MAX); // 限幅,实际上这似乎和pid输出限幅重复了
tmp = float_to_uint(set, T_MIN, T_MAX, 12); // 数值最后在 -12~+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;
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将发送buff置零
memset(motor_can->tx_buff + 6, 0, sizeof(uint16_t));
}
CANTransmit(motor_can, 1);
}
}
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 HTMotorCalibEncoder(HTMotorInstance *motor)
// {
// HTMotorSetMode(CMD_ZERO_POSITION, motor);
// }

13
modules/motor/HTmotor/HT04.h
View File

@ -19,10 +19,15 @@
typedef struct // HT04
{
float last_angle;
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;
} HTMotor_Measure_t;
/* HT电机类型定义*/
@ -91,11 +96,7 @@ void HTMotorStop(HTMotorInstance *motor);
void HTMotorEnable(HTMotorInstance *motor);
/**
* @brief 校准电机编码器
* @attention 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* @brief 校准电机编码器,设置的当前位置为0
*
* @param motor
*/

4
modules/motor/LKmotor/LK9025.c
View File

@ -104,10 +104,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);

18
modules/motor/motor_def.h
View File

@ -45,7 +45,7 @@ typedef enum
/* 反馈来源设定,若设为OTHER_FEED则需要指定数据来源指针,详见Motor_Controller_s*/
typedef enum
{
MOTOR_FEED=0,
MOTOR_FEED = 0,
OTHER_FEED,
} Feedback_Source_e;
@ -71,13 +71,13 @@ typedef enum
/* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
typedef struct
{
Closeloop_Type_e outer_loop_type; // 最外层的闭环,未设置时默认为最高级的闭环
Closeloop_Type_e close_loop_type; // 使用几个闭环(串级)
Motor_Reverse_Flag_e motor_reverse_flag; // 是否反转
Closeloop_Type_e outer_loop_type; // 最外层的闭环,未设置时默认为最高级的闭环
Closeloop_Type_e close_loop_type; // 使用几个闭环(串级)
Motor_Reverse_Flag_e motor_reverse_flag; // 是否反转
Feedback_Reverse_Flag_e feedback_reverse_flag; // 反馈是否反向
Feedback_Source_e angle_feedback_source; // 角度反馈类型
Feedback_Source_e speed_feedback_source; // 速度反馈类型
Feedfoward_Type_e feedforward_flag; // 前馈标志
Feedback_Source_e angle_feedback_source; // 角度反馈类型
Feedback_Source_e speed_feedback_source; // 速度反馈类型
Feedfoward_Type_e feedforward_flag; // 前馈标志
} Motor_Control_Setting_s;
@ -87,8 +87,8 @@ typedef struct
{
float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
float *other_speed_feedback_ptr;
// float *speed_foward_ptr;
// float *current_foward_ptr;
float *speed_feedforward_ptr;
float *current_feedforward_ptr;
PIDInstance current_PID;
PIDInstance speed_PID;

2
modules/remote/remote_control.c
View File

@ -117,7 +117,6 @@ static void RemoteControlRxCallback()
*/
static void RCLostCallback(void *id)
{
// @todo 遥控器丢失的处理
USARTServiceInit(rc_usart_instance); // 尝试重新启动接收
}
@ -130,7 +129,6 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle)
rc_usart_instance = USARTRegister(&conf);
// 进行守护进程的注册,用于定时检查遥控器是否正常工作
// @todo 当前守护进程直接在这里注册,后续考虑将其封装到遥控器的初始化函数中,即可以让用户决定reload_count的值(是否有必要?)
Daemon_Init_Config_s daemon_conf = {
.reload_count = 10, // 100ms未收到数据视为离线,遥控器的接收频率实际上是1000/14Hz(大约70Hz)
.callback = RCLostCallback,