更换cpp架构 有标准冲突待解决

application/balance/balance.cpp

@@ -61,6 +61,15 @@ Matrixf<2,1> balance::get_control() {
     return balance_LQR_.get_control();
 }
 
+void balance::L_target_set(float L) {
+    target_length_ = L;
+}
+
+void balance::Phi_target_set(float Phi) {
+
+}
+
+
 void get_lqr_k(float L0, float K[12])
 {
     float t2;

application/balance/balance.h

@@ -16,10 +16,14 @@ typedef struct
 
 class balance {
 public:
-    balance(Balance_Init_config_s* InitConfig);
+    balance()= default;
+    explicit balance(Balance_Init_config_s* InitConfig);
     void feedback_update(float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,float MotionAccel_z);
     void control_loop();
     void target_set(float x);
+    void L_target_set(float L);
+    void Phi_target_set(float Phi);
+
     Matrixf<2,1> get_control();
 
 private:

application/balance/leg.cpp

@@ -153,3 +153,4 @@ float leg::calculate_FN(float MotionAccel_z,float theta,float theta_dot) {
 
 
 
+

application/balance/leg.h

@@ -40,6 +40,7 @@ class leg
 {
 public:
     leg(Leg_init_config_s* legInitConfig);
+    leg() {};
     void state_update(float phi1,float phi4 ,float phi1_dot,float phi4_dot);
     void input_update(float F,float Tp);
     void feedback_update(float T1_fb,float T2_fb);

application/chassis/chassis.cpp

@@ -67,10 +67,7 @@ static LKMotorInstance *wheel_motor_r,*wheel_motor_l;
 static float chassis_vx, chassis_vy;     // 将云台系的速度投影到底盘
 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
 
-
+balance balance_chassis;
-
-
-balance balance_chassis();
 
 BalanceObserver Observer;
 
@@ -83,25 +80,25 @@ void ChassisInit()
 {
     // 四个关节电机的参数,改tx_id和反转标志位即可
     Motor_Init_Config_s chassis_motor_config = {
-        .can_init_config.can_handle = &hcan2,
-        .controller_setting_init_config = {
-            .angle_feedback_source = MOTOR_FEED,
-            .speed_feedback_source = MOTOR_FEED,
-            .outer_loop_type = OPEN_LOOP,
-            .close_loop_type = OPEN_LOOP,//| CURRENT_LOOP,
-        },
-        .motor_type = HT04,
-    };
-
-    Motor_Init_Config_s wheel_motor_config = {
-            .can_init_config.can_handle = &hcan1,
             .controller_setting_init_config = {
-                    .angle_feedback_source = MOTOR_FEED,
-                    .speed_feedback_source = MOTOR_FEED,
                     .outer_loop_type = OPEN_LOOP,
                     .close_loop_type = OPEN_LOOP,//| CURRENT_LOOP,
+                    .angle_feedback_source = MOTOR_FEED,
+                    .speed_feedback_source = MOTOR_FEED,
+            },
+            .motor_type = HT04,
+            .can_init_config = {.can_handle = &hcan2},
+
+    };
+    Motor_Init_Config_s wheel_motor_config = {
+            .controller_setting_init_config = {
+                    .outer_loop_type = OPEN_LOOP,
+                    .close_loop_type = OPEN_LOOP,//| CURRENT_LOOP,
+                    .angle_feedback_source = MOTOR_FEED,
+                    .speed_feedback_source = MOTOR_FEED,
             },
             .motor_type = LK9025,
+            .can_init_config = {.can_handle = &hcan1},
     };
     //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
     chassis_motor_config.can_init_config.tx_id = 1;
@@ -159,18 +156,22 @@ void ChassisInit()
                             .Ki = 100,//5,
                             .Kd = 20,//10,//6,//6,
                             .MaxOut = 100,
-                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter,
+                            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit |
-                            .Derivative_LPF_RC = 0.001f,//1/(2*PI*10),
+                                                                      PID_Derivative_On_Measurement |
+                                                                      PID_DerivativeFilter),
                             .IntegralLimit = 100,
+                            .Derivative_LPF_RC = 0.001f,//1/(2*PI*10),
                     },
                     .phi0_PID_conf = {
                             .Kp = 80,
                             .Ki = 0,
                             .Kd = 5,
                             .MaxOut = 5,
-                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement| PID_DerivativeFilter,
+                            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit |
-                            .Derivative_LPF_RC = 1/(2*PI*10),
+                                                                      PID_Derivative_On_Measurement |
+                                                                      PID_DerivativeFilter),
                             .IntegralLimit = 100,
+                            .Derivative_LPF_RC = 1/(2*PI*10),
                     },
                     .init_target_L0 = 0.15f,
                     .F_feedforward = 0,
@@ -183,12 +184,9 @@ void ChassisInit()
                     .Improve = static_cast<PID_Improvement_e>(PID_Derivative_On_Measurement | PID_DerivativeFilter),
                     .Derivative_LPF_RC = 0.01f,
             },
-            .LQR_state_num = 6,
-            .LQR_control_num = 2,
-
     };
 
