Leave green mat if align to goal takes too long

Leave green mat if align to goal takes too long.
Reverse direction of goal align if time is up.

Reverse direction of goal align if time is up.
Add check that beacon is close

Add check that beacon is close.