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 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 OperatorControl() { compressor.Start(); DriverStationLCD *screen = DriverStationLCD::GetInstance(); while (IsOperatorControl()) { screen -> PrintfLine(DriverStationLCD::kUser_Line1,"LeftTopM1_%f", logitech.GetRawAxis(2)); screen -> PrintfLine(DriverStationLCD::kUser_Line2,"LeftBackM2_%f", logitech.GetRawAxis(2)); screen -> PrintfLine(DriverStationLCD::kUser_Line3,"RightTopM1_%f", logitech.GetRawAxis(4)); //screen -> PrintfLine(DriverStationLCD::kUser_Line4,"RightTopM2_%f", logitech.GetRawAxis(4)); //screen -> PrintfLine(DriverStationLCD::kUser_Line5,"Button_%d", buttonOne.Get()); //screen -> PrintfLine(DriverStationLCD::kUser_Line6,"toggle_%d", togglebuttonOne.Get()); /** CATAPULT **/ loadCatapult(); shootCatapult(); //Press A to shoot /** RETRIEVAL **/ if(logitech.GetRawButton(6)) //Press Upper Right Trigger to go down { armSolenoidOne.Set(DoubleSolenoid::kForward); armSolenoidTwo.Set(DoubleSolenoid::kForward); } else if(logitech.GetRawButton(8)) //Press Lower Right Trigger to go up { armSolenoidOne.Set(DoubleSolenoid::kReverse); armSolenoidTwo.Set(DoubleSolenoid::kReverse); } else { armSolenoidOne.Set(DoubleSolenoid::kOff); armSolenoidTwo.Set(DoubleSolenoid::kOff); } /**DRIVING**/ if (fabs(logitech.GetRawAxis(2)) > driveStickBuffer) { leftFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); leftBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); } else { leftFront.Set(0); leftBack.Set(0); } if (fabs(logitech.GetRawAxis(4)) > driveStickBuffer) { rightFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); rightBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); } else { rightFront.Set(0); rightBack.Set(0); } // END TANK DRIVE /***********************/ if ((buttonOne.Get()) == 1) { screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Not Pressed!"); } else { screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Pressed!"); } if ((buttonTwo.Get()) == 1) { screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Not Pressed!"); } else { screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Pressed!"); } if (((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 0)) { screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Right Position"); } //both on or off else if ((((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 1))||((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 0)) { screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Middle Position"); } //Left button on && right off else if (((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 1)) { screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Left Position"); } /***********************/ screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Pressure:%f", compressor.GetPressureSwitchValue()); screen -> PrintfLine(DriverStationLCD::kUser_Line5,"Left_%f Right_&f", leftFront.Get(), rightFront.Get()); screen -> PrintfLine(DriverStationLCD::kUser_Line6,"But6_%d But8_%d", logitech.GetRawButton(6), logitech.GetRawButton(8)); Wait(0.005); screen -> UpdateLCD(); } compressor.Stop(); }