diff --git a/src/main/deploy/trajectories/N5-S0-C0123_driveToC0.pathblob b/src/main/deploy/trajectories/N5-S0-C0123_driveToC0.pathblob index 88b146ac..e937b477 100644 Binary files a/src/main/deploy/trajectories/N5-S0-C0123_driveToC0.pathblob and b/src/main/deploy/trajectories/N5-S0-C0123_driveToC0.pathblob differ diff --git a/src/main/deploy/trajectories/N5-S0-C0123_driveToC1.pathblob b/src/main/deploy/trajectories/N5-S0-C0123_driveToC1.pathblob index 8ad5b347..0c7f2600 100644 Binary files a/src/main/deploy/trajectories/N5-S0-C0123_driveToC1.pathblob and b/src/main/deploy/trajectories/N5-S0-C0123_driveToC1.pathblob differ diff --git a/src/main/deploy/trajectories/N5-S0-C0123_driveToC2.pathblob b/src/main/deploy/trajectories/N5-S0-C0123_driveToC2.pathblob index 8a31ca9b..4fd66c78 100644 Binary files a/src/main/deploy/trajectories/N5-S0-C0123_driveToC2.pathblob and b/src/main/deploy/trajectories/N5-S0-C0123_driveToC2.pathblob differ diff --git a/src/main/deploy/trajectories/N5-S0-C0123_driveToS0.pathblob b/src/main/deploy/trajectories/N5-S0-C0123_driveToS0.pathblob index 815968c8..53233387 100644 Binary files a/src/main/deploy/trajectories/N5-S0-C0123_driveToS0.pathblob and b/src/main/deploy/trajectories/N5-S0-C0123_driveToS0.pathblob differ diff --git a/src/main/deploy/trajectories/N5-S1-C234_driveToC2.pathblob b/src/main/deploy/trajectories/N5-S1-C234_driveToC2.pathblob index b971091a..d970a4ab 100644 Binary files a/src/main/deploy/trajectories/N5-S1-C234_driveToC2.pathblob and b/src/main/deploy/trajectories/N5-S1-C234_driveToC2.pathblob differ diff --git a/src/main/deploy/trajectories/N5-S1-C234_driveToC3.pathblob b/src/main/deploy/trajectories/N5-S1-C234_driveToC3.pathblob index a171a738..7c19ca3a 100644 Binary files a/src/main/deploy/trajectories/N5-S1-C234_driveToC3.pathblob and b/src/main/deploy/trajectories/N5-S1-C234_driveToC3.pathblob differ diff --git a/src/main/deploy/trajectories/N5-S1-C234_driveToC4.pathblob b/src/main/deploy/trajectories/N5-S1-C234_driveToC4.pathblob index 20613717..0df965f4 100644 Binary files a/src/main/deploy/trajectories/N5-S1-C234_driveToC4.pathblob and b/src/main/deploy/trajectories/N5-S1-C234_driveToC4.pathblob differ diff --git a/src/main/deploy/trajectories/N5-S1-C234_driveToS1.pathblob b/src/main/deploy/trajectories/N5-S1-C234_driveToS1.pathblob index 3e1a87fd..5ad5b79a 100644 Binary files a/src/main/deploy/trajectories/N5-S1-C234_driveToS1.pathblob and b/src/main/deploy/trajectories/N5-S1-C234_driveToS1.pathblob differ diff --git a/src/main/deploy/trajectories/N6-S12-C0123_driveToC0.pathblob b/src/main/deploy/trajectories/N6-S12-C0123_driveToC0.pathblob index df297cd3..6ed9eedd 100644 Binary files a/src/main/deploy/trajectories/N6-S12-C0123_driveToC0.pathblob and b/src/main/deploy/trajectories/N6-S12-C0123_driveToC0.pathblob differ diff --git a/src/main/deploy/trajectories/N6-S12-C0123_driveToC1.pathblob b/src/main/deploy/trajectories/N6-S12-C0123_driveToC1.pathblob index 7c6344d5..6753d8e0 100644 Binary files a/src/main/deploy/trajectories/N6-S12-C0123_driveToC1.pathblob and b/src/main/deploy/trajectories/N6-S12-C0123_driveToC1.pathblob differ diff --git a/src/main/deploy/trajectories/N6-S12-C0123_driveToC2.pathblob b/src/main/deploy/trajectories/N6-S12-C0123_driveToC2.pathblob index 48069c61..e619599d 100644 Binary files a/src/main/deploy/trajectories/N6-S12-C0123_driveToC2.pathblob and b/src/main/deploy/trajectories/N6-S12-C0123_driveToC2.pathblob differ diff --git a/src/main/deploy/trajectories/N6-S12-C0123_driveToS1.pathblob b/src/main/deploy/trajectories/N6-S12-C0123_driveToS1.pathblob index 815968c8..53233387 100644 Binary files a/src/main/deploy/trajectories/N6-S12-C0123_driveToS1.pathblob and b/src/main/deploy/trajectories/N6-S12-C0123_driveToS1.pathblob differ diff --git a/src/main/deploy/trajectories/N6-S12-C0123_driveToS2.pathblob b/src/main/deploy/trajectories/N6-S12-C0123_driveToS2.pathblob index c5ea9c07..3f001940 100644 Binary files a/src/main/deploy/trajectories/N6-S12-C0123_driveToS2.pathblob and b/src/main/deploy/trajectories/N6-S12-C0123_driveToS2.pathblob differ diff --git a/src/main/deploy/trajectories/davisEthicalAuto_driveToCenterline0.pathblob b/src/main/deploy/trajectories/davisEthicalAuto_driveToCenterline0.pathblob index 308ce8a6..23acec27 100644 Binary files a/src/main/deploy/trajectories/davisEthicalAuto_driveToCenterline0.pathblob and b/src/main/deploy/trajectories/davisEthicalAuto_driveToCenterline0.pathblob differ diff --git a/src/main/deploy/trajectories/davisEthicalAuto_driveToCenterline1.pathblob b/src/main/deploy/trajectories/davisEthicalAuto_driveToCenterline1.pathblob index ff759772..ab80429f 100644 Binary files a/src/main/deploy/trajectories/davisEthicalAuto_driveToCenterline1.pathblob and b/src/main/deploy/trajectories/davisEthicalAuto_driveToCenterline1.pathblob differ diff --git a/src/main/deploy/trajectories/davisEthicalAuto_driveToCenterline2.pathblob b/src/main/deploy/trajectories/davisEthicalAuto_driveToCenterline2.pathblob index 6a404367..619dd54b 100644 Binary files a/src/main/deploy/trajectories/davisEthicalAuto_driveToCenterline2.pathblob and b/src/main/deploy/trajectories/davisEthicalAuto_driveToCenterline2.pathblob differ diff --git a/src/main/deploy/trajectories/davisEthicalAuto_driveToPodium.pathblob b/src/main/deploy/trajectories/davisEthicalAuto_driveToPodium.pathblob index 700e8b89..c890d692 100644 Binary files a/src/main/deploy/trajectories/davisEthicalAuto_driveToPodium.pathblob and b/src/main/deploy/trajectories/davisEthicalAuto_driveToPodium.pathblob differ diff --git a/src/main/java/org/littletonrobotics/frc2024/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java index 91816999..b30bded7 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Constants.java +++ b/src/main/java/org/littletonrobotics/frc2024/Constants.java @@ -21,7 +21,7 @@ */ public final class Constants { public static final double loopPeriodSecs = 0.02; - private static RobotType robotType = RobotType.SIMBOT; + private static RobotType robotType = RobotType.DEVBOT; public static final boolean tuningMode = true; public static RobotType getRobot() { diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 01741495..d149a309 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -9,7 +9,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; @@ -28,17 +27,21 @@ import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels; import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIO; import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSim; +import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSparkFlex; import org.littletonrobotics.frc2024.subsystems.rollers.Rollers; import org.littletonrobotics.frc2024.subsystems.rollers.RollersSensorsIO; import org.littletonrobotics.frc2024.subsystems.rollers.RollersSensorsIOReal; +import org.littletonrobotics.frc2024.subsystems.rollers.backpack.Backpack; +import org.littletonrobotics.frc2024.subsystems.rollers.backpack.BackpackIOSim; +import org.littletonrobotics.frc2024.subsystems.rollers.backpack.BackpackIOSparkFlex; import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder; import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIO; import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOKrakenFOC; import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOSim; import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer; import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIO; +import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIODevbot; import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSim; -import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSparkFlex; import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake; import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIO; import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIOKrakenFOC; @@ -82,6 +85,7 @@ public RobotContainer() { Feeder feeder = null; Indexer indexer = null; Intake intake = null; + Backpack backpack = null; Arm arm = null; // Create subsystems @@ -106,12 +110,13 @@ public RobotContainer() { new AprilTagVisionIONorthstar( AprilTagVisionConstants.instanceNames[1], AprilTagVisionConstants.cameraIds[1])); - // flywheels = new Flywheels(new FlywheelsIOSparkFlex()); + flywheels = new Flywheels(new FlywheelsIOSparkFlex()); feeder = new Feeder(new FeederIOKrakenFOC()); - indexer = new Indexer(new IndexerIOSparkFlex()); + indexer = new Indexer(new IndexerIODevbot()); intake = new Intake(new IntakeIOKrakenFOC()); - rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIOReal()); + backpack = new Backpack(new BackpackIOSparkFlex()); + rollers = new Rollers(feeder, indexer, intake, backpack, new RollersSensorsIOReal()); arm = new Arm(new ArmIOKrakenFOC()); } @@ -128,7 +133,8 @@ public RobotContainer() { feeder = new Feeder(new FeederIOSim()); indexer = new Indexer(new IndexerIOSim()); intake = new Intake(new IntakeIOSim()); - rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIO() {}); + backpack = new Backpack(new BackpackIOSim()); + rollers = new Rollers(feeder, indexer, intake, backpack, new RollersSensorsIO() {}); arm = new Arm(new ArmIOSim()); } @@ -166,7 +172,7 @@ public RobotContainer() { intake = new Intake(new IntakeIO() {}); } if (rollers == null) { - rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIO() {}); + rollers = new Rollers(feeder, indexer, intake, backpack, new RollersSensorsIO() {}); } if (arm == null) { arm = new Arm(new ArmIO() {}); @@ -245,7 +251,6 @@ private void configureButtonBindings() { () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0))); controller .rightTrigger() - .and(readyToShoot) .onTrue( Commands.parallel( Commands.waitSeconds(0.5), @@ -269,21 +274,11 @@ private void configureButtonBindings() { .intake() .alongWith(Commands.waitUntil(superstructure::atGoal).andThen(rollers.ejectFloor())) .withName("Eject To Floor")); + // Test rollers with amp score + controller.b().whileTrue(rollers.ampScore()); controller .y() - .onTrue( - Commands.runOnce( - () -> - robotState.resetPose( - AllianceFlipUtil.apply( - new Pose2d( - Units.inchesToMeters(36.0), - FieldConstants.Speaker.centerSpeakerOpening.getY(), - new Rotation2d())))) - .ignoringDisable(true)); - controller - .b() .onTrue( Commands.runOnce( () -> diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java index d8e9ae11..1da1db8e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java @@ -79,7 +79,7 @@ public static Command waitUntilXCrossed(double xPosition, boolean towardsCenterl public static Command intake(Superstructure superstructure, Rollers rollers) { return parallel( superstructure.intake(), rollers.floorIntake().beforeStarting(superstructure::atGoal)) - .until(rollers::hasGamepiece); + .until(rollers::gamepieceStaged); } /** Shoots note, ending after rollers have spun */ diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java index 0ccc1bde..b67d02a5 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java @@ -255,8 +255,8 @@ public class DriveTrajectories { List.of( PathSegment.newBuilder() .addPoseWaypoint(startingSourceFace) - .addPoseWaypoint(intakingCenterlinePoses[0]) - .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.6))) + .addPoseWaypoint(intakingCenterlinePoses[0], 100) + .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.6)), 100) .build())); paths.put( diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java index 153635f8..420125cf 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -33,9 +33,9 @@ public class Flywheels extends SubsystemBase { private static final LoggedTunableNumber shootingRightRpm = new LoggedTunableNumber("Flywheels/ShootingRightRpm", 4000.0); private static final LoggedTunableNumber idleLeftRpm = - new LoggedTunableNumber("Flywheels/IdleLeftRpm", 1500.0); + new LoggedTunableNumber("Flywheels/IdleLeftRpm", 200); private static final LoggedTunableNumber idleRightRpm = - new LoggedTunableNumber("Flywheels/IdleRightRpm", 1000.0); + new LoggedTunableNumber("Flywheels/IdleRightRpm", 200); private static final LoggedTunableNumber intakingRpm = new LoggedTunableNumber("Flywheels/IntakingRpm", -2000.0); private static final LoggedTunableNumber ejectingRpm = diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java index f6f5976e..7fddd054 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java @@ -11,6 +11,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import lombok.Getter; +import lombok.Setter; +import org.littletonrobotics.frc2024.subsystems.rollers.backpack.Backpack; import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder; import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer; import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake; @@ -21,6 +23,7 @@ public class Rollers extends SubsystemBase { private final Feeder feeder; private final Indexer indexer; private final Intake intake; + private final Backpack backpack; private final RollersSensorsIO sensorsIO; private final RollersSensorsIOInputsAutoLogged sensorInputs = @@ -32,15 +35,28 @@ public enum Goal { STATION_INTAKE, EJECT_TO_FLOOR, QUICK_INTAKE_TO_FEED, - FEED_TO_SHOOTER + FEED_TO_SHOOTER, + AMP_SCORE } - @Getter private Goal goal = Goal.IDLE; + public enum GamepieceState { + NONE, + SHOOTER_STAGED + } - public Rollers(Feeder feeder, Indexer indexer, Intake intake, RollersSensorsIO sensorsIO) { + @Getter private Goal goal = Goal.IDLE; + @Getter @Setter private GamepieceState gamepieceState = GamepieceState.NONE; + + public Rollers( + Feeder feeder, + Indexer indexer, + Intake intake, + Backpack backpack, + RollersSensorsIO sensorsIO) { this.feeder = feeder; this.indexer = indexer; this.intake = intake; + this.backpack = backpack; this.sensorsIO = sensorsIO; setDefaultCommand(runOnce(this::goIdle).withName("Rollers Idling")); @@ -60,27 +76,25 @@ public void periodic() { feeder.setGoal(Feeder.Goal.IDLING); indexer.setGoal(Indexer.Goal.IDLING); intake.setGoal(Intake.Goal.IDLING); + backpack.setGoal(Backpack.Goal.IDLING); } case FLOOR_INTAKE -> { feeder.setGoal(Feeder.Goal.FLOOR_INTAKING); indexer.setGoal(Indexer.Goal.FLOOR_INTAKING); intake.setGoal(Intake.Goal.FLOOR_INTAKING); + backpack.setGoal(Backpack.Goal.IDLING); if (sensorInputs.shooterStaged) { indexer.setGoal(Indexer.Goal.IDLING); + gamepieceState = GamepieceState.SHOOTER_STAGED; } } - case STATION_INTAKE -> { - feeder.setGoal(Feeder.Goal.IDLING); - indexer.setGoal(Indexer.Goal.STATION_INTAKING); - intake.setGoal(Intake.Goal.IDLING); - if (sensorInputs.shooterStaged) { // TODO: add this banner sensor - indexer.setGoal(Indexer.Goal.IDLING); - } - } + case STATION_INTAKE -> {} case EJECT_TO_FLOOR -> { feeder.setGoal(Feeder.Goal.EJECTING); indexer.setGoal(Indexer.Goal.EJECTING); intake.setGoal(Intake.Goal.EJECTING); + backpack.setGoal(Backpack.Goal.IDLING); + gamepieceState = GamepieceState.NONE; } case QUICK_INTAKE_TO_FEED -> { feeder.setGoal(Feeder.Goal.SHOOTING); @@ -91,20 +105,30 @@ public void periodic() { feeder.setGoal(Feeder.Goal.SHOOTING); indexer.setGoal(Indexer.Goal.SHOOTING); intake.setGoal(Intake.Goal.IDLING); + backpack.setGoal(Backpack.Goal.IDLING); + gamepieceState = GamepieceState.NONE; + } + case AMP_SCORE -> { + feeder.setGoal(Feeder.Goal.IDLING); + indexer.setGoal(Indexer.Goal.EJECTING); + intake.setGoal(Intake.Goal.IDLING); + backpack.setGoal(Backpack.Goal.AMP_SCORING); + gamepieceState = GamepieceState.NONE; } } feeder.periodic(); indexer.periodic(); intake.periodic(); + backpack.periodic(); } private void goIdle() { goal = Goal.IDLE; } - public boolean hasGamepiece() { - return sensorInputs.shooterStaged; + public boolean gamepieceStaged() { + return gamepieceState == GamepieceState.SHOOTER_STAGED; } public Command floorIntake() { @@ -130,4 +154,8 @@ public Command feedShooter() { .alongWith(NoteVisualizer.shoot()) .withName("Rollers Feed Shooter"); } + + public Command ampScore() { + return startEnd(() -> goal = Goal.AMP_SCORE, this::goIdle).withName("Rollers Amp Scoring"); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/Backpack.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/Backpack.java index bb6737b1..f46a4ec2 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/Backpack.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/Backpack.java @@ -1,28 +1,35 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + package org.littletonrobotics.frc2024.subsystems.rollers.backpack; +import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.Setter; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystem; -import java.util.function.DoubleSupplier; - @Setter @Getter public class Backpack extends GenericRollerSystem { - @RequiredArgsConstructor - @Getter - public enum Goal implements VoltageGoal { - IDLING(() -> 0), - AMP_SCORING(new LoggedTunableNumber("Backpack/AmpScoringVoltage")); + @RequiredArgsConstructor + @Getter + public enum Goal implements VoltageGoal { + IDLING(() -> 0), + EJECTING(new LoggedTunableNumber("Backpack/EjectingVoltage", -12.0)), + AMP_SCORING(new LoggedTunableNumber("Backpack/AmpScoringVoltage", 12.0)); - private final DoubleSupplier voltageSupplier; - } + private final DoubleSupplier voltageSupplier; + } - private Goal goal = Goal.IDLING; + private Goal goal = Goal.IDLING; - public Backpack(BackpackIO io) { - super("Backpack", io); - } + public Backpack(BackpackIO io) { + super("Backpack", io); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIO.java index 523b66b1..404046b8 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIO.java @@ -1,3 +1,10 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + package org.littletonrobotics.frc2024.subsystems.rollers.backpack; import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystemIO; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIOSim.java index 939f4b63..3cc4ae1e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIOSim.java @@ -1,14 +1,21 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + package org.littletonrobotics.frc2024.subsystems.rollers.backpack; import edu.wpi.first.math.system.plant.DCMotor; import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystemIOSim; public class BackpackIOSim extends GenericRollerSystemIOSim implements BackpackIO { - private static final DCMotor motorModel = DCMotor.getNeoVortex(1); - private static final double reduction = (1.0 / 1.0); - private static final double moi = 0.001; + private static final DCMotor motorModel = DCMotor.getNeoVortex(1); + private static final double reduction = (1.0 / 1.0); + private static final double moi = 0.001; - public BackpackIOSim() { - super(motorModel, reduction, moi); - } + public BackpackIOSim() { + super(motorModel, reduction, moi); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIOSparkFlex.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIOSparkFlex.java index 16451773..b58a00d7 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIOSparkFlex.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIOSparkFlex.java @@ -1,15 +1,22 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + package org.littletonrobotics.frc2024.subsystems.rollers.backpack; import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystemIOSparkFlex; public class BackpackIOSparkFlex extends GenericRollerSystemIOSparkFlex implements BackpackIO { - private static final int id = 0; - private static final int currentLimitAmps = 40; - private static final boolean invert = false; - private static final boolean brake = true; - private static final double reduction = (1.0 / 1.0); - - public BackpackIOSparkFlex() { - super(id, currentLimitAmps, invert, brake, reduction); - } + private static final int id = 6; + private static final int currentLimitAmps = 40; + private static final boolean invert = true; + private static final boolean brake = true; + private static final double reduction = (1.0 / 1.0); + + public BackpackIOSparkFlex() { + super(id, currentLimitAmps, invert, brake, reduction); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java index 6f399376..01cf27fb 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java @@ -11,8 +11,8 @@ import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.Setter; -import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystem; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; +import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystem; @Setter @Getter diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java index bfc03713..7d6a64ea 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java @@ -11,8 +11,8 @@ import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.Setter; -import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystem; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; +import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystem; @Setter @Getter diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIODevbot.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIODevbot.java new file mode 100644 index 00000000..b170a3af --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIODevbot.java @@ -0,0 +1,49 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.littletonrobotics.frc2024.subsystems.rollers.indexer; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystemIOSparkFlex; + +public class IndexerIODevbot extends GenericRollerSystemIOSparkFlex implements IndexerIO { + private static final double reduction = (18.0 / 12.0); + private static final int id = 27; + private static final int currentLimitAmps = 40; + private static final boolean inverted = false; + + private final TalonFX middleMotor; + + public IndexerIODevbot() { + super(id, currentLimitAmps, inverted, true, reduction); + middleMotor = new TalonFX(7, "rio"); + + TalonFXConfiguration middleConfig = new TalonFXConfiguration(); + middleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + middleConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + middleConfig.CurrentLimits.SupplyCurrentLimit = currentLimitAmps; + middleConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + middleMotor.getConfigurator().apply(middleConfig); + } + + @Override + public void runVolts(double volts) { + super.runVolts(volts); + middleMotor.setControl(new VoltageOut(volts).withUpdateFreqHz(0.0)); + } + + @Override + public void stop() { + super.stop(); + middleMotor.setControl(new NeutralOut()); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSparkFlex.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSparkFlex.java deleted file mode 100644 index 13df1c6e..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSparkFlex.java +++ /dev/null @@ -1,21 +0,0 @@ -// Copyright (c) 2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package org.littletonrobotics.frc2024.subsystems.rollers.indexer; - -import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystemIOSparkFlex; - -public class IndexerIOSparkFlex extends GenericRollerSystemIOSparkFlex implements IndexerIO { - private static final double reduction = (18.0 / 12.0); - private static final int id = 6; - private static final int currentLimitAmps = 40; - private static final boolean inverted = false; - - public IndexerIOSparkFlex() { - super(id, currentLimitAmps, inverted, true, reduction); - } -} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java index ea28b1fa..5c2a288f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java @@ -11,8 +11,8 @@ import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.Setter; -import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystem; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; +import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystem; @Setter @Getter diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java index c1af7ed4..71b0de9a 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java @@ -61,6 +61,7 @@ public enum Goal { CUSTOM(new LoggedTunableNumber("Arm/CustomSetpoint", 20.0)); private final DoubleSupplier armSetpointSupplier; + private double getRads() { return Units.degreesToRadians(armSetpointSupplier.getAsDouble()); } @@ -123,7 +124,7 @@ public void periodic() { setpointState = motionProfile.calculate( Constants.loopPeriodSecs, - new TrapezoidProfile.State(inputs.armPositionRads, inputs.armVelocityRadsPerSec), + new TrapezoidProfile.State(setpointState.position, setpointState.velocity), new TrapezoidProfile.State( MathUtil.clamp( goal.getRads(), diff --git a/src/main/java/org/littletonrobotics/frc2024/util/drivers/rollers/GenericRollerSystem.java b/src/main/java/org/littletonrobotics/frc2024/util/drivers/rollers/GenericRollerSystem.java index 92641dbe..8aa9f8ba 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/drivers/rollers/GenericRollerSystem.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/drivers/rollers/GenericRollerSystem.java @@ -9,7 +9,6 @@ import java.util.function.DoubleSupplier; import lombok.RequiredArgsConstructor; -import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystemIOInputsAutoLogged; import org.littletonrobotics.junction.Logger; @RequiredArgsConstructor