Diff: main.cpp

Revision:

5:348cd7d2a094

Parent:

4:c73ced5d5754

Child:

6:ae2ae12328b7

--- a/main.cpp	Tue Sep 25 14:02:15 2018 +0000
+++ b/main.cpp	Wed Oct 10 11:56:34 2018 +0000
@@ -2,7 +2,7 @@
 #include "FastPWM.h"
 #include "MODSERIAL.h"
 #include "HIDScope.h"
-
+#include "QEI.h"
 
 AnalogIn  potmeter1(PTC10);
 AnalogIn  potmeter2(PTC11);
@@ -17,10 +17,16 @@
 // D6 for motor 2
 FastPWM motor1_pwm(D5);
 FastPWM motor2_pwm(D6);
+// For Encoder reading
+QEI Encoder1(D11, D10, NC, 4200) ;  // Encoder motor 1, (pin 1A, pin 1B, index pin(not used), counts/rev)
+QEI Encoder2(D9, D8, NC, 4200) ;    // Encoder motor 2, (pin 2A, pin 2B, index pin (not used), counts/rev)
 
 // Voor het laten zien van de data op de pc.
 HIDScope scope(2);              // Aantal kanalen wat doorgegeven wordt aan de hidscope
 Ticker AInTicker;
+Ticker  Encoder;
+
+// Declare variables for motor
 float U1;
 float U2;
 float potwaarde1;
@@ -33,11 +39,36 @@
     scope.send();               // Zendt de waardes naar de pc 
     }
 
+void ReadEncoder()
+{
+    const float pi = 3.14159265359;
+
+    // Declare variables
+    int counts1; 
+    int counts2;
+    float theta1;
+    float theta2;
+    
+    // Get counts
+    counts1 = Encoder1.getPulses();         // Counts of outputshaft of motor 1
+    counts2 = Encoder2.getPulses();         // Counts of outputshaft of motor 2
+
+    // Get angles
+    theta1 = (float(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
+    theta2 = (float(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
+
+    pc.baud(115200);
+    pc.printf("Hoek motor 1 = %0.2f, hoek motor 2 = %0.2f \n \r ", theta1, theta2);
+}
+
+
+
 int main(void)
 {
-    motor1_pwm.period_ms(60); // period is 60 ms
-    AInTicker.attach(&ReadAnalogIn,0.01f);  //Elke 0.01 sec. Lees de analoge waarde
- 
+    motor1_pwm.period_ms(60);               // Period is 60 ms
+    AInTicker.attach(&ReadAnalogIn,0.01f);  // Elke 0.01 sec. Lees de analoge waarde
+    Encoder.attach(&ReadEncoder, 1.0f);     // Lees elke 1 sec de encoder uit
+
     while(true){
         potwaarde1 = potmeter1.read();  // Lees de potwaardes uit. Tussen 0 en 1
         potwaarde2 = potmeter2.read();
@@ -53,7 +84,7 @@
         directionM1 = U1 > 0.0f; //either true or false
         directionM2 = U2 > 0.0f;
         
-        wait(0.002f);
-        }
+        wait(0.002f);   
+     }
     
 }
\ No newline at end of file