Diff: main.cpp

Revision:

16:d9252437bd92

Parent:

15:b80555a4a8b9

Child:

17:f713758f6238

--- a/main.cpp	Sun May 14 04:45:21 2017 +0000
+++ b/main.cpp	Sun May 14 20:58:55 2017 +0000
@@ -23,7 +23,10 @@
 const int desiredCountR = 1400;
 const int desiredCountL = 1475;
 
-const int oneCellCount = 5300;
+const int oneCellCount = 5400;
+const int oneCellCountMomentum = 4600;      // one cell count is actually approximately 5400, but this value is considering momentum!
+
+void pidOnEncoders();
 
 void turnLeft()
 {
@@ -208,54 +211,20 @@
     left_motor.brake();
 }
 
-void moveForwardOneCellEncoder(){   // 0 is left wheel!
-    double speed0 = 0.15;
-    double speed1 = 0.15;
-
-    int counter = 0;
-    int initial0 = encoder0.getPulses(); // left
-    int initial1 = encoder1.getPulses(); // right
-
-    double kpLeft = 0.0000005;       
-    double kpRight = 0.0000005;
-
-    int desiredCount0 = initial0 + oneCellCount;
-    int desiredCount1 = initial1 + oneCellCount;
-
-    int count0 = initial0;
-    int count1 = initial1;
-    
-    int error = 0;  
-    
-    while (1){
-        if (count0 < desiredCount0 && count1 < desiredCount1){
-            count0 = encoder0.getPulses();
-            count1 = encoder1.getPulses();
+void moveForwardOneCellEncoder(){
+    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum;
+    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum;
 
-            error = count0 - count1;       // if error is positive, then left has moved more, so we want to decrease its speed and increase the right speed
+    left_motor.forward(0.12);
+    right_motor.forward(0.10);
+    wait_ms(2);
+    while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
+        // serial.printf("Encoder0 count is: %d, Encoder1 count is: %d, desiredEncoder0 = %d, desiredEncoder1 = %d\n", encoder0.getPulses(), encoder1.getPulses(), desiredCount0, desiredCount1);
+        pidOnEncoders();
+    }
 
-            if (error > 0){
-                speed0 = 0.15 - error*kpLeft;
-                speed1 = 0.15 + error*kpRight;
-            }
-            else if (error <= 0){
-                speed0 = 0.15 + error*kpLeft;
-                speed1 = 0.15 - error*kpRight;
-            }
-            // serial.printf("The error is %d \n", error);
-            // serial.printf("The Left speed is: %f and the right speed is: %f \n", speed0, speed1);
-            right_motor.move(speed0);
-            left_motor.move(speed1);
-            counter = 0;
-        } 
-        else {
-            break;
-        }
-    }
-    greenLed.write(1);
-    blueLed.write(0);
+    left_motor.brake();
     right_motor.brake();
-    left_motor.brake();
 }
 
 void moveForwardUntilWallIr()
@@ -373,8 +342,10 @@
     count1 = encoder1.getPulses();
     int diff = count0 - count1;
     double kp = 0.000075;
-    double kd = 0.000125;
+    double kd = 0.000135;
     int prev = 0;
+
+    int counter = 0;
     while(1)
     {
         count0 = encoder0.getPulses();
@@ -396,12 +367,15 @@
             left_motor.move( -d );
             right_motor.move( d );
         }
-        else
-        {
-            left_motor.brake();
-            right_motor.brake();   
-        }
+        // else
+        // {
+        //     left_motor.brake();
+        //     right_motor.brake();   
+        // }
         prev = x;
+        counter++;
+        if (counter == 4)
+            break;
     }
 }
 
@@ -465,13 +439,12 @@
     //left_motor.forward( 0.2 );
 
     while (1) {
-       //moveForwardOneCellEncoder(); 
-        pidOnEncoders();
+        moveForwardOneCellEncoder();
+        //pidOnEncoders();
        //moveForwardUntilWallIr();
     //        wait(2);
     //        turnRight();
     //        wait(1);
-    //        moveForwardOneCellEncoder();
     //        moveForwardUntilWallIr();
 
         break;