Diff: main.cpp

Revision:

34:69342782fb68

Parent:

33:68ce1f74ab5f

Child:

35:a5bd9ef82210

--- a/main.cpp	Fri May 26 06:23:19 2017 +0000
+++ b/main.cpp	Fri May 26 17:21:04 2017 +0000
@@ -37,7 +37,7 @@
 0.07        0.06
 
 */
- 
+
 #define IP_CONSTANT 1.93
 #define II_CONSTANT 0.00001
 #define ID_CONSTANT 0.175
@@ -250,11 +250,6 @@
             ir3 = IRP_3.getSamples( SAMPLE_NUM );
 
         }
-
-        //backward();
-        //wait_ms(40);
-        //brake();
-
         left_motor.brake();
         right_motor.brake();
     }
@@ -443,7 +438,7 @@
             rightSpeed = HIGH_PWM_VOLTAGE_R + abs(PIDSum*HIGH_PWM_VOLTAGE_R);
             leftSpeed = HIGH_PWM_VOLTAGE_L - abs(PIDSum*HIGH_PWM_VOLTAGE_L);
         }
-        
+
         //serial.printf("%f, %f\n", leftSpeed, rightSpeed);
         if (leftSpeed > 0.30) leftSpeed = 0.30;
         if (leftSpeed < 0) leftSpeed = 0;
@@ -456,6 +451,15 @@
 
         previousError = currentError;
     }
+    
+    // GO BACK A BIT BEFORE BRAKING??
+    left_motor.backward(0.01);
+    right_motor.backward(0.01);
+    wait_us(100);
+    // DELETE THIS IF IT DOES NOT WORK!!
+    
+    left_motor.brake();
+    right_motor.brake();
     if (encoder0.getPulses() >= 0.6*desiredCount0 && encoder1.getPulses() >= 0.6*desiredCount1) {
         if (currDir % 4 == 0) {
             mouseY += 1;
@@ -507,22 +511,22 @@
             }
         }
     }
-
-    //serial.printf("What?\n");
-    left_motor.brake();
-    right_motor.brake();
 }
 
-bool isWallInFront(int x, int y){
+bool isWallInFront(int x, int y)
+{
     return (wallArray[MAZE_LEN - y - 1][x]  & F_WALL);
 }
-bool isWallInBack(int x, int y){
+bool isWallInBack(int x, int y)
+{
     return (wallArray[MAZE_LEN - y - 1][x]  & B_WALL);
 }
-bool isWallOnRight(int x, int y){
+bool isWallOnRight(int x, int y)
+{
     return (wallArray[MAZE_LEN - y - 1][x]  & R_WALL);
 }
-bool isWallOnLeft(int x, int y){
+bool isWallOnLeft(int x, int y)
+{
     return (wallArray[MAZE_LEN - y - 1][x]  & L_WALL);
 }
 
@@ -742,7 +746,8 @@
     return dirToGo;
 }
 
-bool hasVisited(int x, int y){
+bool hasVisited(int x, int y)
+{
     return visitedCells[MAZE_LEN - 1 - y][x];
 }
 
@@ -913,7 +918,7 @@
 //        wait_ms(300);                          // reduce this once we fine tune this!
 
 
-       //////////////////////// TESTING CODE GOES BELOW ///////////////////////////
+        //////////////////////// TESTING CODE GOES BELOW ///////////////////////////
         nCellEncoderAndIR(4);
         wait_ms(200);
         turnRight();