Diff: main.cpp

Revision:

7:9e965efc430e

Parent:

6:c97264a28123

Child:

8:668afaa63c96

diff -r c97264a28123 -r 9e965efc430e main.cpp
--- a/main.cpp	Tue Oct 31 21:37:31 2017 +0000
+++ b/main.cpp	Tue Oct 31 22:49:47 2017 +0000
@@ -19,8 +19,8 @@
 Ticker tick_wasd; //ticker voor toetsenbord aansturing
 char key;
 double ts=0.001;
-int q1_puls;
-int q2_puls;
+double q1_puls;
+double q2_puls;
 int n = 0;
 
 // kinematica gegevens
@@ -36,8 +36,8 @@
 double q2_pos;
 
 // EMG Input_k
-double vx = 0;
-double vy = 0;
+double vx;
+double vy;
 
 
 // PID gegevens
@@ -63,6 +63,29 @@
 double Kp2 = 0.00015;   // proportional coefficient motor 2
 double Ki2 = 0.0035;   // integrating  coefficient motor 2
 
+void toetsen()
+{
+    if(pc.readable()==true)  
+    {   key = pc.getc();
+        if (key=='a')
+        {
+        vx= vx + 5;        //reference wordt 500 pulses
+        }
+        else if(key=='d')
+        {
+        vx= vx - 5;          //reference wordt 0 pulses
+        }
+        if (key=='w')
+        {
+        vy= vy + 5;        //reference wordt 500 pulses
+        }
+        else if(key=='s')
+        {
+        vy= vy -5;          //reference wordt 0 pulses
+        }
+     }
+}
+
 void wasd()
 {
     static char oldkey = 'p';
@@ -74,23 +97,23 @@
 
         if (key=='a')
         {
-        vx = 0.04;     //referentie snelheid m/s
-        vy = 0;
+        vx = vx - 10;     //referentie snelheid m/s
+        vy = vy;
         }
         else if(key=='d')
         {
-        vx = -0.04;
-        vy = 0;
+        vx = vx - 10;
+        vy = vy;
         }
         else if(key=='w')
         {
-        vx = 0;
-        vy = 0.04;          
+        vx = vx;
+        vy = vy + 25;          
         }
         else if(key=='s')
         {
-        vx = 0;
-        vy = -0.04;          
+        vx = vx;
+        vy = vy - 25;          
         }
         else
         {
@@ -125,6 +148,10 @@
 
 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);
@@ -189,7 +216,14 @@
      kinematics();
                  
      // PD controller       gebruikt PD control en stuurt motor aan
-     controller();    
+     controller();  
+     if(n==500){         
+    printf("PI1 = %f     PI2 = %f\n\r", ref1, ref2);
+    n=0;
+    }
+    else{
+             n=n+1;
+             }       
 }
 
 
@@ -198,7 +232,7 @@
     pc.baud(115200);
     pulses2rad=(2*pi)/4200;
     rad2pulses=4200/(2*pi);
-    tick_wasd.attach_us(&wasd, 1000);
+    tick_wasd.attach_us(&toetsen, 1000);
     tick_sample.attach_us(&aansturing, 1000); //sample frequency 1000 Hz;
     led_g = 0;