Added setJointStates(), some reorganization
Added function for motorPower