Diff: main.cpp

Revision:

24:d255752065d1

Parent:

21:363271dcfe1f

Child:

25:76e9e5597416

--- a/main.cpp	Mon Oct 22 14:50:31 2018 +0000
+++ b/main.cpp	Mon Oct 29 13:11:39 2018 +0000
@@ -38,12 +38,25 @@
 QEI         Encoder2(D9,D8,NC,4200);        // Counterclockwise motor rotation is the positive direction
 QEI         Encoder3(D13,D12,NC,4200);      // Counterclockwise motor rotation is the positive direction
 Ticker      motor;
+Ticker      encoders;
 
 // Global variables
-const float    pi = 3.14159265358979;
+const float     pi = 3.14159265358979;
 float u3 = 0.0;         // Normalised variable for the movement of motor 3
+const float     fCountsRad = 4200.0;
 
 // Functions
+float   AngleCalc(float counts)
+{   float angle = ((float)counts*2.0*pi)/fCountsRad;
+    while (angle > 2.0*pi)
+    {   angle = angle - 2.0*pi;
+    }
+    while (angle < -2.0*pi)
+    {   angle = angle + 2.0*pi;
+    }
+    return angle;
+}
+
 void Encoderinput()
 {   int counts1;
     int counts2;
@@ -54,11 +67,11 @@
     counts1 = Encoder1.getPulses();
     counts2 = Encoder2.getPulses();
     counts3 = Encoder3.getPulses();
-    angle1 = ((float)counts1*2.0*pi)/4200.0;
-    angle2 = ((float)counts2*2.0*pi)/4200.0;
-    angle3 = ((float)counts3*2.0*pi)/4200.0;
+    angle1 = AngleCalc(counts1);
+    angle2 = AngleCalc(counts2);
+    angle3 = AngleCalc(counts3);
          
-    pc.printf("Counts3: %i  Angle3: %f      Counts2: %i  Angle2: %f\r\n",counts3,angle3,counts2,angle2);
+    pc.printf("Counts1,2,3: %i  %i  %i      Angle1,2,3: %f  %f  %f\r\n",counts1,counts2,counts3,angle1,angle2,angle3);
 }
 
 void draaibuttons()         
@@ -73,7 +86,6 @@
             {   u3 = 1.0f;
             }
         }
-    
     else if (button1 == 0 && button2 == 1)
         {   u3 = u3 - 0.1f;
             if (u3>1.0f)
@@ -127,13 +139,9 @@
     button1.rise(&draaibuttons);       
     button2.rise(&draaibuttons);
     
-    pin3.period_us(50);
-    motor.attach(draai, 0.001);
-   
-    pin5.period_us(50);
-    motor.attach(draai, 0.001);
-    
-    pin6.period_us(50);
+    pin3.period(0.2);
+    pin5.period(0.2);
+    pin6.period(0.2);
     motor.attach(draai, 0.001);