// HandleArm
// * Manage solenoids for arm up-down
// ----> ASSUMES kForward on DoubleSolenoid is the down position.
// * Handle intake motors
void HandleArm()
{
if (gamepad.GetEvent(BUTTON_ARM) == kEventClosed && armDown)
{
arm.Set(DoubleSolenoid::kReverse);
armDown = false;
}
else if (gamepad.GetEvent(BUTTON_ARM) == kEventClosed)
{
arm.Set(DoubleSolenoid::kForward);
armDown = true;
}
if (gamepad.GetDPadEvent(BUTTON_INTAKE_COLLECT) == kEventClosed)
{
intake.Set(INTAKE_COLLECT);
}
else if (gamepad.GetDPadEvent(BUTTON_INTAKE_COLLECT) == kEventOpened)
{
intake.Set(0.0);
}
if(gamepad.GetDPadEvent(BUTTON_INTAKE_EJECT) == kEventClosed)
{
intake.Set(INTAKE_EJECT);
}
if (gamepad.GetDPadEvent(BUTTON_INTAKE_EJECT) == kEventOpened)
{
intake.Set(0.0);
}
}
// Test Autonomous
void Autonomous()
{
robotDrive.SetSafetyEnabled(false);
// STEP 1: Set all of the states.
// SAFETY AND SANITY - SET ALL TO ZERO
loaded = winchSwitch.Get();
loading = false;
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
// STEP 2: Move forward to optimum shooting position
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// STEP 3: Drop the arm for a clean shot
arm.Set(DoubleSolenoid::kForward);
Wait(1.0); // Ken
// STEP 4: Launch the catapult
LaunchCatapult();
Wait (1.0); // Ken
if (ds->GetDigitalIn(1))
{
// STEP 5: Start the intake motor and backup to our origin position to pick up another ball
InitiateLoad();
intake.Set(-INTAKE_COLLECT);
while (CheckLoad());
Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
Wait(1.0); // For the ball to collect
// STEP 6: Shut off the intake, bring up the arm and move to shooting position
intake.Set(0.0);
arm.Set(DoubleSolenoid::kReverse);
Wait (1.0); // "Settle down"
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// Step 7: drop the arm for a clean shot and shoot
arm.Set(DoubleSolenoid::kForward);
Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// UNTESTED KICKED OFF FIELD
Wait(1.0); // For arm to go down
LaunchCatapult();
}
// Get us fully into the zone for 5 points
Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
}
void shootCatapult()
{
if (logitech.GetRawButton(2) && buttonOne.Get()==0 && buttonTwo.Get()==0)
{
dogSolenoid.Set(DoubleSolenoid::kReverse);
Wait(0.5);
ratchetSolenoid.Set(DoubleSolenoid::kReverse);
Wait(1);
}
}
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);
}
}
void RawControl::armHeightPnu(bool one, bool two) {
if (one) {
low->Set(DoubleSolenoid::kForward);
} else
low->Set(DoubleSolenoid::kReverse);
if (two) {
high->Set(DoubleSolenoid::kForward);
} else
high->Set(DoubleSolenoid::kReverse);
}
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
}
}
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
gamepad.EnableButton(BUTTON_COLLECTOR_FWD);
gamepad.EnableButton(BUTTON_COLLECTOR_REV);
gamepad.EnableButton(BUTTON_SHOOTER);
gamepad.EnableButton(BUTTON_CLAW_1_LOCKED);
gamepad.EnableButton(BUTTON_CLAW_2_LOCKED);
gamepad.EnableButton(BUTTON_CLAW_1_UNLOCKED);
gamepad.EnableButton(BUTTON_CLAW_2_UNLOCKED);
gamepad.EnableButton(BUTTON_STOP_ALL);
gamepad.EnableButton(BUTTON_JOG_FWD);
gamepad.EnableButton(BUTTON_JOG_REV);
stick2.EnableButton(BUTTON_SHIFT);
// Set inital states for all switches and buttons
gamepad.Update();
indexSwitch.Update();
greenClawLockSwitch.Update();
yellowClawLockSwitch.Update();
stick2.Update();
// Set initial states for all pneumatic actuators
shifter.Set(DoubleSolenoid::kReverse);
greenClaw.Set(DoubleSolenoid::kReverse);
yellowClaw.Set(DoubleSolenoid::kReverse);
compressor.Start ();
while (IsOperatorControl())
{
gamepad.Update();
stick2.Update();
indexSwitch.Update();
greenClawLockSwitch.Update();
yellowClawLockSwitch.Update();
HandleCollectorInputs();
HandleDriverInputsManual();
HandleArmInputs();
HandleShooterInputs();
HandleResetButton();
UpdateStatusDisplays();
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
}
void HandleDriverInputsManual(void)
{
myRobot.ArcadeDrive(stick);
if(kEventClosed == stick2.GetEvent(BUTTON_SHIFT))
{
// Shift into high gear.
shifter.Set(DoubleSolenoid::kForward);
}
else if(kEventOpened == stick2.GetEvent(BUTTON_SHIFT))
{
// Shift into low gear.
shifter.Set(DoubleSolenoid::kReverse);
}
}
// HandleEject
// * Handle eject piston.
void HandleEject()
{
if (gamepad.GetEvent(BUTTON_PASS) == kEventClosed)
{
ejectTimer.Start();
eject.Set(DoubleSolenoid::kForward);
}
if (ejectTimer.HasPeriodPassed(EJECT_WAIT))
{
ejectTimer.Stop();
ejectTimer.Reset();
eject.Set(DoubleSolenoid::kReverse);
}
}
// HandleDriverInputs
// * Drive motors according to joystick values
// * Shift (Button 7 on left joystick)
// ----> ASSUMES kForward = high gear
void HandleDriverInputs()
{
if(kEventOpened == leftStick.GetEvent(BUTTON_SHIFT))
{
// Shift into high gear.
shifters.Set(DoubleSolenoid::kForward);
}
else if(kEventClosed == leftStick.GetEvent(BUTTON_SHIFT))
{
// Shift into low gear.
shifters.Set(DoubleSolenoid::kReverse);
}
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
RobotDemo(void):
myRobot(LEFT_DRIVE_PWM, RIGHT_DRIVE_PWM), // these must be initialized in the same order
stick(1), // as they are declared above.
stick2(2),
gamepad(3),
collectorMotor(PICKUP_PWM),
indexerMotor(INDEX_PWM),
shooterMotor(SHOOTER_PWM),
armMotor (ARM_PWM),
shifter(SHIFTER_A,SHIFTER_B),
greenClaw(CLAW_1_LOCKED, CLAW_1_UNLOCKED),
yellowClaw(CLAW_2_LOCKED, CLAW_2_UNLOCKED),
potentiometer(ARM_ROTATION_POT),
indexSwitch(INDEXER_SW),
compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE)
{
m_collectorMotorRunning = false;
m_shooterMotorRunning = false;
dsLCD = DriverStationLCD::GetInstance();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 " NAME);
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);
dsLCD->UpdateLCD();
myRobot.SetExpiration(0.1);
shifter.Set(DoubleSolenoid::kReverse);
}
// Real Autonomous
// * Code to be run autonomously for the first ten (10) seconds of the match.
// * Launch catapult
// * Drive robot forward ENCODER_DIST ticks.
void Autonomous()
{
robotDrive.SetSafetyEnabled(false);
// STEP 1: Set all of the states.
// SAFETY AND SANITY - SET ALL TO ZERO
loaded = winchSwitch.Get();
loading = false;
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
// STEP 2: Move forward to optimum shooting position
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// STEP 3: Drop the arm for a clean shot
arm.Set(DoubleSolenoid::kForward);
Wait(1.0); // Ken
// STEP 4: Launch the catapult
LaunchCatapult();
Wait (1.0); // Ken
// Get us fully into the zone for 5 points
Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
}
// Runs during test mode
// Test
// *
void Test()
{
shifters.Set(DoubleSolenoid::kForward);
leftDriveEncoder.Start();
leftDriveEncoder.Reset();
int start = leftDriveEncoder.Get();
while (IsTest()) {
if (rightStick.GetRawButton(7)) {
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
else {
robotDrive.ArcadeDrive(rightStick.GetY()/2, -rightStick.GetX()/2);
}
if (gamepad.GetEvent(4) == kEventClosed) {
start = leftDriveEncoder.Get();
}
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "lde: %d", leftDriveEncoder.Get() - start);
dsLCD->UpdateLCD();
gamepad.Update();
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl()
{
comp->Start();
while(IsOperatorControl())
{
if(wasenabled == false && IsEnabled() == true)
{
//arm->initialize();
wasenabled = true;
//Wait(.5);
}
wasenabled = IsEnabled();
d->go();
//r->Set((r->kForward));
//ds->PrintfLine(ds->kUser_Line1, "button 11: %d " ,stick->GetRawButton(11));
//ds->PrintfLine(ds->kUser_Line2, "button 2: %d", stick->GetRawButton(2));
//arm->pickup();
if(stick->GetRawButton(7))
{
armlock->Set(armlock->kForward);
}
else
{
armlock->Set(armlock->kReverse);
}
ds->PrintfLine(ds->kUser_Line2, "button 7: %d", stick->GetRawButton(7));
//arm->shoot();
ds->UpdateLCD();
}
}
void HandleArmInputs(void)
{
if (gamepad.GetLeftY() < -0.1)
{
if (potentiometer.GetVoltage() < 4.5)
{
armMotor.Set(1.0);
}
else
{
armMotor.Set(0.0);
}
}
else if (gamepad.GetLeftY() > 0.1)
{
if (potentiometer.GetVoltage() > .5)
{
armMotor.Set(-1.0);
}
else
{
armMotor.Set(0.0);
}
}
else
{
armMotor.Set(0.0);
}
if (gamepad.GetEvent(BUTTON_CLAW_1_LOCKED) == kEventClosed)
{
greenClaw.Set(DoubleSolenoid::kForward);
}
else if (gamepad.GetEvent(BUTTON_CLAW_1_UNLOCKED) == kEventClosed)
{
greenClaw.Set(DoubleSolenoid::kReverse);
}
else if (gamepad.GetEvent(BUTTON_CLAW_2_LOCKED) == kEventClosed)
{
yellowClaw.Set(DoubleSolenoid::kForward);
}
else if (gamepad.GetEvent(BUTTON_CLAW_2_UNLOCKED) == kEventClosed)
{
yellowClaw.Set(DoubleSolenoid::kReverse);
}
}
void TeleopPeriodic(void)
{
Scheduler::GetInstance()->Run();
myDrive_all->TankDrive(drivestick_left->GetY(), drivestick_right->GetY());
ABCheck = Xbox_Button_A->Get(); // Gets the value of the A button on the xbox
BBCheck = Xbox_Button_B->Get(); // Gets the value of the B button on the xbox
if (ABCheck == 1) // If A button is pressed
{
FirstSolenoid->Set(DoubleSolenoid::kForward); // Make piston extend
}
if (BBCheck == 1) // If B button is pressed
{
FirstSolenoid->Set(DoubleSolenoid::kReverse); // Make piston retract
}
if ((ABCheck == 0) && (BBCheck == 0)) // If A and B buttons are not pressed
{
FirstSolenoid->Set(DoubleSolenoid::kOff); // Make piston stay where it is
}
XBCheck = Xbox_Button_X->Get(); // Gets value of the X button on the xbox
YBCheck = Xbox_Button_Y->Get(); // Get value of the Y button on the xbox
if (XBCheck == 1)//If X button is pressed
{
if (limitSwitch->Get() == 0) // If limitswitch is pressed
{
intake_Motor->Set(0); // Stop test motor
}
if (limitSwitch->Get() == 1) // If limitswitch is not pressed
{
intake_Motor->Set(1); // Test motor goes forward at 0.5 speed
}
}
if (YBCheck == 1) // If Y button is pressed
{
intake_Motor->Set(-1); // Test motor goes backward at -0.5 speed
}
if ((XBCheck == 0) && (YBCheck == 0)) // If X and Y buttons are not pressed
{
intake_Motor->Set(0); // Test motor stops
}
}
/**
* Initialization code for test mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters test mode.
*/
void TestInit()
{
printf(">>> TestInit\n");
#ifdef HAVE_COMPRESSOR
compressor->Start();
#endif
#ifdef HAVE_ARM
arm->Set(DoubleSolenoid::kOff);
#endif
#ifdef HAVE_INJECTOR
injectorL->Set(DoubleSolenoid::kOff);
injectorR->Set(DoubleSolenoid::kOff);
#endif
#ifdef HAVE_EJECTOR
ejector->Set(false);
#endif
#ifdef HAVE_LEGS
legs->Set(false);
#endif
#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
jagVbus(topWheel1, 0.0);
#endif
#ifdef HAVE_TOP_PWM1
topWheel1->Set(0.0);
#endif
#ifdef HAVE_TOP_CAN2
jagVbus(topWheel2, 0.0);
#endif
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
jagVbus(bottomWheel1, 0.0);
#endif
#ifdef HAVE_BOTTOM_PWM1
bottomWheel1->Set(0.0);
#endif
#ifdef HAVE_BOTTOM_CAN2
jagVbus(bottomWheel2, 0.0);
#endif
#endif
printf("<<< TestInit\n");
}
// Code to be run during the remaining 2:20 of the match (after Autonomous())
//
// OperatorControl
// * Calls all the above methods
void OperatorControl()
{
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
arm.Set(DoubleSolenoid::kReverse);
/* TODO: Investigate. At least year's (GTR East) competition, we reached the conclusion that disabling this was
* the only way we could get out robot code to work (reliably). Should this be set to false?
*/
robotDrive.SetSafetyEnabled(false);
Timer clock;
int sanity = 0;
int bigSanity = 0;
loading = false;
loaded = winchSwitch.Get();
RegisterButtons();
gamepad.Update();
leftStick.Update();
compressor.Start();
while (IsOperatorControl() && IsEnabled())
{
clock.Start();
HandleDriverInputs();
HandleShooter();
HandleArm();
// HandleEject();
while (!clock.HasPeriodPassed(LOOP_PERIOD)); // add an IsEnabled???
clock.Reset();
sanity++;
if (sanity >= 100)
{
bigSanity++;
sanity = 0;
dsLCD->PrintfLine(DriverStationLCD::kUser_Line4, "%d", bigSanity);
}
gamepad.Update();
leftStick.Update();
dsLCD->UpdateLCD();
}
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
}
void HandleDriverInputsAutomatic(void)
{
//myRobot.ArcadeDrive(stick);
if(DoubleSolenoid::kReverse == shifter.Get())
{
if(stick.GetY() < -0.25)
{
shifter.Set(DoubleSolenoid::kForward);
}
}
// If the robot is in low gear and is over 0.2 input,
// then switch into high gear.
else if(stick.GetY() > -0.2)
{
shifter.Set(DoubleSolenoid::kReverse);
}
SquareInputs();
}
void TeleopPeriodic(void)
{
//if ( _driverStation->IsNewControlData() )
//{
GamepadDrive(_driverGamepad);
GamepadArmControl(_armGamepad);
if ( _beastController->minibotDeployed() )
{
_minibotDeployerSolenoid->Set(DoubleSolenoid::kForward);
}
else
{
_minibotDeployerSolenoid->Set(DoubleSolenoid::kReverse);
}
//}
LogToDashboard();
}
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
gamepad.EnableButton(BUTTON_COLLECTOR_FWD);
gamepad.EnableButton(BUTTON_COLLECTOR_REV);
gamepad.EnableButton(BUTTON_SHOOTER);
gamepad.EnableButton(BUTTON_CLAW_1_LOCKED);
gamepad.EnableButton(BUTTON_CLAW_2_LOCKED);
gamepad.EnableButton(BUTTON_CLAW_1_UNLOCKED);
gamepad.EnableButton(BUTTON_CLAW_2_UNLOCKED);
stick2.EnableButton(BUTTON_SHIFT);
// Set inital states for all switches and buttons
gamepad.Update();
indexSwitch.Update();
stick2.Update();
// Set initial states for all pneumatic actuators
shifter.Set(DoubleSolenoid::kReverse);
greenClaw.Set(DoubleSolenoid::kReverse);
yellowClaw.Set(DoubleSolenoid::kReverse);
compressor.Start ();
while (IsOperatorControl())
{
gamepad.Update();
stick2.Update();
indexSwitch.Update();
HandleCollectorInputs();
HandleDriverInputsManual();
HandleArmInputs();
HandleShooterInputs();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Voltage: %f", potentiometer.GetVoltage()); dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
}
/**
* Initialization code for teleop mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters teleop mode.
*/
void TeleopInit()
{
printf(">>> TeleopInit\n");
#ifdef HAVE_COMPRESSOR
compressor->Start();
#endif
#ifdef HAVE_ARM
arm->Set(DoubleSolenoid::kForward);
#endif
#ifdef HAVE_INJECTOR
injectorL->Set(DoubleSolenoid::kReverse);
injectorR->Set(DoubleSolenoid::kReverse);
#endif
#ifdef HAVE_EJECTOR
ejector->Set(false);
#endif
#ifdef HAVE_LEGS
// legs->Set(true);
#endif
// StartWheels();
printf("<<< TeleopInit\n");
}
/**
* Initialization code for disabled mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters disabled mode.
*/
void DisabledInit()
{
printf(">>> DisabledInit\n");
StopWheels();
#ifdef HAVE_ARM
arm->Set(DoubleSolenoid::kOff);
#endif
#ifdef HAVE_INJECTOR
injectorL->Set(DoubleSolenoid::kOff);
injectorR->Set(DoubleSolenoid::kOff);
#endif
#ifdef HAVE_EJECTOR
// ejector->Set(false);
#endif
#ifdef HAVE_LEGS
// legs->Set(false);
#endif
#ifdef HAVE_COMPRESSOR
compressor->Stop();
#endif
printf("<<< DisabledInit\n");
}
void TeleopPeriodic()
{
SmartDashboard::PutNumber("joystickX",stick.GetX());
SmartDashboard::PutNumber("joystickY",stick.GetY());
//SmartDashboard::PutBoolean("f*****g buttons", stick.GetRawButton(1));
//SmartDashboard::PutNumber("potentiometer voltage", pot.GetVoltage());
SmartDashboard::PutBoolean("infra",infra.Get());
SmartDashboard::PutNumber("accelX",accel.GetX());
SmartDashboard::PutNumber("accelY",accel.GetY());
SmartDashboard::PutNumber("accelZ",accel.GetZ());
servo.Set(
trueMap(stick.GetX(), 1, -1, 1, 0) // trueMap allows use of entire joystick
);
SmartDashboard::PutNumber("servo", servo.Get());
jag1.Set(stick.GetY());
jag2.Set(stick.GetY());
//tal1.Set(stick.GetY());
SmartDashboard::PutNumber("jag1", jag1.Get());
SmartDashboard::PutNumber("jag2", jag2.Get());
/*SmartDashboard::PutNumber("encpos", enc.Get());
SmartDashboard::PutNumber("encspd", enc.GetRate());*/
if (stick.GetRawButton(1) && !actuatePressed) {
pistonVal=!pistonVal;
piston.Set(pistonVal ? DoubleSolenoid::kForward : DoubleSolenoid::kReverse);
actuatePressed = true;
}
else if (!stick.GetRawButton(1))
actuatePressed = false;
SmartDashboard::PutBoolean("piston forward", piston.Get() == DoubleSolenoid::kForward);
}
RobotDemo(void):
leftDriveMotor(LEFT_DRIVE_PWM),
rightDriveMotor(RIGHT_DRIVE_PWM),
myRobot(&leftDriveMotor, &rightDriveMotor), // these must be initialized in the same order
stick(1), // as they are declared above.
stick2(2),
gamepad(3),
collectorMotor(PICKUP_PWM),
indexerMotor(INDEX_PWM),
shooterMotor(SHOOTER_PWM),
armMotor (ARM_PWM),
leftDriveEncoder(LEFT_DRIVE_ENC_A, LEFT_DRIVE_ENC_B),
shifter(SHIFTER_A,SHIFTER_B),
greenClaw(CLAW_1_LOCKED, CLAW_1_UNLOCKED),
yellowClaw(CLAW_2_LOCKED, CLAW_2_UNLOCKED),
potentiometer(ARM_ROTATION_POT),
indexSwitch(INDEXER_SW),
greenClawLockSwitch(CLAW_1_LOCK_SENSOR),
yellowClawLockSwitch(CLAW_2_LOCK_SENSOR),
compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE),
jogTimer(),
shooterTimer()
{
m_collectorMotorRunning = false;
m_shooterMotorRunning = false;
m_jogTimerRunning = false;
m_shiftCount = MAX_SHIFTS;
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 " NAME);
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);
dsLCD->UpdateLCD();
myRobot.SetExpiration(0.1);
shifter.Set(DoubleSolenoid::kReverse);
leftDriveEncoder.SetDistancePerPulse(DRIVE_ENCODER_DISTANCE_PER_PULSE);
leftDriveEncoder.SetMaxPeriod(1.0);
leftDriveEncoder.SetReverseDirection(true); // change to true if necessary
leftDriveEncoder.Start();
}
void Autonomous()
{
compressor.Start();//start compressor
myRobot.SetSafetyEnabled(false);
myRobot.Drive(-0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
while (!shoot.Get()){//while shooter isnt cocked pull it back
shooter.SetSpeed(1); //set motor
}
shooterSole.Set(shooterSole.kForward);//Sets Solenoid forward, shoot ball
shooterSole.Set(shooterSole.kReverse);//Sets Solenoid backward
myRobot.Drive(1.0, -1.0);//turn around for 0.5 seconds, we have to check to see if it takes that long
Wait(0.5);//wait for 0.5 seconds
myRobot.Drive(0.0, 0.0);//stop robot
}
void HandleArmInputs(void)
{
if (!m_jogTimerRunning)
{
if (gamepad.GetLeftY() < -0.1)
{
if (potentiometer.GetVoltage() < 4.5)
{
armMotor.Set(ARM_FWD);
}
else
{
armMotor.Set(0.0);
}
}
else if (gamepad.GetLeftY() > 0.1)
{
if (potentiometer.GetVoltage() > .5)
{
armMotor.Set(ARM_REV);
}
else
{
armMotor.Set(0.0);
}
}
else if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kUp))
{
armMotor.Set(ARM_FWD);
jogTimer.Start();
jogTimer.Reset();
m_jogTimerRunning = true;
}
else if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kDown))
{
armMotor.Set(ARM_REV);
jogTimer.Start();
jogTimer.Reset();
m_jogTimerRunning = true;
}
else
{
armMotor.Set(0.0);
}
}
else if (jogTimer.HasPeriodPassed(JOG_TIME))
{
armMotor.Set(0);
jogTimer.Stop();
jogTimer.Reset();
m_jogTimerRunning = false;
}
if (gamepad.GetEvent(BUTTON_CLAW_1_LOCKED) == kEventClosed)
{
greenClaw.Set(DoubleSolenoid::kForward);
}
else if (gamepad.GetEvent(BUTTON_CLAW_1_UNLOCKED) == kEventClosed)
{
greenClaw.Set(DoubleSolenoid::kReverse);
}
else if (gamepad.GetEvent(BUTTON_CLAW_2_LOCKED) == kEventClosed)
{
yellowClaw.Set(DoubleSolenoid::kForward);
}
else if (gamepad.GetEvent(BUTTON_CLAW_2_UNLOCKED) == kEventClosed)
{
yellowClaw.Set(DoubleSolenoid::kReverse);
}
}
void Test() // DONT TOUCH THIS AREA. I KEEL YOU.
{
DriverStationLCD *screen = DriverStationLCD::GetInstance();
int counter = 0;
bool solenoidTest=0;
while (IsTest())
{
if(logitech.GetRawButton(9)) //press rightBack
{
solenoidTest=1;
compressor.Start();
}
if(logitech.GetRawButton(10)) //press Start
{
solenoidTest=0;
compressor.Stop();
}
if(solenoidTest)
{
if(logitech.GetRawButton(1)) //press X
{
rightArmSolenoid.Set(DoubleSolenoid::kForward);
leftArmSolenoid.Set(DoubleSolenoid::kForward);
}
else if(logitech.GetRawButton(2)) //press A
{
rightArmSolenoid.Set(DoubleSolenoid::kReverse);
leftArmSolenoid.Set(DoubleSolenoid::kReverse);
}
else
{
leftArmSolenoid.Set(DoubleSolenoid::kOff);
rightArmSolenoid.Set(DoubleSolenoid::kOff);
}
if(logitech.GetRawButton(3)) //PRess URTrigger
{
retrievalMotor.Set(logitech.GetRawAxis(2));
}
else
{
retrievalMotor.Set(0);
}
if(logitech.GetRawButton(4))
{
winchMotor.Set(logitech.GetRawAxis(2));
}
else
{
winchMotor.Set(0);
}
if(logitech.GetRawButton(5))
{
ratchetSolenoid.Set(DoubleSolenoid::kForward);
}
else if(logitech.GetRawButton(7))
{
ratchetSolenoid.Set(DoubleSolenoid::kReverse);
}
else
{
ratchetSolenoid.Set(DoubleSolenoid::kOff);
}
if(logitech.GetRawButton(6))
{
dogSolenoid.Set(DoubleSolenoid::kForward);
}
else if(logitech.GetRawButton(8))
{
dogSolenoid.Set(DoubleSolenoid::kReverse);
}
else
{
dogSolenoid.Set(DoubleSolenoid::kOff);
}
}
else
{
if(logitech.GetRawButton(1)) //Press X
{
rightFront.Set(logitech.GetRawAxis(2)); //Press left joystick
}
else
{
rightFront.Set(0);
}
if(logitech.GetRawButton(2)) //Press A
{
rightBack.Set(logitech.GetRawAxis(2));
}
else
{
rightBack.Set(0);
}
if(logitech.GetRawButton(3)) //Press B
{
leftFront.Set(logitech.GetRawAxis(2));
}
else
{
leftFront.Set(0);
}
if(logitech.GetRawButton(4)) //Press Y
{
leftBack.Set(logitech.GetRawAxis(2));
}
else
{
leftBack.Set(0);
}
if(logitech.GetRawButton(5)) //Press ULTrigger
{
retrievalMotor.Set(logitech.GetRawAxis(2));
}
else
{
retrievalMotor.Set(0);
}
if(logitech.GetRawButton(6)) //PRess URTrigger
{
winchMotor.Set(logitech.GetRawAxis(2));
}
else
{
winchMotor.Set(0);
}
if(logitech.GetRawButton(7)) //Press LLTrigger
{
ringLight.Set(Relay::kForward);
}
else
{
ringLight.Set(Relay::kOff);
}
if(logitech.GetRawButton(8)) //Press LRTrigger
{
compressor.Start();
}
else
{
compressor.Stop();
}
}
/****** MANUAL LOAD FUNCTION END *****/
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"LeftJoystick: %f", logitech.GetRawAxis(2));
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"RF:%f RB:%f LF:%f LB:%f", rightFront.Get(), rightBack.Get(), leftFront.Get(), leftBack.Get()); // Print WinchMotor State
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Solenoid Testing:%d", solenoidTest);
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"rightArmSolenoid:%d", rightArmSolenoid.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line5,"time:%d", counter);
counter ++;
Wait(0.005); // Waits to run the loop every 0.005 seconds so the cRIO doesn't explode
screen->UpdateLCD();
}
}
void OperatorControl()
{
compressor.Start(); // The class can run independently. This compressor will start and stop on its own to keep pressure under 120 PSI
DriverStationLCD *screen = DriverStationLCD::GetInstance(); // Enables Driver Station Printing
bool autoLoad = false; //autoLoad function
retrievalMotor.Set(1); //Starts with retrieval wheels on
while (IsOperatorControl())
{
/****** AUTO LOAD TOGGLE START *****/
if(logitech.GetRawButton(10))
{
if(autoLoad)
{
autoLoad = false;
Wait(0.2);
}
else if(!autoLoad)
{
autoLoad = true;
Wait(0.2);
}
}
/****** AUTO LOAD TOGGLE END *****/
/** RETRIEVAL WHEELS: RED button (B) turns the arm wheels off, GREEN button (A) turns arm wheels on, BLUE button (X) turns the arm wheels in reverse**/
if(logitech.GetRawButton(1))
{
retrievalMotor.Set(-1);
}
if(logitech.GetRawButton(3))
{
retrievalMotor.Set(1);
}
if(logitech.GetRawButton(3))
{
retrievalMotor.Set(0);
}
/** OPTIONS ON DEM WHEELZ END **/
/****** TANK DRIVE START ******/
if (fabs(logitech.GetRawAxis(2)) > driveStickBuffer) // If the left Y Axis on the Joystick is moved beyond the driveStickBuffer
{
if (logitech.GetRawButton(11))
{
leftFront.Set(-1);
leftBack.Set(-1);
}
else
{
leftFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); // The left motors are facing in an opposite direction
leftBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); // The left motors are facing in an opposite direction
}
}
else
{
leftFront.Set(0); // Turns Left Motor 1 off
leftBack.Set(0); // Turns Left Motor 2 off
}
if (fabs(logitech.GetRawAxis(4)) > driveStickBuffer) // If the Y Axis on the Joystick is moved beyond the driveStickBuffer
{
if (logitech.GetRawButton(12))
{
rightFront.Set(1);
rightBack.Set(1);
}
else
{
rightFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); // Sets motor speed to Joystick position times the driveSpeedMultiplier
rightBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); // Sets motor speed to Joystick position times the driveSpeedMultiplier
}
}
else
{
rightFront.Set(0); // Turns Right Motor 1 off
rightBack.Set(0); // Turns Right Motor 2 off
}
/****** TANK DRIVE END ******/
/****** BALL RETRIEVAL START ******/ /*CHECK THE POLARITY OF YOUR SOLENOIDS*/
if (logitech.GetRawButton(8)) //Press Lower Right Trigger to go down and STOP ball motor
{
leftArmSolenoid.Set(DoubleSolenoid::kReverse); // Pushes the pneumatic piston forward to lower the retrieval arm
rightArmSolenoid.Set(DoubleSolenoid::kReverse); // Pushes the pneumatic piston forward to lower the retrieval arm
if(retrievalMotor.Get()==1)
{
retrievalMotor.Set(0); // Toggles the motor that spins the wheel to bring in the ball
}
else
{
retrievalMotor.Set(1);
}
}
else if (logitech.GetRawButton(6)) //Press Upper Trigger to go up and STOP ball motor
{
leftArmSolenoid.Set(DoubleSolenoid::kForward); // Brings the pneumatic piston backward to raise the retrieval arm
rightArmSolenoid.Set(DoubleSolenoid::kForward); // Brings the pneumatic piston backward to raise the retrieval arm
if(retrievalMotor.Get()==1)
{
retrievalMotor.Set(0); // Toggles the motor that spins the wheel to bring in the ball
}
else
{
retrievalMotor.Set(1);
}
}
else
{
leftArmSolenoid.Set(DoubleSolenoid::kOff); // Does nothing to the retreival arm piston //
rightArmSolenoid.Set(DoubleSolenoid::kOff); //////////////////////////////////////////////
}
/****** BALL RETRIEVAL END ******/
/******* AUTOLOAD CATAPULT START ******/ // Limit Buttons not pressed, both dogSolenoid and ratchetSolenoids are disengaged
if (autoLoad)
{
dogSolenoid.Set(DoubleSolenoid::kForward); // Brings the pneumatic piston backward to raise the retrieval arm
winchMotor.Set(0.1);
Wait(0.3);
winchMotor.Set(0);
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to lower the retrieval arm
Wait(0.1);
while(leftLimitSwitch.Get()==1 && rightLimitSwitch.Get()==1)
{
winchMotor.Set(1);
/**ALLOW FOR WHILE LOOP TANK DRIVING**/
if (fabs(logitech.GetRawAxis(2)) > driveStickBuffer) // If the left Y Axis on the Joystick is moved beyond the driveStickBuffer
{
leftFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); // The left motors are facing in an opposite direction
leftBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); // The left motors are facing in an opposite direction
}
else
{
leftFront.Set(0); // Turns Left Motor 1 off
leftBack.Set(0); // Turns Left Motor 2 off
}
if (fabs(logitech.GetRawAxis(4)) > driveStickBuffer) // If the Y Axis on the Joystick is moved beyond the driveStickBuffer
{
rightFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); // Sets motor speed to Joystick position times the driveSpeedMultiplier
rightBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); // Sets motor speed to Joystick position times the driveSpeedMultiplier
}
else
{
rightFront.Set(0); // Turns Motor 2 off
rightBack.Set(0); // Turns Motor 2 off
}
/**END ALLOW FOR WHILE LOOP TANK DRIVING **/
if(logitech.GetRawButton(9)) //kills the winch
{
winchMotor.Set(0);
}
else
{
winchMotor.Set(1);
}
}
winchMotor.Set(0); // Stops the Winch Motor since one or more buttons are pressed
}
/****** AUTOLOAD CATAPULT END ******/
/****** MANUAL LOAD CATAPULT START ******/
if (!autoLoad)
{
if (logitech.GetRawButton(5)) //pressing Upper Left Trigger loads
{
dogSolenoid.Set(DoubleSolenoid::kForward); // Brings the pneumatic piston backward to raise the retrieval arm
winchMotor.Set(0.1);
Wait(0.3);
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to lower the retrieval arm
while(leftLimitSwitch.Get()==1 && rightLimitSwitch.Get()==1)
{
winchMotor.Set(1);
/**ALLOW FOR WHILE LOOP TANK DRIVING**/
if (fabs(logitech.GetRawAxis(2)) > driveStickBuffer) // If the left Y Axis on the Joystick is moved beyond the driveStickBuffer
{
leftFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); // The left motors are facing in an opposite direction
leftBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); // The left motors are facing in an opposite direction
}
else
{
leftFront.Set(0); // Turns Left Motor 1 off
leftBack.Set(0); // Turns Left Motor 2 off
}
if (fabs(logitech.GetRawAxis(4)) > driveStickBuffer) // If the Y Axis on the Joystick is moved beyond the driveStickBuffer
{
rightFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); // Sets motor speed to Joystick position times the driveSpeedMultiplier
rightBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); // Sets motor speed to Joystick position times the driveSpeedMultiplier
}
else
{
rightFront.Set(0); // Turns Motor 2 off
rightBack.Set(0); // Turns Motor 2 off
}
if(logitech.GetRawButton(9)) //kills the winch
{
winchMotor.Set(0);
}
else
{
winchMotor.Set(1);
}
}
winchMotor.Set(0); //shuts the winch motor off
}
}
/****** MANUAL LOAD CATAPULT END ******/
/****** CATAPULT RELEASE START ******/ // Limit Buttons pressed, winchMotor is inactive, and lower left trigger on the joystick is pressed
if ((leftLimitSwitch.Get()== 0 || rightLimitSwitch.Get()== 0) && winchMotor.Get() == 0 && logitech.GetRawButton(7))
{
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.05); // Giving the pistons time to disengage properly
ratchetSolenoid.Set(DoubleSolenoid::kReverse); // Brings the pneumatic piston backward to disengage the ratchet
if(autoLoad)
{
Wait(3); // Waits 3 seconds after shooting before starting to load the catapult
}
else
{
}
}
/****** CATAPULT RELEASE END ******/
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Pressure:%f", compressor.GetPressureSwitchValue()); // Print Pneumatics Pressure
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"LimSwi:%d and %d", leftLimitSwitch.Get(), rightLimitSwitch.Get()); // Print Left& Buttons
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"WinchMotor:%f", winchMotor.Get()); // Print WinchMotor State
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Autoload:%s", autoLoad? "On" : "Off" ); // Print state of autoLoad
//screen -> PrintfLine(DriverStationLCD::kUser_Line5,""); // Free Line
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"LDri:%f RDri:%f", leftFront.Get(), rightFront.Get()); // Print Tank Drive Motor State
Wait(0.005); // Waits to run the loop every 0.005 seconds so the cRIO doesn't explode
screen->UpdateLCD(); // Updates the Driverstation LCD in the loop
}
compressor.Stop(); // Allows the compressor to stop running if the mode is switched outside of OperatorControl
}
/****** AUTO FUNCTIONS END *******/
void Autonomous()
{
int counter=0;
int autonomousEngagement = 0;
DriverStationLCD *screen = DriverStationLCD::GetInstance();
compressor.Start(); //starts compressor class
rightArmSolenoid.Set(DoubleSolenoid::kReverse); //brings the arms down
leftArmSolenoid.Set(DoubleSolenoid::kReverse);
/*** ENSURES THE CATAPULT IS LOADED AND LOADS IF UNLOADED ***/
if (leftLimitSwitch.Get() == 1 && rightLimitSwitch.Get() == 1)
{
winchMotor.Set(0.1); // Gears need to be moving slowly to allow the dog gear to engage properly
dogSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to engage the dog gear
Wait(0.2); // Giving the pistons time to engage properly
winchMotor.Set(0); // Now that the dog gear is engaged, the gears do not have to move
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to engage the ratchet
Wait(0.2); // Giving the pistons time to engage properly
}
while (leftLimitSwitch.Get() == 1 && rightLimitSwitch.Get() == 1) // If Limit Switch Buttons are not pressed
{
winchMotor.Set(1); //Now starts the winch motor to load the catapult
}
// If the Catapult Left & Limit Switches are (0,0), (0,1), (1,0)
{
winchMotor.Set(0); // Stops the Winch Motor since one or more buttons are pressed
if ((dogSolenoid.Get() == DoubleSolenoid::kReverse) && (ratchetSolenoid.Get() == DoubleSolenoid::kForward)) // If the Dog Gear is disengaged but the ratchet is engaged
{
winchMotor.Set(0.05); // Gears need to be moving slowly to allow the dog gear to engage properly. Might want to test this since the catapult's already loaded.
dogSolenoid.Set(DoubleSolenoid::kForward); // Engages the dog gear so both dog gear and ratchet are engaged before shooting for safety
Wait(0.1); // Giving the pistons time to engage properly
winchMotor.Set(0); // Now that the dog gear is engaged, the gears do not have to move
}
else if ((dogSolenoid.Get() == DoubleSolenoid::kForward) && (ratchetSolenoid.Get() == DoubleSolenoid::kReverse)) // If the dog gear is engaged but the ratchet is disengaged
{
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Engages the ratchet so that both dog gear and ratchet are engaged before shooting for safety
Wait(0.1); // Giving the pistons time to engage properly
}
}
/*** DONE LOADING THE CATAPULT ***/
float pLower = 5; // min height of rectangle for comparison
float pUpper = 15; // max height of rectangle for comparison
int criteriaCount = 1; // number of elements to include/exclude at a time
int rejectMatches = 1; // when set to true, particles that do not meet the criteria are discarded
int connectivity = 1; // declares connectivity value as 1; so corners are not ignored
int filterFunction; // removes small blobs
int borderSetting; // variable to store border settings, limit for rectangle
int borderSize = 1; // border for the camera frame (if you don't put this, DriverStation gets mad at you)
ParticleFilterCriteria2 particleCriteria;
ParticleFilterOptions2 particleFilterOptions;
int numParticles;
particleCriteria.parameter = IMAQ_MT_BOUNDING_RECT_HEIGHT; //The Morphological measurement we use
particleCriteria.lower = pLower; // The lower bound of the criteria range
particleCriteria.upper = pUpper; // The upper bound of the criteria range
particleCriteria.calibrated = FALSE; // We aren't calibrating to real world measurements. We don't need this.
particleCriteria.exclude = TRUE; // Remove all particles that aren't in specific pLower and pUpper range
particleFilterOptions.rejectMatches = rejectMatches; // Set to 1 above, so images that do not meet the criteria are discarded
particleFilterOptions.rejectBorder = 0; // Set to 0 over here so border images are not discarded
particleFilterOptions.connectivity8 = connectivity; // Sets the image image to 8 bit
while ((IsAutonomous()))
{
if (logitech.GetRawButton(4))
{
autonomousEngagement = 1;
}
if (autonomousEngagement == 0) // If real autonomous is not engaged start
{
if (logitech.GetRawButton(1))
{
driveForward();
}
if (logitech.GetRawButton(9))
{
dogSolenoid.Set(DoubleSolenoid::kForward); // Brings the pneumatic piston backward to raise the retrieval arm
winchMotor.Set(0.1);
Wait(0.3);
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to lower the retrieval arm
while(leftLimitSwitch.Get()==1 && rightLimitSwitch.Get()==1)
{
winchMotor.Set(1);
}
}
if (logitech.GetRawButton(2))
{
autonomousCatapultRelease();
}
if (logitech.GetRawButton(3))
{
stopDriving();
}
if (logitech.GetRawButton(5))
{
turnLeft();
}
if (logitech.GetRawButton(7))
{
turnLeftMore();
}
if (logitech.GetRawButton(6))
{
turnRight();
}
if (logitech.GetRawButton(8))
{
turnRightMore();
}
}// If real autonomous is not engaged end
HSLImage* imgpointer; // declares an image container as an HSL (hue-saturation-luminence) image
imgpointer = camera.GetImage(); //tells camera to capture image
ringLight.Set(Relay::kForward); //turns ringlight on
BinaryImage* binIMG = NULL; // declares a container to hold a binary image
binIMG = imgpointer -> ThresholdHSL(0, 255, 0, 255, 235, 255); // thresholds HSL image and places in the binary image container
delete imgpointer; // deletes the HSL image to free up memory on the cRIO
Image* modifiedImage = imaqCreateImage(IMAQ_IMAGE_U8, 0); //create a binary 8-bit format shell for the image
filterFunction = imaqParticleFilter4(modifiedImage, binIMG -> GetImaqImage(), &particleCriteria, criteriaCount, &particleFilterOptions, NULL, &numParticles); //The Particle Filter Function we use. (The ones before it are outdated)
borderSetting = imaqSetBorderSize(modifiedImage, borderSize); // Sets a border size so DriverStation is happy
delete binIMG; //Deletes the Binary image
int functionCountParticles; // stores number of particles
int particleAmount; // stores the number of particles for the measure particle function
functionCountParticles = imaqCountParticles(modifiedImage, TRUE, &particleAmount); // Counts the number of particles
int functionOne; // The first measuring particle function (specifically for particle #1)
int functionTwo; // The second measuring particle function (specifically for particle #2)
double particleOneOrientation; // TRULY ARBITRARY name of the first particle it find
double particleTwoOrientation; // TRULY ARBITRARY name of the second particle it finds
functionOne = imaqMeasureParticle(modifiedImage, 0, FALSE, IMAQ_MT_ORIENTATION, &particleOneOrientation); // Measures orientation of particle 1
functionTwo = imaqMeasureParticle(modifiedImage, 1, FALSE, IMAQ_MT_ORIENTATION, &particleTwoOrientation); // Measures orientation of particle 2
screen->PrintfLine(DriverStationLCD::kUser_Line2,"P1: %f", particleOneOrientation); // Prints particle 1's orientation
screen->PrintfLine(DriverStationLCD::kUser_Line3,"P2: %f", particleTwoOrientation); // Prints particle 2's orientation
imaqDispose(modifiedImage); // Deletes the filtered image
/**LEFT POSITION**/
if ((leftPositionSwitch.Get() == 1) && (rightPositionSwitch.Get() == 0)) // Left switch set on, switch set off
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Left Position:F"); // Left position and facing forward
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10))
// The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Left Position Hot!");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRight();
//driveForward();
Wait(3);
stopDriving();
//autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Left Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRight();
driveForward();
Wait(4);
stopDriving();
//autonomousCatapultRelease();
}
}
/**CENTER POSITION**/
else if ((leftPositionSwitch.Get() == 0) && (rightPositionSwitch.Get() == 0)) // Left switch off and right switch off
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position:R"); // Middle position and facing
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10)) // The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeftMore();
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
}
/** RIGHT POSITION**/
else if ((leftPositionSwitch.Get() == 1) && (rightPositionSwitch.Get() == 1))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position:R"); // Middle position and facing
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10)) // The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeftMore();
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
}
else if (((leftPositionSwitch.Get()) == 1) && ((rightPositionSwitch.Get()) == 0)) // Left switch off and switch on
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Right Position"); // position and facing forward
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || ((particleTwoOrientation > 0) && (particleTwoOrientation < 10))) // The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeft();
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4, "Right Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
}
counter++;
screen -> PrintfLine(DriverStationLCD::kUser_Line5,"R: %f L: %f)", rightFront.Get(), leftFront.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Counter %d", counter);
screen->UpdateLCD();
}
compressor.Stop();
}