Merge branch 'master' of https://github.com/Team3132/2020CompRobot in…

…to testOI

src/main/java/frc/robot/Constants.java

@@ -114,7 +114,7 @@ private Constants() {
 	public static final double INTAKE_POSITION_I = 0;
 	public static final double INTAKE_POSITION_D = 0;
 
-	public static final int INTAKE_SOLENOID_PORT = 0;
+	public static final int INTAKE_SOLENOID_PORT = 1;
 
 	public static final int[] TEST_SPARK_MOTOR_CAN_ID_LIST = {50, 51};
 
@@ -191,6 +191,13 @@ private Constants() {
 	public static final double TURN_TO_ANGLE_MAX_VELOCITY_JERK = 50;
 	public static final double TURN_TO_ANGLE_ANGLE_SCALE = 0.3;
 
+	// Climber
+	public static final int CLIMBER_PTO_SOLENOID_PORT = 3;
+	public static final int CLIMBER_BRAKE_SOLENOID_PORT = 0; 
+
+	// Buddy climb
+	public static final int BUDDYCLIMB_SOLENOID_PORT = 7;
+
 	// logging information constants
 	public static final String WEB_BASE_PATH = "/media/sda1";		// where web server's data lives
 	public static final String LOG_BASE_PATH = WEB_BASE_PATH;		// log files (has to be inside web server)

src/main/java/frc/robot/OI.java

@@ -11,8 +11,6 @@
 import frc.robot.controller.Sequence;
 import frc.robot.controller.Sequences;
 import frc.robot.interfaces.*;
-import frc.robot.interfaces.ClimberInterface.ClimberAction;
-import frc.robot.interfaces.ClimberInterface.ClimberAction.Type;
 import frc.robot.lib.GamepadButtonsX;
 import frc.robot.lib.OperatorBoxButtons;
 import frc.robot.lib.WheelColour;
@@ -86,25 +84,28 @@ public void configureJoysticks(FlightStick driverLeft, FlightStick driverRight,
 
 	public void configureDriverJoystick(FlightStick leftStick, FlightStick rightStick, String name) {
 
-		//intake 
+		// Intake 
 		onTriggered(rightStick.getButton(1), Sequences.startIntaking());
 		onUntriggered(rightStick.getButton(1), Sequences.stopIntaking());
 
-		//slowdrive
+		// slowdrive
 		onTriggered(leftStick.getButton(1), Sequences.startSlowDriveForward());
 		onUntriggered(leftStick.getButton(1), Sequences.setDrivebaseToArcade());
 
-		//release/enable ratchets (empty sequence)
+		// release/enable ratchets (empty sequence)
 		onTriggered(rightStick.getButton(5), Sequences.getEmptySequence());
 		onUntriggered(rightStick.getButton(5), Sequences.getEmptySequence());		
 
-		//shift PTO mode, toggle (empty sequence)
+		// shift PTO mode, toggle (empty sequence)
 		onTriggered(rightStick.getButton(6), Sequences.getEmptySequence());
 		onUntriggered(rightStick.getButton(6), Sequences.getEmptySequence());		
 
-		//vision lineup
+		// Vision lineup
 		onTriggered(rightStick.getButton(2), Sequences.visionAim());
 		onUntriggered(rightStick.getButton(2), Sequences.getEmptySequence());	
+
+		//onTriggered(rightStick.getButton(4), Sequences.deployBuddyClimber());
+		//onTriggered(rightStick.getButton(5), Sequences.retractBuddyClimber());
 	}
 
 	public void configureOperatorJoystick(InputDevice stick, String name) {
@@ -247,7 +248,8 @@ private void mapOverrideSwitch(InputDevice box, int disableButton, int manualBut
 							  box.getButton(manualButton)),
 					() -> overrideableSubsystem.setAutomaticMode());
 	}
-
+
+
 	/**
 	 * Configure the rules for the user interfaces
 	 */

src/main/java/frc/robot/controller/Controller.java

@@ -1,5 +1,5 @@
 package frc.robot.controller;
-
+  
 import java.util.Iterator;
 import java.util.function.BooleanSupplier;
 import java.util.function.Supplier;
@@ -15,29 +15,30 @@
 import frc.robot.subsystems.Subsystems;
 
 /**
- * The controller of State Sequences while ensuring the robot is safe at every step.
+ * The controller of State Sequences while ensuring the robot is safe at every
+ * step.
  * 
- * Allows higher level code to specify just the states that the robot needs
- * to pass through but it doesn't need to care how it gets there - this code
- * will ensure it gets there safely.
+ * Allows higher level code to specify just the states that the robot needs to
+ * pass through but it doesn't need to care how it gets there - this code will
+ * ensure it gets there safely.
  * 
  * This is very similar to commands, with the differences to a command-based
- * approach being:
- *  - Unlike commands, the activation logic is concentrated in one place, making
- *    it much safer to add new functionality.
- *  - Every state doesn't need to be aware of every other state (much simpler).
- *  - Creating strings of sequences is much simpler and shorter than commands.
- *  - Arbitrary combinations of parallel and sequential commands aren't supported,
- *    only a series of parallel operations.
+ * approach being: - Unlike commands, the activation logic is concentrated in
+ * one place, making it much safer to add new functionality. - Every state
+ * doesn't need to be aware of every other state (much simpler). - Creating
+ * strings of sequences is much simpler and shorter than commands. - Arbitrary
+ * combinations of parallel and sequential commands aren't supported, only a
+ * series of parallel operations.
  * 
- * This could be made faster, but we need to be careful it doesn't make it unsafe.
+ * This could be made faster, but we need to be careful it doesn't make it
+ * unsafe.
  */
 public class Controller implements Runnable, DashboardUpdater {
 	private final Subsystems subsystems;
 	private final Clock clock;
 	private final DashboardInterface dashboard;
 	private final Log log;
-	private Sequence sequence = new Sequence("idle");  // Current sequence we are working through.
+	private Sequence sequence = new Sequence("idle"); // Current sequence we are working through.
 	private boolean sequenceHasChanged = true;
 	private boolean sequenceHasFinished = false;
 	private String blockedBy = "";
@@ -52,18 +53,19 @@ public Controller(Subsystems subsystems, Supplier<WheelColour> fmsColour) {
 		this.fmsColour = fmsColour;
 		(new Thread(this)).start();
 	}
-	
+
 	synchronized public void doSequence(Sequence sequence) {
 		if (this.sequence == sequence) {
 			// Exactly the same same sequence. Only start it again if it has
 			// finished. Used in the whileTriggered(...) case.
 			// Intentionally using == instead of .equalTo().
-			if (!sequenceHasFinished) return;
+			if (!sequenceHasFinished)
+				return;
 		}
 		this.sequence = sequence;
 		sequenceHasChanged = true;
 		logSub("Sequence has changed to %s sequence", sequence.getName());
-		notifyAll();  // Tell the run() method that there is a new sequence.
+		notifyAll(); // Tell the run() method that there is a new sequence.
 	}
 
 	/**
@@ -111,28 +113,27 @@ public void run() {
 	}
 
 	/**
-	 * For use by unit tests only. 
+	 * For use by unit tests only.
+	 * 
 	 * @return if an unhandled excpetions has occured in the controller
 	 */
 	public boolean isAlive() {
 		return isAlive;
 	}
-	
+
 	/**
 	 * Does the simple, dumb and most importantly, safe thing.
 	 * 
 	 * See the design doc before changing this.
 	 * 
-	 * Steps through:
-	 *  - Wait for all subsystems to finish moving.
-	 *  - Deploy or retract the intake if necessary.
-	 *  - 
-	 *  
+	 * Steps through: - Wait for all subsystems to finish moving. - Deploy or
+	 * retract the intake if necessary. -
+	 * 
 	 * Note if the step asks for something which will cause harm to the robot, the
 	 * request will be ignored. For example if the lift was moved into a position
 	 * the intake could hit it and then the intake was moved into the lift, the
 	 * intake move would be ignored.
-	 *  
+	 * 
 	 * @param desiredState The state to leave the robot in.
 	 */
 	private void applyState(State desiredState) {
@@ -143,7 +144,7 @@ private void applyState(State desiredState) {
 		logSub("Applying requested state: %s", desiredState);
 		//logSub("Waiting subsystems to finish moving before applying state");
 		waitForIntake();
-		
+
 		// Get the current state of the subsystems.
 		State currentState = new State(subsystems, clock);
 		logSub("Current state: %s", currentState);
@@ -155,7 +156,7 @@ private void applyState(State desiredState) {
 		logSub("Calculated new 'safe' state: %s", desiredState);
 
 		// The time beyond which we are allowed to move onto the next state
-		double endTime = desiredState.timeAction.calculateEndTime(clock.currentTime());	
+		double endTime = desiredState.timeAction.calculateEndTime(clock.currentTime());
 
 
 		// Start driving if necessary.
@@ -165,8 +166,6 @@ private void applyState(State desiredState) {
 		subsystems.intake.setMotorOutput(desiredState.intakeMotorOutput);
 
 		subsystems.loader.setTargetSpinnerMotorRPM(desiredState.loaderSpinnerMotorRPM);
-
-		subsystems.climber.setDesiredAction(desiredState.climber);
 
 		subsystems.colourWheel.setArmExtended(desiredState.extendColourWheel);
 		subsystems.colourWheel.setDesiredAction(desiredState.colourAction);
@@ -175,7 +174,6 @@ private void applyState(State desiredState) {
 		//subsystems.jevois.setCameraMode(desiredState.cameraMode);
 		maybeWaitForBalls(desiredState.expectedNumberOfBalls);
 		waitForIntake();
-		waitForClimber();
 		maybeWaitForShooter(desiredState.shooterUpToSpeed);
 		maybeWaitForColourWheel();
 		// Wait for driving to finish if needed.
@@ -202,12 +200,7 @@ private void waitForIntake() {
 		waitUntil(() -> subsystems.intake.isRetracted() || subsystems.intake.isExtended(), "intake to finish moving");
 	}
 
-	/**
-	 * Blocks waiting till the climber is in position.
-	 */
-	private void waitForClimber() {
-		waitUntil(() -> subsystems.climber.isInPosition(), "climber");
-	}
+
 
 	/**
 	 * Maybe wait for the shooter to get up to the target speed.

src/main/java/frc/robot/controller/Sequences.java

@@ -276,8 +276,47 @@ public static Sequence colourWheelClockwise() {
 		return seq;
 	}
 
+	// Climber
+	public static Sequence enableClimbMode() {
+		Sequence seq = new Sequence("Enable climb mode");
+		seq.add().enableClimbMode();
+		return seq;
+	}
+
+	public static Sequence enableDriveMode() {
+		Sequence seq = new Sequence("Enable drive mode");
+		seq.add().enableDriveMode();
+		return seq;
+	}
+
+	public static Sequence climberBrake() {
+		Sequence seq = new Sequence("Climber brake apply");
+		seq.add().applyClimberBrake();
+		return seq;
+	}
+
+	public static Sequence climberStopBrake() {
+		Sequence seq = new Sequence("Climber brake release");
+		seq.add().releaseClimberBrake();
+		return seq;	
+	}
+
+	// Buddy Climb
+	public static Sequence deployBuddyClimber() {
+		Sequence seq = new Sequence("Deploying buddy climb attatchment");
+		seq.add().deployBuddyClimb();
+		return seq;
+	}
+
+	public static Sequence retractBuddyClimber() {
+		Sequence seq = new Sequence("Retracting buddy climb attatchment");
+		seq.add().retractBuddyClimb();
+		return seq;
+	}
+
 	// For testing. Needs to be at the end of the file.
 	public static Sequence[] allSequences = new Sequence[] { 
+
 		getEmptySequence(), 
 		getStartSequence(), 
 		getResetSequence(),