Diff: main.cpp

Revision:

0:46cf63cba59a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 01 09:47:36 2017 +0000
@@ -0,0 +1,102 @@
+/*
+* Parts of the code copied from PES lecture slides
+*/
+
+// include all necessary libraries
+#include "mbed.h"
+#include "QEI.h"
+
+//intialize all pins
+PwmOut motor1(D5);
+PwmOut motor2(D6);
+DigitalOut motor1Dir(D4); // direction of motor 1 (1 is ccw 0 is cw (looking at the shaft from the front))
+DigitalOut motor2Dir(D7); // direction of motor 2 (1 is ccw 0 is cw (looking at the shaft from the front))
+QEI motor1Encoder (D10,D11, NC, 624,QEI::X4_ENCODING);
+QEI motor2Encoder (D12,D13, NC, 624,QEI::X4_ENCODING);
+DigitalIn button1(D8); //button to move cw
+DigitalIn button2(D9); //button to move ccw
+
+//initialize variables
+double angleIncrement = 0.036; // increment of angle when button pressed (1 is a whole rotation (360 degrees))
+
+const double motor1KP=1.3; //Proportional gain of motor1 PI control
+const double motor1KI=0.5; //Integral gain of motor1 PI control
+const double motor1KD=0.5; // Differential gain of motor1 PID control
+
+const double motor2KP=1.3; //Proportional gain of motor1 PI control
+const double motor2KI=0.5; //Integral gain of motor1 PI control
+const double motor2KD=0.5; // Differential gain of motor1 PID control
+
+const double N=100; //LP filter coefficient
+const double encoderToMotor= 0.000119047619047619; //proportion of the rotation of the motor to the rotation of the encoder
+const double controllerTickerTime=0.01; //ticker frequency
+
+double motor1ErrorInt=0; //error of motor1 for the integrating part of PI controller
+double motor1ErrorDif=0; //error of motor1 for the integrating part of PI controller
+double desiredPos1 =0; //desired position of motor1
+
+double motor2ErrorInt=0; //error of motor1 for the integrating part of PI controller
+double motor2ErrorDif=0; //error of motor1 for the integrating part of PI controller
+double desiredPos2 =0; //desired position of motor1
+//initialize ticker for checking and correcting the angle
+Ticker myControllerTicker;
+
+
+double PIDController(double error, const double Kp, const double Ki, const double Kd, double Ts, const double N,double &intError, double &DifError){
+    const double a1 = -4/(N*Ts+2);
+    const double a2 = -(N*Ts-2)/(N*Ts+2);
+    const double b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
+    const double b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2);
+    const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
+
+    double v = error - a1*intError - a2*DifError;
+    double u = b0*v + b1*intError + b2*DifError;
+    DifError = intError; intError = v;
+    return u;
+}
+
+
+void motorButtonController(){
+    double position1= encoderToMotor*motor1Encoder.getPulses();
+    double posError1 = desiredPos1 - position1; 
+    
+    //change direction based on error sign
+    if(PIDController( posError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif)>0) {
+        motor1Dir=0;
+    } else {
+        motor1Dir =1;
+    }
+    //set motor speed based on PI controller error
+    motor1 = fabs(PIDController( posError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif));
+    
+    double position2= encoderToMotor*motor2Encoder.getPulses();
+    double posError2 = desiredPos2 - position2; 
+    
+    //change direction based on error sign
+    if(PIDController( posError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif)>0) {
+        motor2Dir=0;
+    } else {
+        motor2Dir =1;
+    }
+    //set motor speed based on PI controller error
+    motor2 = fabs(PIDController( posError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif));
+
+}
+
+int main(){
+    wait(2);
+    myControllerTicker.attach(&motorButtonController, controllerTickerTime);
+    while(1) {
+        if(!button1) {
+            desiredPos1+=angleIncrement;
+            desiredPos2-=angleIncrement;
+            wait(0.5f);
+        }
+
+        if(!button2) {
+            desiredPos1-=angleIncrement;
+            desiredPos2+=angleIncrement;
+            wait(0.5f);
+        }
+    }
+}
\ No newline at end of file