Diff: main.cpp

Revision:

4:e98ad06f227c

Parent:

3:3b5b85a32c9a

Child:

5:949c2861a79b

Child:

6:f40e26afc9aa

diff -r 3b5b85a32c9a -r e98ad06f227c main.cpp
--- a/main.cpp	Thu Nov 02 13:01:45 2017 +0000
+++ b/main.cpp	Thu Nov 02 16:06:05 2017 +0000
@@ -23,12 +23,12 @@
 double ref2 = 0;
 double Kp1 = 0.002;    //kp motor 2 = 0.0001
 double Ki1 = 0;
-double Kp2 = 0.0001;
-double Ki2 = 0;
+double Kp2 = 0.0005;
+double Ki2 = 0.0005;
 int q1_puls;
 int q2_puls;
 double rad2pulses=(2100/pi);
-double pulses2rad = (2*pi)/4200;
+double pulses2rad = (pi)/2100;
 double ts = 0.001;
 double PI1;
 int n;
@@ -63,42 +63,31 @@
 void kinematica()
 {
     // encoders uitlezen    
-    q1_puls = -q1_enc.getPulses();
+    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 = 0.04f*sin( t.read()/20 );
-    vy = 0;
+    vx = oldvx + stap;
+    vy = oldvy + stap
     
-    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;   
+    q1 = 
+    q2 = 
     
-    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);
-    }
+    //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;
+  
+    
 }
 
 
 void controller()
 {     
-
+kinematica();
     //PID 1
 error1_1 = ref1 - q1_puls;
 error_I_1 = error_I2_1 + ts*((error1_1 + error2_1)/2);
@@ -111,6 +100,8 @@
     //PID 2
 error1_2 = ref2 - q2_puls;
 error_I_2 = error_I2_2 + ts*((error1_2 + error2_2)/2);
+error_I2_2 = error_I_2;
+error2_2 = error1_2; 
 
 PI2 = Kp2*error1_2 + Ki2*(error_I_2);
 
@@ -118,12 +109,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)
@@ -138,7 +129,7 @@
     }
     
     if(n==500){         
-    printf("PI1 = %f   PI2 = %f     ref1 = %f   ref2 = %f\n\r", PI1, PI2, ref1, ref2);
+    printf("q1_puls = %d   q2_puls = %d    ref1 = %f   ref2 = %f\n\r", q1_puls, q2_puls, ref1, ref2);
     n=0;
     }
     else{
@@ -147,24 +138,14 @@
 
 }
 
-void stapfunc()
-{
- if (ref1==0){
-     ref1 = 1000;
-     }
-     else if (ref1==1000)
-     {
-         ref1=0; 
-     }  
-}
 
 int main()
 {
     pc.baud(115200);
     t.start();
     aansturing.attach_us(&controller, 1000);
-    kinmat.attach(&kinematica, 0.1); 
-    //stepres.attach(&stapfunc, 3);
+    //kinmat.attach(&kinematica, 0.001); 
+    
     
     while(true){