Revision 65:2ac186553959, committed 2018-06-19

Comitter:

tnhnrl

Date:

Tue Jun 19 18:10:50 2018 +0000

Parent:

64:a8939bc127ab

Child:

66:0f20870117b7

Commit message:

Version with limit switches in output

Changed in this revision

--- a/IMU/IMU.cpp	Mon Jun 18 21:04:09 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,240 +0,0 @@
-#include "IMU.h"
-
-IMU::IMU(PinName Tx, PinName Rx): 
-    _rs232(Tx,Rx)
-{
-}
-
-void IMU::initialize() {
-    //set up the spi bus and frequency
-    _rs232.baud(115200);
-
-    // initialize the processing state machine
-    state = SYNC0; 
-    
-    // initialize to zeros
-    euler[0] = 0.0;
-    euler[1] = 0.0;
-    euler[2] = 0.0;
-    
-    // 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 stops an interval timer trigger of the IMU update function
-void IMU::start() {
-    interval.attach(this, &IMU::update, .05);  //this should be a 1 Hz sample rate
-}
-
-// 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() {
-    
-//    // DEBUGGING an example packet
-//    Serial pc(USBTX, USBRX);
-//    //char data[20] = {0x75,0x65,0x80,0x0E,0x0E,0x04,0x3E,0x7A,0x63,0xA0,0xBB,0x8E,0x3B,0x29,0x7F,0xE5,0xBF,0x7F,0x84,0xEE}; // 3d accel
-//    char data[20] = {0x75,0x65,0x80,0x0E,0x0E,0x0C,0xBA,0xE3,0xED,0x9B,0x3C,0x7D,0x6D,0xDF,0xBF,0x85,0x5C,0xF5,0x41,0xBB}; // euler cf
-//    for (int j=0; j<20; j++) {
-        //byte = data[j];
-        
-    while (_rs232.readable()) {
-        // read a single byte
-        byte = _rs232.getc();
-        
-        // state machine to process byte-by-byte
-        switch (state) {
-        case SYNC0 : 
-            if (byte == 0x75) {
-                packet[0] = byte; // save into the packet
-                state = SYNC1;
-            }
-            break;
-            
-        case SYNC1 :
-            if (byte == 0x65) {
-                packet[1] = byte; // save into the packet
-                state = SET;
-            }
-            else {
-                state = SYNC0;
-            }
-            break;
-            
-        case SET :
-            descriptor = byte; // save descriptor set
-            packet[2] = byte; // save into the packet
-            state = LEN;
-            break;
-            
-        case LEN :
-            len = byte; // save payload length
-            packet[3] = byte; // save into the packet
-            state = PAY;
-            i = 0; // reset payload field length counter
-            break;
-            
-        case PAY :
-            if (i < len) { // keep adding until get all the payload length
-                packet[4+i] = byte; // add byte to the packet, skipping over the header 4-bytes
-                i++; // increment payload counter
-            }
-            else {
-                state = CRC0;
-            }
-            if (i >= len) { // not an elseif, since we want to escape when i==len
-                state = CRC0;
-            }
-            break;
-            
-        case CRC0 :
-            crc0 = byte; // save the msb of the checksum
-            state = CRC1;
-            break;
-            
-        case CRC1 :
-            crc1 = byte; // save the lsb of the checksum
-            checksum = ((unsigned int)crc0 << 8) + (unsigned int)crc1; // make checksum into a uint16
-            if (checksum == calcChecksum(packet, len+4)) { // passed checksum, wahoo!
-                processPayload(descriptor, packet[4], &packet[4]); // process the payload part of the packet, starting at byte 4
-            }
-            state = SYNC0; // reset to SYNC0 state
-            break;
-
-        default :
-            state = SYNC0;
-        }
-    }
-    return;
-}
-
-void IMU::processPayload(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+2])*180/_PI;  // roll Euler angle convert in degrees
-            euler[1] = floatFromChar(&payload[PITCH_OFFSET+2])*180/_PI; // pitch Euler angle convert in degrees
-            euler[2] = floatFromChar(&payload[YAW_OFFSET+2])*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+2]);   // latitude in decimal degrees
-            latLonAlt[1] = doubleFromChar(&payload[LONGITUDE_OFFSET+2]);  // longitude in decimal degrees
-            latLonAlt[2] = doubleFromChar(&payload[HEIGHT_MSL_OFFSET+2]); // altitude above mean sea level in meters
-        }
-    }
-}
-
-void IMU::processGnssFixInformation(char length, unsigned char * payload) {
-    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+2]) & 0x01; // bitand on LSB to see 2d fix flag
-            is3dFixValid = char(payload[FIX_TYPE_OFFSET+2]) & 0x02; // bitand on LSB to see 3d fix flag
-            numSV = char(payload[NUM_SV_OFFSET+2]); // number of GNSS satellite vehicles
-        }
-    }
-}
-
-float IMU::floatFromChar(unsigned char * value) {
-    unsigned char temp[4];
-    temp[0] = value[3];
-    temp[1] = value[2];
-    temp[2] = value[1];
-    temp[3] = value[0];
-    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;
-    unsigned int myChecksum = 0;
-    
-    for (int i=0; i<checksum_range; i++) {
-        checksum_byte1 += mip_packet[i];
-        checksum_byte2 += checksum_byte1;
-    }
-    myChecksum = ((unsigned int)checksum_byte1 << 8) + (unsigned int)checksum_byte2;
-    return myChecksum;
-}
\ No newline at end of file

