Merged release/0.0.3 into master

3494_2017_repo/src/org/usfirst/frc/team3494/robot/AutoGenerator.java

@@ -0,0 +1,46 @@
+package org.usfirst.frc.team3494.robot;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive;
+import org.usfirst.frc.team3494.robot.commands.auto.XYDrive;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * Class containing methods that return valid lists to pass to
+ * {@link org.usfirst.frc.team3494.robot.commands.auto.ConstructedAuto}.
+ * 
+ * @since 0.0.3
+ * @see org.usfirst.frc.team3494.robot.commands.auto.ConstructedAuto
+ */
+public class AutoGenerator {
+	/**
+	 * Test method. Drives to XY (36, 36) (inches).
+	 * 
+	 * @since 0.0.3
+	 * @see org.usfirst.frc.team3494.robot.commands.auto.ConstructedAuto
+	 * @see org.usfirst.frc.team3494.robot.commands.auto.XYDrive
+	 * @return A list of commands suitable for use with
+	 *         {@link org.usfirst.frc.team3494.robot.commands.auto.ConstructedAuto}.
+	 */
+	public static ArrayList<Command> autoOne() {
+		ArrayList<Command> list = new ArrayList<Command>();
+		list.add(new XYDrive(36, 36));
+		return list;
+	}
+
+	/**
+	 * Drives to the baseline and earns us five points (hopefully.)
+	 * 
+	 * @see org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive
+	 * @since 0.0.3
+	 * @return A list of commands suitable for use with
+	 *         {@link org.usfirst.frc.team3494.robot.commands.auto.ConstructedAuto}.
+	 */
+	public static ArrayList<Command> crossBaseLine() {
+		ArrayList<Command> list = new ArrayList<Command>();
+		list.add(new DistanceDrive(93.25, UnitTypes.INCHES));
+		return list;
+	}
+}

3494_2017_repo/src/org/usfirst/frc/team3494/robot/OI.java

@@ -1,7 +1,7 @@
 package org.usfirst.frc.team3494.robot;
 
 import org.usfirst.frc.team3494.robot.commands.auto.AngleTurn;
-import org.usfirst.frc.team3494.robot.commands.auto.DistanceDrive;
+import org.usfirst.frc.team3494.robot.commands.auto.XYDrive;
 import org.usfirst.frc.team3494.robot.commands.intake.SwitchPosition;
 
 import edu.wpi.first.wpilibj.Joystick;
