Revision 19:7b66a518b6f8, committed 2017-05-15

Comitter:

sahilmgandhi

Date:

Mon May 15 00:54:41 2017 +0000

Parent:

18:6a4db94011d3

Child:

20:82836745332e

Commit message:

Got the PID to work on both encoder and IR at the same time ... BUT i still cannot get it to recognize it to turn the right direction.

Changed in this revision

--- a/main.cpp	Sun May 14 23:18:57 2017 +0000
+++ b/main.cpp	Mon May 15 00:54:41 2017 +0000
@@ -16,15 +16,15 @@
 */
 
 // Constants for when HIGH_PWM_VOLTAGE = 0.1
-#define IP_CONSTANT 9
+#define IP_CONSTANT 8.9
 #define II_CONSTANT 1
-#define ID_CONSTANT 3
+#define ID_CONSTANT 3.1
 
 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 = 4600;      // one cell count is actually approximately 5400, but this value is considering momentum!
 
 float receiverOneReading = 0.0;
 float receiverTwoReading = 0.0;
@@ -222,16 +222,16 @@
     int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
     int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
 
-    left_motor.forward(0.12);
+    left_motor.forward(0.11);
     right_motor.forward(0.10);
-    wait_ms(2);
+    wait_ms(1);
     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
-        receiverTwoReading = IRP_2.getSamples(20);
-        receiverThreeReading = IRP_3.getSamples(20);
-        if (receiverTwoReading <= IRP_2.sensorAvg/2.5)
+        receiverTwoReading = IRP_2.getSamples(5);
+        receiverThreeReading = IRP_3.getSamples(5);
+        if (receiverThreeReading < 0.0007f)
+            turnFlag |= RIGHT_FLAG;
+        else if (receiverTwoReading < 0.0009f)
             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();
     }
@@ -242,16 +242,17 @@
 
 void handleTurns(){
     if (turnFlag == 0x1){
-        moveForwardCellEncoder(0.5);
+        // moveForwardCellEncoder(0.3);
         turnLeft();
     }
     else if (turnFlag == 0x2){
-        moveForwardCellEncoder(0.5);
+        // moveForwardCellEncoder(0.3);
         turnRight();
     }
     else if (turnFlag == 0x3){
-        moveForwardCellEncoder(0.5);
+        // moveForwardCellEncoder(0.3);
         turnLeft();
+        turnRight();
     }
 }
 
@@ -401,13 +402,57 @@
         // }
         prev = x;
         counter++;
-        if (counter == 4)
+        if (counter == 5)
             break;
     }
 }
 
 void oneCellEncoderAndIR(){
+    double currentError = 0;
+    double previousError = 0;
+    double derivError = 0;
+    double sumError = 0;
 
+    double HIGH_PWM_VOLTAGE = 0.1;
+    double rightSpeed = 0.10;
+    double leftSpeed = 0.10;
+
+    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum;
+    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum;
+
+    left_motor.forward(0.11);
+    right_motor.forward(0.10);
+
+    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) ) ;
+        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);
+        pidOnEncoders();
+
+        previousError = currentError;
+        ir2 = IRP_2.getSamples( SAMPLE_NUM );
+        ir3 = IRP_3.getSamples( SAMPLE_NUM );
+    }
+
+    left_motor.brake();
+    right_motor.brake();
 }
 
 int main()
@@ -470,21 +515,18 @@
     //left_motor.forward( 0.2 );
 
     while (1) {
-        moveForwardCellEncoder(1);
-        wait(0.5);
-        handleTurns();
-        wait_ms(5);
-        moveForwardCellEncoder(1);
-        wait(0.5);
-        handleTurns();
+        oneCellEncoderAndIR();
+        // moveForwardCellEncoder(1);
+        // wait(0.5);
+        // handleTurns();
+        // wait(0.5);
+        // moveForwardCellEncoder(1);
+        // wait(0.5);
+        // handleTurns();
         break;
         // moveForwardOneCellEncoder();
         //pidOnEncoders();
        //moveForwardUntilWallIr();
-    //        wait(2);
-    //        turnRight();
-    //        wait(1);
-    //        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) ) ;