Diff: main.cpp

Revision:

9:334b1596637b

Parent:

8:008a7bf80fa0

Child:

10:f60f9849980a

--- a/main.cpp	Wed Oct 26 07:59:23 2016 +0000
+++ b/main.cpp	Wed Oct 26 14:00:22 2016 +0000
@@ -20,39 +20,36 @@
 InterruptIn knop_triceps(SW3);
 InterruptIn knop_switch(D9);
 
-volatile double q1 = 0;
-volatile double q2 = 0;
-volatile double q1_begin;
-volatile double q2_begin;
-volatile double l1 = 0.3626;
-volatile double l2 = 0.420;
-volatile double q1_v;
-volatile double q2_v;
-volatile double q1_ref;
-volatile double q2_ref;
-volatile double ctrlOutput_M1 = 0;
-volatile double ctrlOutput_M2 = 0;
-volatile double vx;
-volatile double vy;
+volatile float q1 = 0;
+volatile float q2 = 0;
+volatile float q1_begin;
+volatile float q2_begin;
+volatile float l1 = 0.3626;
+volatile float l2 = 0.420;
+volatile float q1_v;
+volatile float q2_v;
+ float q1_ref = 0;
+ float q2_ref = 0;
+ float q1_error = 0;
+ float q2_error = 0;
+ float q1_error_prev = 0;
+ float q2_error_prev = 0;
+ float q1DerivativeError = 0;
+ float q2DerivativeError = 0;
+ float q1IntError = 0;
+ float q2IntError = 0;
+ float TotalError1= 0;
+ float TotalError2= 0;
+ float motorValue1Out = 0.0;
+ float motorValue2Out = 0.0;
+volatile float ctrlOutput_M1 = 0;
+volatile float ctrlOutput_M2 = 0;
+volatile float vx;
+volatile float vy;
 volatile bool translatie_richting = true;  //true is verticaal, false is horizontaal
 
-const double TS = 0.02;
-const double M1_Kp = 0.5, M1_Ki = 0.00, M1_Kd = 0;        // Waardes vinden?
-const double M2_Kp = 0.5, M2_Ki = 0.00, M2_Kd = 0;
-const double N = 0.1;
-
-Ticker begin_waarde_q;
-int n = 0;
-void begin_waarde()
-{
-    n++;
-    if(n == 2)
-    {
-        q1_begin = wheel_M1.getPulses() / (1334.355/2);
-        q2_begin = wheel_M2.getPulses() / (1334.355/2);
-        begin_waarde_q.detach();
-    }
-}
+const float TS = 0.02;
+const float MotorGain = 8.4;       // bij pwm = 1 draait (losse) motor met 8.4 rad/s
 
 Ticker update_encoder_ticker;
 volatile bool go_flag_update_encoder = false;
@@ -65,7 +62,8 @@
 {
     q1 = wheel_M1.getPulses()/(1334.355/2);
     q2 = wheel_M2.getPulses()/(1334.355/2);
-    pc.printf("q1 = %f \tq1_ref = %f \tPID1 = %f \tq2 = %f \tq2_ref = %f \tPID2 = %f \tq1_v=%f \tq2_v=%f\n\r",q1, q1_ref, ctrlOutput_M1,q2,q2_ref,ctrlOutput_M2,q1_v,q2_v);
+    //pc.printf("q1 = %f \tq1_ref = %f \tPID1 = %f \tq2 = %f \tq2_ref = %f \tPID2 = %f \ttotalerror1 = %f \ttotalerror2 = %f\n\r",q1, q1_ref, ctrlOutput_M1,q2,q2_ref,ctrlOutput_M2,TotalError1,TotalError2);
+    pc.printf("vx = %f \tvy = %f \tq1_v = %f \tq2_v = %f\n\r",vx,vy,q1_v,q2_v);
 }
 
 volatile bool go_flag_initialize = false;