--- a/IMU/IMU.h	Mon Jun 18 21:04:09 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,90 +0,0 @@
-#include "mbed.h"
-#include "MODSERIAL.h"
-
-// for Microstrain's MIPS protocol, try this link, or search on microstrain.com
-// http://www.microstrain.com/sites/default/files/3dm-gx5-45_dcp_manual_8500-0064_0.pdf
-
-#define _PI ((float) 3.14159265359)
-
-// 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                0x80
-#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(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;
-    
-    char byte;
-    
-    unsigned char state, len, descriptor, i, packet[256];
-    unsigned int checksum, crc0, crc1;
-    
-/*    unsigned char state, len, descriptor, i, crc0, crc1, payload[30];
-    unsigned int checksum;*/
-    
-    float euler[3];
-    double latLonAlt[3];
-    bool is2dFixValid;
-    bool is3dFixValid;
-    char numSV;
-
-    void processPayload(char type, char length, unsigned char * payload);
-    void processEulerCfPacket(char length, unsigned char * payload);
-    void processLatLonAltPacket(char length, unsigned char * payload);
-    void processGnssFixInformation(char length, unsigned char * payload);
-
-    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/LinearActuator/LinearActuator.cpp	Mon Jun 18 21:04:09 2018 +0000
+++ b/LinearActuator/LinearActuator.cpp	Tue Jun 19 18:10:50 2018 +0000
@@ -298,6 +298,10 @@
 bool LinearActuator::getSwitch() {
     return _limit;
 }
