功率控制（需要调参数）、拨弹反转（未验证）

application/chassis/chassis.c

@@ -157,15 +157,28 @@ static void MecanumCalculate()
  */
 static void LimitChassisOutput()
 {
-    // 功率限制待添加
+    float P_cmd = motor_rf->motor_controller.motor_power_predict +
-    // referee_data->PowerHeatData.chassis_power;
+                  motor_rb->motor_controller.motor_power_predict +
-    // referee_data->PowerHeatData.chassis_power_buffer;
+                  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);
 }
 
 /**

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发弹丸发射的时间

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;
 

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;
 
 /* 电机类型枚举 */

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");