Diff: main.cpp

Revision:

22:681190ff98f0

Parent:

21:9a6cb07bdcb6

Child:

23:690b0ca34ee9

--- a/main.cpp	Thu May 18 02:52:22 2017 +0000
+++ b/main.cpp	Fri May 19 01:15:36 2017 +0000
@@ -19,21 +19,24 @@
 // #define IP_CONSTANT 8.85
 // #define II_CONSTANT 0.005
 // #define ID_CONSTANT 3.15
-#define IP_CONSTANT 7.85
+#define IP_CONSTANT 6.85
 #define II_CONSTANT 0.095
-#define ID_CONSTANT 20.85
+#define ID_CONSTANT 15.85
 
 const int desiredCountR = 1400;
 const int desiredCountL = 1475;
 
 const int oneCellCount = 5400;
-const int oneCellCountMomentum = 4700;      // one cell count is actually approximately 5400, but this value is considering momentum!
+const int oneCellCountMomentum = 4800;      // 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;
 
+float averageDiv = 170;
+float initAverage = 4;
+
 void pidOnEncoders();
 
 
@@ -236,12 +239,12 @@
         receiverTwoReading = IRP_2.getSamples(100);
         receiverThreeReading = IRP_3.getSamples(100);
         // serial.printf("Average 2: %f Average 3: %f Sensor 2: %f Sensor 3: %f\n", IRP_2.sensorAvg, IRP_3.sensorAvg, receiverTwoReading, receiverThreeReading);
-        if (receiverThreeReading < IRP_3.sensorAvg/4){
+        if (receiverThreeReading < IRP_3.sensorAvg/averageDiv){
             // redLed.write(1);
             // blueLed.write(0);
             turnFlag |= RIGHT_FLAG;
         }
-        else if (receiverTwoReading < IRP_2.sensorAvg/4){
+        else if (receiverTwoReading < IRP_2.sensorAvg/averageDiv){
             // redLed.write(0);
             // blueLed.write(1);
             turnFlag |= LEFT_FLAG;
@@ -394,8 +397,11 @@
     while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { // while the front facing IR's arent covered
         
         if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) {
-            serial.printf("asdfasdf \n");
-            moveForwardEncoder(); 
+            //moveForwardEncoder(); 
+        }else if(IRP_2.getSamples(SAMPLE_NUM) < 0.005){ // left wall gone
+            //moveForwardRightWall();
+        }else if(IRP_3.getSamples(SAMPLE_NUM) < 0.005){ // right wall gone
+            //moveForwardLeftWall();
         }else{
         // will move forward using encoders only 
         // if cell ahead doesn't have a wall on either one side or both sides
@@ -407,7 +413,7 @@
                 blueLed.write(1);
             }
             sumError += currentError;
-            currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
+            currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg/initAverage) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg/initAverage) ) ;
             derivError = currentError - previousError;
             double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
             if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
@@ -434,7 +440,7 @@
         }
 
         //backward();
-        wait_ms(40);
+        //wait_ms(40);
         //brake();
 
         left_motor.brake();
@@ -533,7 +539,7 @@
     }
 }
 
-void oneCellEncoderAndIR(double cellCount){
+void nCellEncoderAndIR(double cellCount){
     double currentError = 0;
     double previousError = 0;
     double derivError = 0;
@@ -554,18 +560,27 @@
 
     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
     float ir3 = IRP_3.getSamples( SAMPLE_NUM );
-        
+
+    float prevRec2 = 0.0;
+    float prevRec3 = 0.0;
+    
+
+
+
     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
         receiverTwoReading = IRP_2.getSamples(100);
         receiverThreeReading = IRP_3.getSamples(100);
-        if (receiverTwoReading <= IRP_2.sensorAvg/4)
-            currentError = receiverThreeReading - IRP_3.sensorAvg;
-        else if (receiverThreeReading <= IRP_3.sensorAvg/4)
-            currentError = receiverTwoReading - IRP_2.sensorAvg;
+        if (receiverTwoReading <= IRP_2.sensorAvg/averageDiv)
+            currentError = prevRec2 -(receiverThreeReading - IRP_3.sensorAvg/initAverage);
+        else if (receiverThreeReading <= IRP_3.sensorAvg/averageDiv)
+            currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverage) - prevRec3;
         // else if (receiverTwoReading <= IRP_2.sensorAvg/2 && receiverThreeReading <= IRP_3.sensorAvg/2)       // scenario when both left and right wall missing, use encoders only
         //     moveForwardCellEncoder(2);
-        else
-            currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg));
+        else{
+            currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverage)  - ( receiverThreeReading - IRP_3.sensorAvg/initAverage);
+            prevRec2 = receiverTwoReading - IRP_2.sensorAvg/initAverage;
+            prevRec3 = receiverThreeReading - IRP_3.sensorAvg/initAverage;
+        }
         
         sumError += currentError;
         derivError = currentError - previousError;
@@ -655,13 +670,17 @@
 
     //right_motor.forward( 0.2 );
     //left_motor.forward( 0.2 );
-
+    turnRight180();
+    wait_ms(60);
     while (1) {
         
-        //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
+        
+        nCellEncoderAndIR(3);
+        break;
 
-        // oneCellEncoderAndIR(5);
-        moveForwardUntilWallIr();
+        // 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));
+        //nCellEncoderAndIR(3);
+        //break;
         //moveForwardEncoder();
         //serial.printf("ded \n");
         //break;