+
+bool LinearActuator::getHardwareSwitchStatus() {
+    return _limitSwitch.read();
+}
  
  void LinearActuator::setDeadband(float deadband) {
     _deadband = deadband;

--- a/LinearActuator/LinearActuator.hpp	Mon Jun 18 21:04:09 2018 +0000
+++ b/LinearActuator/LinearActuator.hpp	Tue Jun 19 18:10:50 2018 +0000
@@ -69,6 +69,8 @@
     void setPIDHighLimit(float high_limit);
     void setPIDLowLimit(float low_limit);
     
+    bool getHardwareSwitchStatus(); //new
+    
 protected:
     PololuHBridge _motor;
     PosVelFilter _filter;

--- a/StateMachine/StateMachine.cpp	Mon Jun 18 21:04:09 2018 +0000
+++ b/StateMachine/StateMachine.cpp	Tue Jun 19 18:10:50 2018 +0000
@@ -871,7 +871,7 @@
 
 void StateMachine::showDebugMenu() {
     pc().printf("\r\r\n\nDEBUG KEYBOARD MENU (06/15/2018):\r\r\n");
-    pc().printf(" T to go into CHECK NEUTRAL TUNING (This is on a timer! Uses NEUTRAL positions!)\r\n");
+    pc().printf(" Y to go into CHECK NEUTRAL TUNING (This is on a timer! Uses NEUTRAL positions!)\r\n");
     pc().printf(" V to POSITION DIVE (initiate motor position-based dive cycle)\r\n");
     pc().printf(" N to find neutral\r\n");
     pc().printf(" M to initiate multi-dive cycle\r\n");
@@ -894,7 +894,7 @@
     pc().printf("9/0 to decrease/increase heading setpoint: %0.1f deg\r\n",_heading_command);
     pc().printf("k/l to decrease/increase BCE offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BCE_dive_offset, _neutral_bce_pos_mm - _BCE_dive_offset, _neutral_bce_pos_mm + _BCE_dive_offset);
     pc().printf(";/' to decrease/increase BMM offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BMM_dive_offset, _neutral_batt_pos_mm - _BMM_dive_offset, _neutral_batt_pos_mm + _BMM_dive_offset);  
-    pc().printf("-/+ to decrease/increase timeout: %d s\r\n",_timeout);
+    pc().printf("T to enter in the timeout (default is 60 seconds): %d s\r\n",_timeout);
     
 // FIXED WITH NEW CODE
     pc().printf(" 1 BCE PID sub-menu\r\n");
@@ -1143,6 +1143,11 @@
 /***************************** DEBUG MENU *****************************/             
         if (_debug_menu_on) {
             
+            if (user_input == 'T') {
+                pc().printf("Please enter the timeout (timer) value below: \n\r");
+                _timeout = fabs(getFloatUserInput());
+            }
+            
             if (user_input == 'D') {
                 _keyboard_state = DIVE;
             }
@@ -1173,7 +1178,7 @@
                 _keyboard_state = EMERGENCY_CLIMB;
             }
             
-            else if (user_input == 'T') {
+            else if (user_input == 'Y') {
                 _keyboard_state = CHECK_TUNING;
             }
             
@@ -1309,16 +1314,7 @@
                 _depth_command += 0.5;         //increment the depth setpoint
                 depthLoop().setCommand(_depth_command);
                 pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand());
-            }
-            else if (user_input == '-') {
-                _timeout -= 10.0;               //decrement the timeout
-                pc().printf(">>> timeout decreased: %d\r\n", _timeout);
-            }
-            else if (user_input == '=' or user_input == '+') {
-                _timeout += 10.0;               //increment the timeout
-                pc().printf(">>> timeout increased: %d\r\n", _timeout);
-            }
-            
+            }            
             else if (user_input == '5') {
                 keyboard_menu_RUDDER_SERVO_settings();
             }
@@ -1350,7 +1346,7 @@
                 headingLoop().setCommand(_heading_command);
                 pc().printf(">>> (+) new HEADING setpoint: %0.3f deg (+)\r\n", headingLoop().getCommand());
             }
-            else if (user_input == 'Y') {
+            else if (user_input == 'U') {
                 keyboard_menu_POSITION_READINGS();
             }
             
@@ -1439,7 +1435,11 @@
 /***************************** DEBUG MENU *****************************/
             
 /***************************** SIMPLE MENU *****************************/
-        else {       
+        else {
+            if (user_input == 'T') {
+                pc().printf("Please enter the timeout (timer) value below: \n\r");
+                _timeout = fabs(getFloatUserInput());
+            }      
             if (user_input == 'V') {
                 _keyboard_state = POSITION_DIVE;
             }
@@ -1512,16 +1512,7 @@
                 pc().printf("depth  P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
                 pc().printf("pitch  P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
             }
-            
-            else if (user_input == '-') {
-                _timeout -= 10.0;               //decrement the timeout
-                pc().printf(">>> timeout decreased: %d\r\n", _timeout);
-            }
-            else if (user_input == '=' or user_input == '+') {
-                _timeout += 10.0;               //increment the timeout
-                pc().printf(">>> timeout increased: %d\r\n", _timeout);
-            }
-            
+                        
             else if (user_input == '?') {
                 pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
                 wait(0.5);
@@ -1592,7 +1583,7 @@
             wait(1);         
             
             
-            pc().printf("BCE POS (CMD): %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS_psi: %0.2f [depth_ft: %0.2f], PITCH: %0.2f, HEADING: %0.2f, rudder_servo_pwm: %0.1f [0(%d),1(%d),2(%d),6(%d),4(%d),5(%d),6(%d),7(%d)]\r",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),depth().getPsi(),depthLoop().getPosition(),imu().getPitch(),imu().getHeading(),rudder().getPosition_pwm(),adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),adc().readCh5(),adc().readCh6(),adc().readCh7());
+            pc().printf("BCE POS (CMD): %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS_psi: %0.2f [depth_ft: %0.2f], PITCH: %0.2f, HEADING: %0.2f, rudder_servo_pwm: %0.1f [0(%d),1(%d),2(%d),6(%d),4(%d),5(%d),6(%d),7(%d)] Switch: BCE(%d) BMM(%d)\r",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),depth().getPsi(),depthLoop().getPosition(),imu().getPitch(),imu().getHeading(),rudder().getPosition_pwm(),adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),adc().readCh5(),adc().readCh6(),adc().readCh7(), bce().getHardwareSwitchStatus(),batt().getHardwareSwitchStatus());
             //pc().printf("BCE POS (CMD): %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS_psi: %0.2f (unfiltered: %0.2f) [depth_ft: %0.2f], PITCH: %0.2f, HEADING: %0.2f, rudder_servo_pwm: %0.1f\r",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),depth().getPsi(),depth().getRawPSI(),depthLoop().getPosition(),imu().getPitch(),imu().getHeading(),rudder().getPosition_pwm());
             //pc().printf("\n0(%d),1(%d),2(%d),6(%d),4(%d),5(%d),6(%d),7(%d)\r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),adc().readCh5(),adc().readCh6(),adc().readCh7()); 
             

--- a/System/StaticDefs.cpp	Mon Jun 18 21:04:09 2018 +0000
+++ b/System/StaticDefs.cpp	Tue Jun 19 18:10:50 2018 +0000
@@ -71,11 +71,6 @@
     return depth;
 }
 
-//IMU & imu() {
-//    static IMU imu(p13, p14);       // tx, rx pin
-//    return imu;    
-//}
-
 IMU & imu() {
     static IMU imu(p28, p27);       // tx, rx pin
     return imu;    

--- a/main.cpp	Mon Jun 18 21:04:09 2018 +0000
+++ b/main.cpp	Tue Jun 19 18:10:50 2018 +0000
@@ -37,60 +37,163 @@
     
     Modified FSG PCB V_1_4
         - adcLed = 0; in adc
+    
+    Modified FSG PCB V_1_5
+        - IMU update
 */
- 
+
+/* removed unused variables */
 #include "mbed.h"
 #include "StaticDefs.hpp"
+
+#ifndef lround
+#define lround(var)    (long)(var+0.5f)
+#endif
+
+////////////////////////////////////////////////////////////////// NEW TICKER
+Ticker systemTicker;
+bool setup_complete = false;
+volatile unsigned int bTick = 0;
+volatile unsigned int timer_counter = 0;
+
+char hex[9];
+
+char * conversion(float input_float) {
+    int integer_number = lround(100.0 * input_float);      //convert floating point input to integer to broadcast over serial (reduce precision)
+    
+    memset(hex, 0, sizeof(hex) );   //  void* memset( void* dest, int ch, size_t count );   // CLEAR IT  (need null at end of string)
+    sprintf(hex, "%8x", integer_number);                                 //generates spaces 0x20
+    return hex;
+}
+
+static unsigned int read_ticker(void) {      //Basically this makes sure you're reading the data at one instance (not while it's changing)
+    unsigned int val = bTick;
+    if( val )
+        bTick = 0;
+    return( val );
+}
+////////////////////////////////////////////////////////////////// NEW TICKER
  
 // loop rate used to determine how fast events trigger in the while loop
-Ticker loop_rate_ticker;
-Ticker log_loop_rate_ticker;
+//Ticker main_loop_rate_ticker;
+//Ticker log_loop_rate_ticker;
 
-volatile bool loop = false; //used so the compiler does not optimize this variable (load from memory, do not assume state of variable)
+volatile bool fsm_loop = false; //used so the compiler does not optimize this variable (load from memory, do not assume state of variable)
 volatile bool log_loop = false; //used so the compiler does not optimize this variable (load from memory, do not assume state of variable)
 
-void loop_trigger() { loop = true;} // loop trigger (used in while loop)
+void loop_trigger() { fsm_loop = true;} // loop trigger (used in while loop)
 void log_loop_trigger() { log_loop = true;} // log loop trigger (used in while loop)
 
-/****************** for rudder servo    ******************/
-Ticker systemTicker;
+static int current_state = 0;     
+static bool file_opened = false;
+
+void FSM() {                    // FSM loop runs at 100 hz
+    if(fsm_loop) {
+        fsm_loop = false;       // wait until the loop rate timer fires again
+        current_state = stateMachine().runStateMachine();       //running State Machine. Returns 0 if sitting idle or keyboard press (SIT_IDLE state).
+    }
+}
+
+void log_function() {    
+    // log loop runs at 1 hz
+    if (log_loop) {
+        //when the state machine is not in SIT_IDLE state (or a random keyboard press)
+
+        if (current_state == TRANSMIT_MBED_LOG or current_state == RECEIVE_SEQUENCE) {                
+            ;   //pass
+        }
+        
+        else if(current_state != 0) {
+            if (!file_opened) {                                 //if the log file is not open, open it
+                mbedLogger().appendLogFile(current_state, 0);   //open MBED file once
+                //sdLogger().appendLogFile(current_state, 0);     //open SD file once
+                           
+                file_opened = true;                             //stops it from continuing to open it
 
-volatile unsigned int timer_counter = 0;
+                pc().printf(">>>>>>>> Recording. Log file opened. <<<<<<<<\n\r");
+            }
+            
+            //record to Mbed file system   
+            led4() = !led4();
+            
+            mbedLogger().appendLogFile(current_state, 1);    //writing data
+            //sdLogger().appendLogFile(current_state, 1);    //writing data
+        }
+        
+        //when the current FSM state is zero (SIT_IDLE), close the file
+        else {
+            //this can only happen once
+            if (file_opened) {
+                mbedLogger().appendLogFile(current_state, 0);    //close log file
+                //sdLogger().appendLogFile(current_state, 0);    //close log file
+                
+                file_opened = false;
+                
+                pc().printf(">>>>>>>> Stopped recording. Log file closed. <<<<<<<<\n\r");
+            }
+        }
+    }   //END OF LOG LOOP
+    
+    log_loop = false;   // wait until the loop rate timer fires again
+}
 
-bool setup_complete = false;
-
-static void system_timer(void) {    
+static void system_timer(void) {
+    bTick = 1;
+    
     timer_counter++;
     
     //only start these updates when everything is properly setup (through setup function)
     if (setup_complete) {
-            if ( (timer_counter % 20) == 0 ) {    // 0.02 second intervals
-                rudder().runServo();
-            }
+        if ( (timer_counter % 1) == 0) {    //this runs at 0.001 second intervals (1000 Hz)
+            adc().update();  //every iteration of this the A/D converter runs   //now this runs at 0.01 second intervals 03/12/2018
+        }
+        
+        if ( (timer_counter % 10) == 0) {
+            bce().update();      //update() inside LinearActuator class (running at 0.01 second intervals)
+            batt().update();
+            led2() = !led2();
+        }
+        
+        if ( (timer_counter % 20) == 0 ) {    // 0.02 second intervals
+            rudder().runServo();
+        }
+        
+        if ( (timer_counter % 50) == 0 ) {    // 0.05 second intervals 
+            //imu().runIMU();
+        }
+        
+        if ( (timer_counter % 100) == 0) {     // 100,000 microseconds = 0.1 second intervals
+            depthLoop().runOuterLoop();
+            pitchLoop().runOuterLoop();
+            headingLoop().runOuterLoop();
+        }
+        
+        if ( (timer_counter % 500) == 0) {     // 500,000 microseconds = 0.5 second intervals
+            //serialComms().getDepthPitchHeading();
+            log_loop = true;
+            log_function();
+       }
     }
 }
 
-/****************** for rudder servo    ******************/
-
 void setup() {
     pc().baud(57600);
-    pc().printf("\n\n\rFSG PCB V1.4 2018-06-18 \n\n\r");
-
-/*    //setup data logger baud rate and write the start of the program (every time you reset)
-    datalogger().baud(57600);
-    datalogger().printf("SYSTEM, RESET\n");
-*/
-      
+    pc().printf("\n\n\r 2018_03_28_wireless (FSG bench test)\n\n\r");
+ 
     // start up the system timer
-    systemTime().start();
+    //systemTimer().start();
  
     // set up and start the adc. This runs on a fixed interval and is interrupt driven
-    adc().initialize();  
-    adc().start();
+    adc().initialize();
+    //adc().start();
+    
+    // setup and run the rudder(servo) pwm signal (start the ticker)
+    //rudder().init();
+    pc().printf("Rudder servo initialized!\n\r");
     
     // set up and start the imu. This polls in the background
     imu().initialize();
-    imu().start();
+    //imu().start();
     
     // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298
     depth().init();
@@ -100,128 +203,97 @@
     local();
     
     // construct the SD card file system
-//    sd_card();
+    //sd_card();
  
     // load config data from files
-    configFileIO().load_BCE_config();      // load the buoyancy engine parameters from the file "bce.txt"
-    configFileIO().load_BATT_config();     // load the battery mass mover parameters from the file "batt.txt"
-    configFileIO().load_DEPTH_config();    // load the depth control loop parameters from the file "depth.txt" (contains neutral position)
-    configFileIO().load_PITCH_config();    // load the depth control loop parameters from the file "pitch.txt" (contains neutral position)
- 
+    configFileIO().load_BCE_config();       // load the buoyancy engine parameters from the file "bce.txt"
+    configFileIO().load_BATT_config();      // load the battery mass mover parameters from the file "batt.txt"
+    
+    configFileIO().load_DEPTH_config();     // load the depth control loop parameters from the file "depth.txt" (contains neutral position)
+    configFileIO().load_PITCH_config();     // load the depth control loop parameters from the file "pitch.txt" (contains neutral position)
+    
+    configFileIO().load_RUDDER_config();    // load the rudder servo inner loop parameters from the file "SERVO.txt"
+    configFileIO().load_HEADING_config();   // load the rudder servo outer loop HEADING control parameters from the file "HEADING.txt" (contains neutral position)
  
     // set up the linear actuators.  adc has to be running first.
+    bce().setPIDHighLimit(bce().getTravelLimit());     //travel limit of this linear actuator
     bce().init();
-    bce().start();
+    //bce().start();  //removed start, it's handled by the interrupt
     bce().pause(); // start by not moving
  
+    batt().setPIDHighLimit(batt().getTravelLimit());    //travel limit of this linear actuator
     batt().init();
-    batt().start();
+    batt().runLinearActuator(); // _init = true;
+    //batt().start();//removed start, it's handled by the interrupt
     batt().pause(); // start by not moving
  
-    // set up the depth and pitch outer loop controllers
+    // set up the depth, pitch, and rudder outer loop controllers
     depthLoop().init();
-    depthLoop().start();
+    //removed start, it's handled by the interrupt
     depthLoop().setCommand(stateMachine().getDepthCommand());
  
     pitchLoop().init();
-    pitchLoop().start();
+    //removed start, it's handled by the interrupt
     pitchLoop().setCommand(stateMachine().getPitchCommand());
     
-    //new 6/8/2018 for testing
     headingLoop().init();           
-    headingLoop().setCommand(0.0);  //setting heading to 0 for testing
-    //new 6/8/2018 for testing
+    //removed start, it's handled by the interrupt
+    //headingLoop().setCommand(stateMachine().getHeadingCommand());         // FIX LATER
+    //heading flag that adjust the PID error is set in the constructor
+    
+    //systemTicker.attach_us(&system_timer, 10000);         // Interrupt timer running at 0.01 seconds       (slower than original ADC time interval)
+    
+    
  
     // show that the PID gains are loading from the file
     pc().printf("bce    P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f  \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
     pc().printf("batt   P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f  \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
-    pc().printf("depth  P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
-    pc().printf("pitch  P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
+    pc().printf("rudder min pwm: %6.2f, max pwm: %6.2f, center pwm: %6.2f, min deg: %6.2f, max deg: %6.2f\r\n", rudder().getMinPWM(), rudder().getMaxPWM(), rudder().getCenterPWM(), rudder().getMinDeg(), rudder().getMaxDeg());
+    
+    pc().printf("depth   P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
+    pc().printf("pitch   P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
+    pc().printf("heading P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f deg (deadband: %0.1f)\r\n", headingLoop().getControllerP(), headingLoop().getControllerI(), headingLoop().getControllerD(), headingLoop().getOutputOffset(), headingLoop().getDeadband());
+    
     pc().printf("\n\r");
          
     //load sequence from file
     sequenceController().loadSequence();
- 
-    // establish the main loop rate
-    loop_rate_ticker.attach(&loop_trigger, 0.1); // fires the ticker at 10 Hz rate
     
-    // setup the data logger rate
-    log_loop_rate_ticker.attach(&log_loop_trigger, 1.0); // fires the ticker at 1 Hz rate (every second)
- 
     //set time of logger (to current or close-to-current time)
-    mbedLogger().setLogTime();   
-//    sdLogger().setLogTime();
+    mbedLogger().setLogTime();
+    //sdLogger().setLogTime();
     
     //create log files if not present on file system
     mbedLogger().initializeLogFile();
-//    sdLogger().initializeLogFile();
-
-    //tare the pressure sensor during setup
-    depth().tare();
+    //sdLogger().initializeLogFile();
+    
+    setup_complete = true;
+}
 
-    setup_complete = true;  //used for interrupt timing
-
-} 
- 
 int main() {
     setup();
     
-    //this is used to control timing on the rudder servo, it's based on the new timing model in the 5/29/18 code with one interrupt running everything...may or may not work here
+    unsigned int tNow = 0;
+    
+    pc().baud(57600);
+    pc().printf("\n\n\r  TICKER TEST 05/25/2018 running at 10 kHz (0.0001 second interval) \n\n\r");
     
     systemTicker.attach_us(&system_timer, 1000);         // Interrupt timer running at 0.001 seconds       (slower than original ADC time interval)
+        
+    while (1) {        
+        if( read_ticker() )                         // read_ticker runs at the speed of 10 kHz
+        {
+            ++tNow;
 
-    while(1) {
-        static int current_state = 0;
-        
-        static bool file_opened = false;
-        
-        // FSM loop runs at 10 hz
-        if(loop) {
-            led1() = !led1(); // blink led 1
-            current_state = stateMachine().runStateMachine();       //running State Machine. Returns 0 if sitting idle or keyboard press (SIT_IDLE state).
-            loop = false; // wait until the loop rate timer fires again
-        }
-        
-        // log loop runs at 1 hz
-        if (log_loop) {
-            //when the state machine is not in SIT_IDLE state (or a random keyboard press)
-            if (current_state == TRANSMIT_MBED_LOG or current_state == RECEIVE_SEQUENCE) {
-                //main_loop_rate_ticker.detach();
-                //log_loop_rate_ticker.detach();
-                
-                ;   //pass
+            if ( (tNow % 10) == 0 ) {   // 0.001 second intervals               //if ( (tNow % 100) == 0 ) {   // 0.01 second intervals
+                fsm_loop = true;
+                FSM();
             }
             
-            else if(current_state != 0) {
-                if (!file_opened) {                                 //if the log file is not open, open it
-                    mbedLogger().appendLogFile(current_state, 0);   //open MBED file once
-//                    sdLogger().appendLogFile(current_state, 0);     //open SD file once
-                               
-                    file_opened = true;                             //stops it from continuing to open it
-
-                    pc().printf(">>>>>>>> Recording. Log file opened. <<<<<<<<\n\r");
-                }
-                
-                //record to Mbed file system     
-                mbedLogger().appendLogFile(current_state, 1);    //writing data
-//                sdLogger().appendLogFile(current_state, 1);    //writing data
-            }
-            
-            //when the current FSM state is zero, reset the file
-            else {
-                //this can only happen once
-                if (file_opened) {
-                    mbedLogger().appendLogFile(current_state, 0);    //close log file
-//                    sdLogger().appendLogFile(current_state, 0);    //close log file
-                    
-                    file_opened = false;
-                    
-                    pc().printf(">>>>>>>> Stopped recording. Log file closed. <<<<<<<<\n\r");
-                }
-            }
-            
-            log_loop = false;   // wait until the loop rate timer fires again
-        }   //END OF LOG LOOP
+//            if ( (tNow % 1000) == 0 ) {   // 1.0 second intervals
+//                log_loop = true;
+//                log_function();
+//            }
+        }
     }
-
 }
\ No newline at end of file