// // Main Tele Operator Mode function. This function is called once, therefore a while loop that checks IsOperatorControl and IsEnabled is used // to maintain control until the end of tele operator mode // void OperatorControl() { //myRobot.SetSafetyEnabled(true); timer->Start(); Relay* reddlight = new Relay(4); //Timer* lighttimer = new Timer(); //lighttimer->Start(); while (IsOperatorControl() && IsEnabled()) { reddlight->Set(reddlight->kForward); /* if (lighttimer->Get()<=0.5) { reddlight->Set(reddlight->kForward); } else if(lighttimer->Get()<1){ reddlight->Set(reddlight->kOff); } else { lighttimer->Reset(); } */ // // Get inputs // driverInput->GetInputs(); drive->GetInputs(); catapult->GetInputs(); feeder->GetInputs(); // // Pass values between components as necessary // //catapult->SetSafeToFire(feeder->GetAngle()<95); // // Execute one step on each component // drive->ExecStep(); catapult->ExecStep(); feeder->ExecStep(); // // Set Outputs on all components // catapult->SetOutputs(); feeder->SetOutputs(); // // Wait for step timer to expire. This allows us to control the amount of time each step takes. Afterwards, restart the // timer for the next loop // while (timer->Get()<(PERIOD_IN_SECONDS)); timer->Reset(); } }
void TeleopPeriodic() { if(!isWait) { //drive drive->drive(oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS)), setTurnSpeed(oi->joyDrive->GetRawAxis(TURN_X_AXIS)); drive->shift(oi->joyDrive->GetRawButton(8), oi->joyDrive->GetRawButton(9)); //manipulator manip->moveArm(oi->joyManip->GetRawButton(6), oi->joyManip->GetRawButton(7)); manip->intakeBall(oi->joyManip->GetRawButton(INTAKE_BUTTON), oi->joyManip->GetRawButton(OUTTAKE_BUTTON), (oi->getManipJoystick()->GetThrottle()+1)/2); catapult->launchBall(); manip->toggleCompressor(oi->joyDrive->GetRawButton(COMPRESSOR_BUTTON)); } //camera motor mount if(oi->joyManip->GetRawButton(10)) { bCameraLatch = true; } else if(oi->joyManip->GetRawButton(11)) { bCameraLatch = false; } manip->toggleCameraPosition(bCameraLatch); }
void TeleopPeriodic() { manip->toggleCompressor(oi->joyDrive->GetRawButton(COMPRESSOR_BUTTON)); //drive drive->drive(oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS), oi->joyDrive->GetRawAxis(TURN_X_AXIS)); //drive->shift(oi->joyDrive->GetRawButton(8), oi->joyDrive->GetRawButton(9)); //manipulator // TJF: Replaced "magic numbers" with named constants manipArm->moveArm(oi->joyDrive->GetRawButton(UP_INTAKE_BUTTON), oi->joyDrive->GetRawButton(DOWN_INTAKE_BUTTON)); //manipArm->moveArm(oi->joyDrive->GetRawButton(6), oi->joyDrive->GetRawButton(7)); manip->intakeBall(oi->joyDrive->GetRawButton(INTAKE_BUTTON), oi->joyDrive->GetRawButton(OUTTAKE_BUTTON), (oi->joyDrive->GetThrottle()+1)/2); // TJF: removed only because it doesn't work yet catapult->launchBall(); printSmartDashboard(); // 01/18/2016 moved this function because it needed to be updated constantly instead of initializing it only once. }
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; } } } }