@@ -75,12 +73,45 @@
     go_flag_initialize = true;
 }
 
+Ticker PIDcontrol;
+volatile bool go_flag_controller = false;
+
+void flag_controller()
+{
+    go_flag_controller = true;
+}
+
+volatile bool active_PID_ticker = false;
+
+void begin_hoeken()
+{
+    wait(3);
+    float q1_ref = wheel_M1.getPulses()/(1334.355/2);
+    float q2_ref = wheel_M2.getPulses()/(1334.355/2);
+    active_PID_ticker = true;
+}
+
+
 void initialize()
 {
-    q1_ref = 15*2*3.1415/360;
-    q2_ref = -30*2*3.1415/360;
+    dir_M1 = 0; //ccw
+    dir_M2 = 1; //cw
+    while (q1 < 20*2*3.1415/360) {
+        q1 = wheel_M1.getPulses()/(1334.355/2);
+        pwm_M1 = 0.01;
+    }
+    pwm_M1 = 0;
+
+    while (q2 > -45*2*3.1415/360) {
+        q2 = wheel_M2.getPulses()/(1334.355/2);
+        pwm_M2 = 0.01;
+    }
+    pwm_M2 = 0;
+    ledg = !ledg;
+    begin_hoeken();
 }
 
+
 void biceps()
 {
     if (translatie_richting) {      // verticaal / up
@@ -125,10 +156,10 @@
 }
 
 Ticker update_ref_ticker;
-volatile double J_1;
-volatile double J_2;
-volatile double J_3;
-volatile double J_4;
+volatile float J_1;
+volatile float J_2;
+volatile float J_3;
+volatile float J_4;
 volatile bool go_flag_update_ref = false;
 void flag_update_ref()
 {
@@ -148,9 +179,6 @@
     q1_v = J_1 * vx + J_2 * vy;
     q2_v = J_3 * vx + J_4 * vy;
 
-    q1_ref = q1 + q1_v*TS;
-    q2_ref = q2 + q2_v*TS;
-    
     if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) {                // WAARDES VINDEN 0.8726 (50 graden)
         q1_v = 0;
         q2_v = 0;
@@ -164,45 +192,66 @@
         q1_v = 0;
         q2_v = 0;
     }
+    
+    q1_ref = q1 + q1_v*TS;
+    q2_ref = q2 + q2_v*TS;
 }
 
