void SquareInputs(void)
{
if(stick.GetY() < 0)
{
if(DoubleSolenoid::kReverse == shifter.Get())
{
myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * -4.0), stick.GetX());
}
else if(DoubleSolenoid::kForward == shifter.Get())
{
myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * -1.0), stick.GetX());
}
}
else if(stick.GetY() > 0)
{
if(DoubleSolenoid::kReverse == shifter.Get())
{
myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * 4.0), stick.GetX());
}
else if(DoubleSolenoid::kForward == shifter.Get())
{
myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * 1.0), stick.GetX());
}
}
}
void LogToDashboard()
{
_arm->logInfo();
_lightSensors->logInfo();
// minibot deployment
SmartDashboard::Log(MinibotDeployable(), "Minibot Deployable");
SmartDashboard::Log(_minibotDeployerSolenoid->Get(), "Minibot Deployed");
}
void loadCatapult()
{
if (buttonOne.Get()==1 && buttonTwo.Get()==1 && dogSolenoid.Get()==DoubleSolenoid::kReverse)
{
dogSolenoid.Set(DoubleSolenoid::kForward);
Wait(0.5);
ratchetSolenoid.Set(DoubleSolenoid::kForward);
Wait(0.5);
catapultMotor.Set(1);
}
}
void 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);
}
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 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();
}
}
/****** 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();
}