Fix for finding not optimal goal
Fix for finding not optimal goal

FRC900/2016VisionCode,FRC900/2016VisionCode,FRC900/2016VisionCode,FRC900/2016VisionCode,FRC900/2016VisionCode,FRC900/2016VisionCode,FRC900/2016VisionCode