diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 67f5d91..88aeaf1 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -84,6 +84,7 @@ void ChassisInit() { .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP, //| CURRENT_LOOP, + .power_limit_flag = POWER_LIMIT_ON, }, .motor_type = M3508, }; @@ -114,6 +115,8 @@ void ChassisInit() { }}; cap = SuperCapInit(&cap_conf); // 超级电容初始化 + // PowerMeter_Init_Config_s + // 发布订阅初始化,如果为双板,则需要can comm来传递消息 #ifdef CHASSIS_BOARD Chassis_IMU_data = INS_Init(); // 底盘IMU初始化 @@ -165,20 +168,47 @@ static void OmniCalculate() { vt_lf /= (WHEEL_BASE * 1.414f); } +static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f}; +///依据3508电机功率模型，预测电机输出功率 +static float EstimatePower(DJIMotorInstance* chassis_motor) +{ + + float I_cmd = chassis_motor->motor_controller.current_PID.Output; + float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm + + float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd; + + return power; +} +float vofa_send_data[6]; /** * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值 * */ -static void LimitChassisOutput() { - // 功率限制待添加 - // referee_data->PowerHeatData.chassis_power; - // referee_data->PowerHeatData.chassis_power_buffer; +static void LimitChassisOutput() +{ + float P_cmd = motor_rf->motor_controller.motor_power_predict + + motor_rb->motor_controller.motor_power_predict + + motor_lb->motor_controller.motor_power_predict + + motor_lf->motor_controller.motor_power_predict + 3.6f; + float P_max = 60 - 10; + float K = P_max/P_cmd; + vofa_send_data[2] = P_cmd; + + motor_rf->motor_controller.motor_power_scale = K; + motor_rb->motor_controller.motor_power_scale = K; + motor_lf->motor_controller.motor_power_scale = K; + motor_lb->motor_controller.motor_power_scale = K; + + { + DJIMotorSetRef(motor_lf, vt_lf); + DJIMotorSetRef(motor_rf, vt_rf); + DJIMotorSetRef(motor_lb, vt_lb); + DJIMotorSetRef(motor_rb, vt_rb); + } + + - // 完成功率限制后进行电机参考输入设定 - DJIMotorSetRef(motor_lf, vt_lf); - DJIMotorSetRef(motor_rf, vt_rf); - DJIMotorSetRef(motor_lb, vt_lb); - DJIMotorSetRef(motor_rb, vt_rb); } /** @@ -235,8 +265,7 @@ void ChassisTask() { static float sin_theta, cos_theta; cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); -// sin_theta = 0; -// cos_theta = 1; + chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta; chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 4051dc1..e5e1317 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -118,12 +118,12 @@ static void RemoteControlSet() { // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],小陀螺 { -// chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; -// gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; + chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],,底盘跟随云台 { + chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; } diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 34e65ee..4342ce9 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -15,7 +15,7 @@ static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 - +static float gravity_current = 0; static void LimitPitchAngleAuto() { /** 注意电机角度与陀螺仪角度方向是否相同 * 目前 add > 0, 向下转动 @@ -97,7 +97,7 @@ void GimbalInit() { .MaxOut = 500, }, .speed_PID = { - .Kp = -800, // 50 + .Kp = 0,//-800, // 50 .Ki = 0, // 350 .Kd = 0, // 0 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, @@ -107,6 +107,7 @@ void GimbalInit() { .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[0], + .current_feedforward_ptr = &gravity_current, }, .controller_setting_init_config = { .angle_feedback_source = OTHER_FEED, @@ -114,6 +115,7 @@ void GimbalInit() { .outer_loop_type = ANGLE_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, + .feedforward_flag = CURRENT_FEEDFORWARD, }, .motor_type = GM6020, .motor_control_type = CURRENT_CONTROL @@ -170,6 +172,8 @@ void GimbalTask() { // 在合适的地方添加pitch重力补偿前馈力矩 // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩 // ... + float theta = - gimba_IMU_data->Pitch/180*PI; + gravity_current = (5000)*arm_cos_f32(theta); // float vofa_send_data[4]; // vofa_send_data[0] = pitch_motor->motor_controller.speed_PID.Ref; diff --git a/application/robot.c b/application/robot.c index 3102179..4a73566 100644 --- a/application/robot.c +++ b/application/robot.c @@ -32,7 +32,7 @@ void RobotInit() #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) RobotCMDInit(); GimbalInit(); - ShootInit(); + //ShootInit(); #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) @@ -50,7 +50,7 @@ void RobotTask() #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) RobotCMDTask(); GimbalTask(); - ShootTask(); + //ShootTask(); #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) diff --git a/application/robot_def.h b/application/robot_def.h index 490fff0..c891701 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -26,7 +26,7 @@ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 -#define YAW_CHASSIS_ALIGN_ECD 3055 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 +#define YAW_CHASSIS_ALIGN_ECD 7856 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 #define PITCH_HORIZON_ECD 1670 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 #define PITCH_MAX_ANGLE 25 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) @@ -201,7 +201,7 @@ typedef struct typedef struct { attitude_t gimbal_imu_data; - uint16_t yaw_motor_single_round_angle; + float yaw_motor_single_round_angle; } Gimbal_Upload_Data_s; typedef struct diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index 06f1722..57efbf2 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -2,6 +2,7 @@ #include "general_def.h" #include "bsp_dwt.h" #include "bsp_log.h" +#include "user_lib.h" static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用 /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */ @@ -246,6 +247,18 @@ void DJIMotorSetRef(DJIMotorInstance *motor, float ref) { motor->motor_controller.pid_ref = ref; } +static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f}; +//依据3508电机功率模型，预测电机输出功率 +static float EstimatePower(DJIMotorInstance* chassis_motor,float output) +{ + float I_cmd = chassis_motor->motor_controller.current_PID.Output; + float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm + + float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd; + return power; +} + + // 为所有电机实例计算三环PID,发送控制报文 void DJIMotorControl() { // 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性 @@ -303,6 +316,38 @@ void DJIMotorControl() { if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) pid_ref *= -1; + //功率限制 + if(motor_setting->power_limit_flag == POWER_LIMIT_ON) + { + float I_cmd = pid_ref; + float w = measure->speed_aps /6 ;//aps to rpm + motor_controller->motor_power_predict = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd; + //这里K应该使用所有底盘电机一起计算 (在底盘任务中) + //float K = motor_controller->motor_power_max / motor_controller->motor_power_predict; + float K = motor_controller->motor_power_scale; + if(K>=1 || motor_controller->motor_power_predict < 0)//未超过最大功率 或做负功的电机 不做限制 + { + + } + else if(K<1) + { + float P_cmd = K * motor_controller->motor_power_predict; //对输出功率进行缩放 + + float a = motor_power_K[2] ; + float b = motor_power_K[0] * w; + float c = motor_power_K[1] * w * w - P_cmd; + + if(pid_ref < 0) + pid_ref = (-b - sqrtf(b*b-4*a*c))/(2*a); + else + pid_ref = (-b + sqrtf(b*b-4*a*c))/(2*a); + } + //if( motor_controller->motor_power_predict < ) + } + + // 获取最终输出 此处注意set不要超过int16_t能表达的最大数 +-32767 + + pid_ref = float_constrain(pid_ref,-30000,30000); // 获取最终输出 set = (int16_t) pid_ref; diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index 868cc97..f97b6b9 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -68,6 +68,12 @@ typedef enum MOTOR_ENALBED = 1, } Motor_Working_Type_e; +typedef enum +{ + NO_POWER_LIMIT = 0, + POWER_LIMIT_ON = 1, +} Power_Limit_Type_e; + /* 电机控制设置,包括闭环类型,反转标志和反馈来源 */ typedef struct { @@ -78,6 +84,7 @@ typedef struct Feedback_Source_e angle_feedback_source; // 角度反馈类型 Feedback_Source_e speed_feedback_source; // 速度反馈类型 Feedfoward_Type_e feedforward_flag; // 前馈标志 + Power_Limit_Type_e power_limit_flag; //功率限制标志 } Motor_Control_Setting_s; @@ -104,6 +111,10 @@ typedef struct PIDInstance angle_PID; float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环 + + float motor_power_max; //每个电机分配的功率上限 + float motor_power_predict; //根据模型预测的电机功率 + float motor_power_scale; //电机功率缩放比例 } Motor_Controller_s; /* 电机类型枚举 */