update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
OuterLoop/OuterLoop.cpp
- Committer:
- CodyMarquardt
- Date:
- 2019-06-28
- Revision:
- 99:9d0849f5fcd7
- Parent:
- 87:6d95f853dab3
File content as of revision 99:9d0849f5fcd7:
#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 setIHiLimit(3.0); //give each outerloop instance an integral saturation limit of some default setILoLimit(3.0); if (_sensor == 2) { _pid.setHeadingFlag(true); //used to update the handling for the heading PID control loop _filter.setHeadingFlag(true); } } // 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(); } void OuterLoop::runOuterLoop() { update(); } // 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(); // change to getting meters _sensorVal = depth().getDepthM(); } else if (_sensor == 1) { _sensorVal = imu().getPitch(); } else if (_sensor == 2) { _sensorVal = imu().getHeading(); } else if (_sensor == 3) { _sensorVal = sensors().getAltimeterReading_m(); // CAM changed from counts to meters // _sensorVal = imu().getPitch(); // 27mar2019 testing sensors error? } 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; } //uses position velocity filter class void OuterLoop::setFilterFrequency(float frequency) { _filterFrequency = frequency; _filter.writeWn(frequency); } float OuterLoop::getFilterFrequency() { return _filterFrequency; //new 7/11/18 } void OuterLoop::setDeadband(float deadband) { _deadband = deadband; _pid.setDeadBand(_deadband); } float OuterLoop::getDeadband() { return _deadband; } bool OuterLoop::toggleDeadband(bool toggle) { _pid.toggleDeadBand(toggle); return toggle; } void OuterLoop::setOutputOffset(float offset) { _offset = offset; } float OuterLoop::getOutputOffset() { return _offset; } //not used yet void OuterLoop::setIHiLimit (float limit){ _pid.setHiLimit(limit); } void OuterLoop::setILoLimit (float limit){ _pid.setHiLimit(limit); }