Diff: main.cpp

Revision:

5:931594a366b7

Parent:

4:30d8610b63a6

Child:

6:6bc6ce1fe94e

--- a/main.cpp	Mon Oct 17 14:17:27 2016 +0000
+++ b/main.cpp	Wed Oct 19 12:21:58 2016 +0000
@@ -2,6 +2,7 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 #include "math.h"
+#include "HIDScope.h"
 
 //Defining ports
 DigitalOut motor1DirectionPin (D4);
@@ -10,6 +11,7 @@
 AnalogIn potmeter(A0);
 Serial pc(USBTX,USBRX);
 QEI encoder(D12,D13,NC,32);
+HIDScope scope(1);
 
 // Setting tickers and printers
 Ticker tick;
@@ -17,6 +19,9 @@
 Ticker pos;
 
 const float pi = 3.14159265359;
+const float ts = 1.0/1000.0;
+const int velocityChannel = 0;
+
 //Get reference velocity
 float GetReferenceVelocity()
 {
@@ -81,14 +86,19 @@
 
 volatile float radians;
 volatile float velocity;
+volatile float prevRadians = 0;
 
 // position and things w/ encoder & QEI
 void getPosition() {
-  const float ts = 1.0/1000.0;
   volatile int pulses = encoder.getPulses();
   radians = (pulses / (2 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton
-  volatile float prevRadians = radians;
-  velocity = (radians - prevRadians) / ts;
+  volatile float difference = radians - prevRadians;
+  velocity = difference / ts;
+  
+  scope.set(velocityChannel,velocity);
+  scope.send();
+  
+  prevRadians = radians;
   }
   
 void print() {
@@ -100,8 +110,9 @@
     
 int main()
 {
+    encoder.reset();    //not entirely sure if necessary
     motor1MagnitudePin.period(1.0/100000.0);
-    pos.attach(getPosition,1);
+    pos.attach(getPosition,ts);
     callMotor.attach(MeasureAndControl,1);
     pc.baud(115200);
     tick.attach(print,1);