Exemple #1
0
	void OperatorControl()
	{
		GetWatchdog().SetEnabled(true);
		intake->LiftIntake();
		LEDLight->Set(Relay::kOff);
		while (IsOperatorControl() && !IsDisabled())
		{
			myRobot->TankDrive(leftStick, rightStick);
			rpi->Read();
			lcd->Printf(DriverStationLCD::kUser_Line1, 1, "L: %f", leftEnco->GetDistance());
			lcd->Printf(DriverStationLCD::kUser_Line2, 1, "R: %i", rpi->GetMissingPacketcount());
			lcd->Printf(DriverStationLCD::kUser_Line3, 1, "%i", rpi->GetXPos());
			lcd->Printf(DriverStationLCD::kUser_Line4, 1, "%i", catapult->GetLoadedLimit1());
			lcd->Printf(DriverStationLCD::kUser_Line5, 1, "%i", catapult->GetLoadedLimit2());
			lcd->UpdateLCD();
			//Driver controls

			//Move the intake in if the R-2 trigger is pressed
			if(rightStick->GetRawButton(2))
			{
				intake->RollIn();
			}
			//Move it out if the R-3 trigger is pressed
			else if(rightStick->GetRawButton(3))
			{
				intake->RollOut();
			}
			//Intake the ball only far enough for a pass if R-3 is pressed
			/*else if(rightStick->GetRawButton(3))
			{
				intake->GetBallForPass();
			}*/
			//If none of them are pressed, stop the intake
			else
			{
				intake->Stop();
			}

			//Catapult stuff
			if(rightStick->GetRawButton(1) && leftStick->GetRawButton(1))
			{
				catapult->StartShoot();
			}
			else if(leftStick->GetRawButton(10))
			{
				catapult->StartLoad();
			}
			else if(leftStick->GetRawButton(11))
			{
				catapult->StartRelease();
			}

			//If the right-6 button and l-10 button are pressed, stop the catapult
			if(rightStick->GetRawButton(6) && leftStick->GetRawButton(10))
			{
				catapult->Stop();
			}

			//These functions need to called all of the time, but don't do anything until
			//Their start method is called
			catapult->ReleaseHold();
			catapult->Shoot();
			catapult->Load();

			GetWatchdog().Feed();
			Wait(0.005);				// wait for a motor update time
		}
	}
