base level code restored

+ kinematics controllers work
+ need to program better gain scheduling
+ need trajectory controller