Collision check working on the robot
Need to work on the logic side now