most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown

Dependencies:   mbed MODSERIAL FATFileSystem

main.cpp

Committer:
tnhnrl
Date:
20 months ago
Revision:
49:47ffa4feb6db
Parent:
48:20e681885161
Child:
50:1d59ea7c7a1c

File content as of revision 49:47ffa4feb6db:

/*
    Modified 2018-02-01 revA by Troy
        - Fixed bugs with file opening and closing
        - Made a directory printing function in the MbedLogger class that does not use the Directory List library (removed)
    Modified 2018-02-02 revA by Troy
        - Placed mbed file transmitter functionality into MbedLogger class
        - created a macro for printing out files to PC and serial port simultaneously (placed in main)
    Modified 2018-02-07 revA by Troy
        - SDFileSystem class added to code along with sd card file system in System "StaticDefs"
        - Had to fix bug with SDFileSystem, the imported version uses a buggy FATFileSystem class
        - Update came from https://os.mbed.com/teams/mbed-official/code/FATFileSystem/  (not the library importer tool)
        - Modified MbedLogger constructor to include string for file system, e.g. "/sd/" and "/local/"
        - Modified MbedLogger class to use local or sd card
        - Created SD card file system using reader on pins p11, p12, p13, p14 (SPI interface)
    Modified 2018-02-08 revA by Troy
        - Test code without ADC running to check behavior (timing issue)
        - Modified sequence to use sequence.txt and fixed warnings
        - Fixed CRC issue with file reception (the command that the MBED sends out)
        - Issue with IMU data on p27, need to check wiring and or MAX232
    Modified 2018-02-14 revA by Troy
        - IMU tested and working correctly
        - Data logging on SD card and MBED working simultaneously
        - State Machine is being used to transmit data over Xbee instead of a separate ticker
        - Servo driver tested separately in FSG_servo_test program (verified servo works separately) 
        - Electrical issue with PWM signal interfering with motor driver signal (disabled servo for now)
        - Ability to erase MBED (only the MBED) log file through keyboard console
    Modified 2018-02-15 revA by Troy
        - Made minor changes including cleaning up some functions
    Modified 2018-02-15 revB by Troy
        - Modified StateMachine class to include manual motor-driving mode and FSM-driven battery and buoyancy engine motor inner loops (CHECK_TUNING)
        - Fixed some on-screen printouts in the StateMachine class
        - Added CHECK_TUNING state to MbedLogger printouts to MBED and SD card
        - Pool tested code worked with several dives.  Neutral was BCE: 162 mm, BATT: 36 mm with CHECK_TUNING state; FIND_NEUTRAL (auto-tune) produced 161.0 and 34 mm
*/
 
#include "mbed.h"
#include "StaticDefs.hpp"
 
// loop rate used to determine how fast events trigger in the while loop
Ticker 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 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 log_loop_trigger() { log_loop = true;} // log loop trigger (used in while loop)

void setup() {
    pc().baud(57600);
    pc().printf("\n\n\rFSG POOL TEST 2017-02-15 revA\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");
 
    // start up the system timer
    systemTime().start();
 
    // 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().init();
    depth().tare();
    
    // construct the MBED local file system
    local();
    
    // construct the SD card file system
    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)
 
 
    // set up the linear actuators.  adc has to be running first.
    bce().init();
    bce().start();
    bce().pause(); // start by not moving
 
    batt().init();
    batt().start();
    batt().pause(); // start by not moving
 
    // set up the depth and pitch outer loop controllers
    depthLoop().init();
    depthLoop().start();
    depthLoop().setCommand(stateMachine().getDepthCommand());
 
    pitchLoop().init();
    pitchLoop().start();
    pitchLoop().setCommand(stateMachine().getPitchCommand());
 
    // 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("\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();
    
    //create log files if not present on file system
    mbedLogger().initializeLogFile();
    sdLogger().initializeLogFile();
} 
 
int main() {
    setup();
    
    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_LOG or current_state == RECEIVE_SEQUENCE) {
                //main_loop_rate_ticker.detach();
                //log_loop_rate_ticker.detach();
                
                ;   //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

                    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
    }
}