From 78591b956e1b961d38e0abf13ec10dcfb1228fd9 Mon Sep 17 00:00:00 2001 From: Yumnah Date: Mon, 1 Apr 2024 18:57:31 -0700 Subject: [PATCH 01/19] Added intake sensor debouncer and feeder sensor --- .../robot/commands/intake/AllInCommand.java | 95 ++++++++++--------- .../robot/subsystems/IntakeSubsystem.java | 43 ++++++++- 2 files changed, 87 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 2885b2ea..9ed0e7e5 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -30,52 +30,55 @@ public void initialize() { @Override public void execute() { // intake rejecting - // if (intakeSubsystem.intakeFrontSeesNote()) { - // if (!intakeSubsystem.getRejectOverride()) { - // intakeSubsystem.intakeLeftReject(); - // intakeSubsystem.intakeRightReject(); - // } else { - // intakeSubsystem.intakeLeftStop(); - // intakeSubsystem.intakeRightStop(); - // } - - // if (controls != null && !rumbledIntakeFront) { - // Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); - // rumbledIntakeFront = true; - // } - // } - - // // back intake sensor does not exist (but it might?) - - // if (intakeSubsystem.intakeLeftSeesNote()) { - // if (!intakeSubsystem.getRejectOverride()) { - // intakeSubsystem.intakeFrontReject(); - // intakeSubsystem.intakeRightReject(); - // } else { - // intakeSubsystem.intakeFrontStop(); - // intakeSubsystem.intakeRightStop(); - // } - - // if (controls != null && !rumbledIntakeLeft) { - // Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); - // rumbledIntakeLeft = true; - // } - // } - - // if (intakeSubsystem.intakeRightSeesNote()) { - // if (!intakeSubsystem.getRejectOverride()) { - // intakeSubsystem.intakeFrontReject(); - // intakeSubsystem.intakeLeftReject(); - // } else { - // intakeSubsystem.intakeFrontStop(); - // intakeSubsystem.intakeLeftStop(); - // } - - // if (controls != null && !rumbledIntakeRight) { - // Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); - // rumbledIntakeRight = true; - // } - // } + if (intakeSubsystem.intakeFrontSeesNote()) { + if (intakeSubsystem.debouncedIntakeFrontSensor()) { + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeLeftReject(); + intakeSubsystem.intakeRightReject(); + } + } else { + intakeSubsystem.intakeLeftStop(); + intakeSubsystem.intakeRightStop(); + } + + if (controls != null && !rumbledIntakeFront) { + Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + rumbledIntakeFront = true; + } + } + if (intakeSubsystem.intakeLeftSeesNote()) { + if (intakeSubsystem.debouncedIntakeLeftSensor()) { + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeFrontReject(); + intakeSubsystem.intakeRightReject(); + } + } else { + intakeSubsystem.intakeFrontStop(); + intakeSubsystem.intakeRightStop(); + } + + if (controls != null && !rumbledIntakeLeft) { + Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + rumbledIntakeLeft = true; + } + } + + if (intakeSubsystem.intakeRightSeesNote()) { + if (intakeSubsystem.debouncedIntakeRightSensor()) { + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeFrontReject(); + intakeSubsystem.intakeLeftReject(); + } + } else { + intakeSubsystem.intakeFrontStop(); + intakeSubsystem.intakeLeftStop(); + } + + if (controls != null && !rumbledIntakeRight) { + Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + rumbledIntakeRight = true; + } + } // all intake motors rejecting after index if (intakeSubsystem.indexSensorHasNote()) { diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 7d2be044..457cf213 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -8,8 +8,8 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkLimitSwitch; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -46,9 +46,14 @@ public class IntakeSubsystem extends SubsystemBase { // Sensors private final SparkLimitSwitch indexSensor; - private final DigitalInput feederSensor; - + private final SparkLimitSwitch feederSensor; private final SparkLimitSwitch intakeFrontSensor; + + // debounce ! ! + private final Debouncer intakeFrontSensorDebouncer; + private final Debouncer intakeRightSensorDebouncer; + private final Debouncer intakeLeftSensorDebouncer; + // private final SparkLimitSwitch intakeBackSensor; private final SparkLimitSwitch intakeLeftSensor; private final SparkLimitSwitch intakeRightSensor; @@ -81,7 +86,7 @@ public IntakeSubsystem() { feederMotor = new CANSparkFlex(FEEDER_MOTOR, MotorType.kBrushless); indexSensor = indexMotorUpper.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - feederSensor = new DigitalInput(FEEDER_SENSOR); + feederSensor = feederMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); intakeFrontSensor = intakeMotorFront.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); // intakeBackSensor = @@ -89,10 +94,16 @@ public IntakeSubsystem() { intakeLeftSensor = intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); intakeRightSensor = intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // sensor must be true for 0.1 seconds before being actually true + intakeFrontSensorDebouncer = new Debouncer(0.1); + intakeRightSensorDebouncer = new Debouncer(0.1); + intakeLeftSensorDebouncer = new Debouncer(0.1); + // todo: MOVE THIS TO CONFIGURE MOTOR intakeFrontSensor.enableLimitSwitch(false); intakeLeftSensor.enableLimitSwitch(false); intakeRightSensor.enableLimitSwitch(false); + feederSensor.enableLimitSwitch(false); resetMotors(); @@ -219,7 +230,7 @@ public boolean indexSensorHasNote() { } public boolean feederSensorHasNote() { - return !feederSensor.get() && !getSensorOverride(); + return feederSensor.isPressed() && !getSensorOverride(); } public boolean intakeFrontSeesNote() { @@ -234,6 +245,28 @@ public boolean intakeRightSeesNote() { return intakeRightSensor.isPressed(); } + // //debounce sensors + public boolean debouncedIntakeFrontSensor() { + // if (intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed())) { + // return true; + // } + return intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed()); + } + + public boolean debouncedIntakeLeftSensor() { + // if (intakeLeftSensorDebouncer.calculate(intakeLeftSensor.isPressed())) { + // return true; + // } + return intakeLeftSensorDebouncer.calculate(intakeFrontSensor.isPressed()); + } + + public boolean debouncedIntakeRightSensor() { + // if (intakeRightSensorDebouncer.calculate(intakeRightSensor.isPressed())) { + // return true; + // } + return intakeRightSensorDebouncer.calculate(intakeFrontSensor.isPressed()); + } + // override methods on shuffleboard public boolean getSensorOverride() { return sensorOverride.getBoolean(false); From 66d780990d1b751e361ba1c9d05ecb4684edd94f Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Mon, 1 Apr 2024 20:27:47 -0700 Subject: [PATCH 02/19] offsets go crazy --- src/main/deploy/deploy | 1 - .../robot/subsystems/LauncherSubsystem.java | 15 ++++++++------- vendordeps/REVLib.json | 10 +++++----- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/deploy/deploy b/src/main/deploy/deploy index bb82515d..63c250c6 100644 --- a/src/main/deploy/deploy +++ b/src/main/deploy/deploy @@ -1,3 +1,2 @@ -Files placed in this directory will be deployed to the RoboRIO into the 'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index e8e2060a..f9083b75 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -34,18 +34,19 @@ public class LauncherSubsystem extends SubsystemBase { // HARDWARE private static final double PIVOT_GEARING_RATIO = 1.0 / 180.0; - private static final float PIVOT_SOFTSTOP_FORWARD = 0.84f; + private static final float PIVOT_SOFTSTOP_FORWARD = 0.93f; private static final float PIVOT_SOFTSTOP_BACKWARD = 0.635f; private static final float PIVOT_DISABLE_OFFSET = 0.04f; + private static final int PIVOT_OFFSET = 36; // ANGLE VALUES - public static final int AMP_AIM_ANGLE = 288; - public static final int SUBWOOFER_AIM_ANGLE = 256; - public static final int PODIUM_AIM_ANGLE = 238; - public static final int TRAP_AIM_ANGLE = 317; + public static final int AMP_AIM_ANGLE = 288 + PIVOT_OFFSET; + public static final int SUBWOOFER_AIM_ANGLE = 256 + PIVOT_OFFSET; + public static final int PODIUM_AIM_ANGLE = 238 + PIVOT_OFFSET; + public static final int TRAP_AIM_ANGLE = 317 + PIVOT_OFFSET; public static final double MANUAL_MODIFIER = 0.02; - public static final double RETRACTED_ANGLE = 242; + public static final double RETRACTED_ANGLE = 242 + PIVOT_OFFSET; // offset for FF so parallel to floor is 0 - public static final double FF_PIVOT_OFFSET = 225; + public static final double FF_PIVOT_OFFSET = 225 + PIVOT_OFFSET; // MOTOR VALUES // max Free Speed: 6784 RPM diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index a8295814..f85acd40 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.1", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.1" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.1", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.1", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.1", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From 47a71b6b1425d260a2d974e14aba1eaa793a143d Mon Sep 17 00:00:00 2001 From: Yumnah Date: Tue, 2 Apr 2024 07:59:29 -0700 Subject: [PATCH 03/19] commented removed sensors, debounced feeder sensor --- .../robot/commands/intake/AllInCommand.java | 104 +++++++++--------- .../robot/subsystems/IntakeSubsystem.java | 7 ++ 2 files changed, 59 insertions(+), 52 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 9ed0e7e5..280b340b 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -30,55 +30,55 @@ public void initialize() { @Override public void execute() { // intake rejecting - if (intakeSubsystem.intakeFrontSeesNote()) { - if (intakeSubsystem.debouncedIntakeFrontSensor()) { - if (!intakeSubsystem.getRejectOverride()) { - intakeSubsystem.intakeLeftReject(); - intakeSubsystem.intakeRightReject(); - } - } else { - intakeSubsystem.intakeLeftStop(); - intakeSubsystem.intakeRightStop(); - } - - if (controls != null && !rumbledIntakeFront) { - Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); - rumbledIntakeFront = true; - } - } - if (intakeSubsystem.intakeLeftSeesNote()) { - if (intakeSubsystem.debouncedIntakeLeftSensor()) { - if (!intakeSubsystem.getRejectOverride()) { - intakeSubsystem.intakeFrontReject(); - intakeSubsystem.intakeRightReject(); - } - } else { - intakeSubsystem.intakeFrontStop(); - intakeSubsystem.intakeRightStop(); - } - - if (controls != null && !rumbledIntakeLeft) { - Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); - rumbledIntakeLeft = true; - } - } - - if (intakeSubsystem.intakeRightSeesNote()) { - if (intakeSubsystem.debouncedIntakeRightSensor()) { - if (!intakeSubsystem.getRejectOverride()) { - intakeSubsystem.intakeFrontReject(); - intakeSubsystem.intakeLeftReject(); - } - } else { - intakeSubsystem.intakeFrontStop(); - intakeSubsystem.intakeLeftStop(); - } - - if (controls != null && !rumbledIntakeRight) { - Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); - rumbledIntakeRight = true; - } - } + // if (intakeSubsystem.intakeFrontSeesNote()) { + // if (intakeSubsystem.debouncedIntakeFrontSensor()) { + // if (!intakeSubsystem.getRejectOverride()) { + // intakeSubsystem.intakeLeftReject(); + // intakeSubsystem.intakeRightReject(); + // } + // } else { + // intakeSubsystem.intakeLeftStop(); + // intakeSubsystem.intakeRightStop(); + // } + + // if (controls != null && !rumbledIntakeFront) { + // Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + // rumbledIntakeFront = true; + // } + // } + // if (intakeSubsystem.intakeLeftSeesNote()) { + // if (intakeSubsystem.debouncedIntakeLeftSensor()) { + // if (!intakeSubsystem.getRejectOverride()) { + // intakeSubsystem.intakeFrontReject(); + // intakeSubsystem.intakeRightReject(); + // } + // } else { + // intakeSubsystem.intakeFrontStop(); + // intakeSubsystem.intakeRightStop(); + // } + + // if (controls != null && !rumbledIntakeLeft) { + // Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + // rumbledIntakeLeft = true; + // } + // } + + // if (intakeSubsystem.intakeRightSeesNote()) { + // if (intakeSubsystem.debouncedIntakeRightSensor()) { + // if (!intakeSubsystem.getRejectOverride()) { + // intakeSubsystem.intakeFrontReject(); + // intakeSubsystem.intakeLeftReject(); + // } + // } else { + // intakeSubsystem.intakeFrontStop(); + // intakeSubsystem.intakeLeftStop(); + // } + + // if (controls != null && !rumbledIntakeRight) { + // Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + // rumbledIntakeRight = true; + // } + // } // all intake motors rejecting after index if (intakeSubsystem.indexSensorHasNote()) { @@ -110,9 +110,9 @@ public void end(boolean interrupted) { Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule(); } - rumbledIntakeFront = false; - rumbledIntakeLeft = false; - rumbledIntakeRight = false; + // rumbledIntakeFront = false; + // rumbledIntakeLeft = false; + // rumbledIntakeRight = false; rumbledIndex = false; } diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 0a7ec1f3..e3000fc7 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -53,6 +53,7 @@ public class IntakeSubsystem extends SubsystemBase { private final Debouncer intakeFrontSensorDebouncer; private final Debouncer intakeRightSensorDebouncer; private final Debouncer intakeLeftSensorDebouncer; + private final Debouncer feederSensorDebouncer; // private final SparkLimitSwitch intakeBackSensor; private final SparkLimitSwitch intakeLeftSensor; @@ -99,6 +100,8 @@ public IntakeSubsystem() { intakeRightSensorDebouncer = new Debouncer(0.1); intakeLeftSensorDebouncer = new Debouncer(0.1); + feederSensorDebouncer = new Debouncer(0.1); + // todo: MOVE THIS TO CONFIGURE MOTOR intakeFrontSensor.enableLimitSwitch(false); intakeLeftSensor.enableLimitSwitch(false); @@ -267,6 +270,10 @@ public boolean debouncedIntakeRightSensor() { return intakeRightSensorDebouncer.calculate(intakeFrontSensor.isPressed()); } + public boolean debouncedFeederSensor() { + return feederSensorDebouncer.calculate(feederSensor.isPressed()); + } + // override methods on shuffleboard public boolean getSensorOverride() { return sensorOverride.getBoolean(false); From a89df25387a907f7df32e2ef2be70f776e4993c6 Mon Sep 17 00:00:00 2001 From: Yumnah Date: Tue, 2 Apr 2024 17:54:38 -0700 Subject: [PATCH 04/19] addressing comments added IR feeder sensor, changed debouncers to Both, feeder stops if either sensor triggers --- .../robot/subsystems/IntakeSubsystem.java | 36 +++++++++---------- 1 file changed, 17 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index e3000fc7..0a49e65e 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -10,6 +10,7 @@ import com.revrobotics.SparkLimitSwitch; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -46,14 +47,15 @@ public class IntakeSubsystem extends SubsystemBase { // Sensors private final SparkLimitSwitch indexSensor; - private final SparkLimitSwitch feederSensor; + private final static SparkLimitSwitch feederSensor; + private final DigitalInput feederSensorIR; private final SparkLimitSwitch intakeFrontSensor; // debounce ! ! private final Debouncer intakeFrontSensorDebouncer; private final Debouncer intakeRightSensorDebouncer; private final Debouncer intakeLeftSensorDebouncer; - private final Debouncer feederSensorDebouncer; + private final static Debouncer feederSensorDebouncer; // private final SparkLimitSwitch intakeBackSensor; private final SparkLimitSwitch intakeLeftSensor; @@ -88,6 +90,7 @@ public IntakeSubsystem() { indexSensor = indexMotorUpper.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); feederSensor = feederMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + feederSensorIR = new DigitalInput(FEEDER_SENSOR); intakeFrontSensor = intakeMotorFront.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); // intakeBackSensor = @@ -96,17 +99,11 @@ public IntakeSubsystem() { intakeRightSensor = intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); // sensor must be true for 0.1 seconds before being actually true - intakeFrontSensorDebouncer = new Debouncer(0.1); - intakeRightSensorDebouncer = new Debouncer(0.1); - intakeLeftSensorDebouncer = new Debouncer(0.1); + intakeFrontSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); + intakeRightSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); + intakeLeftSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); - feederSensorDebouncer = new Debouncer(0.1); - - // todo: MOVE THIS TO CONFIGURE MOTOR - intakeFrontSensor.enableLimitSwitch(false); - intakeLeftSensor.enableLimitSwitch(false); - intakeRightSensor.enableLimitSwitch(false); - feederSensor.enableLimitSwitch(false); + feederSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); resetMotors(); @@ -232,12 +229,16 @@ public boolean indexSensorHasNote() { return indexSensor.isPressed(); } + + public static boolean debouncedFeederSensor() { + return feederSensorDebouncer.calculate(feederSensor.isPressed()); + } public boolean feederSensorHasNote() { - return feederSensor.isPressed() && !getSensorOverride(); + return IntakeSubsystem.debouncedFeederSensor() && !getSensorOverride() || !feederSensorIR.get() && !getSensorOverride(); } public boolean intakeFrontSeesNote() { - return intakeFrontSensor.isPressed(); + return intakeFrontSensor.isPressed(); } public boolean intakeLeftSeesNote() { @@ -248,7 +249,7 @@ public boolean intakeRightSeesNote() { return intakeRightSensor.isPressed(); } - // //debounce sensors + //debounce sensors public boolean debouncedIntakeFrontSensor() { // if (intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed())) { // return true; @@ -270,10 +271,7 @@ public boolean debouncedIntakeRightSensor() { return intakeRightSensorDebouncer.calculate(intakeFrontSensor.isPressed()); } - public boolean debouncedFeederSensor() { - return feederSensorDebouncer.calculate(feederSensor.isPressed()); - } - + // override methods on shuffleboard public boolean getSensorOverride() { return sensorOverride.getBoolean(false); From 5b879b289ea02b68c38c03f338685fb3b92d7ee4 Mon Sep 17 00:00:00 2001 From: Yumnah Date: Tue, 2 Apr 2024 18:14:28 -0700 Subject: [PATCH 05/19] fixed some bugs --- .../robot/subsystems/IntakeSubsystem.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 0a49e65e..df7af191 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -47,7 +47,7 @@ public class IntakeSubsystem extends SubsystemBase { // Sensors private final SparkLimitSwitch indexSensor; - private final static SparkLimitSwitch feederSensor; + private static SparkLimitSwitch feederSensor; private final DigitalInput feederSensorIR; private final SparkLimitSwitch intakeFrontSensor; @@ -55,7 +55,7 @@ public class IntakeSubsystem extends SubsystemBase { private final Debouncer intakeFrontSensorDebouncer; private final Debouncer intakeRightSensorDebouncer; private final Debouncer intakeLeftSensorDebouncer; - private final static Debouncer feederSensorDebouncer; + private final Debouncer feederSensorDebouncer; // private final SparkLimitSwitch intakeBackSensor; private final SparkLimitSwitch intakeLeftSensor; @@ -229,16 +229,17 @@ public boolean indexSensorHasNote() { return indexSensor.isPressed(); } - - public static boolean debouncedFeederSensor() { + public boolean debouncedFeederSensor() { return feederSensorDebouncer.calculate(feederSensor.isPressed()); } + public boolean feederSensorHasNote() { - return IntakeSubsystem.debouncedFeederSensor() && !getSensorOverride() || !feederSensorIR.get() && !getSensorOverride(); + return debouncedFeederSensor() && !getSensorOverride() + || !feederSensorIR.get() && !getSensorOverride(); } public boolean intakeFrontSeesNote() { - return intakeFrontSensor.isPressed(); + return intakeFrontSensor.isPressed(); } public boolean intakeLeftSeesNote() { @@ -249,7 +250,7 @@ public boolean intakeRightSeesNote() { return intakeRightSensor.isPressed(); } - //debounce sensors + // debounce sensors public boolean debouncedIntakeFrontSensor() { // if (intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed())) { // return true; @@ -271,7 +272,6 @@ public boolean debouncedIntakeRightSensor() { return intakeRightSensorDebouncer.calculate(intakeFrontSensor.isPressed()); } - // override methods on shuffleboard public boolean getSensorOverride() { return sensorOverride.getBoolean(false); From bb16e2c2523dfeaf51f6a9370370bf0a0c4b891d Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Tue, 2 Apr 2024 18:55:27 -0700 Subject: [PATCH 06/19] Adjust subwoofer angle, try making vision target use PIVOT_OFFSET --- .../robot/commands/launcher/FullTargetCommand.java | 2 +- .../team2412/robot/subsystems/LauncherSubsystem.java | 11 ++++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java index 8640c576..c0f6c347 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java @@ -66,7 +66,7 @@ public void execute() { Math.atan2(relativeSpeaker.getY(), relativeSpeaker.getX()) + Math.PI); double distance = relativeSpeaker.getTranslation().getNorm(); LauncherDataPoint dataPoint = LAUNCHER_DATA.get(distance); - launcherSubsystem.setAngle(dataPoint.angle); + launcherSubsystem.setAngleWithOffset(dataPoint.angle); launcherSubsystem.launch(dataPoint.rpm); launcherSubsystem.updateDistanceEntry(distance); diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 2c85f950..e17c095f 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -41,7 +41,7 @@ public class LauncherSubsystem extends SubsystemBase { private static final int PIVOT_OFFSET = 36; // ANGLE VALUES public static final int AMP_AIM_ANGLE = 288 + PIVOT_OFFSET; - public static final int SUBWOOFER_AIM_ANGLE = 256 + PIVOT_OFFSET; + public static final int SUBWOOFER_AIM_ANGLE = 252 + PIVOT_OFFSET; public static final int PODIUM_AIM_ANGLE = 238 + PIVOT_OFFSET; public static final int TRAP_AIM_ANGLE = 317 + PIVOT_OFFSET; public static final double MANUAL_MODIFIER = 0.02; @@ -233,6 +233,15 @@ public double getAngle() { return Units.rotationsToDegrees(launcherAngleEncoder.getPosition()); } + /** + * Sets the launcher angle, taking the offset into account. + * + * @param launcherAngle Launcher angle. PIVOT_OFFSET will be added to this. + */ + public void setAngleWithOffset(double launcherAngle) { + setAngle(launcherAngle + PIVOT_OFFSET); + } + public void setAngle(double launcherAngle) { if (launcherAngle != AMP_AIM_ANGLE) { angleSetpoint = launcherAngle + setAngleOffsetEntry.getDouble(0); From 73478cdba24044853f509dc27c3803d6232b2c2f Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Tue, 2 Apr 2024 19:21:05 -0700 Subject: [PATCH 07/19] Slightly reduce P gain for launcher angle --- .../java/frc/team2412/robot/subsystems/LauncherSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index e17c095f..2effc1de 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -173,7 +173,7 @@ public void configMotors() { launcherAngleTwoMotor.follow(launcherAngleOneMotor, true); // PID - launcherAngleOnePIDController.setP(5.421); + launcherAngleOnePIDController.setP(5.2); launcherAngleOnePIDController.setI(0); launcherAngleOnePIDController.setD(0.066248); launcherAngleOnePIDController.setOutputRange(-ANGLE_MAX_SPEED, ANGLE_MAX_SPEED); @@ -235,7 +235,7 @@ public double getAngle() { /** * Sets the launcher angle, taking the offset into account. - * + * * @param launcherAngle Launcher angle. PIVOT_OFFSET will be added to this. */ public void setAngleWithOffset(double launcherAngle) { From 27e1e26865748154757ac8de772ce10c8136359b Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Tue, 2 Apr 2024 19:21:29 -0700 Subject: [PATCH 08/19] Make full manual also use PIVOT_OFFSET --- .../java/frc/team2412/robot/subsystems/LauncherSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 2effc1de..b70377ea 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -388,7 +388,7 @@ private void initShuffleboard() { new Trigger(() -> manualModeEntry.getBoolean(false)) .whileTrue( run(() -> { - setAngle(setLauncherAngleEntry.getDouble(getAngle())); + setAngleWithOffset(setLauncherAngleEntry.getDouble(getAngle())); launch(setLauncherSpeedEntry.getDouble(SPEAKER_SHOOT_SPEED_RPM)); }) .withName("Full Manual")); From 475b206b6df79a54f1080923e893f495979a6513 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Tue, 2 Apr 2024 19:29:21 -0700 Subject: [PATCH 09/19] Disable front and side intakes --- .../robot/subsystems/IntakeSubsystem.java | 42 +++++++++++++------ 1 file changed, 30 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 396be013..011aa9f2 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -34,6 +34,8 @@ public class IntakeSubsystem extends SubsystemBase { public static final double FEEDER_IN_SPEED = 0.65; public static final double FEEDER_REVERSE_SPEED = -0.3; + private static final boolean enableFrontAndSideIntakes = false; + // Motors private final CANSparkMax intakeMotorFront; private final CANSparkMax intakeMotorLeft; @@ -133,9 +135,11 @@ private void resetMotors() { } public void intakeSet(double speed) { - intakeMotorFront.set(speed); - intakeMotorLeft.set(speed); - intakeMotorRight.set(speed); + if (enableFrontAndSideIntakes) { + intakeMotorFront.set(speed); + intakeMotorLeft.set(speed); + intakeMotorRight.set(speed); + } ingestMotor.set(speed); } @@ -150,10 +154,12 @@ public void intakeReverse() { public void intakeSteal() { - intakeMotorLeft.set(INTAKE_IN_SPEED); - intakeMotorRight.set(INTAKE_IN_SPEED); + if (enableFrontAndSideIntakes) { + intakeMotorLeft.set(INTAKE_IN_SPEED); + intakeMotorRight.set(INTAKE_IN_SPEED); + intakeMotorFront.set(INTAKE_REJECT_SPEED); + } ingestMotor.set(INTAKE_REJECT_SPEED); - intakeMotorFront.set(INTAKE_REJECT_SPEED); indexMotorUpper.set(INTAKE_IN_SPEED); } @@ -163,15 +169,21 @@ public void intakeStop() { } public void intakeFrontStop() { - intakeMotorFront.set(0); + if (enableFrontAndSideIntakes) { + intakeMotorFront.set(0); + } } public void intakeLeftStop() { - intakeMotorLeft.set(0); + if (enableFrontAndSideIntakes) { + intakeMotorLeft.set(0); + } } public void intakeRightStop() { - intakeMotorRight.set(0); + if (enableFrontAndSideIntakes) { + intakeMotorRight.set(0); + } } // intake reject methods @@ -180,15 +192,21 @@ public void intakeReject() { } public void intakeFrontReject() { - intakeMotorFront.set(INTAKE_REJECT_SPEED); + if (enableFrontAndSideIntakes) { + intakeMotorFront.set(INTAKE_REJECT_SPEED); + } } public void intakeLeftReject() { - intakeMotorLeft.set(INTAKE_REJECT_SPEED); + if (enableFrontAndSideIntakes) { + intakeMotorLeft.set(INTAKE_REJECT_SPEED); + } } public void intakeRightReject() { - intakeMotorRight.set(INTAKE_REJECT_SPEED); + if (enableFrontAndSideIntakes) { + intakeMotorRight.set(INTAKE_REJECT_SPEED); + } } // index methods From af62d77c826f2b3b7ff5a03494cd304351347535 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Tue, 2 Apr 2024 19:31:27 -0700 Subject: [PATCH 10/19] Configure full manual shuffleboard entry more --- .../java/frc/team2412/robot/subsystems/LauncherSubsystem.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index b70377ea..f28bb5ce 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -384,6 +384,8 @@ private void initShuffleboard() { Shuffleboard.getTab("Launcher") .add("Full manual mode", false) .withPosition(3, 0) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kToggleSwitch) .getEntry(); new Trigger(() -> manualModeEntry.getBoolean(false)) .whileTrue( From 8b661a8c3159d341dc7a864754638283a904b2dd Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Tue, 2 Apr 2024 19:35:11 -0700 Subject: [PATCH 11/19] debouncer changes --- .../team2412/robot/subsystems/IntakeSubsystem.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index df7af191..eee7a125 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -56,6 +56,7 @@ public class IntakeSubsystem extends SubsystemBase { private final Debouncer intakeRightSensorDebouncer; private final Debouncer intakeLeftSensorDebouncer; private final Debouncer feederSensorDebouncer; + private final Debouncer feederSensorIRDebouncer; // private final SparkLimitSwitch intakeBackSensor; private final SparkLimitSwitch intakeLeftSensor; @@ -104,6 +105,7 @@ public IntakeSubsystem() { intakeLeftSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); feederSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); + feederSensorIRDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); resetMotors(); @@ -230,12 +232,16 @@ public boolean indexSensorHasNote() { } public boolean debouncedFeederSensor() { - return feederSensorDebouncer.calculate(feederSensor.isPressed()); + // return feederSensorDebouncer.calculate(feederSensor.isPressed()); + + boolean feederSensorSignal = feederSensorDebouncer.calculate(feederSensor.isPressed()); + boolean feederSensorIRSignal = feederSensorIRDebouncer.calculate(feederSensorIR.get()); + + return feederSensorSignal || feederSensorIRSignal; } public boolean feederSensorHasNote() { - return debouncedFeederSensor() && !getSensorOverride() - || !feederSensorIR.get() && !getSensorOverride(); + return debouncedFeederSensor() && !getSensorOverride(); } public boolean intakeFrontSeesNote() { From d46f7a8f1a7c881caaa2e59595e52f0d4a0b6c83 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Tue, 2 Apr 2024 19:42:31 -0700 Subject: [PATCH 12/19] unstati --- .../java/frc/team2412/robot/subsystems/IntakeSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index eee7a125..72f3a4fd 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -47,7 +47,7 @@ public class IntakeSubsystem extends SubsystemBase { // Sensors private final SparkLimitSwitch indexSensor; - private static SparkLimitSwitch feederSensor; + private final SparkLimitSwitch feederSensor; private final DigitalInput feederSensorIR; private final SparkLimitSwitch intakeFrontSensor; From 099328520fc95570c5cdcbd7436c4ff6d5874da8 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Tue, 2 Apr 2024 19:47:44 -0700 Subject: [PATCH 13/19] yohohoho merry christmas jameson --- .../robot/commands/intake/AllInCommand.java | 50 ------------------- 1 file changed, 50 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 280b340b..32e841b3 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -29,56 +29,6 @@ public void initialize() { @Override public void execute() { - // intake rejecting - // if (intakeSubsystem.intakeFrontSeesNote()) { - // if (intakeSubsystem.debouncedIntakeFrontSensor()) { - // if (!intakeSubsystem.getRejectOverride()) { - // intakeSubsystem.intakeLeftReject(); - // intakeSubsystem.intakeRightReject(); - // } - // } else { - // intakeSubsystem.intakeLeftStop(); - // intakeSubsystem.intakeRightStop(); - // } - - // if (controls != null && !rumbledIntakeFront) { - // Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); - // rumbledIntakeFront = true; - // } - // } - // if (intakeSubsystem.intakeLeftSeesNote()) { - // if (intakeSubsystem.debouncedIntakeLeftSensor()) { - // if (!intakeSubsystem.getRejectOverride()) { - // intakeSubsystem.intakeFrontReject(); - // intakeSubsystem.intakeRightReject(); - // } - // } else { - // intakeSubsystem.intakeFrontStop(); - // intakeSubsystem.intakeRightStop(); - // } - - // if (controls != null && !rumbledIntakeLeft) { - // Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); - // rumbledIntakeLeft = true; - // } - // } - - // if (intakeSubsystem.intakeRightSeesNote()) { - // if (intakeSubsystem.debouncedIntakeRightSensor()) { - // if (!intakeSubsystem.getRejectOverride()) { - // intakeSubsystem.intakeFrontReject(); - // intakeSubsystem.intakeLeftReject(); - // } - // } else { - // intakeSubsystem.intakeFrontStop(); - // intakeSubsystem.intakeLeftStop(); - // } - - // if (controls != null && !rumbledIntakeRight) { - // Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); - // rumbledIntakeRight = true; - // } - // } // all intake motors rejecting after index if (intakeSubsystem.indexSensorHasNote()) { From b99ec55b551a38f79969e7322c57619260d6cd31 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Tue, 2 Apr 2024 20:04:17 -0700 Subject: [PATCH 14/19] goo goo ga ga --- .../frc/team2412/robot/subsystems/IntakeSubsystem.java | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index eee7a125..479f347b 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -78,6 +78,8 @@ public class IntakeSubsystem extends SubsystemBase { // reject override private GenericEntry rejectOverride; + private boolean feederSensorSignal; + public IntakeSubsystem() { intakeMotorFront = new CANSparkMax(INTAKE_MOTOR_FRONT, MotorType.kBrushless); @@ -234,8 +236,8 @@ public boolean indexSensorHasNote() { public boolean debouncedFeederSensor() { // return feederSensorDebouncer.calculate(feederSensor.isPressed()); - boolean feederSensorSignal = feederSensorDebouncer.calculate(feederSensor.isPressed()); - boolean feederSensorIRSignal = feederSensorIRDebouncer.calculate(feederSensorIR.get()); + feederSensorSignal = feederSensorDebouncer.calculate(feederSensor.isPressed()); + feederSensorIRSignal = feederSensorIRDebouncer.calculate(feederSensorIR.get()); return feederSensorSignal || feederSensorIRSignal; } @@ -333,6 +335,9 @@ public void initShuffleboard() { shuffleboardTab.addBoolean("Index Sensor - ", this::indexSensorHasNote).withSize(1, 1); shuffleboardTab.addBoolean("Feeder Sensor - ", this::feederSensorHasNote).withSize(1, 1); + shuffleboardTab.addBoolean("Feeder switch", () -> feederSensorDebouncer.calculate(feederSensor.isPressed())); + shuffleboardTab.addBoolean("Feeder Sensor", () -> feederSensorIRDebouncer.calculate(feederSensorIR.get())); + // no intake back sensor shuffleboardTab.addBoolean("Intake Front Sensor - ", this::intakeFrontSeesNote).withSize(1, 1); shuffleboardTab.addBoolean("Intake Left Sensor - ", this::intakeLeftSeesNote).withSize(1, 1); From 41fb5dd289f46fe0660780d66928bb538a71f3e1 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Tue, 2 Apr 2024 20:05:06 -0700 Subject: [PATCH 15/19] real sensor logic --- .../java/frc/team2412/robot/subsystems/IntakeSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 3acfe279..c4d750f9 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -236,8 +236,8 @@ public boolean indexSensorHasNote() { public boolean debouncedFeederSensor() { // return feederSensorDebouncer.calculate(feederSensor.isPressed()); - feederSensorSignal = feederSensorDebouncer.calculate(feederSensor.isPressed()); - feederSensorIRSignal = feederSensorIRDebouncer.calculate(feederSensorIR.get()); + boolean feederSensorSignal = feederSensorDebouncer.calculate(feederSensor.isPressed()); + boolean feederSensorIRSignal = feederSensorIRDebouncer.calculate(feederSensorIR.get()); return feederSensorSignal || feederSensorIRSignal; } From 9a5cadfae234c4f1ccae1eaa9fba60decceeab00 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Tue, 2 Apr 2024 20:24:33 -0700 Subject: [PATCH 16/19] Fix feeder sensor inversion --- .../frc/team2412/robot/subsystems/IntakeSubsystem.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index c4d750f9..7a7eddb6 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -237,7 +237,7 @@ public boolean debouncedFeederSensor() { // return feederSensorDebouncer.calculate(feederSensor.isPressed()); boolean feederSensorSignal = feederSensorDebouncer.calculate(feederSensor.isPressed()); - boolean feederSensorIRSignal = feederSensorIRDebouncer.calculate(feederSensorIR.get()); + boolean feederSensorIRSignal = feederSensorIRDebouncer.calculate(!feederSensorIR.get()); return feederSensorSignal || feederSensorIRSignal; } @@ -335,8 +335,10 @@ public void initShuffleboard() { shuffleboardTab.addBoolean("Index Sensor - ", this::indexSensorHasNote).withSize(1, 1); shuffleboardTab.addBoolean("Feeder Sensor - ", this::feederSensorHasNote).withSize(1, 1); - shuffleboardTab.addBoolean("Feeder switch", () -> feederSensorDebouncer.calculate(feederSensor.isPressed())); - shuffleboardTab.addBoolean("Feeder Sensor", () -> feederSensorIRDebouncer.calculate(feederSensorIR.get())); + shuffleboardTab.addBoolean( + "Feeder beambreak", () -> feederSensorDebouncer.calculate(feederSensor.isPressed())); + shuffleboardTab.addBoolean( + "Feeder IR", () -> feederSensorIRDebouncer.calculate(!feederSensorIR.get())); // no intake back sensor shuffleboardTab.addBoolean("Intake Front Sensor - ", this::intakeFrontSeesNote).withSize(1, 1); From 8cec3aca1c0f792f72e6621c970c00d9dc1ccd9f Mon Sep 17 00:00:00 2001 From: Robototes DS Date: Tue, 2 Apr 2024 20:29:16 -0700 Subject: [PATCH 17/19] improve drivebase tuning a ton --- src/main/deploy/swerve/modules/backleft.json | 2 +- src/main/deploy/swerve/modules/backright.json | 2 +- src/main/deploy/swerve/modules/frontleft.json | 2 +- src/main/deploy/swerve/modules/frontright.json | 2 +- .../java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java | 2 +- src/main/java/swervelib/motors/TalonFXSwerve.java | 3 ++- 6 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index d368d2e7..c2735a21 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -28,7 +28,7 @@ "i": 0, "d": 0, "kS": 0.18532, - "kV": 0.347198082, + "kV": 1.6909, "kA": 0.28447 }, "angleTuning": { diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index a3d7b30d..37bf4195 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -28,7 +28,7 @@ "i": 0, "d": 0, "kS": 0.15508, - "kV": 0.35947706641, + "kV": 1.7507, "kA": 0.26534 }, "angleTuning": { diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index 7e101bfb..e83987de 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -28,7 +28,7 @@ "i": 0, "d": 0, "kS": 0.026162, - "kV": 0.3849384, + "kV": 1.8747, "kA": 0.39611 }, "angleTuning": { diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index ed88758f..fff7a39b 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -28,7 +28,7 @@ "i": 0, "d": 0, "kS": 0.12121, - "kV": 0.368676, + "kV": 1.7955, "kA": 0.33215 }, "angleTuning": { diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 2a6e8f2e..3e6fc02a 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -49,7 +49,7 @@ public class DrivebaseSubsystem extends SubsystemBase { Robot.getInstance().getRobotType() == RobotType.BONK ? 3.0 : Robot.getInstance().getRobotType() == RobotType.COMPETITION - ? 6.0 + ? 4.7 : Robot.getInstance().getRobotType() == RobotType.CRANE ? 3.0 : 1.0; // Auto align stuff, dw abt it diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index ace788bd..113eaaa7 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -315,7 +315,8 @@ public void setReference(double setpoint, double feedforward, double position) { // } if (isDriveMotor) { - motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint).withFeedForward(feedforward)); + motor.setControl( + m_velocityVoltageSetter.withVelocity(setpoint)); // .withFeedForward(feedforward)); } else { motor.setControl(m_angleVoltageSetter.withPosition(setpoint / 360.0)); } From a9c9919a299f9ea238f70d6d195bf6b522c68279 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Tue, 2 Apr 2024 20:34:52 -0700 Subject: [PATCH 18/19] Bump REVLib to 2024.2.4 --- vendordeps/REVLib.json | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 60eacf84..f85acd40 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.3", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.3" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From ed4032a2953c7787ab66feee8d5607b3133328fe Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Wed, 3 Apr 2024 09:23:22 -0700 Subject: [PATCH 19/19] Undo accidental deploy file change --- src/main/deploy/deploy | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/deploy/deploy b/src/main/deploy/deploy index 63c250c6..bb82515d 100644 --- a/src/main/deploy/deploy +++ b/src/main/deploy/deploy @@ -1,2 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the 'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function to get a proper path relative to the deploy directory. \ No newline at end of file