Diff: main.cpp

Revision:

2:d7286c36595f

Parent:

1:08e8cc33fcae

Child:

3:07fedd2e252c

--- a/main.cpp	Fri Oct 11 09:53:22 2019 +0000
+++ b/main.cpp	Fri Oct 11 11:49:34 2019 +0000
@@ -14,7 +14,7 @@
 DigitalIn encA(D13);
 DigitalIn encB(D12);
 QEI encoder(D13,D12,NC,64,QEI::X4_ENCODING);
-float T_encoder = 0.01;
+float Ts = 0.01;
 float angle;
 float omega;
 
@@ -32,7 +32,13 @@
 double u2;
 double potValue;
 double pi2= 6.283185;
-float fout;
+float e; //e = error
+float Kp=0.1;
+float Ki=0.1;
+float Kd=0.7;
+float u_k;
+float u_i;
+float u_d;
 // PC connection
 MODSERIAL pc(USBTX, USBRX);
 
@@ -48,26 +54,40 @@
 volatile int countsPrev = 0;
 volatile int deltaCounts;
 
-// motor1Direction = 1;
-volatile int motor1Toggle = 1;
-
 float factorin = 6.23185/64; // Convert encoder counts to angle in rad
 float gearratio = 131.25; // Gear ratio of gearbox
 
+
+float PID_controller(float e){
+    static float error_integral=0;
+    
+    //Proportional part:
+    u_k=Kp*e;
+    
+    //Integral part
+    error_integral=error_integral+e*Ts;
+    u_i=Ki*error_integral;
+    
+    // Sum and return
+    return u_k+u_i;    
+}
+
 void motorControl()
 {
     angle = counts * factorin / gearratio; // Angle of motor shaft in rad
-    omega = deltaCounts / T_encoder * factorin / gearratio; // Angular velocity of motor shaft in rad/s
+    omega = deltaCounts / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     potValue= potmeter.read();
     u1= (potValue*2*pi2)-pi2;
-    fout=u1-angle;
-    u2= fout/pi2;
-    motorpwm= abs(u2);
+    e=u1-angle;
+    
+    u2=PID_controller(e);
+    
+        motorpwm= abs(u2);
     if (u2<0){
         motordir= 0;}
     else {
          motordir= 1;}
-    motor1Power.pulsewidth(motorpwm * PWM_period * motor1Toggle);
+    motor1Power.pulsewidth(motorpwm * PWM_period );
     motor1Direction= motordir;
 }
 
@@ -88,14 +108,14 @@
     motorTicker.attach(motorControl, 0.01);
     
     
-    encoderTicker.attach(readEncoder, T_encoder);
+    encoderTicker.attach(readEncoder, Ts);
    
     while (true) {
 
         pc.printf("Potmeter: %d \r\n", potValue);
         pc.printf("Counts: %i   DeltaCounts: %i\r\n", counts, deltaCounts);
         pc.printf("Angle:  %f   Omega:       %f\r\n", angle, omega);
-        pc.printf("U1: %f   Error:  %f     \r\n",u1, fout);
+        pc.printf("U1: %f   Error:  %f     \r\n",u1, e);
         
         wait(0.5);
     }