From 8e7935876ebbdccbb519cb36cbdd1dd793530cef Mon Sep 17 00:00:00 2001 From: NeoZng Date: Sun, 4 Dec 2022 23:15:18 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E9=80=9A=E7=94=A8=E5=AE=9A?= =?UTF-8?q?=E4=B9=89=E5=A4=B4=E6=96=87=E4=BB=B6,=E4=B8=BB=E8=A6=81?= =?UTF-8?q?=E5=86=85=E5=AE=B9=E6=98=AF=E8=A7=92=E5=BA=A6=E8=BD=AC=E6=8D=A2?= =?UTF-8?q?.=E7=BB=9F=E4=B8=80=E6=8E=A7=E5=88=B6=E7=9A=84=E8=BE=93?= =?UTF-8?q?=E5=85=A5,=E4=B8=BA=E5=90=8E=E7=BB=ADmodel-based=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E5=81=9A=E5=87=86=E5=A4=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Makefile | 6 ++-- application/chassis/chassis.c | 1 + application/cmd/robot_cmd.c | 1 + application/gimbal/gimbal.c | 37 ++++++++++++---------- application/robot_def.h | 4 --- application/shoot/shoot.c | 33 ++++++++++++-------- application/shoot/shoot.h | 2 +- modules/general_def.h | 13 ++++++++ modules/motor/dji_motor.c | 58 +++++++++++++++++++---------------- modules/motor/dji_motor.h | 31 +++++++++---------- modules/motor/dji_motor.md | 20 ++++++++++-- modules/motor/servo_motor.c | 0 modules/motor/servo_motor.h | 0 modules/motor/servo_motor.md | 0 14 files changed, 126 insertions(+), 80 deletions(-) create mode 100644 modules/general_def.h create mode 100644 modules/motor/servo_motor.c create mode 100644 modules/motor/servo_motor.h create mode 100644 modules/motor/servo_motor.md diff --git a/Makefile b/Makefile index 4cacdb9..d065732 100644 --- a/Makefile +++ b/Makefile @@ -121,7 +121,8 @@ modules/master_machine/seasky_protocol.c \ modules/motor/dji_motor.c \ modules/motor/HT04.c \ modules/motor/LK9025.c \ -modules/mode/step_motor.c \ +modules/motor/step_motor.c \ +modules/motor/servo_motor.c \ modules/motor/motor_task.c \ modules/referee/crc.c \ modules/referee/referee.c \ @@ -228,7 +229,8 @@ C_INCLUDES = \ -Imodules/super_cap \ -Imodules/can_comm \ -Imodules/message_center \ --Imodules/monitor +-Imodules/monitor \ +-Imodules/ # compile gcc flags ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index d8a9b42..f4e4d70 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -17,6 +17,7 @@ #include "super_cap.h" #include "message_center.h" #include "referee.h" +#include "general_def.h" #include "arm_math.h" diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 79a2633..3a3a610 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -4,6 +4,7 @@ #include "ins_task.h" #include "master_process.h" #include "message_center.h" +#include "general_def.h" /* gimbal_cmd应用包含的模块实例指针和交互信息存储*/ #ifndef ONE_BOARD diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 213fcfb..fbad603 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -3,10 +3,15 @@ #include "dji_motor.h" #include "ins_task.h" #include "message_center.h" +#include "general_def.h" -// 需要将控制值统一修改为角度制而不是编码器ECD值 -#define YAW_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值 -#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值 +/* 根据每个机器人进行设定,若对云台有改动需要修改 */ +#define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值 +#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值 + +// 自动将编码器转换成角度值 +#define YAW_CHASSIS_ALIGN_ANGLE +#define PTICH_HORIZON_ALIGN_ANGLE static attitude_t *Gimbal_IMU_data; // 云台IMU数据 static dji_motor_instance *yaw_motor; // yaw电机 @@ -43,7 +48,7 @@ void GimbalInit() .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, - .outer_loop_type=ANGLE_LOOP, + .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP, .reverse_flag = MOTOR_DIRECTION_REVERSE, }, @@ -70,7 +75,7 @@ void GimbalInit() .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, - .outer_loop_type=ANGLE_LOOP, + .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP, .reverse_flag = MOTOR_DIRECTION_REVERSE, }, @@ -84,8 +89,8 @@ void GimbalInit() } // /** -// * @brief -// * +// * @brief +// * // */ // static void TransitionMode() // { @@ -111,17 +116,17 @@ void GimbalTask() DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw); - DJIMotorSetRef(pitch_motor,gimbal_cmd_recv.pitch); + DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); + DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); break; - //是否考虑直接让云台和底盘重合,其他模式都使用陀螺仪反馈? + // 是否考虑直接让云台和底盘重合,其他模式都使用陀螺仪反馈? case GIMBAL_FREE_MODE: DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, MOTOR_FEED); DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, MOTOR_FEED); DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, MOTOR_FEED); DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, MOTOR_FEED); - DJIMotorSetRef(yaw_motor,YAW_ALIGN_ECD); - DJIMotorSetRef(pitch_motor,PITCH_HORIZON_ECD); + DJIMotorSetRef(yaw_motor, YAW_ALIGN_ECD); + DJIMotorSetRef(pitch_motor, PITCH_HORIZON_ECD); break; default: break; @@ -142,9 +147,9 @@ void GimbalTask() */ // 获取反馈数据 - gimbal_feedback_data.gimbal_imu_data=*Gimbal_IMU_data; - gimbal_feedback_data.yaw_motor_ecd=pitch_motor->motor_measure.ecd; - + gimbal_feedback_data.gimbal_imu_data = *Gimbal_IMU_data; + gimbal_feedback_data.yaw_motor_ecd = pitch_motor->motor_measure.ecd; + // 推送消息 - PubPushMessage(gimbal_pub,&gimbal_feedback_data); + PubPushMessage(gimbal_pub, &gimbal_feedback_data); } \ No newline at end of file diff --git a/application/robot_def.h b/application/robot_def.h index 9cdcfd3..5e5b980 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -12,10 +12,6 @@ #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) - #include "ins_task.h" #include "master_process.h" #include "stdint-gcc.h" diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 8ddc090..e5d07b9 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -3,6 +3,7 @@ #include "dji_motor.h" #include "message_center.h" #include "bsp_dwt.h" +#include "general_def.h" #define ONE_BULLET_DELTA_ANGLE 0 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define REDUCTION_RATIO 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f @@ -19,9 +20,8 @@ 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; +// dwt定时,计算冷却用 +static float hibernate_time = 0, dead_time = 0; void ShootInit() { @@ -125,30 +125,39 @@ void ShootTask() // 从cmd获取控制数据 SubGetMessage(shoot_sub, &shoot_cmd_recv); - // 根据控制模式进行电机参考值设定和模式切换 - switch (shoot_cmd_recv.load_mode) + // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机 + if (shoot_cmd_recv.load_mode == SHOOT_STOP) { - // 停止三个电机 - case SHOOT_STOP: DJIMotorStop(friction_l); DJIMotorStop(friction_r); DJIMotorStop(loader); - break; + } + + // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可 + if (hibernate_time + dead_time > DWT_GetTimeline_ms()) + return; + + // 若不在休眠状态,根据控制模式进行电机参考值设定和模式切换 + switch (shoot_cmd_recv.load_mode) + { // 停止拨盘 case LOAD_STOP: DJIMotorOuterLoop(loader, SPEED_LOOP); DJIMotorSetRef(loader, 0); break; - // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入) - // 激活能量机关/干扰对方用,英雄用. - case LOAD_1_BULLET: + // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入)F + case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用. DJIMotorOuterLoop(loader, ANGLE_LOOP); - DJIMotorSetRef(loader, loader->motor_measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 增加一发弹丸 + DJIMotorSetRef(loader, loader->motor_measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 增加一发弹丸的角度 + hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 + dead_time = 150; // 完成1发弹丸发射的时间 break; // 三连发,如果不需要后续可能删除 case LOAD_3_BULLET: DJIMotorOuterLoop(loader, ANGLE_LOOP); DJIMotorSetRef(loader, loader->motor_measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发 + hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 + dead_time = 300; // 完成3发弹丸发射的时间 break; // 连发模式,对速度闭环,射频后续修改为可变 case LOAD_BURSTFIRE: diff --git a/application/shoot/shoot.h b/application/shoot/shoot.h index a6463d1..56c97b4 100644 --- a/application/shoot/shoot.h +++ b/application/shoot/shoot.h @@ -5,4 +5,4 @@ void ShootInit(); void ShootTask(); -#endif //SHOOT_H \ No newline at end of file +#endif // SHOOT_H \ No newline at end of file diff --git a/modules/general_def.h b/modules/general_def.h new file mode 100644 index 0000000..b8cbf11 --- /dev/null +++ b/modules/general_def.h @@ -0,0 +1,13 @@ +#ifndef GENERAL_DEF_H +#define GENERAL_DEF_H + +#define PI 3.1415926535f +#define PI2 (PI * 2.0f) // 2 pi + +#define RAD_2_ANGLE (180.0f / PI) +#define ANGLE_2_RAD (PI / 180.0f) + +#define RPM_2_ANGLE_PER_SEC (360.0f/60.0f) // ×360°/60sec + + +#endif // !GENERAL_DEF_H \ No newline at end of file diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index 3534cca..ee45e8d 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -1,6 +1,6 @@ #include "dji_motor.h" +#include "general_def.h" -#define PI2 (3.141592f * 2) // 2pi #define ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用 @@ -129,20 +129,24 @@ static void DecodeDJIMotor(can_instance *_instance) if (dji_motor_info[i]->motor_can_instance == _instance) { rxbuff = _instance->rx_buff; - measure = &dji_motor_info[i]->motor_measure; + measure = &dji_motor_info[i]->motor_measure; // measure要多次使用,保存指针减小访存开销 + // resolve data and apply filter to current and speed measure->last_ecd = measure->ecd; measure->ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]); - // 增加滤波 - measure->speed_rpm = (1 - SPEED_SMOOTH_COEF) * measure->speed_rpm + SPEED_SMOOTH_COEF * (int16_t)(rxbuff[2] << 8 | rxbuff[3]); - measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]); + measure->angle_single_round = ECD_ANGLE_COEF* measure->ecd; + measure->speed_angle_per_sec = (1 - SPEED_SMOOTH_COEF) * measure->speed_angle_per_sec + + RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (int16_t)(rxbuff[2] << 8 | rxbuff[3]); + measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + + RPM_2_ANGLE_PER_SEC * CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]); measure->temperate = rxbuff[6]; - // multi round calc - if (measure->ecd - measure->last_ecd > 4096) + + // multi rounds calc,计算的前提是两次采样间电机转过的角度小于180° + if (measure->ecd - measure->last_ecd > 4096) measure->total_round--; else if (measure->ecd - measure->last_ecd < -4096) measure->total_round++; - measure->total_angle = measure->total_round * 360 + measure->ecd * ECD_ANGLE_COEF; // @todo simplify the calculation + measure->total_angle = measure->total_round * 360 + measure->angle_single_round; break; } } @@ -164,8 +168,10 @@ dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config) PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID); dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; + // group motors, because 4 motors share the same CAN control message MotorSenderGrouping(&config->can_init_config); + // register motor to CAN bus config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback dji_motor_info[idx]->motor_can_instance = CANRegister(&config->can_init_config); @@ -173,7 +179,6 @@ dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config) return dji_motor_info[idx++]; } -// 改变反馈来源 void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedback_Source_e type) { if (loop == ANGLE_LOOP) @@ -186,12 +191,28 @@ void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedba } } +void DJIMotorStop(dji_motor_instance *motor) +{ + motor->stop_flag = MOTOR_STOP; +} + +void DJIMotorEnable(dji_motor_instance *motor) +{ + motor->stop_flag = MOTOR_ENALBED; +} + +void DJIMotorOuterLoop(dji_motor_instance *motor, Closeloop_Type_e outer_loop) +{ + motor->motor_settings.outer_loop_type = outer_loop; +} + // 设置参考值 void DJIMotorSetRef(dji_motor_instance *motor, float ref) { motor->motor_controller.pid_ref = ref; } -// 计算三环PID,发送控制报文 + +// 为所有电机实例计算三环PID,发送控制报文 void DJIMotorControl() { // 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销 @@ -231,7 +252,7 @@ void DJIMotorControl() if (motor_setting->speed_feedback_source == OTHER_FEED) pid_measure = *motor_controller->other_speed_feedback_ptr; else - pid_measure = motor_measure->speed_rpm; + pid_measure = motor_measure->speed_angle_per_sec; // 更新pid_ref进入下一个环 motor_controller->pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, motor_controller->pid_ref); } @@ -272,18 +293,3 @@ void DJIMotorControl() } } } - -void DJIMotorStop(dji_motor_instance *motor) -{ - motor->stop_flag = MOTOR_STOP; -} - -void DJIMotorEnable(dji_motor_instance *motor) -{ - 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 025cfb2..1adfb74 100644 --- a/modules/motor/dji_motor.h +++ b/modules/motor/dji_motor.h @@ -22,19 +22,20 @@ #define DJI_MOTOR_CNT 12 /* 滤波系数设置为1的时候即关闭滤波 */ -#define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.8 -#define CURRENT_SMOOTH_COEF 0.98f // this coef must be greater than 0.95 +#define SPEED_SMOOTH_COEF 0.9f // better to be greater than 0.85 +#define CURRENT_SMOOTH_COEF 0.98f // this coef *must* be greater than 0.95 /* DJI电机CAN反馈信息*/ typedef struct { - uint16_t ecd; // 0-8192 - uint16_t last_ecd; - int16_t speed_rpm; // rounds per minute - int16_t given_current; // 实际电流 - uint8_t temperate; - int16_t total_round; // 总圈数,注意方向 - int32_t total_angle; // 总角度,注意方向 + uint16_t ecd; // 0-8191,刻度总共有8192格 + uint16_t last_ecd; // 上一次读取的编码器值 + float angle_single_round; // 单圈角度 + float speed_angle_per_sec; // 度/秒 rounds per minute + int16_t given_current; // 实际电流 + uint8_t temperate; // 温度 Celsius + int16_t total_round; // 总圈数,注意方向 + int32_t total_angle; // 总角度,注意方向 } dji_motor_measure; /** @@ -59,9 +60,8 @@ typedef struct uint8_t sender_group; uint8_t message_num; - Motor_Type_e motor_type; - - Motor_Working_Type_e stop_flag; + Motor_Type_e motor_type; // 电机类型 + Motor_Working_Type_e stop_flag; // 启停标志 } dji_motor_instance; @@ -111,22 +111,21 @@ void DJIMotorControl(); * @brief 停止电机,注意不是将设定值设为零,而是直接给电机发送的电流值置零 * */ -void DJIMotorStop(dji_motor_instance* motor); +void DJIMotorStop(dji_motor_instance *motor); /** * @brief 启动电机,此时电机会响应设定值 * 初始化时不需要此函数,因为stop_flag的默认值为0 * */ -void DJIMotorEnable(dji_motor_instance* motor); +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 92380c9..3f048d8 100644 --- a/modules/motor/dji_motor.md +++ b/modules/motor/dji_motor.md @@ -15,9 +15,17 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详 **==设定值的单位==** -1. 位置环为角度，角度制。 -2. 速度环为角速度，单位为度/每秒 -3. +1. ==位置环为**角度制**（0-360，total_angle可以为任意值）== + +2. ==速度环为角速度，单位为**度/每秒**（deg/sec）== + +3. ==电流环为mA== + +4. ==GM6020的输入设定为**力矩**，待测量（-30000~30000）== + + ==M3508的输入设定为-20A~20A （-16384~16384）== + + ==M2006的输入设定为-10A~10A （-10000~10000）== 如果你希望更改电机的反馈来源，比如进入小陀螺模式/视觉模式（这时候你想要云台保持静止，使用IMU的yaw角度值作为反馈来源），只需要调用`DJIMotorChangeFeed()`，电机便可立刻切换反馈数据来源至IMU。 @@ -419,6 +427,12 @@ static void DecodeDJIMotor(can_instance *_instance) - `DecodeDJIMotor()`是解析电机反馈报文的函数，在`DJIMotorInit()`中会将其注册到该电机实例对应的`can_instance`中（即`can_instance`的`can_module_callback()`）。这样，当该电机的反馈报文到达时，`bsp_can.c`中的回调函数会调用解包函数进行反馈数据解析。 该函数还会对电流和速度反馈值进行滤波，消除高频噪声；同时计算多圈角度和单圈绝对角度。 + + **电机反馈的电流值为说明书中的映射值，需转换为实际值。** + + **反馈的速度单位是rpm（转每分钟），转换为角度每秒。** + + **反馈的位置是编码器值（0~8191），转换为角度。** ## 使用范例 diff --git a/modules/motor/servo_motor.c b/modules/motor/servo_motor.c new file mode 100644 index 0000000..e69de29 diff --git a/modules/motor/servo_motor.h b/modules/motor/servo_motor.h new file mode 100644 index 0000000..e69de29 diff --git a/modules/motor/servo_motor.md b/modules/motor/servo_motor.md new file mode 100644 index 0000000..e69de29