From e94bb504b8c74d5ee38c5d5232a41af8677dbc37 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Sun, 4 Dec 2022 20:26:15 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E4=BA=86djimotor=E5=8F=91?= =?UTF-8?q?=E9=80=81=E6=97=B6=E7=9A=84=E8=87=B4=E5=91=BD=E9=94=99=E8=AF=AF?= =?UTF-8?q?,=E7=BC=96=E5=86=99=E4=BA=86shoot=20app=E7=9A=84=E6=A1=86?= =?UTF-8?q?=E6=9E=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 7 ++- application/chassis/chassis.c | 25 +++++--- application/gimbal/gimbal.c | 5 +- application/robot_def.h | 46 +++++++-------- application/shoot/shoot.c | 105 +++++++++++++++++++++++++++++++--- modules/imu/ins_task.c | 8 +-- modules/imu/ins_task.md | 4 ++ modules/monitor/monitor.c | 4 +- modules/motor/dji_motor.c | 54 ++++++++++------- modules/motor/dji_motor.h | 19 ++++-- modules/motor/dji_motor.md | 47 ++++++++++++--- modules/motor/motor_def.h | 22 ++++--- 12 files changed, 254 insertions(+), 92 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index e5588d7..f09fc5c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -30,7 +30,12 @@ "robot_cmd.h": "c", "chassis_cmd.h": "c", "referee.h": "c", - "arm_math.h": "c" + "arm_math.h": "c", + "bsp_dwt.h": "c", + "compare": "c", + "*.tcc": "c", + "type_traits": "c", + "typeinfo": "c" }, "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", } \ No newline at end of file diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 67ff38c..d8a9b42 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -26,7 +26,7 @@ #define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0 #define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0 #define RADIUS_WHEEL 60 // 轮子半径 -#define REDUCTION_RATIO 19 // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换 +#define REDUCTION_RATIO 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换 /* 自动计算的参数 */ #define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) @@ -39,11 +39,11 @@ #include "ins_task.h" static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm IMU_Data_t *Chassis_IMU_data; -#endif // CHASSIS_BOARD +#endif // CHASSIS_BOARD static referee_info_t *referee_data; // 裁判系统的数据 // static SuperCAP* cap; 尚未增加超级电容 static dji_motor_instance *motor_lf; // left right forward back -static dji_motor_instance *motor_rf; +static dji_motor_instance *motor_rf; static dji_motor_instance *motor_lb; static dji_motor_instance *motor_rb; @@ -76,6 +76,7 @@ void ChassisInit() .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, + .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, .reverse_flag = MOTOR_DIRECTION_REVERSE, }, @@ -97,6 +98,7 @@ void ChassisInit() .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, + .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, .reverse_flag = MOTOR_DIRECTION_REVERSE, }, @@ -118,6 +120,7 @@ void ChassisInit() .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, + .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, .reverse_flag = MOTOR_DIRECTION_REVERSE, }, @@ -139,6 +142,7 @@ void ChassisInit() .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, + .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, .reverse_flag = MOTOR_DIRECTION_REVERSE, }, @@ -198,7 +202,6 @@ static void EstimateSpeed() // 根据电机速度和imu的速度解算 // chassis_feedback_data.vx vy wz // ... - } void ChassisTask() @@ -208,6 +211,7 @@ void ChassisTask() SubGetMessage(chassis_sub, &chassis_cmd_recv); // 根据控制模式设定旋转速度 + // 后续增加不同状态的过渡模式? switch (chassis_cmd_recv.chassis_mode) { case CHASSIS_NO_FOLLOW: @@ -220,7 +224,10 @@ void ChassisTask() // chassis_cmd_recv.wz // 当前维持定值,后续增加不规则的变速策略 break; case CHASSIS_ZERO_FORCE: - DJIMotorStop(); // 如果出现重要模块离线或遥控器设置为急停,让电机停止 + DJIMotorStop(motor_lf); // 如果出现重要模块离线或遥控器设置为急停,让电机停止 + DJIMotorStop(motor_rf); + DJIMotorStop(motor_lb); + DJIMotorStop(motor_rb); break; default: break; @@ -242,11 +249,11 @@ void ChassisTask() // 根据电机的反馈速度计算 EstimateSpeed(); - //获取裁判系统数据 - // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red - chassis_feedback_data.enemy_color = referee_data->GameRobotStat.robot_id > 7 ? 1 : 0; // + // 获取裁判系统数据 + // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red + chassis_feedback_data.enemy_color = referee_data->GameRobotStat.robot_id > 7 ? 1 : 0; // chassis_feedback_data.bullet_speed = referee_data->GameRobotStat.shooter_id1_17mm_speed_limit; - chassis_feedback_data.rest_heat=referee_data->PowerHeatData.shooter_heat0; + chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0; // 推送反馈消息 PubPushMessage(chassis_pub, &chassis_feedback_data); diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index d4fa6b9..213fcfb 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -43,6 +43,7 @@ void GimbalInit() .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, + .outer_loop_type=ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP, .reverse_flag = MOTOR_DIRECTION_REVERSE, }, @@ -69,6 +70,7 @@ void GimbalInit() .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, + .outer_loop_type=ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP, .reverse_flag = MOTOR_DIRECTION_REVERSE, }, @@ -101,7 +103,8 @@ void GimbalTask() switch (gimbal_cmd_recv.gimbal_mode) { case GIMBAL_ZERO_FORCE: - DJIMotorStop(); + DJIMotorStop(yaw_motor); + DJIMotorStop(pitch_motor); break; case GIMBAL_GYRO_MODE: DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); diff --git a/application/robot_def.h b/application/robot_def.h index b4a2b8c..9cdcfd3 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -4,17 +4,17 @@ * @author Even * @version 0.1 * @date 2022-12-02 - * + * * @copyright Copyright (c) HNU YueLu EC 2022 all rights reserved - * + * */ #ifndef ROBOT_DEF_H #define ROBOT_DEF_H #define PI 3.14159f -#define RAD_2_ANGLE (180.0f/PI) -#define ANGLE_2_RAD (PI/180.0f) +#define RAD_2_ANGLE (180.0f / PI) +#define ANGLE_2_RAD (PI / 180.0f) #include "ins_task.h" #include "master_process.h" @@ -29,7 +29,6 @@ /* 重要参数定义,注意根据不同机器人进行修改 */ #define YAW_MID_ECD - #if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \ (defined(ONE_BOARD) && defined(GIMBAL_BOARD)) || \ (defined(CHASSIS_BOARD) && defined(GIMBAL_BOARD)) @@ -55,7 +54,7 @@ typedef enum // 底盘模式设置 /** * @brief 后续考虑修改为云台跟随底盘,而不是让底盘去追云台,云台的惯量比底盘小. - * + * */ typedef enum { @@ -68,9 +67,9 @@ typedef enum // 云台模式设置 typedef enum { - GIMBAL_ZERO_FORCE, // 电流零输入 - GIMBAL_FREE_MODE, // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle - GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式 + GIMBAL_ZERO_FORCE, // 电流零输入 + GIMBAL_FREE_MODE, // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle + GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式 } gimbal_mode_e; // 发射模式设置 @@ -83,11 +82,12 @@ typedef enum typedef enum { LID_CLOSE, // 弹舱盖打开 - LID_ON, // 弹舱盖关闭 + LID_OPEN, // 弹舱盖关闭 } lid_mode_e; typedef enum { + SHOOT_STOP, // 停止整个发射模块,后续可能隔离出来 LOAD_STOP, // 停止发射 LOAD_REVERSE, // 反转 LOAD_1_BULLET, // 单发 @@ -98,13 +98,9 @@ typedef enum // 功率限制,从裁判系统获取 typedef struct { // 功率控制 - + } Chassis_Power_Data_s; - - - - /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */ /** * @brief 对于双板情况,遥控器和pc在云台,裁判系统在底盘 @@ -120,8 +116,8 @@ typedef struct float offset_angle; // 底盘和归中位置的夹角 chassis_mode_e chassis_mode; - //UI部分 - // ... + // UI部分 + // ... } Chassis_Ctrl_Cmd_s; @@ -137,17 +133,15 @@ typedef struct // cmd发布的发射控制数据,由shoot订阅 typedef struct -{ // 发射弹速控制 +{ loader_mode_e load_mode; lid_mode_e lid_mode; shoot_mode_e shoot_mode; - Bullet_Speed_e bullet_speed; + Bullet_Speed_e bullet_speed; // 弹速枚举 uint8_t rest_heat; + float shoot_rate; //连续发射的射频,unit per s,发/秒 } Shoot_Ctrl_Cmd_s; - - - /* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/ /** * @brief 由cmd订阅,其他应用也可以根据需要获取. @@ -165,11 +159,11 @@ typedef struct // 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 + uint8_t rest_heat; // 剩余枪口热量 + Bullet_Speed_e bullet_speed; // 弹速限制 + Enemy_Color_e enemy_color; // 0 for blue, 1 for red - //是否需要剩余电量?(电容) + // 是否需要剩余电量?(电容) } Chassis_Upload_Data_s; diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index a60e3f5..8ddc090 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -2,18 +2,27 @@ #include "robot_def.h" #include "dji_motor.h" #include "message_center.h" +#include "bsp_dwt.h" -/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 - */ +#define ONE_BULLET_DELTA_ANGLE 0 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 +#define REDUCTION_RATIO 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f +#define NUM_PER_CIRCLE 1 // 拨盘一圈的装载量 + +/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */ static dji_motor_instance *friction_l; // 左摩擦轮 static dji_motor_instance *friction_r; // 右摩擦轮 static dji_motor_instance *loader; // 拨盘电机 +// static servo_instance *lid; 需要增加弹舱盖 static Publisher_t *shoot_pub; static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自gimbal_cmd的发射控制信息 static Subscriber_t *shoot_sub; static Shoot_Upload_Data_s shoot_feedback_data; // 来自gimbal_cmd的发射控制信息 +// 定时,计算冷却用 +static uint32_t INS_DWT_Count = 0; +static float dt = 0, t = 0; + void ShootInit() { // 左摩擦轮 @@ -38,6 +47,8 @@ void ShootInit() .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, + + .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, .reverse_flag = MOTOR_DIRECTION_REVERSE, }, @@ -64,11 +75,12 @@ void ShootInit() .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, + .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, .reverse_flag = MOTOR_DIRECTION_REVERSE, }, .motor_type = M3508}; - // 拨盘电机 + // 拨盘电机 Motor_Init_Config_s loader_config = { .can_init_config = { .can_handle = &hcan1, @@ -76,9 +88,13 @@ void ShootInit() }, .controller_param_init_config = { .angle_PID = { + // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降 .Kd = 10, .Ki = 1, .Kd = 2, + }, + .angle_PID = { + }, .speed_PID = { @@ -88,12 +104,13 @@ void ShootInit() }, }, .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, - .close_loop_type = SPEED_LOOP | CURRENT_LOOP, - .reverse_flag = MOTOR_DIRECTION_REVERSE, + .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, + .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转 + .close_loop_type = ANGLE_LOOP | SPEED_LOOP | CURRENT_LOOP, + .reverse_flag = MOTOR_DIRECTION_REVERSE, // 注意方向设置为拨盘的拨出的击发方向 }, - .motor_type = M2006}; + .motor_type = M2006 // 英雄使用m3508 + }; friction_l = DJIMotorInit(&left_friction_config); friction_r = DJIMotorInit(&right_friction_config); @@ -105,4 +122,76 @@ void ShootInit() void ShootTask() { + // 从cmd获取控制数据 + SubGetMessage(shoot_sub, &shoot_cmd_recv); + + // 根据控制模式进行电机参考值设定和模式切换 + switch (shoot_cmd_recv.load_mode) + { + // 停止三个电机 + case SHOOT_STOP: + DJIMotorStop(friction_l); + DJIMotorStop(friction_r); + DJIMotorStop(loader); + break; + // 停止拨盘 + case LOAD_STOP: + DJIMotorOuterLoop(loader, SPEED_LOOP); + DJIMotorSetRef(loader, 0); + break; + // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入) + // 激活能量机关/干扰对方用,英雄用. + case LOAD_1_BULLET: + DJIMotorOuterLoop(loader, ANGLE_LOOP); + DJIMotorSetRef(loader, loader->motor_measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 增加一发弹丸 + break; + // 三连发,如果不需要后续可能删除 + case LOAD_3_BULLET: + DJIMotorOuterLoop(loader, ANGLE_LOOP); + DJIMotorSetRef(loader, loader->motor_measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发 + break; + // 连发模式,对速度闭环,射频后续修改为可变 + case LOAD_BURSTFIRE: + DJIMotorOuterLoop(loader, SPEED_LOOP); + DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO / NUM_PER_CIRCLE); + // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度 + break; + // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈) + // 可能需要从switch-case中独立出来 + case LOAD_REVERSE: + DJIMotorOuterLoop(loader, SPEED_LOOP); + // ... + break; + default: + break; + } + + // 根据收到的弹速设置设定摩擦轮参考值,需实测后填入 + switch (shoot_cmd_recv.bullet_speed) + { + case SMALL_AMU_15: + DJIMotorSetRef(friction_l, 0); + DJIMotorSetRef(friction_l, 0); + break; + case SMALL_AMU_18: + DJIMotorSetRef(friction_l, 0); + DJIMotorSetRef(friction_l, 0); + break; + case SMALL_AMU_30: + DJIMotorSetRef(friction_l, 0); + DJIMotorSetRef(friction_l, 0); + break; + default: + break; + } + + // 开关弹舱盖 + if (shoot_cmd_recv.lid_mode == LID_CLOSE) + { + //... + } + else if (shoot_cmd_recv.lid_mode == LID_OPEN) + { + //... + } } \ No newline at end of file diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index 073d972..81eb97b 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -19,17 +19,17 @@ #include "user_lib.h" static INS_t INS; -IMU_Param_t IMU_Param; -PID_t TempCtrl = {0}; +static IMU_Param_t IMU_Param; +static PID_t TempCtrl = {0}; const float xb[3] = {1, 0, 0}; const float yb[3] = {0, 1, 0}; const float zb[3] = {0, 0, 1}; // 用于获取两次采样之间的时间间隔 -uint32_t INS_DWT_Count = 0; +static uint32_t INS_DWT_Count = 0; static float dt = 0, t = 0; -float RefTemp = 40; // 恒温设定温度 +static float RefTemp = 40; // 恒温设定温度 static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]); diff --git a/modules/imu/ins_task.md b/modules/imu/ins_task.md index 4eb801a..26889f4 100644 --- a/modules/imu/ins_task.md +++ b/modules/imu/ins_task.md @@ -8,3 +8,7 @@ ## 算法解析 介绍EKF四元数姿态解算的教程在:[四元数EKF姿态更新算法](https://zhuanlan.zhihu.com/p/454155643) + + +## 模块移植 +由于历史遗留问题,IMU模块耦合程度高.后续实现BSP_SPI,将bmi088 middleware删除.仅保留BMI088读取的协议和寄存器定义等,单独实现IMU模块. \ No newline at end of file diff --git a/modules/monitor/monitor.c b/modules/monitor/monitor.c index 8a7d7ac..d3e4ad6 100644 --- a/modules/monitor/monitor.c +++ b/modules/monitor/monitor.c @@ -1 +1,3 @@ -#include "monitor.h" \ No newline at end of file +#include "monitor.h" +#include "bsp_dwt.h" + diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index 9d223ed..3534cca 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -1,10 +1,9 @@ #include "dji_motor.h" -#define PI2 3.141592f -#define ECD_ANGLE_COEF 0.043945f // 360/8192 +#define PI2 (3.141592f * 2) // 2pi +#define ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用 -static uint8_t stop_flag = 0; /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */ static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL}; @@ -196,6 +195,7 @@ void DJIMotorSetRef(dji_motor_instance *motor, float ref) void DJIMotorControl() { // 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销 + // 同样可以提高可读性 static uint8_t group, num; static int16_t set; static dji_motor_instance *motor; @@ -213,40 +213,51 @@ void DJIMotorControl() motor_controller = &motor->motor_controller; motor_measure = &motor->motor_measure; - // pid_ref会顺次通过被启用的环充当数据的载体 - if (motor_setting->close_loop_type & ANGLE_LOOP) // 计算位置环 + // pid_ref会顺次通过被启用的闭环充当数据的载体 + // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出 + if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP) { if (motor_setting->angle_feedback_source == OTHER_FEED) pid_measure = *motor_controller->other_angle_feedback_ptr; - else // MOTOR_FEED - pid_measure = motor_measure->total_angle; + else // MOTOR_FEED + pid_measure = motor_measure->total_angle; // 对total angle闭环,防止在边界处出现突跃 // 更新pid_ref进入下一个环 motor_controller->pid_ref = PID_Calculate(&motor_controller->angle_PID, pid_measure, motor_controller->pid_ref); } - if (motor_setting->close_loop_type & SPEED_LOOP) // 计算速度环 + // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环 + if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type | (ANGLE_LOOP | SPEED_LOOP))) { if (motor_setting->speed_feedback_source == OTHER_FEED) pid_measure = *motor_controller->other_speed_feedback_ptr; else pid_measure = motor_measure->speed_rpm; + // 更新pid_ref进入下一个环 motor_controller->pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, motor_controller->pid_ref); } - if (motor_setting->close_loop_type & CURRENT_LOOP) // 计算电流环 + // 计算电流环,只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身的反馈 + if (motor_setting->close_loop_type & CURRENT_LOOP) { motor_controller->pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->given_current, motor_controller->pid_ref); } - set = (int16_t)motor_controller->pid_ref; // 获取最终输出 - if (motor_setting->reverse_flag == MOTOR_DIRECTION_REVERSE) // 设置反转 - set *= -1; + // 获取最终输出 + set = (int16_t)motor_controller->pid_ref; + if (motor_setting->reverse_flag == MOTOR_DIRECTION_REVERSE) + set *= -1; // 设置反转 // 分组填入发送数据 group = motor->sender_group; num = motor->message_num; - sender_assignment[group].tx_buff[num] = 0xff & set >> 8; + sender_assignment[group].tx_buff[2 * num] = 0xff & set >> 8; sender_assignment[group].tx_buff[2 * num + 1] = 0xff & set; + + // 电机是否停止运行 + if (motor->stop_flag == MOTOR_STOP) + { // 若该电机处于停止状态,直接将buff置零 + memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u); + } } else // 遇到空指针说明所有遍历结束,退出循环 break; @@ -257,21 +268,22 @@ void DJIMotorControl() { if (sender_enable_flag[i]) { - if (stop_flag) // 如果紧急停止,将所有发送信息置零 - { - memset(sender_assignment[i].tx_buff, 0, 8); - } CANTransmit(&sender_assignment[i]); } } } -void DJIMotorStop() +void DJIMotorStop(dji_motor_instance *motor) { - stop_flag = 1; + motor->stop_flag = MOTOR_STOP; } -void DJIMotorEnable() +void DJIMotorEnable(dji_motor_instance *motor) { - stop_flag = 0; + motor->stop_flag = MOTOR_ENALBED; +} + +void DJIMotorOuterLoop(dji_motor_instance *motor, Closeloop_Type_e outer_loop) +{ + motor->motor_settings.outer_loop_type = outer_loop; } diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h index b2d083e..025cfb2 100644 --- a/modules/motor/dji_motor.h +++ b/modules/motor/dji_motor.h @@ -61,6 +61,8 @@ typedef struct Motor_Type_e motor_type; + Motor_Working_Type_e stop_flag; + } dji_motor_instance; /** @@ -106,16 +108,25 @@ void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedba void DJIMotorControl(); /** - * @brief 停止所有电机,注意不是将设定值设为零,而是直接给电机发送的电流值置零 + * @brief 停止电机,注意不是将设定值设为零,而是直接给电机发送的电流值置零 * */ -void DJIMotorStop(); +void DJIMotorStop(dji_motor_instance* motor); /** - * @brief 启动所有电机,此时电机会响应设定值 + * @brief 启动电机,此时电机会响应设定值 * 初始化时不需要此函数,因为stop_flag的默认值为0 * */ -void DJIMotorEnable(); +void DJIMotorEnable(dji_motor_instance* motor); + +/** + * @brief 修改电机闭环目标(外层闭环) + * + * @param motor 要修改的电机实例指针 + * @param outer_loop 外层闭环类型 + */ +void DJIMotorOuterLoop(dji_motor_instance *motor, Closeloop_Type_e outer_loop); + #endif // !DJI_MOTOR_H diff --git a/modules/motor/dji_motor.md b/modules/motor/dji_motor.md index ba532c7..92380c9 100644 --- a/modules/motor/dji_motor.md +++ b/modules/motor/dji_motor.md @@ -11,7 +11,15 @@ > 如果你不需要理解该模块的工作原理，你只需要查看这一小节。 -dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详尽的封装。你不再需要关心PID的计算以及CAN报文的发送和接收解析，你只需要专注于根据应用层的需求，设定合理的期望值，并通过`DJIMotorSetRef()`设置对应电机的输入参考即可。如果你希望更改电机的反馈来源，比如进入小陀螺模式（这时候你想要云台保持静止，使用IMU的yaw角度值作为反馈来源），只需要调用`DJIMotorChangeFeed()`，电机便可立刻切换反馈数据来源至IMU。 +dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详尽的封装。你不再需要关心PID的计算以及CAN报文的发送和接收解析，你只需要专注于根据应用层的需求，设定合理的期望值，并通过`DJIMotorSetRef()`设置对应电机的输入参考即可。 + +**==设定值的单位==** + +1. 位置环为角度，角度制。 +2. 速度环为角速度，单位为度/每秒 +3. + +如果你希望更改电机的反馈来源，比如进入小陀螺模式/视觉模式（这时候你想要云台保持静止，使用IMU的yaw角度值作为反馈来源），只需要调用`DJIMotorChangeFeed()`，电机便可立刻切换反馈数据来源至IMU。 要获得一个电机，请通过`DJIMotorInit()`并传入一些参数，他就会返回一个电机的指针。你也不再需要查看这些电机和电调的说明书，**只需要设置其电机id**（6020为拨码开关值，2006和3508为电调的闪动次数），该模块会自动为你计算CAN发送和接收ID并搞定所有硬件层的琐事。 @@ -41,9 +49,9 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详 CURRENT_LOOP SPEED_LOOP ANGLE_LOOP - CURRENT_LOOP| SPEED_LOOP // 同时对电流和速度闭环 - SPEED_LOOP | ANGLE_LOOP // 同时对速度和位置闭环 - CURRENT_LOOP| SPEED_LOOP |ANGLE_LOOP // 三环全开 + CURRENT_LOOP | SPEED_LOOP // 同时对电流和速度闭环 + SPEED_LOOP | ANGLE_LOOP // 同时对速度和位置闭环 + CURRENT_LOOP | SPEED_LOOP |ANGLE_LOOP // 三环全开 ``` - 是否反转 @@ -120,9 +128,11 @@ Motor_Init_Config_s config = { .motor_type = M3508, // 要注册的电机为3508电机 .can_init_config = {.can_handle = &hcan1, // 挂载在CAN1 .tx_id = 1}, // C620每隔一段时间闪动1次,设置为1 - // 采用电机编码器角度与速度反馈,启用速度环和电流环,不反转 + // 采用电机编码器角度与速度反馈,启用速度环和电流环,不反转,最外层闭环为速度环 .controller_setting_init_config = {.angle_feedback_source = MOTOR_FEED, - .close_loop_type = SPEED_LOOP | CURRENT_LOOP, + + .outer_loop_type = SPEED_LOOP, + .close_loop_type = SPEED_LOOP | CURRENT_LOOP, .speed_feedback_source = MOTOR_FEED, .reverse_flag = MOTOR_DIRECTION_NORMAL}, // 电流环和速度环PID参数的设置,不采用计算优化则不需要传入Improve参数 @@ -218,7 +228,9 @@ typedef struct /* sender assigment*/ uint8_t sender_group; uint8_t message_num; - + + uint8_t stop_flag; + Motor_Type_e motor_type; } dji_motor_instance; ``` @@ -233,6 +245,7 @@ typedef struct ```c typedef struct /* 电机控制配置 */ { + Closeloop_Type_e outer_loop_type; Closeloop_Type_e close_loop_type; Reverse_Flag_e reverse_flag; Feedback_Source_e angle_feedback_source; @@ -257,6 +270,9 @@ typedef struct ``` 以M3508为例，假设需要进行**速度闭环**和**电流闭环**，那么在初始化时就将这个变量的值设为`CURRENT_LOOP | SPEED_LOOP`。在`DJIMotorControl()`中，函数将会根据此标志位判断设定的参考值需要经过那些控制器的计算。 + 另外,你还需要设置当前电机的最外层闭环，即电机的闭环目标为什么类型的值。初始化时需要设置`outer_loop_type`。以M2006作为拨盘电机时为例，你希望它在单发/双发等固定发射数量的模式下对位置进行闭环（拨盘转过一定角度对应拨出一颗弹丸），但你也有可能希望在连发的时候让拨盘连续的转动，以一定的频率发射弹丸。我们提供了`DJIMotorOuterLoop()`用于修改电机的外层闭环，改变电机的闭环对象。 + + > 注意，务必分清串级控制（多环）和外层闭环的区别。前者是为了提高内环的性能，使得其能更好地跟随外环参考值；而后者描述的是系统真实的控制目标（闭环目标）。如3508，没有电流环仍然可以对速度完成闭环，对于高层的应用来说，它们本质上不关心电机内部是否还有电流环，它们只把外层闭环为速度的电机当作一个**速度伺服执行器**，**外层闭环**描述的就是真正的闭环目标。 - 为了避开恼人的正负号，提高代码的可维护性，在初始化电机时设定`reverse_flag`使得所有电机都按照你想要的方向旋转，其定义如下： @@ -321,6 +337,12 @@ void DJIMotorChangeFeed(dji_motor_instance *motor, Feedback_Source_e type); void DJIMotorControl(); + +void DJIMotorStop(dji_motor_instance *motor); + +void DJIMotorEnable(dji_motor_instance *motor); + +void DJIMotorOuterLoop(dji_motor_instance *motor); ``` - `DJIMotorInit()`是用于初始化电机对象的接口，传入包括电机can配置、电机控制配置、电机控制器配置以及电机类型在内的初始化参数。**它将会返回一个电机实例指针**，你应当在应用层保存这个指针，这样才能操控这个电机。 @@ -337,6 +359,12 @@ void DJIMotorControl(); 2. 根据反转标志位，确定是否将输出反转 3. 根据每个电机的发送分组，将最终输出值填入对应的分组buff 4. 检查每一个分组，若该分组有电机，发送报文 + +- `DJIMotorStop()`和`DJIMotorEnable()`用于控制电机的启动和停止。当电机被设为stop的时候，不会响应任何的参考输入。 + +- `DJIMotorOuterLoop()`用于修改电机的外部闭环类型，即电机的真实闭环目标。 + + ## 私有函数和变量 @@ -350,7 +378,7 @@ static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL}; 这是管理所有电机实例的入口。idx用于电机初始化。 ```c -#define PI2 3.141592f +#define PI2 (3.141592f * 2) #define ECD_ANGLE_COEF 3.835e-4 // ecd/8192*pi ``` @@ -404,6 +432,7 @@ Motor_Init_Config_s config = { }, .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, + .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .speed_feedback_source = MOTOR_FEED, .reverse_flag = MOTOR_DIRECTION_NORMAL @@ -436,4 +465,4 @@ dji_motor_instance *djimotor = DJIMotorInit(&config); DJIMotorSetRef(djimotor, 10); ``` -前提是已经将`MotorTask()`放入实时系统任务当中。你也可以单独执行`MotorControl()`。 \ No newline at end of file +前提是已经将`DJIMotorControl()`放入实时系统任务当中或以一定d。你也可以单独执行`DJIMotorControl()`。 \ No newline at end of file diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index e489656..0f109d1 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -17,7 +17,6 @@ #define LIMIT_MIN_MAX(x, min, max) (x) = (((x) <= (min)) ? (min) : (((x) >= (max)) ? (max) : (x))) - /** * @brief 闭环类型,如果需要多个闭环,则使用或运算 * 例如需要速度环和电流环: CURRENT_LOOP|SPEED_LOOP @@ -49,13 +48,20 @@ typedef enum MOTOR_DIRECTION_REVERSE = 1 } Reverse_Flag_e; +typedef enum +{ + MOTOR_STOP = 0, + MOTOR_ENALBED = 1, +} Motor_Working_Type_e; + /* 电机控制设置,包括闭环类型,反转标志和反馈来源 */ typedef struct { - Closeloop_Type_e close_loop_type; - Reverse_Flag_e reverse_flag; - Feedback_Source_e angle_feedback_source; - Feedback_Source_e speed_feedback_source; + Closeloop_Type_e outer_loop_type; // 最外层的闭环,未设置时默认为最高级的闭环 + Closeloop_Type_e close_loop_type; // 使用几个闭环(串级) + Reverse_Flag_e reverse_flag; // 是否反转 + Feedback_Source_e angle_feedback_source; // 角度反馈类型 + Feedback_Source_e speed_feedback_source; // 速度反馈类型 } Motor_Control_Setting_s; @@ -63,9 +69,9 @@ typedef struct // 后续增加前馈数据指针 typedef struct { - float *other_angle_feedback_ptr; + float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针 float *other_speed_feedback_ptr; - // float *angle_foward_ptr; + // float *angle_foward_ptr; //前馈数据指针 // float *speed_foward_ptr; // float *current_foward_ptr; @@ -83,7 +89,7 @@ typedef enum M3508 = 1, M2006 = 2, LK9025 = 3, - HT04 = 4 + HT04 = 4, } Motor_Type_e; /**