Adam Sher
/
Single_Axis
stuff n stuff
Diff: main.cpp
- Revision:
- 0:6044f996df91
diff -r 000000000000 -r 6044f996df91 main.cpp --- /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; + } + } + } +}