Merge pull request #13 from roboblazers7617/intake-alignment-speed

Add alignment speed to intake

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

@@ -156,8 +156,10 @@ public static class IntakeConstants {
 		public static final int MOTOR_CAN_ID = 21;
 
 		public static final int NOTE_SENSOR_DIO = 9;
+		public static final int NOTE_ALIGNMENT_SENSOR_DIO = 6;
 
-		public static final double INTAKE_SPEED = 0.5;
+		public static final double INTAKE_SPEED = 0.95;
+		public static final double ALIGMNMENT_SPEED = 0.080;
 		public static final double OUTAKE_SPEED = -0.25;
 		public static final double FEEDER_SPEED = 0.25; // What speed should a note be fed into the shooter at?
 	}

src/main/java/frc/robot/subsystems/Head.java

@@ -34,6 +34,7 @@ public class Head extends SubsystemBase {
 
 	private final CANSparkMax intakeMotor = new CANSparkMax(IntakeConstants.MOTOR_CAN_ID, MotorType.kBrushless);
 	private final DigitalInput isNoteInSensor = new DigitalInput(IntakeConstants.NOTE_SENSOR_DIO);
+	private final DigitalInput isNoteInAlignmentSensor = new DigitalInput(IntakeConstants.NOTE_ALIGNMENT_SENSOR_DIO);
 
 	private boolean shooterIdle = true; // Is the shooter set to the idle speed?
 	private double shooterSetPoint = 0; // What speed should the shooter be spinning?
@@ -109,8 +110,12 @@ public Command IntakePiece() {
 		return Commands.runOnce(() -> {
 			setIntakeSpeed(IntakeConstants.INTAKE_SPEED);
 		}, this)
+				.andThen(Commands.waitUntil(() -> isNoteWithinAlignmentSensor()))
+				.andThen(Commands.runOnce(() -> {
+					setIntakeSpeed(IntakeConstants.ALIGMNMENT_SPEED);
+				}))
 				.andThen(Commands.waitUntil(() -> isNoteWithinSensor()))
-				.finallyDo(() -> {
+				.andThen(() -> {
 					setIntakeSpeed(0);
 				});
 	}
@@ -158,7 +163,7 @@ public Command SpinUpShooterForSpeaker() {
 	public Command SpinUpShooterForAmp() {
 		return SpinUpShooter(ShooterConstants.AMP_SPEED);
 	}
-
+	
 	public Command SpinUpShooterForPodium() {
 		return SpinUpShooter(ShooterConstants.PODIUM_SPEED);
 	}
@@ -220,15 +225,15 @@ public Command ShootAuto(double rpm) {
 	public Command ShootInSpeaker() {
 		return Shoot(ShooterConstants.SPEAKER_SPEED);
 	}
-
+  
 	public Command ShootInSpeakerAuto(){
 		return ShootAuto(ShooterConstants.AUTO_SPEED);
 	}
-
-	public Command ShootOverDBot(){
+	
+	public Command ShootOverDBot() {
 		return Shoot(ShooterConstants.DBOT_SPEED);
 	}
-
+	
 	public Command ShootPodium() {
 		return Shoot(ShooterConstants.PODIUM_SPEED);
 	}
@@ -241,6 +246,10 @@ public boolean isNoteWithinSensor() {
 		return !isNoteInSensor.get();
 	}
 
+	public boolean isNoteWithinAlignmentSensor() {
+		return !isNoteInAlignmentSensor.get();
+	}
+
 	public Command ToggleBreakModes() {
 		return new InstantCommand(() -> {
 			if (intakeMotor.getIdleMode() == IdleMode.kBrake) {
@@ -250,8 +259,8 @@ public Command ToggleBreakModes() {
 			}
 		});
 	}
-
-	public Command EnableBrakeMode(){
+	
+	public Command EnableBrakeMode() {
 		return new InstantCommand(() -> {
 			intakeMotor.setIdleMode(IdleMode.kBrake);
 		});