Robot() :
		robotDrive(Motor1, Motor2),	// these must be initialized in the same order
		stick(5),		// as they are declared above.
		lw(LiveWindow::GetInstance()),
		autoLoopCounter(0),
		Motor1(21),
		Motor2(12),
		Slave1(20),
		Slave2(14),
		t_motor(13),
		arm_Motor(23),
		finger_Motor(22),
		intake_Spin_Motor(11),
		intake_Winch_Motor(13),
		stick2(4),
		autoLoopCounter2(0)
	{
		robotDrive.SetExpiration(0.1);
		robotDrive.SetSafetyEnabled(false);
		Slave1.SetControlMode(CANSpeedController::kFollower);
		Slave1.Set(21);
		Slave2.SetControlMode(CANSpeedController::kFollower);
		Slave2.Set(12);
		Motor2.SetInverted(true); //12
		Slave2.SetInverted(true);//14
		arm_Motor.SetInverted(false);//23
		t_motor.SetInverted(true);//23
//		t_motor.SetControlMode(CANSpeedController::kVoltage);
//		t_motor.Set(0);
//		CameraServer::GetInstance()->SetQuality(50);

//		CameraServer::GetInstance()->SetSize(2);
//		//the camera name (ex "cam0") can be found through the roborio web interface
//		CameraServer::GetInstance()->StartAutomaticCapture("cam0");

		t_motor.SetControlMode(CANSpeedController::kPercentVbus);
//		t_motor.SetVoltageCompensationRampRate(24.0);
		t_motor.SetFeedbackDevice(CANTalon::QuadEncoder);
		t_motor.SetPosition(0);
//		t_motor.SetPID(1, 0, 0);
		arm_Motor.SetControlMode(CANSpeedController::kPercentVbus);
		finger_Motor.SetControlMode(CANSpeedController::kPercentVbus);
//		ourRangefinder = new AnalogInput(0);

	}
	void OperatorControl()
		{
			while (IsOperatorControl() && IsEnabled())
			{
				robotDrive.ArcadeDrive(scaler(stick.GetZ()),scaler(stick.GetY()));
				SmartDashboard::PutNumber("StickZ",stick.GetZ());
				SmartDashboard::PutNumber("StickZscaled",scaler(stick.GetZ()));


				finger_Motor.Set(scaler(stick2.GetRawAxis(thumbpadL_Y)));
//				scalerValue = stick.GetRawAxis(3);
				arm_Motor.Set(scaler(stick2.GetRawAxis(thumbpadR_Y)));
				manualShooter();
				shootingModes();
				toggleIntake();
				toggleIntakeMode();
				setScalerValue();
//				double volts = ourRangefinder->GetVoltage();
//				SmartDashboard::PutNumber("Voltage",volts);
				//t_motor.Set(stick2.GetZ());
				//t_motor.Set(stick.GetAxis(Joystick::kDefaultThrottleAxis));
//				t_motor.Set(stick.GetAxis(Joystick::kThrottleAxis));
//				finger_Motor.Set(stick2.GetAxis(Joystick::kThrottleAxis));
//				arm_Motor.Set(stick2.GetY());

		//		Current Control mode Debug
				SmartDashboard::PutNumber("Motor30 Current",t_motor.GetOutputCurrent());
				SmartDashboard::PutNumber("Position",t_motor.GetPosition());
//				t_motor.Set(stick.GetAxis(Joystick::Slider));
//				if (stick.GetRawButton(3) == true){
//					t_motor.SetPosition(10000);
//				}

//			if (stick2.GetRawButton(7) == true and buttonpress == false){
//

//									}
//			else if (stick2.GetRawButton(3) == false and buttonpress == true){
//					buttonpress = false;// drive with arcade style (use right stick)
//
//						}
			SmartDashboard::PutBoolean("buttonpress state",buttonpress);

			if (stick2.GetRawButton(buttonBack) == true){
				stateDisarmed = true;
				stateArming1 = false;
				stateArming2 = false;
				stateArmed = false;
				stateFiring1 = false;
				stateFiring2 = false;
				t_motor.SetPosition(0);

			}
			toggleIntake();
//			if (stick2.GetRawButton(5) == true){
//				intake_Spin_Motor.Set(1);
//			}
//			if (stick2.GetRawButton(5) == false){
//				intake_Spin_Motor.Set(0);
//			}
			if (stick.GetRawButton(2) == true and buttonpress2 == false){
				myServo->SetAngle(175);
			}
			if (stick.GetRawButton(2) == false and buttonpress2 == true){
				myServo->SetAngle(5);


			}
//			servoSetPos(servoState);
//			myServo->Set(.5);
			Wait(0.005);// wait for a motor update time
//			motorSetPos(launcherState);
		}
	}