Revision 1.0

Still needs teleop and voltage to angle function.