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
}
}
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;
}
}
}
}