Diff: main.cpp

Revision:

6:81dda0c8eb71

Parent:

5:532b9a72e012

--- a/main.cpp	Mon Oct 07 13:06:33 2019 +0000
+++ b/main.cpp	Mon Oct 07 13:11:37 2019 +0000
@@ -12,17 +12,31 @@
 DigitalOut motor1_dir(D4);
 AnalogIn pod1(A0);
 AnalogIn pod2(A1);
+
 Ticker measure;
 
-//Initial parameters control
 int m1_count; 
 float m1_angle;
 float pi = 3.14;
 float pod1_angle;
-float pod2_angle;
-float kp = 10;
 float motor1_er;
-float motor1_pwm_signal;
+float pod2_tick;
+float PWM_m1;
+
+//PID variables
+bool q = true;
+float error_prev;
+float Kp = 8;
+float Ki = 1.02;
+float Kd = 0.1;
+float Ts = 0.01;
+float u;
+float u_p;
+float u_i;
+float u_d;
+
+static double error_integral = 0;
+static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
 
 //initial parameters time and HIDSCOPE
 volatile float theoretical_time = 0;
@@ -40,7 +54,7 @@
     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
     scope.set(0, m1_count );
     scope.set(1,  theoretical_time);
-    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
+    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
     *  Ensure that enough channels are available (HIDScope scope( 2 ))
     *  Finally, send all channels to the PC at once */
     scope.send();
@@ -59,30 +73,49 @@
 sample();
 }
 
-/** Counter function
-* this functions counts the encoder for position referene
-**/
 void getCounts(){
     m1_count = motor1.getPulses();
     m1_angle = (float)m1_count / 4200.0f * (2.0f * pi);
     pod1_angle = (float)pod1 * 2.0f * pi;
-    pod2_angle = (float)pod2; 
+    pod2_tick = (float)pod2;
     }
     
 void error(void){
     motor1_er = pod1_angle - m1_angle;
     }
-void pwm_motor1(){
-    motor1_pwm_signal = kp * fabs((motor1_er * pod2_angle));
+    
+void PID_control(void){
+    //P action
+    u_p = Kp * fabs(motor1_er);
+    
+    //I action 
+    error_integral = error_integral + motor1_er * Ts;
+    u_i = Ki * error_integral;
+    
+    //D action
+    if(q){
+        error_prev = motor1_er;
+        q=false;
+        }
+        
+    float error_derivative = (motor1_er - error_prev)/Ts;
+    float filtered_error_derivative = LowPassFilter.step(error_derivative);
+    u_d = Kd * filtered_error_derivative;
+    error_prev = motor1_er;
+    
+    u = u_p + u_i + u_d;
+    
+    PWM_m1 = fabs(motor1_er * u);
     }
+    
 void control(){
-    if(fabs(motor1_er) < 0.1f){
+    if(fabs(motor1_er) < 0.005f){
         motor1_pwm.write(0);
     }else if(motor1_er < 0.0f){
-            motor1_pwm.write(motor1_pwm_signal);
+            motor1_pwm.write(PWM_m1);
             motor1_dir = 1;
     }else if(motor1_er > 0.0f){
-            motor1_pwm.write(motor1_pwm_signal);
+            motor1_pwm.write(PWM_m1);
             motor1_dir = 0;   
             }
     }
@@ -90,8 +123,8 @@
 void MeasureandControl(){
     getCounts();
     error();
-    pwm_motor1();
     control();
+    PID_control();
     }
 
 int main()
@@ -103,7 +136,7 @@
     measure.attach(MeasureandControl,0.01);
     sample_timer.attach(StepResponse,duration_theoretical_time);
     while(true){
-        pc.printf("\r\nMotor angle: %f Pod1 angle: %f K_p: %f Error: %f",m1_angle,pod1_angle,motor1_pwm_signal,motor1_er);
+        pc.printf("\r\nMotor angle: %f Pod angle: %f Error: %f \r\n u = %f , u_d = %f",m1_angle,pod1_angle,motor1_er,u,u_d);
         wait(0.8);
     }
 }
\ No newline at end of file