Flying Sea Glider / Mbed 2 deprecated 2019_19feb19_jcw_noSD

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MODSERIAL FATFileSystem

Controller/controller.cpp@9:d5fcdcb3c89d, 2017-10-20 (annotated)

Committer:
mkelly10
Date:
Fri Oct 20 11:41:22 2017 +0000
Revision:
9:d5fcdcb3c89d
Tested 10/19/17 Folders

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mkelly10 9:d5fcdcb3c89d 1 #include "controller.hpp"
mkelly10 9:d5fcdcb3c89d 2 #include "StaticDefs.hpp"
mkelly10 9:d5fcdcb3c89d 3
mkelly10 9:d5fcdcb3c89d 4 PIDController::PIDController()
mkelly10 9:d5fcdcb3c89d 5 {
mkelly10 9:d5fcdcb3c89d 6 _integral = 0.0;
mkelly10 9:d5fcdcb3c89d 7 _lastTime = 0;
mkelly10 9:d5fcdcb3c89d 8 _loLimit = 0;
mkelly10 9:d5fcdcb3c89d 9 _hiLimit = 400;
mkelly10 9:d5fcdcb3c89d 10 _deadbandFlag = false;
mkelly10 9:d5fcdcb3c89d 11 _deadband = 0.5;
mkelly10 9:d5fcdcb3c89d 12
mkelly10 9:d5fcdcb3c89d 13 }
mkelly10 9:d5fcdcb3c89d 14
mkelly10 9:d5fcdcb3c89d 15 void PIDController::update(float position, float velocity, float dt)
mkelly10 9:d5fcdcb3c89d 16 {
mkelly10 9:d5fcdcb3c89d 17
mkelly10 9:d5fcdcb3c89d 18 _error = _setPoint - position;
mkelly10 9:d5fcdcb3c89d 19
mkelly10 9:d5fcdcb3c89d 20 _integral += (_error*dt);
mkelly10 9:d5fcdcb3c89d 21
mkelly10 9:d5fcdcb3c89d 22 _output = _Pgain*_error + _Igain*_integral + _Dgain*velocity ;
mkelly10 9:d5fcdcb3c89d 23
mkelly10 9:d5fcdcb3c89d 24 // limiting on output & integral anti-windup
mkelly10 9:d5fcdcb3c89d 25 if (_output > 1.0) {
mkelly10 9:d5fcdcb3c89d 26 _output = 1.0;
mkelly10 9:d5fcdcb3c89d 27 _integral -= _error*dt; //implement saturation instead of reset
mkelly10 9:d5fcdcb3c89d 28 } else if (_output < -1) {
mkelly10 9:d5fcdcb3c89d 29 _output = -1.0;
mkelly10 9:d5fcdcb3c89d 30 _integral += _error*dt; //implement saturation instead of reset
mkelly10 9:d5fcdcb3c89d 31 }
mkelly10 9:d5fcdcb3c89d 32
mkelly10 9:d5fcdcb3c89d 33 //add in some deadband
mkelly10 9:d5fcdcb3c89d 34 //add a case statement for deadband
mkelly10 9:d5fcdcb3c89d 35 //add a variable for deadband amount
mkelly10 9:d5fcdcb3c89d 36 if (_deadbandFlag) {
mkelly10 9:d5fcdcb3c89d 37 if (abs(_error) < _deadband) {
mkelly10 9:d5fcdcb3c89d 38 _output = 0.0;
mkelly10 9:d5fcdcb3c89d 39 }
mkelly10 9:d5fcdcb3c89d 40 }
mkelly10 9:d5fcdcb3c89d 41
mkelly10 9:d5fcdcb3c89d 42 }
mkelly10 9:d5fcdcb3c89d 43
mkelly10 9:d5fcdcb3c89d 44 void PIDController::writeSetPoint(float cmd)
mkelly10 9:d5fcdcb3c89d 45 {
mkelly10 9:d5fcdcb3c89d 46 _setPoint = cmd; //<float>(cmd, _loLimit, _hiLimit);
mkelly10 9:d5fcdcb3c89d 47 }
mkelly10 9:d5fcdcb3c89d 48
mkelly10 9:d5fcdcb3c89d 49 float PIDController::getOutput()
mkelly10 9:d5fcdcb3c89d 50 {
mkelly10 9:d5fcdcb3c89d 51 return _output;
mkelly10 9:d5fcdcb3c89d 52 }
mkelly10 9:d5fcdcb3c89d 53
mkelly10 9:d5fcdcb3c89d 54 void PIDController::setPgain(float gain)
mkelly10 9:d5fcdcb3c89d 55 {
mkelly10 9:d5fcdcb3c89d 56 _Pgain = gain;
mkelly10 9:d5fcdcb3c89d 57 }
mkelly10 9:d5fcdcb3c89d 58
mkelly10 9:d5fcdcb3c89d 59 void PIDController::setIgain(float gain)
mkelly10 9:d5fcdcb3c89d 60 {
mkelly10 9:d5fcdcb3c89d 61 _Igain = gain;
mkelly10 9:d5fcdcb3c89d 62 }
mkelly10 9:d5fcdcb3c89d 63
mkelly10 9:d5fcdcb3c89d 64 void PIDController::setDgain(float gain)
mkelly10 9:d5fcdcb3c89d 65 {
mkelly10 9:d5fcdcb3c89d 66 _Dgain = gain;
mkelly10 9:d5fcdcb3c89d 67 }
mkelly10 9:d5fcdcb3c89d 68
mkelly10 9:d5fcdcb3c89d 69 void PIDController::toggleDeadBand(bool toggle)
mkelly10 9:d5fcdcb3c89d 70 {
mkelly10 9:d5fcdcb3c89d 71 _deadbandFlag = toggle;
mkelly10 9:d5fcdcb3c89d 72 return;
mkelly10 9:d5fcdcb3c89d 73 }
mkelly10 9:d5fcdcb3c89d 74 void PIDController::setDeadBand(float deadband)
mkelly10 9:d5fcdcb3c89d 75 {
mkelly10 9:d5fcdcb3c89d 76 _deadband = deadband;
mkelly10 9:d5fcdcb3c89d 77 }