Diff: main.cpp

Revision:

7:1444604f10d4

Parent:

6:4d254faf2428

Child:

8:008a7bf80fa0

--- a/main.cpp	Mon Oct 24 10:50:17 2016 +0000
+++ b/main.cpp	Tue Oct 25 13:03:24 2016 +0000
@@ -12,33 +12,47 @@
 PwmOut pwm_M2 (D5);
 DigitalOut dir_M1 (D7);
 DigitalOut dir_M2 (D4);
-InterruptIn knop(SW3);
+
+DigitalOut ledg (LED_GREEN);
+DigitalOut ledr (LED_RED);
+DigitalOut ledb (LED_BLUE);
+InterruptIn knop_biceps(SW2);
+InterruptIn knop_triceps(SW3);
+InterruptIn knop_switch(D9);
 
 volatile double q1 = 0;
 volatile double q2 = 0;
-
-volatile bool go_flag_initialize = false;
-
-void flag_initialize()
-{
-    go_flag_initialize = true;
-}
-
+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;
-void initialize()
-{
-    q1_ref = 0.125; //2*3.1415 /(2*3.1415);
-    q2_ref = 0*3.1415 /(2*3.1415);
-}
+volatile double ctrlOutput_M1 = 0;
+volatile double ctrlOutput_M2 = 0;
+volatile double vx;
+volatile double vy;
+volatile bool translatie_richting = true;  //true is verticaal, false is horizontaal
 
 const double TS = 0.02;
-const double M1_Kp = 1, M1_Ki = 0.00, M1_Kd = 0;
-const double M2_Kp = 0.3, M2_Ki = 0.00, M2_Kd = 0;
-const double N = 0;
+const double M1_Kp = 0.5, M1_Ki = 0.00, M1_Kd = 0.1;        // Waardes vinden?
+const double M2_Kp = 0.5, M2_Ki = 0.00, M2_Kd = 0.1;
+const double N = 0.1;
 
-volatile double ctrlOutput_M1 = 0;
-volatile double ctrlOutput_M2 = 0;
+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();
+    }
+}
 
 Ticker update_encoder_ticker;
 volatile bool go_flag_update_encoder = false;
@@ -51,7 +65,104 @@
 {
     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\n\r",q1, q1_ref, ctrlOutput_M1,q2,q2_ref,ctrlOutput_M2);
+    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);
+}
+
+volatile bool go_flag_initialize = false;
+
+void flag_initialize()
+{
+    go_flag_initialize = true;
+}
+
+void initialize()
+{
+    q1_ref = 15*2*3.1415/360;
+    q2_ref = -30*2*3.1415/360;
+}
+
+void biceps()
+{
+    if (translatie_richting) {      // verticaal / up
+        vx = 0;
+        vy = 0.02;
+    } else {                        // horizontaal / right
+        vx = 0.02;
+        vy = 0;
+    }
+}
+
+void triceps()
+{
+    if (translatie_richting) {      // verticaal / down
+        vx = 0;
+        vy = -0.02;
+    } else {                        // horizontaal / left
+        vx = -0.02;
+        vy = 0;
+    }
+
+}
+
+void switcher()
+{
+    if ( (vx == 0) && (vy == 0) ) {
+        translatie_richting = !translatie_richting;
+    } else {
+        vx = 0;
+        vy = 0;
+    }
+
+    if (translatie_richting == 1) {
+        ledr = 1;                   // blauw - verticaal
+        ledg = 1;
+        ledb = 0;
+    } else {
+        ledr = 0;                   // rood - horizontaal
+        ledg = 1;
+        ledb = 1;
+    }
+}
+
+Ticker update_ref_ticker;
+volatile double J_1;
+volatile double J_2;
+volatile double J_3;
+volatile double J_4;
+volatile bool go_flag_update_ref = false;
+void flag_update_ref()
+{
+    go_flag_update_ref = true;
+}
+
+void update_ref()
+{
+    q1 = wheel_M1.getPulses() / (1334.355/2);     // rad
+    q2 = wheel_M2.getPulses() / (1334.355/2);
+
+    J_1 = -(l2*sin(q1 + q2))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
+    J_2 = (l2*cos(q1 + q2))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
+    J_3 = (l2*sin(q1 + q2) + l1*sin(q1))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
+    J_4 = -(l2*cos(q1 + q2) + l1*cos(q1))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
+
+    q1_v = J_1 * vx + J_2 * vy;
+    q2_v = J_3 * vx + J_4 * vy;
+
+    q1_ref = q1_ref + q1_v*TS + q1_begin;
+    q2_ref = q2_ref + q2_v*TS + q2_begin;
+    if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) {                // WAARDES VINDEN 0.8726 (50 graden)
+        q1_v = 0;
+        q2_v = 0;
+    } else if ( (q1 < -(90*2*3.1415/360)) && (q1_v < 0) ) {
+        q1_v = 0;
+        q2_v = 0;
+    } else if ( (q2 < (-140*2*3.1415/360)) && (q2_v < 0) ) {        // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452
+        q1_v = 0;
+        q2_v = 0;
+    } else if ( (q2 > 0) && (q2_v > 0) ) {
+        q1_v = 0;
+        q2_v = 0;
+    }
 }
 
 BiQuad pidf_M1;
@@ -102,29 +213,40 @@
     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);
+    
     // flag functions/tickers
-    update_encoder_ticker.attach(&flag_update_encoder, 0.02f);
+    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);
-    
-    // initialize function
-    knop.fall(&flag_initialize);
-    if (go_flag_initialize == true) {
-        go_flag_initialize = false;
-        initialize();
-    }
+    begin_waarde_q.attach(&begin_waarde, 3);
     
     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;
             update_encoder();
         }
+        // update joint positions/velocities
+        if (go_flag_update_ref == true) {
+            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;