diff --git a/application/balance/balance.cpp b/application/balance/balance.cpp index 547b69f..ba2a0bc 100644 --- a/application/balance/balance.cpp +++ b/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; diff --git a/application/balance/balance.h b/application/balance/balance.h index f448e58..a9c765e 100644 --- a/application/balance/balance.h +++ b/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: diff --git a/application/balance/leg.cpp b/application/balance/leg.cpp index 411d519..5ebcc11 100644 --- a/application/balance/leg.cpp +++ b/application/balance/leg.cpp @@ -153,3 +153,4 @@ float leg::calculate_FN(float MotionAccel_z,float theta,float theta_dot) { + diff --git a/application/balance/leg.h b/application/balance/leg.h index ab3aa96..27f181c 100644 --- a/application/balance/leg.h +++ b/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); diff --git a/application/chassis/chassis.cpp b/application/chassis/chassis.cpp index 1ee3151..7c56bb2 100644 --- a/application/chassis/chassis.cpp +++ b/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, - .Derivative_LPF_RC = 0.001f,//1/(2*PI*10), + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | + 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, - .Derivative_LPF_RC = 1/(2*PI*10), + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | + 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_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_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]); - - 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); + balance_chassis.feedback_update(leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n[2]); 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; - BalanceControl.leg_right.target_L0 = float_constrain(BalanceControl.leg_right.target_L0,0.15f,0.30f); - BalanceControl.leg_right.target_phi0 = PI/2; + static float L_target = 0; + L_target += chassis_cmd_recv.vy/5280 * 0.0003f; + L_target = float_constrain(L_target,0.15f,0.30f); - BalanceControl.leg_left.target_L0 = BalanceControl.leg_right.target_L0; - BalanceControl.leg_left.target_L0 = float_constrain(BalanceControl.leg_left.target_L0,0.15f,0.30f); - BalanceControl.leg_left.target_phi0 = PI/2; + balance_chassis.L_target_set(L_target); - - 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); + //vofa_justfloat_output(vofa_send_data,32,&huart1); static float target_x = 0; static float target_yaw = 0;