Rewrote Robot Code: Opinions/Thoughts?

[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);
	}
}

[ahartnet]

A couple of comments:

1) Many motor controllers have an input directly on the motor controller for limit switches. You obviously can do this in code still, but just wanted to point it out.
2) You should look into command based programming. While it's "overkill" for your project - if you're hoping to take your programming to the next level in FRC, the command based programming allows you to set up various commands/subsystems that can then be reused in groups of commands. I personally am a big fan of thinking of programming commands into the robot vs programming a button that turn X motor. https://wpilib.screenstepslive.com/s...ed-programming
3) I would absolutely break up your for loop into two separate ones. With what you have right now, if you only had 2 automodes you'd get an index out of bounds mode. And really, the number of drive motors you have shouldnt change. Just code those four lines out rather than have a for loop for it.

Nothing other than the max function use jumps out as a particularly bad practices to me. I would prefer renaming the dm# to leftFront, leftBack, rightFront, rightBack, and a more useful description for ds#.

You might consider having something to the driver station to report if you're limited your drivetrain speed or not.

[euhlmann]

A lot of these recommendations are based on the assumption that people will want to read your code (that includes you, two weeks or so after you finish this project and stop looking at the code).

	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";

Don't define choices twice. It just makes everything confusing. Try this instead:

private String[] autonomousChoices = {
	"Tank Drive",
	"Arcade Drive",
	"Rough Terrain",
	"Portcullis",
	"Chival de Frise",
	"Low Bar (shooting)",
	"Low Bar (without shooting)",
	"Moat"
};

The same thing applies for drive motors, and also cameras/doublesolenoids if you wanted to make those arrays (I think you should). Just make sure you define your arrays as "VictorSP[] driveMotors" for example, and not "Object[] driveMotors". That makes it a bit easier to read.

There's no need to publish arm speeds in the Robot constructor - you won't know them at that point, plus the robot is disabled. Instead, publish them in the teleop loop.

Instead of using magic numbers like "ds1 = new DoubleSolenoid(0, 1)", use a RobotMap file to store the numbers. That makes it easier to read. Right now it's not easy to figure out what 0 and 1 mean in the context. Instead, you can have nice code like "sampleTextSolenoid = new DoubleSolenoid(RobotMap.SAMPLE_TEXT_HARDWARE_DOWN, RobotMap.SAMPLE_TEXT_HARDWARE_UP)" where it's clear what the numbers mean.

Change some functions a bit. "closeOpenCam" might be better as "reinitializeCameraStream(Camera setActive, Camera setInactive)"
"rumbleABit" can be refactored as "rumbleFor(double seconds)"

Finally:
This isn't code golf!! As @ahartnet said, separate the logic for drive motors and auto modes. When writing code like this, always go for readability and not shortest length possible.

[AMendenhall]

In reference to euhlmann's statement about your declaration of drive motors:

Just make sure you define your arrays as "VictorSP[] driveMotors" for example, and not "Object[] driveMotors". That makes it a bit easier to read.

Not only does defining the array with type VictorSP make your code easy to read, it can prevent errors from happening in future.

Imagine this scenario:

Object driveMotor;
...
driveMotor = new VictorSP(1);
...
driveMotor.set(0.5);

Now you'll get a compiler error on the line "driveMotor.set(0.5);" This is because the compiler (and your code) doesn't know that driveMotor references a VictorSP. Sure, you know that since you assigned it to one in the previous line, but driveMotor was <i>declared</i> at the beginning as an object. You'd have to cast to a VictorSP, but that could lead to runtime errors because driveMotor could reference a USBCamera or a Date or a Potato.

The point is, when you declare a new variable, its type is all the rest of your code has to go by. Thus, you should be as specific as possible when declaring its type (by this I mean the lowest in the tree. In Java, every class extends Object. If a class has subclasses, unless for some reason the type isn't supposed to be known, you should declare it as one of those subclasses).

Amended:

VictorSP driveMotor;
...
driveMotor = new VictorSP(1);
...
//No Errors!
driveMotor.set(0.5);