Diff: main.cpp

Revision:

32:69acb14778ea

Parent:

31:9b71b44e0867

Child:

33:68ce1f74ab5f

--- a/main.cpp	Wed May 24 21:47:21 2017 +0000
+++ b/main.cpp	Fri May 26 03:46:03 2017 +0000
@@ -136,8 +136,8 @@
     right_motor.move(speed0);
     left_motor.move(speed1);
 
-    float ir1 = IRP_1.getSamples(50);
-    float ir4 = IRP_4.getSamples(50);
+    double ir1 = IRP_1.getSamples(50);
+    double ir4 = IRP_4.getSamples(50);
 
     if((ir1 + ir4)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4) {
         return;
@@ -340,6 +340,7 @@
 
 void nCellEncoderAndIR(double cellCount)
 {
+    //serial.printf("I'm inside IR!");
     double currentError = 0;
     double previousError = 0;
     double derivError = 0;
@@ -353,8 +354,9 @@
 
     int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
     int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
+    serial.printf("%d, %d\n", desiredCount0, desiredCount1 );
 
-    left_motor.forward(0.16);
+    left_motor.forward(0.15);
     right_motor.forward(0.15);
 
     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
@@ -365,13 +367,16 @@
 
     int state = 0;
 
+    //serial.printf("Enter encoder while loop\n");
+    //serial.printf("%d, %d\n", encoder0.getPulses(), encoder1.getPulses() );
     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
-        receiverTwoReading = IRP_2.getSamples(100);
-        receiverThreeReading = IRP_3.getSamples(100);
-        receiverOneReading = IRP_1.getSamples(100);
-        receiverFourReading = IRP_4.getSamples(100);
+        //serial.printf("Entered encoder while loop\n");
+        receiverTwoReading = IRP_2.getSamples( SAMPLE_NUM );
+        receiverThreeReading = IRP_3.getSamples( SAMPLE_NUM );
+        receiverOneReading = IRP_1.getSamples( SAMPLE_NUM );
+        receiverFourReading = IRP_4.getSamples( SAMPLE_NUM );
 
-        if(  receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ) {
+        if(  receiverOneReading > IRP_1.sensorAvg * 3 || receiverFourReading > IRP_4.sensorAvg * 3 ) {
             break;
         }
 
@@ -402,6 +407,7 @@
             blueLed.write(1);
         }
 
+        //serial.printf("Entering switch\n");
         switch(state) {
             case(0): { // both walls there
                 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL)  - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
@@ -420,6 +426,7 @@
                 break;
             }
         }
+        //serial.printf("Exiting switch");
 
         sumError += currentError;
         derivError = currentError - previousError;
@@ -431,6 +438,8 @@
             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;
         if (rightSpeed > 0.30) rightSpeed = 0.30;
@@ -438,7 +447,7 @@
 
         right_motor.forward(rightSpeed);
         left_motor.forward(leftSpeed);
-        pidOnEncoders();
+        //pidOnEncoders();
 
         previousError = currentError;
     }
@@ -446,7 +455,7 @@
         if (currDir % 4 == 0) {
             mouseY += 1;
         } else if (currDir % 4 == 1) {
-            mouseX + 1;
+            mouseX += 1;
         } else if (currDir % 4 == 2) {
             mouseY -= 1;
         } else if (currDir % 4 == 3) {
@@ -494,6 +503,7 @@
         }
     }
 
+    //serial.printf("What?\n");
     left_motor.brake();
     right_motor.brake();
 }
@@ -522,7 +532,7 @@
         goingToCenter = false;
         changeManhattanDistance(goingToCenter);
     } else if (!goingToCenter && (currentDistance == 0)) {
-        goingToCenter == true;
+        goingToCenter = true;
         changeManhattanDistance(goingToCenter);
     }
 
@@ -861,17 +871,19 @@
     wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
     visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
 
-    int prevEncoder0Count = 0;
-    int prevEncoder1Count = 0;
-    int currEncoder0Count = 0;
-    int currEncoder1Count = 0;
+    //int prevEncoder0Count = 0;
+    //int prevEncoder1Count = 0;
+    //int currEncoder0Count = 0;
+    //int currEncoder1Count = 0;
 
-    bool overrideFloodFill = false;
+    //bool overrideFloodFill = false;
     //right_motor.forward( 0.2 );
     //left_motor.forward( 0.2 );
     //turnRight180();
     //wait_ms(1500);
-    int nextMovement = 0;
+    //int nextMovement = 0;
+    //serial.printf("I'm about to enter!");
+    nCellEncoderAndIR(3);
     while (1) {
         // prevEncoder0Count = encoder0.getPulses();
 //        prevEncoder1Count = encoder1.getPulses();
@@ -999,7 +1011,8 @@
 
         ////////////////////////////////////////////////////////////////
 
-        //nCellEncoderAndIR(3);
+        //nCellEncoderAndIR(4);
+        //while(1);
         //break;
 
         //serial.printf("IRS= >: %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100));
@@ -1018,8 +1031,8 @@
         //pidOnEncoders();
         // moveForwardUntilWallIr();
         //serial.printf("Pulse Count=> e0:%d, e1:%d      \r\n", encoder0.getPulses(),encoder1.getPulses());
-        double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
-        serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
+        //double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
+        //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
 
         //reading = Rec_4.read();
 //        serial.printf("reading: %f\n", reading);