Diff: main.cpp

Revision:

1:5265e3a6f3d7

Parent:

0:0170bad0f358

Child:

2:9a3221b22855

--- a/main.cpp	Sat Dec 03 23:42:47 2016 +0000
+++ b/main.cpp	Wed Dec 07 20:36:22 2016 +0000
@@ -15,31 +15,47 @@
 int Rcount = 0;
 int Lcount = 0;
 int Error = 0;
-float Rspeed = .5;
-float Lspeed = .5;
-int Instr = 1;
-int Speed = 50;
-int Rtot;
-int Ltot;
+float Rspeed = .4;
+float Lspeed = .4;
+int Instr = 5;
+int Rtot=0;
+int Ltot=0;
+char c;
+int pics = 0;
+int TError = 0;
+char V;
+char M;
+
 
 Ticker Sampler;
 
  
 void RSample() {
-    Rcount++;
-//    Rtot = Rtot + Rcount;
+    Rtot++;
 // pc.printf("Rcount: %d\n\r",Rcount);  
 }
 
 void LSample() {
-    Lcount++;
-//    Ltot = Ltot + Lcount;
+    Ltot++;
 }
 
 void callback() {
     // Note: you need to actually read from the serial to clear the RX interrupt
-    printf("%c\n",pc.getc());
+    //printf("%c\n",pc.getc());
+    //printf("Hello world");
+    c = pc.getc();
     led = !led;
+    if (c == '0') { //Stop
+        Instr = 0;   
+    } else if (c == '1') { //Forward
+        Instr = 1;
+    }else if (c == '2') { //Left
+        Instr = 2;
+    } else if (c == '3') { //Right
+        Instr = 3;
+    } else if (c == '4') { //REverse
+        Instr = 4;
+    }
 }
  
 int main() {
@@ -51,45 +67,128 @@
     Lencoder.rise(&LSample);    
     
     while(1) { 
+    
+       //printf("Instr = %d\n", Instr);
+       
+       if (Instr == 0) {
+            Rspeed = 0;
+            Lspeed = 0;
+        } else if (Instr == 1) {
+            TError = Ltot - Rtot;
+            if(TError > 0) Rspeed = Rspeed+.01;
+            else if (TError == 0) Rspeed = Rspeed;
+            else Rspeed = Rspeed - .01;
+            Rspeed = Rspeed + .0008;
+        } else if (Instr == 2) {
+             TError = Ltot - Rtot;
+            if(TError > 0) Rspeed = Rspeed+.01;
+            else if (TError == 0) Rspeed = Rspeed;
+            else Rspeed = Rspeed - .01;
+            Rspeed = Rspeed + .0008;
+            LMotor.speed(0);
+            RMotor.speed(0);
+            if (Lspeed >0){
+                Lspeed = -Lspeed;
+             }
+            
+            
+        } else if (Instr == 3) {
+               TError = Ltot - Rtot;
+            if(TError > 0) Rspeed = abs(Rspeed)+.01;
+            else if (TError == 0) Rspeed = Rspeed;
+            else Rspeed = abs(Rspeed) - .01;
+            Rspeed = Rspeed + .0008;
+                LMotor.speed(0);
+                RMotor.speed(0);
+                if (Rspeed >0){
+                    Rspeed = -Rspeed;
+                }
+            
+        } else if (Instr == 4) {
+              TError = Ltot - Rtot;
+            if(TError > 0) Rspeed = abs(Rspeed)+.0075;
+            else if (TError == 0) Rspeed = Rspeed;
+            else Rspeed = abs(Rspeed) - .005;
+            Rspeed = Rspeed + .00075;
+                if (Rspeed > 0){
+                Rspeed = -Rspeed;
+                }
+                if (Lspeed > 0){
+                Lspeed = -Lspeed;
+                }
+                LMotor.speed(0);
+                RMotor.speed(0);
+        }
+        else if (Instr == 5) {
+        
+        LMotor.speed(0);
+        RMotor.speed(0);
+        wait(.2);
+        while(Rtot < 55 | Ltot < 55){
+        LMotor.speed(.3);
+        RMotor.speed(-.3);}
+        LMotor.speed(0);
+        RMotor.speed(0);
+        pc.printf( "RTot: %d\n\r",Rtot);
+        pc.printf("LTot: %d\n\r",Ltot); 
+        Rtot = 0;
+        Ltot = 0;
+        wait(15); // TAKE PICTURE
+        while (V == 0){ // V = whether picture is valid or not
+        wait(10);// Continue to take pictures)
+        }
+            if (M == 1){  // if match found drives forward
+            LMotor.speed(Lspeed);
+            RMotor.speed(Rspeed);
+            wait(1);
+            LMotor.speed(0);
+            RMotor.speed(0);
+            wait(1);//snap tons of photos
+            LMotor.speed(-Lspeed);
+            RMotor.speed(-Rspeed);
+            }
+        }
+        /*
     switch (Instr){
     
     // stop instruction
-        case (0):
+        case 0:
         Rspeed = 0;
         Lspeed = 0;
         break;
     // forward instruction
-        case (1) :  //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 
+        case 1 :  //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 
         Error =  Lcount - Rcount;
         //pc.printf("Lcount in sampler: %f\n\r",Lcount);
         //pc.printf("RCount in sampler: %f\n\r",Rcount);
         //pc.printf("Error in sampler: %f\n\r",Error);
         //pc.printf("Rspeed: %f \n \r", Rspeed);
-        Rspeed = Rspeed + ((float)Error / 80);
+        Rspeed = Rspeed + ((float)Error / 10);
         break;
         
     // Left turn Instruction
-        case (2):   
+        case 2:   
         Error =  Lcount - Rcount;
         //pc.printf("Lcount in sampler: %f\n\r",Lcount);
         //pc.printf("RCount in sampler: %f\n\r",Rcount);
         //pc.printf("Error in sampler: %f\n\r",Error);
         //pc.printf("Rspeed: %f \n \r", Rspeed);
-        Rspeed = Rspeed + ((float)Error / 80);
+        Rspeed = Rspeed + ((float)Error / 10);
         LMotor.speed(0);
         RMotor.speed(0);
         if (Lspeed >0){
-        Lspeed = -Lspeed;}
+        Lspeed = -Lspeed;
+        }
         break;
         
     // Right turn Instruction
-        case (3):   
+        case 3:   
         Error =  Lcount - Rcount;
         //pc.printf("Lcount in sampler: %f\n\r",Lcount);
         //pc.printf("RCount in sampler: %f\n\r",Rcount);
         //pc.printf("Error in sampler: %f\n\r",Error);
         //pc.printf("Rspeed: %f \n \r", Rspeed);
-        Rspeed = Rspeed + ((float)Error / 80);
+        Rspeed = Rspeed + ((float)Error / 10);
         LMotor.speed(0);
         RMotor.speed(0);
         if (Rspeed >0){
@@ -97,13 +196,13 @@
         break;
         
     // reverse instruction
-        case (4):  //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 
+        case 4:  //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 
         Error =  Rcount - Lcount;
         //pc.printf("Lcount in sampler: %f\n\r",Lcount);
         //pc.printf("RCount in sampler: %f\n\r",Rcount);
         //pc.printf("Error in sampler: %f\n\r",Error);
         //pc.printf("Rspeed: %f \n \r", Rspeed);
-        Rspeed = Rspeed + ((float)Error / 80);
+        Rspeed = Rspeed + ((float)Error / 10);
         if (Rspeed > 0){
         Rspeed = -Rspeed;
         }
@@ -115,11 +214,15 @@
         break;
        }
        
+       */
+if (Rtot >10 || Ltot > 10){
+                Rtot = 0;
+                Ltot = 0;}
 LMotor.speed(Lspeed);
 RMotor.speed(Rspeed);
 Lcount = 0; //Restart the counters
 Rcount = 0;
+
 wait(.02);
 }
-}
-
+}
\ No newline at end of file