Diff: pidControl.cpp

Revision:

11:7f41fac17c48

Parent:

10:f05bd773b66c

Child:

12:84f2c63f9b98

--- a/pidControl.cpp	Wed Oct 21 13:44:06 2015 +0000
+++ b/pidControl.cpp	Thu Oct 22 14:44:18 2015 +0000
@@ -22,10 +22,11 @@
 PinName m2_pot = A0;
 PinName m1_pot = A1;
 
-const double pid_upper_limit = 1, pid_lower_limit = 0;
+const double motor1_kp = 0.75f, motor1_ki = 0.005f, motor1_kd = 0.05f;
+const double motor2_kp = 0.75f, motor2_ki = 0.005f, motor2_kd = 0.05f;
 
-const double motor1_kp = 0.75f, motor1_ki = 0.001f, motor1_kd = 0.005f;
-const double motor2_kp = 0.75f, motor2_ki = 0.001f, motor2_kd = 0.005f;
+const double motor1_push_kp = 1.5f, motor1_push_ki = 0.005f, motor1_push_kd = 0.1f;
+const double motor2_push_kp = 1.5f, motor2_push_ki = 0.005f, motor2_push_kd = 0.1f;
 
 const double m1_f_a1 = 1.0, m1_f_a2 = 2.0, m1_f_b0 = 1.0, m1_f_b1 = 3.0, m1_f_b2 = 4.0;
 const double m2_f_a1 = 1.0, m2_f_a2 = 2.0, m2_f_b0 = 1.0, m2_f_b1 = 3.0, m2_f_b2 = 4.0;
@@ -60,6 +61,8 @@
 biquadFilter m1_filter(m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2);
 biquadFilter m2_filter(m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2);
 
+bool pushing;
+
 float getPotRad(AnalogIn pot)
 {
     return pot.read() * 4.0f - 2.0f;
@@ -106,11 +109,16 @@
 void PID_init(double angleA, double angleB)
 {
     pc.printf("Initializing...\r\n");
-    
-   // pwm1.period(0.00001);
-   // pwm1.pulsewidth(0.000005);
-   // pwm2.period(0.00001);
-   // pwm2.pulsewidth(0.000005);
+
+    pwm1.period_ms(0.01);
+    pwm1.pulsewidth_ms(0.005);
+    pwm2.period_ms(0.01);
+    pwm2.pulsewidth_ms(0.005);
+
+    // pwm1.period(0.00001);
+    // pwm1.pulsewidth(0.000005);
+    // pwm2.period(0.00001);
+    // pwm2.pulsewidth(0.000005);
 
     // Set proper baudrate
     // pc.baud(115200);
@@ -124,7 +132,7 @@
     double prev = -1.0;
     while(cont) {
         moveOneMotor(1,0);
-        wait(0.5);
+        wait(.1);
         double temp = toRadians(enc1.getPulses());
         //pc.printf("PREV | TEMP = %f | %f\r\n",prev,temp);
         if(fabs(prev-temp) < 0.005) {
@@ -141,13 +149,13 @@
     pc.printf("RAD: 1 DEG: %d\r\n",rad2deg(1));
 
     moveOneMotor(1,1);
-    wait(1.5);
+    wait(1.1);
     pwm1 = 0;
 
     cont = true;
     while(cont) {
         moveOneMotor(2,0);
-        wait(0.5);
+        wait(.1);
         double temp = toRadians(enc2.getPulses());
         if(fabs(prev-temp) < 0.005) {
             pwm2 = 0;
@@ -161,12 +169,12 @@
     pc.printf("Angle B: %d\r\n\r\n", rad2deg(toRadians(enc2.getPulses())));
 
     moveOneMotor(2,1);
-    wait(1.3);
+    wait(1);
     pwm2 = 0;
 
     offsetA = deg2rad(243);      // 243
     offsetB = deg2rad(-63);      // -63
-    
+
     desiredRotation1 = toRadians(enc1.getPulses());
     desiredRotation2 = toRadians(enc2.getPulses());
 
@@ -186,18 +194,28 @@
 void rotate(double a, double b)
 {
     desiredRotation1 = -(a - offsetA);
-    desiredRotation2 = -(b - offsetB); 
+    desiredRotation2 = -(b - offsetB);
+    pc.printf("DR1: %d (%f) | DR2: %d (%f)\r\n",rad2deg(desiredRotation1), desiredRotation1, rad2deg(desiredRotation2), desiredRotation2);
 }
 
-double getOffset(int a) {
+void push(double a, double b)
+{
+    pushing = true;
+    desiredRotation1 = -(a - offsetA);
+    desiredRotation2 = -(b - offsetB);
+}
+
+double getOffset(int a)
+{
     if(a == 1) return offsetA;
     else return offsetB;
 }
-    
+
 
-void PID_stop() {
+void PID_stop()
+{
     pwm1 = 0;
-    pwm2 = 0;   
+    pwm2 = 0;
 }
 
 void moveTick()
@@ -214,9 +232,14 @@
     //pc.printf("Error1: %f | Error2: %f\r\n",error1, error2);
 
     // PID control
-    double control1 = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
-    double control2 = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative, m2_filter);
-
+    double control1, control2;
+    if(pushing) {
+        control1 = pid_control(error1, motor1_push_kp, motor1_push_ki, tickRate, m1_error_integral, motor1_push_kd, previous_error1, m1_error_derivative, m1_filter);
+        control2 = pid_control(error2, motor2_push_kp, motor2_push_ki, tickRate, m2_error_integral, motor2_push_kd, previous_error2, m2_error_derivative, m2_filter);
+    } else {
+        control1 = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
+        control2 = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative, m2_filter);
+    }
     //pc.printf("CUR ROT 1: %f\r\nCUR ROT 2: %f\r\n",control1, control2);
 
     int d1 = getPDirection(control1,1);
@@ -224,11 +247,13 @@
     float speed1 = fabs(control1);
     float speed2 = fabs(control2);
 
-    // if (speed1 < 0.1f) speed1 = 0.0f;
-    // if (speed2 < 0.1f) speed2 = 0.0f;
-    
-    //pc.printf("A: %d | B: %d \r\n",rad2deg(toRadians(enc1.getPulses())), rad2deg(toRadians(enc2.getPulses()))); 
-    
+    //pc.printf("SPEED 1: %f\r\nSPEED 2: %f", speed1, speed2);
+
+    if (speed1 < 0.02f) speed1 = 0.0f;
+    if (speed2 < 0.02f) speed2 = 0.0f;
+
+    //pc.printf("A: %d | B: %d \r\n",rad2deg(toRadians(enc1.getPulses())), rad2deg(toRadians(enc2.getPulses())));
+
     dir1 = d1;
     dir2 = d2;
     pwm1 = speed1;