Fix bug in simpleteleop