Diff: main.cpp

Revision:

44:85bf2c0cd518

Parent:

43:f22168a05c3e

Child:

45:8b0bee6baf38

--- a/main.cpp	Sun May 28 10:09:56 2017 +0000
+++ b/main.cpp	Fri Jun 02 18:50:51 2017 +0000
@@ -38,9 +38,9 @@
 
 */
 
-#define IP_CONSTANT 2.00
+#define IP_CONSTANT 1.6
 #define II_CONSTANT 0.00001
-#define ID_CONSTANT 0.2
+#define ID_CONSTANT 0.24
 
 void pidOnEncoders();
 
@@ -64,7 +64,7 @@
     receiverFourReading = IRP_4.getSamples(100);
     right_motor.move(speed0);
     left_motor.move(speed1);
-    wait_us(60000);
+    wait_ms(100);
 
     double distance_to_go = (oneCellCount)*num;
 
@@ -143,7 +143,7 @@
 
     right_motor.move(speed0);
     left_motor.move(speed1);
-    wait_us(60000);
+    wait_us(100000);
 
     double distance_to_go = (oneCellCount)*num;
 
@@ -515,7 +515,7 @@
     
     left_motor.brake();
     right_motor.brake();
-    if (encoder0.getPulses() >= 0.5*desiredCount0 && encoder1.getPulses() >= 0.5*desiredCount1) {
+    if (encoder0.getPulses() >= 0.4*desiredCount0 && encoder1.getPulses() >= 0.4*desiredCount1) {
         if (currDir % 4 == 0) {
             mouseY += 1;
         } else if (currDir % 4 == 1) {
@@ -972,6 +972,14 @@
        //  prevEncoder0Count = encoder0.getPulses();
        // prevEncoder1Count = encoder1.getPulses();
 
+        // turnRight();
+        // wait_ms(2000);
+        // turnLeft();
+        // wait_ms(2000);
+        // turnRight();
+        // wait_ms(2000);
+        // turnRight();
+        // wait_ms(2000);
         
        if (!overrideFloodFill){
            nextMovement = chooseNextMovement();
@@ -981,8 +989,9 @@
                 //redLed.write(1);
                 //greenLed.write(0);
                 //blueLed.write(1);
-               encoderAfterTurn(1);
-               // nCellEncoderAndIR(1);
+               // encoderAfterTurn(1);
+               moveForwardEncoder(0.4);
+               nCellEncoderAndIR(0.6);
            }
            else if (nextMovement == 1){
                justTurned = true;
@@ -1000,10 +1009,54 @@
                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;
+        } 
        }
 
-       wait_ms(700);                          // reduce this once we fine tune this!
-        
+       wait_ms(500);                          // reduce this once we fine tune this!
+
 
         //////////////////////// TESTING CODE GOES BELOW ///////////////////////////