From 40cd575e91861e91b0d9b80400ee713fe63da26f Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Wed, 13 Mar 2024 20:11:44 +0800 Subject: [PATCH] =?UTF-8?q?EC=E7=94=B5=E6=9C=BA=E5=A2=9E=E5=8A=A0=E5=A4=9A?= =?UTF-8?q?=E5=9C=88=E8=A7=92=E5=BA=A6=E5=A4=84=E7=90=86=20=E5=A2=9E?= =?UTF-8?q?=E5=8A=A0=E5=A4=A7yaw=E8=BD=B4=E7=9B=B8=E5=AF=B9=E8=A7=92?= =?UTF-8?q?=E5=BA=A6=E8=AE=A1=E7=AE=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 8 ++++---- application/gimbal/gimbal.c | 17 +++++++++++++---- modules/motor/ECmotor/ECA8210.c | 10 ++++++++-- modules/motor/ECmotor/ECA8210.h | 3 ++- 4 files changed, 27 insertions(+), 11 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 954d667..7521f0a 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -267,10 +267,10 @@ void ChassisTask() case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态 //chassis_cmd_recv.wz = 0; - chassis_cmd_recv.wz = 100.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); + //chassis_cmd_recv.wz = 100.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); break; case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 - chassis_cmd_recv.wz = 1.5f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); + //chassis_cmd_recv.wz = 1.5f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); break; case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 //chassis_cmd_recv.wz = 4000 * 10; @@ -290,8 +290,8 @@ void ChassisTask() // 根据控制模式进行正运动学解算,计算底盘输出 - MecanumCalculate(); - //OmniCalculate(); + //MecanumCalculate(); + OmniCalculate(); //vt_rf = 5000; // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值 LimitChassisOutput(); diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index c831bfb..84a5b84 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -143,8 +143,8 @@ void GimbalInit() .controller_setting_init_config = { .angle_feedback_source = OTHER_FEED, .speed_feedback_source = OTHER_FEED, - .outer_loop_type = ANGLE_LOOP, - .close_loop_type = ANGLE_LOOP|SPEED_LOOP|CURRENT_LOOP, + .outer_loop_type = OPEN_LOOP, + .close_loop_type = OPEN_LOOP,//ANGLE_LOOP|SPEED_LOOP|CURRENT_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = ECA8210, @@ -178,12 +178,16 @@ void GimbalTask() case GIMBAL_ZERO_FORCE: DJIMotorStop(yaw_motor); DJIMotorStop(pitch_motor); + ECMotorStop(big_yaw_motor); break; // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 case GIMBAL_GYRO_MODE: // 后续只保留此模式 DJIMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); - ECMotorEnable(big_yaw_motor); + ECMotorEnable(big_yaw_motor); +// DJIMotorStop(yaw_motor); +// DJIMotorStop(pitch_motor); +// ECMotorStop(big_yaw_motor); DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); @@ -238,7 +242,12 @@ void GimbalTask() // 设置反馈数据,主要是imu和yaw的ecd gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; - gimbal_feedback_data.yaw_motor_single_round_angle = loop_float_constrain(big_yaw_motor->measure.angle_single_round, -PI, PI); + + static float big_yaw_relative_angle; + + big_yaw_relative_angle =big_yaw_motor->measure.total_angle * (4.0f/3.0f); + + gimbal_feedback_data.yaw_motor_single_round_angle = loop_float_constrain(big_yaw_relative_angle,-PI,PI); gimbal_feedback_data.mini_yaw_encode_angle = yaw_motor->measure.angle_single_round; // 推送消息 diff --git a/modules/motor/ECmotor/ECA8210.c b/modules/motor/ECmotor/ECA8210.c index a6190f3..44f1ffe 100644 --- a/modules/motor/ECmotor/ECA8210.c +++ b/modules/motor/ECmotor/ECA8210.c @@ -43,14 +43,20 @@ static void ECMotorDecode(CANInstance *_instance) DaemonReload(motor->daemon); // 喂狗 measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt); - - measure->angle_single_round = (float)(ECMotor_Report.angle_raw-32768) * 0.0003814697f; + measure->last_angle = measure->angle_single_round; + measure->angle_single_round = (float)(ECMotor_Report.angle_raw-32768) * 0.0003814697f; //总角度 单位rad +-12.5f //measure->speed_rads = (float)(ECMotor_Report.speed_raw-2048) * 0.008789f; measure->real_current = (float)(ECMotor_Report.current_raw-2048) * 0.014648f; measure->temperature = (ECMotor_Report.temperature_raw - 50) /2; measure->speed_rads = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_rads + SPEED_SMOOTH_COEF * (float)(ECMotor_Report.speed_raw-2048) * 0.008789f; + + if((measure->angle_single_round - measure->last_angle) >= 20.0f) + measure->total_round -- ; + else if ((measure->angle_single_round - measure->last_angle) <= -20.0f) + measure->total_round ++; + measure->total_angle = measure->total_round * 25.0f + measure->angle_single_round; } static void ECMotorLostCallback(void *motor_ptr) diff --git a/modules/motor/ECmotor/ECA8210.h b/modules/motor/ECmotor/ECA8210.h index 8aaac21..f22ac9b 100644 --- a/modules/motor/ECmotor/ECA8210.h +++ b/modules/motor/ECmotor/ECA8210.h @@ -57,7 +57,8 @@ typedef struct // EC-A8120 { uint16_t last_ecd; // 上一次读取的编码器值 uint16_t ecd; // 当前编码器值 - float angle_single_round; // 单圈角度 + float angle_single_round; // 单位rad +-12.5f + float last_angle; float speed_rads; // speed rad/s float real_current; // 实际电流 uint8_t temperature; // 温度,C°