void ManageCompressor () {
if (m_compressor->GetPressureSwitchValue()) {
m_compressor->Stop();
} else {
m_compressor->Start();
}
}
void RobotDemo::toggleCompressor(){
if(compressor->Enabled()){
compressor->Stop();
}
else{
compressor->Start();
}
}
/******************************** CONTINUOUS ROUTINES ********************************/
void DisabledContinuous(void) {
printf("Running in disabled continuous...\n");
GetWatchdog().Feed();
//Stop the presses...
drivetrain->Drive(0.0, 0.0);
compressor->Stop();
}
void toggleCompressor(bool start, bool stop)
{
if(start)
{
comp599->Start();
}
else if(stop)
{
comp599->Stop();
}
}
void DisabledInit(void) {
printf("Robot disabled initializing...\n");
GetWatchdog().Feed();
//Stop the presses...
drivetrain->Drive(0.0, 0.0);
compressor->Stop();
printf("Robot disabled initialization complete.\n");
}
void teleDrive()
{
drive->setLinVelocity(-oi->getDriveJoystick()->GetY(Joystick::kRightHand));
drive->setTurnSpeed(oi->getDriveJoystick()->GetX(Joystick::kRightHand), oi->getDriveJoystickButton(1));
drive->drive();
drive->shift(oi->getDriveJoystickButton(8), oi->getDriveJoystickButton(9));
if(oi->getDriveJoystickButton(6))
{
comp599->Start();
}
else if(oi->getDriveJoystickButton(7))
{
comp599->Stop();
}
}
void OperatorControl()
{
compressor.Start();
while (IsOperatorControl())
{
if(stick.GetRawButton(6)) //press the upper right trigger
{
piston.Set(true);
}
else if(stick.GetRawButton(8)) //press the lower right trigger
{
piston.Set(false);
}
}
compressor.Stop();
}
/**
* 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 RA14Robot::DisabledInit() {
myCompressor->Stop();
Config::LoadFromFile("config.txt");
if (alreadyInitialized) {
missionTimer->Stop();
Config::Dump();
}
#ifndef DISABLE_SHOOTER
myCam->Reset();
#endif //Ends DISABLE_SHOOTER
/*
if(fout.is_open()) {
cout << "Closing logging.csv..." << endl;
fout.close();
}
*/
}
/**
* 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 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 TeleopInit()
{
pcm.SetClosedLoopControl(false);
pcm.Stop();
//pcm.SetCompressor(false);
}
void tick_itf_pneumatic() {
Concurrent::IPCMutex *mtx = Memory::shared_mutex()->pcm;
for (int i = 0; i < 2; i++) {
MTX_LOCK(mtx, i);
Memory::Shared::IO::Pneumatics *pcm = Memory::shared()->pneumatics(i);
if (pcm->get_init()) {
Solenoid **sol = sols[i];
int modid = pcm->get_pcm_can_id();
if (!pcm->get_bootstrap()) {
cmps[i] = new Compressor(modid);
pcm->set_closed_loop(cmps[i]->GetClosedLoopControl());
for (int j = 0; j < 8; j++) {
sol[j] = new Solenoid(modid, j);
}
pcm->set_bootstrap(true);
}
Compressor *cmp = cmps[i];
pcm->set_fault_comp_too_high(cmp->GetCompressorCurrentTooHighFault());
pcm->set_fault_comp_not_conn_sticky(cmp->GetCompressorCurrentTooHighStickyFault());
pcm->set_fault_comp_shorted(cmp->GetCompressorShortedFault());
pcm->set_fault_comp_shorted_sticky(cmp->GetCompressorShortedStickyFault());
pcm->set_fault_comp_not_conn(cmp->GetCompressorNotConnectedFault());
pcm->set_fault_comp_not_conn_sticky(cmp->GetCompressorNotConnectedStickyFault());
if (pcm->get_comp_sticky_clear_pending()) {
cmp->ClearAllPCMStickyFaults();
pcm->set_comp_sticky_clear_pending(false);
}
pcm->set_pressure_switch(cmp->GetPressureSwitchValue());
if (pcm->get_closed_loop_mode_pending()) {
cmp->SetClosedLoopControl(pcm->get_closed_loop());
pcm->set_closed_loop_mode_pending(true);
}
pcm->set_is_enabled(cmp->Enabled());
if (pcm->get_start_pending()) {
cmp->Start();
pcm->set_start_pending(false);
}
if (pcm->get_stop_pending()) {
cmp->Stop();
pcm->set_stop_pending(false);
}
for (int j = 0; j < 8; j++) {
Solenoid *s = sol[j];
s->Set(pcm->get_solenoid(j));
pcm->set_solenoid_black(j, s->IsBlackListed());
}
Solenoid *s = sol[0];
pcm->set_fault_sol_volt(s->GetPCMSolenoidVoltageFault(modid));
pcm->set_fault_sol_volt_sticky(s->GetPCMSolenoidVoltageStickyFault(modid));
if (pcm->get_sol_sticky_clear_pending()) {
s->ClearAllPCMStickyFaults(modid);
pcm->set_sol_sticky_clear_pending(true);
}
pcm->set_comp_current(cmp->GetCompressorCurrent());
}
MTX_UNLOCK(mtx, i);
}
}
void Autonomous(void)
{
GetWatchdog().Kill();
cmp->Start();//compressor
cd->MecanumDrive(0, 0, 0);
minibot->Set(DoubleSolenoid::kForward);
while(IsAutonomous() && IsEnabled())
{
GetWatchdog().Kill();
AlmostZeroPi = manEnc->GetDistance() -1;//Zeros encoder on manipulator with margin for error
/* Before including below, check line tracking is proper, then fix below code for bottom scoring
cd->MecanumDrive(0, -.4, 0);
Wait(.15f);
if(IO.GetDigital(11))
{
while(!track->TrackLine(-1))
{
if(!IsEnabled()) break;
}
while(!track->TrackLine(0))
{
if(!IsEnabled()) break;
}
};
if(IO.GetDigital(13))
{
while(!track->TrackLine(0))
{
if(!IsEnabled()) break;
}
};
if(IO.GetDigital(15))
{
while(!track->TrackLine(1))
{
if(!IsEnabled()) break;
}
while(!track->TrackLine(0))
{
if(!IsEnabled()) break;
}
*/};/*
break;
};
//man open && score here
cd->MecanumDrive(0, .35, 0);
Wait(1.0f);
//move manipulator to 0
cd->MecanumDrive(0, .35, 0);
Wait(.8f);
cd->MecanumDrive(0, 0, 0);*/
if (!game){//if you arent in the game, kill compressor and brings manipulator to 0 so you dont need to reset
cmp->Stop();
while (manEnc->GetDistance() < AlmostZeroPi)
{
manJag->Set(.1);
}
}
}
void Reset() {
m_compressor->Stop();
m_fakePressureSwitch->Set(false);
}
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();
}
void OperatorControl(void)
{
// feed watchdog --- TheVessel.SetSafetyEnabled(true);
GetWatchdog().Kill();
if (!game){
AlmostZeroPi = manEnc->GetDistance() -1;//Zeros manipulator encoder-- only here for tele op practice w/out auto
cmp->Start();
}
cd->MecanumDrive(0, 0, 0);
minibot->Set(DoubleSolenoid::kForward);
while(IsOperatorControl() && IsEnabled())
{
GetWatchdog().Kill();
if(IO.GetDigital(10)) minibot->Set(DoubleSolenoid::kReverse);
else if(!IO.GetDigital(10)) minibot->Set(DoubleSolenoid::kForward);
cd->MecanumDrive(controller);//passes joystick controller's input to cd mechdrv
if(stick->GetRawButton(1))
{
}
else if(stick->GetRawButton(2))
{
}
else if(stick->GetRawButton(3))
{
reset = true;// ENABLES NO RESTRICTION MANIPULATOR MOVEMENT
}
else if(stick->GetRawButton(4))
{
cout << "Manipulator Encoder Value: " << manEnc->Get() << "/n";//prints manip encoder to console if open;
Wait(.1f);//pause so not above a billion times
}
else if(stick->GetRawButton(6))
{
open->Set(true);//self explanatory, eote pnuematics
closed->Set(false);
}
else if(stick->GetRawButton(8))
{
closed->Set(true);
open->Set(false);
}
ShoulderJag->Set(stick->GetY());//shoulder moves with stick->GetY()
if (stick->GetTwist() < 0 || manEnc->GetDistance() < AlmostZeroPi || reset)//man extending, enc not at 0, or reset true
{
manJag->Set(stick->GetTwist()*.3);//shoulder above but for wrist, also, only 3/10ths power
}
}
cmp->Stop();//stops compressor
while (manEnc->GetDistance() < AlmostZeroPi)//brings manipulator back to 0 so not need reset
{
manJag->Set(.1);
}
}