void ManageCompressor () {
if (m_compressor->GetPressureSwitchValue()) {
m_compressor->Stop();
} else {
m_compressor->Start();
}
}
void Run(CompressorInputs input) {
//cout << input.enable;
//compressor->Set(Relay::kOn);
//compressor->Set(input.enable ? Relay::kOn : Relay::kOff);
//cout << compressor->GetPressureSwitchValue();
lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Pressure Switch: %x",
compressor->GetPressureSwitchValue());
}
/**
* Initialization code for autonomous mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters autonomous mode.
*/
void RobotDemo::AutonomousInit() {
compressor->Start(); //starting compressor to ready for teleop
SmartDashboard::PutBoolean("Left-Spinner", spinnerStatusLeft);
SmartDashboard::PutBoolean("Right-Spinner", spinnerStatusLeft);
SmartDashboard::PutBoolean("S1-Status", s1Status);
SmartDashboard::PutBoolean("S2-Status", s2Status);
SmartDashboard::PutBoolean("S3-Status", s3Status);
SmartDashboard::PutNumber("Compressor-Pressure", compressor->GetPressureSwitchValue());
//based on switch input on I/O tab of Driver Station--tells robot whether it
//bool sideStart = true;
//bool centerStartToLeft = false;
//bool centerStartToRight = false;
retract(s2,s2Status); //catcher arms come in first
Wait(.2);
retract(s3,s3Status);
retract(s1,s1Status);
//test this module,
/*bool sideStart = ds->GetDigitalIn(1); //is starting in the middle.
bool centerStartToLeft = ds->GetDigitalIn(2); //center -> left low goal
bool centerStartToRight = ds->GetDigitalIn(3); //center -> right low goal
*/
//if(sideStart){ //either side--face right
move_horizontal(-15.5); //MUST make sure robot is facing right direction!!!
spinnerLeft(SPINNER_SPEED);
spinnerRight(-SPINNER_SPEED);
Wait(3.0); //arbitrary time -- test
stopSpinnersLeft();
stopSpinnersRight();
stop();
//}
}
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
}
void OperatorControl()
{
compressor.Start();
myRobot.SetSafetyEnabled(true);
myRobot.SetInvertedMotor(myRobot.kRearLeftMotor, true);//Invert rear left Motor
myRobot.SetInvertedMotor(myRobot.kRearRightMotor, true);//Invert rear right motor
myRobot.SetInvertedMotor(myRobot.kFrontLeftMotor, true);//Invert rear right motor
myRobot.SetInvertedMotor(myRobot.kFrontRightMotor, true);
DriverStation *ds;
DriverStationLCD *dsLCD;
ds = DriverStation::GetInstance();
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1,1, "Starting Teleop");
dsLCD->UpdateLCD();
while (true)
{
if (!compressor.GetPressureSwitchValue()){
compressor.Start();
}
myRobot.ArcadeDrive(stick);
/*PNEUMATIC CODE*/
if (stick.GetRawButton(8)){
shooterSole.Set(shooterSole.kForward);
}
if (stick.GetRawButton(1)){
shooterSole.Set(shooterSole.kReverse);
}
/*SHOOTER CODE*/
if (stick.GetRawButton(2)){
shooter.SetSpeed(1);
}
if (stick.GetRawButton(4)){
shooter.SetSpeed(-1);
dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Shooter Should be negative");
dsLCD->UpdateLCD();
}
if (!stick.GetRawButton(4) || !stick.GetRawButton(2)){
shooter.SetSpeed(0);
}
/* FORKLIFT CODE*/
if (!stick.GetRawButton(5) || !stick.GetRawButton(6)){
forklift.SetSpeed(0);
}
if (stick.GetRawButton(5)){
forklift.SetSpeed(1);
}
if (stick.GetRawButton(6)){
forklift.SetSpeed(-1);
dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Forklift Should be negative");
dsLCD->UpdateLCD();
}
if (!shoot.Get()){
shooter.SetSpeed(0);
shooterSole.Set(shooterSole.kForward);
}
if (stick.GetRawButton(11)){
//myRobot.m_rearLeftMotor.SetSpeed(1);
//myRobot.m_rearRightMotor.SetSpeed(1);
//myRobot.m_frontLeftMotor.SetSpeed(1);
//myRobot.m_frontRightMotor.SetSpeed(1);
}
//Wait(0.005);// wait for a motor update time
}
}
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 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();
}