OuterLoop.cpp

00001 #include "mbed.h"
00002 #include "OuterLoop.hpp"
00003 #include "StaticDefs.hpp"
00004  
00005 // this is where the variables that can be set are set when the object is created
00006 OuterLoop::OuterLoop(float interval, int sensor):
00007     _filter(),
00008     _pid(),
00009     _pulse()
00010 {
00011     _Pgain = 0.5;
00012     _Igain = 0.0;
00013     _Dgain = 0.1;
00014     
00015     _filterFrequency = 2.0;
00016     _deadband = 0.0;
00017 
00018     _dt = interval;
00019     _sensor = sensor; // select the sensor ... hacky
00020     setIHiLimit(3.0);  //give each outerloop instance an integral saturation limit of some default
00021     setILoLimit(3.0);
00022     
00023     if (_sensor == 2) {
00024         _pid.setHeadingFlag(true);  //used to update the handling for the heading PID control loop
00025         _filter.setHeadingFlag(true);
00026     }
00027 }
00028 
00029 // initialize and start all of the member objects.
00030 void OuterLoop::init() {
00031     // load gains into pid controller this should eventually be in a config file
00032     setFilterFrequency(_filterFrequency);
00033     setControllerP(_Pgain);
00034     setControllerI(_Igain);
00035     setControllerD(_Dgain);
00036     
00037     // setup the controller object
00038     toggleDeadband(true);
00039     setDeadband(_deadband);
00040 }
00041 
00042 // attaches the update function to an interval timer
00043 void OuterLoop::start() {
00044     _pulse.attach(this,&OuterLoop::update, _dt);
00045 }
00046  
00047 // detaches the update function from the interval timer
00048 void OuterLoop::stop() {
00049     _pulse.detach();
00050 }
00051 
00052 void OuterLoop::runOuterLoop() {
00053     update();
00054 }
00055 
00056 // todo: these sensor reads should be somehow attached as callbacks, not hard-coded like this 
00057 void OuterLoop::update() { 
00058     // update the position velocity filter
00059     if (_sensor == 0) {
00060            //   _sensorVal = depth().getDepthFt();  //  change to getting meters
00061         _sensorVal = depth().getDepthM();
00062     } else if (_sensor == 1) {
00063         _sensorVal = imu().getPitch();
00064     } else if (_sensor == 2) {
00065         _sensorVal = imu().getHeading();
00066     } else if (_sensor == 3) {
00067       _sensorVal = sensors().getAltimeterChannelReadings();  // 27mar2019    this is still in counts
00068      //    _sensorVal = imu().getPitch();  // 27mar2019   testing sensors error?
00069     } else {
00070         error("\n\r This sensor option does not exist");
00071     }
00072     
00073     // use the sensor reading to update the PVF
00074     _filter.update(_dt, _sensorVal);
00075     
00076     // refresh the filter results and load into class variables
00077     refreshPVState();
00078  
00079     // update the PID controller with latest data
00080     _pid.update(_position, _velocity, _filter.getDt());
00081 }
00082 
00083 void OuterLoop::setCommand(float value) {
00084     _SetPoint = value;
00085     _pid.writeSetPoint(_SetPoint);
00086 }
00087  
00088 float OuterLoop::getCommand() {
00089     return _SetPoint;
00090 }
00091 
00092 float OuterLoop::getOutput() {
00093     /* PID output + offset to drive the motors to the correct position */
00094     return _pid.getOutput() + _offset;
00095 }
00096  
00097 void OuterLoop::refreshPVState() {
00098     _position = _filter.getPosition();
00099     _velocity = _filter.getVelocity();
00100 }
00101  
00102 float OuterLoop::getPosition() {
00103     return _position;
00104 }
00105  
00106 float OuterLoop::getVelocity() {
00107     return _velocity;
00108 }
00109  
00110 void OuterLoop::setControllerP(float P) {
00111     _Pgain = P;
00112     _pid.setPgain(_Pgain);
00113 }
00114  
00115 float OuterLoop::getControllerP() {
00116     return _Pgain;
00117 }
00118  
00119 void OuterLoop::setControllerI(float I) {
00120     _Igain = I;
00121     _pid.setIgain(_Igain);
00122 }
00123  
00124 float OuterLoop::getControllerI() {
00125     return _Igain;
00126 }
00127  
00128 void OuterLoop::setControllerD(float D) {
00129     _Dgain = D;
00130     _pid.setDgain(_Dgain);
00131 }
00132  
00133 float OuterLoop::getControllerD() {
00134     return _Dgain;
00135 }
00136 
00137 //uses position velocity filter class
00138 void OuterLoop::setFilterFrequency(float frequency) {
00139     _filterFrequency = frequency;
00140     _filter.writeWn(frequency);   
00141 }
00142 
00143 float OuterLoop::getFilterFrequency() {
00144     return _filterFrequency;               //new 7/11/18
00145 }
00146  
00147 void OuterLoop::setDeadband(float deadband) {
00148     _deadband = deadband;
00149     _pid.setDeadBand(_deadband);
00150 }
00151 
00152 float OuterLoop::getDeadband() {
00153     return _deadband;
00154 }
00155  
00156 bool OuterLoop::toggleDeadband(bool toggle) {
00157     _pid.toggleDeadBand(toggle);
00158     return toggle;
00159 }
00160 
00161 void OuterLoop::setOutputOffset(float offset) {
00162     _offset = offset;
00163 }
00164  
00165 float OuterLoop::getOutputOffset() {
00166     return _offset;
00167 }
00168 
00169 //not used yet
00170 void OuterLoop::setIHiLimit (float limit){
00171     _pid.setHiLimit(limit);
00172 }
00173 
00174 void OuterLoop::setILoLimit (float limit){
00175     _pid.setHiLimit(limit);    
00176 }