重力补偿、功率限制

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,22 +168,49 @@ 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() {
+static void LimitChassisOutput()
-    // 功率限制待添加
+{
-    // referee_data->PowerHeatData.chassis_power;
+    float P_cmd = motor_rf->motor_controller.motor_power_predict +
-    // referee_data->PowerHeatData.chassis_power_buffer;
+                  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);
     }
 
+
+
+}
+
 /**
  * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算
  *        对于双板的情况,考虑增加来自底盘板IMU的数据
@@ -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;
 

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;
+        chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
-//        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
-        chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
         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;
     }

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;

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)

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

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;
 

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