Implemented Pyramid Collisions to robot movement and added PID reset type stop for hard collisions