Diff: main.cpp

Revision:

2:0686b2132556

Parent:

1:7969189824ff

Child:

3:3b5b85a32c9a

--- a/main.cpp	Thu Nov 02 09:20:15 2017 +0000
+++ b/main.cpp	Thu Nov 02 09:45:25 2017 +0000
@@ -27,6 +27,7 @@
 int q1_puls;
 int q2_puls;
 double rad2pulses=4200/(2*pi);
+double pulses2rad = (2*pi)/4200;
 double ts = 0.001;
 double PI1;
 int n;
@@ -41,16 +42,46 @@
 double error_I2_2 = 0;
 double PI2;
 
+// kinematica gegevens
+// lengte armen
+double L1 = 0.250;
+double L2 = 0.355;
+double L3 = 0.150;
+
+// reference position
+double q1=0;    // positie q1 in radialen
+double q2=0;    // positie q2 in radialen
+double q1_pos;
+double q2_pos;
+
+// EMG Input_k
+double vx;
+double vy;
+
+
+void kinematica()
+{
+    // encoders uitlezen    
+    q1_puls = q1_enc.getPulses();
+    q1_pos = q1_puls*pulses2rad;       // berekent positie q1 in radialen
+    q2_puls = q2_enc.getPulses();
+    q2_pos = q2_puls*pulses2rad;       // berekent positie q2 in radialen  
+    
+    vx = 4*sin( 2*t.read() );
+    vy = 0;
+    
+    q1 = ((-(L3 + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vx + ((L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * ts  + q1_pos;
+    q2 = (((L3 - L1*sin(q1_pos) + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos))*vx) +  ((L1*cos(q1_pos) - L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * ts + q2_pos;
+
+ 
+    ref1 = q1*rad2pulses;
+    ref2 = q2*rad2pulses;   
+}
+
+
 void controller()
 {   
-    refrad2 =  1.57f*sin(t.read()); //motor 2
-    ref2 = refrad2 * rad2pulses;   
-    refrad1 = -0.5f*cos(t.read()) + 0.6f; //motor 1
-    ref1 = refrad2 * rad2pulses;
-    
-    // encoders uitlezen    
-    q1_puls = q1_enc.getPulses();
-    q2_puls = q2_enc.getPulses();
+    kinematica();   
 
     //PID 1
 error1_1 = ref1 - q1_puls;
@@ -91,7 +122,7 @@
     }
     
     if(n==500){         
-    printf("error1 = %f   error2 = %f\n\r", error1_1, error1_2);
+    printf("PI1 = %f   PI2 = %f\n\r", ref1, ref2);
     n=0;
     }
     else{