Added autonomous demo stuff - under testing