void TestBGrabber()
		{
			//ROLLERS	
			if (m_operator->GetRawAxis(TRIGGERS) > 0.4) {
				m_roller->Set(0.5);
			}
			else if (m_operator->GetRawAxis(TRIGGERS) < -0.4)
				m_roller->Set(0.5);
			else {
				m_roller->Set(0.0);
			}	

			//BALL CATCH (#Sweg)
			if (m_operator->GetRawButton(BUTTON_A)) {
				m_catch->Set(true); 
			}
			else if (m_operator->GetRawButton(BUTTON_B)) {
				m_catch->Set(false);
			}

			//bArm OPEN / CLOSE
			if (m_operator->GetRawButton(BUTTON_X)) {
				m_bArm->Set(true);
			}
			else if (m_operator->GetRawButton(BUTTON_Y)) {
				m_bArm->Set(false);
			}
		}
	void stopDriving()
	{
		leftFront.Set(0);
		leftBack.Set(0);
		rightFront.Set(0);
		rightBack.Set(0);
	}
	//Grab first two and turn to go right
	void AutonomousType10() {
		SmartDashboard::PutString("STATUS:", "STARTING AUTO 10");
		robotDrive.MecanumDrive_Cartesian(0, -0.2, 0);
		if (WaitF(1.2))
			return;
		robotDrive.MecanumDrive_Cartesian(0, 0, 0);
		chainLift.SetSpeed(0.5);
		while (IsAutonomous() && maxUp.Get() && midPoint.Get()) {
		}
		chainLift.SetSpeed(0);
		robotDrive.MecanumDrive_Cartesian(0, 0.4, 0);
		if (WaitF(1.6))
			return;

		robotDrive.MecanumDrive_Polar(0, 0, 0.3);
		if (WaitF(2.6))
			return;

		robotDrive.MecanumDrive_Cartesian(0, -0.4, 0);
		if (WaitF(1))
			return;

		robotDrive.MecanumDrive_Polar(0, 0, -0.3);
		if (WaitF(2.6))
			return;

		robotDrive.MecanumDrive_Cartesian(0, -0.4, 0);
		if (WaitF(1.6))
			return;
		robotDrive.MecanumDrive_Cartesian(0, 0, 0);
		SmartDashboard::PutString("STATUS:", "AUTO 10 COMPLETE");
	}
	//Choose which auto to use
	void AutonomousInit() {

		chainLift.SetSpeed(0);
		canGrabber.SetSpeed(0);
		robotDrive.MecanumDrive_Cartesian(0, 0, 0);
		SmartDashboard::PutString("STATUS:", "STARTING AUTO");
		robotDrive.SetSafetyEnabled(false);
		chainLift.SetSafetyEnabled(false);

		SmartDashboard::PutBoolean("Auto switch A: ", autoSwitch1.Get());
		SmartDashboard::PutBoolean("Auto switch B: ", autoSwitch2.Get());

		//Select auto type
		if (autoSwitch1.Get()) {
			if (autoSwitch2.Get())
				AutonomousType4();
			else
				//1 on 2 grab n back
				AutonomousType8();
		} else {
			if (autoSwitch2.Get())
				//1 off, 2 on: grab n turn
				AutonomousType10();
			else {
				SmartAutoPicker();
			}
			//Do Nothing
		}
	}
	void AutonomousType4() {
		SmartDashboard::PutString("STATUS:", "STARTING AUTO 4");

		//Lift, turn, drive
		chainLift.SetSpeed(0.5);
		while (midPoint.Get() && maxUp.Get()) {
		}
		chainLift.SetSpeed(0);
		robotDrive.MecanumDrive_Polar(0, 0, 0.3);

		if (WaitF(2.5))
			return;
		robotDrive.MecanumDrive_Polar(0.25, 0, 0);
		if (WaitF(5.6))
			return;
		robotDrive.MecanumDrive_Polar(0, 0, 0.3);
		if (WaitF(2))
			return;
		robotDrive.MecanumDrive_Polar(0, 0, 0);
		//chainLift.SetSpeed(-0.4);
		//while (maxDown.Get() && IsAutonomous()) {
		//}
		chainLift.SetSpeed(0);
		robotDrive.MecanumDrive_Polar(0, 0, 0);
		SmartDashboard::PutString("STATUS:", "AUTO 4 COMPLETE");
	}
