Revision 9:182b33cabd45, committed 2017-11-01

Comitter:

JornJan

Date:

Wed Nov 01 10:18:46 2017 +0000

Parent:

8:668afaa63c96

Commit message:

1 november;

Changed in this revision

--- a/main.cpp	Wed Nov 01 00:00:23 2017 +0000
+++ b/main.cpp	Wed Nov 01 10:18:46 2017 +0000
@@ -14,13 +14,16 @@
 QEI         q2_enc(D11, D10, NC, 32);       // encoder motor 2 instellen
 const double pi = 3.1415926535897;  // waarde voor pi aanmaken
 
+double checkm1;
+double checkm2; 
+
 // globale gegevens
 Ticker tick_sample; // ticker voor aanroepen aansturing
 Ticker tick_wasd; //ticker voor toetsenbord aansturing
 char key;
 double ts=0.001;
-double q1_puls;
-double q2_puls;
+int q1_puls;
+int q2_puls;
 int n = 0;
 double flex = 0;
 
@@ -58,39 +61,12 @@
 double error_I_2=0;
 double error_I2_2=0;
 
-double Kp1 = 0.0002;   // proportional coefficient motor 1
-double Ki1 = 0.0003;   // integrating  coefficient motor 1
-
-double Kp2 = 0.00015;   // proportional coefficient motor 2
-double Ki2 = 0.0035;   // integrating  coefficient motor 2
+double Kp1 = 0.2;   // proportional coefficient motor 1
+double Ki1 = 0.02;   // integrating  coefficient motor 1
 
-void toetsen()
-{
-    if(pc.readable()==true)  
-    {   key = pc.getc();
-        if (key=='a')
-        {
-        vx = -20;        //reference wordt 500 pulses
-        }
-        else if(key=='d')
-        {
-        vx = 60;          //reference wordt 0 pulses
-        }
-        if (key=='w')
-        {
-        vy= 75;        //reference wordt 500 pulses
-        }
-        else if(key=='s')
-        {
-        vy= -85;          //reference wordt 0 pulses
-        }
-     }
-     else
-     {
-         vx = 0;
-         vy = 0;
-         }
-}
+double Kp2 = 0.0015;   // proportional coefficient motor 2
+double Ki2 = 0.035;   // integrating  coefficient motor 2
+
 
 void wasd()
 {
@@ -103,23 +79,23 @@
 
         if (key=='a')
         {
-        vx = 45;     //referentie snelheid m/s
+        vx = 0.02;     //referentie snelheid m/s
         vy = 0;
         }
         else if(key=='d')
         {
-        vx = -27;
+        vx = -0.02;
         vy = 0;
         }
         else if(key=='w')
         {
         vx = 0;
-        vy = 75;          
+        vy = 0.02;          
         }
         else if(key=='s')
         {
         vx = 0;
-        vy = -75;          
+        vy = -0.02;          
         }
         else
         {
@@ -148,7 +124,7 @@
     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;
+    ref1 = q1*rad2pulses;   // converteert de radialen weer terug naar pulses voor verwerking in PID
     ref2 = q2*rad2pulses;
     }
 
@@ -156,26 +132,54 @@
 {  
     //PID 1
 error1_1 = ref1 - q1_puls;
-error_I_1 = error_I2_1 + ts*((error1_1 - error2_1)/2);
+error_I_1 = error_I2_1 + ts*((error1_1 + error2_1)/2);
 
-PI1 = Kp1*error1_1 + Ki1*(error_I_1);
+PI1 = Kp1*error1_1 + Ki1*error_I_1;
 
-error2_1   = error1_1; 
+error2_1   = error1_1; // opslaan variabelen voor integraal onderdeel
 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);
+error_I_2 = error_I2_2 + ts*((error1_2 + error2_2)/2);
 
 PI2 = Kp2*error1_2 + Ki2*(error_I_2);
 
-error2_2   = error1_2; 
+error2_2   = error1_2; // opslaan variabelen voor integraal onderdeel
 error_I2_2 = error_I_2;
 
     //Motor control
-    if (PI1<0 && PI2<0)
+    if (PI1==0 || PI2==0)
     {
-    motor1DirectionPin = 0;
+        if (PI1<=0)
+        {
+            motor1DirectionPin = 0;
+            motor1MagnitudePin = fabs(PI1);
+            motor2MagnitudePin = 0;
+        }
+        else if (PI1>0)
+        {
+            motor1DirectionPin = 1;
+            motor1MagnitudePin = fabs(PI1);
+            motor2MagnitudePin = 0;
+        }
+        else if (PI2<=0)
+        {
+            motor2DirectionPin = 0;
+            motor2MagnitudePin = fabs(PI2);
+            motor1MagnitudePin = 0;
+        }
+        else if  (PI2>0)
+        {
+            motor2DirectionPin = 1;
+            motor2MagnitudePin = fabs(PI2);
+            motor1MagnitudePin = 0;
+        }
+    }
+                      
+    else if (PI1<0 && PI2<0)
+    {
+    motor1DirectionPin = 1;
     motor1MagnitudePin = fabs(PI1);
     motor2DirectionPin = 0;
     motor2MagnitudePin = fabs(PI2);
@@ -198,11 +202,14 @@
     
     else if (PI1>0 && PI2>0)
     {
-    motor1DirectionPin = 1;
+    motor1DirectionPin = 0;
     motor1MagnitudePin = fabs(PI1);
     motor2DirectionPin = 1;
     motor2MagnitudePin = fabs(PI2);
     }
+    
+    checkm1 = motor1MagnitudePin;
+    checkm2 = motor2MagnitudePin;
 }
 
 void aansturing()
@@ -218,8 +225,9 @@
                  
      // PD controller       gebruikt PD control en stuurt motor aan
      controller();  
+     
      if(n==500){         
-    printf("PI1 = %f     PI2 = %f\n\r", vx, vy);
+    printf("checkm1 = %f    checkm2 = %f\n\r", ref1, ref2);
     n=0;
     }
     else{