Added code to detect the green mat and stop

Added code to detect the green mat and stop, then kick the ball and
drive away.
Rotates to align with goal and backs off the mat

Detects the green mat and proceeds onto it for a set time.
Rotates based on the camera & first triggered green mat sensor.
Backs off until it clears the mat.
Kicks a goal.