Exemple #2
0
	void Autonomous()
	{
		GetWatchdog().SetEnabled(false);
		Timer* hotGoalTimer = new Timer();
		Timer* reloadTimer = new Timer();
		Timer* intakeTimer = new Timer();
		Timer* intakeDropTimer = new Timer();
		bool goalFound = false;
		//bool rightSideHot = false;
		hotGoalTimer->Reset();
		hotGoalTimer->Start();
		gyro->Reset();
		leftEnco->Reset();
		rightEnco->Reset();
		LEDLight->Set(Relay::kForward);

		//Find out the type of autonomous we are using
		int autonType = (int)SmartDashboard::GetNumber(NUM_BALL_AUTO);
		if(autonType == 2)//Set the auton mode to two if we are doing a two ball auto
		{
			autonMode = TWO_BALL_AUTON;
			autonStep = AUTON_TWO_WAIT_FOR_INTAKE;
		}
		else//Set the auton to one if the value on SD is set to 1 or another random value
		{
			autonMode = ONE_BALL_AUTON;
			autonStep = AUTON_ONE_FIND_HOT;
		}

		//Bring the intake down
		intake->DropIntake();
		intakeDropTimer->Reset();
		intakeDropTimer->Start();

		while(IsAutonomous() && !IsDisabled())
		{
			rpi->Read();
			lcd->Printf(DriverStationLCD::kUser_Line1, 1, "L: %f", leftEnco->GetDistance());
			lcd->Printf(DriverStationLCD::kUser_Line2, 1, "R: %f", rightEnco->GetDistance());
			lcd->Printf(DriverStationLCD::kUser_Line3, 1, "T: %f", hotGoalTimer->Get());
			lcd->Printf(DriverStationLCD::kUser_Line4, 1, "i: %i", rpi->GetMissingPacketcount());
			lcd->Printf(DriverStationLCD::kUser_Line5, 1, "%i", rpi->GetXPos());
			lcd->Printf(DriverStationLCD::kUser_Line6, 1, "%i", rpi->GetYPos());
			lcd->UpdateLCD();
			LEDLight->Set(Relay::kForward);
			if(autonMode == ONE_BALL_AUTON)
			{
				switch(autonStep)
				{
				case AUTON_ONE_FIND_HOT:
					//Reload the catapult and find the hot goal
					rpi->Read();
					if(!goalFound)
					{
						//This is put into an if statement to protect against the 
						//rpi finding the hot goal and then losing it
						goalFound = ((rpi->GetXPos() != RPI_ERROR_VALUE) ||
								(rpi->GetYPos() != RPI_ERROR_VALUE));
					}
					//Wait for the goal to be hot and the intake to move down
					if(((goalFound) || (hotGoalTimer->Get() >= 6.75)) && intakeDropTimer->Get() >= INTAKE_DROP_WAIT)
					{
						autonStep = AUTON_ONE_SHOOT;
						catapult->StartRelease();
					}
					break;
				case AUTON_ONE_SHOOT:
					//Shoot the catapult
					if(!catapult->ReleaseHold())
					{
						//Move on to the next step when the catapult is released
						autonStep = AUTON_ONE_WAIT;
						//Start the wait timer
						reloadTimer->Reset();
						reloadTimer->Start();
					}
					break;
				case AUTON_ONE_WAIT:
					if(reloadTimer->Get() >= CATAPULT_WAIT_TIME)
					{
						autonStep = AUTON_ONE_DRIVE_FORWARDS;
						reloadTimer->Stop();
						//Start reloading the catapult
						catapult->StartLoad();
						gyro->Reset();
					}
					break;
				case AUTON_ONE_DRIVE_FORWARDS:
					//Drive forwards into the alliance zone and reload the catapult
					if((!GyroDrive(0, 0.75, 36)) && (!(bool)catapult->Load()))
					{
						autonStep = AUTON_END;
					}
					break;
				case AUTON_END:
					break;
				}
			}
			else if(autonMode == TWO_BALL_AUTON)
			{
				switch(autonStep)
				{
				/*case AUTON_TWO_RELOAD:
					//Determine if the hot goal is on the right
					if((rpi->GetXPos() != RPI_ERROR_VALUE) && 
							(rpi->GetYPos() != RPI_ERROR_VALUE))
					{
						rightSideHot = true;
					}
					//Reload the catapult
					if(!((bool)catapult->Load()))
					{
						autonStep = AUTON_TWO_FIRST_SHOOT;
						catapult->StartShoot();
					}
					if((goalFound))
					{
						//If the goal is found, the right side is hot and we can go to the next step
						rightSideHot = true;
						autonStep = AUTON_TWO_FIRST_SHOOT;
					}
					else if(hotGoalTimer->Get() >= 0.5)
					{
						//If the timer runs of, the right side is not hot and we can go to the next step
						rightSideHot = false;
						autonStep = AUTON_TWO_FIRST_TURN;
					}
					break;*/
				case AUTON_TWO_WAIT_FOR_INTAKE:
					//wait for the intake to drop down
					if(intakeDropTimer->Get() >= INTAKE_DROP_WAIT)
					{
						autonStep = AUTON_TWO_FIRST_SHOOT;
						catapult->StartShoot();
					}
					break;
				case AUTON_TWO_FIRST_SHOOT:
					if(catapult->GetLoadingState() == LOAD_RELEASE_TENSION)
					{
						intake->RollIn();
					}
					if(!((bool)catapult->Shoot()))
					{
						autonStep = AUTON_TWO_INTAKE;
						intakeTimer->Reset();
						intakeTimer->Start();
					}
					break;
				case AUTON_TWO_INTAKE:
					intake->RollIn();
					if(intakeTimer->Get() >= 1.5)
					{
						intake->Stop();
						intakeTimer->Stop();
						autonStep = AUTON_TWO_SECOND_SHOOT;
						catapult->StartRelease();
					}
					break;
				case AUTON_TWO_SECOND_SHOOT:
					if(!catapult->ReleaseHold())
					{
						autonStep = AUTON_TWO_WAIT;
						reloadTimer->Reset();
						reloadTimer->Start();
					}
					break;
				case AUTON_TWO_WAIT:
					if(reloadTimer->Get() >= 0.5)
					{
						reloadTimer->Stop();
						leftEnco->Reset();
						rightEnco->Reset();
						catapult->StartLoad();
						gyro->Reset();
						autonStep = AUTON_TWO_DRIVE_FORWARDS;
					}
					break;
				case AUTON_TWO_DRIVE_FORWARDS:
					if((!GyroDrive(0, 0.9, 36)) && (!((bool)catapult->Load())))
					{
						autonStep = AUTON_END;
					}
					break;
					/*case AUTON_TWO_FIRST_TURN:
					//Turn to the left 5* if the right goal is not hot
					if(!rightSideHot)
					{
						if(!GyroTurn(-5, 0.5))
						{
							autonStep = AUTON_TWO_FIRST_SHOOT;
						}
					}
					else
					{
						autonStep = AUTON_TWO_FIRST_SHOOT;
					}
					break;
				case AUTON_TWO_FIRST_SHOOT:
					//Release the catapult to shoot
					if(!catapult->ReleaseHold())
					{
						autonStep = AUTON_TWO_FIRST_WAIT;
						reloadTimer->Reset();
						reloadTimer->Start();
					}
					break;
				case AUTON_TWO_FIRST_WAIT:
					if(reloadTimer->Get() >= CATAPULT_WAIT_TIME)
					{
						autonStep = AUTON_TWO_SECOND_TURN;
						catapult->StartLoad();
						reloadTimer->Stop();
					}
					break;
				case AUTON_TWO_SECOND_TURN:
					//Turn the robot so it's facing forwards and reload the catapult
					if((!GyroTurn(0, 0.5)) && (!(bool)catapult->Load()))
					{
						autonStep = AUTON_TWO_GET_SECOND_BALL;
					}
					break;
				case AUTON_TWO_GET_SECOND_BALL:
					//Start up the intake and drive back to pick up the second ball
					intake->RollIn();
					if(!GyroDrive(0, -0.5, -12))
					{
						autonStep = AUTON_TWO_THIRD_TURN;
						leftEnco->Reset();
						rightEnco->Reset();
					}
					break;
				case AUTON_TWO_THIRD_TURN:
					//If the right goal was originally hot, turn left
					if(rightSideHot)
					{
						if(!GyroTurn(-5, 0.5))
						{
							autonStep = AUTON_TWO_SECOND_SHOOT;
						}
					}
					else
					{
						autonStep = AUTON_TWO_SECOND_SHOOT;
					}
					break;
				case AUTON_TWO_SECOND_SHOOT:
					intake->Stop();
					if(!catapult->ReleaseHold())
					{
						autonStep = AUTON_TWO_SECOND_WAIT;
						reloadTimer->Reset();
						reloadTimer->Start();
					}
					break;
				case AUTON_TWO_SECOND_WAIT:
					if(reloadTimer->Get() >= CATAPULT_WAIT_TIME)
					{
						autonStep = AUTON_TWO_DRIVE_FORWARDS;
						catapult->StartLoad();
						reloadTimer->Stop();
					}
					break;
				case AUTON_TWO_DRIVE_FORWARDS:
					if(!GyroDrive(0, 0.75, 60) && (!(bool)catapult->Load()))
					{
						autonStep = AUTON_END;
					}
					break;*/
				case AUTON_END:
					break;
				}
			}
		}
	}