Diff: pidControl.cpp

Revision:

9:d04d028ccfe8

Parent:

8:ef6b758b73da

Child:

10:f05bd773b66c

--- a/pidControl.cpp	Tue Oct 20 11:38:06 2015 +0000
+++ b/pidControl.cpp	Tue Oct 20 14:30:35 2015 +0000
@@ -5,6 +5,7 @@
 #include "PinDetect.h"
 #include "pidControl.h"
 #include "biquadFilter.h"
+#include "compute.h"
 
 Serial pc(USBTX, USBRX);
 
@@ -33,6 +34,9 @@
 float tickRate = 0.01f;
 float graphTickRate = 0.01f;
 
+double offsetA;
+double offsetB;
+
 float toRadians(int pulses);
 
 AnalogIn pot1(m1_pot);
@@ -125,7 +129,7 @@
         return (motor == 1)?0:1;
 }
 
-void PID_init()
+void PID_init(double angleA, double angleB)
 {
     pc.printf("Initializing...\r\n");
 
@@ -138,12 +142,57 @@
     pc.printf("Encoders reset\r\n");
 
     go_motor = true;
+    shutup = false;
 
     // Start tickers
     potTicker.attach(&readPot, tickRate);
     motorTicker.attach(&getMotorRotation, tickRate);
     graphTicker.attach(&sendGraph, graphTickRate);
     pc.printf("Tickers attached\r\n");
+    
+    bool cont = true;
+    double a =0.0;
+    double b = 0.0;
+    double prev = -1.0;
+    while(cont) {
+        a -= 0.1;
+        pc.printf("%f\r\n",a);
+        moveOneMotor(1,a);
+        wait(1);
+        double temp = toRadians(enc1.getPulses());
+        pc.printf("PREV | TEMP = %f | %f\r\n",prev,temp);
+        if(fabs(prev-temp) < 0.005) {
+            pwm1 = 0;
+            cont = false;
+            offsetA = deg2rad(200)-temp;
+        }
+        else 
+            prev = temp;
+    }
+    
+    pc.printf("\r\nOFFSET: %f | OFFSET: %d\r\n",offsetA, rad2deg(offsetA));
+    pc.printf("\r\nTO: %f | TO: %d",deg2rad(135)-offsetA, rad2deg(deg2rad(135)-offsetA));
+    
+    moveOneMotor(1,deg2rad(135)-offsetA);
+    wait(3);
+    
+    cont = true;
+    while(cont) {
+        pwm1 = 0;
+        b -= 0.1;
+        moveOneMotor(2,b);
+        wait(1);
+        double temp = toRadians(enc2.getPulses());
+        if(fabs(prev-temp) < 0.005) {
+            pwm2 = 0;
+            cont = false;
+            offsetB = deg2rad(-45)-temp;
+        }
+        else 
+            prev = temp;
+    }
+
+    //rotate(offsetA+angleA,offsetB+angleB);
 
     pc.printf("Initialized\r\n");
 }
@@ -154,7 +203,7 @@
     b = toRadians(enc2.getPulses());
 }
 
-int move(double a, double b)
+void rotate(double a, double b)
 {
     if (shutup) {
         pwm1 = 0;
@@ -175,6 +224,8 @@
         // 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);
+        
+        pc.printf("CUR ROT 1: %f\r\nCUR ROT 2: %f\r\n",control1, control2);
 
         int d1 = getPDirection(control1,1);
         int d2 = getPDirection(control2,2);
@@ -188,4 +239,46 @@
         pwm1 = speed1;
         pwm2 = speed2;
     }
+}
+
+void moveOneMotor(int num, double angle) {
+    if(num == 1) {
+        desiredRotation1 = angle;
+        currentRotation1 = toRadians(enc1.getPulses());
+        double previous_error1 = error1;
+        error1 = desiredRotation1 - currentRotation1;
+
+        double control = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
+        
+        int d = getPDirection(control,1);
+        float speed = fabs(control);
+        pc.printf("SPEED: %f\r\n",speed);
+        if (speed <= 0.1f) speed = 0.0f;
+        if(speed > 0.35f) speed = 0.35f;
+        speed = 0.3;
+        
+        dir1 = d;
+        pwm1 = speed;
+        
+        pc.printf("DIR 1: %f | PWM 2: %f\r\n",d, speed); 
+    }
+    else {
+        desiredRotation2 = angle;
+        currentRotation2 = toRadians(enc2.getPulses());
+        double previous_error2 = error2;
+        error2 = desiredRotation2 - currentRotation2;
+
+        double control = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative, m2_filter);
+        
+        int d = getPDirection(control,1);
+        float speed = fabs(control);
+        if (speed <= 0.1f) speed = 0.0f;
+        if(speed > 0.35f) speed = 0.35f;
+        speed = 0.3;
+        
+        dir2 = d;
+        pwm2 = speed;
+        
+        pc.printf("DIR 2: %f | PWM 2: %f\r\n",d, speed); 
+    }
 }
\ No newline at end of file