Diff: main.cpp

Revision:

5:21e846902e3e

Parent:

4:95bf97b44237

Child:

6:c97264a28123

--- a/main.cpp	Tue Oct 31 15:41:40 2017 +0000
+++ b/main.cpp	Tue Oct 31 21:09:34 2017 +0000
@@ -10,8 +10,8 @@
 PwmOut      motor1MagnitudePin(D5);  //Motorkracht op D5 (connected op het bord)
 DigitalOut  motor2DirectionPin(D7);  //Motorrichting op D4 (connected op het bord)
 PwmOut      motor2MagnitudePin(D6);  //Motorkracht op D5 (connected op het bord)
-QEI q1_enc(D13, D12, NC, 32);       //encoder motor 1 instellen
-QEI q2_enc(D11, D10, NC, 32);       // encoder motor 2 instellen
+QEI         q1_enc(D13, D12, NC, 32);       //encoder motor 1 instellen
+QEI         q2_enc(D11, D10, NC, 32);       // encoder motor 2 instellen
 const double pi = 3.1415926535897;  // waarde voor pi aanmaken
 
 // globale gegevens
@@ -47,15 +47,20 @@
 double ref2;
 double PD1;
 double PD2;
-double error1=0;
-double error1m=0;
-double error2=0;
-double error2m=0;
+double error1_1=0;
+double error1_2=0;
+double error2_1=0;
+double error2_2=0;
+double error_I_1=0;
+double error_I2_1=0;
+double error_I_2=0;
+double error_I2_2=0;
 
-double Kp;   // proportional coefficient       (
-double Kd;   // differential coefficient
-double Ki;   // integrating  coefficient
+double Kp1;   // proportional coefficient motor 1
+double Ki1;   // integrating  coefficient motor 1
 
+double Kp2;   // proportional coefficient motor 2
+double Ki2;   // integrating  coefficient motor 2
 
 void wasd()
 {
@@ -110,11 +115,14 @@
 void kinematics()
     {
                
-    q1 = ((-(L3 + L2*cos(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vx + ((L2*sin(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vy) * ts  + q1_pos;
-    q2 = (((L3 - L1*sin(q1) + L2*cos(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1))*vx) +  ((L1*cos(q1) - L2*sin(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*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) * 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(double q1ref, double q2ref)
+void controller_oud(double q1ref, double q2ref)
     {
         // error = qset
         // referentie bepalen    
@@ -163,29 +171,51 @@
         
         
     }
-    
-void controller(double q1ref, double q2ref) //HIER WAS JE BEZIG@!#!@#!@#@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
+
+void controller()
 {
-pulses1 = q1_enc.getPulses();
-error1_1 = ref1 - pulses1;
-error1_I = error1_I2 + ts*((error1_1 - error2_1)/2);
+    //PID 1
+error1_1 = ref1 - q1_puls;
+error_I_1 = error_I2_1 + ts*((error1_1 - error2_1)/2);
+
+PI1 = Kp1*error1_1 + Ki1*(error_I_1);
+
+error2_1   = error1_1; 
+error_I2_1 = error_I_1;
+
+    //PID 2
+error1_2 = ref2 - q2_puls;
+error_I_2 = error_I2_2 + ts*((error1_2 - error2_2)/2);
+
+PI2 = Kp2*error1_2 + Ki2*(error_I_2);
 
-PI = Kp*error1 + Ki*(error_I);
+error2_2   = error1_2; 
+error_I2_2 = error_I_2;
+
+    //Motor control 1
+    if (PI1<0 &&)
+    {
+    motor1DirectionPin = 0;
+    motor1MagnitudePin = fabs(PI1);
 
-error2   = error1; 
-error_I2 = error_I;
+    }
+    else if (PI1>0)
+    {
+    motor1DirectionPin = 1;
+    motor1MagnitudePin = fabs(PI1);
+    }
 
-    //Motor control
-    if (PI<0 )
+    //Motor control 2
+    if (PI2<0 )
     {
     motor2DirectionPin = 0;
-    motor2MagnitudePin = fabs(PI);
+    motor2MagnitudePin = fabs(PI2);
 
     }
-    else if (PI>0)
+    else if (PI2>0)
     {
     motor2DirectionPin = 1;
-    motor2MagnitudePin = fabs(PI);
+    motor2MagnitudePin = fabs(PI2);
     }
 }
 
@@ -201,15 +231,7 @@
      kinematics();
                  
      // PD controller       gebruikt PD control en stuurt motor aan
-     controller(q1, q2);
-    
-    if(n==500){     
-    printf("motor1 = %f     motor2 = %f\n\r", PD1, PD2);
-    n=0;
-    }
-    else{
-             n=n+1;
-             }     
+     controller(q1, q2);    
     }
 
 
@@ -217,8 +239,7 @@
 {
     pc.baud(115200);
     pulses2rad=(2*pi)/4200;
-    Kp = 100;
-    Ki = ;
+    rad2pulses=4200/(2*pi);
     tick_sample.attach_us(&aansturing, 1000); //sample frequency 1000 Hz;
     tick_wasd.attach(&wasd, 0.1);
     led_g = 0;