Diff: main.cpp

Revision:

5:1b8032a15afe

Parent:

4:db3e61625e18

Child:

6:84a01494d836

--- a/main.cpp	Mon Oct 10 14:00:30 2016 +0000
+++ b/main.cpp	Mon Oct 10 14:47:39 2016 +0000
@@ -1,8 +1,12 @@
 #include "mbed.h"
 #include <math.h>
 #include "MODSERIAL.h"
+#include "QEI.h"
+#include "HIDScope.h"
 
-DigitalOut led1 (D12); 
+DigitalIn encoder1A (D13); //Channel A van Encoder 1
+DigitalIn encoder1B (D12); //Channel B van Encoder 1
+DigitalOut led1 (D11); 
 DigitalOut led2 (D10);
 AnalogIn potMeterIn(PTB2);
 DigitalIn button1(D5);
@@ -10,14 +14,30 @@
 PwmOut motor1MagnitudePin(D6);
 
 Serial pc(USBTX,USBRX);
-Ticker MeasureTicker, TimeTracker;
+Ticker MeasureTicker, TimeTracker, sampleT;
+HIDScope    scope(2);
 
 float referenceVelocity = 0;
+int counts;
+double DerivativeCounts;
+int countsPrev = 0;
 
-volatile bool MeasureTicker_go=false, TimeTracker_go=false;
+volatile bool MeasureTicker_go=false, TimeTracker_go=false, sampleT_go=false;
 
 void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
 void TimeTracker_act(){TimeTracker_go=true;};
+void sampleT_act(){sampleT_go=true;};
+
+// encoder in HIDScope setten
+void sample()
+{
+    scope.set(0, counts);
+    DerivativeCounts = (counts-countsPrev)/0.001;
+    scope.set(1, DerivativeCounts);
+    countsPrev = counts; 
+    scope.send();
+}
+
 
 float GetReferenceVelocity()
 {
@@ -75,6 +95,7 @@
      float Potmeter = potMeterIn.read();
      pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
      pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
+      
 }
 
 int main()
@@ -82,23 +103,33 @@
  //Initialize
  led1=0;
  led2=0;
+ int counts;
  float Potmeter = potMeterIn.read();
  MeasureTicker.attach(&MeasureTicker_act, 0.01f); 
  TimeTracker.attach(&TimeTracker_act, 0.3f);
- pc.baud(115200);  
+ pc.baud(115200);
+ QEI Encoder(D12, D13, NC, 32); // turns on encoder
+ sampleT.attach(&sampleT_act, 0.001f);
  pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
  pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
  
  while(1)
     {
+        
         if (MeasureTicker_go){
             MeasureTicker_go=false;
             MeasureAndControl();
+            counts = Encoder.getPulses();  // gives position
+            pc.printf("Encoder counts: %i \r\n", counts); 
         }
         if (TimeTracker_go){
             TimeTracker_go=false;
             TimeTrackerF();
         }
+        if (sampleT_go){
+            sampleT_go=false;
+            sample();
+        }
     }
 }