Feedback gains: Tilt = Tilt rate = Position = Velocity =
Tmax = dt= All estimated. Height of accelerometer above the axle =
You can change the position target, phitarget = 0

<![CDATA[ //Setup window ScaleWindow(-1,-1,tmax,1); DrawAxes(0,-1,tmax,1); xtarget=0; x=0; xest=0; v=0; vest=0; theta=0; omega=0; phi=4; phirate=0; phitarget=0; u=0; //Input motor current Colour("black"); Label("Position/10",tmax/2,.7); Colour("blue"); Label("Velocity/10",tmax/2,.6); Colour("red"); Label("Tilt*10",tmax/2,.5); Colour("green"); Label("Tilt-rate",tmax/2,0.4); Colour("gray"); Label("Drive u",tmax/2,0.3); MoveTo (t, x); Colour("black"); loop(); ]]>
//Loop window <![CDATA[ u=kphi*(phi-phitarget)+kphidot*phirate+ktheta*theta+komega*omega; Colour("black"); if (u>1){u=1; Colour("red");} //This applies the limit if (u<-1){u=-1; Colour("red");} theta=theta+omega*dt; omega=omega+(80*theta+2*phirate-50*u)*dt; phi=phi+phirate*dt phirate=phirate+(-4*phirate+100*u)*dt x=(theta+phi)*r; v=(omega+phirate)*r; t=t+dt; Colour("black"); Spot(t,x/10); Colour("blue"); Spot(t,v/10); Colour("red"); Spot(t,theta*10); Colour("green"); Spot(t,omega); Colour("gray"); Spot(t,u); if(t<tmax){setTimeout("loop();",10);} ]]>