void RobotInit() override {
		chooser = new SendableChooser();
		chooser->AddDefault("Low Bar Low Goal", new Option(1));
		chooser->AddObject("Low Bar High Goal", new Option(2));
		SmartDashboard::PutData("Auto", chooser);

		CameraServer::GetInstance()->SetQuality(30);
		CameraServer::GetInstance()->StartAutomaticCapture("cam0");

		lstick = new Joystick(0);
		rstick = new Joystick(1);
		controller = new Joystick(2);

		myRobot = new SigmaDrive();
		myRobot->setExpiration(0.1);

		mySword = new ShooterIntake();

		Robot *bot = this;
		Modes = new AutonomousModes(myRobot, mySword);

		Drive = new Task("Drive", driveWrapper, bot);
		Shooter_Intake= new Task("ShooterIntake", shootWrapper, bot);

		myRobot->ResetDisplacement();
		myRobot->gyro->Calibrate();
	}

	void RobotInit()
	{
		camImage = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
		binImage = imaqCreateImage(IMAQ_IMAGE_U8, 0);
		imMask = new ImageMask(camImage, binImage, imaqError = 0);

		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
		SmartDashboard::PutData("Auto Modes", chooser);

		imaqError = IMAQdxOpenCamera("cam1", IMAQdxCameraControlModeController, &session);
		if(imaqError != IMAQdxErrorSuccess){
			DriverStation::ReportError("IMAQdxOpencamera error: " + std::to_string((long)imaqError));
		}
		imaqError = IMAQdxConfigureGrab(session);
		if(imaqError != IMAQdxErrorSuccess){
			DriverStation::ReportError("IMAQdxConfigureGram error: " + std::to_string((long)imaqError));
		}
		/*
		CameraServer::GetInstance()->SetQuality(50);
		//the camera name (ex "cam0") can be found through the roborio web interface
		CameraServer::GetInstance()->StartAutomaticCapture("cam1");
		*/
		//camImage = imaqCreateImage(IMAQ_IMAGE_RGB, 0);

		//tal = new CANTalon(1);
		//banner = new DigitalInput(4);
		//banner2 = new DigitalInput(5);
		//tal->SetFeedbackDevice(CANTalon::CtreMagEncoder_Absolute);
		//x = 0;
	}

	void RobotInit()
	{
		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
		SmartDashboard::PutData("Auto Modes", chooser);
	}

	void RobotInit()
	{
		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
		SmartDashboard::PutData("Auto Modes", chooser);
		triggerPin = new DigitalOutput(8);
		echoPin = new DigitalInput(9);
	}

	void RobotInit()
	{
		joystick = new Joystick(0);
		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
		SmartDashboard::PutData("Auto Modes", chooser);
		holder=new Holder(HOLDER_GATE,HOLDER_PUSHER,REVGATELIMIT,FWDGATELIMIT,IRSENSOR);
		state = WAIT_FOR_BUTTON;
	}

 CommandBasedRobot() {
     compressor = new Compressor(PRESSURE_SWITCH_PORT, COMPRESSOR_RELAY_PORT);
     compressor->Start();
     
     driveStyle = new SendableChooser();
     driveStyle->AddDefault("Arcade", new ArcadeDrive());
     driveStyle->AddObject("Tank", new TankDrive());
     
     CommandBase::init();
 }

	void RobotInit()
	{
		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
		SmartDashboard::PutData("Auto Modes", chooser);

		imu = new ADIS16448_IMU;
		imu->Reset();
		imu->Calibrate();

	}

	void RobotInit()
	{	//pusher = 2, gate = 4
		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
		SmartDashboard::PutData("Auto Modes", chooser);
		lidar = new Lidar(I2C::kMXP, 0x62);
		leftWheel = new SRXSpeed(3, P_CONSTANT, 0.0, 0.0, 1);
		rightWheel = new SRXSpeed(1, P_CONSTANT, 0.0, 0.0, 1);
		leftWheel->SetInverted(true);
		stick = new Joystick(0);
		angle = new AngleAccelerometer(I2C::kOnboard);
		angleMotor = new CANTalon(5);
		anglePID = new PIDController(.05, 0, 0, angle, angleMotor);
		launch = new Launcher(leftWheel, rightWheel, anglePID);
	}

	void RobotInit()
	{
		manipArm = new ManipArm();
		drive = new Drive();
		manip = new Manipulator();
		// TJF: new catapult constructor needs pointer to an OperatorInterface
		//catapult = new Catapult();
		oi = new OperatorInterface();
		catapult = new Catapult(oi);
		chooser = new SendableChooser();
		autonomous = new Autonomous();

		chooser->AddDefault("Go Straight Auto", new Autonomous()); //the second parameter require constructor not a function
		chooser->AddObject("Random Auto",  new Autonomous(true));
		SmartDashboard::PutData("Autonomous Modes", chooser); //not displaying because there are no input
	}

	void RobotInit()
	{
		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
		SmartDashboard::PutData("Auto Modes", chooser);

		//Initialize the I2C connectin on address 84
		spi = new SPI(SPI::Port::kOnboardCS0); //you can change the port; kOnboardCS0-3
		spi->Init();
		spi->SetClockRate(500000);
		spi->SetMSBFirst();
		spi->SetSampleDataOnFalling();
		spi->SetClockActiveLow();
		spi->SetChipSelectActiveLow();
	}

	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
	void AutonomousInit()
	{
		autoSelected = *((std::string*)chooser->GetSelected());
		//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
		std::cout << "Auto selected: " << autoSelected << std::endl;

		rotation = 0.0;
				//*((double*)posChooser->GetSelected());

		//goal = *((std::string*)goalChooser->GetSelected());
		shoot = "No";
		//*((std::string*)shootChooser->GetSelected());

		defenseCrossed = false;
		done = false;


		std::cout << "Here" << std::endl;
		drive->SetMaxOutput(1.0);
		std::cout << "there" << std::endl;
		//Make sure to reset the encoder!
		leftEnc->Reset();
		rightEnc->Reset();
		gyro->Reset();
		autoCounter = 0;
		timer->Reset();
	}

	void RobotInit() {

		CameraServer::GetInstance()->SetQuality(50);
		CameraServer::GetInstance()->StartAutomaticCapture("cam0");

		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*) &autoNameDefault);
		chooser->AddObject(autoNameCustom, (void*) &autoNameCustom);
		SmartDashboard::PutData("Auto Modes", chooser);
		SmartDashboard::PutString("hello", "I'm here");
		//LEFTDRIVE1 = new CANTalon(3);
		//LEFTDRIVE2 = new CANTalon(2);
		//RIGHTDRIVE1 = new CANTalon(1);
		//RIGHTDRIVE2 = new CANTalon(4);
		LEFTDRIVE1 = new CANTalon(1);
		LEFTDRIVE2 = new CANTalon(4);
		RIGHTDRIVE1 = new CANTalon(3);
		RIGHTDRIVE2 = new CANTalon(2);
		TOPMOTOR1 = new CANTalon(5);

		//m_canTalonRightRear + m_canTalonRightFront = new CANTalon(RightCANWheels);
		//m_canTalonLeftRear + m_canTalonLeftFront = new CANTalon (LeftCANWheels);
		//m_PWMTalonRightRearTop + m_PWMTalonRightFrontTop = new Talon(RightWheels);
		//m_PWMTalonLeftRearTop + m_PWMTalonLeftFrontTop = new Talon(LeftWheels);

	//	m_PWMTalonRightRearTop = new Talon(8);
	//	m_PWMTalonRightFrontTop = new Talon(6);
	//	m_PWMTalonLeftRearTop = new Talon(9);
	//	m_PWMTalonLeftFrontTop = new Talon(7);



		m_lStick = new Joystick(1);
		//m_rStick = new Joystick(0);

		//drive = new RobotDrive(RightCANWheels, LeftCANWheels, RightWheels, LeftWheels);

		//drive = new RobotDrive(m_PWMTalonLeftFrontTop, m_PWMTalonLeftRearTop,m_PWMTalonRightFrontTop, m_PWMTalonRightRearTop);
		drive = new RobotDrive(LEFTDRIVE1, LEFTDRIVE2, RIGHTDRIVE1, RIGHTDRIVE2);

		//drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
		//drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, false);
		//drive->SetInvertedMotor(RobotDrive::kRearRightMotor, false);

		leftEncoder = new Encoder(0, 1);
		rightEncoder = new Encoder(2, 3);
	}

	void RobotInit()
	{
//		Auto chooser
		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
		SmartDashboard::PutData("Auto Modes", chooser);

//		Drive declarations
		frontLeftTalon = new CANTalon(frontLeftChannel);
		rearLeftTalon = new CANTalon(rearLeftChannel);
		frontRightTalon = new Victor(frontRightChannel);
		rearRightTalon = new CANTalon(rearRightChannel);
		//frontLeftTalon->SetInverted(true);
		rearLeftTalon->SetInverted(true);
		yawGyro = new ADXRS450_Gyro();
		robotDrive = new RobotDrive(frontLeftTalon, rearLeftTalon, frontRightTalon, rearRightTalon);
		flightStick = new Joystick(flightstickChannel);
		robotDrive -> SetSafetyEnabled(false);

//		Shooter variable declarations MAKE SURE YOU PLUG THEM INTO THE RIGHT PORTS WENDY
		motor1 = new Talon(8); // loook above
		motor2 = new Jaguar(4);
		motor3 = new Jaguar(5);
		shootStick = new Joystick(0);
		shooterSwitch = new DigitalInput(0);

//		Distance sensor declarations
		distanceSensor = new AnalogInput(0);
		getVcc = new AnalogInput(2);

//		Servo
		doorLift = new Servo(9);

//		Camera stuff
		camera1 = new USBCamera("cam0", false);
		camera2 = new USBCamera("cam1", false);
		camera1->OpenCamera();
		camera2->OpenCamera();
		camera1->StartCapture();
		camera2->StartCapture();

		//Camera servos
		frontCamServo = new Servo(10); // WHAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAT
		backCamServo = new Servo(11); // WHAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAT
	}

	void RobotInit()
	{
		CommandBase::init();
		chooser = new SendableChooser();
		chooser->AddDefault("Default Auto", new AutoParameterizedDriveCmd());
		//chooser->AddObject("My Auto", new MyAutoCommand());
		SmartDashboard::PutData("Auto Modes", chooser);
	}

	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
	void AutonomousInit()
	{
		autoSelected = *((std::string*)chooser->GetSelected());
		//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
		std::cout << "Auto selected: " << autoSelected << std::endl;

		if(autoSelected == autoNameCustom){
			//Custom Auto goes here
		} else {
			//Default Auto goes here
		}
	}

	virtual void RobotInit() {
		CommandBase::init();
		
		DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
		dsLCD->Clear();
		dsLCD->UpdateLCD();
		
		//Wait(0.1);

		//_autonomousCommand = new AutonomousCommand();
		_autonomousCommand = NULL;
		_teleopCommand = NULL;
		
		_moveToAndDropBridgeCmd = new MoveToAndDropBridgeCmd();
		_noOpCmd = new NoOpCmd();
		_driveInSquareCmd = new DriveInSquareCmd();
		_kinectDriveCmd = new KinectDriveCmd(CommandBase::oi->kinectLeft, CommandBase::oi->kinectRight, CommandBase::oi->kinectBridgeBtn);
		_testRobotCmd = new TestRobotCmd(CommandBase::oi->driveTrigger);
		_autoFireCmd = new AutonomousFireCmd();

		//_driveWithJoysticksCmd = new DriveWithJoysticksCmd();
		
		
		_autoChooser = new SendableChooser();
		//		_autoChooser->AddDefault("Autonomous Fire", _autoFireCmd);
		_autoChooser->AddDefault("Move To And Drop Bridge", _moveToAndDropBridgeCmd);
//		_autoChooser->AddObject("Move To And Drop Bridge", _moveToAndDropBridgeCmd);
		_autoChooser->AddObject("Autonomous Fire", _autoFireCmd);
		_autoChooser->AddObject("Drive with Kinect", _kinectDriveCmd);
		_autoChooser->AddObject("Drive In Square", _driveInSquareCmd);
		SmartDashboard::GetInstance()->PutData("AutoMenu", _autoChooser);
		
		_teleopChooser = new SendableChooser();
		_teleopChooser->AddDefault("Drive With Joysticks", _noOpCmd);
		_teleopChooser->AddObject("Test Robot", _testRobotCmd);
		SmartDashboard::GetInstance()->PutData("TeleopMenu", _teleopChooser);
//		SmartDashboard::GetInstance()->PutData("SchedulerData", Scheduler::GetInstance());
//		SmartDashboard::GetInstance()->PutData("DriveTrainSubsystem", CommandBase::driveTrainSubsystem);
//		SmartDashboard::GetInstance()->PutData("AzimuthSubsystem", CommandBase::azimuthSubsystem);
	}

	void RobotInit()
	{
		frontLeft = new Talon( frontLeftChannel );
		backLeft = new Talon( backLeftChannel );
		frontRight = new Talon( frontRightChannel );
		backRight = new Talon( backRightChannel );

		MasterInterLink = new InterLinkElite( Master_InterLink_ID );
		SlaveInterLink = new InterLinkElite( Slave_InterLink_ID );
		ds = DriverStation::GetInstance();
		lw = LiveWindow::GetInstance();

		BuddyBoxEnableChooser = new SendableChooser();
		BuddyBoxEnableChooser->AddDefault( "Buddy Box Disabled", (void*) false );
		BuddyBoxEnableChooser->AddObject( "Buddy Box Enabled", (void*) true );
		SmartDashboard::PutData( "BuddyBoxEnableChooser", BuddyBoxEnableChooser );

		SlaveSpeedControlChooser = new SendableChooser();
		SlaveSpeedControlChooser->AddDefault( "Trainer Controls Speed", (void*) false );
		SlaveSpeedControlChooser->AddObject( "Slave Controls Speed", (void*) true );
		SmartDashboard::PutData( "SlaveSpeedControlChooser", SlaveSpeedControlChooser );
	}

	void RobotInit()
	{
		constants = new Constants();
		Drivestick = new Joystick(0);
		Operatorstick = new Joystick(1);
		lw = LiveWindow::GetInstance();
		solenoid = new Solenoid(5);

		Drive = new SixWheelDrive (constants);
		ShootIntake = new ShooterIntake (constants);

		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
		SmartDashboard::PutData("Auto Modes", chooser);

		//Timer for the motion profiler
		//double CurrentTime=0;
		//double Time;
		//Timer Time;
		//RollerEncoder = new Encoder(constants->Get("RollerEncoderArmA"), constants->Get("RollerEncoderArmB"));

	}

	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
	 * below the Gyro
	 *s
	 * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
	void AutonomousInit() {
		autoSelected = *((std::string*) chooser->GetSelected());
		std::string autoSelected = SmartDashboard::GetString("Auto Selector",
				autoNameDefault);
		std::cout << "Auto selected: " << autoSelected << std::endl;


		//myRobot.SetSafetyEnabled(false);
		//myRobot.Drive(-0.5, 0.0);

		if (autoSelected == autoNameCustom) {
			//Custom Auto goes here
			//autoSpeed = 1;
			//Wait(2);
			//autoSpeed = 0;
		} else {
			//Default Auto goes here
		}
	}

	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
	void Autonomous()
	{
		std::string autoSelected = *((std::string*)chooser->GetSelected());
		//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
		std::cout << "Auto selected: " << autoSelected << std::endl;

		if(autoSelected == autoNameCustom){
			//Custom Auto goes here
			std::cout << "Running custom Autonomous" << std::endl;
			myRobot.SetSafetyEnabled(false);
			myRobot.Drive(-0.5, 1.0); 	// spin at half speed
			Wait(2.0); 				//    for 2 seconds
			myRobot.Drive(0.0, 0.0); 	// stop robot
		} else {
			//Default Auto goes here
			std::cout << "Running default Autonomous" << std::endl;
			myRobot.SetSafetyEnabled(false);
			myRobot.Drive(-0.5, 0.0); 	// drive forwards half speed
			Wait(2.0); 				//    for 2 seconds
			myRobot.Drive(0.0, 0.0); 	// stop robot
		}

	}