Diff: main.cpp

Revision:

5:949c2861a79b

Parent:

4:e98ad06f227c

--- a/main.cpp	Thu Nov 02 16:06:05 2017 +0000
+++ b/main.cpp	Thu Nov 02 18:40:41 2017 +0000
@@ -15,7 +15,6 @@
 Timer t;
 Ticker aansturing;
 Ticker stepres;
-Ticker kinmat;
 
 double ref1 = 0;
 double refrad1;
@@ -33,6 +32,8 @@
 double PI1;
 int n;
 
+double stap = 0.01;
+
 double error1_1;
 double error2_1 = 0;
 double error_I_1;
@@ -56,8 +57,8 @@
 double q2_pos;
 
 // EMG Input_k
-double vx;
-double vy;
+double vx = 0;
+double vy = 0;
 
 
 void kinematica()
@@ -68,19 +69,27 @@
     q2_puls = q2_enc.getPulses();
     q2_pos =  q2_puls*pulses2rad;       // berekent positie q2 in radialen  
     
-    vx = oldvx + stap;
-    vy = oldvy + stap
+    
+    px = oldpx + stap;
+    py = oldpy + stap;
+    
+    if (px >= pxmax || px <= pxmin)
+    {
+        vx = oldvx;
+    }
+    else if (py >= pymax || py <= pymin)
+    {
+        vy = oldvy;   
+    }
     
     q1 = 
     q2 = 
+
+    ref1 = q1 * rad2pulses;
+    ref2 = q2 * rad2pulses;  
     
-    //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;
-
-    //ref1 = -q1*rad2pulses;
-    ref2 = q2*rad2pulses;
-    //ref2 =200;
-  
+    px = oldpx;
+    py = oldpy;
     
 }
 
@@ -109,12 +118,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)
@@ -144,7 +153,6 @@
     pc.baud(115200);
     t.start();
     aansturing.attach_us(&controller, 1000);
-    //kinmat.attach(&kinematica, 0.001); 
     
     
     while(true){