Diff: pidControl.cpp

Revision:

12:84f2c63f9b98

Parent:

11:7f41fac17c48

Child:

13:41d897396fb5

--- a/pidControl.cpp	Thu Oct 22 14:44:18 2015 +0000
+++ b/pidControl.cpp	Mon Oct 26 12:13:12 2015 +0000
@@ -35,10 +35,7 @@
 float tickRate = 0.01f;
 float graphTickRate = 0.01f;
 
-double offsetA;
-double offsetB;
-
-float toRadians(int pulses);
+double offsetA, offsetB;
 
 AnalogIn pot1(m1_pot);
 AnalogIn pot2(m2_pot);
@@ -63,30 +60,20 @@
 
 bool pushing;
 
-float getPotRad(AnalogIn pot)
-{
-    return pot.read() * 4.0f - 2.0f;
-}
-
+/*
+* Calculates the number of radians given a number of pulses of an encoder
+*   int pulses: The number of pulses to convert into radians
+*/
 float toRadians(int pulses)
 {
-    int remaining = pulses;// % sigPerRev;
+    int remaining = pulses;
     float percent = (float) remaining / (float) sigPerRev;
     return percent * 2.0f;
 }
 
-double p_control(double error, double kp)
-{
-    return kp * error;
-}
-
-double pi_control(double error, double kp, double ki, double ts, double &error_integral)
-{
-    error_integral = error_integral + ts * error;
-    double result =  kp * error + ki * error_integral;
-    return result;
-}
-
+/*
+* Calculates the result of the PID Controller
+*/
 double pid_control(double error, double kp, double ki, double ts, double &error_integral,
                    double kd, double previous_error, double &error_derivative, biquadFilter filter)
 {
@@ -98,6 +85,11 @@
     return result;
 }
 
+/*
+* Get the direction of the motor
+*   double control: The desired direction
+*   int motor:      The ID of the motor [1 == motor 1 | 2 == motor 2]
+*/
 int getPDirection(double control, int motor)
 {
     if (control >= 0)
@@ -106,52 +98,51 @@
         return (motor == 1)?0:1;
 }
 
-void PID_init(double angleA, double angleB)
+/*
+* The initializator of the PID Controller
+*/
+void PID_init()
 {
-    pc.printf("Initializing...\r\n");
+    pc.printf("Initializing PID Controller...\r\n");
 
+    /* Setting the pulse width modulation */
     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);
-
-    // Reset encoders
+    
+    /* Resetting the encoders */
     enc1.reset();
     enc2.reset();
     pc.printf("Encoders reset\r\n");
 
+    /*  
+    * Calibrate the first motor
+    *   Rotate it until it hits the base, then reset the encoder and calculate the offset.  
+    */
     bool cont = true;
     double prev = -1.0;
     while(cont) {
         moveOneMotor(1,0);
         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;
         } else
             prev = temp;
     }
-
     enc1.reset();
 
-    pc.printf("Angle B: %d\r\n\r\n", rad2deg(toRadians(enc1.getPulses())));
-
-    pc.printf("RAD: 1 DEG: %d\r\n",rad2deg(1));
-
+    /* Move the first motor to a better starting position */
     moveOneMotor(1,1);
     wait(1.1);
     pwm1 = 0;
 
+    /*  
+    * Calibrate the second motor
+    *   Rotate it until it hits the base, then reset the encoder and calculate the offset.  
+    */
     cont = true;
     while(cont) {
         moveOneMotor(2,0);
@@ -166,38 +157,47 @@
 
     enc2.reset();
 
-    pc.printf("Angle B: %d\r\n\r\n", rad2deg(toRadians(enc2.getPulses())));
-
+    /* Move the first motor to a better starting position */
     moveOneMotor(2,1);
     wait(1);
     pwm2 = 0;
 
+    /* Calculate both offsets */
     offsetA = deg2rad(243);      // 243
     offsetB = deg2rad(-63);      // -63
 
+    /* Set the default desired rotation */
     desiredRotation1 = toRadians(enc1.getPulses());
     desiredRotation2 = toRadians(enc2.getPulses());
-
-    pc.printf("Angle A: %d | Angle B: %d\r\n\r\n", rad2deg(toRadians(enc1.getPulses())), rad2deg(toRadians(enc2.getPulses())));
-
-    pc.printf("A: -%d + %d = %d\r\nB: -%d + %d = %d\r\n",rad2deg(offsetA),135,rad2deg(-offsetA+deg2rad(135)),rad2deg(offsetB),45,rad2deg(-offsetB+deg2rad(45)));
-
-    pc.printf("Initialized\r\n");
 }
 
+/*
+* Gives the current rotation of the motors.
+*   double& a: The variable to store the rotation of motor 1
+*   double& b: The variable to store the rotation of motor 2
+*/
 void getCurrent(double& a, double& b)
 {
     a = toRadians(enc1.getPulses());
     b = toRadians(enc2.getPulses());
 }
 
+/*
+* Rotates the motors to certain rotations
+*   double a: The desired rotation of motor 1
+*   double b: The desired rotation of motor 2
+*/
 void rotate(double a, double b)
 {
     desiredRotation1 = -(a - offsetA);
     desiredRotation2 = -(b - offsetB);
-    pc.printf("DR1: %d (%f) | DR2: %d (%f)\r\n",rad2deg(desiredRotation1), desiredRotation1, rad2deg(desiredRotation2), desiredRotation2);
 }
 
+/*
+* Rotates the motors to certain rotations, while using the PID Controller used for the pushing motion
+*   double a: The desired rotation of motor 1
+*   double b: The desired rotation of motor 2
+*/
 void push(double a, double b)
 {
     pushing = true;
@@ -205,19 +205,28 @@
     desiredRotation2 = -(b - offsetB);
 }
 
+/*
+* Returns the offset of the motors calulated in the initialization phase.
+*   int a: The id of the motor [1 == motor 1 | 2 == motor 2]
+*/
 double getOffset(int a)
 {
     if(a == 1) return offsetA;
     else return offsetB;
 }
 
-
+/*
+* Stops the motors.
+*/
 void PID_stop()
 {
     pwm1 = 0;
     pwm2 = 0;
 }
 
+/*
+* Calculates the speed and direction of the motors using a PID Controller and the current rotation of the motors, given a desired rotation.
+*/
 void moveTick()
 {
     currentRotation1 = toRadians(enc1.getPulses());
@@ -229,8 +238,6 @@
     error1 = desiredRotation1 - currentRotation1;
     error2 = desiredRotation2 - currentRotation2;
 
-    //pc.printf("Error1: %f | Error2: %f\r\n",error1, error2);
-
     // PID control
     double control1, control2;
     if(pushing) {
@@ -240,37 +247,33 @@
         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);
     int d2 = getPDirection(control2,2);
     float speed1 = fabs(control1);
     float speed2 = fabs(control2);
 
-    //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;
     pwm2 = speed2;
 }
 
+/* 
+* Moves one motor with a speed of 0.2.
+*   int num: the ID of the motor [1 == motor 1 | 2 == motor 2]
+*   int dir: the direction of the motor
+*/
 void moveOneMotor(int num, int dir)
 {
     if(num == 1) {
         dir1 = dir;
         pwm1 = 0.2;
-
-        //pc.printf("DIR 1: %f\r\n",dir);
     } else {
         dir2 = dir;
         pwm2 = 0.2;
-
-        // pc.printf("DIR 2: %f\r\n",dir);
     }
 }
\ No newline at end of file