File content as of revision 12:3b83eb9a98bc:

#include "mbed.h"
#include <iostream>
#include "globals.h"
#include "lsc.h"
#include "stateMachine.h"
#include "control.h"

/***********************************|Pin Declarations|*************************************************************/
//Communication
Serial bt(PTE22, PTE23); //Serial Pins (Tx, Rx)
//LEDs

//Checkpoint Interrupts
InterruptIn navRt(PTD2);
InterruptIn navLft(PTD3);


/***********************************|Variable Declarations|*************************************************************/

//bool lTrig = false;
//bool rTrig = false;
//
//int rightCount;
//int leftCount;


//void applyBrake()
//{  
//    spHolder = Setpoint;
//    brake.write(1);
//    Setpoint = 0.0;
//}
//
//void releaseBrake()
//{
//    brake.write(0);
//    Setpoint = spHolder;
//}
bool paramChanged = false;

void serCb()
{
    char x = bt.getc();
    if (x == 'u')
    {
        Setpoint += 0.025;
        paramChanged = true;
    }
    else if(x == 'h')
    {
        Setpoint -= 0.025;
        paramChanged = true;
    }    
    else
    {
        bt.printf("Invalid input");
    }
    if(Setpoint > MAXM) Setpoint = MAXM;
    if(Setpoint < MINM) Setpoint = MINM;    
}

//void incL()
//{      
//    leftCount++;
//    lTrig = true;
//}
//
//void incR()
//{
//    rightCount++;
//    rTrig = true;
//}

int main()
{   
    bt.baud(115200);
    bt.attach(&serCb);
    cameraInit();       
    controlInit();
    initStateMachine();
    
//    rightCount = 0;
//    leftCount = 0; 
    
    //navRt.fall(&incR);
//    navLft.fall(&incL);   
    while(1) {
        systemState();
        if(paramChanged){
            bt.printf("%f\r\n", Setpoint);
            paramChanged = false;
        }
    }
}