Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: added balancing function for BalanceBot

Browse Source
This commit is contained in:
Ebin 2018-06-18 16:44:28 +05:30 committed by Randy Mackay
parent 936ebbe1f3
commit 657ff06380
2 changed files with 99 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -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
{

16
libraries/APM_Control/AR_AttitudeControl.h
View File

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