Revision 15:b80555a4a8b9, committed 2017-05-14

Comitter:

kyleliangus

Date:

Sun May 14 04:45:21 2017 +0000

Parent:

14:9e7bb03ddccb

Child:

16:d9252437bd92

Commit message:

Created PID for encoder based on difference of speed rotations.

Changed in this revision

--- a/irpair.cpp	Sat May 13 23:49:02 2017 +0000
+++ b/irpair.cpp	Sun May 14 04:45:21 2017 +0000
@@ -20,41 +20,4 @@
         z += recv.read();
     ir.write( 0 );
     return z / samples;
-}
-
-/*
-inline float IrLED::blinkLED( int i, int samples )
-{
-    float z = 0;
-    if( i == 1 )
-    {
-        IR_LED1.write(1);
-        for( int j = 0; j < samples; j++ )
-            z += IR_Sensor1.read();
-        IR_LED1.write(0);
-    }
-    if( i == 2 )
-    {
-        IR_LED2.write(1);
-        for( int j = 0; j < samples; j++ )
-            z += IR_Sensor2.read();
-        IR_LED2.write(0);
-    }
-    if( i == 3 )
-    {
-        IR_LED3.write(1);   
-        for( int j = 0; j < samples; j++ )
-            z += IR_Sensor3.read();
-        IR_LED4.write(0);
-    }
-    if( i == 4 )
-    {
-        IR_LED4.write(1);
-        for( int j = 0; j < samples; j++ )
-            z += IR_Sensor4.read();
-        IR_LED4.write(0);
-    }
-    if( DEBUGGING )
-        serial.println( "Sample by IR %d: %f\n", i, z );
-    return z / samples;
-}*/
\ No newline at end of file
+}
\ No newline at end of file

--- 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",

--- a/main.h	Sat May 13 23:49:02 2017 +0000
+++ b/main.h	Sun May 14 04:45:21 2017 +0000
@@ -30,8 +30,8 @@
 IRPair IRP_2( PB_0, PA_6 );
 IRPair IRP_1( PB_14, PA_7 );
 
-Motor left_motor( PB_7, PB_8, PB_4 );
-Motor right_motor( PA_10, PA_11, PB_5 );
+Motor left_motor( PB_8, PB_7, PB_4 ); // forward, backwards, enable
+Motor right_motor( PA_11, PA_10, PB_5 ); // forward, backwards, enable
 
 /*
 DigitalOut IR_1(PB_1);

--- a/motor.cpp	Sat May 13 23:49:02 2017 +0000
+++ b/motor.cpp	Sun May 14 04:45:21 2017 +0000
@@ -7,8 +7,9 @@
     }else if(voltage < minSpeed){
         voltage = minSpeed;
     }
-    forw.write(voltage);
-    back.write(0);
+    enableForw.write(0);
+    enableBack.write(1);
+    motorSpeed.write( voltage );
 }
 
 void Motor::forward(double voltage) {
@@ -17,8 +18,9 @@
     }else if(voltage < minSpeed){
         voltage = minSpeed;
     }
-    forw.write(0);
-    back.write(voltage);
+    enableForw.write(1);
+    enableBack.write(0);
+    motorSpeed.write( voltage );
 }
 
 void Motor::move(double voltage) {
@@ -32,11 +34,13 @@
 
 
 void Motor::brake() {
-    forw.write(BRAKE_VOLTAGE);
-    back.write(BRAKE_VOLTAGE);
+    enableForw.write(1);
+    enableBack.write(1);
+    motorSpeed.write( BRAKE_VOLTAGE );
 }
 
 void Motor::coast() {
-    forw.write(0);
-    back.write(0);
+    enableForw.write(0);
+    enableBack.write(0);
+    motorSpeed.write( 0 );
 }
\ No newline at end of file

--- a/motor.h	Sat May 13 23:49:02 2017 +0000
+++ b/motor.h	Sun May 14 04:45:21 2017 +0000
@@ -19,9 +19,8 @@
 {
     public:
         Motor( PinName f, PinName b, PinName e ) : 
-            forw( f ), back( b ), enableMotor( e ), maxSpeed( 0.2 ), minSpeed(0.07)
+            enableForw( f ), enableBack( b ), motorSpeed( e ), maxSpeed( 0.2 ), minSpeed(0.07)
         {
-            enableMotor.write( 1 );
         }
     
         void backward( double voltage );
@@ -31,9 +30,9 @@
         void move( double voltage );
         
     private:
-        PwmOut forw;
-        PwmOut back;
-        DigitalOut enableMotor;  
+        DigitalOut enableForw;
+        DigitalOut enableBack;
+        PwmOut motorSpeed;  
         const double maxSpeed;
         const double minSpeed;
 };