Diff: main.cpp

Revision:

20:82836745332e

Parent:

19:7b66a518b6f8

Child:

21:9a6cb07bdcb6

--- a/main.cpp	Mon May 15 00:54:41 2017 +0000
+++ b/main.cpp	Wed May 17 02:01:34 2017 +0000
@@ -16,15 +16,15 @@
 */
 
 // Constants for when HIGH_PWM_VOLTAGE = 0.1
-#define IP_CONSTANT 8.9
-#define II_CONSTANT 1
-#define ID_CONSTANT 3.1
+#define IP_CONSTANT 8.85
+#define II_CONSTANT 0.005
+#define ID_CONSTANT 3.15
 
 const int desiredCountR = 1400;
 const int desiredCountL = 1475;
 
 const int oneCellCount = 5400;
-const int oneCellCountMomentum = 4600;      // one cell count is actually approximately 5400, but this value is considering momentum!
+const int oneCellCountMomentum = 4700;      // one cell count is actually approximately 5400, but this value is considering momentum!
 
 float receiverOneReading = 0.0;
 float receiverTwoReading = 0.0;
@@ -78,6 +78,7 @@
     right_motor.brake();
     left_motor.brake();
     turnFlag = 0;           // zeroing out the flags!
+    currDir -= 1;
 }
 
 void turnRight()
@@ -98,7 +99,6 @@
     double error0 = count0 - desiredCount0;
     double error1 = count1 - desiredCount1;
 
-
     while(1) {
 
         if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
@@ -125,6 +125,7 @@
     right_motor.brake();
     left_motor.brake();
     turnFlag = 0;
+    currDir += 1;
 }
 
 void turnLeft180()
@@ -171,6 +172,7 @@
 
     right_motor.brake();
     left_motor.brake();
+    currDir -= 2;
 }
 
 void turnRight180()
@@ -216,23 +218,30 @@
     }
     right_motor.brake();
     left_motor.brake();
+    currDir += 2;
 }
 
 void moveForwardCellEncoder(double cellNum){
     int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
     int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
 
-    left_motor.forward(0.11);
-    right_motor.forward(0.10);
+    left_motor.forward(0.125);
+    right_motor.forward(0.095);
     wait_ms(1);
     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
-        receiverTwoReading = IRP_2.getSamples(5);
-        receiverThreeReading = IRP_3.getSamples(5);
-        if (receiverThreeReading < 0.0007f)
+        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){
+            // redLed.write(1);
+            // blueLed.write(0);
             turnFlag |= RIGHT_FLAG;
-        else if (receiverTwoReading < 0.0009f)
+        }
+        else if (receiverTwoReading < IRP_2.sensorAvg/4){
+            // redLed.write(0);
+            // blueLed.write(1);
             turnFlag |= LEFT_FLAG;
-        // serial.printf("Encoder0 count is: %d, Encoder1 count is: %d, desiredEncoder0 = %d, desiredEncoder1 = %d\n", encoder0.getPulses(), encoder1.getPulses(), desiredCount0, desiredCount1);
+        }
         pidOnEncoders();
     }
 
@@ -266,7 +275,7 @@
     double HIGH_PWM_VOLTAGE = 0.1;
 
     double rightSpeed = 0.1;
-    double leftSpeed = 0.1;
+    double leftSpeed = 0.12;
 
     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
     float ir3 = IRP_3.getSamples( SAMPLE_NUM );
@@ -279,7 +288,7 @@
         } else {
             blueLed.write(1);
         }
-
+        sumError += currentError;
         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;
@@ -305,8 +314,6 @@
         ir3 = IRP_3.getSamples( SAMPLE_NUM );
 
     }
-    //sensor1Turn = IR_Sensor1.read();
-    //sensor4Turn = IR_Sensor4.read();
 
     //backward();
     wait_ms(40);
@@ -369,8 +376,8 @@
     count0 = encoder0.getPulses();
     count1 = encoder1.getPulses();
     int diff = count0 - count1;
-    double kp = 0.000075;
-    double kd = 0.000135;
+    double kp = 0.00011;
+    double kd = 0.00014;
     int prev = 0;
 
     int counter = 0;
@@ -385,12 +392,12 @@
         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 - 50 ) // count1 is bigger, right wheel pushed forward
+        if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward
         {
             left_motor.move( -d );
             right_motor.move( d );
         }
-        else if( x > diff + 50 )
+        else if( x > diff + 40 )
         {
             left_motor.move( -d );
             right_motor.move( d );
@@ -407,7 +414,7 @@
     }
 }
 
-void oneCellEncoderAndIR(){
+void oneCellEncoderAndIR(double cellCount){
     double currentError = 0;
     double previousError = 0;
     double derivError = 0;
@@ -417,17 +424,31 @@
     double rightSpeed = 0.10;
     double leftSpeed = 0.10;
 
-    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum;
-    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum;
+    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
+    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
 
-    left_motor.forward(0.11);
+    left_motor.forward(0.125);
     right_motor.forward(0.10);
 
+    float receiverTwoReading = 0.0;
+    float receiverThreeReading = 0.0;
+
     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
     float ir3 = IRP_3.getSamples( SAMPLE_NUM );
         
     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
-        currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
+        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;
+        // 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));
+        
+        sumError += currentError;
         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
@@ -515,18 +536,18 @@
     //left_motor.forward( 0.2 );
 
     while (1) {
-        oneCellEncoderAndIR();
-        // moveForwardCellEncoder(1);
-        // wait(0.5);
-        // handleTurns();
-        // wait(0.5);
-        // moveForwardCellEncoder(1);
-        // wait(0.5);
-        // handleTurns();
+        // oneCellEncoderAndIR(5);
+        // break;
+        moveForwardCellEncoder(1);
+        wait(0.5);
+        handleTurns();
+        wait(0.5);
+        moveForwardCellEncoder(1);
+        wait(0.5);
+        handleTurns();
         break;
-        // moveForwardOneCellEncoder();
         //pidOnEncoders();
-       //moveForwardUntilWallIr();
+       // moveForwardUntilWallIr();
         //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) ) ;
@@ -535,14 +556,5 @@
 
         //reading = Rec_4.read();
 //        serial.printf("reading: %f\n", reading);
-//        redLed.write(0);
-//        wait_ms(1000);
-//        redLed.write(1);
-//        greenLed.write(0);
-//        wait_ms(1000);
-//        greenLed.write(1);
-//        blueLed.write(0);
-//        wait_ms(1000);
-//        blueLed.write(1);
     }
 }