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:
23 months ago
Revision:
24:c7d9b5bf3829
Parent:
23:434f04ef1fad
Child:
27:0a5b90cd65d6

File content as of revision 24:c7d9b5bf3829:

/*
    Modified 2017-11-29 revA by Troy
        - changelog.txt carries previous changes
        - Incorporated email changes and did quick bench test
        - Redid the sub-state recording method to only update on state changes
*/
 
#include "mbed.h"
#include "StaticDefs.hpp"
 
// loop rate timer for slowing down how fast while(1) runs in main()
Ticker loop_rate_ticker;
volatile bool loop = false;
void loop_trigger() { loop = true; }; // loop trigger
 
void setup() {
    pc().baud(57600);
    pc().printf("\n\n\rFSG POOL TEST 2017-11-29\n\n\r");
 
    // 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 a local file system
    local();
 
    // 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 %3.0f mm, slope %3.3f  \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 %3.0f mm, slope %3.3f  \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
} 
 
int main() {
    setup();
    
    while(1) {
        // does this stuff at the loop rate
        if(loop) {
            led1() = !led1(); // blink
            stateMachine().runStateMachine();
            loop = false; // wait until the loop rate timer fires again
        }
    }
}