void TeleopPeriodic(void)
{
Scheduler::GetInstance()->Run();
myDrive_all->TankDrive(drivestick_left->GetY(), drivestick_right->GetY());
ABCheck = Xbox_Button_A->Get(); // Gets the value of the A button on the xbox
BBCheck = Xbox_Button_B->Get(); // Gets the value of the B button on the xbox
if (ABCheck == 1) // If A button is pressed
{
FirstSolenoid->Set(DoubleSolenoid::kForward); // Make piston extend
}
if (BBCheck == 1) // If B button is pressed
{
FirstSolenoid->Set(DoubleSolenoid::kReverse); // Make piston retract
}
if ((ABCheck == 0) && (BBCheck == 0)) // If A and B buttons are not pressed
{
FirstSolenoid->Set(DoubleSolenoid::kOff); // Make piston stay where it is
}
XBCheck = Xbox_Button_X->Get(); // Gets value of the X button on the xbox
YBCheck = Xbox_Button_Y->Get(); // Get value of the Y button on the xbox
if (XBCheck == 1)//If X button is pressed
{
if (limitSwitch->Get() == 0) // If limitswitch is pressed
{
intake_Motor->Set(0); // Stop test motor
}
if (limitSwitch->Get() == 1) // If limitswitch is not pressed
{
intake_Motor->Set(1); // Test motor goes forward at 0.5 speed
}
}
if (YBCheck == 1) // If Y button is pressed
{
intake_Motor->Set(-1); // Test motor goes backward at -0.5 speed
}
if ((XBCheck == 0) && (YBCheck == 0)) // If X and Y buttons are not pressed
{
intake_Motor->Set(0); // Test motor stops
}
}
void OperatorControl(void)
{
JankyTurret turret(7,11,10);
JankyTargeting targ(&turret);
JankyShooter shoot(SHOOTER_JAGUAR_CHANNEL,SHOOTER_ENCODER_A,SHOOTER_ENCODER_B);
int rpmForShooter=0;
while (IsOperatorControl())
{
//GetImage()
//hsl.Save
// myRobot.ArcadeDrive(stick);
shoot.GetCurrentRPM();
if (button.Get()==true)
{
LEDRelay.Set(Relay::kForward);
if (button_H.Get()==true)
{
targ.SetLMHTarget(BOGEY_H);
SmartDashboard::PutString("Targeting","High Button Pressed");
}
if (button_M.Get()==true)
{
targ.SetLMHTarget(BOGEY_M);
SmartDashboard::PutString("Targeting","Medium Button Pressed");
}
if (button_L.Get()==true)
{
targ.SetLMHTarget(BOGEY_L);
SmartDashboard::PutString("Targeting","Low Button Pressed");
}
if (button_H.Get()==true || button_M.Get()==true || button_L.Get()==true)
{
if (targ.ProcessOneImage())
{
targ.ChooseBogey();
targ.MoveTurret();
rpmForShooter = targ.GetCalculatedRPM();
shoot.setTargetRPM(rpmForShooter);
targ.InteractivePIDSetup();
}
}
targ.StopPID();
shoot.setTargetRPM(0);
}
else
{
float lazysusan =stick.GetZ();
turret.Set(lazysusan);
LEDRelay.Set(Relay::kOff);
// Set shooter speed to zero.
}
// float desired=abs(stick.GetY() *1000) + 100;
// smarty->PutInt("Desired RPM1", (int)desired);
// shoot.setTargetRPM((int)desired);
Wait(0.05);
}
// After teleop runs, save preferences again.
Preferences::GetInstance()->Save();
}
/****************************************
* Runs the motors with arcade steering.*
****************************************/
void OperatorControl(void)
{
//TODO put in servo for lower camera--look in WPI to set
// Watchdog baddog;
// baddog.Feed();
myRobot.SetSafetyEnabled(true);
//SL Earth.Start(); // turns on Earth
// SmartDashboard *smarty = SmartDashboard::GetInstance();
//DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory)
//char debugout [100];
compressor.Start();
gyro.Reset(); // resets gyro angle
int rpmForShooter;
while (IsOperatorControl()) // while is the while loop for stuff; this while loop is for "while it is in Teleop"
{
// baddog.Feed();
//myRobot.SetSafetyEnabled(true);
//myRobot.SetExpiration(0.1);
float leftYaxis = driver.GetY();
float rightYaxis = driver.GetTwist();//RawAxis(5);
myRobot.TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1
float random = gamecomponent.GetY();
float lazysusan = gamecomponent.GetZ();
//bool elevator = Frodo.Get();
float angle = gyro.GetAngle();
bool balance = Smeagol.Get();
SmartDashboard::PutNumber("Gyro Value",angle);
int NumFail = -1;
//bool light = Pippin.Get();
//SL float speed = Earth.GetRate();
//float number = shooter.Get();
//bool highspeed = button1.Get()
//bool mediumspeed = button2.Get();
//bool slowspeed = button3.Get();
bool finder = autotarget.Get();
//bool targetandspin = autodistanceandspin.Get();
SmartDashboard::PutString("Targeting Activation","");
//dslcd->Clear();
//sprintf(debugout,"Number=%f",angle);
//dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout);
//SL sprintf(debugout,"Number=%f",speed);
//SL dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout);
//sprintf(debugout,"Number=%f",number);
//dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout);
//sprintf(debugout,"Finder=%u",finder);
//dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout);
//dslcd->UpdateLCD(); // update the Driver Station with the information in the code
// sprintf(debugout,"Number=%u",maxi);
// dslcd->Printf(DriverstationLCD::kUser_Line6,5,debugout)
bool basketballpusher = julesverne.Get();
bool bridgetipper = joystickbutton.Get();
if (bridgetipper) // if joystick button 7 is pressed (is true)
{
solenoid.Set(true); // then the first solenoid is on
}
else
{
//Wait(0.5); // and then the first solenoid waits for 0.5 seconds
solenoid.Set(false); //and then the first solenoid turns off
}
if (basketballpusher) // if joystick button 6 is pressed (is true)
{
shepard.Set(true); // then shepard is on the run
//Wait(0.5); // and shepard waits for 0.5 seconds
}
else
{
shepard.Set(false); // and then shepard turns off
} //10.19.67.9 IP address of computer;255.0.0.0 subnet mask ALL FOR WIRELESS CONNECTION #2
//}
//cheetah.Set(0.3*lazysusan);
// smarty->PutDouble("pre-elevator",lynx.Get());
lynx.Set(random);
// smarty->PutDouble("elevator",lynx.Get());
// smarty->PutDouble("joystick elevator",random);
if (balance) // this is the start of the balancing code
{
angle = gyro.GetAngle();
myRobot.Drive(-0.03*angle, 0.0);
Wait(0.005);
}
/*if (light) //button 5 turns light on oand off on game controller
flashring.Set(Relay::kForward);
else
flashring.Set(Relay::kOff);
*/
if (finder)
{
flashring.Set(Relay::kForward);
if (button_H.Get()==true)
{
targeting.SetLMHTarget(BOGEY_H);
SmartDashboard::PutString("Targeting","High Button Pressed");
}
if (button_M.Get()==true)
{
targeting.SetLMHTarget(BOGEY_M);
SmartDashboard::PutString("Targeting","Medium Button Pressed");
}
if (button_L.Get()==true)
{
targeting.SetLMHTarget(BOGEY_L);
SmartDashboard::PutString("Targeting","Low Button Pressed");
}
if (button_H.Get()==true || button_M.Get()==true || button_L.Get()==true)
{
if (targeting.ProcessOneImage())
{
NumFail = 0;
SmartDashboard::PutString("Targeting Activation","YES");
targeting.ChooseBogey();
targeting.MoveTurret();
#ifdef USE_HARDWIRED_RPM
shooter.setTargetRPM(HARDWIRED_RPM);
#else
rpmForShooter = targeting.GetCalculatedRPM();
shooter.setTargetRPM(rpmForShooter);
#endif
targeting.InteractivePIDSetup();
}
else
{
NumFail++;
if (NumFail > 10)
targeting.StopPID();
}
SmartDashboard::PutNumber("Numfail", NumFail);
shooter.setTargetRPM(rpmForShooter);
}
else
{
SmartDashboard::PutString("Targeting Activation","NO");
shooter.setTargetRPM(0);
targeting.StopPID();
}
}
else
{
flashring.Set(Relay::kOff);
targeting.StopPID();
turret.Set(lazysusan); // the lazy susan would turn right & left based on how far the person moves the right joystick#2 side to side
//targeting.StopPID();
//if (elevator) //shooter would move at full speed if button is pressed
//TODO Change RPM values
//TODO Disable calculation of RPM values
SmartDashboard::PutNumber("CurrentRPM",shooter.GetCurrentRPM());
if (button_H.Get() == true)
shooter.setTargetRPM((int)2100);
//From front of free throw line, should hit the backboard and go in
//used to be 2700 RPMs
else if (button_M.Get() == true)
shooter.setTargetRPM((int)1900);
//From front of free throw line, should go in the net--can shoot the next ball on the overshoot?
//Used to be 2250 RPMs
else if (button_L.Get() == true)
shooter.setTargetRPM((int)1350);
//From fender, should hit the backboard
//Used to be 2000 RPMs
//shooter.Set(0.5);
else
shooter.setTargetRPM(0);
// else if (mediumspeed)
//shooter.setTargetRPM((int)0);
//else if (slowspeed)
//shooter.setTargetRPM((int)0);
/*if (targetandspin) //code for autotargeting and speed will go here
{
shooter.setTargetRPM((int)1800);
}
else
{*/
//}
myRobot.TankDrive(leftYaxis,rightYaxis);
}
//Wait(0.005);
}
}
OI::OI() {
xBoxController.reset(new Joystick(0));
xBoxController2.reset(new Joystick(1));
// SmartDashboard Buttons
SmartDashboard::PutData("DriveWithJoysticks", new DriveWithJoysticks());
SmartDashboard::PutData("Autonomous Command", new AutonomousCommand());
SmartDashboard::PutData("PanCameraLeft", new PanCameraLeft());
SmartDashboard::PutData("ArmControl", new ArmControl());
JoystickButton* armRaiseButton = new JoystickButton(xBoxController2.get(), ARM_RAISE_BUTTON);
armRaiseButton->WhenPressed(new ArmControl());
JoystickButton* armLowerButton = new JoystickButton(xBoxController2.get(), ARM_LOWER_BUTTON);
armLowerButton->WhenPressed(new ArmControl());
JoystickButton* armExtendButton = new JoystickButton(xBoxController2.get(), ARM_EXTEND_BUTTON);
armExtendButton->WhenPressed(new ArmControl());
JoystickButton* armWinchButton = new JoystickButton(xBoxController2.get(), ARM_WINCH_BUTTON);
armWinchButton->WhenPressed(new ArmControl());
JoystickButton* spitBallButton = new JoystickButton(xBoxController2.get(), SPIT_BALL_BUTTON);
spitBallButton->WhenPressed(new RollingPinThing());
JoystickButton* eatBallButton = new JoystickButton(xBoxController.get(), EAT_BALL_BUTTON);
eatBallButton->WhenPressed(new RollingPinThing());
}
void OperatorControl(void)
{
bool button1Bool;
bool button2Bool;
bool button3Bool;
bool button4Bool;
bool button5Bool;
bool button6Bool;
bool button7Bool;
bool button8Bool;
bool button9Bool;
bool button10Bool;
bool button11Bool;
bool button12Bool;
float LeftaxisYValue;
float LeftaxisXValue;
float RightaxisXValue;
float RightaxisYValue;
float TriggerValue;
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
button1Bool = button1.Get();
SmartDashboard::PutNumber("button1",button1Bool);
button2Bool = button2.Get();
SmartDashboard::PutNumber("button2",button2Bool);
button3Bool = button3.Get();
SmartDashboard::PutNumber("button3",button3Bool);
button4Bool = button4.Get();
SmartDashboard::PutNumber("button4",button4Bool);
button5Bool = button5.Get();
SmartDashboard::PutNumber("button5",button5Bool);
button6Bool = button6.Get();
SmartDashboard::PutNumber("button6",button6Bool);
button7Bool = button7.Get();
SmartDashboard::PutNumber("button7",button7Bool);
button8Bool = button8.Get();
SmartDashboard::PutNumber("button8",button8Bool);
button9Bool = button9.Get();
SmartDashboard::PutNumber("button9",button9Bool);
button10Bool = button10.Get();
SmartDashboard::PutNumber("button10",button10Bool);
button11Bool = button11.Get();
SmartDashboard::PutNumber("button11",button11Bool);
button12Bool = button12.Get();
SmartDashboard::PutNumber("button12",button12Bool);
LeftaxisXValue = stick.GetX();
SmartDashboard::PutNumber("Joystick1 X Axis",LeftaxisXValue);
LeftaxisYValue = 0 - stick.GetY();
SmartDashboard::PutNumber("Joystick1 Y Axis",LeftaxisYValue);
RightaxisXValue = stick.GetTwist();
SmartDashboard::PutNumber("Joystick2 X Axis", RightaxisXValue);
RightaxisYValue = 0 - stick.GetRawAxis(5);
SmartDashboard::PutNumber("Joystick2 Y Axis", RightaxisYValue);
TriggerValue = stick.GetThrottle();
SmartDashboard::PutNumber("Trigger value", TriggerValue);
Wait(0.005); // wait for a motor update time
}
}
