Diff: main.cpp

Revision:

6:1b4a677c468c

Parent:

5:aa582398b2eb

Child:

7:0b883a7a2754

Child:

11:c47a34f047d5

Child:

13:d1f37c9038de

Child:

15:d820f7c1898e

diff -r aa582398b2eb -r 1b4a677c468c main.cpp
--- a/main.cpp	Thu Apr 19 15:42:21 2018 +0000
+++ b/main.cpp	Sat Apr 21 22:43:32 2018 +0000
@@ -8,6 +8,7 @@
 
 #include "mbed.h"
 #include <iostream>
+#include "lsc.h"
 
 #define TI 0.001f
 
@@ -27,22 +28,39 @@
 #define STEER_FREQ 0.02f //50 Hz
 #define STEERUPDATEPERIOD 0.05
 
-AnalogIn _right(PTB1); //Right sensor
-AnalogIn _left(PTB3); //Left sensor
-AnalogIn speed(PTB2);   //tachometer
+/***********************************|Pin Declarations|*************************************************************/
 
-PwmOut servoSig(PTA13); //PWM output to control servo
-PwmOut gateDrive(PTA4);
+//Feedback
+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
+DigitalOut brake(PTD0);
+//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);
+//Button Interrupts
+InterruptIn stopButton(PTD4);
+InterruptIn goButton(PTA12);
+//Camera timers
+//DigitalOut si(PTC8);
+//PwmOut clk(PTC9);
+//InterruptIn edgeDetector(PTD5);
 
-Serial bt(PTE0, PTE1); //COM12
-DigitalOut myled(LED_BLUE);
-DigitalOut brake(PTD0);
+/***********************************|Variable Declarations|*************************************************************/
 
 Ticker control;
 Timer ctrlTimer;
 
-InterruptIn navRt(PTD2);
-InterruptIn navLft(PTD3);
 bool lTrig = false;
 bool rTrig = false;
 
@@ -61,6 +79,52 @@
 
 float Kpm = 0.141;
 
+char state;
+bool go = 0;
+
+void waitState()
+{
+    Setpoint = 0; //Makes sure the car isn't moving
+    state = 'w';
+    //bt.putc(state);
+    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';
+    //bt.putc('s');
+    spHolder = Setpoint;
+    Setpoint = 0;
+    brake.write(1);
+    waitState();
+}
+
 inline void applyBrake()
 {  
     spHolder = Setpoint;
@@ -79,6 +143,7 @@
     bt.printf("Setpoint = %f, Kps = %f, Kd = %f, Kpm = %f, Brake = %f\r\n", Setpoint, Kps, Kd, Kpm, brake.read());
 }
 
+
 void serCb()
 {
     char x = bt.getc();
@@ -141,6 +206,14 @@
     {
         bt.printf("Right = %i Left = %i\r\n", rightCount, leftCount);
     }
+    else if(x== 's')
+    {
+        stopState();
+    }
+    else if(x == 'a')
+    {
+        letsGo();   
+    }    
     else
     {
         bt.printf("Invalid input");
@@ -164,14 +237,16 @@
 
 void steer()
 {
+    
     float L = _left.read();
     float R = _right.read();
     if (L == 0.0 && R == 0.0)
     {
         //off track
-        Setpoint = 0.0;
+        //offTrack();
     }           
-    float fb = L - R; 
+//    float fb = L - R; 
+    float fb = ((float)camMax)/64.0;;
     float e = SET - fb;
     float Controlleroutput = Kps * e - (Kd * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error?? 
     if (Controlleroutput > MAXS) Controlleroutput = MAXS;
@@ -207,17 +282,22 @@
 void cb()
 {
     steer();
-    drive();
+    if (state == 'g'){
+        drive();
+    }
 }
 
 int main()
 {   
     //startup checks
     bt.baud(115200);
-    bt.attach(&serCb);    
+    bt.attach(&serCb);
+    cameraInit();
+    cameraDaq.attach(&acquire, 0.01f);    
     servoSig.period(STEER_FREQ);
     gateDrive.period(.00005f);
     gateDrive.write(Setpoint);
+   
 
     ctrlTimer.start();
     control.attach(&cb, TI);
@@ -229,8 +309,11 @@
     navRt.fall(&incR);
     navLft.fall(&incL);
     
+    goButton.fall(&letsGo);
+    stopButton.fall(&stopState);
+    waitState();
     while(1) {
-        myled = !myled;
+        
         if(lTrig){
             bt.putc('l');
             lTrig = false;
@@ -239,11 +322,7 @@
             bt.putc('r');
             rTrig = false;
         }
-        //bt.printf("%f ",data[0]);
-//        bt.printf("%f ", data[1]);
-//        bt.printf("%f ", data[2]);
-//        bt.printf("%f ", data[3]);
-//        bt.printf("%f ", data[4]);
-//        bt.printf("%f\r\n", data[5]);
+        //bt.printf("Cammax %i\r\n", camMax);
+
     }
 }
\ No newline at end of file