Changed goal points.
Fixed plotting code.