Lots of code work

Got the servos running after the left one was stalling, just needed to
change the frequency driving them. Now the thing is reseting on its own,
probably noise.