Made the waypoint system work (kindof)