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:
mkelly10
Date:
24 months ago
Revision:
5:15bd96205bb2
Parent:
4:66f13fbb035d
Child:
10:085ab7328054

File content as of revision 5:15bd96205bb2:

#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();

    ////Read in and load the battery mover parameters from the text file "batt.txt"
    load_BATT_config();
    batt().init();

    // 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
    systemTime().start();

    //setup and start the adc. This runs on a fixed interval and is interrupt driven
    adc().initialize();
    adc().start();

    //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);

    bce().unpause();

    batt().start();
    batt().pause();
    
    batt().setPosition_mm(positionCmd);
    
    batt().unpause();




    while(1) {

        
        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);
            }
        }
        //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);
    }
}



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

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