Diff: main.cpp

Revision:

3:3b5b85a32c9a

Parent:

2:0686b2132556

Child:

4:e98ad06f227c

--- a/main.cpp	Thu Nov 02 09:45:25 2017 +0000
+++ b/main.cpp	Thu Nov 02 13:01:45 2017 +0000
@@ -15,6 +15,7 @@
 Timer t;
 Ticker aansturing;
 Ticker stepres;
+Ticker kinmat;
 
 double ref1 = 0;
 double refrad1;
@@ -26,7 +27,7 @@
 double Ki2 = 0;
 int q1_puls;
 int q2_puls;
-double rad2pulses=4200/(2*pi);
+double rad2pulses=(2100/pi);
 double pulses2rad = (2*pi)/4200;
 double ts = 0.001;
 double PI1;
@@ -62,26 +63,41 @@
 void kinematica()
 {
     // encoders uitlezen    
-    q1_puls = q1_enc.getPulses();
-    q1_pos = q1_puls*pulses2rad;       // berekent positie q1 in radialen
+    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  
+    q2_pos =  q2_puls*pulses2rad;       // berekent positie q2 in radialen  
     
-    vx = 4*sin( 2*t.read() );
+    vx = 0.04f*sin( t.read()/20 );
     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;
+    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;   
+    
+    if (ref1 >=0)
+    {
+        ref1 = ceil(ref1);
+    }
+    else if (ref1<0)
+    {
+        ref1 = floor(ref1);
+    }
+    if (ref1 >=0)
+    {
+        ref2 = ceil(ref2);
+    }
+    else if (ref1<0)
+    {
+        ref2 = floor(ref2);
+    }
 }
 
 
 void controller()
-{   
-    kinematica();   
+{     
 
     //PID 1
 error1_1 = ref1 - q1_puls;
@@ -113,16 +129,16 @@
     if (PI2>=0)
     {
     motor2DirectionPin = 1;
-    //motor2MagnitudePin = fabs(PI2);   
+    motor2MagnitudePin = fabs(PI2);   
     }
     else if(PI2<0)
     {
     motor2DirectionPin = 0;
-    //motor2MagnitudePin = fabs(PI2);               
+    motor2MagnitudePin = fabs(PI2);               
     }
     
     if(n==500){         
-    printf("PI1 = %f   PI2 = %f\n\r", ref1, ref2);
+    printf("PI1 = %f   PI2 = %f     ref1 = %f   ref2 = %f\n\r", PI1, PI2, ref1, ref2);
     n=0;
     }
     else{
@@ -147,6 +163,7 @@
     pc.baud(115200);
     t.start();
     aansturing.attach_us(&controller, 1000);
+    kinmat.attach(&kinematica, 0.1); 
     //stepres.attach(&stapfunc, 3);
     
     while(true){