Diff: main.cpp

Revision:

6:f40e26afc9aa

Parent:

4:e98ad06f227c

Child:

7:5d9907aa6dbc

--- a/main.cpp	Thu Nov 02 16:06:05 2017 +0000
+++ b/main.cpp	Thu Nov 02 18:56:04 2017 +0000
@@ -68,18 +68,15 @@
     q2_puls = q2_enc.getPulses();
     q2_pos =  q2_puls*pulses2rad;       // berekent positie q2 in radialen  
     
-    vx = oldvx + stap;
-    vy = oldvy + stap
-    
-    q1 = 
-    q2 = 
+    vx= 0.012f*sin(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) * 5  + 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) * 5 + q2_pos;
+    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) * 5 *ts + q1;
+    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) * 5 * ts + q2;
 
-    //ref1 = -q1*rad2pulses;
+    ref1 = -q1*rad2pulses;
     ref2 = q2*rad2pulses;
-    //ref2 =200;
+    
   
     
 }
@@ -109,12 +106,12 @@
     if (PI1<=0)
     {
     motor1DirectionPin = 1;
-    //motor1MagnitudePin = fabs(PI1);
+    motor1MagnitudePin = fabs(PI1);
     }
     else if (PI1>0)
     {
     motor1DirectionPin = 0;
-    //motor1MagnitudePin = fabs(PI1);        
+    motor1MagnitudePin = fabs(PI1);        
     }
     
     if (PI2>=0)