@name Servo Template
@inputs PYR:vector
@outputs 
@persist 
@trigger 
G=entity():isConstrainedTo()

runOnTick(1)

Dir = vec(1,0,0) #orients so 0,0,0 aligns it with the world
#Q = quat(Dir:toAngle())*qRotation(vec(0,0,1),180) #X
#Q = quat(Dir:toAngle())*qRotation(vec(0,1,0),-90) #Z
#Q = quat(Dir:toAngle())*qRotation(vec(0,0,1),90)  #Y


#The axes are misaligned, P rotates on the X axis, Y on the y, R on the z
#thus It's actually Roll, Pitch, Yaw.  I cannot get it to align to PYR so.. oh well. 
Q =quat(Dir:toAngle())*
qRotation(vec(0,1,0),PYR:y())*
qRotation(vec(1,0,0),PYR:x())*
qRotation(vec(0,0,1),PYR:z())
G:applyTorque((150*G:toLocal(rotationVector(Q/quat(G))+G:pos()) - 12*G:angVelVector())*G:inertia())
