update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
Diff: OuterLoop/OuterLoop.cpp
- Revision:
- 55:f4ec445c42fe
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/OuterLoop/OuterLoop.cpp Tue Jun 12 15:41:51 2018 +0000 @@ -0,0 +1,143 @@ +#include "mbed.h" +#include "OuterLoop.hpp" +#include "StaticDefs.hpp" + +// this is where the variables that can be set are set when the object is created +OuterLoop::OuterLoop(float interval, int sensor): + _filter(), + _pid(), + _pulse() +{ + _Pgain = 0.5; + _Igain = 0.0; + _Dgain = 0.1; + + _filterFrequency = 2.0; + _deadband = 0.0; + + _dt = interval; + _sensor = sensor; // select the sensor ... hacky +} + +// initialize and start all of the member objects. +void OuterLoop::init() { + // load gains into pid controller this should eventually be in a config file + setFilterFrequency(_filterFrequency); + setControllerP(_Pgain); + setControllerI(_Igain); + setControllerD(_Dgain); + + // setup the controller object + toggleDeadband(true); + setDeadband(_deadband); +} + +// attaches the update function to an interval timer +void OuterLoop::start() { + _pulse.attach(this,&OuterLoop::update, _dt); +} + +// detaches the update function from the interval timer +void OuterLoop::stop() { + _pulse.detach(); +} + +// todo: these sensor reads should be somehow attached as callbacks, not hard-coded like this +void OuterLoop::update() { + // update the position velocity filter + if (_sensor == 0) { + _sensorVal = depth().getDepthFt(); + } else if (_sensor == 1) { + _sensorVal = imu().getPitch(); + } else if (_sensor == 2) { + _sensorVal = imu().getHeading(); + } else { + error("\n\r This sensor option does not exist"); + } + + // use the sensor reading to update the PVF + _filter.update(_dt, _sensorVal); + + // refresh the filter results and load into class variables + refreshPVState(); + + // update the PID controller with latest data + _pid.update(_position, _velocity, _filter.getDt()); +} + +void OuterLoop::setCommand(float value) { + _SetPoint = value; + _pid.writeSetPoint(_SetPoint); +} + +float OuterLoop::getCommand() { + return _SetPoint; +} + +float OuterLoop::getOutput() { + /* PID output + offset to drive the motors to the correct position */ + return _pid.getOutput() + _offset; +} + +void OuterLoop::refreshPVState() { + _position = _filter.getPosition(); + _velocity = _filter.getVelocity(); +} + +float OuterLoop::getPosition() { + return _position; +} + +float OuterLoop::getVelocity() { + return _velocity; +} + +void OuterLoop::setControllerP(float P) { + _Pgain = P; + _pid.setPgain(_Pgain); +} + +float OuterLoop::getControllerP() { + return _Pgain; +} + +void OuterLoop::setControllerI(float I) { + _Igain = I; + _pid.setIgain(_Igain); +} + +float OuterLoop::getControllerI() { + return _Igain; +} + +void OuterLoop::setControllerD(float D) { + _Dgain = D; + _pid.setDgain(_Dgain); +} + +float OuterLoop::getControllerD() { + return _Dgain; +} + +void OuterLoop::setFilterFrequency(float frequency) { + _filterFrequency = frequency; + _filter.writeWn(frequency); +} + +void OuterLoop::setDeadband(float deadband) { + _deadband = deadband; + _pid.setDeadBand(_deadband); +} + +bool OuterLoop::toggleDeadband(bool toggle) { + _pid.toggleDeadBand(toggle); + return toggle; +} + +void OuterLoop::setOutputOffset(float offset) { + _offset = offset; +} + +float OuterLoop::getOutputOffset() { + return _offset; +} \ No newline at end of file