From 565a460b888f4011d7ef0a45d7eb732376964b46 Mon Sep 17 00:00:00 2001 From: Max Nargang Date: Wed, 27 Mar 2024 12:41:07 -0400 Subject: [PATCH 1/2] Add alignment speed to intake --- src/main/java/frc/robot/Constants.java | 4 +++- src/main/java/frc/robot/subsystems/Head.java | 25 +++++++++++++------- 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 56069e6..0bd673d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -174,8 +174,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 = 1; + public static final double ALIGMNMENT_SPEED = 0.1; 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? } diff --git a/src/main/java/frc/robot/subsystems/Head.java b/src/main/java/frc/robot/subsystems/Head.java index 46a310d..448931d 100644 --- a/src/main/java/frc/robot/subsystems/Head.java +++ b/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 isNoteAcquired = false; // Since none of the sensors will be active when a note is intaken and aligned, this boolean is necessary to know if the robot has a note. private boolean shooterIdle = true; // Is the shooter set to the idle speed? @@ -110,6 +111,10 @@ 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(() -> { isNoteAcquired = true; @@ -161,7 +166,7 @@ public Command SpinUpShooterForSpeaker() { public Command SpinUpShooterForAmp() { return SpinUpShooter(ShooterConstants.AMP_SPEED); } - + public Command SpinUpShooterForPodium() { return SpinUpShooter(ShooterConstants.PODIUM_SPEED); } @@ -206,15 +211,15 @@ public Command Shoot(double rpm) { public Command ShootInSpeaker() { return Shoot(ShooterConstants.SPEAKER_SPEED); } - - public Command ShootInSpeakerAuto(){ + + public Command ShootInSpeakerAuto() { return Shoot(ShooterConstants.AUTO_SPEED); } - - public Command ShootOverDBot(){ + + public Command ShootOverDBot() { return Shoot(ShooterConstants.DBOT_SPEED); } - + public Command ShootPodium() { return Shoot(ShooterConstants.PODIUM_SPEED); } @@ -227,6 +232,10 @@ public boolean isNoteWithinSensor() { return !isNoteInSensor.get(); } + public boolean isNoteWithinAlignmentSensor() { + return !isNoteInAlignmentSensor.get(); + } + // Does the intake have a note? public boolean isNoteAcquired() { return isNoteAcquired; @@ -241,8 +250,8 @@ public Command ToggleBreakModes() { } }); } - - public Command EnableBrakeMode(){ + + public Command EnableBrakeMode() { return new InstantCommand(() -> { intakeMotor.setIdleMode(IdleMode.kBrake); }); From 922efd3ff0450f3ce55ef1c6e194dfae0df09bf3 Mon Sep 17 00:00:00 2001 From: Max Nargang Date: Wed, 27 Mar 2024 17:38:36 -0400 Subject: [PATCH 2/2] Intake speed tuning --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/subsystems/Head.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0bd673d..b469b13 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -176,8 +176,8 @@ public static class IntakeConstants { public static final int NOTE_SENSOR_DIO = 9; public static final int NOTE_ALIGNMENT_SENSOR_DIO = 6; - public static final double INTAKE_SPEED = 1; - public static final double ALIGMNMENT_SPEED = 0.1; + 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? } diff --git a/src/main/java/frc/robot/subsystems/Head.java b/src/main/java/frc/robot/subsystems/Head.java index 448931d..a487219 100644 --- a/src/main/java/frc/robot/subsystems/Head.java +++ b/src/main/java/frc/robot/subsystems/Head.java @@ -116,7 +116,7 @@ public Command IntakePiece() { setIntakeSpeed(IntakeConstants.ALIGMNMENT_SPEED); })) .andThen(Commands.waitUntil(() -> isNoteWithinSensor())) - .finallyDo(() -> { + .andThen(() -> { isNoteAcquired = true; setIntakeSpeed(0); });