@@ -13,9 +13,9 @@
  * interface to the commands and command groups that allow control of the robot.
  */
 public class OI {
-	//// CREATING BUTTONS
+	// CREATING BUTTONS
 	// One type of button is a joystick button which is any button on a
-	//// joystick.
+	// joystick.
 	// You create one by telling it which joystick it's on and which button
 	// number it is.
 	// Joystick stick = new Joystick(port);
@@ -52,6 +52,6 @@ public class OI {
 	public OI() {
 		xbox_b.whenPressed(new SwitchPosition());
 		xbox_y.whenPressed(new AngleTurn(90));
-		xbox_x.whenPressed(new DistanceDrive(2, UnitTypes.FEET));
+		xbox_x.whenPressed(new XYDrive(24, 24));
 	}
 }

3494_2017_repo/src/org/usfirst/frc/team3494/robot/Robot.java

@@ -1,5 +1,7 @@
 package org.usfirst.frc.team3494.robot;
 
+import org.usfirst.frc.team3494.robot.commands.auto.ConstructedAuto;
+import org.usfirst.frc.team3494.robot.commands.auto.XYDrive;
 import org.usfirst.frc.team3494.robot.subsystems.Climber;
 import org.usfirst.frc.team3494.robot.subsystems.Drivetrain;
 import org.usfirst.frc.team3494.robot.subsystems.Intake;
@@ -8,6 +10,9 @@
 
 import com.kauailabs.navx.frc.AHRS;
 
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.CameraServer;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
 import edu.wpi.first.wpilibj.IterativeRobot;
 import edu.wpi.first.wpilibj.PowerDistributionPanel;
 import edu.wpi.first.wpilibj.Preferences;
@@ -51,17 +56,21 @@ public class Robot extends IterativeRobot {
 	 */
 	@Override
 	public void robotInit() {
-		// chooser.addDefault("Default Auto", new ExampleCommand());
-		// chooser.addObject("My Auto", new MyAutoCommand());
 		driveTrain = new Drivetrain();
 		climber = new Climber();
 		turret = new Turret();
 		kompressor = new Kompressor();
 		intake = new Intake();
+		intake.setPiston(Value.kReverse);
 		oi = new OI();
 		ahrs = new AHRS(SerialPort.Port.kMXP);
+		// Auto programs come after all subsystems are created
+		chooser.addDefault("Default Auto", new XYDrive(24, 24));
+		chooser.addObject("To the baseline!", new ConstructedAuto(AutoGenerator.crossBaseLine()));
+		@SuppressWarnings("unused")
+		UsbCamera camera = CameraServer.getInstance().startAutomaticCapture();
 		// put chooser on DS
-		// SmartDashboard.putData("Auto mode", chooser);
+		SmartDashboard.putData("Auto mode", chooser);
 		// get preferences
 		prefs = Preferences.getInstance();
 	}
@@ -96,16 +105,22 @@ public void disabledPeriodic() {
 	public void autonomousInit() {
 		autonomousCommand = chooser.getSelected();
 
-		/*
-		 * String autoSelected = SmartDashboard.getString("Auto Selector",
-		 * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
-		 * = new MyAutoCommand(); break; case "Default Auto": default:
-		 * autonomousCommand = new ExampleCommand(); break; }
-		 */
+		// String autoSelected = SmartDashboard.getString("Auto Selector",
+		// "Default");
+		// switch (autoSelected) {
+		// case "My Auto":
+		// autonomousCommand = new MyAutoCommand();
+		// break;
+		// case "Default Auto":
+		// default:
+		// autonomousCommand = new ExampleCommand();
+		// break;
+		// }
 
 		// schedule the autonomous command (example)
-		if (autonomousCommand != null)
+		if (autonomousCommand != null) {
 			autonomousCommand.start();
+		}
 	}
 
 	/**
@@ -122,8 +137,9 @@ public void teleopInit() {
 		// teleop starts running. If you want the autonomous to
 		// continue until interrupted by another command, remove
 		// this line or comment it out.
-		if (autonomousCommand != null)
+		if (autonomousCommand != null) {
 			autonomousCommand.cancel();
+		}
 	}
 
 	/**
@@ -133,6 +149,8 @@ public void teleopInit() {
 	public void teleopPeriodic() {
 		Scheduler.getInstance().run();
 		SmartDashboard.putNumber("Gyro Z", ahrs.getAngle());
+		SmartDashboard.putNumber("distance", Robot.driveTrain.getLeftDistance(UnitTypes.RAWCOUNT));
+		SmartDashboard.putNumber("distance inches", Robot.driveTrain.getLeftDistance(UnitTypes.INCHES));
 
 		SmartDashboard.putNumber("Motor 0", Robot.pdp.getCurrent(0));
 		SmartDashboard.putNumber("Motor 1", Robot.pdp.getCurrent(1));

3494_2017_repo/src/org/usfirst/frc/team3494/robot/RobotMap.java

@@ -31,6 +31,8 @@ public class RobotMap {
 	// encoders
 	public static final int ENCODER_RIGHT_A = 0;
 	public static final int ENCODER_RIGHT_B = 1;
+	public static final int ENCODER_LEFT_A = 2;
+	public static final int ENCODER_LEFT_B = 3;
 	// intake
 	public static final int INTAKE_MOTOR = 0;
 	public static final int UP_MOTOR = 2;

3494_2017_repo/src/org/usfirst/frc/team3494/robot/UnitTypes.java

@@ -6,5 +6,5 @@
  * @since 0.0.0
  */
 public enum UnitTypes {
-	INCHES, MILLIMETERS, FEET, RAWCOUNT;
+	INCHES, FEET, MILLIMETERS, CENTIMETERS, RAWCOUNT, ROTATIONS;
 }

3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/AngleTurn.java

@@ -6,7 +6,8 @@
 
 /**
  * Turns the robot using the gyro board mounted to the RoboRIO. The angle to
- * turn by must be specified in the constructor.
+ * turn by must be specified in the constructor. Angle tolerance is specified by
+ * Robot.prefs (key {@code angle tolerance}.)
  * 
  * @since 0.0.2
  * @see org.usfirst.frc.team3494.robot.Robot
@@ -15,13 +16,24 @@
 public class AngleTurn extends Command {
 
 	private double angle;
-	private static double tolerance = Robot.prefs.getDouble("angle tolerance", 2.5);
+	private static double tolerance;
 
+	/**
+	 * Constructor.
+	 * 
+	 * @param angle
+	 *            The number of degrees to turn.
+	 */
 	public AngleTurn(double angle) {
 		// Use requires() here to declare subsystem dependencies
 		// eg. requires(chassis);
 		requires(Robot.driveTrain);
 		this.angle = angle;
+		try {
+			tolerance = Robot.prefs.getDouble("angle tolerance", 2.5);
+		} catch (NullPointerException e) {
+			tolerance = 2.5;
+		}
 	}
 
 	// Called just before this Command runs the first time
@@ -32,7 +44,6 @@ protected void initialize() {
 	// Called repeatedly when this Command is scheduled to run
 	protected void execute() {
 		if (!((Robot.ahrs.getAngle() > this.angle - tolerance) && (Robot.ahrs.getAngle() < this.angle + tolerance))) {
-			System.out.println(this.angle);
 			if (this.angle > 0) {
 				Robot.driveTrain.adjustedTankDrive(-0.4, 0.4);
 				Robot.driveTrain.resetRight();

3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/ConstructedAuto.java

@@ -0,0 +1,26 @@
+package org.usfirst.frc.team3494.robot.commands.auto;
+
+import java.util.ArrayList;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ * Creates an auto program from a passed-in list of commands.
+ * 
+ * @since 0.0.3
+ */
+public class ConstructedAuto extends CommandGroup {
+	/**
+	 * Constructor.
+	 * 
+	 * @param commands
+	 *            The list of commands to make an auto sequence from, <b>in
+	 *            order.</b>
+	 */
+	public ConstructedAuto(ArrayList<Command> commands) {
+		for (Command c : commands) {
+			this.addSequential(c);
+		}
+	}
+}

3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/DistanceDrive.java

@@ -16,6 +16,15 @@ public class DistanceDrive extends Command {
 	private double dist;
 	private UnitTypes unit;
 
+	/**
+	 * Constructor.
+	 * 
+	 * @param distance
+	 *            The distance to drive.
+	 * @param unitType
+	 *            The unit that the distance is in.
+	 * @see org.usfirst.frc.team3494.robot.UnitTypes
+	 */
 	public DistanceDrive(double distance, UnitTypes unitType) {
 		// Use requires() here to declare subsystem dependencies
 		// eg. requires(chassis);
@@ -34,9 +43,11 @@ protected void initialize() {
 	// Called repeatedly when this Command is scheduled to run
 	@Override
 	protected void execute() {
-		System.out.println("Driving forward: " + this.dist);
-		Robot.driveTrain.adjustedTankDrive(0.3, 0.3);
-		System.out.println("Right distance: " + Robot.driveTrain.getRightDistance(this.unit));
+		if (this.dist > Robot.driveTrain.getRightDistance(this.unit)) {
+			Robot.driveTrain.adjustedTankDrive(0.3, 0.3);
+		} else {
+			return;
+		}
 	}
 
 	// Make this return true when this Command no longer needs to run execute()

3494_2017_repo/src/org/usfirst/frc/team3494/robot/commands/auto/XYDrive.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc.team3494.robot.commands.auto;
+
+import org.usfirst.frc.team3494.robot.UnitTypes;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ * Drives to a point relative to the robot.
+ * 
+ * @since 0.0.3
+ */
+public class XYDrive extends CommandGroup {
+
+	private double hypot;
+	private double angle;
+
+	/**
+	 * Constructor.
+	 * 
+	 * @param rise
+	 *            The distance along the Y axis that the point is from the
+	 *            robot.
+	 * @param run
+	 *            The distance along the X axis that the point is from the
+	 *            robot.
+	 */
+	public XYDrive(double rise, double run) {
+		double run2 = run * run;
+		double rise2 = rise * rise;
+		this.hypot = Math.sqrt(run2 + rise2);
+		this.angle = Math.toDegrees(Math.atan2(rise, run));
+		addSequential(new AngleTurn(angle));
+		addSequential(new DistanceDrive(hypot, UnitTypes.INCHES));
+	}
+}