Here it is:
Code:
package org.usfirst.frc.team2221.robot;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.GenericHID.Hand;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.RobotDrive.MotorType;
public class Robot extends IterativeRobot {
/*--------------------------------------------------------------------*/
/*----------------------Setup Talons---------------------*/
Talon frontLeft = new Talon(0);
Talon rearLeft = new Talon(1);
Talon frontRight = new Talon(2);
Talon rearRight = new Talon(3);
Talon Spare = new Talon(4);
/*--------------------------------------------------------------------*/
/*---------------------CONTROLLERS-----------------------------*/
XboxController xbox1 = new XboxController(0); // Driver Xbox (xboxController)
XboxController xbox2 = new XboxController(1); // Manipulator Xbox (xboxController)
/*--------------------------------------------------------------------*/
/*----------------------Controller Settings----------------------------------*/
double xbox1_value_x; // Xbox1 x-values
double xbox1_value_y; // Xbox1 y-values
double xbox2_value_triggerRight = xbox2.getTriggerAxis(Hand.kRight);
double xbox2_value_triggerLeft = xbox2.getTriggerAxis(Hand.kLeft);
/*--------------------------------------------------------------------*/
/*---------------------Setup Arcade Drive-----------------------------*/
RobotDrive arcadeDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
/*--------------------------------------------------------------------*/
/*----------------------Autonomous Chooser---------------------------*/
SendableChooser<String> chooser = new SendableChooser<>();
final String leftAuto = "Drive Left";
final String straightAuto = "Drive Straight";
final String rightAuto = "Drive Right";
final String spyAuto = "Spybot";
/*--------------------------------------------------------------------*/
/*-----------------------Setup Compressor----------------------*/
Compressor comp = new Compressor(0); // the Zero is the PCM NODE ID in the Web Environment
/*--------------------------------------------------------------------*/
/*---------------------Setup Solenoids (And Triggers)---------------*/
DoubleSolenoid solenoid1 = new DoubleSolenoid(0, 1);
DoubleSolenoid solenoid2 = new DoubleSolenoid(2, 3);
DoubleSolenoid solenoid3 = new DoubleSolenoid(4, 5);
boolean solenoid1_trigger;
boolean solenoid2_trigger;
boolean solenoid3_trigger;
final String auto1 = "autoOne";
String autoSelected;
private double autoStartTime;
public Robot() {
arcadeDrive.setExpiration(0.1);
}
@Override
public void robotInit() {
comp.setClosedLoopControl(true);
arcadeDrive.setInvertedMotor(MotorType.kFrontLeft, false);
arcadeDrive.setInvertedMotor(MotorType.kFrontRight, false);
arcadeDrive.setInvertedMotor(MotorType.kRearLeft, false);
arcadeDrive.setInvertedMotor(MotorType.kRearRight, true);
}
@Override
public void autonomousInit(){
autoStartTime = Timer.getFPGATimestamp();
autoSelected = (String) chooser.getSelected();
System.out.println("Auto selected: " + autoSelected);
Timer timer = new Timer();
timer.start();
}
@Override
public void autonomousPeriodic() {
double currTime = Timer.getFPGATimestamp();
double timeElapsed = currTime - autoStartTime;
System.out.println("Auto selected: " + autoSelected);
switch (autoSelected) {
case leftAuto:
// Disable watchdog when autonomous driving
arcadeDrive.setSafetyEnabled(false);
System.out.print("You're left");
if ( timeElapsed < 2)
{
arcadeDrive.drive(1, 0);
}
else if (timeElapsed < 4)
{
arcadeDrive.drive(0.75, .6);
}
else if (timeElapsed < 6)
{
arcadeDrive.drive(0, 0);
solenoid2.set(DoubleSolenoid.Value.kForward);
}
else if (timeElapsed < 8.5)
{
solenoid2.set(DoubleSolenoid.Value.kReverse);
arcadeDrive.drive(-1, .8);
}
else if (timeElapsed < 9.5)
{
arcadeDrive.drive(-.25, 0);
}
else if (timeElapsed > 11)
{
arcadeDrive.drive(0, 0);
}
arcadeDrive.setSafetyEnabled(true);
break;
case "straightAuto":
arcadeDrive.setSafetyEnabled(false);
System.out.print("you're straight");
if(timeElapsed < 2)
{
arcadeDrive.drive(.5, 1);
}
else if(timeElapsed < 3)
{
arcadeDrive.drive(0, 0);
}
else if (timeElapsed < 3.5)
{
solenoid2.set(DoubleSolenoid.Value.kForward);
}
else if (timeElapsed < 4.5)
{
solenoid2.set(DoubleSolenoid.Value.kReverse);
}
else if (timeElapsed < 5)
{
arcadeDrive.drive(-.75, 0);
}
else if (timeElapsed > 8)
{
arcadeDrive.drive(0, 0);
}
else if (timeElapsed > 10)
{
arcadeDrive.setSafetyEnabled(true);
}
break;
case "rightAuto":
System.out.print("you're right");
arcadeDrive.setSafetyEnabled(false);
if(2.5 < timeElapsed && timeElapsed < 5)
{
arcadeDrive.drive(1, 0);
}
else if (timeElapsed < 7)
{
arcadeDrive.drive(0.75, -.6);
solenoid2.set(DoubleSolenoid.Value.kForward);
}
else if (timeElapsed < 9){
solenoid2.set(DoubleSolenoid.Value.kReverse);
arcadeDrive.drive(-1, -.8);
}
else if (timeElapsed < 11 )
{
arcadeDrive.drive(-.25, 0);
}
else if (timeElapsed > 12)
{
arcadeDrive.drive(0, 0);
arcadeDrive.setSafetyEnabled(true);
}
break;
default:
arcadeDrive.setSafetyEnabled(false);
System.out.print("YOU'RE LEFT");
if ( timeElapsed < 2)
{
arcadeDrive.drive(1, 0);
}
else if (timeElapsed < 4)
{
arcadeDrive.drive(0.75, .6);
}
else if (timeElapsed < 6)
{
arcadeDrive.drive(0, 0);
solenoid2.set(DoubleSolenoid.Value.kForward);
}
else if (timeElapsed < 8.5)
{
solenoid2.set(DoubleSolenoid.Value.kReverse);
arcadeDrive.drive(-1, .8);
}
else if (timeElapsed < 9.5)
{
arcadeDrive.drive(-.25, 0);
}
else if (timeElapsed > 11)
{
arcadeDrive.drive(0, 0);
}
// Enable watchdog on driving again
arcadeDrive.setSafetyEnabled(true);
break;
}
}
@Override
public void teleopInit(){
}
@Override
public void teleopPeriodic() {
arcadeDrive.setSafetyEnabled(true);
//-----------------------Driver Controller---------------------------------//
if(xbox1.getAButton()){
xbox1_value_x = xbox1.getX(Hand.kLeft) * 0.75;
xbox1_value_y = xbox1.getY(Hand.kLeft) * 0.75;
}
else{
xbox1_value_x = xbox1.getX(Hand.kLeft);
xbox1_value_y = xbox1.getY(Hand.kLeft);
}
arcadeDrive.arcadeDrive(xbox1_value_x, xbox1_value_y);
//-----------------------Manipulator Controller---------------------------------//
if(xbox2.getBButton()){
if(solenoid1_trigger != true){
solenoid1.set(DoubleSolenoid.Value.kForward);
solenoid1_trigger = true;
xbox2.setRumble(RumbleType.kRightRumble, 1);
Timer.delay(.5);
xbox2.setRumble(RumbleType.kRightRumble, 0);
}
else
{
solenoid1.set(DoubleSolenoid.Value.kReverse);
solenoid1_trigger = false;
Timer.delay(.5);
}}
else if(xbox2.getXButton()){
if(solenoid2_trigger != true){
solenoid2.set(DoubleSolenoid.Value.kForward);
solenoid2_trigger = true;
xbox2.setRumble(RumbleType.kRightRumble, 1);
Timer.delay(.5);
xbox2.setRumble(RumbleType.kRightRumble, 0);
}
else
{
solenoid2.set(DoubleSolenoid.Value.kReverse);
solenoid2_trigger = false;
Timer.delay(.5);
}
}
else if(xbox2.getYButton()){
if(solenoid3_trigger != true){
solenoid3.set(DoubleSolenoid.Value.kForward);
solenoid3_trigger = true;
xbox2.setRumble(RumbleType.kRightRumble, 1);
Timer.delay(.5);
xbox2.setRumble(RumbleType.kRightRumble, 0);
}
else
{
solenoid3.set(DoubleSolenoid.Value.kReverse);
solenoid3_trigger = false;
Timer.delay(.5);
}
}
if(xbox2.getBumper(Hand.kLeft) == true){
Spare.set(xbox2_value_triggerLeft);
}
else if (xbox2.getBumper(Hand.kRight) == true){
Spare.set(xbox2_value_triggerRight);
}
}
}