-    //Leg_Init(&legInstance,&leg_r_init_conf);
+    balance_chassis = balance(&balanceInitConfig);
 
     Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
     //转向环
@@ -197,7 +195,7 @@ void ChassisInit()
             .Ki = 0,
             .Kd = 0.0f,
             .MaxOut = 2,
-            .Improve =  PID_Derivative_On_Measurement|PID_DerivativeFilter,
+            .Improve =  static_cast<PID_Improvement_e>(PID_Derivative_On_Measurement | PID_DerivativeFilter),
             .Derivative_LPF_RC = 0.001f
     };
     PIDInit(&Turn_Loop_PID,&turn_pid_cfg);
@@ -289,10 +287,7 @@ static void Chassis_State_update()
 
     float x_dot = speed * 0.135f/2;
 
-    Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n[2]);
+    balance_chassis.feedback_update(leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n[2]);
-
-    Leg_feedback_update(&BalanceControl.leg_right,motor_rf->measure.real_current / HT_CMD_COEF,motor_rb->measure.real_current / HT_CMD_COEF);
-    Leg_feedback_update(&BalanceControl.leg_left,motor_lf->measure.real_current / HT_CMD_COEF,motor_lb->measure.real_current / HT_CMD_COEF);
 
     if( ChassisState == FAIL )
     {
@@ -373,27 +368,13 @@ void ChassisTask()
 
     float vofa_send_data[8];
 
-    BalanceControl.leg_right.target_L0 += chassis_cmd_recv.vy/5280 * 0.0003f;
+    static float L_target = 0;
-    BalanceControl.leg_right.target_L0 = float_constrain(BalanceControl.leg_right.target_L0,0.15f,0.30f);
+    L_target += chassis_cmd_recv.vy/5280 * 0.0003f;
-    BalanceControl.leg_right.target_phi0 = PI/2;
+    L_target = float_constrain(L_target,0.15f,0.30f);
 
-    BalanceControl.leg_left.target_L0 = BalanceControl.leg_right.target_L0;
+    balance_chassis.L_target_set(L_target);
-    BalanceControl.leg_left.target_L0 = float_constrain(BalanceControl.leg_left.target_L0,0.15f,0.30f);
-    BalanceControl.leg_left.target_phi0 = PI/2;
 
-
+    //vofa_justfloat_output(vofa_send_data,32,&huart1);
-    vofa_send_data[0] = BalanceControl.leg_right.legState.L0;
-    vofa_send_data[1] = BalanceControl.leg_left.legState.L0;
-    vofa_send_data[2] = BalanceControl.leg_length_PID.Ref;
-    vofa_send_data[3] = BalanceControl.leg_length_PID.Measure;
-    vofa_send_data[4] = BalanceControl.leg_right.legInput.F;
-    vofa_send_data[5] = BalanceControl.leg_left.legInput.F;
-    vofa_send_data[6] = Observer.Estimate_X[3];
-    vofa_send_data[7] = BalanceControl.state[3];
-
-
-
-    vofa_justfloat_output(vofa_send_data,32,&huart1);
 
     static float target_x = 0;
     static float target_yaw = 0;