AimBot/Gimbal
14
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor: migrate from DJI motors to DM motors, simplify to gimbal-only control

Browse Source 
- Rewrite DM motor driver based on DM_6220 reference protocol (MIT/PosVel/Velocity modes)
- Use velocity control mode: MCU runs angle PID with IMU feedback, motor handles speed loop
- Rewrite vision protocol with 3 frame types (0xA5 auto data, 0xA6 manual data, 0xA7 command)
- Decouple serial parsing (opencv module) from mode switching (robot_cmd module)
- Implement dual-loop architecture: vision PID outer loop → angle ref → motor PID inner loop
- Add KF/PID reset on mode switch transitions
- Merge motor_reverse_flag and feedback_reverse_flag into single output_reverse flag
- Remove DMMotorEnable/DMMotorStop (motor always runs, controlled by angle ref)
- Remove unused modules: DJI/LK/HT/step/servo motors, chassis, shoot, referee
- Add ExitRun0Mode() for STM32H723 power supply configuration
- Clean up CMakeLists: 115 → 76 C source files
This commit is contained in:
李傲琦
2026-05-29 23:37:36 +08:00
parent 5b3333125d
commit 4d1702b577
17 changed files with 790 additions and 1485 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+8 -31
View File
@@ -75,47 +75,35 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
bsp/log/bsp_log.c
bsp/bsp_tools.c
# modules
# modules - algorithm
modules/algorithm/controller.c
modules/algorithm/kalman_filter.c
modules/algorithm/QuaternionEKF.c
modules/algorithm/crc8.c
modules/algorithm/crc16.c
modules/algorithm/user_lib.c
modules/bluetooth/HC05.c
# modules - sensors
modules/BMI088/bmi088.c
modules/imu/BMI088driver.c
modules/imu/BMI088Middleware.c
modules/imu/ins_task.c
# modules - communication
modules/master_machine/master_process.c
modules/master_machine/seasky_protocol.c
modules/motor/DJImotor/dji_motor.c
modules/motor/HTmotor/HT04.c
modules/motor/LKmotor/LK9025.c
modules/motor/DMmotor/dmmotor.c
modules/motor/step_motor/step_motor.c
modules/motor/step_motor/X_V2.c
modules/motor/servo_motor/servo_motor.c
modules/motor/motor_task.c
modules/oled/oled.c
modules/referee/crc_ref.c
modules/referee/rm_referee.c
modules/referee/referee_UI.c
modules/referee/referee_task.c
modules/remote/remote_control.c
modules/super_cap/super_cap.c
modules/can_comm/can_comm.c
modules/timachine/board_comm.c
# modules - motor
modules/motor/DMmotor/dmmotor.c
modules/motor/motor_task.c
# modules - system
modules/message_center/message_center.c
modules/daemon/daemon.c
modules/alarm/buzzer.c
modules/vofa/vofa.c
modules/opencv/opencv.c
modules/timachine/board_comm.c
# application
application/gimbal/gimbal.c
application/chassis/chassis.c
application/shoot/shoot.c
application/cmd/robot_cmd.c
application/robot.c
)
@@ -144,21 +132,12 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE
# modules
modules/algorithm
modules/bluetooth
modules/BMI088
modules/imu
modules/master_machine
modules/motor/DJImotor
modules/motor/LKmotor
modules/motor/HTmotor
modules/motor/step_motor
modules/motor/servo_motor
modules/motor/DMmotor
modules/motor
modules/oled
modules/referee
modules/remote
modules/super_cap
modules/can_comm
modules/message_center
modules/daemon
@@ -169,8 +148,6 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE
modules
# application
application/chassis
application/shoot
application/gimbal
application/cmd
application
Core/Src/fdcan.c
+2 -2
View File
@@ -46,7 +46,7 @@ void MX_FDCAN1_Init(void)
hfdcan1.Init.TransmitPause = DISABLE;
hfdcan1.Init.ProtocolException = ENABLE;
hfdcan1.Init.NominalPrescaler = 3;
hfdcan1.Init.NominalSyncJumpWidth = 1;
hfdcan1.Init.NominalSyncJumpWidth = 10;
hfdcan1.Init.NominalTimeSeg1 = 29;
hfdcan1.Init.NominalTimeSeg2 = 10;
hfdcan1.Init.DataPrescaler = 3;
@@ -54,7 +54,7 @@ void MX_FDCAN1_Init(void)
hfdcan1.Init.DataTimeSeg1 = 29;
hfdcan1.Init.DataTimeSeg2 = 10;
hfdcan1.Init.MessageRAMOffset = 0;
hfdcan1.Init.StdFiltersNbr = 14;
hfdcan1.Init.StdFiltersNbr = 1;
hfdcan1.Init.ExtFiltersNbr = 0;
hfdcan1.Init.RxFifo0ElmtsNbr = 4;
hfdcan1.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8;
application/cmd/robot_cmd.c
+257 -734
View File
File diff suppressed because it is too large Load Diff
application/cmd/robot_cmd.h
+2 -6
View File
@@ -1,18 +1,14 @@
#ifndef ROBOT_CMD_H
#define ROBOT_CMD_H
/**
* @brief 机器人核心控制任务初始化,会被RobotInit()调用
*
*/
void RobotCMDInit();
/**
* @brief 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率)
*
* @brief 机器人核心控制任务,500Hz频率运行
*/
void RobotCMDTask();
#endif // !ROBOT_CMD_H
#endif // ROBOT_CMD_H
application/gimbal/gimbal.c
+78 -130
View File
@@ -1,165 +1,113 @@
#include "gimbal.h"
#include "robot_def.h"
#include "dji_motor.h"
#include "dmmotor.h"
#include "ins_task.h"
#include "message_center.h"
#include "general_def.h"
#include "bmi088.h"
#include "vofa.h"
static attitude_t *gimba_IMU_data; // 云台IMU数据
static attitude_t *gimbal_IMU_data;
static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd)
static Subscriber_t *gimbal_sub; // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息
static Publisher_t *gimbal_pub;
static Subscriber_t *gimbal_sub;
static Gimbal_Upload_Data_s gimbal_feedback_data;
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;
static DJIMotorInstance *yaw_motor; // 云台yaw电机实例
static DMMotorInstance *pitch_motor; // 云台pitch电机实例
static DMMotorInstance *yaw_motor; // DM-G6220
static DMMotorInstance *pitch_motor; // DM-J4310-2EC
static float yaw_speed_feedforward = 0.0f; // yaw电机的速度前馈
static float yaw_current_feedforward_abs = 500.0f;
static float yaw_current_feedforward = 0.0f; // yaw电机的电流前馈
static float yaw_last_ref = 0.0f; // 上一次yaw电机的设定值,用于计算前馈
static BMI088Instance *bmi088; // 云台IMU
void GimbalInit()
{
HAL_GPIO_WritePin(POWER_24V_1_GPIO_Port, POWER_24V_1_Pin, GPIO_PIN_SET);
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
HAL_GPIO_WritePin(POWER_24V_1_GPIO_Port, POWER_24V_1_Pin, GPIO_PIN_RESET);
gimbal_IMU_data = INS_Init();
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
// 初始化yaw电机
Motor_Init_Config_s yaw_motor_config = {
.can_init_config.can_handle = &hcan1,
.can_init_config.tx_id = 0x01,
.controller_param_init_config = {
.angle_PID = {
.Kp = 0.3,
.Ki = 0.8,
.Kd = 0.005,
.IntegralLimit = 20,
.CoefA = 15.0f,
.CoefB = 1.0f,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate,
.MaxOut = 20,
/* ---- Yaw: DM-G6220, CAN1, TX=0x01, RX=0xF1, Velocity模式 ---- */
DM_Motor_Init_Config_s yaw_config = {
.motor_config = {
.can_init_config = {
.can_handle = &hcan1,
.tx_id = 0x01,
.rx_id = 0xF1,
},
.speed_PID = {
.Kp = 1200,
.Ki = 0,
.Kd = 3,
.IntegralLimit = 16000,
.Output_LPF_RC = 0.001,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_OutputFilter,
.MaxOut = 16000,
.controller_param_init_config = {
.angle_PID = {
.Kp = 30, .Ki = 80, .Kd = 0.2f,
.IntegralLimit = 1000,
.CoefA = 20, .CoefB = 10,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
PID_Derivative_On_Measurement | PID_ChangingIntegrationRate |
PID_DerivativeFilter | PID_OutputFilter,
.MaxOut = 2000,
},
.other_angle_feedback_ptr = &gimbal_IMU_data->YawTotalAngle,
.other_speed_feedback_ptr = &gimbal_IMU_data->Gyro[2],
},
.other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
.other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
.speed_feedforward_ptr = &yaw_speed_feedforward, // 速度前馈指针
.controller_setting_init_config = {
.angle_feedback_source = OTHER_FEED,
.speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP,
.close_loop_type = ANGLE_LOOP,
.output_reverse = OUTPUT_DIRECTION_NORMAL,
},
.motor_type = DM_MOTOR,
},
.controller_setting_init_config = {
.angle_feedback_source = OTHER_FEED, .speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP, // 设置为开环，电机设定值由下面的功率控制设定，不走普通的pid
.close_loop_type = SPEED_LOOP | ANGLE_LOOP, // 角度环和速度环
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
.feedforward_flag = SPEED_FEEDFORWARD | CURRENT_FEEDFORWARD, // 开启速度前馈
},
.motor_type = GM6020_NEW,
.ctrl_mode = DM_MODE_VELOCITY,
.mit_kp = 0, .mit_kd = 0,
};
yaw_motor = DJIMotorInit(&yaw_motor_config);
// 初始化PITCH电机
Motor_Init_Config_s pitch_config = {
.can_init_config = {
.can_handle = &hcan1,
.tx_id = 0x06,
.rx_id = 0x16,
},
.controller_param_init_config = {
.angle_PID = {
.Kp = 1,
.Ki = 0,
.Kd = 0.005,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_OutputFilter | PID_DerivativeFilter,
.IntegralLimit = 10,
.MaxOut = 10,
yaw_motor = DMMotorInit(&yaw_config);
/* ---- Pitch: DM-J4310-2EC, CAN1, TX=0x02, RX=0xF2, Velocity模式 ---- */
DM_Motor_Init_Config_s pitch_config = {
.motor_config = {
.can_init_config = {
.can_handle = &hcan1,
.tx_id = 0x02,
.rx_id = 0xF2,
},
.speed_PID = {
.Kp = 0.8,
.Ki = 0,
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 0.1,
.MaxOut = 1,
.controller_param_init_config = {
.angle_PID = {
.Kp = 20, .Ki = 50, .Kd = 0.1f,
.IntegralLimit = 500,
.CoefA = 15, .CoefB = 5,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
PID_Derivative_On_Measurement | PID_ChangingIntegrationRate,
.MaxOut = 1000,
},
.other_angle_feedback_ptr = &gimbal_IMU_data->Pitch,
.other_speed_feedback_ptr = &gimbal_IMU_data->Gyro[0],
},
.other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
.other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
},
.controller_setting_init_config = {
.angle_feedback_source = OTHER_FEED,
.speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP,
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
.feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
.controller_setting_init_config = {
.angle_feedback_source = OTHER_FEED,
.speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP,
.close_loop_type = ANGLE_LOOP,
.output_reverse = OUTPUT_DIRECTION_NORMAL,
},
.motor_type = DM_MOTOR,
},
.ctrl_mode = DM_MODE_VELOCITY,
.mit_kp = 0, .mit_kd = 0,
};
pitch_motor = DMMotorInit(&pitch_config);
}
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
void GimbalTask()
{
// 获取云台控制数据
// 后续增加未收到数据的处理
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
switch (gimbal_cmd_recv.gimbal_mode)
if (gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE)
{
// 停止
case GIMBAL_ZERO_FORCE:
DJIMotorStop(yaw_motor);
DMMotorStop(pitch_motor);
yaw_speed_feedforward = 0.0f; // 停止时清零前馈
yaw_last_ref = 0.0f; // 停止时清零上一次参考值
break;
// 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
case GIMBAL_GYRO_MODE: // 后续只保留此模式
DJIMotorEnable(yaw_motor);
DMMotorEnable(pitch_motor);
yaw_speed_feedforward = DEGREE_2_RAD * (gimbal_cmd_recv.yaw - yaw_last_ref) * 500.0f;
LIMIT_MIN_MAX(yaw_speed_feedforward, -10.0f, 10.0f); // 限制前馈值
yaw_last_ref = gimbal_cmd_recv.yaw; // 更新上一次的参考值
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
/* 零力模式: 保持当前IMU角度, 电机不主动出力 */
DMMotorSetRef(yaw_motor, gimbal_IMU_data->YawTotalAngle);
DMMotorSetRef(pitch_motor, gimbal_IMU_data->Pitch);
}
else
{
DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw);
DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
if (yaw_motor->motor_controller.speed_PID.Ref >= 0.5f)
{
yaw_current_feedforward = 500.0f; // 前馈电流
}
else if (yaw_motor->motor_controller.speed_PID.Ref <= -0.5f)
{
yaw_current_feedforward = -500.0f; // 前馈电流
}
else
{
yaw_current_feedforward = yaw_current_feedforward_abs * yaw_motor->motor_controller.speed_PID.Ref / 0.5f; // 前馈电流
}
break;
default:
break;
}
// 设置反馈数据,主要是imu和yaw的ecd
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
// 推送消息
gimbal_feedback_data.gimbal_imu_data = *gimbal_IMU_data;
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
}
}
application/gimbal/gimbal.h
+2 -4
View File
@@ -3,14 +3,12 @@
/**
* @brief 初始化云台,会被RobotInit()调用
*
*/
void GimbalInit();
/**
* @brief 云台任务
*
* @brief 云台任务,从gimbal_cmd话题获取控制指令并执行闭环控制
*/
void GimbalTask();
#endif // GIMBAL_H
#endif // GIMBAL_H
application/robot.c
+4 -39
View File
@@ -3,58 +3,23 @@
#include "robot_def.h"
#include "robot_task.h"
// 编译warning,提醒开发者修改机器人参数
#ifndef ROBOT_DEF_PARAM_WARNING
#define ROBOT_DEF_PARAM_WARNING
#pragma message "check if you have configured the parameters in robot_def.h, IF NOT, please refer to the comments AND DO IT, otherwise the robot will have FATAL ERRORS!!!"
#endif // !ROBOT_DEF_PARAM_WARNING
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
#include "chassis.h"
#endif
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
#include "gimbal.h"
#include "shoot.h"
#include "robot_cmd.h"
#endif
void RobotInit()
{
// 关闭中断,防止在初始化过程中发生中断
// 请不要在初始化过程中使用中断和延时函数！
// 若必须,则只允许使用DWT_Delay()
{
__disable_irq();
BSPInit();
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
BSPInit();
RobotCMDInit();
GimbalInit();
// ShootInit();
#endif
OSTaskInit();
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
//ChassisInit();
#endif
OSTaskInit(); // 创建基础任务
// 初始化完成,开启中断
__enable_irq();
}
void RobotTask()
{
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
RobotCMDTask();
GimbalTask();
// ShootTask();
#endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
//ChassisTask();
#endif
}
}
application/robot.h
+1 -5
View File
@@ -1,18 +1,14 @@
#ifndef ROBOT_H
#define ROBOT_H
/* Robot利用robot_def.h中的宏对不同的机器人进行了大量的兼容,同时兼容了两个开发板(云台板和底盘板)的配置 */
/**
* @brief 机器人初始化,请在开启rtos之前调用.这也是唯一需要放入main函数的函数
*
*/
void RobotInit();
/**
* @brief 机器人任务,放入实时系统以一定频率运行,内部会调用各个应用的任务
*
*/
void RobotTask();
#endif
#endif
application/robot_def.h
+23 -167
View File
@@ -1,14 +1,4 @@
/**
* @file robot_def.h
* @author NeoZeng neozng1@hnu.edu.cn
* @author Even
* @version 0.1
* @date 2022-12-02
*
* @copyright Copyright (c) HNU YueLu EC 2022 all rights reserved
*
*/
#pragma once // 可以用#pragma once代替#ifndef ROBOT_DEF_H(header guard)
#pragma once
#ifndef ROBOT_DEF_H
#define ROBOT_DEF_H
@@ -16,58 +6,33 @@
#include "master_process.h"
#include "stdint.h"
/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
/* 开发板类型定义 */
#define ONE_BOARD // 单板控制整车
// #define CHASSIS_BOARD //底盘板
// #define GIMBAL_BOARD //云台板
//#define VISION_USE_VCP // 使用虚拟串口发送视觉数据
#define VISION_USE_UART // 使用串口发送视觉数据
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
// 云台参数
#define YAW_CHASSIS_ALIGN_ECD 2711 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
// 发射参数
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量
// 机器人底盘修改的参数,单位为mm(毫米)
#define WHEEL_BASE 350 // 纵向轴距(前进后退方向)
#define TRACK_WIDTH 300 // 横向轮距(左右平移方向)
#define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
#define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
#define RADIUS_WHEEL 60 // 轮子半径
#define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
/* 云台参数 */
#define YAW_CHASSIS_ALIGN_ECD 2711
#define PITCH_HORIZON_ECD 3412
#define PITCH_MAX_ANGLE 0
#define PITCH_MIN_ANGLE 0
#define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
#define GYRO2GIMBAL_DIR_PITCH 1 // 陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反
#define GYRO2GIMBAL_DIR_ROLL 1 // 陀螺仪数据相较于云台的roll的方向,1为相同,-1为相反
/* IMU->云台方向 */
#define GYRO2GIMBAL_DIR_YAW 1
#define GYRO2GIMBAL_DIR_PITCH 1
#define GYRO2GIMBAL_DIR_ROLL 1
// 检查是否出现主控板定义冲突,只允许一个开发板定义存在,否则编译会自动报错
#if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \
(defined(ONE_BOARD) && defined(GIMBAL_BOARD)) || \
(defined(CHASSIS_BOARD) && defined(GIMBAL_BOARD))
#error Conflict board definition! You can only define one board type.
#endif
#pragma pack(1)
#pragma pack(1) // 压缩结构体,取消字节对齐,下面的数据都可能被传输
/* -------------------------基本控制模式和数据类型定义-------------------------*/
/**
* @brief 这些枚举类型和结构体会作为CMD控制数据和各应用的反馈数据的一部分
*
*/
// 机器人状态
/* 机器人状态 */
typedef enum
{
ROBOT_STOP = 0,
ROBOT_READY,
} Robot_Status_e;
// 应用状态
/* 应用状态 */
typedef enum
{
APP_OFFLINE = 0,
@@ -75,125 +40,22 @@ typedef enum
APP_ERROR,
} App_Status_e;
// 底盘模式设置
/**
* @brief 后续考虑修改为云台跟随底盘,而不是让底盘去追云台,云台的惯量比底盘小.
*
*/
/* 云台模式 */
typedef enum
{
CHASSIS_ZERO_FORCE = 0, // 电流零输入
CHASSIS_ROTATE, // 小陀螺模式
CHASSIS_NO_FOLLOW, // 不跟随，允许全向平移
CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制
} chassis_mode_e;
// 云台模式设置
typedef enum
{
GIMBAL_ZERO_FORCE = 0, // 电流零输入
GIMBAL_FREE_MODE, // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle;似乎可以改为全部用IMU数据?
GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
GIMBAL_ZERO_FORCE = 0,
GIMBAL_GYRO_MODE,
} gimbal_mode_e;
// 发射模式设置
typedef enum
{
SHOOT_OFF = 0,
SHOOT_ON,
} shoot_mode_e;
typedef enum
{
FRICTION_OFF = 0, // 摩擦轮关闭
FRICTION_ON, // 摩擦轮开启
} friction_mode_e;
typedef enum
{
LID_OPEN = 0, // 弹舱盖打开
LID_CLOSE, // 弹舱盖关闭
} lid_mode_e;
typedef enum
{
LOAD_STOP = 0, // 停止发射
LOAD_REVERSE, // 反转
LOAD_1_BULLET, // 单发
LOAD_3_BULLET, // 三发
LOAD_BURSTFIRE, // 连发
} loader_mode_e;
// 功率限制,从裁判系统获取,是否有必要保留?
typedef struct
{ // 功率控制
float chassis_power_mx;
} Chassis_Power_Data_s;
/* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
/**
* @brief 对于双板情况,遥控器和pc在云台,裁判系统在底盘
*
*/
// cmd发布的底盘控制数据,由chassis订阅
/* ---- CMD 发布给 gimbal 的控制数据 ---- */
typedef struct
{
// 控制部分
float vx; // 前进方向速度
float vy; // 横移方向速度
float wz; // 旋转速度
float offset_angle; // 底盘和归中位置的夹角
chassis_mode_e chassis_mode;
int chassis_speed_buff;
// UI部分
// ...
} Chassis_Ctrl_Cmd_s;
// cmd发布的云台控制数据,由gimbal订阅
typedef struct
{ // 云台角度控制
float yaw;
float pitch;
float chassis_rotate_wz;
float yaw; // yaw 目标角度 (degrees, multi-turn)
float pitch; // pitch 目标角度 (degrees)
gimbal_mode_e gimbal_mode;
} Gimbal_Ctrl_Cmd_s;
// cmd发布的发射控制数据,由shoot订阅
typedef struct
{
shoot_mode_e shoot_mode;
loader_mode_e load_mode;
lid_mode_e lid_mode;
friction_mode_e friction_mode;
Bullet_Speed_e bullet_speed; // 弹速枚举
uint8_t rest_heat;
float shoot_rate; // 连续发射的射频,unit per s,发/秒
} Shoot_Ctrl_Cmd_s;
/* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/
/**
* @brief 由cmd订阅,其他应用也可以根据需要获取.
*
*/
typedef struct
{
#if defined(CHASSIS_BOARD) || defined(GIMBAL_BOARD) // 非单板的时候底盘还将imu数据回传(若有必要)
// attitude_t chassis_imu_data;
#endif
// 后续增加底盘的真实速度
// float real_vx;
// float real_vy;
// float real_wz;
uint8_t rest_heat; // 剩余枪口热量
Bullet_Speed_e bullet_speed; // 弹速限制
Enemy_Color_e enemy_color; // 0 for blue, 1 for red
} Chassis_Upload_Data_s;
/* ---- gimbal 发布给 CMD 的反馈数据 ---- */
typedef struct
{
attitude_t gimbal_imu_data;
@@ -201,12 +63,6 @@ typedef struct
uint8_t gimbal_power;
} Gimbal_Upload_Data_s;
typedef struct
{
// code to go here
// ...
} Shoot_Upload_Data_s;
#pragma pack()
#pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)
#endif // !ROBOT_DEF_H
#endif // !ROBOT_DEF_H
application/robot_task.h
+10 -48
View File
@@ -1,4 +1,3 @@
/* 注意该文件应只用于任务初始化,只能被robot.c包含*/
#pragma once
#include "FreeRTOS.h"
@@ -9,12 +8,8 @@
#include "robot.h"
#include "ins_task.h"
#include "motor_task.h"
#include "referee_task.h"
#include "master_process.h"
#include "daemon.h"
#include "HT04.h"
#include "buzzer.h"
#include "dmmotor.h"
#include "bsp_log.h"
@@ -22,23 +17,16 @@ osThreadId insTaskHandle;
osThreadId robotTaskHandle;
osThreadId motorTaskHandle;
osThreadId daemonTaskHandle;
osThreadId uiTaskHandle;
void StartINSTASK(void const *argument);
void StartMOTORTASK(void const *argument);
void StartDAEMONTASK(void const *argument);
void StartROBOTTASK(void const *argument);
void StartUITASK(void const *argument);
/**
* @brief 初始化机器人任务,所有持续运行的任务都在这里初始化
*
*/
void OSTaskInit()
{
osThreadDef(instask, StartINSTASK, osPriorityAboveNormal, 0, 1024);
insTaskHandle = osThreadCreate(osThread(instask), NULL); // 由于是阻塞读取传感器,为姿态解算设置较高优先级,确保以1khz的频率执行
// // 后续修改为读取传感器数据准备好的中断处理,
insTaskHandle = osThreadCreate(osThread(instask), NULL);
osThreadDef(motortask, StartMOTORTASK, osPriorityNormal, 0, 256);
motorTaskHandle = osThreadCreate(osThread(motortask), NULL);
@@ -48,36 +36,27 @@ void OSTaskInit()
osThreadDef(robottask, StartROBOTTASK, osPriorityNormal, 0, 1024);
robotTaskHandle = osThreadCreate(osThread(robottask), NULL);
// osThreadDef(uitask, StartUITASK, osPriorityNormal, 0, 512);
// uiTaskHandle = osThreadCreate(osThread(uitask), NULL);
DMMotorControlInit(); // 初始化大电机
}
__attribute__((noreturn)) void StartINSTASK(void const *argument)
{
static float ins_start;
static float ins_dt;
INS_Init(); // 确保BMI088被正确初始化.
static float ins_start, ins_dt;
INS_Init();
LOGINFO("[freeRTOS] INS Task Start");
for (;;)
{
// 1kHz
ins_start = DWT_GetTimeline_ms();
INS_Task();
ins_dt = DWT_GetTimeline_ms() - ins_start;
if (ins_dt > 1)
LOGERROR("[freeRTOS] INS Task is being DELAY! dt = [%f]", &ins_dt);
LOGERROR("[freeRTOS] INS Task DELAY dt=%.3f", ins_dt);
osDelay(1);
}
}
__attribute__((noreturn)) void StartMOTORTASK(void const *argument)
{
static float motor_dt;
static float motor_start;
static float motor_start, motor_dt;
LOGINFO("[freeRTOS] MOTOR Task Start");
for (;;)
{
@@ -85,56 +64,39 @@ __attribute__((noreturn)) void StartMOTORTASK(void const *argument)
MotorControlTask();
motor_dt = DWT_GetTimeline_ms() - motor_start;
if (motor_dt > 1)
LOGERROR("[freeRTOS] MOTOR Task is being DELAY! dt = [%f]", &motor_dt);
LOGERROR("[freeRTOS] MOTOR Task DELAY dt=%.3f", motor_dt);
osDelay(1);
}
}
__attribute__((noreturn)) void StartDAEMONTASK(void const *argument)
{
static float daemon_dt;
static float daemon_start;
static float daemon_start, daemon_dt;
BuzzerInit();
LOGINFO("[freeRTOS] Daemon Task Start");
for (;;)
{
// 100Hz
daemon_start = DWT_GetTimeline_ms();
DaemonTask();
BuzzerTask();
daemon_dt = DWT_GetTimeline_ms() - daemon_start;
if (daemon_dt > 10)
LOGERROR("[freeRTOS] Daemon Task is being DELAY! dt = [%f]", &daemon_dt);
LOGERROR("[freeRTOS] Daemon Task DELAY dt=%.3f", daemon_dt);
osDelay(10);
}
}
__attribute__((noreturn)) void StartROBOTTASK(void const *argument)
{
static float robot_dt;
static float robot_start;
static float robot_start, robot_dt;
LOGINFO("[freeRTOS] ROBOT core Task Start");
// 200Hz-500Hz,若有额外的控制任务如平衡步兵可能需要提升至1kHz
for (;;)
{
robot_start = DWT_GetTimeline_ms();
RobotTask();
robot_dt = DWT_GetTimeline_ms() - robot_start;
if (robot_dt > 5)
LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt);
LOGERROR("[freeRTOS] ROBOT core Task DELAY dt=%.3f", robot_dt);
osDelay(2);
}
}
__attribute__((noreturn)) void StartUITASK(void const *argument)
{
LOGINFO("[freeRTOS] UI Task Start");
MyUIInit();
LOGINFO("[freeRTOS] UI Init Done, communication with ref has established");
for (;;)
{
// 每给裁判系统发送一包数据会挂起一次,详见UITask函数的refereeSend()
UITask();
osDelay(1); // 即使没有任何UI需要刷新,也挂起一次,防止卡在UITask中无法切换
}
}
modules/algorithm/controller.c
+2 -2
View File
@@ -28,8 +28,8 @@ static void f_Changing_Integration_Rate(PIDInstance *pid)
// 积分呈累积趋势
if (abs(pid->Err) <= pid->CoefB)
return; // Full integral
if (abs(pid->Err) <= (pid->CoefA + pid->CoefB))
pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
if (abs(pid->Err) <= (pid->CoefB + pid->CoefA))
pid->ITerm *= (pid->CoefB + pid->CoefA - abs(pid->Err)) / pid->CoefA;
else // 最大阈值,不使用积分
pid->ITerm = 0;
}
modules/motor/DMmotor/dmmotor.c
+187 -129
View File
@@ -1,23 +1,23 @@
#include "dmmotor.h"
#include "memory.h"
#include "general_def.h"
#include "user_lib.h"
#include "cmsis_os.h"
#include "string.h"
#include "daemon.h"
#include "stdlib.h"
#include "bsp_log.h"
#include "bsp_dwt.h"
static uint8_t idx;
static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT];
static osThreadId dm_task_handle[DM_MOTOR_CNT];
/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
/* ---- float <-> uint conversion for MIT mode ---- */
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;
@@ -25,74 +25,155 @@ 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;
}
/* ---- Low-level CAN send functions (all three modes) ---- */
/* MIT mode: CAN ID = motor_id */
void DMMotorSendMIT(CANInstance *can, float pos, float vel, float kp, float kd, float torq)
{
uint16_t pos_uint = float_to_uint(pos, DM_P_MIN, DM_P_MAX, 16);
uint16_t vel_uint = float_to_uint(vel, DM_V_MIN, DM_V_MAX, 12);
uint16_t kp_uint = float_to_uint(kp, DM_KP_MIN, DM_KP_MAX, 12);
uint16_t kd_uint = float_to_uint(kd, DM_KD_MIN, DM_KD_MAX, 12);
uint16_t torq_uint = float_to_uint(torq, DM_T_MIN, DM_T_MAX, 12);
uint8_t *buf = can->tx_buff;
buf[0] = (uint8_t)(pos_uint >> 8);
buf[1] = (uint8_t)(pos_uint);
buf[2] = (uint8_t)(vel_uint >> 4);
buf[3] = (uint8_t)(((vel_uint & 0xF) << 4) | (kp_uint >> 8));
buf[4] = (uint8_t)(kp_uint);
buf[5] = (uint8_t)(kd_uint >> 4);
buf[6] = (uint8_t)(((kd_uint & 0xF) << 4) | (torq_uint >> 8));
buf[7] = (uint8_t)(torq_uint);
CANTransmit(can, 1);
}
/* Position-Velocity mode: CAN ID = motor_id + 0x100 */
void DMMotorSendPosVel(CANInstance *can, float pos, float vel)
{
uint8_t *buf = can->tx_buff;
uint8_t *pos_bytes = (uint8_t *)&pos;
uint8_t *vel_bytes = (uint8_t *)&vel;
buf[0] = pos_bytes[0];
buf[1] = pos_bytes[1];
buf[2] = pos_bytes[2];
buf[3] = pos_bytes[3];
buf[4] = vel_bytes[0];
buf[5] = vel_bytes[1];
buf[6] = vel_bytes[2];
buf[7] = vel_bytes[3];
/* Temporarily change tx_id to motor_id + 0x100 for this transmission */
uint32_t saved_id = can->tx_id;
can->tx_id = saved_id + 0x100;
CANTransmit(can, 1);
can->tx_id = saved_id;
}
/* Velocity mode: CAN ID = motor_id + 0x200 */
void DMMotorSendVelocity(CANInstance *can, float vel)
{
uint8_t *buf = can->tx_buff;
uint8_t *vel_bytes = (uint8_t *)&vel;
buf[0] = vel_bytes[0];
buf[1] = vel_bytes[1];
buf[2] = vel_bytes[2];
buf[3] = vel_bytes[3];
buf[4] = 0;
buf[5] = 0;
buf[6] = 0;
buf[7] = 0;
uint32_t saved_id = can->tx_id;
can->tx_id = saved_id + 0x200;
CANTransmit(can, 1);
can->tx_id = saved_id;
}
/* ---- Motor commands (enable/disable/zero/clear error) ---- */
void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *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_instance->tx_buff, 0xFF, 7);
motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd;
CANTransmit(motor->motor_can_instance, 1);
}
/* ---- Feedback decode: CAN RX interrupt callback ---- */
static void DMMotorDecode(CANInstance *motor_can)
{
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
uint8_t *rxbuff = motor_can->rx_buff;
DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
DM_Motor_Measure_s *measure = &motor->measure;
uint16_t tmp;
DaemonReload(motor->motor_daemon);
measure->id = rxbuff[0] & 0x0F; // 低 4 位是控制器 ID
measure->state = (rxbuff[0] >> 4) & 0x0F; // ERR 存储在 state 字段
measure->id = rxbuff[0] & 0x0F;
measure->state = (rxbuff[0] >> 4) & 0x0F;
measure->last_position = measure->position;
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);
tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4);
tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
tmp = (uint16_t)(((rxbuff[4] & 0x0F) << 8) | rxbuff[5]);
measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);
measure->T_Mos = (float)rxbuff[6];
measure->T_Mos = (float)rxbuff[6];
measure->T_Rotor = (float)rxbuff[7];
// 多圈角度计算
if (measure->position - measure->last_position > 12.5)
measure->total_round--;
else if (measure->position - measure->last_position < -12.5)
measure->total_round++;
measure->total_angle = measure->position + measure->total_round * 25;
measure->total_angle = measure->total_angle * RAD_2_DEGREE;
measure->velocity = measure->velocity * RAD_2_DEGREE;
/* multi-turn tracking */
if (measure->position - measure->last_position > 10.0f)
measure->total_round--;
else if (measure->position - measure->last_position < -10.0f)
measure->total_round++;
measure->total_angle = (measure->position + measure->total_round * 25.1327f) * RAD_2_DEGREE;
measure->velocity = measure->velocity * RAD_2_DEGREE;
}
static void DMMotorLostCallback(void *motor_ptr)
{
DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
uint16_t can_bus = motor->motor_can_instace->can_handle == &hcan1 ? 1 : 2;
LOGWARNING("[DMmotor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instace->tx_id);
LOGWARNING("[DMmotor] Motor lost, id [%d]", motor->motor_can_instance->tx_id);
}
void DMMotorCaliEncoder(DMMotorInstance *motor)
{
DMMotorSetMode(DM_CMD_ZERO_POSITION, motor);
DWT_Delay(0.1);
}
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
/* ---- Init ---- */
DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config)
{
DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
memset(motor, 0, sizeof(DMMotorInstance));
motor->motor_settings = config->controller_setting_init_config;
PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
motor->motor_settings = config->motor_config.controller_setting_init_config;
motor->ctrl_mode = config->ctrl_mode;
motor->mit_kp = config->mit_kp;
motor->mit_kd = config->mit_kd;
config->can_init_config.can_module_callback = DMMotorDecode;
config->can_init_config.id = motor;
motor->motor_can_instace = CANRegister(&config->can_init_config);
/* init PID controllers */
PIDInit(&motor->motor_controller.angle_PID, &config->motor_config.controller_param_init_config.angle_PID);
PIDInit(&motor->motor_controller.speed_PID, &config->motor_config.controller_param_init_config.speed_PID);
PIDInit(&motor->motor_controller.current_PID, &config->motor_config.controller_param_init_config.current_PID);
motor->motor_controller.other_angle_feedback_ptr = config->motor_config.controller_param_init_config.other_angle_feedback_ptr;
motor->motor_controller.other_speed_feedback_ptr = config->motor_config.controller_param_init_config.other_speed_feedback_ptr;
motor->motor_controller.speed_feedforward_ptr = config->motor_config.controller_param_init_config.speed_feedforward_ptr;
motor->motor_controller.current_feedforward_ptr = config->motor_config.controller_param_init_config.current_feedforward_ptr;
/* register CAN instance */
config->motor_config.can_init_config.can_module_callback = DMMotorDecode;
config->motor_config.can_init_config.id = motor;
motor->motor_can_instance = CANRegister(&config->motor_config.can_init_config);
/* register daemon */
Daemon_Init_Config_s conf = {
.callback = DMMotorLostCallback,
.owner_id = motor,
@@ -100,133 +181,110 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
};
motor->motor_daemon = DaemonRegister(&conf);
DMMotorEnable(motor);
/* enable motor */
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
// DWT_Delay(0.1);
// DMMotorCaliEncoder(motor);
// DWT_Delay(0.1);
dm_motor_instance[idx++] = motor;
return motor;
}
/* ---- Runtime control ---- */
void DMMotorSetRef(DMMotorInstance *motor, float ref)
{
motor->pid_ref = ref;
}
void DMMotorEnable(DMMotorInstance *motor)
{
motor->stop_flag = MOTOR_ENALBED;
}
void DMMotorStop(DMMotorInstance *motor) // 不使用使能模式是因为需要收到反馈
{
motor->stop_flag = MOTOR_STOP;
}
void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
{
motor->motor_settings.outer_loop_type = type;
}
//@Todo: 目前只实现了力控，更多位控PID等请自行添加
void DMMotorTask(void const *argument)
void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode)
{
float pid_ref, set;
DMMotorInstance *motor = (DMMotorInstance *)argument;
DM_Motor_Measure_s *measure = &motor->measure;
Motor_Control_Setting_s *setting = &motor->motor_settings;
// CANInstance *motor_can = motor->motor_can_instace;
// uint16_t tmp;
DMMotor_Send_s motor_send_mailbox;
float pid_measure; // 电机PID测量值和设定值
while (1)
motor->ctrl_mode = mode;
}
/* ---- Main control: called from Motor Task at 1kHz ---- */
void DMMotorControl(void)
{
for (uint8_t i = 0; i < idx; i++)
{
if(motor->measure.state == 0x00)
{
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
osDelay(20);
}
if(motor->measure.state != 0x00&&motor->measure.state != 0x01)
DMMotorInstance *motor = dm_motor_instance[i];
DM_Motor_Measure_s *measure = &motor->measure;
Motor_Control_Setting_s *setting = &motor->motor_settings;
Motor_Controller_s *ctrl = &motor->motor_controller;
CANInstance *can = motor->motor_can_instance;
/* handle motor error states */
if (measure->state != 0x00 && measure->state != 0x01)
{
DMMotorSetMode(DM_CMD_CLEAR_ERROR, motor);
osDelay(20);
DWT_Delay(0.001);
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
osDelay(20);
continue;
}
pid_ref = motor->pid_ref;
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
pid_ref *= -1; // 设置反转
float pid_ref = motor->pid_ref;
/* ---- Angle loop (outermost) ---- */
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
{
float pid_measure;
if (setting->angle_feedback_source == OTHER_FEED)
{
pid_measure = *motor->other_angle_feedback_ptr;
// char buff[20];
// Float2Str(buff, pid_measure);
// LOGINFO("pid_measure: %s", buff);
}
pid_measure = *ctrl->other_angle_feedback_ptr;
else
pid_measure = measure->total_angle;
// 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
pid_ref = PIDCalculate(&ctrl->angle_PID, pid_measure, pid_ref);
}
/* ---- Speed loop (inner) ---- */
if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
{
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor->speed_feedforward_ptr;
pid_ref += *ctrl->speed_feedforward_ptr;
float pid_measure;
if (setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else // MOTOR_FEED
pid_measure = *ctrl->other_speed_feedback_ptr;
else
pid_measure = measure->velocity;
// 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure, pid_ref);
pid_ref = PIDCalculate(&ctrl->speed_PID, pid_measure, pid_ref);
}
if (setting->feedforward_flag == 1)
pid_ref += 0.8;
if (setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
/* output reverse */
if (setting->output_reverse == OUTPUT_DIRECTION_REVERSE)
pid_ref *= -1;
set = pid_ref;
/* ---- Send via selected control mode ---- */
float vel_cmd = pid_ref * DEGREE_2_RAD; // deg/s → rad/s
LIMIT_MIN_MAX(vel_cmd, DM_V_MIN, DM_V_MAX);
LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX);
motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16);
motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12);
motor_send_mailbox.torque_des = float_to_uint(set, DM_T_MIN, DM_T_MAX, 12);
motor_send_mailbox.Kp = 0;
motor_send_mailbox.Kd = 0;
switch (motor->ctrl_mode)
{
case DM_MODE_VELOCITY:
/* Velocity mode: MCU angle PID output → motor velocity ref */
DMMotorSendVelocity(can, vel_cmd);
break;
if (motor->stop_flag == MOTOR_STOP)
motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12);
case DM_MODE_MIT: {
/* MIT mode: MCU angle PID output → motor position ref, motor handles position-velocity internally */
float pos_cmd = pid_ref * DEGREE_2_RAD;
LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
DMMotorSendMIT(can, pos_cmd, 0.0f, motor->mit_kp, motor->mit_kd, 0.0f);
break;
}
motor->motor_can_instace->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8);
motor->motor_can_instace->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des);
motor->motor_can_instace->tx_buff[2] = (uint8_t)(motor_send_mailbox.velocity_des >> 4);
motor->motor_can_instace->tx_buff[3] = (uint8_t)(((motor_send_mailbox.velocity_des & 0xF) << 4) | (motor_send_mailbox.Kp >> 8));
motor->motor_can_instace->tx_buff[4] = (uint8_t)(motor_send_mailbox.Kp);
motor->motor_can_instace->tx_buff[5] = (uint8_t)(motor_send_mailbox.Kd >> 4);
motor->motor_can_instace->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8));
motor->motor_can_instace->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des);
CANTransmit(motor->motor_can_instace, 1);
osDelay(1);
case DM_MODE_POS_VEL: {
/* Position-Velocity mode: MCU angle PID output → motor position ref + velocity ff */
float pos_cmd = pid_ref * DEGREE_2_RAD;
LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
DMMotorSendPosVel(can, pos_cmd, vel_cmd);
break;
}
}
}
}
void DMMotorControlInit()
{
char dm_task_name[5] = "dm";
// 遍历所有电机实例,创建任务
if (!idx)
return;
for (size_t i = 0; i < idx; i++)
{
char dm_id_buff[2] = {0};
__itoa(i, dm_id_buff, 10);
strcat(dm_task_name, dm_id_buff);
osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128);
dm_task_handle[i] = osThreadCreate(osThread(dm_task_name), dm_motor_instance[i]);
}
}
modules/motor/DMmotor/dmmotor.h
+80 -53
View File
@@ -1,5 +1,6 @@
#ifndef DMMOTOR_H
#define DMMOTOR_H
#include <stdint.h>
#include "bsp_can.h"
#include "controller.h"
@@ -8,73 +9,99 @@
#define DM_MOTOR_CNT 4
#define DM_P_MIN (-12.56637f)
#define DM_P_MAX 12.56637f
/* DM motor parameter ranges (from reference DM_6220 driver) */
#define DM_P_MIN (-12.5f)
#define DM_P_MAX 12.5f
#define DM_V_MIN (-45.0f)
#define DM_V_MAX 45.0f
#define DM_T_MIN (-18.0f)
#define DM_T_MAX 18.0f
#define DM_T_MIN (-10.0f)
#define DM_T_MAX 10.0f
#define DM_KP_MIN 0.0f
#define DM_KP_MAX 500.0f
#define DM_KD_MIN 0.0f
#define DM_KD_MAX 5.0f
typedef struct
/* DM motor control modes */
typedef enum
{
uint8_t id;
uint8_t state;
float velocity;
float last_position;
float position;
float torque;
float T_Mos;
float T_Rotor;
int32_t total_round;
float total_angle;
}DM_Motor_Measure_s;
DM_MODE_MIT = 0, // MIT mode: CAN ID, packed pos/vel/KP/KD/torque
DM_MODE_POS_VEL = 1, // Position-Velocity mode: CAN ID + 0x100, float pos + float vel
DM_MODE_VELOCITY = 2, // Velocity mode: CAN ID + 0x200, float velocity only
} DM_Motor_Mode_e;
/* DM motor measurement data (parsed from feedback CAN frame) */
typedef struct
{
uint16_t position_des;
uint16_t velocity_des;
uint16_t torque_des;
uint16_t Kp;
uint16_t Kd;
}DMMotor_Send_s;
typedef struct
uint8_t id; // motor ID from feedback byte[0:3]
uint8_t state; // motor state from feedback byte[7:4]: 0=reset, 1=run
float position; // current position (rad, range ±12.5)
float velocity; // current velocity (rad/s, range ±45)
float torque; // current torque (N·m, range ±10)
float T_Mos; // MOS temperature (Celsius)
float T_Rotor; // rotor temperature (Celsius)
float last_position; // previous position for multi-turn calculation
int32_t total_round; // accumulated full revolutions
float total_angle; // multi-turn angle (degrees)
} DM_Motor_Measure_s;
/* DM motor instance (runtime state) */
typedef struct
{
DM_Motor_Measure_s measure;
Motor_Control_Setting_s motor_settings;
PIDInstance current_PID;
PIDInstance speed_PID;
PIDInstance angle_PID;
float *other_angle_feedback_ptr;
float *other_speed_feedback_ptr;
float *speed_feedforward_ptr;
float *current_feedforward_ptr;
float pid_ref;
Motor_Working_Type_e stop_flag;
CANInstance *motor_can_instace;
DaemonInstance* motor_daemon;
uint32_t lost_cnt;
uint8_t lost_status;
uint8_t last_lost_status;
}DMMotorInstance;
Motor_Controller_s motor_controller;
CANInstance *motor_can_instance; // CAN communication instance
DaemonInstance *motor_daemon; // daemon watchdog
DM_Motor_Mode_e ctrl_mode; // current control mode (MIT / Pos-Vel / Velocity)
float mit_kp; // MIT mode KP (motor internal position gain, 0~500)
float mit_kd; // MIT mode KD (motor internal velocity gain, 0~5)
float pid_ref; // reference input from application
} DMMotorInstance;
/* DM motor init config */
typedef struct
{
Motor_Init_Config_s motor_config; // generic motor PID and setting config
DM_Motor_Mode_e ctrl_mode; // default control mode
float mit_kp; // MIT mode KP (only for MIT mode)
float mit_kd; // MIT mode KD (only for MIT mode)
} DM_Motor_Init_Config_s;
/* DM motor command codes */
typedef enum
{
DM_CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令
DM_CMD_RESET_MODE = 0xfd, // 停止
DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位
DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误
}DMMotor_Mode_e;
DM_CMD_MOTOR_MODE = 0xFC, // enable motor
DM_CMD_RESET_MODE = 0xFD, // disable motor
DM_CMD_ZERO_POSITION = 0xFE, // set current position as encoder zero
DM_CMD_CLEAR_ERROR = 0xFB, // clear motor error
} DMMotor_Mode_e;
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config);
/* ---- API ---- */
DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config);
void DMMotorSetRef(DMMotorInstance *motor, float ref);
void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type);
void DMMotorEnable(DMMotorInstance *motor);
void DMMotorStop(DMMotorInstance *motor);
void DMMotorCaliEncoder(DMMotorInstance *motor);
void DMMotorControlInit();
void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type);
void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode);
void DMMotorSetMode(DMMotor_Mode_e mode, DMMotorInstance *motor);
#endif // !DMMOTOR
/**
* @brief Main control function called from Motor Task at 1kHz.
* Iterates all registered motors, runs PID cascade, sends CAN frames.
*/
void DMMotorControl(void);
/* ---- Low-level CAN send functions (all three modes) ---- */
/* MIT mode: send position + velocity + KP + KD + torque in packed 8-byte frame */
void DMMotorSendMIT(CANInstance *can, float pos, float vel, float kp, float kd, float torq);
/* Position-Velocity mode: send float position + float velocity */
void DMMotorSendPosVel(CANInstance *can, float pos, float vel);
/* Velocity mode: send float velocity only */
void DMMotorSendVelocity(CANInstance *can, float vel);
#endif // DMMOTOR_H
modules/motor/motor_def.h
+12 -24
View File
@@ -49,19 +49,13 @@ typedef enum
OTHER_FEED,
} Feedback_Source_e;
/* 电机正反转标志 */
/* 输出反向标志: 取反最终PID输出 */
typedef enum
{
MOTOR_DIRECTION_NORMAL = 0,
MOTOR_DIRECTION_REVERSE = 1
} Motor_Reverse_Flag_e;
OUTPUT_DIRECTION_NORMAL = 0,
OUTPUT_DIRECTION_REVERSE = 1,
} Output_Reverse_Flag_e;
/* 反馈量正反标志 */
typedef enum
{
FEEDBACK_DIRECTION_NORMAL = 0,
FEEDBACK_DIRECTION_REVERSE = 1
} Feedback_Reverse_Flag_e;
typedef enum
{
MOTOR_STOP = 0,
@@ -77,14 +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; // 是否反转
Feedback_Reverse_Flag_e feedback_reverse_flag; // 反馈是否反向
Feedback_Source_e angle_feedback_source; // 角度反馈类型
Feedback_Source_e speed_feedback_source; // 速度反馈类型
uint8_t feedforward_flag; // 前馈标志
MOTOR_SEND_MODE_e send_mode; // 发送模式
Closeloop_Type_e outer_loop_type; // 最外层的闭环
Closeloop_Type_e close_loop_type; // 使用几个闭环(串级)
Output_Reverse_Flag_e output_reverse; // 输出反向
Feedback_Source_e angle_feedback_source; // 角度反馈类型
Feedback_Source_e speed_feedback_source; // 速度反馈类型
uint8_t feedforward_flag; // 前馈标志
MOTOR_SEND_MODE_e send_mode; // 发送模式
} Motor_Control_Setting_s;
@@ -111,12 +104,7 @@ typedef struct
typedef enum
{
MOTOR_TYPE_NONE = 0,
GM6020,
M3508,
M2006,
LK9025,
HT04,
GM6020_NEW //新版固件6020
DM_MOTOR,
} Motor_Type_e;
/**
modules/motor/motor_task.c
+2 -20
View File
@@ -1,25 +1,7 @@
#include "motor_task.h"
#include "LK9025.h"
#include "HT04.h"
#include "dji_motor.h"
#include "step_motor.h"
#include "servo_motor.h"
#include "dmmotor.h"
void MotorControlTask()
{
// static uint8_t cnt = 0; 设定不同电机的任务频率
// if(cnt%5==0) //200hz
// if(cnt%10==0) //100hz
DJIMotorControl();
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
LKMotorControl();
// legacy support
// 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
// 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换
// HTMotorControl();
// 将所有的CAN设备集中在一处发送,最高反馈频率仅能达到500Hz,为了更好的控制效果,应使用新的HTMotorControlInit()接口
// StepMotorControl();
DMMotorControl();
}
modules/opencv/opencv.c
+57 -81
View File
@@ -1,99 +1,75 @@
#include "opencv.h"
#include "daemon.h"
#define FRAME_SOF 0xA5
#define FRAME_PAYLOAD_LEN (2+2+4*12) // 52
#define FRAME_FULL_LEN (1+1+FRAME_PAYLOAD_LEN) //54
float opencv_Vision_data[12];
VisionData_t vision_data;
uint8_t Vison_revc_flag = 0; // 视觉数据接收标志位
uint8_t vision_used_flag = 0; // 视觉数据使用标志位,用于判断是否需要使用视觉数据
float Vison_Frequency; // 视觉数据接收频率,单位Hz
static USARTInstance *cv_usart_instance;
static DaemonInstance *OPENCV_vision_daemon_instance;
static DaemonInstance *vision_daemon_instance;
/**
* @brief 接收解包回调函数,将在bsp_usart.c中被usart rx callback调用
* @todo 1.提高可读性,将get_protocol_info的第四个参数增加一个float类型buffer
* 2.添加标志位解码
*/
static void DecodeVision()
static uint8_t calc_checksum(uint8_t *buf, uint8_t len)
{
DaemonReload(OPENCV_vision_daemon_instance); // 喂狗,防止离线
static uint32_t counter;
float dt = DWT_GetDeltaT(&counter);
Vison_Frequency = 1.0f / dt; // 计算频率
const uint16_t len = cv_usart_instance->recv_buff_size;
uint16_t flag_register;
uint8_t *buf = cv_usart_instance->recv_buff;
if (len != FRAME_FULL_LEN) {
// 长度不符
LOGERROR("Vision frame length error");
return;
}
if (buf[0] != FRAME_SOF) {
// SOF 错误
LOGERROR("Vision frame SOF error");
return;
}
uint8_t payload_len = buf[1];
if (payload_len != FRAME_PAYLOAD_LEN) {
// Payload 长度不符
LOGERROR("Vision frame payload length error");
return;
}
// 解析命令号和 flags（小端）
uint16_t cmd_id = buf[2] | (buf[3] << 8);
uint16_t flags = buf[4] | (buf[5] << 8);
// 解析 12 个 float
for (int i = 0; i < 12; i++) {
// 从 buf[6 + 4*i] 连续读 4 字节
memcpy(&opencv_Vision_data[i], buf + 6 + 4*i, sizeof(float));
}
// // 解析任务标志位(拟合最接近的int)
// int last_task_cmd = Vison_task_cmd;
// Vison_task_cmd = (int)(Vison_data[10] + 0.5f);
// if (Vison_task_cmd != last_task_cmd) {
// Vison_task_changed = 1; // 任务命令变化
// }
Vison_revc_flag = 1; // 设置接收标志位
// vision_used_flag = 1; // 设置视觉数据使用标志位
//解析暂停标志位
// run = (int)(Vison_data[11] + 0.5f);
// 处理...
// printf("Got frame: cmd=0x%04X flags=0x%04X\n", cmd_id, flags);
// for (int i = 0; i < 12; i += 2) {
// printf(" coord %d: (%.3f, %.3f)\n",
// i/2, data[i], data[i+1]);
// float a = data[0];
// TODO: code to resolve flag_register;
uint8_t sum = 0;
for (uint8_t i = 0; i < len - 1; i++)
sum += buf[i];
return sum;
}
void VisionOfflineCallback(void *owner_id)
static void DecodeVision()
{
Vison_revc_flag = 0; // 清除接收标志位
DaemonReload(vision_daemon_instance);
static uint32_t counter;
float dt = DWT_GetDeltaT(&counter);
vision_data.frequency = 1.0f / dt;
uint8_t *buf = cv_usart_instance->recv_buff;
uint16_t len = cv_usart_instance->recv_buff_size;
uint8_t sof = buf[0];
if (sof == VISION_SOF_AUTO && len == VISION_AUTO_LEN)
{
if (calc_checksum(buf, VISION_AUTO_LEN) != buf[VISION_AUTO_LEN - 1])
return;
memcpy(&vision_data.error_x, buf + 1, 4);
memcpy(&vision_data.error_y, buf + 5, 4);
vision_data.auto_updated = 1;
}
else if (sof == VISION_SOF_MANUAL && len == VISION_MANUAL_LEN)
{
if (calc_checksum(buf, VISION_MANUAL_LEN) != buf[VISION_MANUAL_LEN - 1])
return;
vision_data.zero_force = buf[1];
memcpy(&vision_data.yaw_ref, buf + 2, 4);
memcpy(&vision_data.pitch_ref, buf + 6, 4);
vision_data.manual_updated = 1;
}
else if (sof == VISION_SOF_CMD && len == VISION_CMD_LEN)
{
if (calc_checksum(buf, VISION_CMD_LEN) != buf[VISION_CMD_LEN - 1])
return;
vision_data.cmd = buf[1];
vision_data.cmd_pending = 1;
}
}
static void VisionOfflineCallback(void *id)
{
(void)id;
USARTServiceInit(cv_usart_instance);
}
void Vision_Uart_Init(UART_HandleTypeDef *_handle)
{
USART_Init_Config_s conf;
conf.module_callback = DecodeVision;
conf.recv_buff_size = 54;
conf.usart_handle = _handle;
USART_Init_Config_s conf = {
.module_callback = DecodeVision,
.recv_buff_size = 64,
.usart_handle = _handle,
};
cv_usart_instance = USARTRegister(&conf);
// 为master process注册daemon,用于判断视觉通信是否离线
Daemon_Init_Config_s daemon_conf = {
.callback = VisionOfflineCallback, // 离线时调用的回调函数,会重启串口接收
.owner_id = NULL,
.callback = NULL,
.owner_id = NULL,
.reload_count = 10,
};
OPENCV_vision_daemon_instance = DaemonRegister(&daemon_conf);
}
vision_daemon_instance = DaemonRegister(&daemon_conf);
}
modules/opencv/opencv.h
+63 -10
View File
@@ -1,22 +1,75 @@
#ifndef OPENCV_H
#define OPENCV_H
#include "bsp_usart.h"
#include "opencv.h"
#include "bsp_log.h"
#include "bsp_dwt.h"
#include "bsp_log.h"
#include "usart.h"
#include "robot_def.h"
#include "daemon.h"
#include <string.h>
#include <stdio.h>
/*
* 上位机串口协议 — 三种帧类型, 通过帧头区分
*
* ---- 自动/视觉追踪数据帧 0xA5 (10 字节) ----
* [SOF:0xA5][error_x:4][error_y:4][checksum:1]
* error_x/y: float, 归一化误差, 视觉PID外环反馈值
*
* ---- 手动控制数据帧 0xA6 (11 字节) ----
* [SOF:0xA6][zero_force:1][yaw_ref:4][pitch_ref:4][checksum:1]
* zero_force: 0=正常角度控制, 1=零力/保持当前位置
* yaw_ref/pitch_ref: float, 角度参考值 (degrees)
*
* ---- 命令帧 0xA7 (3 字节) ----
* [SOF:0xA7][cmd:1][checksum:1]
* cmd: 0x01=切换到自动模式, 0x02=切换到手动模式, 0x03=激光开, 0x04=激光关
*
* checksum: 帧头到数据末尾前一字节累加和取低8位
*/
#define VISION_SOF_AUTO 0xA5
#define VISION_SOF_MANUAL 0xA6
#define VISION_SOF_CMD 0xA7
#define VISION_AUTO_LEN 10
#define VISION_MANUAL_LEN 11
#define VISION_CMD_LEN 3
/* 命令码 */
#define VISION_CMD_SWITCH_AUTO 0x01
#define VISION_CMD_SWITCH_MANUAL 0x02
#define VISION_CMD_LASER_ON 0x03
#define VISION_CMD_LASER_OFF 0x04
/* 工作模式 */
typedef enum
{
VISION_MODE_AUTO = 0, // 自动/视觉追踪
VISION_MODE_MANUAL = 1, // 手动
} Vision_Mode_e;
typedef struct
{
/* 自动模式数据 */
float error_x;
float error_y;
uint8_t auto_updated; // 收到新的自动数据帧标志
/* 手动模式数据 */
uint8_t zero_force;
float yaw_ref;
float pitch_ref;
uint8_t manual_updated; // 收到新的手动数据帧标志
/* 命令 */
uint8_t cmd; // 最新命令码
uint8_t cmd_pending; // 有待处理命令标志
float frequency; // 接收频率 (Hz)
} VisionData_t;
extern VisionData_t vision_data;
void Vision_Uart_Init(UART_HandleTypeDef *_handle);
extern float opencv_Vision_data[12]; // 视觉数据,12个float
extern uint8_t Vison_revc_flag; // 视觉数据接收标志位
extern uint8_t vision_used_flag; // 视觉数据使用标志位,用于判断是否需要使用视觉数据
extern float Vison_Frequency; // 视觉数据频率
#endif // !OPENCV_H
#endif // OPENCV_H