diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 0bbd0b0..35179f0 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -157,15 +157,28 @@ static void MecanumCalculate() */ static void LimitChassisOutput() { - // 功率限制待添加 - // referee_data->PowerHeatData.chassis_power; - // referee_data->PowerHeatData.chassis_power_buffer; + 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; + + + 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); } /** diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 7dd2fce..3aa5310 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -16,6 +16,7 @@ static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自cmd的发射控制信息 static Subscriber_t *shoot_sub; static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息 +static float stop_location; // dwt定时,计算冷却用 static float hibernate_time = 0, dead_time = 0; @@ -107,7 +108,16 @@ void ShootInit() shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s)); shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s)); } - +static void Stop_Check() +{ + if(loader->measure.total_angle < stop_location*0.9) + { + DJIMotorOuterLoop(loader, ANGLE_LOOP); + DJIMotorSetRef(loader,stop_location); + hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 + dead_time = 1000; + } +} /* 机器人发射机构控制核心任务 */ void ShootTask() { @@ -139,10 +149,12 @@ void ShootTask() case LOAD_STOP: DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环 DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快 + break; // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射) case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用. DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环 + stop_location = loader->measure.total_angle; DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度 hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 dead_time = 2000; // 完成1发弹丸发射的时间 diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index 7a9985e..4e8e9f7 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -228,7 +228,16 @@ 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() { @@ -290,6 +299,34 @@ 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)pid_ref; diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index d73476e..c33ce7e 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -67,7 +67,11 @@ typedef enum MOTOR_STOP = 0, MOTOR_ENALBED = 1, } Motor_Working_Type_e; - +typedef enum +{ + NO_POWER_LIMIT = 0, + POWER_LIMIT_ON = 1, +} Power_Limit_Type_e; /* 电机控制设置,包括闭环类型,反转标志和反馈来源 */ typedef struct { @@ -78,7 +82,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; /* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/ @@ -95,6 +99,10 @@ typedef struct PIDInstance angle_PID; float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环 + + float motor_power_max; //每个电机分配的功率上限 + float motor_power_predict; //根据模型预测的电机功率 + float motor_power_scale; //电机功率缩放比例 } Motor_Controller_s; /* 电机类型枚举 */ diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 4035304..228ff68 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -103,6 +103,7 @@ static void RemoteControlRxCallback() */ static void RCLostCallback(void *id) { + DaemonReload(rc_daemon_instance); // 遥控器没数据也可以喂狗 memset(rc_ctrl, 0, sizeof(rc_ctrl)); // 清空遥控器数据 USARTServiceInit(rc_usart_instance); // 尝试重新启动接收 LOGWARNING("[rc] remote control lost");