Diff: main.cpp

Revision:

8:668afaa63c96

Parent:

7:9e965efc430e

Child:

9:182b33cabd45

--- a/main.cpp	Tue Oct 31 22:49:47 2017 +0000
+++ b/main.cpp	Wed Nov 01 00:00:23 2017 +0000
@@ -22,6 +22,7 @@
 double q1_puls;
 double q2_puls;
 int n = 0;
+double flex = 0;
 
 // kinematica gegevens
 // lengte armen
@@ -41,8 +42,8 @@
 
 
 // PID gegevens
-double pulses2rad;
-double rad2pulses;
+double pulses2rad=(2*pi)/4200;
+double rad2pulses=4200/(2*pi);
 double position;
 double ref1;
 double ref2;
@@ -57,8 +58,8 @@
 double error_I_2=0;
 double error_I2_2=0;
 
-double Kp1 = 0.0001;   // proportional coefficient motor 1
-double Ki1 = 0.0002;   // integrating  coefficient motor 1
+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
@@ -69,21 +70,26 @@
     {   key = pc.getc();
         if (key=='a')
         {
-        vx= vx + 5;        //reference wordt 500 pulses
+        vx = -20;        //reference wordt 500 pulses
         }
         else if(key=='d')
         {
-        vx= vx - 5;          //reference wordt 0 pulses
+        vx = 60;          //reference wordt 0 pulses
         }
         if (key=='w')
         {
-        vy= vy + 5;        //reference wordt 500 pulses
+        vy= 75;        //reference wordt 500 pulses
         }
         else if(key=='s')
         {
-        vy= vy -5;          //reference wordt 0 pulses
+        vy= -85;          //reference wordt 0 pulses
         }
      }
+     else
+     {
+         vx = 0;
+         vy = 0;
+         }
 }
 
 void wasd()
@@ -97,23 +103,23 @@
 
         if (key=='a')
         {
-        vx = vx - 10;     //referentie snelheid m/s
-        vy = vy;
+        vx = 45;     //referentie snelheid m/s
+        vy = 0;
         }
         else if(key=='d')
         {
-        vx = vx - 10;
-        vy = vy;
+        vx = -27;
+        vy = 0;
         }
         else if(key=='w')
         {
-        vx = vx;
-        vy = vy + 25;          
+        vx = 0;
+        vy = 75;          
         }
         else if(key=='s')
         {
-        vx = vx;
-        vy = vy - 25;          
+        vx = 0;
+        vy = -75;          
         }
         else
         {
@@ -147,11 +153,7 @@
     }
 
 void controller()
-{
-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   
+{  
     //PID 1
 error1_1 = ref1 - q1_puls;
 error_I_1 = error_I2_1 + ts*((error1_1 - error2_1)/2);
@@ -177,7 +179,6 @@
     motor1MagnitudePin = fabs(PI1);
     motor2DirectionPin = 0;
     motor2MagnitudePin = fabs(PI2);
-
     }
     else if (PI1>0 && PI2<0)
     {
@@ -218,7 +219,7 @@
      // PD controller       gebruikt PD control en stuurt motor aan
      controller();  
      if(n==500){         
-    printf("PI1 = %f     PI2 = %f\n\r", ref1, ref2);
+    printf("PI1 = %f     PI2 = %f\n\r", vx, vy);
     n=0;
     }
     else{
@@ -230,11 +231,9 @@
 int main()
 {
     pc.baud(115200);
-    pulses2rad=(2*pi)/4200;
-    rad2pulses=4200/(2*pi);
-    tick_wasd.attach_us(&toetsen, 1000);
+    tick_wasd.attach_us(&wasd, 100000);
     tick_sample.attach_us(&aansturing, 1000); //sample frequency 1000 Hz;
-    led_g = 0; 
+    led_g = 1; 
      
     while(true)
     {