APM_Control: added balancing function for BalanceBot

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
 {

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;
 };