Revision 10:085ab7328054, committed 2017-10-23

Comitter:

danstrider

Date:

Mon Oct 23 12:50:53 2017 +0000

Parent:

9:d5fcdcb3c89d

Child:

11:3b241ecb75ed

Commit message:

checked out on the hardware

Changed in this revision

--- a/Controller/controller.cpp	Fri Oct 20 11:41:22 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,77 +0,0 @@
-#include "controller.hpp"
-#include "StaticDefs.hpp"
-
-PIDController::PIDController()
-{
-    _integral = 0.0;
-    _lastTime = 0;
-    _loLimit = 0;
-    _hiLimit = 400;
-    _deadbandFlag = false;
-    _deadband = 0.5;
-
-}
-
-void PIDController::update(float position, float velocity, float dt)
-{
-
-    _error = _setPoint - position;
-
-    _integral += (_error*dt);
-
-    _output = _Pgain*_error + _Igain*_integral + _Dgain*velocity ;
-
-    // limiting on output & integral anti-windup
-    if (_output > 1.0) {
-        _output = 1.0;
-        _integral -= _error*dt;  //implement saturation instead of reset
-    } else if (_output  < -1) {
-        _output = -1.0;
-        _integral += _error*dt;  //implement saturation instead of reset
-    }
-
-    //add in some deadband
-    //add a case statement for deadband
-    //add a variable for deadband amount
-    if (_deadbandFlag) {
-        if (abs(_error) < _deadband) {
-            _output = 0.0;
-        }
-    }
-
-}
-
-void PIDController::writeSetPoint(float cmd)
-{
-    _setPoint = cmd;      //<float>(cmd, _loLimit, _hiLimit);
-}
-
-float PIDController::getOutput()
-{
-    return _output;
-}
-
-void PIDController::setPgain(float gain)
-{
-    _Pgain = gain;
-}
-
-void PIDController::setIgain(float gain)
-{
-    _Igain = gain;
-}
-
-void PIDController::setDgain(float gain)
-{
-    _Dgain = gain;
-}
-
-void PIDController::toggleDeadBand(bool toggle)
-{
-    _deadbandFlag = toggle;
-    return;
-}
-void PIDController::setDeadBand(float deadband)
-{
-    _deadband = deadband;
-}
\ No newline at end of file

--- a/Controller/controller.hpp	Fri Oct 20 11:41:22 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,64 +0,0 @@
-#ifndef MBED_DEPTHCONTROLLER_H
-#define MBED_DEPTHCONTROLLER_H
-
-#include "mbed.h"
-
-class PIDController
-{
-public:
-    PIDController();
-    
-    void update(float position, float velocity, float dt);
-    
-    float getOutput();
-    
-    void setPgain(float gain);
-    void setIgain(float gain);
-    void setDgain(float gain);
-    
-    void writeSetPoint(float cmd);
-    
-    void setHiLimit(float limit);
-    void setLoLimit(float limit);
-    
-    void toggleDeadBand(bool toggle); //implement this
-    void setDeadBand(float deadband); //implement this
-    
-protected:
-    float _setPoint;
-    float _Pgain;
-    float _Dgain;
-    float _Igain;
-    
-    float _hiLimit; //these variables clamp the allowable set points 
-    float _loLimit; //these variables clamp the allowable set points 
-    
-    float _error;
-    float _deadband;
-    bool _deadbandFlag;
-    
-    float _integral;
-    float _lastTime;
-    float _dt;
-    
-       
-    float _output;
-    
- //   bool configFlag;
- //   int readConfiguration();
-
-};
-
-template <typename T>
-T clamp(T value, T min, T max)
-{
-    if(value < min) {
-        return min;
-    } else if(value > max) {
-        return max;
-    } else {
-        return value;
-    }
-};
-
-#endif
\ No newline at end of file

--- a/IMU/IMU.cpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/IMU/IMU.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -1,73 +1,157 @@
 #include "IMU.h"
 
-IMU::IMU(): _rs232(p13,p14) //maybe this should configurable on construction
+IMU::IMU(PinName Tx, PinName Rx): 
+    _rs232(Tx,Rx)
 {
-
 }
 
