Revision 23:690b0ca34ee9, committed 2017-05-20

Comitter:

christine222

Date:

Sat May 20 09:11:08 2017 +0000

Parent:

22:681190ff98f0

Child:

24:e7063765d6f0

Commit message:

ncellencoderirwall function working for all walls (both, 1 side, no sides) need to tune detecting the sides though, there's a very precise threshold that we need to find

Changed in this revision

--- a/main.cpp	Fri May 19 01:15:36 2017 +0000
+++ b/main.cpp	Sat May 20 09:11:08 2017 +0000
@@ -19,12 +19,13 @@
 // #define IP_CONSTANT 8.85
 // #define II_CONSTANT 0.005
 // #define ID_CONSTANT 3.15
-#define IP_CONSTANT 6.85
+#define IP_CONSTANT 13.5
 #define II_CONSTANT 0.095
-#define ID_CONSTANT 15.85
+#define ID_CONSTANT 8.85
 
-const int desiredCountR = 1400;
-const int desiredCountL = 1475;
+const int desiredCount180 = 2900;
+const int desiredCountR = 1500;
+const int desiredCountL = 1575;
 
 const int oneCellCount = 5400;
 const int oneCellCountMomentum = 4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
@@ -34,16 +35,50 @@
 float receiverThreeReading = 0.0;
 float receiverFourReading = 0.0;
 
-float averageDiv = 170;
-float initAverage = 4;
+//IRSAvg= >: 0.143701, 0.128285
+
+
+
+
+
+
+
+        //facing wall ir2 and ir3
+        //0.144562, 0.113971 in maze
+
+        // normal hall ir2 and ir3
+        //0.013665, 0.010889  in maze
+
+        //0.014462, 0.009138 
+        // 0.013888, 0.010530 
+
+
+        //no walls ir2 and ir3
+        //0.003265, 0.002904  in maze9
+
+        //0.003162, 0.003123 
+        //0.003795, 
+
+//0.005297, 0.007064 
+
+
+
+
+float averageDivL = 23.0; //blue
+float initAverageL = 10.35;
+
+float averageDivR = 24; //red
+float initAverageR = 12.82; //4.5
+
+float averageDivUpper = 0.9;
 
 void pidOnEncoders();
 
 
 void turnLeft()
 {
-    double speed0 = 0.15;
-    double speed1 = -0.15;
+    double speed0 = 0.11;
+    double speed1 = -0.13;
 
     int counter = 0;
     int initial0 = encoder0.getPulses();
@@ -90,8 +125,8 @@
 
 void turnRight()
 {
-    double speed0 = -0.15;
-    double speed1 = 0.15;
+    double speed0 = -0.11;
+    double speed1 = 0.13;
 
     int counter = 0;
     int initial0 = encoder0.getPulses();
@@ -184,15 +219,15 @@
 
 void turnRight180()
 {
-    double speed0 = -0.15;
-    double speed1 = 0.15;
+    double speed0 = -0.16;
+    double speed1 = 0.16;
 
     int counter = 0;
     int initial0 = encoder0.getPulses();
     int initial1 = encoder1.getPulses();
 
-    int desiredCount0 = initial0 + desiredCountR*2;
-    int desiredCount1 = initial1 - desiredCountR*2;
+    int desiredCount0 = initial0 + desiredCount180;
+    int desiredCount1 = initial1 - desiredCount180;
 
     int count0 = initial0;
     int count1 = initial1;
@@ -239,12 +274,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/averageDiv){
+        if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){
             // redLed.write(1);
             // blueLed.write(0);
             turnFlag |= RIGHT_FLAG;
         }
-        else if (receiverTwoReading < IRP_2.sensorAvg/averageDiv){
+        else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){
             // redLed.write(0);
             // blueLed.write(1);
             turnFlag |= LEFT_FLAG;
@@ -334,14 +369,14 @@
 
 
 
-    double speed0 = 0.1;
-    double speed1 = 0.13;
+    double speed0 = 0.10;
+    double speed1 = 0.12;
     right_motor.move(speed0);
     left_motor.move(speed1);
 
 
-    while((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005) && ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.25) ) {
-
+    while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) {
+    //while(     (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4   ){
         //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
   
         count0 = encoder0.getPulses() - initial0;
@@ -372,9 +407,69 @@
     }
 
     //pidOnEncoders();
-    pidBrake();
-    //right_motor.brake();
-    //left_motor.brake();
+    //pidBrake();
+    right_motor.brake();
+    left_motor.brake();
+    return;
+}
+
+
+void moveForwardWallEncoder(){
+
+    int count0;
+    int count1;
+    count0 = encoder0.getPulses();
+    count1 = encoder1.getPulses();
+    int initial1 = count1;
+    int initial0 = count0;
+    int diff = count0 - count1;
+    double kp = 0.00015;
+    double kd = 0.00019;
+    int prev = 0;
+
+
+
+    double speed0 = 0.10;
+    double speed1 = 0.12;
+    right_motor.move(speed0);
+    left_motor.move(speed1);
+
+
+    //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) {
+    while(     (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4   ){
+        //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
+  
+        count0 = encoder0.getPulses() - initial0;
+        count1 = encoder1.getPulses() - initial1;
+        int x = count0 - count1;
+        //double d = kp * x + kd * ( x - prev );
+        double kppart = kp * x;
+        double kdpart = kd * (x-prev);
+        double d = kppart + kdpart;
+        
+        //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
+        if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward
+        {
+            left_motor.move( speed1-0.8*d );
+            right_motor.move( speed0+d );
+        }
+        else if( x > diff + 40 )
+        {
+            left_motor.move( speed1-0.8*d );
+            right_motor.move( speed0+d );
+        }
+        // else
+        // {
+        //     left_motor.brake();
+        //     right_motor.brake();   
+        // }
+        prev = x; 
+    }
+
+    //pidOnEncoders();
+    //pidBrake();
+    right_motor.brake();
+    left_motor.brake();
     return;
 }
 
@@ -397,7 +492,7 @@
     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)) {
-            //moveForwardEncoder(); 
+            //moveForwardWallEncoder(); 
         }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
