Fixed bug where q_l was not set to the sensed position from encoders during first control loop (which could cause high velocities)