Diff: main.cpp

Revision:

17:f713758f6238

Parent:

16:d9252437bd92

Child:

18:6a4db94011d3

--- a/main.cpp	Sun May 14 20:58:55 2017 +0000
+++ b/main.cpp	Sun May 14 23:07:23 2017 +0000
@@ -26,6 +26,11 @@
 const int oneCellCount = 5400;
 const int oneCellCountMomentum = 4600;      // one cell count is actually approximately 5400, but this value is considering momentum!
 
+float receiverOneReading = 0.0;
+float receiverTwoReading = 0.0;
+float receiverThreeReading = 0.0;
+float receiverFourReading = 0.0;
+
 void pidOnEncoders();
 
 void turnLeft()
@@ -72,6 +77,7 @@
 
     right_motor.brake();
     left_motor.brake();
+    turnFlag = 0;           // zeroing out the flags!
 }
 
 void turnRight()
@@ -118,6 +124,7 @@
 
     right_motor.brake();
     left_motor.brake();
+    turnFlag = 0;
 }
 
 void turnLeft180()
@@ -211,14 +218,20 @@
     left_motor.brake();
 }
 
-void moveForwardOneCellEncoder(){
-    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum;
-    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum;
+void moveForwardCellEncoder(double cellNum){
+    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
+    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
 
     left_motor.forward(0.12);
     right_motor.forward(0.10);
     wait_ms(2);
     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
+        receiverTwoReading = IRP_2.getSamples(20);
+        receiverThreeReading = IRP_3.getSamples(20);
+        if (receiverTwoReading <= IRP_2.sensorAvg/2.5)
+            turnFlag |= LEFT_FLAG;
+        else if (receiverThreeReading <= IRP_3.sensorAvg/2.5)
+            turnFlag |= RIGHT_FLAG;
         // serial.printf("Encoder0 count is: %d, Encoder1 count is: %d, desiredEncoder0 = %d, desiredEncoder1 = %d\n", encoder0.getPulses(), encoder1.getPulses(), desiredCount0, desiredCount1);
         pidOnEncoders();
     }
@@ -227,6 +240,21 @@
     right_motor.brake();
 }
 
+void handleTurns(){
+    if (turnFlag == 0x1){
+        moveForwardCellEncoder(0.5);
+        turnLeft();
+    }
+    else if (turnFlag == 0x2){
+        moveForwardCellEncoder(0.5);
+        turnRight();
+    }
+    else if (turnFlag == 0x3){
+        moveForwardCellEncoder(0.5);
+        turnLeft();
+    }
+}
+
 void moveForwardUntilWallIr()
 {
     double currentError = 0;
@@ -251,7 +279,6 @@
             blueLed.write(1);
         }
 
-
         currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
         derivError = currentError - previousError;
         double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
@@ -379,6 +406,10 @@
     }
 }
 
+void oneCellEncoderAndIR(){
+
+}
+
 int main()
 {
     //Set highest bandwidth.
@@ -439,21 +470,26 @@
     //left_motor.forward( 0.2 );
 
     while (1) {
-        moveForwardOneCellEncoder();
+        moveForwardCellEncoder(1);
+        wait(0.3);
+        handleTurns();
+        wait_ms(5);
+        moveForwardCellEncoder(1);
+        wait(0.3);
+        handleTurns();
+        break;
+        // moveForwardOneCellEncoder();
         //pidOnEncoders();
        //moveForwardUntilWallIr();
     //        wait(2);
     //        turnRight();
     //        wait(1);
     //        moveForwardUntilWallIr();
-
-        break;
         //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
         //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);