@@ -413,7 +508,7 @@
                 blueLed.write(1);
             }
             sumError += currentError;
-            currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg/initAverage) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg/initAverage) ) ;
+            currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg/initAverageL) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg/initAverageR) ) ;
             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
@@ -552,8 +647,8 @@
     int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
     int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
 
-    left_motor.forward(0.125);
-    right_motor.forward(0.10);
+    left_motor.forward(0.17);
+    right_motor.forward(0.15);
 
     float receiverTwoReading = 0.0;
     float receiverThreeReading = 0.0;
@@ -561,26 +656,72 @@
     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
     float ir3 = IRP_3.getSamples( SAMPLE_NUM );
 
-    float prevRec2 = 0.0;
-    float prevRec3 = 0.0;
-    
+    int state = 0;
+
+
 
 
 
     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
         receiverTwoReading = IRP_2.getSamples(100);
         receiverThreeReading = IRP_3.getSamples(100);
-        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 = ( receiverTwoReading - IRP_2.sensorAvg/initAverage)  - ( receiverThreeReading - IRP_3.sensorAvg/initAverage);
-            prevRec2 = receiverTwoReading - IRP_2.sensorAvg/initAverage;
-            prevRec3 = receiverThreeReading - IRP_3.sensorAvg/initAverage;
+        
+        if ((IRP_1.getSamples(100) > IRP_1.sensorAvg*0.1) || (IRP_4.getSamples(100) > IRP_4.sensorAvg*0.1) ){
+            // almost to the end
+            redLed.write(1);
+            greenLed.write(1);
+            blueLed.write(1);
+            moveForwardWallEncoder();
+            break;
+
+        }else if(    (receiverThreeReading < IRP_3.sensorAvg/(averageDivR*0.8)) && (receiverTwoReading < IRP_2.sensorAvg/(averageDivL*0.8))   ){
+            // both sides gone
+            redLed.write(1);
+            greenLed.write(1);
+            blueLed.write(1);
+            moveForwardEncoder();
+        }else if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){// right wall gone
+            // RED RED RED RED RED
+            state = 1;
+            redLed.write(0);
+            greenLed.write(1);
+            blueLed.write(1);
+        }else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){// left wall gone
+            // BLUE BLUE BLUE BLUE
+            state = 2;
+            redLed.write(1);
+            greenLed.write(1);
+            blueLed.write(0);
+        }else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper))    &&     (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){
+            // both walls there
+            state = 0;
+            redLed.write(1);
+            greenLed.write(0);
+            blueLed.write(1);
         }
