Diff: main.cpp

Revision:

36:9c4cc9944b69

Parent:

35:a5bd9ef82210

Child:

37:3dcc95e9321c

--- a/main.cpp	Sat May 27 00:41:19 2017 +0000
+++ b/main.cpp	Sat May 27 02:40:10 2017 +0000
@@ -190,6 +190,7 @@
     return;
 }
 
+/*
 void moveForwardUntilWallIr()
 {
     double currentError = 0;
@@ -254,7 +255,7 @@
         right_motor.brake();
     }
 }
-
+*/
 void printDipFlag()
 {
     if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
@@ -349,18 +350,18 @@
     double derivError = 0;
     double sumError = 0;
 
-    double HIGH_PWM_VOLTAGE_R = 0.15;
-    double HIGH_PWM_VOLTAGE_L = 0.16;
+    double HIGH_PWM_VOLTAGE_R = 0.11;
+    double HIGH_PWM_VOLTAGE_L = 0.11;
 
-    double rightSpeed = 0.15;
-    double leftSpeed = 0.16;
+    double rightSpeed = 0.11;
+    double leftSpeed = 0.11;
 
     int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
     int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
     serial.printf("%d, %d\n", desiredCount0, desiredCount1 );
 
-    left_motor.forward(0.15);
-    right_motor.forward(0.15);
+    left_motor.forward(0.11);
+    right_motor.forward(0.11);
 
     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
     float ir3 = IRP_3.getSamples( SAMPLE_NUM );
@@ -802,6 +803,18 @@
     }
 }
 
+void waitButton4()
+{
+    //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
+    int init_val = dipFlags & BUTTON4_FLAG;
+    while( (dipFlags & BUTTON4_FLAG) == init_val )
+    {
+        // do nothing until ready
+    }
+    //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
+    wait( 2 );
+}
+
 int main()
 {
     //Set highest bandwidth.
@@ -857,13 +870,7 @@
     dipButton3.fall(&disableButton3);
     dipButton4.fall(&disableButton4);
 
-    serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
-    while( (dipFlags & BUTTON4_FLAG) == 0 )
-    {
-        // do nothing until ready
-    }
-    serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
-    wait( 2 );
+    //waitButton4();
 
     // init the wall, and mouse loc arrays:
     wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
@@ -875,9 +882,9 @@
     //int currEncoder1Count = 0;
 
     //bool overrideFloodFill = false;
-    right_motor.forward( 0.2 );
-    left_motor.forward( 0.2 );
-    //turnRight180();
+    right_motor.forward( 0.11 );
+    left_motor.forward( 0.11 );
+    //turn180();
     //wait_ms(1500);
     //int nextMovement = 0;
     while (1) {
@@ -902,7 +909,7 @@
 //            }
 //            else if (nextMovement == 4){
 //                justTurned = true;
-//                turnRight180();
+//                turn180();
 //            }
 //        }
 //        else{
@@ -927,30 +934,22 @@
 
 
         //////////////////////// TESTING CODE GOES BELOW ///////////////////////////
-        nCellEncoderAndIR(4);
-        wait_ms(200);
-        turnRight();
-        wait_ms(200);
-        nCellEncoderAndIR(3);
-        wait_ms(200);
-        turnRight();
-        wait_ms(200);
-        nCellEncoderAndIR(2);
-        wait_ms(200);
-        turnRight();
-        wait_ms(200);
-        nCellEncoderAndIR(1);
-        wait_ms(200);
+        
+        turnLeft();
+        wait_ms(300);
+        turnLeft();
+        wait_ms(300);
+        turnLeft();
+        wait_ms(300);
         turnLeft();
-        wait_ms(200);
-        nCellEncoderAndIR(1);
-        wait_ms(200);
-        turnLeft();
-        wait_ms(200);
-        nCellEncoderAndIR(1);
-        wait_ms(200);
-        turnRight180();
-        break;
+        /*
+        wait_ms(300);
+        turn180();
+        wait_ms(300);
+        turn180();
+        wait_ms(300);*/
+        
+        waitButton4();
         // serial.printf("Encoder 0 is : %d \t Encoder 1 is %d\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 );