Path following working with minor bugs