Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

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

PidController/PidController.cpp@10:085ab7328054, 2017-10-23 (annotated)

Committer:
danstrider
Date:
Mon Oct 23 12:50:53 2017 +0000
Revision:
10:085ab7328054
checked out on the hardware

Who changed what in which revision?

UserRevisionLine numberNew contents of line
danstrider 10:085ab7328054 1 #include "PidController.hpp"
danstrider 10:085ab7328054 2
danstrider 10:085ab7328054 3 PIDController::PIDController() {
danstrider 10:085ab7328054 4 _error = 0.0;
danstrider 10:085ab7328054 5 _integral = 0.0;
danstrider 10:085ab7328054 6 _loLimit = 0.0;
danstrider 10:085ab7328054 7 _hiLimit = 0.0;
danstrider 10:085ab7328054 8 _deadbandFlag = false;
danstrider 10:085ab7328054 9 _deadband = 0.0;
danstrider 10:085ab7328054 10 }
danstrider 10:085ab7328054 11
danstrider 10:085ab7328054 12 void PIDController::update(float position, float velocity, float dt) {
danstrider 10:085ab7328054 13 // error update equations
danstrider 10:085ab7328054 14 _error = _setPoint - position;
danstrider 10:085ab7328054 15 _integral += (_error*dt);
danstrider 10:085ab7328054 16
danstrider 10:085ab7328054 17 // pid controller equation
danstrider 10:085ab7328054 18 _output = _Pgain*_error + _Igain*_integral + _Dgain*velocity ;
danstrider 10:085ab7328054 19
danstrider 10:085ab7328054 20 // limiting on output & integral anti-windup
danstrider 10:085ab7328054 21 if (_hiLimit != 0.0 and _output > _hiLimit) {
danstrider 10:085ab7328054 22 _output = _hiLimit;
danstrider 10:085ab7328054 23 _integral -= _error*dt; // implement saturation
danstrider 10:085ab7328054 24 }
danstrider 10:085ab7328054 25 if (_loLimit != 0.0 and _output < _loLimit) {
danstrider 10:085ab7328054 26 _output = _loLimit;
danstrider 10:085ab7328054 27 _integral += _error*dt; // implement saturation
danstrider 10:085ab7328054 28 }
danstrider 10:085ab7328054 29
danstrider 10:085ab7328054 30 // within deadband on error zeros output
danstrider 10:085ab7328054 31 if (_deadbandFlag) {
danstrider 10:085ab7328054 32 if (abs(_error) < _deadband) {
danstrider 10:085ab7328054 33 _output = 0.0;
danstrider 10:085ab7328054 34 }
danstrider 10:085ab7328054 35 }
danstrider 10:085ab7328054 36 }
danstrider 10:085ab7328054 37
danstrider 10:085ab7328054 38 void PIDController::writeSetPoint(float cmd) {
danstrider 10:085ab7328054 39 _setPoint = cmd;
danstrider 10:085ab7328054 40 }
danstrider 10:085ab7328054 41
danstrider 10:085ab7328054 42 float PIDController::getOutput() {
danstrider 10:085ab7328054 43 return _output;
danstrider 10:085ab7328054 44 }
danstrider 10:085ab7328054 45
danstrider 10:085ab7328054 46 void PIDController::setPgain(float gain) {
danstrider 10:085ab7328054 47 _Pgain = gain;
danstrider 10:085ab7328054 48 }
danstrider 10:085ab7328054 49
danstrider 10:085ab7328054 50 void PIDController::setIgain(float gain) {
danstrider 10:085ab7328054 51 _Igain = gain;
danstrider 10:085ab7328054 52 }
danstrider 10:085ab7328054 53
danstrider 10:085ab7328054 54 void PIDController::setDgain(float gain) {
danstrider 10:085ab7328054 55 _Dgain = gain;
danstrider 10:085ab7328054 56 }
danstrider 10:085ab7328054 57
danstrider 10:085ab7328054 58 void PIDController::toggleDeadBand(bool toggle) {
danstrider 10:085ab7328054 59 _deadbandFlag = toggle;
danstrider 10:085ab7328054 60 }
danstrider 10:085ab7328054 61 void PIDController::setDeadBand(float deadband) {
danstrider 10:085ab7328054 62 _deadband = deadband;
danstrider 10:085ab7328054 63 }