Diff: main.cpp

Revision:

15:b80555a4a8b9

Parent:

14:9e7bb03ddccb

Child:

16:d9252437bd92

--- a/main.cpp	Sat May 13 23:49:02 2017 +0000
+++ b/main.cpp	Sun May 14 04:45:21 2017 +0000
@@ -16,9 +16,9 @@
 */
 
 // Constants for when HIGH_PWM_VOLTAGE = 0.1
-#define IP_CONSTANT 12
-#define II_CONSTANT 0
-#define ID_CONSTANT 2
+#define IP_CONSTANT 9
+#define II_CONSTANT 1
+#define ID_CONSTANT 3
 
 const int desiredCountR = 1400;
 const int desiredCountL = 1475;
@@ -137,7 +137,7 @@
 
 
     while(1) {
-
+    
         if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
             count0 = encoder0.getPulses();
             count1 = encoder1.getPulses();
@@ -213,11 +213,11 @@
     double speed1 = 0.15;
 
     int counter = 0;
-    int initial0 = encoder0.getPulses();
-    int initial1 = encoder1.getPulses();
+    int initial0 = encoder0.getPulses(); // left
+    int initial1 = encoder1.getPulses(); // right
 
-    double kpLeft = 0.000005;       
-    double kpRight = 0.000005;
+    double kpLeft = 0.0000005;       
+    double kpRight = 0.0000005;
 
     int desiredCount0 = initial0 + oneCellCount;
     int desiredCount1 = initial1 + oneCellCount;
@@ -365,9 +365,48 @@
     printDipFlag();
 }
 
+void pidOnEncoders()
+{
+    int count0;
+    int count1;
+    count0 = encoder0.getPulses();
+    count1 = encoder1.getPulses();
+    int diff = count0 - count1;
+    double kp = 0.000075;
+    double kd = 0.000125;
+    int prev = 0;
+    while(1)
+    {
+        count0 = encoder0.getPulses();
+        count1 = encoder1.getPulses();
+        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 - 50 ) // count1 is bigger, right wheel pushed forward
+        {
+            left_motor.move( -d );
+            right_motor.move( d );
+        }
+        else if( x > diff + 50 )
+        {
+            left_motor.move( -d );
+            right_motor.move( d );
+        }
+        else
+        {
+            left_motor.brake();
+            right_motor.brake();   
+        }
+        prev = x;
+    }
+}
+
 int main()
 {
-    //enableMotors();
     //Set highest bandwidth.
     gyro.setLpBandwidth(LPFBW_42HZ);
     serial.baud(9600);
@@ -375,10 +414,6 @@
 
     wait (0.1);
 
-//    IR_1.write(1);
-//    IR_2.write(1);
-//    IR_3.write(1);
-//    IR_4.write(1);
 
     redLed.write(1);
     greenLed.write(0);
@@ -426,10 +461,13 @@
     dipButton3.fall(&disableButton3);
     dipButton4.fall(&disableButton4);
 
+    //right_motor.forward( 0.2 );
+    //left_motor.forward( 0.2 );
+
     while (1) {
-       moveForwardOneCellEncoder(); 
-        
-       // moveForwardUntilWallIr();
+       //moveForwardOneCellEncoder(); 
+        pidOnEncoders();
+       //moveForwardUntilWallIr();
     //        wait(2);
     //        turnRight();
     //        wait(1);
@@ -438,7 +476,7 @@
 
         break;
         //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());
+        //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) ) ;
 
         //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n",