From 657ff06380c041dcede3859637e857f31fb60a19 Mon Sep 17 00:00:00 2001 From: Ebin Date: Mon, 18 Jun 2018 16:44:28 +0530 Subject: [PATCH] APM_Control: added balancing function for BalanceBot --- libraries/APM_Control/AR_AttitudeControl.cpp | 84 +++++++++++++++++++- libraries/APM_Control/AR_AttitudeControl.h | 16 ++++ 2 files changed, 99 insertions(+), 1 deletion(-) diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index c97c5022b1..da945fe2a5 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -168,6 +168,49 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = { // @User: Standard AP_GROUPINFO("_DECEL_MAX", 9, AR_AttitudeControl, _throttle_decel_max, 0.00f), + // @Param: _BAL_P + // @DisplayName: Pitch control P gain + // @Description: Pitch control P gain for BalanceBots. Converts the error between the desired pitch (in radians) and actual pitch to a motor output (in the range -1 to +1) + // @Range: 0.000 2.000 + // @Increment: 0.01 + // @User: Standard + + // @Param: _BAL_I + // @DisplayName: Pitch control I gain + // @Description: Pitch control I gain for BalanceBots. Corrects long term error between the desired pitch (in radians) and actual pitch + // @Range: 0.000 2.000 + // @User: Standard + + // @Param: _BAL_IMAX + // @DisplayName: Pitch control I gain maximum + // @Description: Pitch control I gain maximum. Constrains the maximum motor output (range -1 to +1) that the I term will generate + // @Range: 0.000 1.000 + // @Increment: 0.01 + // @User: Standard + + // @Param: _BAL_D + // @DisplayName: Pitch control D gain + // @Description: Pitch control D gain. Compensates for short-term change in desired pitch vs actual + // @Range: 0.000 0.100 + // @Increment: 0.001 + // @User: Standard + + // @Param: _BAL_FF + // @DisplayName: Pitch control feed forward + // @Description: Pitch control feed forward + // @Range: 0.000 0.500 + // @Increment: 0.001 + // @User: Standard + + // @Param: _BAL_FILT + // @DisplayName: Pitch control filter frequency + // @Description: Pitch control input filter. Lower values reduce noise but add delay. + // @Range: 0.000 100.000 + // @Increment: 0.1 + // @Units: Hz + // @User: Standard + AP_SUBGROUPINFO(_pitch_to_throttle_pid, "_BAL_", 10, AR_AttitudeControl, AC_PID), + AP_GROUPEND }; @@ -175,7 +218,8 @@ AR_AttitudeControl::AR_AttitudeControl(AP_AHRS &ahrs) : _ahrs(ahrs), _steer_angle_p(AR_ATTCONTROL_STEER_ANG_P), _steer_rate_pid(AR_ATTCONTROL_STEER_RATE_P, AR_ATTCONTROL_STEER_RATE_I, AR_ATTCONTROL_STEER_RATE_D, AR_ATTCONTROL_STEER_RATE_IMAX, AR_ATTCONTROL_STEER_RATE_FILT, AR_ATTCONTROL_DT, AR_ATTCONTROL_STEER_RATE_FF), - _throttle_speed_pid(AR_ATTCONTROL_THR_SPEED_P, AR_ATTCONTROL_THR_SPEED_I, AR_ATTCONTROL_THR_SPEED_D, AR_ATTCONTROL_THR_SPEED_IMAX, AR_ATTCONTROL_THR_SPEED_FILT, AR_ATTCONTROL_DT) + _throttle_speed_pid(AR_ATTCONTROL_THR_SPEED_P, AR_ATTCONTROL_THR_SPEED_I, AR_ATTCONTROL_THR_SPEED_D, AR_ATTCONTROL_THR_SPEED_IMAX, AR_ATTCONTROL_THR_SPEED_FILT, AR_ATTCONTROL_DT), + _pitch_to_throttle_pid(AR_ATTCONTROL_PITCH_THR_P, AR_ATTCONTROL_PITCH_THR_I, AR_ATTCONTROL_PITCH_THR_D, AR_ATTCONTROL_PITCH_THR_IMAX, AR_ATTCONTROL_PITCH_THR_FILT, AR_ATTCONTROL_DT) { AP_Param::setup_object_defaults(this, var_info); } @@ -441,6 +485,44 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor } } +// for balancebot +// return a throttle output from -1 to +1, given a desired pitch angle +// desired_pitch is in radians +float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool armed) +{ + + //reset I term and return if disarmed + if (!armed){ + _pitch_to_throttle_pid.reset_I(); + return 0.0f; + } + + // calculate dt + const uint32_t now = AP_HAL::millis(); + float dt = (now - _balance_last_ms) * 0.001f; + + // if not called recently, reset input filter + if ((_balance_last_ms == 0) || (dt > (AR_ATTCONTROL_TIMEOUT_MS / 1000.0f))) { + dt = 0.0f; + _pitch_to_throttle_pid.reset_filter(); + } else { + _pitch_to_throttle_pid.set_dt(dt); + } + _balance_last_ms = now; + + // calculate pitch error + const float pitch_error = desired_pitch - _ahrs.pitch; + + // pitch error is given as input to PID contoller + _pitch_to_throttle_pid.set_input_filter_all(pitch_error); + + // record desired speed for logging purposes only + _pitch_to_throttle_pid.set_desired_rate(desired_pitch); + + // return output of PID controller + return constrain_float(_pitch_to_throttle_pid.get_pid(), -1.0f, +1.0f); +} + // get forward speed in m/s (earth-frame horizontal velocity but only along vehicle x-axis). returns true on success bool AR_AttitudeControl::get_forward_speed(float &speed) const { diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index db6f48cfa9..7a43a42070 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -20,6 +20,11 @@ #define AR_ATTCONTROL_THR_SPEED_IMAX 1.00f #define AR_ATTCONTROL_THR_SPEED_D 0.00f #define AR_ATTCONTROL_THR_SPEED_FILT 10.00f +#define AR_ATTCONTROL_PITCH_THR_P 30.0f +#define AR_ATTCONTROL_PITCH_THR_I 10.0f +#define AR_ATTCONTROL_PITCH_THR_D 15.0f +#define AR_ATTCONTROL_PITCH_THR_IMAX 1.0f +#define AR_ATTCONTROL_PITCH_THR_FILT 10.0f #define AR_ATTCONTROL_DT 0.02f #define AR_ATTCONTROL_TIMEOUT_MS 200 @@ -84,10 +89,16 @@ public: // return a throttle output from -1 to +1 to perform a controlled stop. stopped is set to true once stop has been completed float get_throttle_out_stop(bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt, bool &stopped); + // for balancebot + // return a throttle output from -1 to +1 given a desired pitch angle + // desired_pitch is in radians + float get_throttle_out_from_pitch(float desired_pitch, bool armed); + // low level control accessors for reporting and logging AC_P& get_steering_angle_p() { return _steer_angle_p; } AC_PID& get_steering_rate_pid() { return _steer_rate_pid; } AC_PID& get_throttle_speed_pid() { return _throttle_speed_pid; } + AC_PID& get_pitch_to_throttle_pid() { return _pitch_to_throttle_pid; } // get forward speed in m/s (earth-frame horizontal velocity but only along vehicle x-axis). returns true on success bool get_forward_speed(float &speed) const; @@ -122,6 +133,8 @@ private: AC_P _steer_angle_p; // steering angle controller AC_PID _steer_rate_pid; // steering rate controller AC_PID _throttle_speed_pid; // throttle speed controller + AC_PID _pitch_to_throttle_pid;// balancebot pitch controller + AP_Float _throttle_accel_max; // speed/throttle control acceleration (and deceleration) maximum in m/s/s. 0 to disable limits AP_Float _throttle_decel_max; // speed/throttle control deceleration maximum in m/s/s. 0 to use ATC_ACCEL_MAX for deceleration AP_Int8 _brake_enable; // speed control brake enable/disable. if set to 1 a reversed output to the motors to slow the vehicle. @@ -141,4 +154,7 @@ private: uint32_t _stop_last_ms; // system time the vehicle was at a complete stop bool _throttle_limit_low; // throttle output was limited from going too low (used to reduce i-term buildup) bool _throttle_limit_high; // throttle output was limited from going too high (used to reduce i-term buildup) + + // balancebot pitch control + uint32_t _balance_last_ms = 0; };