Diff: main.cpp

Revision:

3:c9e09e968552

Parent:

2:9a3221b22855

Child:

4:295160ab06c7

--- a/main.cpp	Wed Dec 07 20:46:37 2016 +0000
+++ b/main.cpp	Sat Dec 10 20:20:07 2016 +0000
@@ -17,16 +17,20 @@
 int Error = 0;
 float Rspeed = .4;
 float Lspeed = .4;
-int Instr = 5;
+float TRspeed = .35;
+float TLspeed = .35;
+
+int Instr = 0;
 int Rtot=0;
 int Ltot=0;
 char c;
 int targets = 0;
-int TError = 0;
+float TError = 0;
 char V;
 char M;
 int current = 1;
 
+
 Ticker Sampler;
 
  
@@ -40,22 +44,25 @@
 }
 
 void callback() {
-    // Note: you need to actually read from the serial to clear the RX interrupt
-    //printf("%c\n",pc.getc());
-    //printf("Hello world");
     c = pc.getc();
+  //  printf("%c",pic);
+  //  pc.printf("%c",pic);
+  //pc.printf("Hello world2 \n \r");
     led = !led;
     if (c == '0') { //Stop
         Instr = 0;   
-    } else if (c == '1') { //Forward
+    } else if (c == '4') { //reverse
         Instr = 1;
-    }else if (c == '2') { //Left
+    } else if (c == '2') { //Left
         Instr = 2;
     } else if (c == '3') { //Right
         Instr = 3;
-    } else if (c == '4') { //REverse
+    } else if (c == '1') { //forward
         Instr = 4;
-    }
+    } else if (c == 'A') { //Rotate + take pic
+        Instr = 5;
+    } else if (c == 'D') { //move foward set amount
+    ;}
 }
  
 int main() {
@@ -66,165 +73,83 @@
     Lencoder.mode(PullUp);
     Lencoder.rise(&LSample);    
     
-    while(1) { 
+    while(1) {  
     
        //printf("Instr = %d\n", Instr);
        
        if (Instr == 0) {
             Rspeed = 0;
             Lspeed = 0;
-        } else if (Instr == 1) {
+        } else if (Instr == 1) { //move backwards (forward, we turned our bot around)
             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 = (float) TError +.05;
+           Rspeed = abs(Rspeed) + (float)TError /100;
+           Lspeed = abs(Lspeed);
+           if(Rspeed < 0)
+           RMotor.speed(0);
+           if(Lspeed < 0)
+           LMotor.speed(0); //sets speed to 0 because motor cannot go from negative to positive without hitting 0
+           
+        } else if (Instr == 2) {        //turn Left 
              TError = Ltot - Rtot;
-            if(TError > 0) Rspeed = Rspeed+.01;
-            else if (TError == 0) Rspeed = Rspeed;
-            else Rspeed = Rspeed - .01;
-            Rspeed = Rspeed + .0008;
+            if(TError > 0) TRspeed = abs(TRspeed)+.01;
+            else if (TError == 0) TRspeed = TRspeed;
+            else TRspeed = abs(TRspeed) - .01;
+           // Rspeed = Rspeed + .0008;
             LMotor.speed(0);
             RMotor.speed(0);
-            if (Lspeed >0){
-                Lspeed = -Lspeed;
-             }
-            
+            TLspeed = -abs(TLspeed); //Lwheel must move in reverse for left turn
+            TRspeed = abs(TRspeed); //Rwheel must move forward for left turn
             
-        } else if (Instr == 3) {
+        } else if (Instr == 3) {        // turn right
                TError = Ltot - Rtot;
-            if(TError > 0) Rspeed = abs(Rspeed)+.01;
-            else if (TError == 0) Rspeed = Rspeed;
-            else Rspeed = abs(Rspeed) - .01;
-            Rspeed = Rspeed + .0008;
+            if(TError > 0) TRspeed = abs(TRspeed)+.01;
+            else if (TError == 0) TRspeed = TRspeed;
+            else TRspeed = abs(TRspeed) - .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;
-                }
+            TRspeed = -abs(TRspeed); 
+            TLspeed = abs(TLspeed);
+        } else if (Instr == 4) {        // go forward (reverse, we turned our bot around)
+                TError = Ltot - Rtot; 
+                Rspeed = abs(Rspeed) + (float) TError / 100;
+                Rspeed = -abs(Rspeed);
+                Lspeed = -abs(Lspeed);
                 LMotor.speed(0);
                 RMotor.speed(0);
-        }
-        else if (Instr == 5) {
+            }
+        else if (Instr == 5) { //Rotate + Pic
         
-        for (current < targets+1){
+
         LMotor.speed(0);
         RMotor.speed(0);
         wait(.2);
-        while(Rtot < 55 | Ltot < 55){
-        LMotor.speed(.3);
-        RMotor.speed(-.3);}
+        while(Rtot < 50 | Ltot < 50){
+        LMotor.speed(.4);
+        RMotor.speed(-.4);}
         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);
-            wait(1);
-            current = targets+1; //if a match is found, stops the search
-            }
-            else current++;
-        }
+        wait(1);
+        pc.printf("P\n"); //commands pi to take pic
         Instr = 0; //turns status to stop after search
-        /*
-    switch (Instr){
-    
-    // stop instruction
-        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. 
-        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 / 10);
-        break;
-        
-    // Left turn Instruction
-        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 / 10);
-        LMotor.speed(0);
-        RMotor.speed(0);
-        if (Lspeed >0){
-        Lspeed = -Lspeed;
         }
-        break;
-        
-    // Right turn Instruction
-        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 / 10);
-        LMotor.speed(0);
-        RMotor.speed(0);
-        if (Rspeed >0){
-        Rspeed = -Rspeed;}
-        break;
-        
-    // reverse instruction
-        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 / 10);
-        if (Rspeed > 0){
-        Rspeed = -Rspeed;
-        }
-        if (Lspeed > 0){
-        Lspeed = -Lspeed;
-        }
-        LMotor.speed(0);
-        RMotor.speed(0);
-        break;
-       }
+        else if (Instr == 6) { //code for nudging forward
+          ; } 
+ 
        
-       */
 if (Rtot >10 || Ltot > 10){
                 Rtot = 0;
                 Ltot = 0;}
-LMotor.speed(Lspeed);
-RMotor.speed(Rspeed);
+                
+if (Instr == 2 | Instr == 3){
+    LMotor.speed(TLspeed);
+    RMotor.speed(TRspeed);}
+else{     
+    LMotor.speed(Lspeed);
+    RMotor.speed(Rspeed);}
 Lcount = 0; //Restart the counters
 Rcount = 0;