Diff: main.cpp

Revision:

15:d820f7c1898e

Parent:

6:1b4a677c468c

Child:

16:2101784637fc

--- a/main.cpp	Sat Apr 21 22:43:32 2018 +0000
+++ b/main.cpp	Tue Apr 24 01:31:48 2018 +0000
@@ -8,7 +8,7 @@
 
 #include "mbed.h"
 #include <iostream>
-#include "lsc.h"
+
 
 #define TI 0.001f
 
@@ -34,7 +34,7 @@
 AnalogIn speed(PTB0);   //tachometer
 AnalogIn _right(PTB1);  //Right sensor
 AnalogIn _left(PTB2);   //Left sensor
-//AnalogIn camIn(PTB3);   //Camera Input
+
 //Output
 PwmOut servoSig(PTA13); //PWM output to control servo position
 PwmOut gateDrive(PTA4); //PWM output to control motor speed
@@ -51,10 +51,10 @@
 //Button Interrupts
 InterruptIn stopButton(PTD4);
 InterruptIn goButton(PTA12);
-//Camera timers
-//DigitalOut si(PTC8);
-//PwmOut clk(PTC9);
-//InterruptIn edgeDetector(PTD5);
+//Checkpoint LEDS
+BusOut checkLED(PTB8,PTB9,PTB10,PTB11);
+
+
 
 /***********************************|Variable Declarations|*************************************************************/
 
@@ -78,17 +78,25 @@
 float Kd = 1.0e-4;
 
 float Kpm = 0.141;
+int checkpoint = 0;
 
 char state;
-bool go = 0;
+
 
 void waitState()
 {
     Setpoint = 0; //Makes sure the car isn't moving
     state = 'w';
     //bt.putc(state);
+    if (_left.read() == 0.0 &&  _right.read() == 0.0)
+    {
+        green = blue = 1;
+        red = 0;   
+    } else {
+    brake.write(0);    
     green = red = 1; 
     blue = 0;        //blue LED indicating car is in wait state 
+    }
 } 
 void offTrack()
 {
@@ -101,18 +109,7 @@
     waitState();
 }
 
-void letsGo()
-{
-        state = 'g';
-        //bt.putc(state);
-        red = blue = 1;
-        green = 0;
-        brake.write(0);
-        go = 1;
-        Setpoint = spHolder;
-        
-        
-}
+
 
 
 void stopState()
@@ -124,7 +121,22 @@
     brake.write(1);
     waitState();
 }
-
+void letsGo()
+{
+        if (_left.read() == 0.0 &&  _right.read() == 0.0)
+        {
+            stopState();
+                
+        }
+        state = 'g';
+        //bt.putc(state);
+        red = blue = 1;
+        green = 0;
+        brake.write(0);
+        Setpoint = spHolder;
+        
+        
+}
 inline void applyBrake()
 {  
     spHolder = Setpoint;
@@ -143,6 +155,11 @@
     bt.printf("Setpoint = %f, Kps = %f, Kd = %f, Kpm = %f, Brake = %f\r\n", Setpoint, Kps, Kd, Kpm, brake.read());
 }
 
+void displayCheckpoint()
+{
+    bt.printf("Checkpoint reached: %i\r\n", checkpoint);   
+} 
+
 
 void serCb()
 {
@@ -226,13 +243,17 @@
 void incL()
 {      
     leftCount++;
+    checkpoint++;
     lTrig = true;
+    displayCheckpoint();
 }
 
 void incR()
 {
     rightCount++;
+    checkpoint++;
     rTrig = true;
+    displayCheckpoint();
 }
 
 void steer()
@@ -242,11 +263,9 @@
     float R = _right.read();
     if (L == 0.0 && R == 0.0)
     {
-        //off track
-        //offTrack();
+        stopState();
     }           
-//    float fb = L - R; 
-    float fb = ((float)camMax)/64.0;;
+    float fb = L - R; 
     float e = SET - fb;
     float Controlleroutput = Kps * e - (Kd * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error?? 
     if (Controlleroutput > MAXS) Controlleroutput = MAXS;
@@ -292,8 +311,6 @@
     //startup checks
     bt.baud(115200);
     bt.attach(&serCb);
-    cameraInit();
-    cameraDaq.attach(&acquire, 0.01f);    
     servoSig.period(STEER_FREQ);
     gateDrive.period(.00005f);
     gateDrive.write(Setpoint);
@@ -313,7 +330,7 @@
     stopButton.fall(&stopState);
     waitState();
     while(1) {
-        
+        checkLED.write(checkpoint);
         if(lTrig){
             bt.putc('l');
             lTrig = false;
@@ -322,7 +339,7 @@
             bt.putc('r');
             rTrig = false;
         }
-        //bt.printf("Cammax %i\r\n", camMax);
+    
 
     }
 }
\ No newline at end of file