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



File content as of revision 3:7824127c5cfd:

#include "mbed.h"
#include "StaticDefs.hpp"
#include "config_functions.h"

#define BCE_P 0.1
#define BCE_I 0.0
#define BCE_D 0.0

#define BCE_ZERO 400    //counts
#define BCE_LIMIT 498.729   //mm
#define POT_SLOPE .121760   // mm/counts

#define BATT_P 0.1
#define BATT_I 0.0
#define BATT_D 0.0

#define BATT_ZERO 100    //counts
#define BATT_LIMIT 50.0 //mm

int main()
    local();  //this makes sure the local file system is constructed
    //Read in and load the BCE parameters from the text file "bce.txt"
    ////Read in and load the battery mover parameters from the text file "batt.txt"
    // do not leave this in. It is just there to check that things are working
    pc().printf("\n\rP: %3.2f I: %3.2f D %3.2f zero: %3i limit %3.2f slope %3.2f", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().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

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

    //start the bce and batt control loops.  ADC has to be running first.
    //The motors will not activate until their respective position filters have
    //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            

    while(1) {
        //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
        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 ;

            if (userInput == 'w') {
                //increment the P gain
                P += 0.01;
            if (userInput == 's') {
                //decrement the P gain
                P -= 0.01;
            if (userInput == 'i') {
                //increment the D gain
                D += 0.001;
            if (userInput == 'k') {
                //decrement the D gain
                D -= 0.001;
            if (userInput == 't') {
                //increment the I gain
                I += 0.001;
            if (userInput == 'g') {
                //decrement the I gain
                I -= 0.001;
            if (userInput == ' ') {
                //reset the h-bridge

            //if (userInput == ' ') {
            //stop the ride
            //    motor_cmd = 0.0 ;

            //clip pwm if over range
            //if (motor_cmd > 1.0) {
            //    motor_cmd = 1.0;
            //if (motor_cmd < -1.0) {
            //    motor_cmd = -1.0;
            // assign the shiny new commands to the pins
            if (userInput == '\r') {
                //    posCon().setPgain(P);
                //    posCon().setIgain(I);
                //    posCon().setDgain(D);

     //   }  

        //pc().printf("Position: %10.1f mm  Velocity: % 2.2f mm/s  Output: % 1.3f   switch: %d    \r", bce().getPosition_mm(), bce().getVelocity_mms(), bce().getOutput(), bce().getSwitch());
        //pc().printf("pos: %3.0f mm  vel: % 2.2f mm/s  Set Point %3.0f  controller output: % 1.3f P: %1.3f I: %1.4f D: %1.4f\r", pvf().getPosition(), pvf().getVelocity(), positionCmd, posCon().getOutput(), P, I, D);