Diff: main.cpp

Revision:

4:295160ab06c7

Parent:

3:c9e09e968552

Child:

5:d73ca4500060

--- a/main.cpp	Sat Dec 10 20:20:07 2016 +0000
+++ b/main.cpp	Sat Dec 10 21:06:01 2016 +0000
@@ -51,13 +51,13 @@
     led = !led;
     if (c == '0') { //Stop
         Instr = 0;   
-    } else if (c == '4') { //reverse
+    } else if (c == '1') { //reverse
         Instr = 1;
     } else if (c == '2') { //Left
         Instr = 2;
     } else if (c == '3') { //Right
         Instr = 3;
-    } else if (c == '1') { //forward
+    } else if (c == '4') { //forward
         Instr = 4;
     } else if (c == 'A') { //Rotate + take pic
         Instr = 5;
@@ -81,10 +81,12 @@
             Rspeed = 0;
             Lspeed = 0;
         } else if (Instr == 1) { //move backwards (forward, we turned our bot around)
+            Rspeed = .4;
+            Lspeed = .4;
             TError = Ltot - Rtot;
-            TError = (float) TError +.05;
-           Rspeed = abs(Rspeed) + (float)TError /100;
-           Lspeed = abs(Lspeed);
+            TError = TError +.05;
+           Rspeed = fabs(Rspeed) + (float)TError /100;
+           
            if(Rspeed < 0)
            RMotor.speed(0);
            if(Lspeed < 0)
@@ -92,30 +94,32 @@
            
         } else if (Instr == 2) {        //turn Left 
              TError = Ltot - Rtot;
-            if(TError > 0) TRspeed = abs(TRspeed)+.01;
+            if(TError > 0) TRspeed = fabs(TRspeed)+.01;
             else if (TError == 0) TRspeed = TRspeed;
-            else TRspeed = abs(TRspeed) - .01;
+            else TRspeed = fabs(TRspeed) - .01;
            // Rspeed = Rspeed + .0008;
             LMotor.speed(0);
             RMotor.speed(0);
-            TLspeed = -abs(TLspeed); //Lwheel must move in reverse for left turn
-            TRspeed = abs(TRspeed); //Rwheel must move forward for left turn
+            TLspeed = -fabs(TLspeed); //Lwheel must move in reverse for left turn
+            TRspeed = fabs(TRspeed); //Rwheel must move forward for left turn
             
         } else if (Instr == 3) {        // turn right
                TError = Ltot - Rtot;
-            if(TError > 0) TRspeed = abs(TRspeed)+.01;
+            if(TError > 0) TRspeed = fabs(TRspeed)+.01;
             else if (TError == 0) TRspeed = TRspeed;
-            else TRspeed = abs(TRspeed) - .01;
+            else TRspeed = fabs(TRspeed) - .01;
            // Rspeed = Rspeed + .0008;
                 LMotor.speed(0);
                 RMotor.speed(0);
-            TRspeed = -abs(TRspeed); 
-            TLspeed = abs(TLspeed);
+            TRspeed = -fabs(TRspeed); 
+            TLspeed = fabs(TLspeed);
         } else if (Instr == 4) {        // go forward (reverse, we turned our bot around)
+                Rspeed = .4;
+                Lspeed = .4;                
                 TError = Ltot - Rtot; 
-                Rspeed = abs(Rspeed) + (float) TError / 100;
-                Rspeed = -abs(Rspeed);
-                Lspeed = -abs(Lspeed);
+                Rspeed = fabs(Rspeed) + (float) TError / 100;
+                Rspeed = -fabs(Rspeed);
+                Lspeed = -fabs(Lspeed);
                 LMotor.speed(0);
                 RMotor.speed(0);
             }
@@ -150,6 +154,7 @@
 else{     
     LMotor.speed(Lspeed);
     RMotor.speed(Rspeed);}
+
 Lcount = 0; //Restart the counters
 Rcount = 0;