Updated goal alignment and avoidance -fixed

Updated goal alignment and avoidance -fixed.
Removed back off green mat code.