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 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); } }
// Test Autonomous void TestAutonomous() { 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); winch.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(0)) { // 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); // 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); Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST); // Step 7: drop the arm for a clean shot and shoot arm.Set(DoubleSolenoid::kForward); // UNTESTED KICKED OFF FIELD Wait(1.0); // Ken 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); winch.Set(0.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 } }
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"); }
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); }
// 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 LogToDashboard() { _arm->logInfo(); _lightSensors->logInfo(); // minibot deployment SmartDashboard::Log(MinibotDeployable(), "Minibot Deployable"); SmartDashboard::Log(_minibotDeployerSolenoid->Get(), "Minibot Deployed"); }
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"); }
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(); }
void TeleopPeriodic() { //camera->GetImage(frame); //imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0.0f); //CameraServer::GetInstance()->SetImage(frame); printf("Left Encoder: %i, Right Encoder: %i, Gyro: %f\n", leftEnc->Get(), rightEnc->Get(), gyro->GetAngle()); drive->ArcadeDrive(driveStick); drive->SetMaxOutput((1-driveStick->GetThrottle())/2); //printf("%f\n", (1-stick->GetThrottle())/2); //leftMotor->Set(0.1); //rightMotor->Set(0.1); if (shootStick->GetRawAxis(3) > 0.5) { launch1->Set(1.0); launch2->Set(1.0); } else if (shootStick->GetRawAxis(2) > 0.5) { printf("Power Counter: %i\n", powerCounter); if (powerCounter < POWER_MAX) { powerCounter++; launch1->Set(-0.8); launch2->Set(-0.8); } else { launch1->Set(-0.6); launch2->Set(-0.6); } } else { launch1->Set(0.0); launch2->Set(0.0); powerCounter = 0.0; } //use this button to spin only one winch, to lift up. if (shootStick->GetRawButton(7)) { otherWinch->Set(0.5); } else if (shootStick->GetRawButton(8)) { otherWinch->Set(-0.5); } else { otherWinch->Set(0.0); } if (shootStick->GetRawButton(5)) { winch->Set(-0.7); if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) { // otherWinch->Set(-0.5); } } else if (shootStick->GetRawButton(6)) { winch->Set(0.7); if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) { // otherWinch->Set(0.5); } } else { winch->Set(0.0); if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) { //, otherWinch->Set(0.0); } } if (shootStick->GetRawButton(1)) { launchPiston->Set(1); } else { launchPiston->Set(0); } if (shootStick->GetRawButton(3)) { Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer); } if (shootStick->GetRawButton(3) && debounce == false) { debounce = true; if (defenseUp) { defensePiston->Set(DoubleSolenoid::Value::kReverse); defenseUp = false; } else { defenseUp =true; defensePiston->Set(DoubleSolenoid::Value::kForward); } } else if (!shootStick->GetRawButton(3)){ debounce = false; } }