File content as of revision 0:6044f996df91:

#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;
            }
        }
    }   
}