Diff: main.cpp

Revision:

0:6044f996df91

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 16 06:59:16 2015 +0000
@@ -0,0 +1,37 @@
+#include "mbed.h"
+#include "AX12.h"
+#include <cmath>
+
+Timer time;
+AX12 dyna(1,2,1);   //tx,rx,ID,Baud
+Serial comp(3,4);   //tx,rx
+AnalogIn pot(A0);   //pot pin
+
+int main()
+{
+    bool done=false;
+    float servoCmdTime=-1;
+    float degPerVolt=330/3.3;
+    comp.printf("Time(s),ServoPosition(deg),FlywheelError(deg)\n");
+    
+    dyna.SetMode(0);    //0-positional control. 1-velocity control
+    dyna.SetGoal(150,0);
+    float flyZero=pot.read()*degPerVolt;
+    
+    time.start();
+    while(!done)
+    {
+        
+        if(time.read()-servoCmdTime >= 0.001)
+        {
+            comp.printf("%.6f,%.2f,%.2f\n", time.read(), dyna.GetPosition(), pot.read()*degPerVolt-flyZero);
+            int goal=150+60*sin(2*pi()*time.read()*time.read()/10); //freq=time(s)/10
+            dyna.SetGoal(goal,1);
+            servoCmdTime=time.read();
+            if(time.read()/10 > 20)
+            {
+                done=true;
+            }
+        }
+    }   
+}