Exemple #6
0
	void TeleopPeriodic()
	{

		rightDrive = rightStick->GetY();
		leftDrive  = leftStick->GetY();
		rightDrive = .6*rightDrive;
		leftDrive  = .6*leftDrive;
		robotDrive->TankDrive(rightDrive, leftDrive);
		ax = accel-> GetX();
		ay = accel-> GetY();
		az = accel-> GetZ();
		SmartDashboard::PutData("Auto Modes", chooser);
		SmartDashboard::PutNumber("ax",ax);
		SmartDashboard::PutNumber("ay",ay);
		SmartDashboard::PutNumber("az",az);
		bool triggerRight = rightStick->GetRawButton(1);
		bool triggerLeft = leftStick->GetRawButton(1);
		SmartDashboard::PutBoolean("trigger", triggerRight);
		SmartDashboard::PutBoolean("trigger", triggerLeft);
		if(triggerRight || triggerLeft){
			pickup->Set(.3);
		}
		else{
			pickup->Set(0);
		}
	}
	void driveBackward() //Creates a function to drive backward (sets it)
	{
		leftFront.Set(-0.75);
		leftBack.Set(-0.75);
		rightFront.Set(-0.75);
		rightBack.Set(-0.75);
	}
	void driveForward() //Creates a function to drive forward (sets it)
	{
		leftFront.Set(0.75);
		leftBack.Set(0.75);
		rightFront.Set(0.75);
		rightBack.Set(0.75);
	}
	/**** Yo yo yo the right side motors are inverted, so a positive left motor (probably) moves forward and a positive right motor (probably) moves backward ****/
	void stopDriving() // Stops drive motors
	{
		leftFront.Set(0);
		leftBack.Set(0);
		rightFront.Set(0);
		rightBack.Set(0);
	}
	void driveForward() //Drives forward
	{
		leftFront.Set(justIncaseMyPolarityDunGoofed*(1 - (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be positive
		leftBack.Set(justIncaseMyPolarityDunGoofed*(1 - (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be positive
		rightFront.Set(justIncaseMyPolarityDunGoofed*(-1 + (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be negative
		rightBack.Set(justIncaseMyPolarityDunGoofed*(-1 + (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be negative
	}
	void TestRamMotion()
	{
		if (m_driver->GetRawButton(BUTTON_LB))
			m_ramMotor->Set(.8);
		else if (m_driver->GetRawButton(BUTTON_RB))
			m_ramMotor->Set(-.2);
		else
			m_ramMotor->Set(0);
	}
	void autonomousCatapultRelease()
	{
		if ((leftLimitSwitch.Get()== 0 || rightLimitSwitch.Get()== 0) && winchMotor.Get() == 0)
		{
			stopDriving(); // Stops the drive so the robot doesn't flip on itself or something
			winchMotor.Set(0); // Redundant line for extra safety that can be removed after testing (The winch should already be off)
			dogSolenoid.Set(DoubleSolenoid::kReverse); // Brings the pneumatic piston backward to disengage the dog gear
			Wait(0.2); // Giving the pistons time to disengage properly
			ratchetSolenoid.Set(DoubleSolenoid::kReverse); // Brings the pneumatic piston backward to disengage the ratchet
			Wait(5); // Waits 5 seconds after shooting before starting to load the catapult
		}
	}
	//Grab first yellow, back up to auto zone, DON'T DROP
	void AutonomousType12() {
		SmartDashboard::PutString("STATUS:", "STARTING AUTO 12");
		chainLift.SetSpeed(0.5);
		while (IsAutonomous() && IsEnabled() && maxUp.Get() && midPoint.Get()) {
		}
		chainLift.SetSpeed(0);
		robotDrive.MecanumDrive_Cartesian(0, 0.4, 0);
		if (WaitF(3))
			return;
		robotDrive.MecanumDrive_Cartesian(0, 0, 0);
		SmartDashboard::PutString("STATUS:", "AUTO 12 COMPLETE");
	}
	//Steal cans
	void AutonomousType13() {
		SmartDashboard::PutString("STATUS:", "STARTING AUTO 13");
		robotDrive.MecanumDrive_Cartesian(0, 0.2, 0);
				if (WaitF(1.2))
					return;
				robotDrive.MecanumDrive_Cartesian(0, 0, 0);
		canGrabber.SetSpeed(1);
		if (WaitF(4))
			return;
		canGrabber.SetSpeed(0);
		LinearAcceleration(1, 0, 1, 0);
		SmartDashboard::PutString("STATUS:", "AUTO 13 COMPLETE");
	}
Exemple #15
0
	virtual void TeleopPeriodic() {

		rightDrive->SetSpeed(-(Driver->GetRawAxis(2)));
		leftDrive->SetSpeed((Driver->GetRawAxis(5)));


		shooterFWD->SetSpeed(-(Operator->GetRawAxis(2)));
		shooterRear->SetSpeed(-(Operator->GetRawAxis(2)));


		//shoioter angle
		if(Operator->GetRawButton(5))
		{
			cout<<"Relay 1 forward"<<endl;
			shooterAngle->Set(Relay::kForward);
		}

		if(Operator->GetRawButton(6))
		{
			cout<<"Relay 1 Reverse"<<endl;
			shooterAngle->Set(Relay::kReverse);
		}


		//Fire button
		if(Operator->GetRawButton(1))
		{
			cout<<"Relay 1 forward"<<endl;
			shooterFire->Set(Relay::kForward);
		}

		if(Operator->GetRawButton(2))
		{
			cout<<"Relay 1 Reverse"<<endl;
			shooterFire->Set(Relay::kReverse);
		}

		if(CompressorSwitch->Get() == 0){
			CompressorRelay->Set(Relay::kForward);
		}else{
			CompressorRelay->Set(Relay::kOff);
		}

		//if(canPDP == 0){
		//	cout << "NULL" << endl;
		//}else{
			//canPDP->GetVoltage(Voltage) ;
			//cout << "0" << endl;
		//}

	}
void RobotDemo::spinnerRight(float speed) {
	float sec = 0.0;
	float currentMotorPower = 0.0;
	
	if(speed<0.05 && speed>-0.05){
		stopSpinnersRight();
	}
	else if(speed>0){
		while(currentMotorPower<speed){
			spinnerR1->Set(currentMotorPower);
			spinnerR2->Set(currentMotorPower);
			sec += 0.1;
			currentMotorPower = ramp(sec);
		}
		spinnerR1->Set(speed); //may need to
		spinnerR2->Set(speed); //reverse these
	}
	else{
		while(currentMotorPower<abs(speed)){
			spinnerR1->Set(-currentMotorPower);
			spinnerR2->Set(-currentMotorPower);
			sec += 0.3;
			currentMotorPower = ramp(sec);
		}
		spinnerR1->Set(speed); //may need to
		spinnerR2->Set(speed); //reverse these
	}
	
}
	void AutonomousType3() {		//Grab a bin/trash bin, and move forward
		SmartDashboard::PutString("STATUS:", "STARTING AUTO 3");
		chainLift.SetSpeed(0.5);
		while (midPoint.Get() && maxUp.Get()) {
		}
		chainLift.SetSpeed(0);
		robotDrive.MecanumDrive_Cartesian(0, -0.75, 0);
		if (WaitF(1.75))
			return;
		robotDrive.MecanumDrive_Cartesian(0, 0, 0);
		chainLift.SetSpeed(-0.5);
		while (maxDown.Get()) {
		}
		chainLift.SetSpeed(0);
		SmartDashboard::PutString("STATUS:", "AUTO 3 COMPLETE");
	}
Exemple #18
0
	/**
	 * Runs the motors with arcade steering.
	 */
	void OperatorControl()
	{
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl() && IsEnabled())
		{
			myRobot.ArcadeDrive(joystick); // drive with arcade style (use right stick)

			if(joystick.GetRawButton(1)) {
				crochets.Set(-0.6);
			}
			else if(joystick.GetRawButton(2)) {
				crochets.Set(0.6);
			}

			Wait(0.005);				// wait for a motor update time
		}
	}
 void Reset() {
   m_talonCounter->Reset();
   m_victorCounter->Reset();
   m_jaguarCounter->Reset();
   m_talon->Set(0.0f);
   m_victor->Set(0.0f);
   m_jaguar->Set(0.0f);
 }
	void AutonomousType2() {		//Pick tote and bin, move to auto zone
		SmartDashboard::PutString("STATUS:", "STARTING AUTO 2");
		chainLift.SetSpeed(0.5);
		while (midPoint.Get() && maxUp.Get()) {
		}
		chainLift.SetSpeed(0);
		robotDrive.MecanumDrive_Polar(0.3, 0, 0);
		if (WaitF(1.6))
			return;
		robotDrive.MecanumDrive_Polar(0, 0, 0);
		chainLift.SetSpeed(-0.2);
		if (WaitF(0.8))
			return;
		chainLift.SetSpeed(0);
		robotDrive.MecanumDrive_Polar(-0.3, 0, 0);
		if (WaitF(1.6))
			return;
		robotDrive.MecanumDrive_Polar(0, 0, 0);
		chainLift.SetSpeed(-0.3);
		while (maxDown.Get()) {
		}
		chainLift.SetSpeed(0);
		robotDrive.MecanumDrive_Polar(0.2, 0, 0);
		if (WaitF(2))
			return;
		robotDrive.MecanumDrive_Polar(0, 0, 0);
		chainLift.SetSpeed(0.4);
		while (midPoint.Get() && maxUp.Get()) {
		}
		chainLift.SetSpeed(0);
		//turn 90 deg
		robotDrive.MecanumDrive_Polar(0, 0, -0.3);
		if (WaitF(4))
			return;
		robotDrive.MecanumDrive_Polar(0.5, 0, 0);
		if (WaitF(2.5))
			return;
		robotDrive.MecanumDrive_Polar(0, 0, 0);
		chainLift.SetSpeed(-0.4);
		while (maxDown.Get() && IsAutonomous()) {
		}
		chainLift.SetSpeed(0);
		SmartDashboard::PutString("STATUS:", "AUTO 2 COMPLETE");

	}
Exemple #21
0
	/**
	 * Runs the motor from the output of a Joystick.
	 */
	void OperatorControl() {
		while (IsOperatorControl() && IsEnabled()) {
			// Set the motor controller's output.
			// This takes a number from -1 (100% speed in reverse) to +1 (100% speed forwards).
			m_motor.Set(m_stick.GetY());

			Wait(kUpdatePeriod); // Wait 5ms for the next update.
		}
	}
	void turnLeftMore() //Creates a function to turn the robot a larger set amount left and then stops movement.
	{
		leftFront.Set(justIncaseMyPolarityDunGoofed*(-1 + (autonomousFastDriveSpeedMultiplier))); // Probably needs to be negative
		leftBack.Set(justIncaseMyPolarityDunGoofed*(-1 + (autonomousFastDriveSpeedMultiplier))); // Probably needs to be negative
		rightFront.Set(justIncaseMyPolarityDunGoofed*(-1 + (autonomousFastDriveSpeedMultiplier))); // Probably needs to be negative
		rightBack.Set(justIncaseMyPolarityDunGoofed*(-1 + (autonomousFastDriveSpeedMultiplier))); // Probably needs to be negative
		Wait(1);
		leftFront.Set(0);
		leftBack.Set(0);
		rightFront.Set(0);
		rightBack.Set(0);
	}
	void turnRight() //Creates a function to turn the robot a set amount right and then stops movement
	{
		leftFront.Set(justIncaseMyPolarityDunGoofed*(1 - (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be positive
		leftBack.Set(justIncaseMyPolarityDunGoofed*(1 - (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be positive
		rightFront.Set(justIncaseMyPolarityDunGoofed*(1 - (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be positive
		rightBack.Set(justIncaseMyPolarityDunGoofed*(1 - (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be positive
		Wait(0.75);
		leftFront.Set(0);
		leftBack.Set(0);
		rightFront.Set(0);
		rightBack.Set(0);
	}
Exemple #24
0
inline void grabberChuteTaskFunc(uint32_t joystickPtr, uint32_t grabTalonPtr, uint32_t grabOuterLimitPtr, uint32_t grabInnerLimitPtr, uint32_t pdpPtr, uint32_t isGrabbingPtr...) {//uint is a pointer and not an integer
	SmartDashboard::PutBoolean("Breakpoint 0", true);
	Wait(.5);

	Joystick *joystick = (Joystick *) joystickPtr;//initializes objects from pointers
	Talon *grabTalon = (Talon *) grabTalonPtr;
	Switch *grabInnerLimit = (Switch *) grabInnerLimitPtr;
	Switch *grabOuterLimit = (Switch *) grabOuterLimitPtr;
	PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr;
	bool *isGrabbing = (bool *) isGrabbingPtr;
	Timer timer;
	timer.Start();

	SmartDashboard::PutBoolean("Breakpoint 1", true);
	Wait(.5);

	*isGrabbing = true;//tells robot.cpp that thread is running

	SmartDashboard::PutBoolean("Breakpoint 2", true);
	Wait(.5);

	while (grabOuterLimit->Get() && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//starts to spin motor to pass startup current
		//grabTalon->Set(1);//move in
	}

	SmartDashboard::PutBoolean("Breakpoint 3", true);
	Wait(.5);

	timer.Reset();

	while (timer.Get() < Constants::grabChuteTime && grabInnerLimit->Get() && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it hasn't reached the current cutoff, hit a limit switch, or been cancelled
		//grabTalon->Set(1);
		SmartDashboard::PutNumber("Current",pdp->GetCurrent(Constants::grabPdpChannel));//displays current on SmartDashboard
		Wait(.5);
	}

	SmartDashboard::PutBoolean("Breakpoint 4", true);
	Wait(.5);

	grabTalon->Set(0);//stop moving
	timer.Stop();
	*isGrabbing = false;//tells that thread is over
}
	void turnRight() //Creates a function to turn the robot a set amount right and then stops movement
	{
		leftFront.Set(0.5);
		leftBack.Set(0.5);
		rightFront.Set(-0.5);
		rightBack.Set(-0.5);
		Wait(1.5);
		leftFront.Set(0);
		leftBack.Set(0);
		rightFront.Set(0);
		rightBack.Set(0);
	}
Exemple #26
0
	void TeleopPeriodic()
	{
		double throttle;

		bool BuddyBoxEnabled = BuddyBoxEnableChooser->GetSelected();
		bool SlaveInControl = MasterInterLink->GetCh5();
		SmartDashboard::PutBoolean( "Slave In Control", BuddyBoxEnabled ? SlaveInControl : false );
		bool SlaveControlsSpeed = SlaveSpeedControlChooser->GetSelected();

		if( BuddyBoxEnabled && SlaveInControl ) ActiveInterLink = SlaveInterLink;
		else ActiveInterLink = MasterInterLink;

		if( SlaveInControl && SlaveControlsSpeed ) throttle = SlaveInterLink->getCh6();
		else throttle = MasterInterLink->getCh6();

		double aile = ActiveInterLink->getAile();
		double elev = ActiveInterLink->getElev();
		double rudd = ActiveInterLink->getRudd();
		SmartDashboard::PutNumber( "Rudder", rudd );
		SmartDashboard::PutNumber( "Throttle", throttle );

		throScale = throttle + 1;

		double driveAngle = atan2( -aile*aileScale, elev*elevScale );
		SmartDashboard::PutNumber( "Drive Angle", driveAngle );

		double driveSpeed = sqrt( aile*aile + elev*elev );
		SmartDashboard::PutNumber( "Drive Speed", driveSpeed );

		frontLeft->Set( (float)(
				throScale * frontLeftSpeed * ( driveSpeed * sin( frontLeftAngle-driveAngle ) + ruddScale * rudd )
				) );
		backLeft->Set( (float)(
				throScale * backLeftSpeed * ( driveSpeed * sin( backLeftAngle-driveAngle ) + ruddScale * rudd )
				) );
		frontRight->Set( (float)(
				throScale * frontRightSpeed * ( driveSpeed * sin( frontRightAngle-driveAngle ) + ruddScale * rudd )
				) );
		backRight->Set( (float)(
				throScale * backRightSpeed * ( driveSpeed * sin( backRightAngle-driveAngle ) + ruddScale * rudd )
				) );
	}
	void loadCatapult()
	{
		if (buttonOne.Get()==1 && buttonTwo.Get()==1 && dogSolenoid.Get()==DoubleSolenoid::kReverse)
		{
			dogSolenoid.Set(DoubleSolenoid::kForward);
			Wait(0.5);
			ratchetSolenoid.Set(DoubleSolenoid::kForward);
			Wait(0.5);
			catapultMotor.Set(1);
		}
	}
Exemple #28
0
inline void grabberPositionTaskFunc(uint32_t joystickPtr, uint32_t grabTalonPtr, uint32_t grabInnerLimitPtr, uint32_t pdpPtr, uint32_t backOutPtr, uint32_t grabPowerPtr, uint32_t isGrabbingPtr...) {//uint is a pointer and not an integer
	Joystick *joystick = (Joystick *) joystickPtr;//initializes objects from pointers
	Talon *grabTalon = (Talon *) grabTalonPtr;
	Switch *grabInnerLimit = (Switch *) grabInnerLimitPtr;
	PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr;
	bool *isGrabbing = (bool *) isGrabbingPtr;
	bool *backOut = (bool *) backOutPtr;
	double *grabPower = (double *) grabPowerPtr;
	Timer timer;
	timer.Start();

	*isGrabbing = true;//tells robot.cpp that thread is running

	while (grabInnerLimit->Get() && timer.Get() < Constants::grabDelay) {//starts to spin motor to pass startup current
		grabTalon->Set(1);//move in
	}

	while (pdp->GetCurrent(Constants::grabPdpChannel) < *grabPower && grabInnerLimit->Get() && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it hasn't reached the current cutoff, hit a limit switch, or been cancelled
		grabTalon->Set(1);
		SmartDashboard::PutNumber("Current",pdp->GetCurrent(Constants::grabPdpChannel));//displays current on SmartDashboard
	}

	if (*backOut) {
		grabTalon->Set(0);//stop moving
		timer.Reset();
		while (timer.Get() < Constants::liftBackoutTime && joystick->GetRawButton(Constants::pickupCancelButton) == false) {
			grabTalon->Set(-.75);
		}
	}

	grabTalon->Set(0);//stop moving
	timer.Stop();
	*isGrabbing = false;//tells that thread is over
}
	RobotDemo():
		logitech(1), // Joystick Port [Laptop USB]
		leftFront(1), leftBack(3), rightFront(2), rightBack(4), // Tank Drive Ports [PWMOUT]
		retrievalMotor(6), winchMotor(5), // Retrieval and Catapult Winch Motor Ports [PWMOUT]
		compressor(5,2), // Compressor connection ports [Relay, Digital IO]
		leftArmSolenoid(5,6), // Left Arm Solenoid ports [Solenoid Breakout, Solenoid Breakout]
		rightArmSolenoid(7,8), //  Arm Solenoid ports [Solenoid Breakout, Solenoid Breakout]
		dogSolenoid(1,2), // Dog Solenoid ports [Solenoid Breakout, Solenoid Breakout]
		ratchetSolenoid(3,4), // Ratchet Solenoid ports [Solenoid Breakout, Solenoid Breakout]
		leftLimitSwitch(8), rightLimitSwitch(9), // Catapult Limit Switch ports [Digital IO]
		/****** AUTONOMOUS ******/
		leftPositionSwitch(6), // Left Autonomous Switch [Digital IO]
		rightPositionSwitch(7), // Right Autonomous Switch [Digital IO]
		ringLight(1, Relay::kBothDirections), // Ringlight [Relay]
		camera(AxisCamera::GetInstance("10.28.53.11")) //IP address of the camera
		{
			//Expirations can be put here. WindRiver gets mad if you remove these braces.
			camera.WriteResolution(AxisCamera::kResolution_320x240); // sets the camera's resolution
			rightFront.SetExpiration(0.1);
			rightBack.SetExpiration(0.1);
			leftFront.SetExpiration(0.1);
			leftBack.SetExpiration(0.1);
			winchMotor.SetExpiration(0.1);
			retrievalMotor.SetExpiration(0.1);
		}
	void CupidShuffle() {
		SmartDashboard::PutString("STATUS:", "TIME 2 GET DOWWWWWWN");
		//Tempo of song
		static double tempo = 0.41666666667;

		//Repeat # of times
		for (int j = 0; j < 10 && IsAutonomous() && IsEnabled(); j++) {
			//to the left to the left to the left to the left
			for (int k = 0; k < 4; k++) {
				robotDrive.MecanumDrive_Cartesian(-0.2, 0, 0);
				Wait(tempo);
				robotDrive.MecanumDrive_Cartesian(0, 0, 0);
				Wait(tempo);
			}

			//to the right to the right to the right
			for (int k = 0; k < 4; k++) {
				robotDrive.MecanumDrive_Cartesian(0.2, 0, 0);
				Wait(tempo);
				robotDrive.MecanumDrive_Cartesian(0, 0, 0);
				Wait(tempo);
			}

			//kick kick kick kick
			for (int k = 0; k < 4; k++) {
				chainLift.SetSpeed(0.3);
				Wait(tempo);
				chainLift.SetSpeed(-0.3);
				Wait(tempo);
			}
			chainLift.SetSpeed(0);

			//walk it by uself (turn 90)
			robotDrive.MecanumDrive_Polar(0, 0, 0.3);
			Wait(tempo * 8);
		}
		SmartDashboard::PutString("STATUS:", "GIT GUD");

	}