Hshine
/
sentry_left
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

EC电机增加多圈角度处理 增加大yaw轴相对角度计算

This commit is contained in:
宋家齐 2024-03-13 20:11:44 +08:00
parent a7b1edaf5a
commit 40cd575e91
4 changed files with 27 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
application/chassis/chassis.c
View File

@ -267,10 +267,10 @@ void ChassisTask()
case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态 case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
//chassis_cmd_recv.wz = 0; //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; break;
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 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; break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
//chassis_cmd_recv.wz = 4000 * 10; //chassis_cmd_recv.wz = 4000 * 10;
@ -290,8 +290,8 @@ void ChassisTask()
// 根据控制模式进行正运动学解算,计算底盘输出 // 根据控制模式进行正运动学解算,计算底盘输出
MecanumCalculate(); //MecanumCalculate();
//OmniCalculate(); OmniCalculate();
//vt_rf = 5000; //vt_rf = 5000;
// 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值 // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
LimitChassisOutput(); LimitChassisOutput();

15
application/gimbal/gimbal.c
View File

@ -143,8 +143,8 @@ void GimbalInit()
.controller_setting_init_config = { .controller_setting_init_config = {
.angle_feedback_source = OTHER_FEED, .angle_feedback_source = OTHER_FEED,
.speed_feedback_source = OTHER_FEED, .speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP, .outer_loop_type = OPEN_LOOP,
.close_loop_type = ANGLE_LOOP|SPEED_LOOP|CURRENT_LOOP, .close_loop_type = OPEN_LOOP,//ANGLE_LOOP|SPEED_LOOP|CURRENT_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
}, },
.motor_type = ECA8210, .motor_type = ECA8210,
@ -178,12 +178,16 @@ void GimbalTask()
case GIMBAL_ZERO_FORCE: case GIMBAL_ZERO_FORCE:
DJIMotorStop(yaw_motor); DJIMotorStop(yaw_motor);
DJIMotorStop(pitch_motor); DJIMotorStop(pitch_motor);
ECMotorStop(big_yaw_motor);
break; break;
// 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
case GIMBAL_GYRO_MODE: // 后续只保留此模式 case GIMBAL_GYRO_MODE: // 后续只保留此模式
DJIMotorEnable(yaw_motor); DJIMotorEnable(yaw_motor);
DJIMotorEnable(pitch_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, ANGLE_LOOP, OTHER_FEED);
DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
@ -238,7 +242,12 @@ void GimbalTask()
// 设置反馈数据,主要是imu和yaw的ecd // 设置反馈数据,主要是imu和yaw的ecd
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; 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; gimbal_feedback_data.mini_yaw_encode_angle = yaw_motor->measure.angle_single_round;
// 推送消息 // 推送消息

10
modules/motor/ECmotor/ECA8210.c
View File

@ -43,14 +43,20 @@ static void ECMotorDecode(CANInstance *_instance)
DaemonReload(motor->daemon); // 喂狗 DaemonReload(motor->daemon); // 喂狗
measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt); measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt);
measure->last_angle = measure->angle_single_round;
measure->angle_single_round = (float)(ECMotor_Report.angle_raw-32768) * 0.0003814697f; 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->speed_rads = (float)(ECMotor_Report.speed_raw-2048) * 0.008789f;
measure->real_current = (float)(ECMotor_Report.current_raw-2048) * 0.014648f; measure->real_current = (float)(ECMotor_Report.current_raw-2048) * 0.014648f;
measure->temperature = (ECMotor_Report.temperature_raw - 50) /2; measure->temperature = (ECMotor_Report.temperature_raw - 50) /2;
measure->speed_rads = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_rads + measure->speed_rads = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_rads +
SPEED_SMOOTH_COEF * (float)(ECMotor_Report.speed_raw-2048) * 0.008789f; 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) static void ECMotorLostCallback(void *motor_ptr)

3
modules/motor/ECmotor/ECA8210.h
View File

@ -57,7 +57,8 @@ typedef struct // EC-A8120
{ {
uint16_t last_ecd; // 上一次读取的编码器值 uint16_t last_ecd; // 上一次读取的编码器值
uint16_t 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 speed_rads; // speed rad/s
float real_current; // 实际电流 float real_current; // 实际电流
uint8_t temperature; // 温度,C° uint8_t temperature; // 温度,C°