Diff: main.cpp

Revision:

17:cacf9e75eda7

Parent:

16:40183eeadb6d

Child:

18:50c04dd523ea

--- a/main.cpp	Mon Sep 23 13:47:47 2019 +0000
+++ b/main.cpp	Mon Sep 23 14:21:30 2019 +0000
@@ -1,5 +1,7 @@
+#include "QEI.h"
 #include "mbed.h"
 #include "FastPWM.h"
+#include "HIDScope.h"
 #include "MODSERIAL.h"
 
 MODSERIAL pc(USBTX, USBRX);
@@ -18,6 +20,10 @@
 DigitalOut direction(D6);
 DigitalOut speed(D7);
 
+HIDScope scope(2);
+QEI encoder(D12,D13,NC,64,QEI::X4_ENCODING);
+Ticker RW_scope;
+
 // variables
 
 volatile char command = 'r';
@@ -28,6 +34,11 @@
 volatile int on_time_ms; // The time the LED should be on, in microseconds
 volatile int off_time_ms;
 
+int degrees;
+volatile double x;
+volatile double x_prev=0;
+volatile double y;
+
 int n=5;
 
 // functions
@@ -139,6 +150,17 @@
     }
 }
 
+void ReadEncoderAndWriteScope()
+{
+    degrees = ((360/64)*encoder.getPulses())/131.25; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
+    x = degrees;
+    scope.set(0,x);
+    y = (x_prev + x)/2.0;
+    scope.set(1,y);
+    x_prev=x;
+    scope.send();
+}
+
 void StateMachine(void)
 {
     switch(CurrentState) {
@@ -185,6 +207,8 @@
 int main()
 {
     pc.baud(115200);
+    pc.printf("Hello World!\n\r");
+    RW_scope.attach(&ReadEncoderAndWriteScope, 0.1);
     ReduceEmission.attach(EnergySaving,20);
     BUT1.fall(plus);
     BUT2.fall(min);