From 8e32fc0e6fdea4774f4a3f2d78c3f5dd030d9473 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Sun, 4 Dec 2022 14:35:42 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=BA=E7=94=B5=E6=9C=BA=E5=A2=9E=E5=8A=A0?= =?UTF-8?q?=E4=BA=86=E5=90=AF=E5=8A=A8=E5=92=8C=E5=81=9C=E6=AD=A2=E6=A8=A1?= =?UTF-8?q?=E5=BC=8F,=E5=AE=8C=E6=88=90=E4=BA=86gimbal=E7=9A=84=E6=A1=86?= =?UTF-8?q?=E6=9E=B6,=E6=A8=A1=E5=BC=8F=E8=BF=87=E6=B8=A1=E5=B0=9A?= =?UTF-8?q?=E6=9C=AA=E5=AE=8C=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 23 ++++----- application/gimbal/gimbal.c | 96 +++++++++++++++++++++++++++++------ application/robot_def.h | 10 ++-- modules/imu/ins_task.h | 1 + modules/motor/dji_motor.c | 19 ++++--- modules/motor/dji_motor.h | 7 +++ modules/motor/motor_def.h | 4 ++ 7 files changed, 122 insertions(+), 38 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index b38a0d6..67ff38c 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -207,19 +207,6 @@ void ChassisTask() // 获取新的控制信息 SubGetMessage(chassis_sub, &chassis_cmd_recv); - // 如果出现重要模块离线或遥控器设置为急停,让电机停止 - if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) - { - DJIMotorStop(); - } - - // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 - // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东) - chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle) - - chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle); - chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle) - - chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle); - // 根据控制模式设定旋转速度 switch (chassis_cmd_recv.chassis_mode) { @@ -232,10 +219,20 @@ void ChassisTask() case CHASSIS_ROTATE: // chassis_cmd_recv.wz // 当前维持定值,后续增加不规则的变速策略 break; + case CHASSIS_ZERO_FORCE: + DJIMotorStop(); // 如果出现重要模块离线或遥控器设置为急停,让电机停止 + break; default: break; } + // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 + // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东) + chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle) - + chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle); + chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle) - + chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle); + // 根据控制模式进行正运动学解算,计算底盘输出 MecanumCalculate(); diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 37b32c1..d4fa6b9 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -4,22 +4,22 @@ #include "ins_task.h" #include "message_center.h" +// 需要将控制值统一修改为角度制而不是编码器ECD值 +#define YAW_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值 +#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值 -#define YAW_ALIGN_ECD 0 +static attitude_t *Gimbal_IMU_data; // 云台IMU数据 +static dji_motor_instance *yaw_motor; // yaw电机 +static dji_motor_instance *pitch_motor; // pitch电机 -static attitude_t* Gimbal_IMU_data; // 云台IMU数据 -static dji_motor_instance* yaw_motor; // yaw电机 -static dji_motor_instance *pitch_motor; // pitch电机 - -static Publisher_t* gimbal_pub; +static Publisher_t *gimbal_pub; static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给gimbal_cmd的云台状态信息 -static Subscriber_t* gimbal_sub; -static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息 - +static Subscriber_t *gimbal_sub; +static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息 void GimbalInit() { - Gimbal_IMU_data=INS_Init(); + Gimbal_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW Motor_Init_Config_s yaw_config = { @@ -36,7 +36,9 @@ void GimbalInit() .speed_PID = { }, - .other_angle_feedback_ptr=&Gimbal_IMU_data->YawTotalAngle, + .other_angle_feedback_ptr = &Gimbal_IMU_data->YawTotalAngle, + // 还需要增加角速度额外反馈指针 + // .other_speed_feedback_ptr=&Gimbal_IMU_data.wz; }, .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, @@ -60,6 +62,9 @@ void GimbalInit() .speed_PID = { }, + .other_angle_feedback_ptr = &Gimbal_IMU_data->Pitch, + // 还需要增加角速度额外反馈指针 + // .other_speed_feedback_ptr=&Gimbal_IMU_data.wy, }, .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, @@ -69,13 +74,74 @@ void GimbalInit() }, .motor_type = GM6020}; - yaw_motor=DJIMotorInit(&yaw_config); - pitch_motor=DJIMotorInit(&pitch_config); + yaw_motor = DJIMotorInit(&yaw_config); + pitch_motor = DJIMotorInit(&pitch_config); - gimbal_pub=PubRegister("gimbal_feed",sizeof(Gimbal_Upload_Data_s)); - gimbal_sub=SubRegister("gimbal_cmd",sizeof(Gimbal_Ctrl_Cmd_s)); + gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); + gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); } +// /** +// * @brief +// * +// */ +// static void TransitionMode() +// { + +// } + void GimbalTask() { + // 获取云台控制数据 + // 后续增加未收到数据的处理 + SubGetMessage(gimbal_sub, &gimbal_cmd_recv); + + // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref + // 是否要增加不同模式之间的过渡? + switch (gimbal_cmd_recv.gimbal_mode) + { + case GIMBAL_ZERO_FORCE: + DJIMotorStop(); + break; + case GIMBAL_GYRO_MODE: + DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); + 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); + 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); + break; + default: + break; + } + // 过渡示例: + /* 需要给每个case增加如下判断,并添加一个过渡行为函数和过渡标志位 + case xxx: + if(last_mode!=xxx) + { + transition_flag=1; + } + break; + + void TransitMode() + { + motor_output=lpf_coef * last_output+(1 - lpf_coef) * concur_output; + } + */ + + // 获取反馈数据 + 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); } \ No newline at end of file diff --git a/application/robot_def.h b/application/robot_def.h index 967ea1f..b4a2b8c 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -53,6 +53,10 @@ typedef enum } App_Status_e; // 底盘模式设置 +/** + * @brief 后续考虑修改为云台跟随底盘,而不是让底盘去追云台,云台的惯量比底盘小. + * + */ typedef enum { CHASSIS_ZERO_FORCE, // 电流零输入 @@ -65,9 +69,8 @@ typedef enum typedef enum { GIMBAL_ZERO_FORCE, // 电流零输入 - GIMBAL_FREE_MODE, // 云台自由运动模式，反馈值为电机total_angle - GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式，反馈值为陀螺仪pitch,total_yaw_angle - GIMBAL_VISUAL_MODE, // 视觉模式，反馈值为陀螺仪，输入为视觉数据 + GIMBAL_FREE_MODE, // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle + GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式 } gimbal_mode_e; // 发射模式设置 @@ -173,6 +176,7 @@ typedef struct typedef struct { attitude_t gimbal_imu_data; + uint16_t yaw_motor_ecd; } Gimbal_Upload_Data_s; typedef struct diff --git a/modules/imu/ins_task.h b/modules/imu/ins_task.h index f8c9212..80c6768 100644 --- a/modules/imu/ins_task.h +++ b/modules/imu/ins_task.h @@ -27,6 +27,7 @@ typedef struct { + // 还需要增加角速度数据 float Roll; float Pitch; float Yaw; diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index 57a1d96..9d223ed 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -4,7 +4,7 @@ #define ECD_ANGLE_COEF 0.043945f // 360/8192 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用 -static uint8_t stop_flag=0; +static uint8_t stop_flag = 0; /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */ static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL}; @@ -123,7 +123,7 @@ static void DecodeDJIMotor(can_instance *_instance) { // 由于需要多次变址访存,直接将buff和measure地址保存在寄存器里避免多次存取 static uint8_t *rxbuff; - static dji_motor_measure *measure; + static dji_motor_measure *measure; for (size_t i = 0; i < DJI_MOTOR_CNT; i++) { @@ -169,7 +169,7 @@ dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config) 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); + dji_motor_info[idx]->motor_can_instance = CANRegister(&config->can_init_config); return dji_motor_info[idx++]; } @@ -246,7 +246,7 @@ void DJIMotorControl() group = motor->sender_group; num = motor->message_num; sender_assignment[group].tx_buff[num] = 0xff & set >> 8; - sender_assignment[group].tx_buff[2*num + 1] = 0xff & set; + sender_assignment[group].tx_buff[2 * num + 1] = 0xff & set; } else // 遇到空指针说明所有遍历结束,退出循环 break; @@ -257,9 +257,9 @@ void DJIMotorControl() { if (sender_enable_flag[i]) { - if(stop_flag) // 如果紧急停止,将所有发送信息置零 + if (stop_flag) // 如果紧急停止,将所有发送信息置零 { - memset(sender_assignment[i].tx_buff,0,8); + memset(sender_assignment[i].tx_buff, 0, 8); } CANTransmit(&sender_assignment[i]); } @@ -268,5 +268,10 @@ void DJIMotorControl() void DJIMotorStop() { - stop_flag=1; + stop_flag = 1; +} + +void DJIMotorEnable() +{ + stop_flag = 0; } diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h index e209a8d..b2d083e 100644 --- a/modules/motor/dji_motor.h +++ b/modules/motor/dji_motor.h @@ -111,4 +111,11 @@ void DJIMotorControl(); */ void DJIMotorStop(); +/** + * @brief 启动所有电机,此时电机会响应设定值 + * 初始化时不需要此函数,因为stop_flag的默认值为0 + * + */ +void DJIMotorEnable(); + #endif // !DJI_MOTOR_H diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index 16835ed..e489656 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -60,10 +60,14 @@ typedef struct } Motor_Control_Setting_s; /* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/ +// 后续增加前馈数据指针 typedef struct { float *other_angle_feedback_ptr; float *other_speed_feedback_ptr; + // float *angle_foward_ptr; + // float *speed_foward_ptr; + // float *current_foward_ptr; PID_t current_PID; PID_t speed_PID;