Diff: main.cpp

Revision:

46:b156ef445742

Parent:

45:8b0bee6baf38

--- a/main.cpp	Fri Jun 02 19:35:26 2017 +0000
+++ b/main.cpp	Sat Jun 03 00:22:44 2017 +0000
@@ -44,6 +44,14 @@
 
 void pidOnEncoders();
 
+void moveBackwards(){
+    right_motor.move(-WHEEL_SPEED);
+    left_motor.move(-WHEEL_SPEED);
+    wait_ms(40);
+    right_motor.brake();
+    left_motor.brake();
+}
+
 void moveForwardEncoder(double num)
 {
     int count0;
@@ -64,7 +72,7 @@
     receiverFourReading = IRP_4.getSamples(100);
     right_motor.move(speed0);
     left_motor.move(speed1);
-    wait_ms(100);
+    wait_ms(150);
 
     double distance_to_go = (oneCellCount)*num;
 
@@ -109,7 +117,7 @@
 
     right_motor.move(speed0);
     left_motor.move(speed1);
-    wait_us(100000);
+    wait_us(150000);
 
     double distance_to_go = (oneCellCount)*num;
 
@@ -320,7 +328,17 @@
 
     int state = 0;
 
+    int previousCount0 = 10000;
+    int previousCount1 = 10000;
+
     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
+        int currCount0 = encoder0.getPulses();
+        int currCount1 = encoder1.getPulses();
+        // serial.printf("currCount0 = %d, currCount1 =%d, previousCount0 = %d, previousCount1 = %d \n", currCount0, currCount1, previousCount0, previousCount1);
+        // if (abs(currCount0 - previousCount0) <= 2 || abs(currCount1 - previousCount1) <= 2){
+        //     moveBackwards();
+        //     return;
+        // }
         // serial.printf("The desiredCount0 is: %d \t The desiredCount1 is: %d\t the 0encoderval is :%d\t the 1encoderval is : %d\t\n", desiredCount0, desiredCount1, encoder0.getPulses(), encoder1.getPulses());
         receiverTwoReading = IRP_2.getSamples( SAMPLE_NUM );
         receiverThreeReading = IRP_3.getSamples( SAMPLE_NUM );
@@ -436,6 +454,8 @@
         pidOnEncoders();
 
         previousError = currentError;
+        previousCount1 = currCount1;
+        previousCount0 = currCount0;
     }
 
     // GO BACK A BIT BEFORE BRAKING??
@@ -898,73 +918,31 @@
     // moveForwardEncoder(1);
 
     while (1) {
-
-        if (!overrideFloodFill) {
-            nextMovement = chooseNextMovement();
-            if (nextMovement == 0) {
-                moveForwardEncoder(0.4);
-                nCellEncoderAndIR(0.6);
-            } else if (nextMovement == 1) {
-                justTurned = true;
-                turnRight();
-            } else if (nextMovement == 2) {
-                justTurned = true;
-                turnLeft();
-            } else if (nextMovement == 3) {
-                nCellEncoderAndIR(1);
-                //encoderAfterTurn(1);
-            } else if (nextMovement == 4) {
-                justTurned = true;
-                turn180();
-            }
-        } else {
-            receiverOneReading = IRP_1.getSamples(100);
-            receiverTwoReading = IRP_2.getSamples(100);
-            receiverThreeReading = IRP_3.getSamples(100);
-            receiverFourReading = IRP_4.getSamples(100);
-
-            if(receiverOneReading > IRP_1.sensorAvg * frontStop || receiverFourReading > IRP_4.sensorAvg * frontStop ) {
-
-                int randomNum = rand() %2;
-                if(randomNum == 0) {
-                    turnRight();
-                } else {
-                    turnLeft();
-                }
-            } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
-                // both walls there
-                nCellEncoderAndIR(1);
-                redLed.write(1);
-                greenLed.write(0);
-                blueLed.write(1);
-            } else if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
-                // both sides gone
-
-                redLed.write(1);
-                greenLed.write(0);
-                blueLed.write(0);
-                // serial.printf("Going to go over to move forward with encoder, and passing %f\n", valToPass);
-                moveForwardEncoder(1);
-                continue;
-            } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone   RED
-                turnRight();
-                redLed.write(0);
-                greenLed.write(1);
-                blueLed.write(1);
-                // moveForwardEncoder(valToPass);
-                // break;
-            } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone
-                turnLeft();
-                redLed.write(1);
-                greenLed.write(1);
-                blueLed.write(0);
-                // moveForwardEncoder(valToPass);
-                // break;
-            }
+        if (receiverOneReading < IRP_1.sensorAvg * frontStop && receiverFourReading < IRP_4.sensorAvg * frontStop ){
+            nCellEncoderAndIR(1);
+            continue;
         }
-
-        wait_ms(500);                          // reduce this once we fine tune this!
-
+        else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone   RED
+            turnRight();
+            wait_ms(100);
+            moveForwardEncoder(0.4);
+            nCellEncoderAndIR(0.6);
+            continue;
+        } 
+        else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone
+            turnLeft();
+            wait_ms(100);
+            moveForwardEncoder(0.4);
+            nCellEncoderAndIR(0.6);
+            continue;
+        }
+        else{
+            turn180();
+            wait_ms(100);
+            moveForwardEncoder(0.4);
+            nCellEncoderAndIR(0.6);
+            continue;
+        }
 
         //////////////////////// TESTING CODE GOES BELOW ///////////////////////////