[Lesafian]

I've decided to sharpen my skills and rewrite our robot code without reference to the old one (reason for TODO:'s on the autonomous choices), I'd like to know if I'm doing anything inefficient or if there's just a better way to do things. Thanks!

package org.usfirst.frc.team6077.robot;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.Joystick.*;
import edu.wpi.first.wpilibj.smartdashboard.*;
import edu.wpi.first.wpilibj.vision.*;

public class Robot extends SampleRobot {

	private RobotDrive drive;
	private VictorSP dm1, dm2, dm3, dm4;
	private TalonSRX armMotor, slideMotor;
	private DigitalInput limitSwitch;
	private Joystick xboxController;
	private SendableChooser driveChooser;
	private SendableChooser autoChooser;
	private CameraServer camServer;
	private USBCamera camera, camera2, selectedCamera;
	private DoubleSolenoid ds1, ds2;
	private Compressor compressor;

	private Object[] driveMotors = { dm1, dm2, dm3, dm4 };
	private String[] autoArray = { autoChoice1, autoChoice2, autoChoice3, autoChoice4, autoChoice5, autoChoice6 };

	private static final String tankDrive = "Tank Drive";
	private static final String arcadeDrive = "Arcade Drive";
	private static final String autoChoice1 = "Rough Terrain";
	private static final String autoChoice2 = "Portcullis";
	private static final String autoChoice3 = "Chival de Frise";
	private static final String autoChoice4 = "Low Bar (shooting)";
	private static final String autoChoice5 = "Low Bar (without shooting)";
	private static final String autoChoice6 = "Moat";

	public Robot() {
		// SmartDashboard
		//
		// Drive Chooser
		driveChooser = new SendableChooser();
		driveChooser.addDefault(arcadeDrive, arcadeDrive);
		driveChooser.addObject(tankDrive, tankDrive);
		SmartDashboard.putData("Drive Choices", driveChooser);

		// Autonomous Chooser & Drive Motors
		autoChooser = new SendableChooser();
		for (int i = 0; i <= (Math.max(autoArray.length, driveMotors.length)); i++) {
			autoChooser.addObject(autoArray[i], autoArray[i]);
			if (i <= driveMotors.length) {
				driveMotors[i] = new VictorSP(i);
			}
		}
		SmartDashboard.putData("Autonomous Choices", autoChooser);

		// Motor Speeds
		SmartDashboard.putNumber("Arm Speed", armMotor.get());
		SmartDashboard.putNumber("Carriage Speed", slideMotor.get());
		SmartDashboard.putNumber("Drive Speed", getDriveSpeed());

		// Drive config
		drive = new RobotDrive(dm1, dm2, dm3, dm4);

		// Joystick(s)
		xboxController = new Joystick(0);

		// Scoring utilities
		armMotor = new TalonSRX(4);
		slideMotor = new TalonSRX(5);

		// Digital Input
		limitSwitch = new DigitalInput(0);

		// Pneumatics
		compressor = new Compressor();
		compressor.setClosedLoopControl(true);
		compressor.start();
		ds1 = new DoubleSolenoid(0, 1);
		ds2 = new DoubleSolenoid(2, 3);

		// Camera
		camera = new USBCamera("cam0");
		camera2 = new USBCamera("cam1");
		camServer = CameraServer.getInstance();
		camServer.setQuality(100);
		closeOpenCam(camera2, camera);

	}

	public void autonomous() {
		switch (autoChooser.getSelected().toString()) {
		case (autoChoice1):
			// TODO:
			break;
		case (autoChoice2):
			// TODO:
			break;
		case (autoChoice3):
			// TODO:
			break;
		case (autoChoice4):
			// TODO:
			break;
		case (autoChoice5):
			// TODO:
			break;
		case (autoChoice6):
			// TODO:
			break;
		}
	}

	public void operatorControl() {
		drive.setSafetyEnabled(true);
		while (isOperatorControl() && isEnabled()) {
			String selectedDrive = driveChooser.getSelected().toString();
			if (selectedDrive.equalsIgnoreCase(arcadeDrive)) {
				drive.arcadeDrive(Values.XBOX_LEFTSTICK_X, -Values.XBOX_LEFTSTICK_Y);
				if (Math.abs(xboxController.getRawAxis(Values.XBOX_RIGHTSTICK_Y)) > 0) {
					slideMotor.set(xboxController.getRawAxis(Values.XBOX_RIGHTSTICK_Y));
				} else {
					slideMotor.set(0);
				}
			} else {
				drive.tankDrive(Values.XBOX_LEFTSTICK_Y, Values.XBOX_RIGHTSTICK_Y);
				if (xboxController.getRawButton(Values.XBOX_Y)) {
					slideMotor.set(1);
				} else if (xboxController.getRawButton(Values.XBOX_B)) {
					slideMotor.set(-1);
				} else {
					slideMotor.set(0);
				}
			}

			// Limit Drive speed;
			if (xboxController.getRawButton(Values.XBOX_A)) {
				drive.setMaxOutput(0.75);
			} else {
				drive.setMaxOutput(1.0);
			}

			// No Turn Driving
			// TODO:

			// Use Arms
			if (Math.abs(xboxController.getRawAxis(Values.XBOX_TRIGGER)) > 0) {
				armMotor.set(xboxController.getRawAxis(Values.XBOX_TRIGGER));
			} else {
				armMotor.set(0);
			}

			// LimitSwitch
			if (limitSwitch.get()) {
				if (slideMotor.getSpeed() > 0) {
					slideMotor.stopMotor();
				}
			}

			// Claw
			if (xboxController.getRawButton(Values.XBOX_LEFT_BUMPER)) {
				if (ds1.get() == DoubleSolenoid.Value.kForward) {
					ds1.set(DoubleSolenoid.Value.kReverse);
				} else {
					ds1.set(DoubleSolenoid.Value.kForward);
				}
			}

			// Shooter;
			if (xboxController.getRawButton(Values.XBOX_RIGHT_BUMPER)) {
				if (ds1.get() == DoubleSolenoid.Value.kForward) {
					rumbleABit();
				} else {
					ds2.set(DoubleSolenoid.Value.kForward);
					Timer.delay(0.75);
					ds2.set(DoubleSolenoid.Value.kReverse);
				}
			}

			// Camera Switcher
			if (xboxController.getRawButton(Values.XBOX_X)) {
				if (selectedCamera == camera) {
					closeOpenCam(camera, camera2);
				} else {
					closeOpenCam(camera2, camera);
				}
			}
		}
	}

	public void test() {
		// TODO: Nothing :P
	}

	private void closeOpenCam(USBCamera stopcam, USBCamera startcam) {
		// Stop first cam
		stopcam.stopCapture();
		stopcam.closeCamera();
		stopcam.updateSettings();

		// Start second cam
		selectedCamera = startcam;
		startcam.setSize(Values.CAM_WIDTH, Values.CAM_HEIGHT);
		startcam.openCamera();
		startcam.startCapture();
		startcam.updateSettings();
		camServer.startAutomaticCapture(selectedCamera);
	}

	private double getDriveSpeed() {
		return (dm1.get() + dm2.get() + dm3.get() + dm4.get()) / 4.0;
	}

	private void rumbleABit() {
		xboxController.setRumble(RumbleType.kLeftRumble, 1);
		xboxController.setRumble(RumbleType.kRightRumble, 1);
		Timer.delay(0.5);
		xboxController.setRumble(RumbleType.kLeftRumble, 0);
		xboxController.setRumble(RumbleType.kRightRumble, 0);
	}
}