Diff: main.cpp

Revision:

0:6f89ab3c1588

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Feb 12 16:25:45 2018 +0000
@@ -0,0 +1,71 @@
+#include "mbed.h"
+#include "QEI.h"
+//#include "mbedWSEsbc.h"
+
+Serial pc(USBTX, USBRX);
+QEI encoder(p16, p17, NC, 3200);
+
+Ticker Tocker;
+float TotalTime;
+float Time;
+float dt = 0.005;
+float refresh = 0.01;
+
+float theta;
+float omega;
+int ticks;
+
+float theta_p;
+float d_theta;
+
+float t2d = 0.1125;
+
+//Declare functions
+void velocity();
+
+int main()
+{
+    int go = 0;
+
+    // Set serial parameters
+    pc.baud(115200);
+    pc.format(8,SerialBase::None,1);
+
+    // Initialize time
+    Time = 0.0;
+    // Attach ticker function
+    Tocker.attach(&velocity,dt);
+
+    while(1) {
+        // Get "go" command from MATLAB
+        pc.scanf("%d,%f",&go, &TotalTime);
+        // Reset time
+        Time = 0.0;
+        if(go == 1) {
+            while(Time <= TotalTime) {
+                //pc.printf("Time: %f; Position: %f (deg); Velocity: %f (deg/s);\n", Time, theta, omega);
+                pc.printf("%f,%f,%f\n", Time, theta, omega);
+                wait(refresh);
+            }
+        }
+    }
+}
+
+void velocity()
+{
+    ticks = encoder.getPulses();
+    theta = ticks*t2d;
+    Time = Time + dt;
+    // Angle in degrees
+
+    //if (theta > 360.0) {
+    //  theta = theta - 360.0;
+    //} else if (theta < 0.0) {
+    //  theta = theta + 360.0;
+    //}
+
+    d_theta = theta - theta_p;
+    omega = d_theta/dt;
+
+    theta_p = theta;
+}
\ No newline at end of file