+
+        switch(state){
+            case(0):{ // both walls there
+                currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL)  - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
+                break;
+            }
+            case(1):{// RED RED RED RED RED
+                currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (0.6*IRP_2.sensorAvg/initAverageL);
+                break;   
+            }
+            case(2):{// blue
+                currentError = (0.8*IRP_3.sensorAvg/initAverageL) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR);
+                break;
+            }
+            default:{
+                currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL)  - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
+                //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL)  - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
+                break;
+            }
+        }
+
+
+
         
         sumError += currentError;
         derivError = currentError - previousError;
@@ -604,20 +745,22 @@
         previousError = currentError;
         ir2 = IRP_2.getSamples( SAMPLE_NUM );
         ir3 = IRP_3.getSamples( SAMPLE_NUM );
+
     }
 
 
 
     left_motor.brake();
     right_motor.brake();
+    return;
 }
 
 int main()
 {
     //Set highest bandwidth.
-    gyro.setLpBandwidth(LPFBW_42HZ);
+    //gyro.setLpBandwidth(LPFBW_42HZ);
     serial.baud(9600);
-    serial.printf("The gyro's address is %s", gyro.getWhoAmI());
+    //serial.printf("The gyro's address is %s", gyro.getWhoAmI());
 
     wait (0.1);
 
@@ -671,19 +814,98 @@
     //right_motor.forward( 0.2 );
     //left_motor.forward( 0.2 );
     turnRight180();
-    wait_ms(60);
+    wait_ms(1500);
     while (1) {
-        
+        //wait_ms(1500);
+        //turnRight();
+        //wait_ms(1500);
+        //turnLeft();
+        nCellEncoderAndIR(4);
+        wait_ms(2000);
+        // turnRight();
+        // wait_ms(500);
+        // nCellEncoderAndIR(1);
+        // wait_ms(500);
+        // turnRight();
+        // wait_ms(500);
+        // nCellEncoderAndIR(1);
+        // wait_ms(500);
+        // turnLeft();
+        // wait_ms(500);
+        // nCellEncoderAndIR(2);
+        // wait_ms(500);
+        // turnRight();
+        // wait_ms(500);
+        // nCellEncoderAndIR(1);
+        // wait_ms(500);
+        // turnRight();
+        // wait_ms(500);
+        // nCellEncoderAndIR(5);
+        // break;
+        // turnRight180();
+
+
+
+        // int number = rand() % 4 + 1;
+        // switch(number){
+        //     case(1):{//turn right
+        //         turnRight();
+        //         break;
+        //     }
+        //     case(2):{ // turn left
+        //         turnLeft();
+        //         break;
+        //     }
+        //     case(3):{// keep going
+
+        //         break;
+        //     }
+        //     case(4):{// turnaround
+        //         turnRight180();
+        //         break;
+        //     }
+        //     default:{// keep going
+        //         break;
+        //     }
+        // }
+
+        // float irbase2 = IRP_2.sensorAvg/initAverageL/averageDivL;
+        // float irbase3 = IRP_3.sensorAvg/initAverageR/averageDivR;
+
+        // float ir3  = IRP_2.getSamples(100)/initAverageL;
+        // float ir2  = IRP_3.getSamples(100)/initAverageR;
         
-        nCellEncoderAndIR(3);
-        break;
+
+
+
+        /*
+        counter2++;
+        counter3++; 
+        ir2tot += IRP_2.getSamples(100);
+        ir3tot += IRP_3.getSamples(100);
+
 
-        // 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));
+        ir2 = ir2tot/counter2;
+        ir3 = ir3tot/counter3;
+    
+
+        serial.printf("IRS= >: %f, %f \r\n", ir2, ir3);
+        */
+        //serial.printf("%f, %f \n", IRP_2.sensorAvg/initAverageL/averageDivL, IRP_3.sensorAvg/initAverageR/averageDivR);
+        //serial.printf("IRBASEnowall= >: %f, %f \r\n", irbase2, irbase3);
+        //break;
+        //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100));
+        //serial.printf("IRSAvg= >: %f, %f \r\n", ir2, ir3);
+        //serial.printf("IRSAvg= >: %f, %f \r\n", IRP_2.sensorAvg, IRP_3.sensorAvg);
+
+
+        ////////////////////////////////////////////////////////////////
+
         //nCellEncoderAndIR(3);
         //break;
-        //moveForwardEncoder();
-        //serial.printf("ded \n");
-        //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));
+    
 
 
         //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));