Autonomous wind, drive, shoot implemented

TODO: Add vision hot test