-BiQuad pidf_M1;
-BiQuad pidf_M2;
-Ticker PIDcontrol_M1;
-Ticker PIDcontrol_M2;
-volatile bool go_flag_M1_controller = false;
-volatile bool go_flag_M2_controller = false;
+void PID(float q1,float q1_ref,float q2,float q2_ref,float TS,float &motorValue1Out, float &motorValue2Out)
+{
+    // linear feedback control
+    float q1_error = q1_ref - q1; //referencePosition1 - Position1;             // proportional angular error in radians
+    float q2_error = q2_ref - q2; //referencePosition1 - Position1;             // proportional angular error in radians
+    float Kp = 1;
 
-void flag_M1_controller()
-{
-    go_flag_M1_controller = true;
+    float q1IntError = q1IntError + q1_error*TS;             // integrated error in radians
+    float q2IntError = q2IntError + q2_error*TS;             // integrated error in radians
+    float Ki = 0.1;
+    
+    float q1DerivativeError = (q1_error - q1_error_prev)/TS;  // derivative of error in radians
+    float q2DerivativeError = (q2_error - q2_error_prev)/TS;  // derivative of error in radians
+    float Kd = 0.0;
+    
+    TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd;         //total controller output = motor input
+    TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd;         //total controller output = motor input
+    
+    if (TotalError1 < 0) {
+        TotalError1=0;
+        }
+    else {
+        TotalError1=TotalError1;
+        }
+    if (TotalError2 < 0) {
+        TotalError2=0;
+        }
+    else {
+        TotalError2=TotalError2;
+        }
+    
+    motorValue1Out = TotalError1/MotorGain;       
+    motorValue2Out = TotalError2/MotorGain;
+    
+    q1_error_prev = q1_error;
+    q2_error_prev = q2_error;
 }
 
-void flag_M2_controller()
+void Controller()
 {
-    go_flag_M2_controller = true;
-}
-
-void M1_controller()
-{
-    ctrlOutput_M1 = pidf_M1.step(q1_ref-q1);
-
+    PID(q1,q1_ref,q2,q2_ref,TS,motorValue1Out,motorValue2Out);
+    ctrlOutput_M1 = motorValue1Out;
+    ctrlOutput_M2 = motorValue2Out;
+    
     if (ctrlOutput_M1 < 0) {
         dir_M1 = 1;
     } else {
         dir_M1 = 0;
     }
     pwm_M1 = abs(ctrlOutput_M1);
-    if (pwm_M1 < 0) {
+    if (pwm_M1 <= 0) {
         pwm_M1 = 0;
-    //} else {
-    //    pwm_M1 = pwm_M1 + 0.1;
+    } else {
+        pwm_M1 = pwm_M1 + 0.05;
     }
-}
-
-void M2_controller()
-{
-    ctrlOutput_M2 = pidf_M2.step(q2_ref-q2);
 
     if (ctrlOutput_M2 < 0) {
         dir_M2 = 1;
@@ -210,10 +259,10 @@
         dir_M2 = 0;
     }
     pwm_M2 = abs(ctrlOutput_M2);
-    if (pwm_M2 < 0) {
+    if (pwm_M2 <= 0) {
         pwm_M2 = 0;
-    //} else {
-    //    pwm_M2 = pwm_M2 + 0.1;
+    } else {
+        pwm_M2 = pwm_M2 + 0.05;
     }
 }
 
@@ -222,8 +271,6 @@
     pc.baud(115200);
     wheel_M1.reset();
     wheel_M2.reset();
-    pidf_M1.PIDF(M1_Kp,M1_Ki,M1_Kd,N,TS);
-    pidf_M2.PIDF(M2_Kp,M2_Ki,M2_Kd,N,TS);
     knop_biceps.rise(&biceps);
     knop_triceps.rise(&triceps);
     knop_switch.rise(&switcher);
@@ -231,21 +278,15 @@
     // flag functions/tickers
     update_encoder_ticker.attach(&flag_update_encoder, TS);
     update_ref_ticker.attach(&flag_update_ref, TS);
-    PIDcontrol_M1.attach(&flag_M1_controller, TS);
-    PIDcontrol_M2.attach(&flag_M2_controller, TS);
-    begin_waarde_q.attach(&begin_waarde, 3);
+    // initialize -> beginposities
+    initialize();
     
-    pc.printf("pidf_M1 is %s", pidf_M1.stable() ? "stable" : "unstable");
-    pc.printf("pidf_M2 is %s", pidf_M2.stable() ? "stable" : "unstable");
+    if (active_PID_ticker == true) {
+    PIDcontrol.attach(&flag_controller, TS);
+    }
     
     while(1) {
-
-        // initialize function
-        initialize();
-        if (go_flag_initialize == true) {
-            go_flag_initialize = false;
-            initialize();
-        }
+        
         // update encoder
         if (go_flag_update_encoder == true) {
             go_flag_update_encoder = false;
@@ -256,15 +297,10 @@
             go_flag_update_ref = false;
             update_ref();
         }
-        // controller M1
-        if (go_flag_M1_controller == true) {
-            go_flag_M1_controller = false;
-            M1_controller();
-        }
-        // controller M2
-        if (go_flag_M2_controller == true) {
-            go_flag_M2_controller = false;
-            M2_controller();
+        // controller M1+M2
+        if (go_flag_controller == true) {
+            go_flag_controller = false;
+            Controller();
         }
     }
 }
\ No newline at end of file