Hang Feature

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

@@ -47,6 +47,33 @@ public static enum Bot {
 		}
 	}
 
+	public static class HangConstants {
+		static {
+			switch (currentBot) {
+				case WOOD_BOT:
+					HAS_HANG = false;
+					break;
+
+				case COMP_BOT:
+				default:
+					HAS_HANG = true;
+					break;
+			}
+		}
+
+		public static final boolean HAS_HANG;
+
+		public static final int LEFT_MOTOR_ID = 0;
+		public static final int RIGHT_MOTOR_ID = 1;
+		public static final int LIMIT_SWITCH_ID = 2;
+
+		public static final boolean LEFT_MOTOR_IS_INVERTED = false;
+		public static final boolean RIGHT_MOTOR_IS_INVERTED = false;
+
+		public static final double speed = 0.2;
+
+	}
+
 	public static class DriverConstants {
 		public static final int DRIVER_JOYSTICK_PORT = 0;
 		public static final int OPERATOR_JOYSTICK_PORT = 1;

src/main/java/frc/robot/RobotContainer.java

@@ -1,6 +1,7 @@
 package frc.robot;
 
 import frc.robot.Constants.DriverConstants;
+import frc.robot.Constants.HangConstants;
 import frc.robot.Constants.IntakeShooterConstants;
 import frc.robot.Constants.LightConstants;
 import frc.robot.Constants.SwerveDrivetrainConstants;
@@ -13,6 +14,8 @@
 import frc.robot.commands.FollowTag;
 import frc.robot.commands.AimAtTag;
 import frc.robot.commands.ArmRotateTo;
+import frc.robot.commands.ChassisRemoteControl;
+import frc.robot.commands.SetHangSpeed;
 import frc.robot.commands.SetLightstripColor;
 import frc.robot.Constants.VisionConstants;
 import frc.robot.subsystems.LightStrip;
@@ -22,6 +25,9 @@
 import frc.robot.subsystems.arm.Arm;
 import frc.robot.subsystems.arm.DummyArm;
 import frc.robot.subsystems.arm.RealArm;
+import frc.robot.subsystems.hang.DummyHang;
+import frc.robot.subsystems.hang.Hang;
+import frc.robot.subsystems.hang.RealHang;
 import frc.robot.subsystems.intake.DummyShooter;
 import frc.robot.subsystems.intake.IntakeShooter;
 import frc.robot.subsystems.intake.RealShooter;
@@ -93,6 +99,12 @@ public class RobotContainer {
 			ArmConstants.RIGHT_ENCODER_ID,
 			ArmConstants.ARE_MOTORS_REVERSED) : new DummyArm();
 
+	private final Hang hang = Constants.HangConstants.HAS_HANG
+			? new RealHang(HangConstants.LEFT_MOTOR_ID, HangConstants.RIGHT_MOTOR_ID,
+					HangConstants.LEFT_MOTOR_IS_INVERTED, HangConstants.RIGHT_MOTOR_IS_INVERTED,
+					HangConstants.LIMIT_SWITCH_ID)
+			: new DummyHang();
+
 	private final IntakeShooter intakeShooter = Constants.IntakeShooterConstants.HAS_INTAKE ? new RealShooter(
 			IntakeShooterConstants.FLYWHEEL_MOTOR_LEFT_ID,
 			IntakeShooterConstants.FLYWHEEL_MOTOR_RIGHT_ID,
@@ -209,10 +221,14 @@ public void setUpOperatorController() {
 		final Command coopLightSignal = new SetLightstripColor(lightStrip, LightConstants.LED_COLOR_RED);
 
 		final Command cancelCommand = new CancelCommands(drivetrain, arm, intakeShooter);
+
+		final SetHangSpeed hangStop = new SetHangSpeed(hang, 0);
+		final SetHangSpeed hangForwarSpeed = new SetHangSpeed(hang, HangConstants.speed);
+		final SetHangSpeed hangBackwardSpeed = new SetHangSpeed(hang, -HangConstants.speed);
 
+		SmartDashboard.putString("Operator Controller", genericHIDType.toString());
 		if (genericHIDType == null) {
 			SmartDashboard.putString("Operator Ctrl", "No Connection");
-
 		} else if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
 			SmartDashboard.putString("Operator Ctrl", "Joystick");
 			final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());
@@ -221,12 +237,24 @@ public void setUpOperatorController() {
 			joystick.button(5).onTrue(armToAmp);
 			joystick.button(6).onTrue(armToSpeaker);
 
-			joystick.button(7).onTrue(amplifyLightSignal);
-			joystick.button(8).onTrue(coopLightSignal);
+			joystick.button(7).or(joystick.button(8)).whileFalse(hangStop);
+			joystick.button(7).onTrue(hangForwarSpeed);
+			joystick.button(8).onTrue(hangBackwardSpeed);
+
+			joystick.button(9).onTrue(amplifyLightSignal);
+			joystick.button(10).onTrue(coopLightSignal);
 		} else {
 			SmartDashboard.putString("Operator Ctrl", "GamePad");
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
 
+			xbox.povDown().onTrue(armToIntake);
+			xbox.povUp().onTrue(armToSpeaker);
+			xbox.povLeft().onTrue(armToAmp);
+
+			xbox.rightBumper().or(xbox.leftBumper()).whileFalse(hangStop);
+			xbox.rightBumper().onTrue(hangForwarSpeed);
+			xbox.leftBumper().onTrue(hangBackwardSpeed);
+
 			xbox.leftTrigger().onTrue(armToSpeaker);
 			xbox.leftBumper().onTrue(startFlyWheel);
 
@@ -237,7 +265,6 @@ public void setUpOperatorController() {
 
 			xbox.povLeft().onTrue(amplifyLightSignal);
 			xbox.povRight().onTrue(coopLightSignal);
-
 			xbox.b().onTrue(cancelCommand);
 		}
 	}

src/main/java/frc/robot/commands/SetHangSpeed.java

@@ -0,0 +1,25 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.hang.Hang;
+
+public class SetHangSpeed extends Command {
+
+	private final Hang hang;
+	private final double speed;
+
+	public SetHangSpeed(Hang hang, double speed) {
+		this.hang = hang;
+		this.speed = speed;
+	}
+
+	@Override
+	public boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	public void initialize() {
+		hang.setSpeed(speed);
+	}
+}

src/main/java/frc/robot/subsystems/hang/DummyHang.java

@@ -0,0 +1,10 @@
+package frc.robot.subsystems.hang;
+
+public class DummyHang extends Hang {
+	public void setSpeed(double speed) {
+	}
+
+	public boolean isAtBottom() {
+		return false;
+	}
+}

src/main/java/frc/robot/subsystems/hang/Hang.java

@@ -0,0 +1,9 @@
+package frc.robot.subsystems.hang;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public abstract class Hang extends SubsystemBase {
+	public abstract void setSpeed(double speed);
+
+	public abstract boolean isAtBottom();
+}

src/main/java/frc/robot/subsystems/hang/RealHang.java

@@ -0,0 +1,40 @@
+package frc.robot.subsystems.hang;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import edu.wpi.first.wpilibj.DigitalInput;
+
+public class RealHang extends Hang {
+
+	private final CANSparkMax leftMotor;
+	private final CANSparkMax rightMotor;
+
+	private final DigitalInput limitSwitch;
+
+	private double speed;
+
+	public RealHang(int leftMotorID, int rightMotorID, boolean leftMotorIsInverted, boolean rightMotorIsInverted,
+			int limitSwitchId) {
+		leftMotor = new CANSparkMax(leftMotorID, MotorType.kBrushless);
+		rightMotor = new CANSparkMax(rightMotorID, MotorType.kBrushless);
+
+		limitSwitch = new DigitalInput(limitSwitchId);
+
+		leftMotor.setInverted(leftMotorIsInverted);
+		rightMotor.setInverted(rightMotorIsInverted);
+	}
+
+	@Override
+	public void periodic() {
+		leftMotor.set(speed);
+		rightMotor.set(speed);
+	}
+
+	public void setSpeed(double speed) {
+		this.speed = speed;
+	}
+
+	public boolean isAtBottom() {
+		return limitSwitch.get();
+	}
+}