Diff: OuterLoop/OuterLoop.cpp

Revision:

62:d502889e74f1

Child:

73:f6f378311c8d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OuterLoop/OuterLoop.cpp	Mon Jun 18 14:45:37 2018 +0000
@@ -0,0 +1,166 @@
+#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);  //use the update handling for the heading PID control loop
+    }
+}
+
+// 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();
+    } 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);
+}
+
+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);    
+}
\ No newline at end of file