Some of the logic for reverse added
Robot now can reset position