Revision 38:fe05f93009a2, committed 2017-05-27

Comitter:

sahilmgandhi

Date:

Sat May 27 21:29:55 2017 +0000

Parent:

37:3dcc95e9321c

Child:

39:058fb32c24e0

Commit message:

Playing around with constants

Changed in this revision

--- a/irpair.cpp	Sat May 27 03:37:24 2017 +0000
+++ b/irpair.cpp	Sat May 27 21:29:55 2017 +0000
@@ -7,7 +7,7 @@
 void IRPair::calibrateSensor() {
 
     ir.write( 1 );
-    wait_us(55);
+    wait_us(60);
 
     for (int i = 0; i < samplesToTake; ++i) 
         sensorAvg += recv.read();
@@ -26,7 +26,7 @@
     //    z1 += recv.read();
     
     ir.write( 1 );
-    wait_us(55);
+    wait_us(60);
     
     for( int i = 0; i < samples; ++i )
         z += recv.read();

--- a/main.cpp	Sat May 27 03:37:24 2017 +0000
+++ b/main.cpp	Sat May 27 21:29:55 2017 +0000
@@ -90,8 +90,10 @@
     double speed1 = WHEEL_SPEED;
     right_motor.move(speed0);
     left_motor.move(speed1);
+    receiverOneReading = IRP_1.getSamples(100);
+    receiverTwoReading = IRP_4.getSamples(100);
 
-    while(  ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num)) {
+    while(  ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num) && receiverOneReading < IRP_1.sensorAvg*4 && receiverFourReading < IRP_4.sensorAvg*4) {
         //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 ));
 
@@ -111,6 +113,65 @@
             left_motor.move( speed1-0.8*d );
             right_motor.move( speed0+d );
         }
+        prev = x;
+        receiverOneReading = IRP_1.getSamples(100);
+        receiverFourReading = IRP_4.getSamples(100);
+    }
+
+    //pidOnEncoders();
+    //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 = WHEEL_SPEED;
+    double speed1 = WHEEL_SPEED;
+    right_motor.move(speed0);
+    left_motor.move(speed1);
+
+    double ir1 = IRP_1.getSamples(50);
+    double ir4 = IRP_4.getSamples(50);
+
+    if(ir1 > IRP_1.sensorAvg*4 || ir4 > IRP_2.sensorAvg*4){
+        return;
+    }
+
+    //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) {
+    //while(     (ir1 + ir4)/2 < ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4   ){
+    while(  ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ) {
+        //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();
@@ -126,136 +187,6 @@
     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 = WHEEL_SPEED;
-    double speed1 = WHEEL_SPEED;
-    right_motor.move(speed0);
-    left_motor.move(speed1);
-
-    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;
-    }
-
-    //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) {
-    //while(     (ir1 + ir4)/2 < ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4   ){
-    while(  ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ) {
-        //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;
-        ir1 = IRP_1.getSamples(50);
-        ir4 = IRP_4.getSamples(50);
-    }
-
-    //pidOnEncoders();
-    //pidBrake();
-    right_motor.brake();
-    left_motor.brake();
-    return;
-}
-
-/*
-void moveForwardUntilWallIr()
-{
-    double currentError = 0;
-    double previousError = 0;
-    double derivError = 0;
-    double sumError = 0;
-
-    double HIGH_PWM_VOLTAGE = 0.1;
-
-    double rightSpeed = 0.25;
-    double leftSpeed = 0.23;
-
-    float ir2 = IRP_2.getSamples( SAMPLE_NUM );
-    float ir3 = IRP_3.getSamples( SAMPLE_NUM );
-
-    int count = encoder0.getPulses();
-    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)) {
-            //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
-            //moveForwardLeftWall();
-        } else {
-            // will move forward using encoders only
-            // if cell ahead doesn't have a wall on either one side or both sides
-
-            int pulseCount = (encoder0.getPulses()-count) % 5600;
-            if (pulseCount > 5400 && pulseCount < 5800) {
-                blueLed.write(0);
-            } else {
-                blueLed.write(1);
-            }
-            sumError += currentError;
-            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
-                rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
-                leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
-            } else { // r is faster than L. speed up l, slow down r
-                rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
-                leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
-            }
-
-            if (leftSpeed > 0.30) leftSpeed = 0.30;
-            if (leftSpeed < 0) leftSpeed = 0;
-            if (rightSpeed > 0.30) rightSpeed = 0.30;
-            if (rightSpeed < 0) rightSpeed = 0;
-
-            right_motor.forward(rightSpeed);
-            left_motor.forward(leftSpeed);
-
-            previousError = currentError;
-
-            ir2 = IRP_2.getSamples( SAMPLE_NUM );
-            ir3 = IRP_3.getSamples( SAMPLE_NUM );
-
-        }
-        left_motor.brake();
-        right_motor.brake();
-    }
-}
-*/
 void printDipFlag()
 {
     if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
@@ -375,7 +306,7 @@
         receiverOneReading = IRP_1.getSamples( SAMPLE_NUM );
         receiverFourReading = IRP_4.getSamples( SAMPLE_NUM );
         // serial.printf("IR2 = %f, IR2AVE = %f, IR3 = %f, IR3_AVE = %f\n", receiverTwoReading, IRP_2.sensorAvg, receiverThreeReading, IRP_3.sensorAvg);
-        if(  receiverOneReading > IRP_1.sensorAvg * 3.8 || receiverFourReading > IRP_4.sensorAvg * 3.8) {
+        if(  receiverOneReading > IRP_1.sensorAvg * 5.5 || receiverFourReading > IRP_4.sensorAvg * 5.5) {
             break;
         }
 
@@ -942,6 +873,22 @@
         wait_ms(300);
         turnRight();
         nCellEncoderAndIR(4);        
+        wait_ms(300);
+        turnLeft();
+        wait_ms(300);
+        nCellEncoderAndIR(1);
+        wait_ms(300);
+        turnLeft();
+        wait_ms(300);
+        nCellEncoderAndIR(3);
+        wait_ms(300);
+        turnRight();
+        wait_ms(300);
+        nCellEncoderAndIR(3);
+        wait_ms(300);
+        turnRight();
+        wait_ms(300);
+        nCellEncoderAndIR(3);
         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) ) ;

--- a/main.h	Sat May 27 03:37:24 2017 +0000
+++ b/main.h	Sat May 27 21:29:55 2017 +0000
@@ -172,12 +172,12 @@
 //#define II_CONSTANT 0.06
 //#define ID_CONSTANT 7.55
 
-const int desiredCount180 = 3400;   // change accordingly to the terrain
+const int desiredCount180 = 3380;   // change accordingly to the terrain
 const int desiredCountR = 1600;
-const int desiredCountL = 1590;
+const int desiredCountL = 1600;
  
 const int oneCellCount = 5400;
-const int oneCellCountMomentum = 4900;//4570 (.15) speed;//4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
+const int oneCellCountMomentum = 5070;//4570 (.15) speed;//4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
  
 double receiverOneReading = 0.0;
 double receiverTwoReading = 0.0;
@@ -186,9 +186,7 @@
 
 float ir1base = 0.0;
 float ir2base = 0.0;
-
 float ir3base = 0.0;
-
 float ir4base = 0.0;
 
 float averageDivUpper = 0.5;
@@ -198,7 +196,7 @@
     double speed0 = 0.11;
     double speed1 = -0.12;    // change back to 0.13 if turns stop working, testing something out!
  
-    double kp = 0.000080;
+    double kp = 0.000082;
  
     int counter = 0;
     int initial0 = encoder0.getPulses();