-void IMU::initialize()
-{
+void IMU::initialize() {
     //set up the spi bus and frequency
     _rs232.baud(115200);
+
+    // initialize the processing state machine
+    state = SYNC0; 
     
-    // Define IMU packet type 
-    unsigned char SYNC1 = 0x75;             // First sync byte will always be 'u' (0x75)
-    unsigned char SYNC2 = 0x65;             // Second sync byte will always be 'e' (0x65)
-    unsigned char descripter_set = 0x80;    // Descriptor set byte for AHRS (0x80)
-    int payload_length = 0x0E;              // Payload length byte for CF Euler Angles (0x0E)
-    int field_length = 0x0E;                // Field length byte for CF Euler Angles (0x0E)
-    unsigned char data_descriptor = 0x0C;   // Data descriptor byte for CF Euler Angles (0x0C)
-    unsigned char data[30];                 // Data sent CF euler angles rpy [radians]      
-    int data_offset = 6;                    // Binary offset
-    int roll_offset = 2;                    // Binary offset
-    int pitch_offset = 6;                   // Binary offset
-    int yaw_offset = 10;                    // Binary offset
-    float euler[3];    
+    // initialize to zeros
+    euler[0] = 0.0;
+    euler[1] = 0.0;
+    euler[2] = 0.0;
     
-    int i = 0;// set packet_length based on field_length (convert from hex to int)
-    unsigned char current = 0x00;
-    unsigned char last = 0x00;
-
+    // initialize to zeros
+    latLonAlt[0] = 0.0;
+    latLonAlt[1] = 0.0;
+    latLonAlt[2] = 0.0;
+    
+    // initialize to no GNSS fix
+    is2dFixValid = false;
+    is3dFixValid = false;
+    numSV = 0;
 }
 
-//This starts an interupt driven trigger of the external ADC
-void IMU::start()
-{
+// this stops an interval timer trigger of the IMU update function
+void IMU::start() {
     interval.attach(this, &IMU::update, 1);  //this should be a 1 Hz sample rate
 }
 
-//This stops it
-void IMU::stop()
-{
+// this stops the interval timer trigger of the IMU update function
+void IMU::stop() {
     interval.detach();
 }
 
+// updated the imu update function with a state machine that doesn't hang if no data is present
+void IMU::update() {
+    while (_rs232.readable()) {
+        // read a single byte
+        byte = _rs232.getc();
+        
+        // state machine to process byte-by-byte
+        switch (state) {
+        case SYNC0 : 
+            if (byte == 0x75) {
+                state = SYNC1;
+            }
+            break;
+            
+        case SYNC1 :
+            if (byte == 0x65) {
+                state = SET;
+            }
+            state = SYNC0;
+            break;
+            
+        case SET :
+            descriptor = byte; // save descriptor set
+            state = LEN;
+            break;
+            
+        case LEN :
+            len = byte; // save payload length
+            state = PAY;
+            i = 0; // reset payload length counter
+            break;
+            
+        case PAY :
+            if (i < len) {
+                payload[i] = byte; // add byte to the payload array
+                i++; // increment payload counter
+            }
+            else {
+                state = CRC0;
+            }
+            break;
+            
+        case CRC0 :
+            crc0 = byte;
+            state = CRC1;
+            break;
+            
+        case CRC1 :
+            crc1 = byte;
+            checksum = crc0<<8 + crc1; // make checksum
+            if (checksum == calcChecksum(payload, len)) {
+                processPacket(descriptor, len, payload); // process packet
+            }
+            state = SYNC0; // reset to SYNC0 state
+            break;
 
-void IMU::update()
-{
-    last = current;
-    current = _rs232.getc();        
-    if (last == SYNC1 && current == SYNC2){  // Detect beginning of packet
-        data[0] = last;
-        data[0] = current;
-        data[0] = _rs232.getc();
-        data[0] = _rs232.getc();
-        i = 0;
-        while(i < field_length){            // Get data
-            data[i] = _rs232.getc();
-            i += 1;
+        default :
+            state = SYNC0; 
         }
-        data[i + data_offset + 1] = _rs232.getc();     // Check sum 1
-        data[i + data_offset + 2] = _rs232.getc();     // Check sum 2    
-    }       
-    euler[0] = float_from_char(&data[roll_offset])*180/_PI;       // roll Euler angle convert to float
-    euler[1] = float_from_char(&data[pitch_offset])*180/_PI;      // pitch Euler angle convert to float
-    euler[2] = float_from_char(&data[yaw_offset])*180/_PI;        // yaw Euler angle convert to float      
-    return ;
+    }
+    return;
 }
 
-float IMU::float_from_char(unsigned char * value)
-{
+void IMU::processPacket(char descriptor, char length, unsigned char * payload) {
+    if (descriptor == IMU_DATA_SET) { // find an IMU data descriptor set
+        if (length > 2) { // make sure there are at least two bytes to see the field descriptor
+            if (payload[1] == EULER_CF_DESCRIPTOR) { // find an euler CF field descriptor
+                processEulerCfPacket(length, payload);
+            }
+        }
+    }
+    else if (descriptor == GNSS_DATA_SET) { // find a GNSS data descriptor set
+        if (length > 2) { // make sure there are at least two bytes to see the field descriptor
+            if (payload[1] == LLH_POSITION_DESCRIPTOR) { // find a lat-lon-alt field descriptor
+                processLatLonAltPacket(length, payload);
+            }
+            else if (payload[1] == GNSS_FIX_INFO_DESCRIPTOR) { // find a gnss fix field descriptor
+                processGnssFixInformation(length, payload);
+            }
+        }
+    }
+}
+
+void IMU::processEulerCfPacket(char length, unsigned char * payload) {
+    if (length >= EULER_CF_LENGTH) { // make sure correct field length
+        if (payload[0] == EULER_CF_LENGTH) { // make sure field length is as expected
+            euler[0] = floatFromChar(&payload[ROLL_OFFSET])*180/_PI;  // roll Euler angle convert in degrees
+            euler[1] = floatFromChar(&payload[PITCH_OFFSET])*180/_PI; // pitch Euler angle convert in degrees
+            euler[2] = floatFromChar(&payload[YAW_OFFSET])*180/_PI;   // yaw Euler angle convert in degrees
+        }
+    }
+}
+
+void IMU::processLatLonAltPacket(char length, unsigned char * payload) {
+    if (length >= LLH_POSITION_LENGTH) { // make sure correct field length
+        if (payload[0] == LLH_POSITION_LENGTH) { // make sure field length is as expected
+            latLonAlt[0] = doubleFromChar(&payload[LATITUDE_OFFSET]);   // latitude in decimal degrees
+            latLonAlt[1] = doubleFromChar(&payload[LONGITUDE_OFFSET]);  // longitude in decimal degrees
+            latLonAlt[2] = doubleFromChar(&payload[HEIGHT_MSL_OFFSET]); // altitude above mean sea level in meters
+        }
+    }
+}
+
+void IMU::processGnssFixInformation(char length, unsigned char * data) {
+    if (length >= GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected
+        if (payload[0] == GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected
+            is2dFixValid = char(payload[FIX_TYPE_OFFSET]) & 0x01; // bitand on LSB to see 2d fix flag
+            is3dFixValid = char(payload[FIX_TYPE_OFFSET]) & 0x02; // bitand on LSB to see 3d fix flag
+            numSV = char(payload[NUM_SV_OFFSET]); // number of GNSS satellite vehicles
+        }
+    }
+}
+
+float IMU::floatFromChar(unsigned char * value) {
     unsigned char temp[4];
     temp[0] = value[3];
     temp[1] = value[2];
@@ -76,3 +160,63 @@
     return *(float *) temp;
 }
 
+double IMU::doubleFromChar(unsigned char * value) {
+    unsigned char temp[8];
+    temp[0] = value[7];
+    temp[1] = value[6];
+    temp[2] = value[5];
+    temp[3] = value[4];
+    temp[4] = value[3];
+    temp[5] = value[2];
+    temp[6] = value[1];
+    temp[7] = value[0];
+    return *(double *) temp;
+}
+
+float IMU::getRoll() {
+    return euler[0];
+}
+
+float IMU::getPitch() {
+    return euler[1];
+}
+
+float IMU::getHeading() {
+    return euler[2];
+}
+
+bool IMU::getIsValid2dFix() {
+    return is2dFixValid;   
+}
+
+bool IMU::getIsValid3dFix() {
+    return is3dFixValid;
+}
+
+char IMU::getNumSV() {
+    return numSV;
+}
+
+double IMU::getLatitude() {
+    return latLonAlt[0];
+}
+
+double IMU::getLongitude() {
+    return latLonAlt[1];
+}
+
+double IMU::getAltitudeMSL() {
+    return latLonAlt[2];
+}
+
+unsigned int IMU::calcChecksum(unsigned char * mip_packet, char checksum_range) {
+    unsigned char checksum_byte1 = 0;
+    unsigned char checksum_byte2 = 0;
+    
+    for (int i=0; i<checksum_range; i++) {
+        checksum_byte1 += mip_packet[i];
+        checksum_byte2 += checksum_byte1;
+    }
+    checksum = ((unsigned int)checksum_byte1 << 8) + (unsigned int)checksum_byte2;
+    return checksum;
+}
\ No newline at end of file

--- a/IMU/IMU.h	Fri Oct 20 11:41:22 2017 +0000
+++ b/IMU/IMU.h	Mon Oct 23 12:50:53 2017 +0000
@@ -3,34 +3,80 @@
 
 #define _PI ((float) 3.14159265359)
 
-class IMU{
+// state machine states
+#define SYNC0 0 // first sync byte state
+#define SYNC1 1 // second sync byte state
+#define SET   2 // aka descriptor set
+#define LEN   3 // payload length
+#define PAY   4 // getting the payload data
+#define CRC0  5 // crc high byte
+#define CRC1  6 // crc low byte
+
+// data set descriptors
+#define IMU_DATA_SET                0x08
+#define GNSS_DATA_SET               0x81
+
+// enumerations for the Euler angle packet we care about
+#define EULER_CF_LENGTH             14
+#define EULER_CF_DESCRIPTOR         0x0C
+#define ROLL_OFFSET                 0
+#define PITCH_OFFSET                4
+#define YAW_OFFSET                  8
+
+// enumerations for the lat-lon-alt packet we care about
+#define LLH_POSITION_LENGTH         44
+#define LLH_POSITION_DESCRIPTOR     0x03
+#define LATITUDE_OFFSET             0
+#define LONGITUDE_OFFSET            8
+#define HEIGHT_MSL_OFFSET           16
+#define VALID_FLAG_OFFSET           40
+
+// enumerations for the lat-lon-alt packet we care about
+#define GNSS_FIX_INFO_LENGTH        8
+#define GNSS_FIX_INFO_DESCRIPTOR    0x0B
+#define FIX_TYPE_OFFSET             0
+#define NUM_SV_OFFSET               1
+#define VALID_FLAGS_OFFSET          4
+
+class IMU {
 public:
-    IMU();
+    IMU(PinName Tx, PinName Rx);
     void initialize();
     void update();
     void start();
     void stop();
+    
+    float getRoll();
+    float getPitch();
+    float getHeading();
    
+    bool getIsValid2dFix();
+    bool getIsValid3dFix();
+    char getNumSV();
+    double getLatitude();
+    double getLongitude();
+    double getAltitudeMSL();
+
 protected:
     Ticker interval;
     MODSERIAL _rs232;
-    unsigned char SYNC1;             // First sync byte will always be 'u' (0x75)
-    unsigned char SYNC2;             // Second sync byte will always be 'e' (0x65)
-    unsigned char descripter_set;    // Descriptor set byte for AHRS (0x80)
-    int payload_length;              // Payload length byte for CF Euler Angles (0x0E)
-    int field_length;                // Field length byte for CF Euler Angles (0x0E)
-    unsigned char data_descriptor;   // Data descriptor byte for CF Euler Angles (0x0C)
-    unsigned char data[30];          // Data sent CF euler angles rpy [radians]      
-    int data_offset;                 // Binary offset
-    int roll_offset;                 // Binary offset
-    int pitch_offset;                // Binary offset
-    int yaw_offset;                  // Binary offset
-    float euler[3];    
+    
+    char byte;
+    unsigned char state, len, descriptor, i, crc0, crc1, payload[30], checksum;
     
-    int i;                       // set packet_length based on field_length (convert from hex to int)
-    unsigned char current;
-    unsigned char last;
-    
-    float float_from_char(unsigned char * value);
-};
+    float euler[3];
+    double latLonAlt[3];
+    bool is2dFixValid;
+    bool is3dFixValid;
+    char numSV;
 
+    void processPacket(char type, char length, unsigned char * data);
+    void processEulerCfPacket(char length, unsigned char * data);
+    void processLatLonAltPacket(char length, unsigned char * data);
+    void processGnssFixInformation(char length, unsigned char * data);
+
+    unsigned int calcChecksum(unsigned char * mip_packet, char checksum_range);
+
+    float floatFromChar(unsigned char * value);
+    double doubleFromChar(unsigned char * value);
+};
\ No newline at end of file

--- a/LTC1298/ltc1298.cpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/LTC1298/ltc1298.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -1,43 +1,45 @@
 #include "ltc1298.hpp"
 
-SpiADC::SpiADC(): _spi(p5,p6,p7),adcLed(LED2),cs(p8) //maybe this should configurable on construction
+SpiADC::SpiADC(PinName mosi, PinName miso, PinName sclk, PinName csel, PinName led) :
+    _spi(mosi, miso, sclk), // mosi, miso, sclk
+    adcLed(led), // status led
+    cs(csel) // chip select
 {
-
 }
 
-void SpiADC::initialize()
-{
+void SpiADC::initialize() {
     //set up the spi bus and frequency
     _spi.format(13,0);
     _spi.frequency(1000000);
+    
     //chip select high puts ltc1298 in standby
-    adcLed = 1;
+    cs = 1;
+    
+    //zero the initial ch0 and ch1 oversampled readings    
     ch0_filt = 0;
     ch1_filt = 0;
-    cs = 1;
 
+    //led on to say hello
+    adcLed = 1;
 }
 
-//This starts an interupt driven trigger of the external ADC
-void SpiADC::start()
-{
+// start an interupt driven trigger of the external ADC
+void SpiADC::start() {
     interval.attach_us(this, &SpiADC::update, 100);  //this should be a 10 kHz sample rate
 }
 
-//This stops it
-void SpiADC::stop()
-{
+// stop the interupt driven trigger
+void SpiADC::stop() {
     interval.detach();
 }
 
-
-void SpiADC::update()
-{
+void SpiADC::update() {
     //flash the LED
     adcLed = !adcLed;
 
     //chip select low starts data conversion
     cs = 0;
+    
     //the next thing is the input data word
     //it is 4 bits and looks like this
     // | start | single/diff | odd/sign | MSB first/LSB first |
@@ -65,12 +67,10 @@
     return ;
 }
 
-int SpiADC::readCh0()
-{
+int SpiADC::readCh0() {
     return ch0_filt;
 }
 
-int SpiADC::readCh1()
-{
+int SpiADC::readCh1() {
     return ch1_filt;
 }
\ No newline at end of file

--- a/LTC1298/ltc1298.hpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/LTC1298/ltc1298.hpp	Mon Oct 23 12:50:53 2017 +0000
@@ -2,12 +2,12 @@
 #define MBED_LTC1298_H
 #include "mbed.h"
 
-#define CH0OVERSAMPLE 4
+#define CH0OVERSAMPLE 8
 #define CH1OVERSAMPLE 8
 
 class SpiADC{
 public:
-    SpiADC();
+    SpiADC(PinName mosi, PinName miso, PinName sclk, PinName csel, PinName led);
     void initialize();
     void update();
     void start();
@@ -16,21 +16,17 @@
     int readCh0();
     int readCh1();
     
-    
 protected:
     SPI _spi;
     Ticker interval;
     DigitalOut adcLed;
     DigitalOut cs;
-    void poll();
+
     int ch0_raw;
     int ch1_raw;
     
     int ch0_filt;
     int ch1_filt;
-    
-    int result;  
-        
 };
 
 

--- a/LinearActuator/LinearActuator.cpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/LinearActuator/LinearActuator.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -3,16 +3,16 @@
 #include "StaticDefs.hpp"
 #include "ConfigFile.h"
  
+// this is where the variables that can be set are set when the object is created
 LinearActuator::LinearActuator(float interval, PinName pwm, PinName dir, PinName reset, PinName limit, int adc_ch):
     _motor(pwm, dir, reset),
     _filter(),
     _pid(),
-    //_time(),
     _pulse(),
     _limitSwitch(limit)
 {
     _limitSwitch.fall(callback(this, &LinearActuator::_switchPressed));
-    // this is where the variables that can be set are set when the object is created
+    
     _Pgain = 0.10;
     _Igain = 0.0;
     _Dgain = 0.0;
@@ -31,8 +31,7 @@
  
 }
  
-void LinearActuator::init()
-{
+void LinearActuator::init() {
     // initialize and start all of the member objects.
     // The pos-velocity filter for this item needs to be allowed to converge
     // Before turning over control to the motor
@@ -48,14 +47,10 @@
     //set deadband and limits
     toggleDeadband(true);
     setDeadband(_deadband);
- 
 }
  
-void LinearActuator::update()
-{
- 
- 
-    //update the position velocity filter
+void LinearActuator::update() { 
+    // update the position velocity filter
     if (_adc_channel == 0) {
         _filter.update(_dt, adc().readCh0());
     } else if (_adc_channel == 1) {
@@ -64,7 +59,7 @@
         error("\n\r This ADC channel does not exist");
     }
  
-    //refresh the filter results and load into class variables
+    // refresh the filter results and load into class variables
     refreshPVState();
  
     // update the PID controller with latest data
@@ -110,183 +105,154 @@
             //dont run
             return;
         }
-        _motor.run(_pid.getOutput());
+        
+        // clamp the output to the motor to -1.0 to 1.0
+        if (_pid.getOutput() > 1.0)
+            _motor.run(1.0);
+        else if (_pid.getOutput() < -1.0)
+            _motor.run(-1.0);
+        else 
+            _motor.run(_pid.getOutput());
     }
 }
  
-void LinearActuator::start()
-{
+void LinearActuator::start() {
     _init = true;
     _pulse.attach(this,&LinearActuator::update, _dt);
-    
 }
  
-void LinearActuator::stop()
-{
+void LinearActuator::stop() {
     _motor.stop();
     _pulse.detach();
 }
  
-void LinearActuator::pause()
-{
+void LinearActuator::pause() {
     //this allows the controller to keep running while turning off the motor output
     _motor.stop();
     //paused flag causes controller output not to be piped to the motor
     _paused = true;
 }
  
-void LinearActuator::unpause()
-{
+void LinearActuator::unpause() {
     //this resumes motor operation
     _paused = false;
 }
  
-void LinearActuator::refreshPVState()
-{
+void LinearActuator::refreshPVState() {
     _position = _filter.getPosition();
     _velocity = _filter.getVelocity();
  
     _position_mm = counts_to_dist(_position);
     _velocity_mms = counts_to_velocity(_velocity);
- 
-//    _position_mL = _mm_to_mL(_position_mm);
-//    _velocity_mLs = _mm_to_mL(_velocity_mms);
- 
 }
  
 // setting and getting variables
-void LinearActuator::setPosition_mm(float dist)
-{
-    // convert mm representation to volume in mL
-//    _SetPoint_mL = _mm_to_mL(dist);
+void LinearActuator::setPosition_mm(float dist) {
     _SetPoint_mm = clamp<float>(dist, 0.0, _extendLimit);  //this is another spot that prevents the requested set point from going out of range, this template function is defined in the controller header file fyi
  
     _pid.writeSetPoint(_SetPoint_mm);
- 
 }
  
-float LinearActuator::getPosition_mm()
-{
+float LinearActuator::getPosition_mm() {
     return _position_mm;
 }
  
-float LinearActuator::getPosition_counts()
-{
-    return _position; // returns raw adc counts (useful for zeroing)
-}
+//float LinearActuator::getPosition_counts() {
+//    return _position; // returns raw adc counts (useful for zeroing)
+//}
  
-float LinearActuator::getVelocity_mms()
-{
+float LinearActuator::getVelocity_mms() {
     return _velocity_mms;
 }
  
-void LinearActuator::setControllerP(float P)
-{
+void LinearActuator::setControllerP(float P) {
     _Pgain = P;
     _pid.setPgain(_Pgain);
     return;
 }
  
-float LinearActuator::getControllerP()
-{
+float LinearActuator::getControllerP() {
     return _Pgain;
 }
  
-void LinearActuator::setControllerI(float I)
-{
+void LinearActuator::setControllerI(float I) {
     _Igain = I;
     _pid.setIgain(_Igain);
     return;
 }
  
-float LinearActuator::getControllerI()
-{
+float LinearActuator::getControllerI() {
     return _Igain;
 }
  
-void LinearActuator::setControllerD(float D)
-{
+void LinearActuator::setControllerD(float D) {
     _Dgain = D;
     _pid.setDgain(_Dgain);
     return;
 }
  
-float LinearActuator::getControllerD()
-{
+float LinearActuator::getControllerD() {
     return _Dgain;
 }
  
-float LinearActuator::getOutput()
-{
+float LinearActuator::getOutput() {
     return _pid.getOutput();
 }
- 
-void LinearActuator::setZeroCounts(int zero)
-{
+
+void LinearActuator::setZeroCounts(int zero) {
     _zeroCounts = clamp<int>(zero, 0, 4096);
     return;
 }
  
-int LinearActuator::getZeroCounts()
-{
+int LinearActuator::getZeroCounts() {
     return _zeroCounts;    
 }
  
-void LinearActuator::setTravelLimit(float limit)
-{
+void LinearActuator::setTravelLimit(float limit) {
     _extendLimit = limit;
     return;
 }
  
-float LinearActuator::getTravelLimit()
-{
+float LinearActuator::getTravelLimit() {
     return _extendLimit;   
 }
  
-void LinearActuator::setPotSlope(float slope)
-{
+void LinearActuator::setPotSlope(float slope) {
     _slope = slope;
     return;
 }
  
-float LinearActuator::getPotSlope()
-{
+float LinearActuator::getPotSlope() {
     return _slope;    
 }
  
-float LinearActuator::counts_to_dist(int count)
-{
+float LinearActuator::counts_to_dist(int count) {
     float conv = _slope*(count-_zeroCounts);
     return conv;
 }
  
-void LinearActuator::setFilterFrequency(float frequency)
-{
+void LinearActuator::setFilterFrequency(float frequency) {
     _filterFrequency = frequency;
     _filter.writeWn(frequency);   
 }
  
-int LinearActuator::dist_to_counts(float dist)
-{
+int LinearActuator::dist_to_counts(float dist) {
     float conv = (dist/_slope)+_zeroCounts;
     return (int) conv;
 }
  
-float LinearActuator::counts_to_velocity(int count)
-{
+float LinearActuator::counts_to_velocity(int count) {
     float conv = count*_slope;
     return conv;
 }
  
-void LinearActuator::_switchPressed()
-{
+void LinearActuator::_switchPressed() {
     //first thing to do is stop the motor
     _motor.stop();
     _limit = true;
 }
  
-void LinearActuator::homePiston()
-{
+void LinearActuator::homePiston() {
     //start calling the update for the Linear Actuator
     //start the controller update and immediatley pause the motor output
     start();
@@ -299,8 +265,8 @@
     
     //Now that the readings are stabilized
     // This sends the motor on a kamakaze mission toward the limit switch
-    // The interrupt should catch and stop it and the piston is now at home
-    //position
+    // The interrupt should catch and stop it, and the piston is now at home
+    // position
     _motor.run(-1.0);
     
     while (1) {
@@ -323,21 +289,17 @@
     }
 }
  
-bool LinearActuator::getSwitch()
-{
+bool LinearActuator::getSwitch() {
     return _limit;
 }
  
- void LinearActuator::setDeadband(float deadband)
-{
+ void LinearActuator::setDeadband(float deadband) {
     _deadband = deadband;
     _pid.setDeadBand(_deadband);
     return;    
 }
  
-bool LinearActuator::toggleDeadband(bool toggle)
-{
+bool LinearActuator::toggleDeadband(bool toggle) {
     _pid.toggleDeadBand(toggle);
     return toggle;
-}   
-            
\ No newline at end of file
+}         
\ No newline at end of file

--- a/LinearActuator/LinearActuator.hpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/LinearActuator/LinearActuator.hpp	Mon Oct 23 12:50:53 2017 +0000
@@ -1,9 +1,9 @@
-#ifndef BCE_HPP
-#define BCE_HPP
+#ifndef LINEARACTUATOR_HPP
+#define LINEARACTUATOR_HPP
  
 #include "mbed.h"
 #include "PololuHbridge.hpp"
-#include "controller.hpp"
+#include "PidController.hpp"
 #include "ltc1298.hpp"
 #include "PosVelFilter.hpp"
  
@@ -13,10 +13,8 @@
 //for this reason it makes sense for it to be its own entity that is started in
 //the main line code
  
-class LinearActuator
-{
+class LinearActuator {
 public:
-    //BuoyancyEngine(PololuHBridge motor, SpiADC adc, PosVelFilter filter, PositionController pid);
     LinearActuator(float interval, PinName pwm, PinName dir, PinName reset, PinName limit, int adc_ch);
     
     // functions for setting up
@@ -31,11 +29,9 @@
     
     // setting and getting variables
     void setPosition_mm(float dist);
-    
     float getPosition_mm();
-    float getPosition_counts();
-    
     float getVelocity_mms();
+//    float getPosition_counts();
     
     void setControllerP(float P);
     float getControllerP();
@@ -56,11 +52,11 @@
     float getPotSlope();
     
     void homePiston();
-    void setFilterFrequency(float frequency);
+    bool getSwitch();
     
     float getOutput();
     
-    bool getSwitch();
+    void setFilterFrequency(float frequency);
     
     void setDeadband(float deadband);
     bool toggleDeadband(bool toggle);
@@ -69,7 +65,6 @@
     PololuHBridge _motor;
     PosVelFilter _filter;
     PIDController _pid;
-    //Timer _time;
     Ticker _pulse;
     InterruptIn _limitSwitch;
     
@@ -77,7 +72,6 @@
     void _switchReleased();
     void _calculateSensorSlope();
     
-    
     bool _init;
     bool _paused;
     bool _limit;
@@ -90,12 +84,15 @@
     
     float _SetPoint_mm;
     
+    // position and velocity in counts (PVF runs on counts)
     float _position;
+    float _velocity;
+
+    // position and velocity converted to mm and mm/s
     float _position_mm;
-    
-    float _velocity;
     float _velocity_mms;
     
+    // linear actuator servo PID gains
     float _Pgain;
     float _Igain;
     float _Dgain;
@@ -104,7 +101,6 @@
     
     int _zeroCounts; //gets assigned by homing function. can also be stored in config
     float _extendLimit; //config variable, limits the extension of the piston, this is same as datum for normal operation,
-    float _pos_vel_wn;  //config variable, natural frequecny of the position velocity filter  
     
     float _slope;
     int dist_to_counts(float dist);
@@ -112,5 +108,18 @@
     float counts_to_velocity(int count);
  
 };
- 
+
+
+template <typename T>
+T clamp(T value, T min, T max)
+{
+    if(value < min) {
+        return min;
+    } else if(value > max) {
+        return max;
+    } else {
+        return value;
+    }
+};
+
 #endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OuterLoop/OuterLoop.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -0,0 +1,130 @@
+#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().getDepth();
+    } 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::getOutput() {
+    return _pid.getOutput();
+}
+ 
+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;
+}         
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OuterLoop/OuterLoop.hpp	Mon Oct 23 12:50:53 2017 +0000
@@ -0,0 +1,69 @@
+#ifndef OUTERLOOP_HPP
+#define OUTERLOOP_HPP
+ 
+#include "mbed.h"
+#include "PidController.hpp"
+#include "PosVelFilter.hpp"
+ 
+// This class is an outer loop controller with its own instance of a position velocity filter.
+ 
+class OuterLoop {
+public:
+    OuterLoop(float interval, int sensor);
+    
+    // functions for setting up
+    void init();
+    void update();
+    void start();
+    void stop();
+    
+    // setting and getting variables
+    void setCommand(float cmd);
+    float getOutput();
+    
+    float getPosition();
+    float getVelocity();
+    
+    void setControllerP(float P);
+    float getControllerP();
+    
+    void setControllerI(float I);
+    float getControllerI();
+    
+    void setControllerD(float D);
+    float getControllerD();
+    
+    void setTravelLimit(float limit);
+    float getTravelLimit();
+    
+    void setFilterFrequency(float frequency);
+    float getFilterFrequency;
+        
+    void setDeadband(float deadband);
+    bool toggleDeadband(bool toggle);
+    
+protected:
+    PosVelFilter _filter;
+    PIDController _pid;
+    Ticker _pulse;
+    
+    void refreshPVState();
+    
+    float _SetPoint;
+    float _sensorVal;
+
+    // position and velocity in raw units
+    float _position;
+    float _velocity;
+
+    // setup parameters
+    float _Pgain;
+    float _Igain;
+    float _Dgain;
+    float _dt;
+    float _filterFrequency;
+    float _deadband;
+    char _sensor;
+};
+ 
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PidController/PidController.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -0,0 +1,63 @@
+#include "PidController.hpp"
+
+PIDController::PIDController() {
+    _error = 0.0;
+    _integral = 0.0;
+    _loLimit = 0.0;
+    _hiLimit = 0.0;
+    _deadbandFlag = false;
+    _deadband = 0.0;
+}
+
+void PIDController::update(float position, float velocity, float dt) {
+    // error update equations
+    _error = _setPoint - position;
+    _integral += (_error*dt);
+
+    // pid controller equation
+    _output = _Pgain*_error + _Igain*_integral + _Dgain*velocity ;
+
+    // limiting on output & integral anti-windup
+    if (_hiLimit != 0.0 and _output > _hiLimit) {
+        _output = _hiLimit;
+        _integral -= _error*dt;  // implement saturation
+    }
+    if (_loLimit != 0.0 and _output < _loLimit) {
+        _output = _loLimit;
+        _integral += _error*dt;  // implement saturation
+    }
+
+    // within deadband on error zeros output
+    if (_deadbandFlag) {
+        if (abs(_error) < _deadband) {
+            _output = 0.0;
+        }
+    }
+}
+
+void PIDController::writeSetPoint(float cmd) {
+    _setPoint = cmd;
+}
+
+float PIDController::getOutput() {
+    return _output;
+}
+
+void PIDController::setPgain(float gain) {
+    _Pgain = gain;
+}
+
+void PIDController::setIgain(float gain) {
+    _Igain = gain;
+}
+
+void PIDController::setDgain(float gain) {
+    _Dgain = gain;
+}
+
+void PIDController::toggleDeadBand(bool toggle) {
+    _deadbandFlag = toggle;
+}
+void PIDController::setDeadBand(float deadband) {
+    _deadband = deadband;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PidController/PidController.hpp	Mon Oct 23 12:50:53 2017 +0000
@@ -0,0 +1,40 @@
+#ifndef PIDCONTROLLER_H
+#define PIDCONTROLLER_H
+
+#include "mbed.h"
+
+class PIDController {
+public:
+    PIDController();
+    
+    void update(float position, float velocity, float dt);
+    float getOutput();
+    
+    void setPgain(float gain);
+    void setIgain(float gain);
+    void setDgain(float gain);
+    
+    void writeSetPoint(float cmd);
+    
+    void setHiLimit(float limit);
+    void setLoLimit(float limit);
+    
+    void toggleDeadBand(bool toggle);
+    void setDeadBand(float deadband);
+    
+protected:
+    float _setPoint;
+    float _error;
+    float _integral;
+    float _output;
+
+    float _Pgain;
+    float _Dgain;
+    float _Igain;
+    float _hiLimit; //these parameters clamp the allowable output
+    float _loLimit; //these parameters clamp the allowable output
+    float _deadband;
+    bool _deadbandFlag;
+};
+
+#endif
\ No newline at end of file

--- a/PosVelFilter/PosVelFilter.cpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/PosVelFilter/PosVelFilter.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -1,51 +1,39 @@
 #include "PosVelFilter.hpp"
-#include "StaticDefs.hpp"
-#include "conversions.hpp"
+//#include "StaticDefs.hpp"
 
-PosVelFilter::PosVelFilter()
-{
-    x1 = 0;
-    x2 = 0;
-    //w_n is the natural frequency of the filter bigger increases frequency response
-    w_n = 1.0;
-
+PosVelFilter::PosVelFilter() {
+    x1 = 0; // pseudo position state
+    x2 = 0; // pseudo velocity state
+    
+    w_n = 1.0; // natural frequency of the filter bigger increases frequency response
 }
 
-void PosVelFilter::update(float deltaT, float counts)
-{
-    //run the pos-vel estimate filter
-    //this derives the timing from last run
-    //last_time = curr_time;
-    //curr_time = time;
+//run the pos-vel estimate filter
+void PosVelFilter::update(float deltaT, float counts) {
     dt = deltaT;
 
     x1_dot = x2;
-    x2_dot = (-2.0*w_n*x2)-(w_n*w_n)*x1+(w_n*w_n)*counts;
+    x2_dot = (-2.0*w_n*x2) - (w_n*w_n)*x1 + (w_n*w_n)*counts;
 
     position = x1;
     velocity = x2;
 
     x1 += x1_dot*dt;
     x2 += x2_dot*dt;
-
 }
 
-float PosVelFilter::getPosition()
-{
+float PosVelFilter::getPosition() {
     return position;
 }
 
-float PosVelFilter::getVelocity()
-{
+float PosVelFilter::getVelocity() {
     return velocity;
 }
 
-float PosVelFilter::getDt()
-{
+float PosVelFilter::getDt() {
     return dt;
 }
 
-void PosVelFilter::writeWn(float wn)
-{
+void PosVelFilter::writeWn(float wn) {
     w_n = wn;
 }
\ No newline at end of file

--- a/PosVelFilter/PosVelFilter.hpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/PosVelFilter/PosVelFilter.hpp	Mon Oct 23 12:50:53 2017 +0000
@@ -1,5 +1,5 @@
-#ifndef MBED_DEPTHFILTER_H
-#define MBED_DEPTHFILTER_H
+#ifndef POSVELFILTER_H
+#define POSVELFILTER_H
 
 #include "mbed.h"
 
@@ -16,7 +16,6 @@
     float getVelocity();
     float getDt();
     
- //   void setConfigFlag();
     void writeWn(float wn);
     
 protected:
@@ -26,17 +25,9 @@
     float x1_dot;
     float w_n; 
     
-    //float last_time;
-    //float curr_time;
     float dt;
-    
-    float conv_distance;
     float position;
     float velocity;
-    
- //   bool readConfiguration();
- //   bool configFlag;
- //   int count;
 };
 
 #endif
\ No newline at end of file

--- a/System/StaticDefs.cpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/System/StaticDefs.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -1,53 +1,79 @@
 #include "StaticDefs.hpp"
 
-//Declare static global variables using 'construct on use' idiom to ensure they are always constructed correctly
-// and avoid "static initialization order fiasco".
+//Declare static global variables using 'construct on use' idiom to ensure they 
+// are always constructed correctly and avoid "static initialization order fiasco".
 
-Timer &systemTime()
-{
+Timer & systemTime() {
     static Timer s;
     return s;
 }
 
-Serial & pc()
-{
-    //static MODSERIAL p(USBTX, USBRX, 512, 512); // tx, rx
-    static Serial p(USBTX, USBRX);
+Ticker & pulse() {
+    static Ticker pulse;
+    return pulse;   
+}
+    
+Serial & pc() {
+    static Serial p(USBTX, USBRX); // tx, rx
     return p;
 }
 
-SpiADC & adc()
-{
-    static SpiADC adc;
+LocalFileSystem & local() {
+    static LocalFileSystem local("local");
+    return local;    
+} 
+
+SpiADC & adc() {
+    static SpiADC adc(p5,p6,p7,p8,LED2);
     return adc;
 }
 
-LinearActuator & bce()
-{
+LinearActuator & bce() {
     static LinearActuator bce(0.01, p25, p29, p30, p18, 0); //interval , pwm, dir, reset, limit switch, adc channel
     return bce;
 }
 
-LinearActuator & batt()
-{
+LinearActuator & batt() {
     static LinearActuator batt(0.01, p23, p21, p22, p17, 1); //interval , pwm, dir, reset, limit switchm, adc channel
     return batt;       
 }
 
-LocalFileSystem & local()
-{
-    static LocalFileSystem local("local");
-    return local;    
-} 
+omegaPX209 & depth() {
+    static omegaPX209 depth(p19); // pin
+    return depth;
+}
 
-PIDController & DepthController()
-{
-    static PIDController DepthController;
-    return DepthController;
+IMU & imu() {
+    static IMU imu(p13,p14); // tx, rx pin
+    return imu;    
+}
+
+OuterLoop & depthLoop() {
+    static OuterLoop depthLoop(0.1, 0); // interval, sensor type
+    return depthLoop;
+}
+
+OuterLoop & pitchLoop() {
+    static OuterLoop pitchLoop(0.1, 1); // interval, sensor type
+    return pitchLoop;
 }
 
-IMU & Microstrain()
-{
-    static IMU Microstrain;
-    return Microstrain;    
+DigitalOut & led1() {
+    static DigitalOut led1(LED1);
+    return led1;
+}
+
+DigitalOut & led2() {
+    static DigitalOut led2(LED2);
+    return led2;
+}
+
+DigitalOut & led3() {
+    static DigitalOut led3(LED3);
+    return led3;
+}
+
+DigitalOut & led4() {
+    static DigitalOut led4(LED4);
+    return led4;
 }
\ No newline at end of file

--- a/System/StaticDefs.hpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/System/StaticDefs.hpp	Mon Oct 23 12:50:53 2017 +0000
@@ -6,22 +6,34 @@
 #include "ltc1298.hpp"
 #include "LinearActuator.hpp"
 #include "IMU.h"
-
+#include "omegaPX209.hpp"
+#include "PosVelFilter.hpp"
+#include "OuterLoop.hpp"
 
 //Declare static global variables using 'construct on use' idiom to ensure they are always constructed correctly
 // and avoid "static initialization order fiasco".
 
 Timer                       &   systemTime();
+Ticker                      &   pulse();
+
 Serial                      &   pc();
 
+LocalFileSystem             &   local();
+
 SpiADC                      &   adc();
 LinearActuator              &   bce();
 LinearActuator              &   batt();
 
-LocalFileSystem             &   local();
+omegaPX209                  &   depth();
+OuterLoop                   &   depthLoop();
+
+IMU                         &   imu();
+OuterLoop                   &   pitchLoop();
 
-PIDController               &   DepthController();
-
-IMU                         &   Microstrain();
+// leds for debugging, global for use in any function
+DigitalOut                  &   led1();
+DigitalOut                  &   led2();
+DigitalOut                  &   led3();
+DigitalOut                  &   led4();
 
 #endif
\ No newline at end of file

--- a/System/config_functions.cpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/System/config_functions.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -5,12 +5,11 @@
 {
     ConfigFile cfg;
     int count = 0;
-    if (!cfg.read("/local/bce.txt")){
+    if (!cfg.read("/local/bce.txt")) {
             error("File Read Error");
-        }
+    }
     char value[BUFSIZ];
  
-    
     if (cfg.getValue("PGain", &value[0] , sizeof(value))) {
         bce().setControllerP(atof(value));
         count++;
@@ -44,7 +43,6 @@
         count++;
     }
     
-    
     return count;     
 }
 
@@ -52,9 +50,9 @@
 {
     ConfigFile cfg;
     int count = 0;
-    if (!cfg.read("/local/batt.txt")){
+    if (!cfg.read("/local/batt.txt")) {
             error("File Read Error");
-        }
+    }
     char value[BUFSIZ];
  
     
@@ -91,6 +89,71 @@
         count++;
     }
     
+    return count;     
+}
+
+int load_DEPTH_config()
+{
+    ConfigFile cfg;
+    int count = 0;
+    if (!cfg.read("/local/depth.txt")) {
+            error("File Read Error");
+    }
+    char value[BUFSIZ];
     
-    return count;     
+    if (cfg.getValue("PGain", &value[0] , sizeof(value))) {
+        depthLoop().setControllerP(atof(value));
+        count++;
+    }
+    if (cfg.getValue("IGain", &value[0] ,sizeof(value))) {
+        depthLoop().setControllerI(atof(value));
+        count++;
+    }
+    if (cfg.getValue("DGain", &value[0] , sizeof(value))) {
+        depthLoop().setControllerD(atof(value));
+        count++;
+    }
+    if (cfg.getValue("filterWn", &value[0], sizeof(value))) {
+        depthLoop().setFilterFrequency(atof(value));
+        count++;
+    }
+    if (cfg.getValue("deadband", &value[0], sizeof(value))) {
+        depthLoop().setDeadband(atof(value));
+        count++;
+    }
+    
+    return count;
+}
+
+int load_PITCH_config()
+{
+    ConfigFile cfg;
+    int count = 0;
+    if (!cfg.read("/local/pitch.txt")){
+            error("File Read Error");
+    }
+    char value[BUFSIZ];
+    
+    if (cfg.getValue("PGain", &value[0] , sizeof(value))) {
+        pitchLoop().setControllerP(atof(value));
+        count++;
+    }
+    if (cfg.getValue("IGain", &value[0] ,sizeof(value))) {
+        pitchLoop().setControllerI(atof(value));
+        count++;
+    }
+    if (cfg.getValue("DGain", &value[0] , sizeof(value))) {
+        pitchLoop().setControllerD(atof(value));
+        count++;
+    }
+    if (cfg.getValue("filterWn", &value[0], sizeof(value))) {
+        pitchLoop().setFilterFrequency(atof(value));
+        count++;
+    }
+    if (cfg.getValue("deadband", &value[0], sizeof(value))) {
+        pitchLoop().setDeadband(atof(value));
+        count++;
+    }
+    
+    return count;
 }
\ No newline at end of file

--- a/System/config_functions.h	Fri Oct 20 11:41:22 2017 +0000
+++ b/System/config_functions.h	Mon Oct 23 12:50:53 2017 +0000
@@ -5,9 +5,11 @@
 #include "ConfigFile.h"
 #include <cstdlib>
 
-// Add load function to LinearActuator class*******************************88
+// Add load function to LinearActuator class*******************************
 int load_BCE_config();
 int load_BATT_config();
+int load_DEPTH_config();
+int load_PITCH_config();
 int load_script();
 
 

--- a/System/conversions.cpp	Fri Oct 20 11:41:22 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,8 +0,0 @@
-#include "conversions.hpp"
-
-float counts_to_dist(int count)
-{
-    float conv = -0.1912*count+507.2;
-    return conv;
-
-};
\ No newline at end of file

--- a/System/conversions.hpp	Fri Oct 20 11:41:22 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,6 +0,0 @@
-#ifndef CONVERSIONS_HPP
-#define CONVERSIONS_HPP
-
-float counts_to_dist(int count);
-
-#endif
\ No newline at end of file

--- a/batt.txt	Fri Oct 20 11:41:22 2017 +0000
+++ b/batt.txt	Mon Oct 23 12:50:53 2017 +0000
@@ -1,12 +1,12 @@
 # configuration file for battery mover parameters
 
 #Gains
-PGain=0.15
+PGain=0.3
 IGain=0.0
 DGain=0.0
 
 #string pot parameters
-zeroCounts=620
+zeroCounts=610
 PistonTravelLimit=75.0
 slope=.12176
 filterWn=6.0

--- a/bce.txt	Fri Oct 20 11:41:22 2017 +0000
+++ b/bce.txt	Mon Oct 23 12:50:53 2017 +0000
@@ -6,7 +6,7 @@
 DGain=0.0
 
 #string pot parameters
-zeroCounts=190
+zeroCounts=195
 PistonTravelLimit=320.0
 slope=.12176
 filterWn=6.0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/depth.txt	Mon Oct 23 12:50:53 2017 +0000
@@ -0,0 +1,10 @@
+# configuration file for depth outer loop parameters
+
+#Gains
+PGain=0.1
+IGain=0.0
+DGain=0.0
+
+#Depth sensor filter parameters
+filterWn=6.0
+deadband=0.5
\ No newline at end of file

--- a/main.cpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/main.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -1,120 +1,155 @@
+/*
+    Starting from Trent's Linear Actuator code from 2017-10-19, these modifications
+    by Dan add an outer loop controller for depth and pitch to command the inner
+    linear actuator loops.
+    Modified 2017-10-20 revA by Dan
+        - added outer loop controller, but it is hanging the mbed. (turns out it was the imu update)
+    Modified 2017-10-22 revA by Dan
+        - outer loop now works with a call to outerloop.update() in main loop(), not with an attached ticker
+    Modified 2017-10-22 revB by Dan
+        - enabled both depth and pitch outer loop controllers
+        - added ability to keyboard reset
+    Modified 2017-10-22 revC by Dan
+        - major update to the IMU library processing to make a state machine that doesn't hang
+        - added lat/lon/alt and GNSS fix information to the IMU library
+        - brought out the pin names into the constructors of IMU, omega, SpiADC
+    Modified 2017-10-22 revD by Dan
+        - everything seems to be working, shy of re-checking on the hardware
+        - Depth sensor call done inside the OuterLoop, but should somehow be set as a callback instead
+        - IMU sensor call done inside the OuterLoop, but should somehow be set as a callback instead
+    Modified 2017-10-23 revA by Dan/Matt
+        - linear actuator hardware works great, limit switches, sensing, etc.
+        - outer loops run, but move the BCE in the wrong direction.
+        - new IMU code doesn't read from the sensor correctly, but doesn't hang up either.
+        - depth sensor worked fine, just needs zero bias adjustment.
+*/
+
 #include "mbed.h"
 #include "StaticDefs.hpp"
 #include "config_functions.h"
 
-int main()
-{
-    float positionCmd = 10.0;
-    char userInput;
-
-    local();  //this makes sure the local file system is constructed
-
-    //Read in and load the BCE parameters from the text file "bce.txt"
-    load_BCE_config(); // should say "bce.load_config(bce.txt);
-    bce().init();
+extern "C" void mbed_reset();           // utilized to reset the mbed
 
-    ////Read in and load the battery mover parameters from the text file "batt.txt"
-    load_BATT_config();
-    batt().init();
+void setup() {
+    pc().printf("\n\n\r\rFSG 2017-10-22 revD\n\r");
 
-    // do not leave this in. It is just there to check that things are working
-    pc().printf("\n\rbce P: %3.2f I: %3.2f D %3.2f zero: %3i limit %3.2f slope %3.2f  \n\r", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
-    pc().printf("\n\rbatt P: %3.2f I: %3.2f D %3.2f zero: %3i limit %3.2f slope %3.2f  \n\r", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
-    //Front load the desired parameters into the linear acuator objects.
-    //This could be done using ConfigFile, if it worked
-
-    //I need to also check up in whether the limits are being passed to the linear
-    //actuator's PID objects. I noticed I have defaults that are appropriate for only
-    //the bouyancy engine
-
-    //start up the system timer
+    // start up the system timer
     systemTime().start();
 
-    //setup and start the adc. This runs on a fixed interval and is interrupt driven
+    // set up and start the adc. This runs on a fixed interval and is interrupt driven
     adc().initialize();
     adc().start();
+    
+    // set up and start the imu. This polls in the background
+    imu().initialize();
+    imu().start();
+    
+    // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298
+    depth().initialize();
+    
+    // construct a local file system
+    local();
 
-    //start the bce and batt control loops.  ADC has to be running first.
-    //The motors will not activate until their respective position filters have
-    //converged, but just to be sure we pause them until we want them to really run
-    bce().start();
-    bce().pause();
-
-    bce().setPosition_mm(positionCmd);
+    // load config data from files
+    load_BCE_config();      // load the buoyancy engine parameters from the file "bce.txt"
+    load_BATT_config();     // load the battery mass mover parameters from the file "batt.txt"
+    load_DEPTH_config();    // load the depth control loop parameters from the file "depth.txt"
+    load_PITCH_config();    // load the depth control loop parameters from the file "pitch.txt"
 
-    bce().unpause();
+    // set up the linear actuators.  adc has to be running first.
+    bce().init();
+    bce().start();
+    bce().setPosition_mm(bce().getPosition_mm()); // start by not moving
 
+    batt().init();
     batt().start();
-    batt().pause();
-    
-    batt().setPosition_mm(positionCmd);
-    
-    batt().unpause();
+    batt().setPosition_mm(batt().getPosition_mm()); // start by not moving
+
+    // set up the depth and pitch outer loop controllers
+    depthLoop().init();
+    depthLoop().setCommand(0.0);
 
+    pitchLoop().init();
+    pitchLoop().setCommand(0.0);
+
+    // do not leave this in. Check that PID gains are loading
+    pc().printf("bce    P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f  \n\r", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
+    pc().printf("batt   P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f  \n\r", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
+    pc().printf("depth  P: %3.2f, I: %3.2f, D %3.2f, \n\r", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD());
+    pc().printf("pitch  P: %3.2f, I: %3.2f, D %3.2f, \n\r", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD());
+    pc().printf("\n\r");
+}
 
-
-
-    while(1) {
-
-        
-        if (pc().readable()) {
-            // get the key
-            userInput = pc().getc();
+void keyboard() {
+    static float setpoint = 50.0;
+    static bool isOuterLoopActive = false; // allow user to select if the outer loop is active
+    char userInput;
+    
+    // check keyboard and make settings changes as requested
+    if (pc().readable()) {
+        // get the key
+        userInput = pc().getc();
 
-            //check command against desired control buttons
-            if (userInput == '=') {
-                //increment the duty cycle
-                positionCmd += 5.0 ;
-            } else if (userInput == '-') {
-                //decrement the duty cycle
-                positionCmd -= 5.0 ;
-            } else if (userInput == '\r') {
-                batt().setPosition_mm(positionCmd);
-                bce().setPosition_mm(positionCmd);
+        // check command against desired control buttons
+        if (userInput == '+' or userInput == '=') {
+            setpoint += 1.0; //increment the command
+        }
+        else if (userInput == '-' or userInput == '_') {
+            setpoint -= 1.0; //decrement the command
+        }
+        else if (userInput == 't' or userInput == 'T') {
+            isOuterLoopActive = !isOuterLoopActive; // toggle the outer loop
+            
+            if (isOuterLoopActive)
+                pc().printf("Outer loops are now active                                \n\r");
+            else
+                pc().printf("Outer loops are now inactive                                \n\r");
+        }
+        else if (userInput == '\r') {
+            if (isOuterLoopActive) {
+                pc().printf("setting outer loop: %3.0f                                \n\r", setpoint);
+                
+                depthLoop().setCommand(setpoint);
+                bce().setPosition_mm(depthLoop().getOutput());  // connect outer to inner loop
+                
+                pitchLoop().setCommand(setpoint);
+                batt().setPosition_mm(pitchLoop().getOutput()); // connect outer to inner loop
+            }
+            else {
+                pc().printf("setting inner loop: %3.0f                                \n\r", setpoint);
+                
+                batt().setPosition_mm(setpoint);
+                bce().setPosition_mm(setpoint);
             }
         }
-        //pc().printf("set point %3.1f  \r", positionCmd);
-        //pc().printf("pos: %3.2f mm pos: %3.2f counts vel: % 3.2f mm/s  Set Point %3.1f       \r", bce().getPosition_mm(), bce().getPosition_counts(), bce().getVelocity_mms(), positionCmd);
-        //pc().printf("pos: %3.2f mm pos: %3.2f counts vel: % 3.2f mm/s  Set Point %3.1f       \r", batt().getPosition_mm(), batt().getPosition_counts(), batt().getVelocity_mms(), positionCmd);
-        pc().printf("pos bce: %3.2f mm, pos batt: %3.2f mm, Set Point %3.1f       \r", bce().getPosition_mm(), batt().getPosition_mm(), positionCmd);
+        else if (userInput == '?') {
+            pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
+            mbed_reset();
+        }
     }
+    
+    if (isOuterLoopActive)
+        pc().printf("setpoint %3.1f, depth: %3.1f ft, bce: %3.1f mm       \r", setpoint, depth().getDepth(), bce().getPosition_mm());
+        //pc().printf("setpoint %3.1f, pitch: %3.1f deg, batt: %3.1f mm       \r", setpoint, imu().getPitch(), batt().getPosition_mm());
+    else
+        pc().printf("setpoint %3.1f, bce:   %3.1f mm       \r", setpoint, bce().getPosition_mm());
+        //pc().printf("setpoint %3.1f, batt:  %3.1f mm       \r", setpoint, batt().getPosition_mm());
 }
 
-
-
-    //read in a script file describing the dive cycle
-    //I envision entries such as
-    // target pitch angle  target depth  target depth rate
-    // 10 degrees          5 ft          0.05 ft/s         example dive request
-    // -10 degrees         0 ft          -0.05 ft/s        example surface request
-
-    //this implies two pid controllers
-    // one that manages the batt mover for pitch
-    // the other manages the buoyance engine for dive speed
-
-    // then some logic is needed to check the box when the desired condition is reached
-    // like a waypoint threshold. This allows you to get away from worrying as much about
-    // keeping time 
+void loop() {
+    led1() = !led1(); // blink
+    
+    keyboard();
     
-    
-    
+    // sequence controllers...
+    // check whether depth has been triggered
+    //    if so, move on to the next line of the script
+    //    if done, surface or repeat
+}
 
-        //psuedo code to outline what we want to do
-        /*
-        check for and parse IMU data
-
-        poll for depth adc reading (eventually this will come from the external adc)
-
-        run depth data through position velocity filter
-
-        update the PID controllers for Pitch and depth rate
-
-        check whether depth has been triggered
-            if so, move on to the next line of the script
-            if done , surface or repeat
-
-        */
-
-
-        //This can be ignored for now this was the old serial comms stuff for when I
-        //was prototyping the BCE controls
\ No newline at end of file
+int main() {
+    setup();
+    while(1) {
+        loop();
+    }
+}
\ No newline at end of file

--- a/omegaPX209/omegaPX209.cpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/omegaPX209/omegaPX209.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -8,20 +8,28 @@
 #include "mbed.h"
 #include "omegaPX209.hpp"
 
-omegaPX209::omegaPX209(): depthP(p19)
+omegaPX209::omegaPX209(PinName pin): 
+    depthP(pin)
 {
-    
 }
 
-void omegaPX209::initialize()
-{
+void omegaPX209::initialize() {
     P = 0;              // Pressure [psi]
     cal = 12;           // Volts per psi
     multiplier = 3.3;   // Maximum voltage in
 }
 
-float omegaPX209::calcP()
-{
+float omegaPX209::getPsi() {
     P = depthP*multiplier*cal;
     return P;
 }
+
+// math sourced from http://www.kylesconverter.com/pressure/feet-of-water-to-pounds-per-square-inch
+float omegaPX209::getDepth() {
+    P = getPsi(); // read the sensor
+    float pascals = (P * 6894.76) - (14.7 * 6894.76); // convert psi to Pascals
+    float depth_m = pascals / (density_of_water_g_cc * 1000 * 9.80665); // convert Pa to fluid depth in meters
+    float depth_ft = 3.28084 * depth_m; // convert meters to feet
+    
+    return depth_ft;
+}
\ No newline at end of file

--- a/omegaPX209/omegaPX209.hpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/omegaPX209/omegaPX209.hpp	Mon Oct 23 12:50:53 2017 +0000
@@ -7,16 +7,21 @@
 
 #include "mbed.h"
 
+#define density_of_water_g_cc 1.00  // g/cm^3   (or 1.03 g/cm^3)
+
 class omegaPX209 {
 public:
-    omegaPX209();    
+    omegaPX209(PinName pin);    
     void initialize();
-    float calcP();              // Calculates pressure
+    
+    float getPsi();            // returns pressure [psi]
+    float getDepth();          // returns water depth [ft]
         
 private:
     AnalogIn depthP;
+    Ticker pulse;
+    
     float P;                    // Pressure [psi]
     float cal;                  // Volts per psi
-    float multiplier;           // Maximum voltage in
-        
+    float multiplier;           // Maximum voltage in (of ADC system)
 };

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pitch.txt	Mon Oct 23 12:50:53 2017 +0000
@@ -0,0 +1,13 @@
+# configuration file for BCE parameters
+
+#Gains
+PGain=0.1
+IGain=0.0
+DGain=0.0
+
+#string pot parameters
+zeroCounts=195
+PistonTravelLimit=320.0
+slope=.12176
+filterWn=6.0
+deadband=0.5