most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown

Dependencies:   mbed MODSERIAL FATFileSystem

Committer:
joel_ssc
Date:
Mon May 13 19:25:26 2019 +0000
Revision:
92:52a91656458a
Parent:
59:4c04d5c7aed1
version for first flight test, timeouts not yet set correctly

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tnhnrl 59:4c04d5c7aed1 1 #include "PidController.hpp"
tnhnrl 59:4c04d5c7aed1 2
tnhnrl 59:4c04d5c7aed1 3 #include "StaticDefs.hpp"
tnhnrl 59:4c04d5c7aed1 4
tnhnrl 59:4c04d5c7aed1 5 PIDController::PIDController() {
tnhnrl 59:4c04d5c7aed1 6 _error = 0.0;
tnhnrl 59:4c04d5c7aed1 7 _integral = 0.0;
tnhnrl 59:4c04d5c7aed1 8 _loLimit = 0.0;
tnhnrl 59:4c04d5c7aed1 9 _hiLimit = 0.0;
tnhnrl 59:4c04d5c7aed1 10 _deadbandFlag = false;
tnhnrl 59:4c04d5c7aed1 11 _deadband = 0.0;
tnhnrl 59:4c04d5c7aed1 12 _headingFlag = false; //default for pressure and depth
tnhnrl 59:4c04d5c7aed1 13 }
tnhnrl 59:4c04d5c7aed1 14
tnhnrl 59:4c04d5c7aed1 15 void PIDController::update(float position, float velocity, float dt) {
tnhnrl 59:4c04d5c7aed1 16 // error update equations
tnhnrl 59:4c04d5c7aed1 17 _error = _setPoint - position;
tnhnrl 59:4c04d5c7aed1 18
tnhnrl 59:4c04d5c7aed1 19 _temp_velocity = velocity;
tnhnrl 59:4c04d5c7aed1 20
tnhnrl 59:4c04d5c7aed1 21 // hack to get heading update correct
tnhnrl 59:4c04d5c7aed1 22 // need to make sure this makes physical sense
tnhnrl 59:4c04d5c7aed1 23 if (_headingFlag) {
joel_ssc 92:52a91656458a 24 if (_error >= 180){ //technincally, this adjustment should continue until error is less than 180.
tnhnrl 59:4c04d5c7aed1 25 _error = _error - 360.0;
joel_ssc 92:52a91656458a 26 }else if(_error <= -180){ // likewise here, continue until error greater than -180
tnhnrl 59:4c04d5c7aed1 27 _error = _error + 360.0;
tnhnrl 59:4c04d5c7aed1 28 }
tnhnrl 59:4c04d5c7aed1 29 }
tnhnrl 59:4c04d5c7aed1 30
tnhnrl 59:4c04d5c7aed1 31 //calculate integral error term (adding anti-windup)
tnhnrl 59:4c04d5c7aed1 32 // ADDED k_aw or anti-windup term for bench testing (typically very small but anything from 0 to 0.9999...)
tnhnrl 59:4c04d5c7aed1 33 float AWgain = 0.1; // AntiWindupGain
tnhnrl 59:4c04d5c7aed1 34 float integral_dot = _Pgain * _Igain * (_error - AWgain * abs(_error) * _integral);
tnhnrl 59:4c04d5c7aed1 35
tnhnrl 59:4c04d5c7aed1 36 //calculate integral error term
tnhnrl 59:4c04d5c7aed1 37 //_integral = _integral + (_error*dt); //original equation
tnhnrl 59:4c04d5c7aed1 38 _integral = _integral + integral_dot * dt;
tnhnrl 59:4c04d5c7aed1 39
tnhnrl 59:4c04d5c7aed1 40 // pid controller equation //derivative error had the wrong sign
tnhnrl 59:4c04d5c7aed1 41 _output = _Pgain*_error + _Igain*_integral - _Dgain*velocity ; //derivative_error = v_setpoint - v_actual * _Dgain
tnhnrl 59:4c04d5c7aed1 42
tnhnrl 59:4c04d5c7aed1 43 // limiting on output & integral anti-windup (preventing integral from getting extremely high)
tnhnrl 59:4c04d5c7aed1 44 // Get the current controller output, and stop integrating if it's saturated
tnhnrl 59:4c04d5c7aed1 45 // one solution, stop integrating when you the position limits
tnhnrl 59:4c04d5c7aed1 46
tnhnrl 59:4c04d5c7aed1 47 // within deadband on error zeros output
tnhnrl 59:4c04d5c7aed1 48 if (_deadbandFlag) {
tnhnrl 59:4c04d5c7aed1 49 if (abs(_error) < _deadband) {
tnhnrl 59:4c04d5c7aed1 50 _output = 0.0;
tnhnrl 59:4c04d5c7aed1 51 }
tnhnrl 59:4c04d5c7aed1 52 }
tnhnrl 59:4c04d5c7aed1 53 }
tnhnrl 59:4c04d5c7aed1 54
tnhnrl 59:4c04d5c7aed1 55 void PIDController::writeSetPoint(float cmd) {
tnhnrl 59:4c04d5c7aed1 56 _setPoint = cmd;
tnhnrl 59:4c04d5c7aed1 57 }
tnhnrl 59:4c04d5c7aed1 58
tnhnrl 59:4c04d5c7aed1 59 float PIDController::getOutput() {
tnhnrl 59:4c04d5c7aed1 60 return _output;
tnhnrl 59:4c04d5c7aed1 61 }
tnhnrl 59:4c04d5c7aed1 62
tnhnrl 59:4c04d5c7aed1 63 void PIDController::setPgain(float gain) {
tnhnrl 59:4c04d5c7aed1 64 _Pgain = gain;
tnhnrl 59:4c04d5c7aed1 65 }
tnhnrl 59:4c04d5c7aed1 66
tnhnrl 59:4c04d5c7aed1 67 void PIDController::setIgain(float gain) {
tnhnrl 59:4c04d5c7aed1 68 _Igain = gain;
tnhnrl 59:4c04d5c7aed1 69 }
tnhnrl 59:4c04d5c7aed1 70
tnhnrl 59:4c04d5c7aed1 71 void PIDController::setDgain(float gain) {
tnhnrl 59:4c04d5c7aed1 72 _Dgain = gain;
tnhnrl 59:4c04d5c7aed1 73 }
tnhnrl 59:4c04d5c7aed1 74
tnhnrl 59:4c04d5c7aed1 75 void PIDController::toggleDeadBand(bool toggle) {
tnhnrl 59:4c04d5c7aed1 76 _deadbandFlag = toggle;
tnhnrl 59:4c04d5c7aed1 77 }
tnhnrl 59:4c04d5c7aed1 78 void PIDController::setDeadBand(float deadband) {
tnhnrl 59:4c04d5c7aed1 79 _deadband = deadband;
tnhnrl 59:4c04d5c7aed1 80 }
tnhnrl 59:4c04d5c7aed1 81
tnhnrl 59:4c04d5c7aed1 82 void PIDController::setHeadingFlag(bool heading_flag) {
tnhnrl 59:4c04d5c7aed1 83 _headingFlag = heading_flag;
tnhnrl 59:4c04d5c7aed1 84 }
tnhnrl 59:4c04d5c7aed1 85
tnhnrl 59:4c04d5c7aed1 86 void PIDController::setHiLimit(float high_limit) {
tnhnrl 59:4c04d5c7aed1 87 _hiLimit = high_limit;
tnhnrl 59:4c04d5c7aed1 88 }
tnhnrl 59:4c04d5c7aed1 89
tnhnrl 59:4c04d5c7aed1 90 void PIDController::setLoLimit(float low_limit){
tnhnrl 59:4c04d5c7aed1 91 _loLimit = low_limit;
tnhnrl 59:4c04d5c7aed1 92 }