removed driver
cleaned up code; mode swichting works fine; trajectory measurement