Diff: main.cpp

Revision:

12:3b83eb9a98bc

Parent:

11:c47a34f047d5

diff -r c47a34f047d5 -r 3b83eb9a98bc main.cpp
--- a/main.cpp	Sun Apr 22 18:05:58 2018 +0000
+++ b/main.cpp	Sun Apr 22 19:22:49 2018 +0000
@@ -9,9 +9,7 @@
 //Communication
 Serial bt(PTE22, PTE23); //Serial Pins (Tx, Rx)
 //LEDs
-DigitalOut blue(LED_BLUE);
-//DigitalOut red(LED_RED);
-//DigitalOut green(LED_GREEN);
+
 //Checkpoint Interrupts
 InterruptIn navRt(PTD2);
 InterruptIn navLft(PTD3);
@@ -19,52 +17,12 @@
 
 /***********************************|Variable Declarations|*************************************************************/
 
-
-
-bool lTrig = false;
-bool rTrig = false;
-
-volatile int rightCount;
-volatile int leftCount;
-bool go = 0;
+//bool lTrig = false;
+//bool rTrig = false;
+//
+//int rightCount;
+//int leftCount;
 
-//void waitState()
-//{
-//    Setpoint = 0; //Makes sure the car isn't moving
-//    state = 'w';
-//    green = red = 1; 
-//    blue = 0;        //blue LED indicating car is in wait state 
-//} 
-//void offTrack()
-//{
-//    Setpoint = 0;
-//    brake.write(1);
-//    state = 'o';
-//    //bt.putc(state);
-//    green = blue = 1;
-//    red = 0;
-//    waitState();
-//}
-//
-//void letsGo()
-//{
-//    state = 'g';
-//    //bt.putc(state);
-//    red = blue = 1;
-//    green = 0;
-//    brake.write(0);
-//    go = 1;
-//    Setpoint = spHolder;     
-//}
-//
-//void stopState()
-//{
-//    state = 's';
-//    spHolder = Setpoint;
-//    Setpoint = 0;
-//    brake.write(1);
-//    waitState();
-//}
 
 //void applyBrake()
 //{  
@@ -101,17 +59,17 @@
     if(Setpoint < MINM) Setpoint = MINM;    
 }
 
-void incL()
-{      
-    leftCount++;
-    lTrig = true;
-}
-
-void incR()
-{
-    rightCount++;
-    rTrig = true;
-}
+//void incL()
+//{      
+//    leftCount++;
+//    lTrig = true;
+//}
+//
+//void incR()
+//{
+//    rightCount++;
+//    rTrig = true;
+//}
 
 int main()
 {   
@@ -119,14 +77,18 @@
     bt.attach(&serCb);
     cameraInit();       
     controlInit();
+    initStateMachine();
     
-    rightCount = 0;
-    leftCount = 0; 
+//    rightCount = 0;
+//    leftCount = 0; 
     
-    navRt.fall(&incR);
-    navLft.fall(&incL);
-    _state = WAIT;   
+    //navRt.fall(&incR);
+//    navLft.fall(&incL);   
     while(1) {
-        bt.printf("%f %f\r\n", data[0], data[1]);
+        systemState();
+        if(paramChanged){
+            bt.printf("%f\r\n", Setpoint);
+            paramChanged = false;
+        }
     }
 }
\ No newline at end of file