From 0e79f65d9f2a1889571f18b2f577b5362659fee7 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Wed, 7 Feb 2024 19:31:46 -0500 Subject: [PATCH 01/45] I don't know... Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- build.gradle | 5 +++- src/main/java/frc/robot/RobotContainer.java | 29 ++++++++++++++----- .../frc/robot/subsystems/drive/Drive.java | 10 ++++--- .../frc/robot/subsystems/intake/Intake.java | 21 ++++++++++---- .../frc/robot/subsystems/intake/IntakeIO.java | 2 -- .../robot/subsystems/intake/IntakeIOSim.java | 10 +++---- .../subsystems/intake/IntakeIOSparkMax.java | 3 +- 7 files changed, 54 insertions(+), 26 deletions(-) diff --git a/build.gradle b/build.gradle index d3902356..b9497c4a 100755 --- a/build.gradle +++ b/build.gradle @@ -1,3 +1,5 @@ +import com.diffplug.spotless.FormatterStep + plugins { id "java" id "edu.wpi.first.GradleRIO" version "2024.1.1" @@ -150,7 +152,8 @@ spotless { exclude "**/build/**", "**/build-*/**" } toggleOffOn() - googleJavaFormat() + def format = googleJavaFormat() + println format removeUnusedImports() trimTrailingWhitespace() endWithNewline() diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8e916894..e555b961 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -159,15 +159,18 @@ private void configureButtonBindings() { () -> -controller.getRightX())); intake.setDefaultCommand( new RunCommand( - () -> intake.setIntakePosition(new Rotation2d(-15.0)), - intake)); // FIXME: NEED THE REAL ANGLE FOR THIS DEFAULT COMMAND STILL! + () -> { + intake.setIntakePosition(Rotation2d.fromDegrees(-130)); + intake.setRollerPercentage(0.0); + }, + intake)); feeder.setDefaultCommand(new RunCommand(feeder::stop, feeder)); // ---- DRIVETRAIN COMMANDS ---- - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + controller.x().whileTrue(Commands.runOnce(drive::stopWithX, drive)); controller .b() - .onTrue( + .whileTrue( Commands.runOnce( () -> { try { @@ -183,17 +186,29 @@ private void configureButtonBindings() { controller .leftTrigger() .and(feeder::getSensorFeed) - .onTrue( + .whileTrue( new RunCommand(() -> feeder.runVolts(6.0), feeder) .until(() -> !feeder.getSensorFeed())); // ---- INTAKE COMMANDS ---- controller .leftBumper() // not a() - .onTrue(new RunCommand(() -> intake.setIntakePosition(new Rotation2d(115.0)), intake)); + .whileTrue( + new RunCommand( + () -> { + intake.setIntakePosition(Rotation2d.fromDegrees(0.0)); + intake.setRollerPercentage(0.75); + }, + intake)); controller .rightBumper() - .onTrue(new RunCommand(() -> intake.setRollerPercentage(0.75), intake)); + .whileTrue( + new RunCommand( + () -> { + intake.setIntakePosition(Rotation2d.fromDegrees(-90.0)); + intake.setRollerPercentage(0.0); + }, + intake)); // ---- SHOOTER COMMANDS ---- controller diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index fe4382de..bc089aa1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -166,7 +166,8 @@ public void periodic() { } // Apply the twist (change since last loop cycle) to the current pose - noGyroRotation = pose.exp(twist).getRotation(); + noGyroRotation = + pose.rotateBy(noGyroRotation.minus(pose.getRotation())).exp(twist).getRotation(); pose = noGyroPoseEstimation.update(noGyroRotation, swerveModulePositions); } } @@ -299,9 +300,10 @@ public void setPose(Pose2d pose) throws GyroConnectionException { if (gyroInputs.connected) this.poseEstimator.resetPosition(gyroInputs.yawPosition, swerveModulePositions, pose); else this.noGyroPoseEstimation.resetPosition(noGyroRotation, swerveModulePositions, pose); - throw new GyroConnectionException( - "Pose estimator was written to without gyroscope data (idk what" - + " will happen after this because now if the gyroscope comes back online pose may act weird"); + // throw new GyroConnectionException( + // "Pose estimator was written to without gyroscope data (idk what" + // + " will happen after this because now if the gyroscope comes back online pose may + // act weird"); } /** diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 2d072a92..1680e8c0 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -44,7 +44,7 @@ public Intake(IntakeIO io) { armFF = new ArmFeedforward(0.0, 0.21, 0.195, 0.0); armFB = new ProfiledPIDController( - 1.0, + 5.0, 0.0, 0.0, new Constraints(RotationsPerSecond.of(3), RotationsPerSecond.per(Second).of(9))); @@ -60,13 +60,21 @@ public Intake(IntakeIO io) { new MechanismLigament2d("Intake", .135, off1.getDegrees(), .1, new Color8Bit(1, 1, 1)); ligament1A = new MechanismLigament2d( - "Intake", 0.232427, -off1A.plus(off1).getDegrees(), .5, new Color8Bit(1, 1, 1)); + "Intake", + 0.232427, + off1A.plus(off1).minus(quarterTurn).getDegrees(), + .5, + new Color8Bit(1, 1, 1)); // ligament1 ligament2 = new MechanismLigament2d("Intake2", .227, off2.getDegrees(), .1, new Color8Bit(1, 1, 1)); ligament2A = new MechanismLigament2d( - "Intake2", 0.232983, -off2A.plus(off2).getDegrees(), .5, new Color8Bit(1, 1, 1)); + "Intake2", + 0.232983, + off2A.plus(off2).minus(quarterTurn).getDegrees(), + .5, + new Color8Bit(1, 1, 1)); root.append(ligament1).append(ligament1A); root.append(ligament2).append(ligament2A); } @@ -77,6 +85,7 @@ public Intake(IntakeIO io) { Rotation2d off2A = new Rotation2d(0.154247715); // double position = 0.0; Rotation2d armTarget = Rotation2d.fromDegrees(0); + Rotation2d quarterTurn = Rotation2d.fromRadians(Math.PI / 2); @Override public void periodic() { @@ -88,10 +97,10 @@ public void periodic() { + armFF.calculate(armFB.getSetpoint().position, armFB.getSetpoint().velocity)); else io.setArmVoltage(0.0); - Logger.recordOutput("Intake", mechanism2d); Rotation2d rotation2d = new Rotation2d(inputs.armPositionRad); - ligament1.setAngle(rotation2d.plus(off1)); - ligament2.setAngle(rotation2d.plus(off2)); + ligament1.setAngle(rotation2d.plus(off1).minus(quarterTurn)); + ligament2.setAngle(rotation2d.plus(off2).minus(quarterTurn)); + Logger.recordOutput("Intake", mechanism2d); } public void setIntakePosition(Rotation2d position) { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index c9d6c042..528b583c 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -27,6 +27,4 @@ public default void setArmVoltage(double volts) {} /** Set intake wheel voltage. */ public default void setRollerPercent(double percent) {} - - } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index fa089739..1762e8d2 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -17,8 +17,8 @@ public class IntakeIOSim implements IntakeIO { 100, .1819, Units.inchesToMeters(7.063364), - PI * -.25, - PI * .6, + PI * -.2, + PI * .8, true, 0); // mass is 8.495 lbs private double armVoltage = 0.0; @@ -35,9 +35,9 @@ public void updateInputs(IntakeIOInputs inputs) { var ct = Timer.getFPGATimestamp(); var dt = (timestamp == null) ? .02 : ct - timestamp; inputs.armCurrentAmps = new double[] {armSim.getCurrentDrawAmps()}; - inputs.armAppliedVolts = armVoltage; - inputs.armPositionRad = armSim.getAngleRads(); - inputs.armVelocityRadPerSec = armSim.getVelocityRadPerSec(); + inputs.armAppliedVolts = -armVoltage; + inputs.armPositionRad = -armSim.getAngleRads(); + inputs.armVelocityRadPerSec = -armSim.getVelocityRadPerSec(); armSim.update(0.02); inputs.rollerCurrentAmps = new double[] {rollerSim.getCurrentDrawAmps()}; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index 5c7411df..11c567fe 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -27,7 +27,8 @@ public class IntakeIOSparkMax implements IntakeIO { private final CANSparkMax arm = new CANSparkMax(0, MotorType.kBrushless); private final CANSparkMax roller = new CANSparkMax(1, MotorType.kBrushless); - private final AbsoluteEncoder encoder = arm.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); + private final AbsoluteEncoder encoder = + arm.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); private final SparkPIDController pid = arm.getPIDController(); public IntakeIOSparkMax() { From d8d77b879ebbd7d94541a8cb6fe654502eeac8a7 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Wed, 7 Feb 2024 19:43:52 -0500 Subject: [PATCH 02/45] Intake visualizer and intake command both work properly Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- .../java/frc/robot/subsystems/intake/Intake.java | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 1680e8c0..dbe520d6 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -60,21 +60,13 @@ public Intake(IntakeIO io) { new MechanismLigament2d("Intake", .135, off1.getDegrees(), .1, new Color8Bit(1, 1, 1)); ligament1A = new MechanismLigament2d( - "Intake", - 0.232427, - off1A.plus(off1).minus(quarterTurn).getDegrees(), - .5, - new Color8Bit(1, 1, 1)); + "Intake", 0.232427, off1A.minus(quarterTurn).getDegrees(), .5, new Color8Bit(1, 1, 1)); // ligament1 ligament2 = new MechanismLigament2d("Intake2", .227, off2.getDegrees(), .1, new Color8Bit(1, 1, 1)); ligament2A = new MechanismLigament2d( - "Intake2", - 0.232983, - off2A.plus(off2).minus(quarterTurn).getDegrees(), - .5, - new Color8Bit(1, 1, 1)); + "Intake2", 0.232983, off2A.minus(quarterTurn).getDegrees(), .5, new Color8Bit(1, 1, 1)); root.append(ligament1).append(ligament1A); root.append(ligament2).append(ligament2A); } @@ -98,8 +90,8 @@ public void periodic() { else io.setArmVoltage(0.0); Rotation2d rotation2d = new Rotation2d(inputs.armPositionRad); - ligament1.setAngle(rotation2d.plus(off1).minus(quarterTurn)); - ligament2.setAngle(rotation2d.plus(off2).minus(quarterTurn)); + ligament1.setAngle(rotation2d.plus(off1).minus(quarterTurn).times(-1)); + ligament2.setAngle(rotation2d.plus(off2).minus(quarterTurn).times(-1)); Logger.recordOutput("Intake", mechanism2d); } From ae0234244f07c290016f056cd6b0a8cf8c293a0f Mon Sep 17 00:00:00 2001 From: KP <86213869+Kanishk-Pandey@users.noreply.github.com> Date: Sun, 11 Feb 2024 11:23:25 -0500 Subject: [PATCH 03/45] fixed new advantage scope files --- build.gradle | 2 +- vendordeps/AdvantageKit.json | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/build.gradle b/build.gradle index d3902356..7acfc04c 100755 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.12.0" } diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index a3a16b69..dc693b54 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.0.0", + "version": "3.0.2", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.0.0" + "version": "3.0.2" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.0.0" + "version": "3.0.2" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.0.0" + "version": "3.0.2" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.0.0", + "version": "3.0.2", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ From ac6de07cc8f0a98536a112ffd18b86d716d1ba13 Mon Sep 17 00:00:00 2001 From: nataliemcwhorter Date: Tue, 13 Feb 2024 14:32:05 -0500 Subject: [PATCH 04/45] made climb build --- src/main/java/frc/robot/subsystems/climb/ClimbIO.java | 4 ++-- src/main/java/frc/robot/subsystems/climb/ClimbIOSim.java | 5 ++--- src/main/java/frc/robot/util/LocalADStarAK.java | 7 ++++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java index dd849b7b..4d517c19 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java @@ -25,14 +25,14 @@ public static class ClimbIOInputs { public double leftVelocityRadPerSec = 0.0; public double leftAppliedVolts = 0.0; public double[] leftCurrentAmps = new double[] {}; - public double leftPosition = 0.0; + public Rotation2d leftPosition; public double[] leftTemperature = new double[] {}; public double rightPositionRad = 0.0; public double rightVelocityRadPerSec = 0.0; public double rightAppliedVolts = 0.0; public double[] rightCurrentAmps = new double[] {}; - public double rightPosition = 0.0; + public Rotation2d rightPosition; public double[] rightTemperature = new double[] {}; } } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIOSim.java b/src/main/java/frc/robot/subsystems/climb/ClimbIOSim.java index 4a3b3919..131f5dd6 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIOSim.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIOSim.java @@ -1,4 +1,3 @@ - package frc.robot.subsystems.climb; import edu.wpi.first.math.geometry.Rotation2d; @@ -33,12 +32,12 @@ public void updateInputs(ClimbIOInputs inputs) { rightSim.update(0.02); inputs.leftPositionRad = leftSim.getAngularPositionRad(); - inputs.leftPosition = new Rotation2d(leftSim.getAngularPositionRad()); + inputs.leftPosition = (Rotation2d) new Rotation2d(leftSim.getAngularPositionRad()); inputs.leftVelocityRadPerSec = leftSim.getAngularVelocityRadPerSec(); inputs.leftAppliedVolts = leftAppliedVolts; inputs.leftCurrentAmps = new double[] {Math.abs(leftSim.getCurrentDrawAmps())}; inputs.rightPositionRad = rightSim.getAngularPositionRad(); - inputs.rightPosition = new Rotation2d(rightSim.getAngularPositionRad()); + inputs.rightPosition = (Rotation2d) new Rotation2d(rightSim.getAngularPositionRad()); inputs.rightVelocityRadPerSec = rightSim.getAngularVelocityRadPerSec(); inputs.rightAppliedVolts = rightAppliedVolts; inputs.rightCurrentAmps = new double[] {Math.abs(rightSim.getCurrentDrawAmps())}; diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index ab3a917d..191b54f2 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -95,7 +95,7 @@ public void setGoalPosition(Translation2d goalPosition) { */ @Override public void setDynamicObstacles( - List> obs, Translation2d currentRobotPos) { + List> obs, Translation2d currentRobotPos) { if (!Logger.hasReplaySource()) { io.adStar.setDynamicObstacles(obs, currentRobotPos); } @@ -129,7 +129,8 @@ public void fromLog(LogTable table) { List pathPoints = new ArrayList<>(); for (int i = 0; i < pointsLogged.length; i += 2) { - pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + pathPoints.add( + new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); } currentPathPoints = pathPoints; @@ -149,4 +150,4 @@ public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState go } } } -} \ No newline at end of file +} From f12549b3fdc4c1fd17f84550a26a6b3ed622bf0b Mon Sep 17 00:00:00 2001 From: KP <86213869+Kanishk-Pandey@users.noreply.github.com> Date: Tue, 13 Feb 2024 15:32:27 -0500 Subject: [PATCH 05/45] feeder pass to shooter STILL NEED STATUS CHECK --- src/main/java/frc/robot/RobotContainer.java | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 97c6f648..0c799b17 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -190,6 +190,16 @@ private void configureButtonBindings() { new RunCommand(() -> feeder.runVolts(6.0), feeder) .until(() -> !feeder.getSensorFeed())); + + // prepare the shooter for dumping into the amp + controller + .a().toggleOnTrue( Commands.sequence( + Commands.run(() -> shooter.setTargetShooterAngleRad(Rotation2d.fromDegrees(-22.5)), shooter).until(() -> false), /* finish when arm in position */ + Commands.run(() -> feeder.runVolts(6.0), feeder).withTimeout(2.0).until(() -> !feeder.getSensorFeed()), + Commands.runOnce(feeder::stop, feeder), + Commands.run(() -> shooter.setTargetShooterAngleRad(Rotation2d.fromDegrees(45.0))) + )); + // ---- INTAKE COMMANDS ---- controller .leftBumper() // not a() From b6f1682f35f62858267dbf1754edee4691e3a0ac Mon Sep 17 00:00:00 2001 From: KP <86213869+Kanishk-Pandey@users.noreply.github.com> Date: Tue, 13 Feb 2024 16:12:08 -0500 Subject: [PATCH 06/45] add feeder shooter handoff --- src/main/java/frc/robot/RobotContainer.java | 36 ++++++++++++++---- src/main/java/frc/robot/util/Mode.java | 7 ++++ src/main/java/frc/robot/util/ModeHelper.java | 40 ++++++++++++++++++++ 3 files changed, 76 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/util/Mode.java create mode 100644 src/main/java/frc/robot/util/ModeHelper.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0c799b17..224a55dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -44,6 +45,8 @@ import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterIOSparkMax; +import frc.robot.util.Mode; +import frc.robot.util.ModeHelper; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; @@ -60,6 +63,17 @@ public class RobotContainer { private final Feeder feeder; private final Intake intake; + private final ModeHelper modeHelper = new ModeHelper(this); + + //TODO: populate switch statements here + public Command getEnterCommand(Mode m) { + return new InstantCommand(); + } + + public Command getExitCommand(Mode m) { + return new InstantCommand(); + } + // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -190,16 +204,24 @@ private void configureButtonBindings() { new RunCommand(() -> feeder.runVolts(6.0), feeder) .until(() -> !feeder.getSensorFeed())); - // prepare the shooter for dumping into the amp controller - .a().toggleOnTrue( Commands.sequence( - Commands.run(() -> shooter.setTargetShooterAngleRad(Rotation2d.fromDegrees(-22.5)), shooter).until(() -> false), /* finish when arm in position */ - Commands.run(() -> feeder.runVolts(6.0), feeder).withTimeout(2.0).until(() -> !feeder.getSensorFeed()), - Commands.runOnce(feeder::stop, feeder), - Commands.run(() -> shooter.setTargetShooterAngleRad(Rotation2d.fromDegrees(45.0))) - )); + .a().onTrue(Commands.runOnce(() -> modeHelper.switchTo(Mode.AMP))); + /* + .toggleOnTrue( + Commands.sequence( + Commands.run( + () -> shooter.setTargetShooterAngleRad(Rotation2d.fromDegrees(-22.5)), + shooter) + .until(() -> false), + Commands.run(() -> feeder.runVolts(6.0), feeder) + .withTimeout(2.0) + .until(() -> !feeder.getSensorFeed()), + Commands.runOnce(feeder::stop, feeder), + Commands.run( + () -> shooter.setTargetShooterAngleRad(Rotation2d.fromDegrees(45.0))))); + */ // ---- INTAKE COMMANDS ---- controller .leftBumper() // not a() diff --git a/src/main/java/frc/robot/util/Mode.java b/src/main/java/frc/robot/util/Mode.java new file mode 100644 index 00000000..1b5f2b68 --- /dev/null +++ b/src/main/java/frc/robot/util/Mode.java @@ -0,0 +1,7 @@ +package frc.robot.util; + +public enum Mode { + NEUTRAL, + AMP, + SPEAKER +} diff --git a/src/main/java/frc/robot/util/ModeHelper.java b/src/main/java/frc/robot/util/ModeHelper.java new file mode 100644 index 00000000..9aa058be --- /dev/null +++ b/src/main/java/frc/robot/util/ModeHelper.java @@ -0,0 +1,40 @@ +package frc.robot.util; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.RobotContainer; + +public class ModeHelper { + + private final RobotContainer container; + public ModeHelper(RobotContainer container) { + this.container = container; + } + + private Mode mode = Mode.NEUTRAL; + + private Command transition = null; + + public void switchTo(Mode target) { + + if(mode != Mode.NEUTRAL && target == mode) { + switchTo(Mode.NEUTRAL); + return; + } + + Command exit = container.getExitCommand(mode); + Command enter = container.getEnterCommand(target); + + transition = Commands.sequence( + exit.withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming), /* command group to exit mode */ + enter + ); + transition.schedule(); + } + + public void cancelTransition() { + if(transition != null) + transition.cancel(); + } +} From 582ce31de58504d9605a770fbd518383e8bfcc92 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Tue, 13 Feb 2024 16:20:32 -0500 Subject: [PATCH 07/45] added new contorller stuff Signed-off-by: KP <86213869+Kanishk-Pandey@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 57 +++++++++++++-------- 1 file changed, 36 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 97c6f648..d7eec7be 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -183,12 +183,12 @@ private void configureButtonBindings() { .ignoringDisable(true)); // ---- FEEDER COMMANDS ---- - controller - .leftTrigger() - .and(feeder::getSensorFeed) - .whileTrue( - new RunCommand(() -> feeder.runVolts(6.0), feeder) - .until(() -> !feeder.getSensorFeed())); + // controller + // .leftTrigger() + // .and(feeder::getSensorFeed) + // .whileTrue( + // new RunCommand(() -> feeder.runVolts(6.0), feeder) + // .until(() -> !feeder.getSensorFeed())); // ---- INTAKE COMMANDS ---- controller @@ -211,17 +211,30 @@ private void configureButtonBindings() { intake)); // ---- SHOOTER COMMANDS ---- + // controller + // .a() + // .whileTrue( + // Commands.startEnd( + // () -> shooter.runVelocity(flywheelSpeedInput.get()), shooter::stop, + // shooter)); controller - .a() + .rightTrigger() + .and(controller.leftTrigger().negate()) .whileTrue( - Commands.startEnd( - () -> shooter.runVelocity(flywheelSpeedInput.get()), shooter::stop, shooter)); - controller.rightTrigger().onTrue(new RunCommand(() -> shooter.runVolts(6.0), shooter)); + new RunCommand( + () -> shooter.runVolts(controller.getRightTriggerAxis() * 12.0), shooter)); controller - .a() + .leftTrigger() + .and(controller.rightTrigger().negate()) .whileTrue( - Commands.startEnd( - () -> shooter.runVelocity(flywheelSpeedInput.get()), shooter::stop, shooter)); + new RunCommand( + () -> shooter.runVolts(controller.getLeftTriggerAxis() * 12.0), shooter)); + // controller + // .a() + // .whileTrue( + // Commands.startEnd( + // () -> shooter.runVelocity(flywheelSpeedInput.get()), shooter::stop, + // shooter)); break; case DriveMotors: @@ -255,14 +268,16 @@ private void configureButtonBindings() { .b() .whileTrue(drivetrainDriveSysID.quasistatic(Direction.kReverse).withTimeout(2.0)) .onFalse(Commands.runOnce(drive::stopWithX, drive)); - controller - .rightTrigger() - .whileTrue( - new RunCommand(() -> shooter.setTargetShooterAngleRad(new Rotation2d(-0.61))) - .andThen( - (new RunCommand( - () -> shooter.runVelocity(5000) /*THIS NUMBER NEEDS TO BE CALIBRATED*/, - intake)))); + // controller + // .rightTrigger() + // .whileTrue( + // new RunCommand(() -> shooter.setTargetShooterAngleRad(new + // Rotation2d(-0.61))) + // .andThen( + // (new RunCommand( + // () -> shooter.runVelocity(5000) /*THIS NUMBER NEEDS TO BE + // CALIBRATED*/, + // intake)))); break; case EverythingElse: break; From 0d6654d7b75a995be08c5453a920aabba86b9b1e Mon Sep 17 00:00:00 2001 From: KP <86213869+Kanishk-Pandey@users.noreply.github.com> Date: Wed, 14 Feb 2024 15:50:46 -0500 Subject: [PATCH 08/45] fixes for shooter post testing Signed-off-by: KP <86213869+Kanishk-Pandey@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 14 +++++--------- .../java/frc/robot/subsystems/shooter/Shooter.java | 6 +++--- .../robot/subsystems/shooter/ShooterIOTalonFX.java | 6 +++--- 3 files changed, 11 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d7eec7be..0d68a253 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,11 +39,7 @@ import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; -import frc.robot.subsystems.intake.IntakeIOSparkMax; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterIO; -import frc.robot.subsystems.shooter.ShooterIOSim; -import frc.robot.subsystems.shooter.ShooterIOSparkMax; +import frc.robot.subsystems.shooter.*; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; @@ -80,9 +76,9 @@ public RobotContainer() { new ModuleIOSparkMax(1), new ModuleIOSparkMax(2), new ModuleIOSparkMax(3)); - shooter = new Shooter(new ShooterIOSparkMax()); + shooter = new Shooter(new ShooterIOTalonFX()); feeder = new Feeder(new FeederIO() {}); - intake = new Intake(new IntakeIOSparkMax()); + intake = new Intake(new IntakeIOSim()); // drive = new Drive( // new GyroIOPigeon2(), // new ModuleIOTalonFX(0), @@ -165,6 +161,7 @@ private void configureButtonBindings() { }, intake)); feeder.setDefaultCommand(new RunCommand(feeder::stop, feeder)); + shooter.setDefaultCommand(new RunCommand(shooter::stop, shooter)); // ---- DRIVETRAIN COMMANDS ---- controller.x().whileTrue(Commands.runOnce(drive::stopWithX, drive)); @@ -219,9 +216,8 @@ private void configureButtonBindings() { // shooter)); controller .rightTrigger() - .and(controller.leftTrigger().negate()) .whileTrue( - new RunCommand( + Commands.run( () -> shooter.runVolts(controller.getRightTriggerAxis() * 12.0), shooter)); controller .leftTrigger() diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 266e10ba..edf9b7c3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -61,9 +61,9 @@ public Shooter(ShooterIO io) { public void periodic() { io.updateInputs(inputs); - io.setFlywheelVoltage( - pid.calculate(inputs.flywheelVelocityRadPerSec) - + ffModel.calculate(pid.getSetpoint().position, pid.getSetpoint().velocity)); + // io.setFlywheelVoltage( + // pid.calculate(inputs.flywheelVelocityRadPerSec) + // + ffModel.calculate(pid.getSetpoint().position, pid.getSetpoint().velocity)); targetShooterAngleRad = pid.getSetpoint().position * ENCODER_ANGLE_FIX; Logger.processInputs("Shooter", inputs); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 3754e1c3..79d9d2a8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -25,8 +25,8 @@ public class ShooterIOTalonFX implements ShooterIO { private static final double GEAR_RATIO = 1.5; - private final TalonFX leader = new TalonFX(0); - private final TalonFX follower = new TalonFX(1); + private final TalonFX leader = new TalonFX(40); + private final TalonFX follower = new TalonFX(41); private final StatusSignal leaderPosition = leader.getPosition(); private final StatusSignal leaderVelocity = leader.getVelocity(); @@ -36,7 +36,7 @@ public class ShooterIOTalonFX implements ShooterIO { public ShooterIOTalonFX() { var config = new TalonFXConfiguration(); - config.CurrentLimits.StatorCurrentLimit = 30.0; + config.CurrentLimits.StatorCurrentLimit = 50.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.MotorOutput.NeutralMode = NeutralModeValue.Coast; leader.getConfigurator().apply(config); From ba750435aa638fe18f15ee8d4359e90c344158d3 Mon Sep 17 00:00:00 2001 From: KP <86213869+Kanishk-Pandey@users.noreply.github.com> Date: Wed, 14 Feb 2024 16:18:17 -0500 Subject: [PATCH 09/45] feeder shooter handoff commit Signed-off-by: KP <86213869+Kanishk-Pandey@users.noreply.github.com> --- src/main/java/frc/robot/util/Mode.java | 6 +-- src/main/java/frc/robot/util/ModeHelper.java | 48 ++++++++++---------- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/util/Mode.java b/src/main/java/frc/robot/util/Mode.java index 1b5f2b68..e1e36de3 100644 --- a/src/main/java/frc/robot/util/Mode.java +++ b/src/main/java/frc/robot/util/Mode.java @@ -1,7 +1,7 @@ package frc.robot.util; public enum Mode { - NEUTRAL, - AMP, - SPEAKER + NEUTRAL, + AMP, + SPEAKER } diff --git a/src/main/java/frc/robot/util/ModeHelper.java b/src/main/java/frc/robot/util/ModeHelper.java index 9aa058be..7e60bb72 100644 --- a/src/main/java/frc/robot/util/ModeHelper.java +++ b/src/main/java/frc/robot/util/ModeHelper.java @@ -2,39 +2,39 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.RobotContainer; public class ModeHelper { - private final RobotContainer container; - public ModeHelper(RobotContainer container) { - this.container = container; - } - - private Mode mode = Mode.NEUTRAL; + private final RobotContainer container; - private Command transition = null; + public ModeHelper(RobotContainer container) { + this.container = container; + } - public void switchTo(Mode target) { + private Mode mode = Mode.NEUTRAL; - if(mode != Mode.NEUTRAL && target == mode) { - switchTo(Mode.NEUTRAL); - return; - } + private Command transition = null; - Command exit = container.getExitCommand(mode); - Command enter = container.getEnterCommand(target); + public void switchTo(Mode target) { - transition = Commands.sequence( - exit.withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming), /* command group to exit mode */ - enter - ); - transition.schedule(); + if (mode != Mode.NEUTRAL && target == mode) { + switchTo(Mode.NEUTRAL); + return; } - public void cancelTransition() { - if(transition != null) - transition.cancel(); - } + Command exit = container.getExitCommand(mode); + Command enter = container.getEnterCommand(target); + + transition = + Commands.sequence( + exit.withInterruptBehavior( + Command.InterruptionBehavior.kCancelIncoming), /* command group to exit mode */ + enter); + transition.schedule(); + } + + public void cancelTransition() { + if (transition != null) transition.cancel(); + } } From be081ba32b1ee8caf4efb6a2d57e137a58d2437a Mon Sep 17 00:00:00 2001 From: KP <86213869+Kanishk-Pandey@users.noreply.github.com> Date: Wed, 14 Feb 2024 16:27:21 -0500 Subject: [PATCH 10/45] adding new neo motor into our code Signed-off-by: KP <86213869+Kanishk-Pandey@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 18 ++++------- .../frc/robot/subsystems/shooter/Shooter.java | 17 +++++----- .../robot/subsystems/shooter/ShooterIO.java | 1 - .../subsystems/shooter/ShooterIOSim.java | 1 - .../subsystems/shooter/ShooterIOSparkMax.java | 1 - .../subsystems/shooter/ShooterIOTalonFX.java | 2 -- .../frc/robot/subsystems/shooter/WristIO.java | 32 +++++++++++++++++++ 7 files changed, 48 insertions(+), 24 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/WristIO.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 224a55dc..675e4390 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -41,10 +41,7 @@ import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.intake.IntakeIOSparkMax; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterIO; -import frc.robot.subsystems.shooter.ShooterIOSim; -import frc.robot.subsystems.shooter.ShooterIOSparkMax; +import frc.robot.subsystems.shooter.*; import frc.robot.util.Mode; import frc.robot.util.ModeHelper; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -65,7 +62,7 @@ public class RobotContainer { private final ModeHelper modeHelper = new ModeHelper(this); - //TODO: populate switch statements here + // TODO: populate switch statements here public Command getEnterCommand(Mode m) { return new InstantCommand(); } @@ -94,7 +91,7 @@ public RobotContainer() { new ModuleIOSparkMax(1), new ModuleIOSparkMax(2), new ModuleIOSparkMax(3)); - shooter = new Shooter(new ShooterIOSparkMax()); + shooter = new Shooter(new ShooterIOTalonFX(), new WristIO() {}); feeder = new Feeder(new FeederIO() {}); intake = new Intake(new IntakeIOSparkMax()); // drive = new Drive( @@ -115,7 +112,7 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); - shooter = new Shooter(new ShooterIOSim()); + shooter = new Shooter(new ShooterIOSim(), new WristIO() {}); feeder = new Feeder(new FeederIOSim()); intake = new Intake(new IntakeIOSim()); break; @@ -129,7 +126,7 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - shooter = new Shooter(new ShooterIO() {}); + shooter = new Shooter(new ShooterIO() {}, new WristIO() {}); feeder = new Feeder(new FeederIO() {}); intake = new Intake(new IntakeIO() {}); break; @@ -205,10 +202,9 @@ private void configureButtonBindings() { .until(() -> !feeder.getSensorFeed())); // prepare the shooter for dumping into the amp - controller - .a().onTrue(Commands.runOnce(() -> modeHelper.switchTo(Mode.AMP))); + controller.a().onTrue(Commands.runOnce(() -> modeHelper.switchTo(Mode.AMP))); - /* + /* .toggleOnTrue( Commands.sequence( Commands.run( diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 266e10ba..441bdba4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -24,7 +24,8 @@ import org.littletonrobotics.junction.Logger; public class Shooter extends SubsystemBase { - private final ShooterIO io; + private final ShooterIO shooterio; + private final WristIO wristio; private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); private ProfiledPIDController pid; private final SimpleMotorFeedforward ffModel; @@ -34,8 +35,9 @@ public class Shooter extends SubsystemBase { private static double targetShooterAngleRad = 0.0; /** Creates a new Shooter. */ - public Shooter(ShooterIO io) { - this.io = io; + public Shooter(ShooterIO io, WristIO wristIO) { + this.shooterio = io; + this.wristio = wristIO; // Switch constants based on mode (the physics simulator is treated as a // separate robot with different tuning) switch (Constants.currentMode) { @@ -59,9 +61,8 @@ public Shooter(ShooterIO io) { @Override public void periodic() { - io.updateInputs(inputs); - - io.setFlywheelVoltage( + shooterio.updateInputs(inputs); + shooterio.setFlywheelVoltage( pid.calculate(inputs.flywheelVelocityRadPerSec) + ffModel.calculate(pid.getSetpoint().position, pid.getSetpoint().velocity)); targetShooterAngleRad = pid.getSetpoint().position * ENCODER_ANGLE_FIX; @@ -70,7 +71,7 @@ public void periodic() { /** Run open loop at the specified voltage. */ public void runVolts(double volts) { - io.setFlywheelVoltage(volts); + shooterio.setFlywheelVoltage(volts); } /** Run closed loop at the specified velocity. */ @@ -85,7 +86,7 @@ public void runVelocity(double velocityRPM) { /** Stops the flywheel. */ public void stop() { - io.flywheelStop(); + shooterio.flywheelStop(); } /** Returns the current velocity in RPM. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 2d768fa2..af3e5dd4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -18,7 +18,6 @@ public interface ShooterIO { @AutoLog public static class ShooterIOInputs { - public double flywheelPositionRad = 0.0; public double flywheelVelocityRadPerSec = 0.0; public double flywheelAppliedVolts = 0.0; public double[] flywheelCurrentAmps = new double[] {}; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index d6a00554..14540f7b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -43,7 +43,6 @@ public class ShooterIOSim implements ShooterIO { public void updateInputs(ShooterIOInputs inputs) { sim.update(0.02); - inputs.flywheelPositionRad = 0.0; inputs.flywheelVelocityRadPerSec = sim.getAngularVelocityRadPerSec(); inputs.flywheelAppliedVolts = appliedVolts; inputs.flywheelCurrentAmps = new double[] {sim.getCurrentDrawAmps()}; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index fe9fc337..1bddaed8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -50,7 +50,6 @@ public ShooterIOSparkMax() { @Override public void updateInputs(ShooterIOInputs inputs) { - inputs.flywheelPositionRad = Units.rotationsToRadians(encoder.getPosition() / GEAR_RATIO); inputs.flywheelVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); inputs.flywheelAppliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 3754e1c3..cc508659 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -53,8 +53,6 @@ public ShooterIOTalonFX() { public void updateInputs(ShooterIOInputs inputs) { BaseStatusSignal.refreshAll( leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); - inputs.flywheelPositionRad = - Units.rotationsToRadians(leaderPosition.getValueAsDouble()) / GEAR_RATIO; inputs.flywheelVelocityRadPerSec = Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / GEAR_RATIO; inputs.flywheelAppliedVolts = leaderAppliedVolts.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/shooter/WristIO.java b/src/main/java/frc/robot/subsystems/shooter/WristIO.java new file mode 100644 index 00000000..73252578 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/WristIO.java @@ -0,0 +1,32 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLog; + +public interface WristIO { + @AutoLog + public static class WristIOInputs { + public double wristPositionRad = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(WristIOInputs inputs) {} + + /** Run open loop at the specified voltage. */ + public default void setFlywheelVoltage(double volts) {} + + /** Stop in open loop. */ + public default void flywheelStop() {} +} From 818159f319db9cb7f08a1113ca18b32cc81aac48 Mon Sep 17 00:00:00 2001 From: KP <86213869+Kanishk-Pandey@users.noreply.github.com> Date: Wed, 14 Feb 2024 17:01:30 -0500 Subject: [PATCH 11/45] wrist is actually here Signed-off-by: KP <86213869+Kanishk-Pandey@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 20 ++++++-- .../frc/robot/subsystems/shooter/Shooter.java | 51 ++++++++++++------- .../frc/robot/subsystems/shooter/WristIO.java | 4 +- 3 files changed, 51 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 675e4390..3bac1578 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -136,7 +136,9 @@ public RobotContainer() { NamedCommands.registerCommand( "Run Flywheel", Commands.startEnd( - () -> shooter.runVelocity(flywheelSpeedInput.get()), shooter::stop, shooter) + () -> shooter.shooterrunVelocity(flywheelSpeedInput.get()), + shooter::stopshooter, + shooter) .withTimeout(5.0)); autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -243,13 +245,19 @@ private void configureButtonBindings() { .a() .whileTrue( Commands.startEnd( - () -> shooter.runVelocity(flywheelSpeedInput.get()), shooter::stop, shooter)); - controller.rightTrigger().onTrue(new RunCommand(() -> shooter.runVolts(6.0), shooter)); + () -> shooter.shooterrunVelocity(flywheelSpeedInput.get()), + shooter::stopshooter, + shooter)); + controller + .rightTrigger() + .onTrue(new RunCommand(() -> shooter.shooterrunvolts(6.0), shooter)); controller .a() .whileTrue( Commands.startEnd( - () -> shooter.runVelocity(flywheelSpeedInput.get()), shooter::stop, shooter)); + () -> shooter.shooterrunVelocity(flywheelSpeedInput.get()), + shooter::stopshooter, + shooter)); break; case DriveMotors: @@ -289,7 +297,9 @@ private void configureButtonBindings() { new RunCommand(() -> shooter.setTargetShooterAngleRad(new Rotation2d(-0.61))) .andThen( (new RunCommand( - () -> shooter.runVelocity(5000) /*THIS NUMBER NEEDS TO BE CALIBRATED*/, + () -> + shooter.shooterrunVelocity( + 5000) /*THIS NUMBER NEEDS TO BE CALIBRATED*/, intake)))); break; case EverythingElse: diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 441bdba4..994bb92d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -20,14 +20,17 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import javax.swing.*; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Shooter extends SubsystemBase { private final ShooterIO shooterio; private final WristIO wristio; - private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - private ProfiledPIDController pid; + private final ShooterIOInputsAutoLogged shooterinputs = new ShooterIOInputsAutoLogged(); + private final WristIOInputsAutoLogged wristinputs = new WristIOInputsAutoLogged(); + private ProfiledPIDController shooterpid; + private ProfiledPIDController wristpid; private final SimpleMotorFeedforward ffModel; private static final double ENCODER_ANGLE_FIX = 1.5; // the ration for turning the shooter @@ -45,13 +48,16 @@ public Shooter(ShooterIO io, WristIO wristIO) { // FIXME: characterize real robot case REPLAY: ffModel = new SimpleMotorFeedforward(0.1, 0.05); - pid = + shooterpid = new ProfiledPIDController( 1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.5, 99)); // FIXME: remove profile? + wristpid = new ProfiledPIDController(4.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); break; case SIM: ffModel = new SimpleMotorFeedforward(0.0, 0.03); - pid = new ProfiledPIDController(0.5, 0.0, 0.0, new TrapezoidProfile.Constraints(0.5, 99)); + shooterpid = + new ProfiledPIDController(0.5, 0.0, 0.0, new TrapezoidProfile.Constraints(0.5, 99)); + wristpid = new ProfiledPIDController(4.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); break; default: ffModel = new SimpleMotorFeedforward(0.0, 0.0); @@ -61,43 +67,54 @@ public Shooter(ShooterIO io, WristIO wristIO) { @Override public void periodic() { - shooterio.updateInputs(inputs); + shooterio.updateInputs(shooterinputs); + wristio.updateInputs(wristinputs); shooterio.setFlywheelVoltage( - pid.calculate(inputs.flywheelVelocityRadPerSec) - + ffModel.calculate(pid.getSetpoint().position, pid.getSetpoint().velocity)); - targetShooterAngleRad = pid.getSetpoint().position * ENCODER_ANGLE_FIX; - Logger.processInputs("Shooter", inputs); + shooterpid.calculate(shooterinputs.flywheelVelocityRadPerSec) + + ffModel.calculate( + shooterpid.getSetpoint().position, shooterpid.getSetpoint().velocity)); + targetShooterAngleRad = shooterpid.getSetpoint().position * ENCODER_ANGLE_FIX; + Logger.processInputs("Shooter", shooterinputs); + Logger.processInputs("Wrist", wristinputs); } /** Run open loop at the specified voltage. */ - public void runVolts(double volts) { + public void shooterrunvolts(double volts) { shooterio.setFlywheelVoltage(volts); } + public void wristrunvolts(double volts){ + wristio.setVoltage(volts); + } + /** Run closed loop at the specified velocity. */ - public void runVelocity(double velocityRPM) { + public void shooterrunVelocity(double velocityRPM) { var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - pid.setGoal(velocityRadPerSec); + shooterpid.setGoal(velocityRadPerSec); // Log flywheel setpoint Logger.recordOutput("Shooter/SetpointRPM", velocityRPM); } /** Stops the flywheel. */ - public void stop() { + public void stopshooter() { shooterio.flywheelStop(); } + public void stopwrist(){ + wristio.wristStop(); + } + /** Returns the current velocity in RPM. */ @AutoLogOutput - public double getVelocityRPM() { - return Units.radiansPerSecondToRotationsPerMinute(inputs.flywheelVelocityRadPerSec); + public double getShooterVelocityRPM() { + return Units.radiansPerSecondToRotationsPerMinute(shooterinputs.flywheelVelocityRadPerSec); } /** Returns the current velocity in radians per second. */ - public double getCharacterizationVelocity() { - return inputs.flywheelVelocityRadPerSec; + public double getShooterCharacterizationVelocity() { + return shooterinputs.flywheelVelocityRadPerSec; } public void setTargetShooterAngleRad(Rotation2d anglediff) { diff --git a/src/main/java/frc/robot/subsystems/shooter/WristIO.java b/src/main/java/frc/robot/subsystems/shooter/WristIO.java index 73252578..c5d79e80 100644 --- a/src/main/java/frc/robot/subsystems/shooter/WristIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/WristIO.java @@ -25,8 +25,8 @@ public static class WristIOInputs { public default void updateInputs(WristIOInputs inputs) {} /** Run open loop at the specified voltage. */ - public default void setFlywheelVoltage(double volts) {} + public default void setVoltage(double volts) {} /** Stop in open loop. */ - public default void flywheelStop() {} + public default void wristStop() {} } From 7a4721ca326b53c4367d52b39b747d4e822a5b8b Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Wed, 14 Feb 2024 17:12:25 -0500 Subject: [PATCH 12/45] fixed intake and merged debug changes Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 24 +++++++------ .../frc/robot/commands/DriveCommands.java | 4 +++ .../frc/robot/subsystems/drive/Module.java | 35 +++++++++--------- .../subsystems/drive/ModuleIOSparkMax.java | 5 ++- .../frc/robot/subsystems/intake/Intake.java | 9 ++++- .../subsystems/intake/IntakeIOSparkMax.java | 36 ++++++++++++------- .../java/frc/robot/util/LocalADStarAK.java | 7 ++-- 7 files changed, 75 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e5d8ac2e..86578032 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,7 +43,6 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOSim; -import frc.robot.subsystems.shooter.ShooterIOSparkMax; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; @@ -80,7 +79,7 @@ public RobotContainer() { new ModuleIOSparkMax(1), new ModuleIOSparkMax(2), new ModuleIOSparkMax(3)); - shooter = new Shooter(new ShooterIOSparkMax()); + shooter = new Shooter(new ShooterIOSim()); feeder = new Feeder(new FeederIO() {}); intake = new Intake(new IntakeIOSparkMax()); // drive = new Drive( @@ -164,10 +163,10 @@ private void configureButtonBindings() { feeder.setDefaultCommand(new RunCommand(feeder::stop, feeder)); // ---- DRIVETRAIN COMMANDS ---- - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + controller.x().whileTrue(Commands.runOnce(drive::stopWithX, drive)); controller .b() - .onTrue( + .whileTrue( Commands.runOnce( () -> { try { @@ -183,17 +182,18 @@ private void configureButtonBindings() { controller .leftTrigger() .and(feeder::getSensorFeed) - .onTrue( + .whileTrue( new RunCommand(() -> feeder.runVolts(6.0), feeder) .until(() -> !feeder.getSensorFeed())); // ---- INTAKE COMMANDS ---- controller .leftBumper() // not a() - .onTrue(new RunCommand(() -> intake.setIntakePosition(new Rotation2d(115.0)), intake)); + .whileTrue( + new RunCommand(() -> intake.setIntakePosition(new Rotation2d(115.0)), intake)); controller .rightBumper() - .onTrue(new RunCommand(() -> intake.setRollerPercentage(0.75), intake)); + .whileTrue(new RunCommand(() -> intake.setRollerPercentage(0.75), intake)); // ---- SHOOTER COMMANDS ---- controller @@ -241,9 +241,13 @@ private void configureButtonBindings() { .whileTrue(drivetrainDriveSysID.quasistatic(Direction.kReverse).withTimeout(2.0)) .onFalse(Commands.runOnce(drive::stopWithX, drive)); controller - .rightTrigger() - .whileTrue(new RunCommand(() -> shooter.setTargetShooterAngleRad(new Rotation2d(-0.61))) - .andThen((new RunCommand(() -> shooter.runVelocity(5000)/*THIS NUMBER NEEDS TO BE CALIBRATED*/, intake)))); + .rightTrigger() + .whileTrue( + new RunCommand(() -> shooter.setTargetShooterAngleRad(new Rotation2d(-0.61))) + .andThen( + (new RunCommand( + () -> shooter.runVelocity(5000) /*THIS NUMBER NEEDS TO BE CALIBRATED*/, + intake)))); break; case EverythingElse: break; diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index e8d4faa5..6e78b593 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -13,7 +13,10 @@ package frc.robot.commands; +import static edu.wpi.first.units.Units.*; + import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; @@ -25,6 +28,7 @@ import java.util.function.DoubleSupplier; public class DriveCommands { + private static final double DEADBAND = 0.1; private DriveCommands() {} diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index d29ef220..0e0211c6 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -49,24 +49,25 @@ public Module(ModuleIO io, int index) { switch (Constants.currentMode) { case REAL: switch (index) { - case 0: - driveFeedforward = new SimpleMotorFeedforward(0.01626, 0.82954, 0.14095); - driveFeedback = new PIDController(0.21268, 0.0, 0.0); - break; - case 1: - driveFeedforward = new SimpleMotorFeedforward(0.081671, 0.82741, 0.081036); - driveFeedback = new PIDController(0.75099, 0.0, 0.0); - break; - case 2: - driveFeedforward = new SimpleMotorFeedforward(0.082023, 0.81434, 0.12098); - driveFeedback = new PIDController(0.096474, 0.0, 0.0); - break; - case 3: - driveFeedforward = new SimpleMotorFeedforward(0.040377, 0.84332, 0.13969); - driveFeedback = new PIDController(0.015087, 0.0, 0.0); - break; + // case 0: + // driveFeedforward = new SimpleMotorFeedforward(0.01626, 0.82954, 0.14095); + // driveFeedback = new PIDController(0.21268, 0.0, 0.0); + // break; + // case 1: + // driveFeedforward = new SimpleMotorFeedforward(0.081671, 0.82741, + // 0.081036); + // driveFeedback = new PIDController(.09, 0.0, 0.0); + // break; + // case 2: + // driveFeedforward = new SimpleMotorFeedforward(0.082023, 0.81434, 0.12098); + // driveFeedback = new PIDController(0.096474, 0.0, 0.0); + // break; + // case 3: + // driveFeedforward = new SimpleMotorFeedforward(0.040377, 0.84332, 0.13969); + // driveFeedback = new PIDController(0.015087, 0.0, 0.0); + // break; default: - driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + driveFeedforward = new SimpleMotorFeedforward(0.1, .83); driveFeedback = new PIDController(0.05, 0.0, 0.0); break; } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index 58d76d9f..9059b9f0 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -13,6 +13,8 @@ package frc.robot.subsystems.drive; +import static java.lang.Double.NaN; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -126,8 +128,9 @@ public ModuleIOSparkMax(int index) { @Override public void updateInputs(ModuleIOInputs inputs) { BaseStatusSignal.refreshAll(turnAbsolutePosition); + double position = driveEncoder.getPosition(); inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO; + Units.rotationsToRadians((position == NaN) ? 0.0 : position) / DRIVE_GEAR_RATIO; inputs.driveVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO; inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 2d072a92..b15fb1d8 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -30,7 +30,14 @@ public Intake(IntakeIO io) { this.io = io; switch (Constants.currentMode) { case REAL: - // FIXME: add arm FF and FB + armFF = new ArmFeedforward(0.0, 0.21, 0.195, 0.0); + armFB = + new ProfiledPIDController( + 1.0, + 0.0, + 0.0, + new Constraints(RotationsPerSecond.of(3), RotationsPerSecond.per(Second).of(9))); + break; case REPLAY: armFF = new ArmFeedforward(0.1, .15, 1.95); armFB = diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index e207a1a4..c10df7c3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -16,6 +16,7 @@ import com.revrobotics.*; import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.math.util.Units; +import org.littletonrobotics.junction.Logger; /** * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with @@ -25,23 +26,27 @@ public class IntakeIOSparkMax implements IntakeIO { private static final double ARM_GEAR_RATIO = 100.0; private static final double ROLLER_GEAR_RATIO = 3.0; - private final CANSparkMax arm = new CANSparkMax(0, MotorType.kBrushless); - private final CANSparkMax roller = new CANSparkMax(1, MotorType.kBrushless); - private final AbsoluteEncoder encoder = arm.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); - private final SparkPIDController pid = arm.getPIDController(); + private final CANSparkMax arm = new CANSparkMax(9, MotorType.kBrushless); + private final CANSparkMax roller = new CANSparkMax(31, MotorType.kBrushless); + private final AbsoluteEncoder encoder = + arm.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); public IntakeIOSparkMax() { arm.restoreFactoryDefaults(); roller.restoreFactoryDefaults(); + arm.setIdleMode(CANSparkBase.IdleMode.kBrake); arm.setCANTimeout(250); roller.setCANTimeout(250); arm.setInverted(false); - roller.follow(arm, false); + // roller.follow(arm, false); - arm.enableVoltageCompensation(12.0); - roller.setSmartCurrentLimit(30); + // arm.enableVoltageCompensation(12.0); + // roller.setSmartCurrentLimit(30); + + encoder.setZeroOffset(/*0.0 */ .878); + encoder.setInverted(true); arm.burnFlash(); roller.burnFlash(); @@ -49,19 +54,24 @@ public IntakeIOSparkMax() { @Override public void updateInputs(IntakeIOInputs inputs) { - inputs.armPositionRad = Units.rotationsToRadians(encoder.getPosition() / ARM_GEAR_RATIO); - inputs.armVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / ARM_GEAR_RATIO); + inputs.armPositionRad = Units.rotationsToRadians(encoder.getPosition()); + inputs.armVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity()); inputs.armAppliedVolts = arm.getAppliedOutput() * arm.getBusVoltage(); - inputs.armCurrentAmps = new double[] {arm.getOutputCurrent(), roller.getOutputCurrent()}; + inputs.armCurrentAmps = new double[] {arm.getOutputCurrent()}; inputs.armTemperature = new double[] {arm.getMotorTemperature()}; inputs.rollerTemperature = new double[] {roller.getMotorTemperature()}; } - - public void setVoltage(double volts) { + @Override + public void setArmVoltage(double volts) { + Logger.recordOutput("ArmSetVoltage", volts); arm.setVoltage(volts); } + + @Override + public void setRollerPercent(double percent) { + roller.setVoltage(percent * roller.getBusVoltage()); + } // // @Override // public void setArmVelocity(double velocityRadPerSec, double ffVolts) { diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index ab3a917d..191b54f2 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -95,7 +95,7 @@ public void setGoalPosition(Translation2d goalPosition) { */ @Override public void setDynamicObstacles( - List> obs, Translation2d currentRobotPos) { + List> obs, Translation2d currentRobotPos) { if (!Logger.hasReplaySource()) { io.adStar.setDynamicObstacles(obs, currentRobotPos); } @@ -129,7 +129,8 @@ public void fromLog(LogTable table) { List pathPoints = new ArrayList<>(); for (int i = 0; i < pointsLogged.length; i += 2) { - pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + pathPoints.add( + new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); } currentPathPoints = pathPoints; @@ -149,4 +150,4 @@ public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState go } } } -} \ No newline at end of file +} From f1397b98e7e6eda8e460aaf07883c7b914061350 Mon Sep 17 00:00:00 2001 From: KP <86213869+Kanishk-Pandey@users.noreply.github.com> Date: Wed, 14 Feb 2024 17:40:12 -0500 Subject: [PATCH 13/45] wrist is actually here but even more, not done yet tho Signed-off-by: KP <86213869+Kanishk-Pandey@users.noreply.github.com> --- .../frc/robot/subsystems/shooter/Shooter.java | 4 +- .../subsystems/shooter/ShooterIOSparkMax.java | 1 + .../subsystems/shooter/WristIOSparkMax.java | 41 +++++++++++++++++++ 3 files changed, 44 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/WristIOSparkMax.java diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 994bb92d..fa0fb2c7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -83,7 +83,7 @@ public void shooterrunvolts(double volts) { shooterio.setFlywheelVoltage(volts); } - public void wristrunvolts(double volts){ + public void wristrunvolts(double volts) { wristio.setVoltage(volts); } @@ -102,7 +102,7 @@ public void stopshooter() { shooterio.flywheelStop(); } - public void stopwrist(){ + public void stopwrist() { wristio.wristStop(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 1bddaed8..0201838c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -23,6 +23,7 @@ * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with * "CANSparkFlex". */ +@Deprecated public class ShooterIOSparkMax implements ShooterIO { private static final double GEAR_RATIO = 1.5; diff --git a/src/main/java/frc/robot/subsystems/shooter/WristIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/WristIOSparkMax.java new file mode 100644 index 00000000..190aadba --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/WristIOSparkMax.java @@ -0,0 +1,41 @@ +package frc.robot.subsystems.shooter; + +import com.revrobotics.*; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; + +public class WristIOSparkMax implements WristIO { + private static final double GEAR_RATIO = 1.5; + + private final CANSparkMax leader = new CANSparkMax(0, CANSparkLowLevel.MotorType.kBrushless); + + private final SparkAbsoluteEncoder encoder = + leader.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); + + public WristIOSparkMax() { + leader.restoreFactoryDefaults(); + leader.setCANTimeout(250); + leader.setInverted(false); + leader.enableVoltageCompensation(12.0); + leader.setSmartCurrentLimit(30); + leader.burnFlash(); + } + + private Rotation2d getWristAngle() { + return Rotation2d.fromRadians(MathUtil.angleModulus(encoder.getPosition() * 6.28) / 1.5); + } + + public void updateInputs(WristIOInputs inputs) { + inputs.wristPositionRad = encoder.getPosition(); + } + + @Override + public void setVoltage(double volts) { + leader.setVoltage(volts); + } + + @Override + public void wristStop() { + leader.stopMotor(); + } +} From 9548d2f1366803c7a7f1eabe5caa1f4e9881a5f6 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Thu, 15 Feb 2024 11:14:50 -0500 Subject: [PATCH 14/45] feeder stuff Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- .../frc/robot/subsystems/feeder/FeederIO.java | 1 + .../robot/subsystems/feeder/FeederIOSim.java | 7 +-- .../subsystems/feeder/FeederIOSparkMax.java | 44 +++++++++++++++++++ 3 files changed, 49 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/feeder/FeederIOSparkMax.java diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java index 1e66fe4e..7f73f34f 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java @@ -19,5 +19,6 @@ public static class FeederIOInputs { public double velocityRadPerSec = 0.0; public double appliedVolts = 0.0; public double[] currentAmps = new double[] {}; + public double[] temperature = new double[] {}; } } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java index d66849b0..39913c75 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java @@ -17,7 +17,7 @@ import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; public class FeederIOSim implements FeederIO { - private SingleJointedArmSim sim = + private final SingleJointedArmSim sim = new SingleJointedArmSim( DCMotor.getNEO(1), 1.5, 0.025, 0.1, -Math.PI / 4, Math.PI / 2, true, 0); // private ProfiledPIDController pid = new ProfiledPIDController(0.0, 0.0, 0.0); @@ -31,8 +31,8 @@ public void updateInputs(FeederIOInputs inputs) { if (closedLoop) { // appliedVolts = MathUtil.clamp(pid.calculate(sim.getAngleRads()) + ffVolts, -12.0, // 12.0); - sim.setInputVoltage(appliedVolts); } + sim.setInputVoltage(appliedVolts); sim.update(0.02); @@ -45,12 +45,13 @@ public void updateInputs(FeederIOInputs inputs) { @Override public void setVoltage(double volts) { closedLoop = false; - appliedVolts = 0.0; + appliedVolts = volts; sim.setInputVoltage(volts); } @Override public void stop() { + sim.setState(sim.getAngleRads(), 0.0); setVoltage(0.0); } } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkMax.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkMax.java new file mode 100644 index 00000000..d2472b84 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkMax.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems.feeder; + +import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; + +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.util.Units; + +public class FeederIOSparkMax implements FeederIO { + private static final double GEAR_RATIO = 1.0; + CANSparkMax feeder = new CANSparkMax(0, kBrushless); + + public FeederIOSparkMax() { + feeder.restoreFactoryDefaults(); + + feeder.setCANTimeout(250); + + feeder.setInverted(false); + + feeder.enableVoltageCompensation(12.0); + feeder.setSmartCurrentLimit(30); + + feeder.burnFlash(); + } + + @Override + public void updateInputs(FeederIOInputs inputs) { + inputs.positionRad = Units.rotationsToRadians(feeder.getEncoder().getPosition() / GEAR_RATIO); + inputs.velocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(feeder.getEncoder().getVelocity() / GEAR_RATIO); + inputs.appliedVolts = feeder.getAppliedOutput() * feeder.getBusVoltage(); + inputs.currentAmps = new double[] {feeder.getOutputCurrent()}; + inputs.temperature = new double[] {feeder.getMotorTemperature()}; + } + + @Override + public void setVoltage(double volts) { + feeder.setVoltage(volts); + } + + @Override + public void stop() { + feeder.setVoltage(0.0); + } +} From 0b309e179b88c3011d198a00576d28437b502ec9 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Thu, 15 Feb 2024 11:28:32 -0500 Subject: [PATCH 15/45] undepricated spark max shooter and added spark flex support. Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- .../shooter/ShooterIOSparkFlex.java | 65 +++++++++++++++++++ .../subsystems/shooter/ShooterIOSparkMax.java | 7 -- 2 files changed, 65 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkFlex.java diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkFlex.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkFlex.java new file mode 100644 index 00000000..6889b369 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkFlex.java @@ -0,0 +1,65 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.shooter; + +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.util.Units; + +public class ShooterIOSparkFlex implements ShooterIO { + private static final double GEAR_RATIO = 1.5; + + private final CANSparkFlex leader = new CANSparkFlex(0, MotorType.kBrushless); + private final CANSparkFlex follower = new CANSparkFlex(1, MotorType.kBrushless); + private final RelativeEncoder encoder = leader.getEncoder(); + + public ShooterIOSparkFlex() { + leader.restoreFactoryDefaults(); + follower.restoreFactoryDefaults(); + + leader.setCANTimeout(250); + follower.setCANTimeout(250); + + leader.setInverted(false); + follower.follow(leader, false); + + leader.enableVoltageCompensation(12.0); + leader.setSmartCurrentLimit(30); + + leader.burnFlash(); + follower.burnFlash(); + } + + @Override + public void updateInputs(ShooterIOInputs inputs) { + inputs.flywheelVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); + inputs.flywheelAppliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); + inputs.flywheelCurrentAmps = + new double[] {leader.getOutputCurrent(), follower.getOutputCurrent()}; + inputs.flywheelTemperature = + new double[] {leader.getMotorTemperature(), follower.getMotorTemperature()}; + } + + @Override + public void setFlywheelVoltage(double volts) { + leader.setVoltage(volts); + } + + @Override + public void flywheelStop() { + leader.stopMotor(); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 0201838c..6077c92a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -16,21 +16,14 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; import edu.wpi.first.math.util.Units; -/** - * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with - * "CANSparkFlex". - */ -@Deprecated public class ShooterIOSparkMax implements ShooterIO { private static final double GEAR_RATIO = 1.5; private final CANSparkMax leader = new CANSparkMax(0, MotorType.kBrushless); private final CANSparkMax follower = new CANSparkMax(1, MotorType.kBrushless); private final RelativeEncoder encoder = leader.getEncoder(); - private final SparkPIDController pid = leader.getPIDController(); public ShooterIOSparkMax() { leader.restoreFactoryDefaults(); From f47a19842611ddbe2cf266b03ebb21d8436da450 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Thu, 15 Feb 2024 15:23:10 -0500 Subject: [PATCH 16/45] stuff Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 45 +++++-- .../shooter/{WristIO.java => HoodIO.java} | 17 ++- ...istIOSparkMax.java => HoodIOSparkMax.java} | 10 +- .../frc/robot/subsystems/shooter/Shooter.java | 126 +++++++++++------- 4 files changed, 128 insertions(+), 70 deletions(-) rename src/main/java/frc/robot/subsystems/shooter/{WristIO.java => HoodIO.java} (71%) rename src/main/java/frc/robot/subsystems/shooter/{WristIOSparkMax.java => HoodIOSparkMax.java} (79%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3bac1578..7ddc9a6a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,9 +13,6 @@ package frc.robot; -import static edu.wpi.first.units.BaseUnits.Voltage; -import static edu.wpi.first.units.Units.Seconds; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; @@ -47,6 +44,9 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; +import static edu.wpi.first.units.BaseUnits.Voltage; +import static edu.wpi.first.units.Units.Seconds; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -91,7 +91,8 @@ public RobotContainer() { new ModuleIOSparkMax(1), new ModuleIOSparkMax(2), new ModuleIOSparkMax(3)); - shooter = new Shooter(new ShooterIOTalonFX(), new WristIO() {}); + shooter = new Shooter(new ShooterIOTalonFX(), new HoodIO() { + }); feeder = new Feeder(new FeederIO() {}); intake = new Intake(new IntakeIOSparkMax()); // drive = new Drive( @@ -112,7 +113,8 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); - shooter = new Shooter(new ShooterIOSim(), new WristIO() {}); + shooter = new Shooter(new ShooterIOSim(), new HoodIO() { + }); feeder = new Feeder(new FeederIOSim()); intake = new Intake(new IntakeIOSim()); break; @@ -126,7 +128,9 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - shooter = new Shooter(new ShooterIO() {}, new WristIO() {}); + shooter = new Shooter(new ShooterIO() { + }, new HoodIO() { + }); feeder = new Feeder(new FeederIO() {}); intake = new Intake(new IntakeIO() {}); break; @@ -136,8 +140,8 @@ public RobotContainer() { NamedCommands.registerCommand( "Run Flywheel", Commands.startEnd( - () -> shooter.shooterrunVelocity(flywheelSpeedInput.get()), - shooter::stopshooter, + () -> shooter.shooterRunVelocity(flywheelSpeedInput.get()), + shooter::stopShooter, shooter) .withTimeout(5.0)); autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -245,18 +249,18 @@ private void configureButtonBindings() { .a() .whileTrue( Commands.startEnd( - () -> shooter.shooterrunVelocity(flywheelSpeedInput.get()), - shooter::stopshooter, + () -> shooter.shooterRunVelocity(flywheelSpeedInput.get()), + shooter::stopShooter, shooter)); controller .rightTrigger() - .onTrue(new RunCommand(() -> shooter.shooterrunvolts(6.0), shooter)); + .onTrue(new RunCommand(() -> shooter.shooterRunVolts(6.0), shooter)); controller .a() .whileTrue( Commands.startEnd( - () -> shooter.shooterrunVelocity(flywheelSpeedInput.get()), - shooter::stopshooter, + () -> shooter.shooterRunVelocity(flywheelSpeedInput.get()), + shooter::stopShooter, shooter)); break; @@ -298,11 +302,24 @@ private void configureButtonBindings() { .andThen( (new RunCommand( () -> - shooter.shooterrunVelocity( + shooter.shooterRunVelocity( 5000) /*THIS NUMBER NEEDS TO BE CALIBRATED*/, intake)))); break; case EverythingElse: + var shooterSysId = + new SysIdRoutine( + new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)), + new Mechanism( + shooter::shooterRunVolts, + (log) -> { + var motor = log.motor("Shooter"); + motor.voltage(shooter.getCharacterizationAppliedVolts()); + motor.angularVelocity(shooter.getCharacterizationVelocity()); + motor.current(shooter.getCharacterizationCurrent()); + }, + shooter, + "FlywheelMotors")); break; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/WristIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java similarity index 71% rename from src/main/java/frc/robot/subsystems/shooter/WristIO.java rename to src/main/java/frc/robot/subsystems/shooter/HoodIO.java index c5d79e80..94b35f4e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/WristIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -15,14 +15,19 @@ import org.littletonrobotics.junction.AutoLog; -public interface WristIO { - @AutoLog - public static class WristIOInputs { - public double wristPositionRad = 0.0; +public interface HoodIO { + /** Updates the set of loggable inputs. */ + public default void updateInputs(HoodIOInputs inputs) { } - /** Updates the set of loggable inputs. */ - public default void updateInputs(WristIOInputs inputs) {} + @AutoLog + public static class HoodIOInputs { + public double armPositionRad = 0.0; + public double armVelocityRadPerSec = 0.0; + public double armAppliedVolts = 0.0; + public double[] armCurrentAmps = new double[]{}; + public double[] armTemperature = new double[]{}; + } /** Run open loop at the specified voltage. */ public default void setVoltage(double volts) {} diff --git a/src/main/java/frc/robot/subsystems/shooter/WristIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java similarity index 79% rename from src/main/java/frc/robot/subsystems/shooter/WristIOSparkMax.java rename to src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java index 190aadba..baedd7e4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/WristIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java @@ -1,10 +1,12 @@ package frc.robot.subsystems.shooter; -import com.revrobotics.*; +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; -public class WristIOSparkMax implements WristIO { +public class HoodIOSparkMax implements HoodIO { private static final double GEAR_RATIO = 1.5; private final CANSparkMax leader = new CANSparkMax(0, CANSparkLowLevel.MotorType.kBrushless); @@ -12,7 +14,7 @@ public class WristIOSparkMax implements WristIO { private final SparkAbsoluteEncoder encoder = leader.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); - public WristIOSparkMax() { + public HoodIOSparkMax() { leader.restoreFactoryDefaults(); leader.setCANTimeout(250); leader.setInverted(false); @@ -25,7 +27,7 @@ private Rotation2d getWristAngle() { return Rotation2d.fromRadians(MathUtil.angleModulus(encoder.getPosition() * 6.28) / 1.5); } - public void updateInputs(WristIOInputs inputs) { + public void updateInputs(HoodIOInputs inputs) { inputs.wristPositionRad = encoder.getPosition(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index fa0fb2c7..cd5f9f9d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -13,111 +13,145 @@ package frc.robot.subsystems.shooter; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.*; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import javax.swing.*; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import static edu.wpi.first.units.Units.*; + public class Shooter extends SubsystemBase { - private final ShooterIO shooterio; - private final WristIO wristio; - private final ShooterIOInputsAutoLogged shooterinputs = new ShooterIOInputsAutoLogged(); - private final WristIOInputsAutoLogged wristinputs = new WristIOInputsAutoLogged(); - private ProfiledPIDController shooterpid; - private ProfiledPIDController wristpid; - private final SimpleMotorFeedforward ffModel; - private static final double ENCODER_ANGLE_FIX = 1.5; - // the ration for turning the shooter + private static final double HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_RATIO = 1.5; + private static final double HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_OFFSET = 0.0; + // the ratio for turning the shooter private static final double TURN_SHOOTER_RATIO = 5.4; - private static double targetShooterAngleRad = 0.0; + private static double targetHoodAngleRad = 0.0; + private final ShooterIO shooterIO; + private final HoodIO hoodIO; + private final ShooterIOInputsAutoLogged shooterInputs = new ShooterIOInputsAutoLogged(); + private final HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); + private final ArmFeedforward hoodFF; + private PIDController shooterVelocityFB; + private SimpleMotorFeedforward shooterVelocityFF; + private ProfiledPIDController hoodFB; + private boolean characterizeMode; /** Creates a new Shooter. */ - public Shooter(ShooterIO io, WristIO wristIO) { - this.shooterio = io; - this.wristio = wristIO; + public Shooter(ShooterIO io, HoodIO hoodIO) { + this.shooterIO = io; + this.hoodIO = hoodIO; // Switch constants based on mode (the physics simulator is treated as a // separate robot with different tuning) switch (Constants.currentMode) { case REAL: // FIXME: characterize real robot case REPLAY: - ffModel = new SimpleMotorFeedforward(0.1, 0.05); - shooterpid = - new ProfiledPIDController( - 1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.5, 99)); // FIXME: remove profile? - wristpid = new ProfiledPIDController(4.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); + hoodFF = new ArmFeedforward(0.1, 0.05, 0); + shooterVelocityFB = + new PIDController(1.0, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); + hoodFB = new ProfiledPIDController(4.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); break; case SIM: - ffModel = new SimpleMotorFeedforward(0.0, 0.03); - shooterpid = - new ProfiledPIDController(0.5, 0.0, 0.0, new TrapezoidProfile.Constraints(0.5, 99)); - wristpid = new ProfiledPIDController(4.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); + hoodFF = new ArmFeedforward(0.0, 0.0, 0.03); + shooterVelocityFB = + new PIDController(0.5, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); + hoodFB = new ProfiledPIDController(4.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); + shooterVelocityFF = new SimpleMotorFeedforward(0, 0); break; default: - ffModel = new SimpleMotorFeedforward(0.0, 0.0); + hoodFF = new ArmFeedforward(0.0, 0.0, 0); break; } } @Override public void periodic() { - shooterio.updateInputs(shooterinputs); - wristio.updateInputs(wristinputs); - shooterio.setFlywheelVoltage( - shooterpid.calculate(shooterinputs.flywheelVelocityRadPerSec) - + ffModel.calculate( - shooterpid.getSetpoint().position, shooterpid.getSetpoint().velocity)); - targetShooterAngleRad = shooterpid.getSetpoint().position * ENCODER_ANGLE_FIX; - Logger.processInputs("Shooter", shooterinputs); - Logger.processInputs("Wrist", wristinputs); + shooterIO.updateInputs(shooterInputs); + hoodIO.updateInputs(hoodInputs); + if (characterizeMode) { + shooterIO.setFlywheelVoltage( + shooterVelocityFB.calculate(shooterInputs.flywheelVelocityRadPerSec) + + this.shooterVelocityFF.calculate(shooterInputs.flywheelVelocityRadPerSec)); + } + hoodIO.setVoltage(hoodFB.calculate(hoodInputs.armPositionRad, hoodInputs.armVelocityRadPerSec)); + targetHoodAngleRad = + hoodInputs.armPositionRad * HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_RATIO + + HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_OFFSET; + Logger.processInputs("Shooter", shooterInputs); + Logger.processInputs("Hood", hoodInputs); + } + + public void setCharacterizeMode(boolean on) { + characterizeMode = on; } /** Run open loop at the specified voltage. */ - public void shooterrunvolts(double volts) { - shooterio.setFlywheelVoltage(volts); + public void shooterRunVolts(Measure voltage) { + shooterIO.setFlywheelVoltage(voltage.in(Volts)); + } + + /** + * Run open loop at the specified voltage. + */ + public void shooterRunVolts(double volts) { + shooterIO.setFlywheelVoltage(volts); + } + + public Measure> getCharacterizationVelocity() { + return RadiansPerSecond.of(this.shooterInputs.flywheelVelocityRadPerSec); + } + + public Measure getCharacterizationCurrent() { + return Amps.of(this.shooterInputs.flywheelCurrentAmps[0]); + } + + public Measure getCharacterizationAppliedVolts() { + return Volts.of(this.shooterInputs.flywheelAppliedVolts); } - public void wristrunvolts(double volts) { - wristio.setVoltage(volts); + public void hoodRunVolts(double volts) { + hoodIO.setVoltage(volts); } /** Run closed loop at the specified velocity. */ - public void shooterrunVelocity(double velocityRPM) { + public void shooterRunVelocity(double velocityRPM) { var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - shooterpid.setGoal(velocityRadPerSec); + shooterVelocityFB.setGoal(velocityRadPerSec); // Log flywheel setpoint Logger.recordOutput("Shooter/SetpointRPM", velocityRPM); } /** Stops the flywheel. */ - public void stopshooter() { - shooterio.flywheelStop(); + public void stopShooter() { + shooterIO.flywheelStop(); } - public void stopwrist() { - wristio.wristStop(); + public void stopHood() { + hoodIO.wristStop(); } /** Returns the current velocity in RPM. */ @AutoLogOutput public double getShooterVelocityRPM() { - return Units.radiansPerSecondToRotationsPerMinute(shooterinputs.flywheelVelocityRadPerSec); + return Units.radiansPerSecondToRotationsPerMinute(shooterInputs.flywheelVelocityRadPerSec); } /** Returns the current velocity in radians per second. */ public double getShooterCharacterizationVelocity() { - return shooterinputs.flywheelVelocityRadPerSec; + return shooterInputs.flywheelVelocityRadPerSec; } public void setTargetShooterAngleRad(Rotation2d anglediff) { - targetShooterAngleRad = anglediff.getRadians() * TURN_SHOOTER_RATIO; + targetHoodAngleRad = anglediff.getRadians() * TURN_SHOOTER_RATIO; } } From ce9a15634284d5b307ad8f8a0497651d80014ec4 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Thu, 15 Feb 2024 16:08:05 -0500 Subject: [PATCH 17/45] stuff Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/subsystems/shooter/HoodIO.java | 10 +++++----- .../frc/robot/subsystems/shooter/HoodIOSparkMax.java | 6 +++--- .../java/frc/robot/subsystems/shooter/Shooter.java | 8 +++++--- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index 94b35f4e..dee47eef 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -22,11 +22,11 @@ public default void updateInputs(HoodIOInputs inputs) { @AutoLog public static class HoodIOInputs { - public double armPositionRad = 0.0; - public double armVelocityRadPerSec = 0.0; - public double armAppliedVolts = 0.0; - public double[] armCurrentAmps = new double[]{}; - public double[] armTemperature = new double[]{}; + public double hoodPositionRad = 0.0; + public double hoodVelocityRadPerSec = 0.0; + public double hoodAppliedVolts = 0.0; + public double[] hoodCurrentAmps = new double[]{}; + public double[] hoodTemperature = new double[]{}; } /** Run open loop at the specified voltage. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java index baedd7e4..34036651 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java @@ -14,7 +14,7 @@ public class HoodIOSparkMax implements HoodIO { private final SparkAbsoluteEncoder encoder = leader.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); - public HoodIOSparkMax() { + public HoodIOSparkMax() { leader.restoreFactoryDefaults(); leader.setCANTimeout(250); leader.setInverted(false); @@ -27,8 +27,8 @@ private Rotation2d getWristAngle() { return Rotation2d.fromRadians(MathUtil.angleModulus(encoder.getPosition() * 6.28) / 1.5); } - public void updateInputs(HoodIOInputs inputs) { - inputs.wristPositionRad = encoder.getPosition(); + public void updateInputs(HoodIOInputs inputs) { + inputs.hoodPositionRad = encoder.getPosition(); } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index cd5f9f9d..f436f094 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -81,9 +81,11 @@ public void periodic() { shooterVelocityFB.calculate(shooterInputs.flywheelVelocityRadPerSec) + this.shooterVelocityFF.calculate(shooterInputs.flywheelVelocityRadPerSec)); } - hoodIO.setVoltage(hoodFB.calculate(hoodInputs.armPositionRad, hoodInputs.armVelocityRadPerSec)); + hoodIO.setVoltage( + hoodFB.calculate(hoodInputs.hoodPositionRad, hoodInputs.hoodVelocityRadPerSec) + + hoodFF.calculate(hoodFB.getSetpoint().position, hoodFB.getSetpoint().velocity)); targetHoodAngleRad = - hoodInputs.armPositionRad * HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_RATIO + hoodInputs.hoodPositionRad * HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_RATIO + HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_OFFSET; Logger.processInputs("Shooter", shooterInputs); Logger.processInputs("Hood", hoodInputs); @@ -125,7 +127,7 @@ public void hoodRunVolts(double volts) { public void shooterRunVelocity(double velocityRPM) { var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - shooterVelocityFB.setGoal(velocityRadPerSec); + shooterVelocityFB.setSetpoint(velocityRadPerSec); // Log flywheel setpoint Logger.recordOutput("Shooter/SetpointRPM", velocityRPM); From 62920a9abc0ffc9ebee06f572a6b22716140ce40 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Thu, 15 Feb 2024 19:10:48 -0500 Subject: [PATCH 18/45] added some code for shooter and a shooter characterization sequence Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 105 ++++++++++-------- .../subsystems/intake/IntakeIOSparkMax.java | 7 +- .../subsystems/shooter/HoodIOSparkMax.java | 2 +- .../robot/subsystems/shooter/ShooterIO.java | 1 + .../subsystems/shooter/ShooterIOTalonFX.java | 10 +- 5 files changed, 72 insertions(+), 53 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0f5cc091..b87089d5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,9 @@ package frc.robot; +import static edu.wpi.first.units.BaseUnits.Voltage; +import static edu.wpi.first.units.Units.Seconds; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; @@ -20,10 +23,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; @@ -37,19 +37,12 @@ import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; -import frc.robot.subsystems.intake.IntakeIOSparkMax; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterIO; -import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.*; import frc.robot.util.Mode; import frc.robot.util.ModeHelper; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; -import static edu.wpi.first.units.BaseUnits.Voltage; -import static edu.wpi.first.units.Units.Seconds; - /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -82,6 +75,8 @@ public Command getExitCommand(Mode m) { private final LoggedDashboardNumber flywheelSpeedInput = new LoggedDashboardNumber("Flywheel Speed", 1500.0); + public static SysIDMode sysIDMode = SysIDMode.EverythingElse; + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { switch (Constants.currentMode) { @@ -90,14 +85,13 @@ public RobotContainer() { drive = new Drive( new GyroIOPigeon2(), - new ModuleIOSparkMax(0), - new ModuleIOSparkMax(1), - new ModuleIOSparkMax(2), - new ModuleIOSparkMax(3)); - shooter = new Shooter(new ShooterIOTalonFX(), new HoodIO() { - }); + new ModuleIOSim(/*0*/ ), + new ModuleIOSim(/*1*/ ), + new ModuleIOSim(/*2*/ ), + new ModuleIOSim(/*3*/ )); + shooter = new Shooter(new ShooterIOTalonFX(), new HoodIOSparkMax() {}); feeder = new Feeder(new FeederIO() {}); - intake = new Intake(new IntakeIOSparkMax()); + intake = new Intake(new IntakeIO() {}); // drive = new Drive( // new GyroIOPigeon2(), // new ModuleIOTalonFX(0), @@ -116,8 +110,7 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); - shooter = new Shooter(new ShooterIOSim(), new HoodIO() { - }); + shooter = new Shooter(new ShooterIOSim(), new HoodIO() {}); feeder = new Feeder(new FeederIOSim()); intake = new Intake(new IntakeIOSim()); break; @@ -131,9 +124,7 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - shooter = new Shooter(new ShooterIO() { - }, new HoodIO() { - }); + shooter = new Shooter(new ShooterIO() {}, new HoodIO() {}); feeder = new Feeder(new FeederIO() {}); intake = new Intake(new IntakeIO() {}); break; @@ -143,8 +134,8 @@ public RobotContainer() { NamedCommands.registerCommand( "Run Flywheel", Commands.startEnd( - () -> shooter.shooterRunVelocity(flywheelSpeedInput.get()), - shooter::stopShooter, + () -> shooter.shooterRunVelocity(flywheelSpeedInput.get()), + shooter::stopShooter, shooter) .withTimeout(5.0)); autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -152,8 +143,6 @@ public RobotContainer() { configureButtonBindings(); } - public static SysIDMode sysIDMode = SysIDMode.Disabled; - public enum SysIDMode { Disabled, DriveMotors, @@ -252,18 +241,25 @@ private void configureButtonBindings() { .a() .whileTrue( Commands.startEnd( - () -> shooter.shooterRunVelocity(flywheelSpeedInput.get()), - shooter::stopShooter, + () -> shooter.shooterRunVelocity(flywheelSpeedInput.get()), + shooter::stopShooter, shooter)); controller .rightTrigger() - .onTrue(new RunCommand(() -> shooter.shooterRunVolts(6.0), shooter)); + .whileTrue( + new StartEndCommand( + () -> shooter.shooterRunVolts(12.0 * controller.getRightTriggerAxis()), + () -> { + shooter.stopShooter(); + shooter.shooterRunVolts(0.0); + }, + shooter)); controller .a() .whileTrue( Commands.startEnd( - () -> shooter.shooterRunVelocity(flywheelSpeedInput.get()), - shooter::stopShooter, + () -> shooter.shooterRunVelocity(flywheelSpeedInput.get()), + shooter::stopShooter, shooter)); break; @@ -305,24 +301,43 @@ private void configureButtonBindings() { .andThen( (new RunCommand( () -> - shooter.shooterRunVelocity( + shooter.shooterRunVelocity( 5000) /*THIS NUMBER NEEDS TO BE CALIBRATED*/, intake)))); break; case EverythingElse: var shooterSysId = - new SysIdRoutine( - new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)), - new Mechanism( - shooter::shooterRunVolts, - (log) -> { - var motor = log.motor("Shooter"); - motor.voltage(shooter.getCharacterizationAppliedVolts()); - motor.angularVelocity(shooter.getCharacterizationVelocity()); - motor.current(shooter.getCharacterizationCurrent()); - }, - shooter, - "FlywheelMotors")); + new SysIdRoutine( + new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(30)), + new Mechanism( + shooter::shooterRunVolts, + (log) -> { + var motor = log.motor("Shooter"); + motor.voltage(shooter.getCharacterizationAppliedVolts()); + motor.angularVelocity(shooter.getCharacterizationVelocity()); + motor.current(shooter.getCharacterizationCurrent()); + }, + shooter, + "FlywheelMotors")); + controller + .a() + .onTrue( + shooterSysId + .dynamic(Direction.kForward) + .withTimeout(5) + .andThen(new WaitCommand(5)) + .andThen( + shooterSysId + .dynamic(Direction.kReverse) + .withTimeout(5) + .andThen(new WaitCommand(5)) + .andThen(shooterSysId.quasistatic(Direction.kForward).withTimeout(60)) + .andThen(new WaitCommand(5)) + .andThen( + shooterSysId + .quasistatic(Direction.kReverse) + .withTimeout(60) + .andThen(new WaitCommand(5))))); break; } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index b80bed8b..b3d217f2 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -13,8 +13,11 @@ package frc.robot.subsystems.intake; -import com.revrobotics.*; +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder; import edu.wpi.first.math.util.Units; import org.littletonrobotics.junction.Logger; @@ -62,7 +65,7 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.rollerTemperature = new double[] {roller.getMotorTemperature()}; } -@Override + @Override public void setArmVoltage(double volts) { Logger.recordOutput("ArmSetVoltage", volts); arm.setVoltage(volts); diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java index 34036651..9af98b9d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java @@ -9,7 +9,7 @@ public class HoodIOSparkMax implements HoodIO { private static final double GEAR_RATIO = 1.5; - private final CANSparkMax leader = new CANSparkMax(0, CANSparkLowLevel.MotorType.kBrushless); + private final CANSparkMax leader = new CANSparkMax(35, CANSparkLowLevel.MotorType.kBrushless); private final SparkAbsoluteEncoder encoder = leader.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index af3e5dd4..2d768fa2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -18,6 +18,7 @@ public interface ShooterIO { @AutoLog public static class ShooterIOInputs { + public double flywheelPositionRad = 0.0; public double flywheelVelocityRadPerSec = 0.0; public double flywheelAppliedVolts = 0.0; public double[] flywheelCurrentAmps = new double[] {}; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index cc508659..9fd86ab3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -17,7 +17,6 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; @@ -25,8 +24,8 @@ public class ShooterIOTalonFX implements ShooterIO { private static final double GEAR_RATIO = 1.5; - private final TalonFX leader = new TalonFX(0); - private final TalonFX follower = new TalonFX(1); + private final TalonFX leader = new TalonFX(40); + private final TalonFX follower = new TalonFX(41); private final StatusSignal leaderPosition = leader.getPosition(); private final StatusSignal leaderVelocity = leader.getVelocity(); @@ -36,7 +35,7 @@ public class ShooterIOTalonFX implements ShooterIO { public ShooterIOTalonFX() { var config = new TalonFXConfiguration(); - config.CurrentLimits.StatorCurrentLimit = 30.0; + config.CurrentLimits.StatorCurrentLimit = 50.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.MotorOutput.NeutralMode = NeutralModeValue.Coast; leader.getConfigurator().apply(config); @@ -53,6 +52,7 @@ public ShooterIOTalonFX() { public void updateInputs(ShooterIOInputs inputs) { BaseStatusSignal.refreshAll( leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); + inputs.flywheelPositionRad = leaderPosition.getValueAsDouble(); inputs.flywheelVelocityRadPerSec = Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / GEAR_RATIO; inputs.flywheelAppliedVolts = leaderAppliedVolts.getValueAsDouble(); @@ -62,7 +62,7 @@ public void updateInputs(ShooterIOInputs inputs) { @Override public void setFlywheelVoltage(double volts) { - leader.setControl(new VoltageOut(volts)); + leader.setVoltage(volts); } @Override From 4d62d8edcf39c76d1b4a822b1ae191a06340aadf Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Thu, 15 Feb 2024 19:11:57 -0500 Subject: [PATCH 19/45] idk formatting maybe? Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- .../frc/robot/subsystems/shooter/HoodIO.java | 7 +++--- .../frc/robot/subsystems/shooter/Shooter.java | 24 +++++++++---------- 2 files changed, 14 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index dee47eef..b91fe680 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -17,16 +17,15 @@ public interface HoodIO { /** Updates the set of loggable inputs. */ - public default void updateInputs(HoodIOInputs inputs) { - } + public default void updateInputs(HoodIOInputs inputs) {} @AutoLog public static class HoodIOInputs { public double hoodPositionRad = 0.0; public double hoodVelocityRadPerSec = 0.0; public double hoodAppliedVolts = 0.0; - public double[] hoodCurrentAmps = new double[]{}; - public double[] hoodTemperature = new double[]{}; + public double[] hoodCurrentAmps = new double[] {}; + public double[] hoodTemperature = new double[] {}; } /** Run open loop at the specified voltage. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index f436f094..657de53a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -13,6 +13,8 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.*; + import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -26,8 +28,6 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import static edu.wpi.first.units.Units.*; - public class Shooter extends SubsystemBase { private static final double HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_RATIO = 1.5; private static final double HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_OFFSET = 0.0; @@ -56,13 +56,13 @@ public Shooter(ShooterIO io, HoodIO hoodIO) { case REPLAY: hoodFF = new ArmFeedforward(0.1, 0.05, 0); shooterVelocityFB = - new PIDController(1.0, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); + new PIDController(1.0, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); hoodFB = new ProfiledPIDController(4.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); break; case SIM: hoodFF = new ArmFeedforward(0.0, 0.0, 0.03); shooterVelocityFB = - new PIDController(0.5, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); + new PIDController(0.5, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); hoodFB = new ProfiledPIDController(4.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); shooterVelocityFF = new SimpleMotorFeedforward(0, 0); break; @@ -78,15 +78,15 @@ public void periodic() { hoodIO.updateInputs(hoodInputs); if (characterizeMode) { shooterIO.setFlywheelVoltage( - shooterVelocityFB.calculate(shooterInputs.flywheelVelocityRadPerSec) - + this.shooterVelocityFF.calculate(shooterInputs.flywheelVelocityRadPerSec)); + shooterVelocityFB.calculate(shooterInputs.flywheelVelocityRadPerSec) + + this.shooterVelocityFF.calculate(shooterInputs.flywheelVelocityRadPerSec)); } hoodIO.setVoltage( - hoodFB.calculate(hoodInputs.hoodPositionRad, hoodInputs.hoodVelocityRadPerSec) - + hoodFF.calculate(hoodFB.getSetpoint().position, hoodFB.getSetpoint().velocity)); + hoodFB.calculate(hoodInputs.hoodPositionRad, hoodInputs.hoodVelocityRadPerSec) + + hoodFF.calculate(hoodFB.getSetpoint().position, hoodFB.getSetpoint().velocity)); targetHoodAngleRad = - hoodInputs.hoodPositionRad * HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_RATIO - + HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_OFFSET; + hoodInputs.hoodPositionRad * HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_RATIO + + HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_OFFSET; Logger.processInputs("Shooter", shooterInputs); Logger.processInputs("Hood", hoodInputs); } @@ -100,9 +100,7 @@ public void shooterRunVolts(Measure voltage) { shooterIO.setFlywheelVoltage(voltage.in(Volts)); } - /** - * Run open loop at the specified voltage. - */ + /** Run open loop at the specified voltage. */ public void shooterRunVolts(double volts) { shooterIO.setFlywheelVoltage(volts); } From 32cc513af72d863d6feaa3b82b622d3683840e4a Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Fri, 16 Feb 2024 16:20:02 -0500 Subject: [PATCH 20/45] removed google formatter (its annoying) Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- build.gradle | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/build.gradle b/build.gradle index b9497c4a..5befcfec 100755 --- a/build.gradle +++ b/build.gradle @@ -1,5 +1,3 @@ -import com.diffplug.spotless.FormatterStep - plugins { id "java" id "edu.wpi.first.GradleRIO" version "2024.1.1" @@ -152,8 +150,7 @@ spotless { exclude "**/build/**", "**/build-*/**" } toggleOffOn() - def format = googleJavaFormat() - println format + // googleJavaFormat() removeUnusedImports() trimTrailingWhitespace() endWithNewline() From 76e204d66a9384922c380e433ad291e0cc6c2e69 Mon Sep 17 00:00:00 2001 From: KP <86213869+Kanishk-Pandey@users.noreply.github.com> Date: Sun, 18 Feb 2024 12:51:41 -0500 Subject: [PATCH 21/45] kraken code Signed-off-by: KP <86213869+Kanishk-Pandey@users.noreply.github.com> --- .../subsystems/feeder/FeederIOTalonFX.java | 66 +++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java new file mode 100644 index 00000000..62b40f0c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java @@ -0,0 +1,66 @@ +package frc.robot.subsystems.feeder; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.util.Units; +import frc.robot.subsystems.shooter.ShooterIO; + +public class FeederIOTalonFX implements FeederIO { + @Override + public void updateInputs(FeederIOInputs inputs) { + FeederIO.super.updateInputs(inputs); + } + + private final TalonFX feedMotor = new TalonFX(43); + private final StatusSignal feedMotorPosition = feedMotor.getPosition(); + private final StatusSignal feedMotorVelocity = feedMotor.getVelocity(); + private final StatusSignal feedMotorAppliedVolts = feedMotor.getMotorVoltage(); + private final StatusSignal feedMotorCurrent = feedMotor.getStatorCurrent(); + private final StatusSignal feedMotorTemperature = feedMotor.getDeviceTemp(); + + public FeederIOTalonFX() { + var config = new TalonFXConfiguration(); + config.CurrentLimits.StatorCurrentLimit = 30.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + feedMotor.getConfigurator().apply(config); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + feedMotorPosition, + feedMotorVelocity, + feedMotorAppliedVolts, + feedMotorCurrent, + feedMotorCurrent); + feedMotor.optimizeBusUtilization(); + } + + public void updateInputs(ShooterIO.ShooterIOInputs inputs) { + BaseStatusSignal.refreshAll( + feedMotorPosition, + feedMotorVelocity, + feedMotorAppliedVolts, + feedMotorCurrent, + feedMotorTemperature); + inputs.flywheelPositionRad = Units.rotationsToRadians(feedMotorPosition.getValueAsDouble()); + inputs.flywheelVelocityRadPerSec = + Units.rotationsToRadians(feedMotorVelocity.getValueAsDouble()); + inputs.flywheelAppliedVolts = feedMotorAppliedVolts.getValueAsDouble(); + inputs.flywheelCurrentAmps = new double[] {feedMotorCurrent.getValueAsDouble()}; + inputs.flywheelTemperature = new double[] {feedMotorTemperature.getValueAsDouble()}; + } + + @Override + public void setVoltage(double volts) { + feedMotor.setControl(new VoltageOut(volts)); + } + + @Override + public void stop() { + feedMotor.stopMotor(); + } +} From a8ede396afc5992282fc289c770478152064da18 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Sun, 18 Feb 2024 12:55:41 -0500 Subject: [PATCH 22/45] stuff Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 45 ++- .../subsystems/shooter/HoodIOSparkMax.java | 23 +- .../frc/robot/subsystems/shooter/Shooter.java | 296 ++++++++++-------- .../robot/subsystems/shooter/ShooterIO.java | 2 + .../subsystems/shooter/ShooterIOTalonFX.java | 25 +- 5 files changed, 229 insertions(+), 162 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b87089d5..dbc55d26 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,9 +13,6 @@ package frc.robot; -import static edu.wpi.first.units.BaseUnits.Voltage; -import static edu.wpi.first.units.Units.Seconds; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; @@ -37,12 +34,16 @@ import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; +import frc.robot.subsystems.intake.IntakeIOSparkMax; import frc.robot.subsystems.shooter.*; import frc.robot.util.Mode; import frc.robot.util.ModeHelper; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; +import static edu.wpi.first.units.BaseUnits.Voltage; +import static edu.wpi.first.units.Units.Seconds; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -75,7 +76,7 @@ public Command getExitCommand(Mode m) { private final LoggedDashboardNumber flywheelSpeedInput = new LoggedDashboardNumber("Flywheel Speed", 1500.0); - public static SysIDMode sysIDMode = SysIDMode.EverythingElse; + public static SysIDMode sysIDMode = SysIDMode.Disabled; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -91,7 +92,8 @@ public RobotContainer() { new ModuleIOSim(/*3*/ )); shooter = new Shooter(new ShooterIOTalonFX(), new HoodIOSparkMax() {}); feeder = new Feeder(new FeederIO() {}); - intake = new Intake(new IntakeIO() {}); + intake = new Intake(new IntakeIOSparkMax() { + }); // drive = new Drive( // new GyroIOPigeon2(), // new ModuleIOTalonFX(0), @@ -308,36 +310,31 @@ private void configureButtonBindings() { case EverythingElse: var shooterSysId = new SysIdRoutine( - new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(30)), + new Config(Voltage.per(Units.Second).of(.1), Voltage.of(9.0), Seconds.of(120)), new Mechanism( shooter::shooterRunVolts, (log) -> { var motor = log.motor("Shooter"); motor.voltage(shooter.getCharacterizationAppliedVolts()); + motor.angularPosition(shooter.getCharacterizationPosition()); motor.angularVelocity(shooter.getCharacterizationVelocity()); motor.current(shooter.getCharacterizationCurrent()); }, shooter, "FlywheelMotors")); controller - .a() - .onTrue( - shooterSysId - .dynamic(Direction.kForward) - .withTimeout(5) - .andThen(new WaitCommand(5)) - .andThen( - shooterSysId - .dynamic(Direction.kReverse) - .withTimeout(5) - .andThen(new WaitCommand(5)) - .andThen(shooterSysId.quasistatic(Direction.kForward).withTimeout(60)) - .andThen(new WaitCommand(5)) - .andThen( - shooterSysId - .quasistatic(Direction.kReverse) - .withTimeout(60) - .andThen(new WaitCommand(5))))); + .a() + .onTrue( + shooterSysId.dynamic(Direction.kForward).withTimeout(5) + .andThen( + new WaitCommand(5), + shooterSysId.dynamic(Direction.kReverse).withTimeout(5), + new WaitCommand(5), + shooterSysId.quasistatic(Direction.kForward).withTimeout(120), + new WaitCommand(5), + shooterSysId.quasistatic(Direction.kReverse).withTimeout(120) + ) + ); break; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java index 9af98b9d..3868e58c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java @@ -4,12 +4,12 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.Logger; public class HoodIOSparkMax implements HoodIO { private static final double GEAR_RATIO = 1.5; - private final CANSparkMax leader = new CANSparkMax(35, CANSparkLowLevel.MotorType.kBrushless); + private final CANSparkMax leader = new CANSparkMax(25, CANSparkLowLevel.MotorType.kBrushless); private final SparkAbsoluteEncoder encoder = leader.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); @@ -19,21 +19,26 @@ public HoodIOSparkMax() { leader.setCANTimeout(250); leader.setInverted(false); leader.enableVoltageCompensation(12.0); - leader.setSmartCurrentLimit(30); - leader.burnFlash(); - } + leader.setSmartCurrentLimit(50); + - private Rotation2d getWristAngle() { - return Rotation2d.fromRadians(MathUtil.angleModulus(encoder.getPosition() * 6.28) / 1.5); + encoder.setInverted(true); + + leader.burnFlash(); } public void updateInputs(HoodIOInputs inputs) { - inputs.hoodPositionRad = encoder.getPosition(); + inputs.hoodPositionRad = MathUtil.angleModulus(encoder.getPosition() * Math.PI * 2) / GEAR_RATIO; + inputs.hoodAppliedVolts = leader.getBusVoltage() * leader.getAppliedOutput(); + inputs.hoodCurrentAmps = new double[]{leader.getOutputCurrent()}; + inputs.hoodVelocityRadPerSec = (encoder.getVelocity() * Math.PI * 2) / GEAR_RATIO; + inputs.hoodTemperature = new double[]{leader.getMotorTemperature()}; } @Override public void setVoltage(double volts) { - leader.setVoltage(volts); + Logger.recordOutput("HoodVoltage", volts); + leader.setVoltage(MathUtil.clamp(volts, -5.0, 5.0)); } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 657de53a..d9d9dd05 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -13,8 +13,7 @@ package frc.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.*; - +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -28,130 +27,173 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import static edu.wpi.first.units.Units.*; + public class Shooter extends SubsystemBase { - private static final double HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_RATIO = 1.5; - private static final double HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_OFFSET = 0.0; - // the ratio for turning the shooter - private static final double TURN_SHOOTER_RATIO = 5.4; - private static double targetHoodAngleRad = 0.0; - private final ShooterIO shooterIO; - private final HoodIO hoodIO; - private final ShooterIOInputsAutoLogged shooterInputs = new ShooterIOInputsAutoLogged(); - private final HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); - private final ArmFeedforward hoodFF; - private PIDController shooterVelocityFB; - private SimpleMotorFeedforward shooterVelocityFF; - private ProfiledPIDController hoodFB; - private boolean characterizeMode; - - /** Creates a new Shooter. */ - public Shooter(ShooterIO io, HoodIO hoodIO) { - this.shooterIO = io; - this.hoodIO = hoodIO; - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.currentMode) { - case REAL: - // FIXME: characterize real robot - case REPLAY: - hoodFF = new ArmFeedforward(0.1, 0.05, 0); - shooterVelocityFB = - new PIDController(1.0, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); - hoodFB = new ProfiledPIDController(4.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); - break; - case SIM: - hoodFF = new ArmFeedforward(0.0, 0.0, 0.03); - shooterVelocityFB = - new PIDController(0.5, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); - hoodFB = new ProfiledPIDController(4.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); - shooterVelocityFF = new SimpleMotorFeedforward(0, 0); - break; - default: - hoodFF = new ArmFeedforward(0.0, 0.0, 0); - break; - } - } - - @Override - public void periodic() { - shooterIO.updateInputs(shooterInputs); - hoodIO.updateInputs(hoodInputs); - if (characterizeMode) { - shooterIO.setFlywheelVoltage( - shooterVelocityFB.calculate(shooterInputs.flywheelVelocityRadPerSec) - + this.shooterVelocityFF.calculate(shooterInputs.flywheelVelocityRadPerSec)); - } - hoodIO.setVoltage( - hoodFB.calculate(hoodInputs.hoodPositionRad, hoodInputs.hoodVelocityRadPerSec) - + hoodFF.calculate(hoodFB.getSetpoint().position, hoodFB.getSetpoint().velocity)); - targetHoodAngleRad = - hoodInputs.hoodPositionRad * HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_RATIO - + HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_OFFSET; - Logger.processInputs("Shooter", shooterInputs); - Logger.processInputs("Hood", hoodInputs); - } - - public void setCharacterizeMode(boolean on) { - characterizeMode = on; - } - - /** Run open loop at the specified voltage. */ - public void shooterRunVolts(Measure voltage) { - shooterIO.setFlywheelVoltage(voltage.in(Volts)); - } - - /** Run open loop at the specified voltage. */ - public void shooterRunVolts(double volts) { - shooterIO.setFlywheelVoltage(volts); - } - - public Measure> getCharacterizationVelocity() { - return RadiansPerSecond.of(this.shooterInputs.flywheelVelocityRadPerSec); - } - - public Measure getCharacterizationCurrent() { - return Amps.of(this.shooterInputs.flywheelCurrentAmps[0]); - } - - public Measure getCharacterizationAppliedVolts() { - return Volts.of(this.shooterInputs.flywheelAppliedVolts); - } - - public void hoodRunVolts(double volts) { - hoodIO.setVoltage(volts); - } - - /** Run closed loop at the specified velocity. */ - public void shooterRunVelocity(double velocityRPM) { - var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - - shooterVelocityFB.setSetpoint(velocityRadPerSec); - - // Log flywheel setpoint - Logger.recordOutput("Shooter/SetpointRPM", velocityRPM); - } - - /** Stops the flywheel. */ - public void stopShooter() { - shooterIO.flywheelStop(); - } - - public void stopHood() { - hoodIO.wristStop(); - } - - /** Returns the current velocity in RPM. */ - @AutoLogOutput - public double getShooterVelocityRPM() { - return Units.radiansPerSecondToRotationsPerMinute(shooterInputs.flywheelVelocityRadPerSec); - } - - /** Returns the current velocity in radians per second. */ - public double getShooterCharacterizationVelocity() { - return shooterInputs.flywheelVelocityRadPerSec; - } - - public void setTargetShooterAngleRad(Rotation2d anglediff) { - targetHoodAngleRad = anglediff.getRadians() * TURN_SHOOTER_RATIO; - } + private static final double HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_RATIO = 1.0; + private static final double HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_OFFSET = 0.0; + // the ratio for turning the shooter +// private static final double TURN_SHOOTER_RATIO = 5.4; + private static double targetHoodAngleRad = 0.0; + private final ShooterIO shooterIO; + private final HoodIO hoodIO; + private final ShooterIOInputsAutoLogged shooterInputs = new ShooterIOInputsAutoLogged(); + private final HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); + private final ArmFeedforward hoodFF; + private PIDController shooterVelocityFB; + private ProfiledPIDController shooterPositionFB; + private SimpleMotorFeedforward shooterVelocityFF; + private ProfiledPIDController hoodFB; + private boolean characterizeMode; + + /** + * Creates a new Shooter. + */ + public Shooter(ShooterIO io, HoodIO hoodIO) { + this.shooterIO = io; + this.hoodIO = hoodIO; + // Switch constants based on mode (the physics simulator is treated as a + // separate robot with different tuning) + switch (Constants.currentMode) { + case REAL: + hoodFB = new ProfiledPIDController(2.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); + if (shooterIO instanceof ShooterIOTalonFX) { + shooterVelocityFB = + new PIDController(0.0050812, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); + shooterVelocityFB.setTolerance(25); + shooterPositionFB = + new ProfiledPIDController(20.817, 0.0, 1.7743, new TrapezoidProfile.Constraints(RadiansPerSecond.of(90), RadiansPerSecond.per(Second).of(27.8736842105))); + shooterPositionFB.setTolerance(0.26, 5); + shooterVelocityFF = new SimpleMotorFeedforward(0.10548, 0.11959, 0.066251); + } + // FIXME: characterize real robot + + hoodFF = new ArmFeedforward(0.0, 0.0, 0); + break; + case REPLAY: + hoodFF = new ArmFeedforward(0.10548, 0.05, 0); + hoodFB = new ProfiledPIDController(4.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); + + shooterVelocityFB = + new PIDController(0.0050812, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); + shooterVelocityFB.setTolerance(25); + shooterPositionFB = + new ProfiledPIDController(20.817, 0.0, 1.7743, new TrapezoidProfile.Constraints(RadiansPerSecond.of(90), RadiansPerSecond.per(Second).of(27.8736842105))); + shooterPositionFB.setTolerance(0.26, 5); + shooterVelocityFF = new SimpleMotorFeedforward(0.10548, 0.11959, 0.066251); + break; + case SIM: + hoodFF = new ArmFeedforward(0.0, 0.0, 0.03); + shooterVelocityFB = + new PIDController(0.5, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); + hoodFB = new ProfiledPIDController(4.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); + shooterVelocityFF = new SimpleMotorFeedforward(0, 0); + break; + default: + hoodFF = new ArmFeedforward(0.0, 0.0, 0); + break; + } + } + + @Override + public void periodic() { + shooterIO.updateInputs(shooterInputs); + hoodIO.updateInputs(hoodInputs); + if (characterizeMode) { + shooterIO.setFlywheelVoltage( + shooterVelocityFB.calculate(shooterInputs.flywheelVelocityRadPerSec) + + this.shooterVelocityFF.calculate(shooterInputs.flywheelVelocityRadPerSec)); + } + Logger.recordOutput("targetHoodAngle", targetHoodAngleRad); + Logger.recordOutput("hoodInputs.hoodPositionRad", hoodInputs.hoodPositionRad); + Logger.recordOutput("pidStuff", "" + hoodFB.getD() + " " + hoodFB.getI() + " " + hoodFB.getP()); + hoodIO.setVoltage( + hoodFB.calculate(hoodInputs.hoodPositionRad, targetHoodAngleRad) + /*+ hoodFF.calcula te(hoodFB.getSetpoint().position, hoodFB.getSetpoint().velocity)*/); +// targetHoodAngleRad = +// hoodInputs.hoodPositionRad * HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_RATIO +// + HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_OFFSET; + Logger.processInputs("Shooter", shooterInputs); + Logger.processInputs("Hood", hoodInputs); + } + + public void setCharacterizeMode(boolean on) { + characterizeMode = on; + } + + /** + * Run open loop at the specified voltage. + */ + public void shooterRunVolts(Measure voltage) { + shooterIO.setFlywheelVoltage(voltage.in(Volts)); + } + + /** + * Run open loop at the specified voltage. + */ + public void shooterRunVolts(double volts) { + shooterIO.setFlywheelVoltage(volts); + } + + public Measure getCharacterizationPosition() { + return Radians.of(this.shooterInputs.flywheelPositionRad); + } + + public Measure> getCharacterizationVelocity() { + return RadiansPerSecond.of(this.shooterInputs.flywheelVelocityRadPerSec); + } + + public Measure getCharacterizationCurrent() { + return Amps.of(this.shooterInputs.flywheelCurrentAmps[0]); + } + + public Measure getCharacterizationAppliedVolts() { + return Volts.of(this.shooterInputs.flywheelAppliedVolts); + } + + public void hoodRunVolts(double volts) { + hoodIO.setVoltage(volts); + } + + /** + * Run closed loop at the specified velocity. + */ + public void shooterRunVelocity(double velocityRPM) { + var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); + + shooterVelocityFB.setSetpoint(velocityRadPerSec); + + // Log flywheel setpoint + Logger.recordOutput("Shooter/SetpointRPM", velocityRPM); + } + + /** + * Stops the flywheel. + */ + public void stopShooter() { + shooterIO.flywheelStop(); + } + + public void stopHood() { + hoodIO.wristStop(); + } + + /** + * Returns the current velocity in RPM. + */ + @AutoLogOutput + public double getShooterVelocityRPM() { + return Units.radiansPerSecondToRotationsPerMinute(shooterInputs.flywheelVelocityRadPerSec); + } + + /** + * Returns the current velocity in radians per second. + */ + public double getShooterCharacterizationVelocity() { + return shooterInputs.flywheelVelocityRadPerSec; + } + + public void setTargetShooterAngleRad(Rotation2d anglediff) { + targetHoodAngleRad = MathUtil.clamp(anglediff.getRadians(), -2, 2); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 2d768fa2..f2d1bc64 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -23,6 +23,8 @@ public static class ShooterIOInputs { public double flywheelAppliedVolts = 0.0; public double[] flywheelCurrentAmps = new double[] {}; public double[] flywheelTemperature = new double[] {}; + public double[] flywheelAncillaryTemperature = new double[]{}; + public double[] flywheelProcessorTemperature = new double[]{}; } /** Updates the set of loggable inputs. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 9fd86ab3..67b9e71a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -22,7 +22,7 @@ import edu.wpi.first.math.util.Units; public class ShooterIOTalonFX implements ShooterIO { - private static final double GEAR_RATIO = 1.5; + private static final double GEAR_RATIO = 1.0; private final TalonFX leader = new TalonFX(40); private final TalonFX follower = new TalonFX(41); @@ -31,7 +31,13 @@ public class ShooterIOTalonFX implements ShooterIO { private final StatusSignal leaderVelocity = leader.getVelocity(); private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage(); private final StatusSignal leaderCurrent = leader.getStatorCurrent(); + private final StatusSignal leaderDeviceTemp = leader.getDeviceTemp(); + private final StatusSignal leaderAncillaryDeviceTemp = leader.getAncillaryDeviceTemp(); + private final StatusSignal leaderProcessorTemp = leader.getProcessorTemp(); private final StatusSignal followerCurrent = follower.getStatorCurrent(); + private final StatusSignal followerDeviceTemp = follower.getDeviceTemp(); + private final StatusSignal followerAncillaryDeviceTemp = follower.getAncillaryDeviceTemp(); + private final StatusSignal followerProcessorTemp = follower.getProcessorTemp(); public ShooterIOTalonFX() { var config = new TalonFXConfiguration(); @@ -44,6 +50,13 @@ public ShooterIOTalonFX() { BaseStatusSignal.setUpdateFrequencyForAll( 50.0, leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); + BaseStatusSignal.setUpdateFrequencyForAll(20, + leaderDeviceTemp, + leaderAncillaryDeviceTemp, + leaderProcessorTemp, + followerDeviceTemp, + followerAncillaryDeviceTemp, + followerProcessorTemp); leader.optimizeBusUtilization(); follower.optimizeBusUtilization(); } @@ -51,13 +64,21 @@ public ShooterIOTalonFX() { @Override public void updateInputs(ShooterIOInputs inputs) { BaseStatusSignal.refreshAll( - leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); + leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent, leaderDeviceTemp, + leaderAncillaryDeviceTemp, leaderProcessorTemp, followerDeviceTemp, followerAncillaryDeviceTemp, + followerProcessorTemp); inputs.flywheelPositionRad = leaderPosition.getValueAsDouble(); inputs.flywheelVelocityRadPerSec = Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / GEAR_RATIO; inputs.flywheelAppliedVolts = leaderAppliedVolts.getValueAsDouble(); inputs.flywheelCurrentAmps = new double[] {leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()}; + inputs.flywheelTemperature + = new double[]{leaderDeviceTemp.getValueAsDouble(), followerDeviceTemp.getValueAsDouble()}; + inputs.flywheelAncillaryTemperature + = new double[]{leaderAncillaryDeviceTemp.getValueAsDouble(), followerAncillaryDeviceTemp.getValueAsDouble()}; + inputs.flywheelProcessorTemperature + = new double[]{leaderProcessorTemp.getValueAsDouble(), followerProcessorTemp.getValueAsDouble()}; } @Override From 7ef9d330048c0451323865b31540aa31a552e967 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Sun, 18 Feb 2024 15:29:27 -0500 Subject: [PATCH 23/45] kinda working Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 36 +++++++++++-------- .../robot/subsystems/drive/ModuleIOSim.java | 6 ++++ .../subsystems/feeder/FeederIOTalonFX.java | 4 ++- .../frc/robot/subsystems/intake/Intake.java | 35 +++++++++++------- .../subsystems/intake/IntakeIOSparkMax.java | 10 +++--- 5 files changed, 57 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dbc55d26..042732d4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,6 +31,7 @@ import frc.robot.subsystems.feeder.Feeder; import frc.robot.subsystems.feeder.FeederIO; import frc.robot.subsystems.feeder.FeederIOSim; +import frc.robot.subsystems.feeder.FeederIOTalonFX; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; @@ -86,12 +87,12 @@ public RobotContainer() { drive = new Drive( new GyroIOPigeon2(), - new ModuleIOSim(/*0*/ ), - new ModuleIOSim(/*1*/ ), - new ModuleIOSim(/*2*/ ), - new ModuleIOSim(/*3*/ )); + new ModuleIOSim(0), + new ModuleIOSim(1), + new ModuleIOSim(2), + new ModuleIOSim(3)); shooter = new Shooter(new ShooterIOTalonFX(), new HoodIOSparkMax() {}); - feeder = new Feeder(new FeederIO() {}); + feeder = new Feeder(new FeederIOTalonFX()); intake = new Intake(new IntakeIOSparkMax() { }); // drive = new Drive( @@ -171,11 +172,16 @@ private void configureButtonBindings() { intake.setDefaultCommand( new RunCommand( () -> { - intake.setIntakePosition(Rotation2d.fromDegrees(-130)); + intake.setIntakePosition(Rotation2d.fromDegrees(-90)); intake.setRollerPercentage(0.0); }, - intake)); - feeder.setDefaultCommand(new RunCommand(feeder::stop, feeder)); + intake).beforeStarting(new InstantCommand(intake::resetArmFB, intake))); + feeder.setDefaultCommand(new RunCommand(() -> { + feeder.runVolts(10); + }, feeder)); + shooter.setDefaultCommand(new RunCommand(() -> { + shooter.setTargetShooterAngleRad(new Rotation2d(0.55)); + }, shooter)); // ---- DRIVETRAIN COMMANDS ---- controller.x().whileTrue(Commands.runOnce(drive::stopWithX, drive)); @@ -198,11 +204,11 @@ private void configureButtonBindings() { .leftTrigger() .and(feeder::getSensorFeed) .whileTrue( - new RunCommand(() -> feeder.runVolts(6.0), feeder) + new RunCommand(() -> feeder.runVolts(0.0), feeder) .until(() -> !feeder.getSensorFeed())); // prepare the shooter for dumping into the amp - controller.a().onTrue(Commands.runOnce(() -> modeHelper.switchTo(Mode.AMP))); + controller.y().onTrue(Commands.runOnce(() -> modeHelper.switchTo(Mode.AMP))); /* .toggleOnTrue( @@ -224,8 +230,8 @@ private void configureButtonBindings() { .whileTrue( new RunCommand( () -> { - intake.setIntakePosition(Rotation2d.fromDegrees(0.0)); - intake.setRollerPercentage(0.75); + intake.setIntakePosition(Rotation2d.fromDegrees(-5)); + intake.setRollerPercentage(.66); }, intake)); controller @@ -234,13 +240,13 @@ private void configureButtonBindings() { new RunCommand( () -> { intake.setIntakePosition(Rotation2d.fromDegrees(-90.0)); - intake.setRollerPercentage(0.0); + intake.setRollerPercentage(0.5); }, intake)); // ---- SHOOTER COMMANDS ---- controller - .a() + .b() .whileTrue( Commands.startEnd( () -> shooter.shooterRunVelocity(flywheelSpeedInput.get()), @@ -257,7 +263,7 @@ private void configureButtonBindings() { }, shooter)); controller - .a() + .x() .whileTrue( Commands.startEnd( () -> shooter.shooterRunVelocity(flywheelSpeedInput.get()), diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index cbc030d8..1018adb1 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -35,6 +35,12 @@ public class ModuleIOSim implements ModuleIO { private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; + public ModuleIOSim() { + } + + public ModuleIOSim(int ignored) { + } + @Override public void updateInputs(ModuleIOInputs inputs) { driveSim.update(LOOP_PERIOD_SECS); diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java index 62b40f0c..ae71470b 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java @@ -5,6 +5,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; 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 edu.wpi.first.math.util.Units; import frc.robot.subsystems.shooter.ShooterIO; @@ -24,8 +25,9 @@ public void updateInputs(FeederIOInputs inputs) { public FeederIOTalonFX() { var config = new TalonFXConfiguration(); - config.CurrentLimits.StatorCurrentLimit = 30.0; + config.CurrentLimits.StatorCurrentLimit = 50.0; config.CurrentLimits.StatorCurrentLimitEnable = true; + config.MotorOutput.withInverted(InvertedValue.Clockwise_Positive); config.MotorOutput.NeutralMode = NeutralModeValue.Coast; feedMotor.getConfigurator().apply(config); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index dd9761e6..5b893b41 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,10 +1,10 @@ package frc.robot.subsystems.intake; -import static edu.wpi.first.units.Units.*; - +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; @@ -14,6 +14,8 @@ import frc.robot.Constants; import org.littletonrobotics.junction.Logger; +import static edu.wpi.first.units.Units.*; + public class Intake extends SubsystemBase { private final MechanismLigament2d ligament1; private final MechanismLigament2d ligament1A; @@ -26,6 +28,13 @@ public class Intake extends SubsystemBase { Mechanism2d mechanism2d = new Mechanism2d(0, 0); + // double position = 0.0; + Rotation2d armTarget = Rotation2d.fromDegrees(-90); + + Rotation2d off1 = new Rotation2d(1.2466477); + Rotation2d off1A = new Rotation2d(0.313352305); + Rotation2d off2 = new Rotation2d(1.36216528); + Rotation2d off2A = new Rotation2d(0.154247715); public Intake(IntakeIO io) { this.io = io; switch (Constants.currentMode) { @@ -33,10 +42,10 @@ public Intake(IntakeIO io) { armFF = new ArmFeedforward(0.0, 0.21, 0.195, 0.0); armFB = new ProfiledPIDController( - 1.0, + 7.0, 0.0, 0.0, - new Constraints(RotationsPerSecond.of(3), RotationsPerSecond.per(Second).of(9))); + new Constraints(RadiansPerSecond.of(1), RadiansPerSecond.per(Second).of(1.5))); break; case REPLAY: armFF = new ArmFeedforward(0.1, .15, 1.95); @@ -62,7 +71,7 @@ public Intake(IntakeIO io) { break; } var root = mechanism2d.getRoot("Root", .305, .220); - + armFB.enableContinuousInput(-Math.PI, Math.PI); ligament1 = new MechanismLigament2d("Intake", .135, off1.getDegrees(), .1, new Color8Bit(1, 1, 1)); ligament1A = @@ -76,23 +85,19 @@ public Intake(IntakeIO io) { "Intake2", 0.232983, off2A.minus(quarterTurn).getDegrees(), .5, new Color8Bit(1, 1, 1)); root.append(ligament1).append(ligament1A); root.append(ligament2).append(ligament2A); - } - Rotation2d off1 = new Rotation2d(1.2466477); - Rotation2d off1A = new Rotation2d(0.313352305); - Rotation2d off2 = new Rotation2d(1.36216528); - Rotation2d off2A = new Rotation2d(0.154247715); - // double position = 0.0; - Rotation2d armTarget = Rotation2d.fromDegrees(0); + } Rotation2d quarterTurn = Rotation2d.fromRadians(Math.PI / 2); @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake", inputs); + if (Math.abs(MathUtil.angleModulus(armFB.getSetpoint().position) - MathUtil.angleModulus(armTarget.getRadians())) > .25) { + } if (RobotController.isSysActive()) io.setArmVoltage( - armFB.calculate(inputs.armPositionRad, armTarget.getRadians()) + armFB.calculate(inputs.armPositionRad, MathUtil.angleModulus(armTarget.getRadians())) + armFF.calculate(armFB.getSetpoint().position, armFB.getSetpoint().velocity)); else io.setArmVoltage(0.0); @@ -102,6 +107,10 @@ public void periodic() { Logger.recordOutput("Intake", mechanism2d); } + public void resetArmFB() { + armFB.reset(new TrapezoidProfile.State(Radians.of(inputs.armPositionRad), RadiansPerSecond.of(inputs.armVelocityRadPerSec))); + } + public void setIntakePosition(Rotation2d position) { armTarget = position; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index b3d217f2..a9f0192e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -18,6 +18,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.util.Units; import org.littletonrobotics.junction.Logger; @@ -42,14 +43,13 @@ public IntakeIOSparkMax() { arm.setCANTimeout(250); roller.setCANTimeout(250); - arm.setInverted(false); + arm.setInverted(true); // roller.follow(arm, false); // arm.enableVoltageCompensation(12.0); // roller.setSmartCurrentLimit(30); - encoder.setZeroOffset(/*0.0 */ .878); - encoder.setInverted(true); + encoder.setInverted(false); arm.burnFlash(); roller.burnFlash(); @@ -57,7 +57,7 @@ public IntakeIOSparkMax() { @Override public void updateInputs(IntakeIOInputs inputs) { - inputs.armPositionRad = Units.rotationsToRadians(encoder.getPosition()); + inputs.armPositionRad = MathUtil.angleModulus(Units.rotationsToRadians(encoder.getPosition())); inputs.armVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity()); inputs.armAppliedVolts = arm.getAppliedOutput() * arm.getBusVoltage(); inputs.armCurrentAmps = new double[] {arm.getOutputCurrent()}; @@ -68,7 +68,7 @@ public void updateInputs(IntakeIOInputs inputs) { @Override public void setArmVoltage(double volts) { Logger.recordOutput("ArmSetVoltage", volts); - arm.setVoltage(volts); + arm.setVoltage(MathUtil.clamp(volts, -10, 10)); } @Override From 1fded873b9ffbf6df28cecc6b8dc4121a0c34475 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Mon, 19 Feb 2024 15:41:51 -0500 Subject: [PATCH 24/45] Commit Message Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 042732d4..d519e957 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -92,7 +92,8 @@ public RobotContainer() { new ModuleIOSim(2), new ModuleIOSim(3)); shooter = new Shooter(new ShooterIOTalonFX(), new HoodIOSparkMax() {}); - feeder = new Feeder(new FeederIOTalonFX()); + feeder = new Feeder(new FeederIOTalonFX() { + }); intake = new Intake(new IntakeIOSparkMax() { }); // drive = new Drive( @@ -180,7 +181,8 @@ private void configureButtonBindings() { feeder.runVolts(10); }, feeder)); shooter.setDefaultCommand(new RunCommand(() -> { - shooter.setTargetShooterAngleRad(new Rotation2d(0.55)); +// shooter.setTargetShooterAngleRad(new Rotation2d(.9)); + shooter.setTargetShooterAngleRad(new Rotation2d(.4)); }, shooter)); // ---- DRIVETRAIN COMMANDS ---- From 7e0bcb7cd4aefbce769b1236282da214e7e29f8b Mon Sep 17 00:00:00 2001 From: KP <86213869+Kanishk-Pandey@users.noreply.github.com> Date: Tue, 20 Feb 2024 16:05:33 -0500 Subject: [PATCH 25/45] climb workie Signed-off-by: KP <86213869+Kanishk-Pandey@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 33 +++++++--- .../frc/robot/subsystems/climb/Climb.java | 61 +++---------------- .../frc/robot/subsystems/climb/ClimbIO.java | 3 - .../robot/subsystems/climb/ClimbIOSim.java | 5 +- .../subsystems/climb/ClimbIOSparkMax.java | 8 ++- .../frc/robot/subsystems/drive/Module.java | 35 +++++------ 6 files changed, 59 insertions(+), 86 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 97c6f648..cbc14f35 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.Units; @@ -32,6 +33,9 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.commands.DriveCommands; +import frc.robot.subsystems.climb.Climb; +import frc.robot.subsystems.climb.ClimbIO; +import frc.robot.subsystems.climb.ClimbIOSparkMax; import frc.robot.subsystems.drive.*; import frc.robot.subsystems.feeder.Feeder; import frc.robot.subsystems.feeder.FeederIO; @@ -39,11 +43,9 @@ import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; -import frc.robot.subsystems.intake.IntakeIOSparkMax; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOSim; -import frc.robot.subsystems.shooter.ShooterIOSparkMax; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; @@ -59,9 +61,11 @@ public class RobotContainer { private final Shooter shooter; private final Feeder feeder; private final Intake intake; + private Climb climb; // Controller private final CommandXboxController controller = new CommandXboxController(0); + private final CommandXboxController operatorController = new CommandXboxController(1); // Dashboard inputs private final LoggedDashboardChooser autoChooser; @@ -76,13 +80,14 @@ public RobotContainer() { drive = new Drive( new GyroIOPigeon2(), - new ModuleIOSparkMax(0), - new ModuleIOSparkMax(1), - new ModuleIOSparkMax(2), - new ModuleIOSparkMax(3)); - shooter = new Shooter(new ShooterIOSparkMax()); + new ModuleIOSparkMax(0) {}, + new ModuleIOSparkMax(1) {}, + new ModuleIOSparkMax(2) {}, + new ModuleIOSparkMax(3) {}); + shooter = new Shooter(new ShooterIO() {}); feeder = new Feeder(new FeederIO() {}); - intake = new Intake(new IntakeIOSparkMax()); + intake = new Intake(new IntakeIO() {}); + climb = new Climb(new ClimbIOSparkMax()); // drive = new Drive( // new GyroIOPigeon2(), // new ModuleIOTalonFX(0), @@ -104,6 +109,7 @@ public RobotContainer() { shooter = new Shooter(new ShooterIOSim()); feeder = new Feeder(new FeederIOSim()); intake = new Intake(new IntakeIOSim()); + climb = new Climb(new ClimbIO() {}); break; default: @@ -118,6 +124,7 @@ public RobotContainer() { shooter = new Shooter(new ShooterIO() {}); feeder = new Feeder(new FeederIO() {}); intake = new Intake(new IntakeIO() {}); + climb = new Climb(new ClimbIO() {}); break; } @@ -165,6 +172,15 @@ private void configureButtonBindings() { }, intake)); feeder.setDefaultCommand(new RunCommand(feeder::stop, feeder)); + // CLIMB DEFAULT COMMAND + climb.setDefaultCommand( + Commands.run( + () -> { + climb.runLeftVolts( + MathUtil.applyDeadband(operatorController.getLeftY(), 0.075) * 12); + climb.runRightVolts(operatorController.getRightY() * 12); + }, + climb)); // ---- DRIVETRAIN COMMANDS ---- controller.x().whileTrue(Commands.runOnce(drive::stopWithX, drive)); @@ -263,6 +279,7 @@ private void configureButtonBindings() { (new RunCommand( () -> shooter.runVelocity(5000) /*THIS NUMBER NEEDS TO BE CALIBRATED*/, intake)))); + break; case EverythingElse: break; diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index 72f8bfb7..79092922 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -1,11 +1,7 @@ package frc.robot.subsystems.climb; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; import org.littletonrobotics.junction.Logger; public class Climb extends SubsystemBase { @@ -13,8 +9,6 @@ public class Climb extends SubsystemBase { private static final double RIGHT_RADIUS = Units.inchesToMeters(0.8); // FIGURE OUT RADIUS private final ClimbIO io; private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged(); - private final SimpleMotorFeedforward ffModel; - private final ProfiledPIDController pidController; private Double leftRelativeOffset = null; // Relative + Offset = Absolute private Double rightRelativeOffset = null; // Relative + Offset = Absolute @@ -29,57 +23,18 @@ public class Climb extends SubsystemBase { public Climb(ClimbIO io) { this.io = io; - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.currentMode) { - case REAL: - // FIXME: characterize real robot - case REPLAY: - ffModel = new SimpleMotorFeedforward(0.1, 0.05); - pidController = - new ProfiledPIDController( - 1.0, - 0.0, - 0.0, - new TrapezoidProfile.Constraints(10, 10)); // fixme: tune velocity and acceleration - break; - case SIM: - ffModel = new SimpleMotorFeedforward(0.0, 0.03); - pidController = - new ProfiledPIDController( - 1.0, - 0.0, - 0.0, - new TrapezoidProfile.Constraints(10, 10)); // fixme: tune velocity and acceleration - break; - default: - ffModel = new SimpleMotorFeedforward(0.0, 0.0); - pidController = - new ProfiledPIDController(0., 0., .0, new TrapezoidProfile.Constraints(0., 0.)); - break; - } } @Override public void periodic() { - // On first cycle, reset relative turn encoder - // Wait until absolute angle is nonzero in case it wasn't initialized yet - if (leftRelativeOffset == null && inputs.leftPosition.getRadians() != 0.0) { - leftRelativeOffset = 0.0; - } - // On first cycle, reset relative turn encoder - // Wait until absolute angle is nonzero in case it wasn't initialized yet - if (rightRelativeOffset == null && inputs.rightPosition.getRadians() != 0.0) { - rightRelativeOffset = 0.0; - } io.updateInputs(inputs); // sets voltages - io.setLeftVoltage( - pidController.calculate(inputs.leftPositionRad, leftGoalPosition) - + ffModel.calculate(pidController.getSetpoint().velocity)); - io.setRightVoltage( - pidController.calculate(inputs.rightPositionRad, rightGoalPosition) - + ffModel.calculate(pidController.getSetpoint().velocity)); + // io.setLeftVoltage( + // pidController.calculate(inputs.leftPositionRad, leftGoalPosition) + // + ffModel.calculate(pidController.getSetpoint().velocity)); + // io.setRightVoltage( + // pidController.calculate(inputs.rightPositionRad, rightGoalPosition) + // + ffModel.calculate(pidController.getSetpoint().velocity)); Logger.processInputs("Climb", inputs); } @@ -110,13 +65,13 @@ public void runRightPosition(double rightPosition) { // idk what this does (for the left) public void resetLeftPosition() { leftOffset = inputs.leftPositionRad; - pidController.reset(inputs.leftPositionRad, leftVelocity = 0); + // pidController.reset(inputs.leftPositionRad, leftVelocity = 0); } // idk what this does (for the right) public void resetRightPosition() { rightOffset = inputs.rightPositionRad; - pidController.reset(inputs.rightPositionRad, rightVelocity = 0); + // pidController.reset(inputs.rightPositionRad, rightVelocity = 0); } /** Returns the current left position of the climb in meters. */ diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java index 4d517c19..2a4f714b 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.climb; -import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; public interface ClimbIO { @@ -25,14 +24,12 @@ public static class ClimbIOInputs { public double leftVelocityRadPerSec = 0.0; public double leftAppliedVolts = 0.0; public double[] leftCurrentAmps = new double[] {}; - public Rotation2d leftPosition; public double[] leftTemperature = new double[] {}; public double rightPositionRad = 0.0; public double rightVelocityRadPerSec = 0.0; public double rightAppliedVolts = 0.0; public double[] rightCurrentAmps = new double[] {}; - public Rotation2d rightPosition; public double[] rightTemperature = new double[] {}; } } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIOSim.java b/src/main/java/frc/robot/subsystems/climb/ClimbIOSim.java index 131f5dd6..f99810d1 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIOSim.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIOSim.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.climb; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.subsystems.climb.ClimbIO.ClimbIOInputs; @@ -32,12 +31,12 @@ public void updateInputs(ClimbIOInputs inputs) { rightSim.update(0.02); inputs.leftPositionRad = leftSim.getAngularPositionRad(); - inputs.leftPosition = (Rotation2d) new Rotation2d(leftSim.getAngularPositionRad()); + // inputs.leftPosition = (Rotation2d) new Rotation2d(leftSim.getAngularPositionRad()); inputs.leftVelocityRadPerSec = leftSim.getAngularVelocityRadPerSec(); inputs.leftAppliedVolts = leftAppliedVolts; inputs.leftCurrentAmps = new double[] {Math.abs(leftSim.getCurrentDrawAmps())}; inputs.rightPositionRad = rightSim.getAngularPositionRad(); - inputs.rightPosition = (Rotation2d) new Rotation2d(rightSim.getAngularPositionRad()); + // inputs.rightPosition = (Rotation2d) new Rotation2d(rightSim.getAngularPositionRad()); inputs.rightVelocityRadPerSec = rightSim.getAngularVelocityRadPerSec(); inputs.rightAppliedVolts = rightAppliedVolts; inputs.rightCurrentAmps = new double[] {Math.abs(rightSim.getCurrentDrawAmps())}; diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIOSparkMax.java b/src/main/java/frc/robot/subsystems/climb/ClimbIOSparkMax.java index 63aefd7b..b1821e73 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIOSparkMax.java @@ -13,6 +13,7 @@ package frc.robot.subsystems.climb; +import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; @@ -27,8 +28,8 @@ public class ClimbIOSparkMax implements ClimbIO { private static final double LEFT_GEAR_RATIO = 16.0; private static final double RIGHT_GEAR_RATIO = 16.0; - private final CANSparkMax left = new CANSparkMax(0, MotorType.kBrushless); - private final CANSparkMax right = new CANSparkMax(1, MotorType.kBrushless); + private final CANSparkMax left = new CANSparkMax(20, MotorType.kBrushless); + private final CANSparkMax right = new CANSparkMax(13, MotorType.kBrushless); private final RelativeEncoder leftEncoder = left.getEncoder(); private final RelativeEncoder rightEncoder = right.getEncoder(); @@ -45,6 +46,9 @@ public ClimbIOSparkMax() { right.enableVoltageCompensation(12.0); // wait for electrical to do climb wiring right.setSmartCurrentLimit(30); // wait for electrical to find the right breaker for the climb + left.setIdleMode(CANSparkBase.IdleMode.kBrake); + right.setIdleMode(CANSparkBase.IdleMode.kBrake); + left.burnFlash(); right.burnFlash(); } diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index d29ef220..350432d2 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -49,24 +49,25 @@ public Module(ModuleIO io, int index) { switch (Constants.currentMode) { case REAL: switch (index) { - case 0: - driveFeedforward = new SimpleMotorFeedforward(0.01626, 0.82954, 0.14095); - driveFeedback = new PIDController(0.21268, 0.0, 0.0); - break; - case 1: - driveFeedforward = new SimpleMotorFeedforward(0.081671, 0.82741, 0.081036); - driveFeedback = new PIDController(0.75099, 0.0, 0.0); - break; - case 2: - driveFeedforward = new SimpleMotorFeedforward(0.082023, 0.81434, 0.12098); - driveFeedback = new PIDController(0.096474, 0.0, 0.0); - break; - case 3: - driveFeedforward = new SimpleMotorFeedforward(0.040377, 0.84332, 0.13969); - driveFeedback = new PIDController(0.015087, 0.0, 0.0); - break; + // case 0: + // driveFeedforward = new SimpleMotorFeedforward(0.01626, 0.82954, 0.14095); + // driveFeedback = new PIDController(0.21268, 0.0, 0.0); + // // break; + // case 1: + // driveFeedforward = new SimpleMotorFeedforward(0.081671, 0.82741, + // 0.081036); + // driveFeedback = new PIDController(0.75099, 0.0, 0.0); + // // brea/**/k; + // case 2: + // driveFeedforward = new SimpleMotorFeedforward(0.082023, 0.81434, 0.12098); + // driveFeedback = new PIDController(0.096474, 0.0, 0.0); + // // break; + // case 3: + // driveFeedforward = new SimpleMotorFeedforward(0.040377, 0.84332, 0.13969); + // driveFeedback = new PIDController(0.015087, 0.0, 0.0); + // break; default: - driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + driveFeedforward = new SimpleMotorFeedforward(0.05, 0.8, .13); driveFeedback = new PIDController(0.05, 0.0, 0.0); break; } From f7e09ed8cceb6875d94a3223c40458ec1a4bf3b7 Mon Sep 17 00:00:00 2001 From: Kanishk Pandey <86213869+GloryBoyK@users.noreply.github.com> Date: Wed, 21 Feb 2024 15:14:19 -0500 Subject: [PATCH 26/45] Add files via upload --- RobotControls.rtf | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 RobotControls.rtf diff --git a/RobotControls.rtf b/RobotControls.rtf new file mode 100644 index 00000000..5493a1ed --- /dev/null +++ b/RobotControls.rtf @@ -0,0 +1,22 @@ +{\rtf1\ansi\ansicpg1252\cocoartf2759 +\cocoatextscaling0\cocoaplatform0{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +{\*\expandedcolortbl;;} +\margl1440\margr1440\vieww11520\viewh8400\viewkind0 +\pard\tx720\tx1440\tx2160\tx2880\tx3600\tx4320\tx5040\tx5760\tx6480\tx7200\tx7920\tx8640\pardirnatural\partightenfactor0 + +\f0\fs24 \cf0 DriverController - Left Joystick: drive motors\ +\pard\tx720\tx1440\tx2160\tx2880\tx3600\tx4320\tx5040\tx5760\tx6480\tx7200\tx7920\tx8640\pardirnatural\partightenfactor0 +\cf0 DriverController - Right Joystick: turn motors\ +DriverController - Right Trigger: Shoot/Dump\ +DriverController - Left Trigger: Intake\ +\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\'96\ +\pard\tx720\tx1440\tx2160\tx2880\tx3600\tx4320\tx5040\tx5760\tx6480\tx7200\tx7920\tx8640\pardirnatural\partightenfactor0 +\cf0 OperatorController - Left Joystick: Left Climb\ +OperatorController - Right Joystick: Right Climb\ +OperatorController - Y Button: Speaker Mode\ +OperatorController - B Button: Amp Mode\ +\pard\tx720\tx1440\tx2160\tx2880\tx3600\tx4320\tx5040\tx5760\tx6480\tx7200\tx7920\tx8640\pardirnatural\partightenfactor0 +\cf0 OperatorController - A Button: Neutral Mode\ +OperatorController - Right Trigger: Start Auto Aim\ +} \ No newline at end of file From 9e1330981e5808d222e372756040429e40f30dae Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Wed, 21 Feb 2024 15:27:55 -0500 Subject: [PATCH 27/45] real modules Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d519e957..a1b4e2c7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -87,10 +87,10 @@ public RobotContainer() { drive = new Drive( new GyroIOPigeon2(), - new ModuleIOSim(0), - new ModuleIOSim(1), - new ModuleIOSim(2), - new ModuleIOSim(3)); + new ModuleIOSparkMax(0), + new ModuleIOSparkMax(1), + new ModuleIOSparkMax(2), + new ModuleIOSparkMax(3)); shooter = new Shooter(new ShooterIOTalonFX(), new HoodIOSparkMax() {}); feeder = new Feeder(new FeederIOTalonFX() { }); From 21934ba599705246d78b097ce5df959478355411 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Thu, 22 Feb 2024 11:15:23 -0500 Subject: [PATCH 28/45] changed `controller` to `driverController` Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 46 ++++++++++----------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cbc14f35..40bd5c60 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,9 +13,6 @@ package frc.robot; -import static edu.wpi.first.units.BaseUnits.Voltage; -import static edu.wpi.first.units.Units.Seconds; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.MathUtil; @@ -49,6 +46,9 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; +import static edu.wpi.first.units.BaseUnits.Voltage; +import static edu.wpi.first.units.Units.Seconds; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -64,7 +64,7 @@ public class RobotContainer { private Climb climb; // Controller - private final CommandXboxController controller = new CommandXboxController(0); + private final CommandXboxController driverController = new CommandXboxController(0); private final CommandXboxController operatorController = new CommandXboxController(1); // Dashboard inputs @@ -161,9 +161,9 @@ private void configureButtonBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); + () -> -driverController.getLeftY(), + () -> -driverController.getLeftX(), + () -> -driverController.getRightX())); intake.setDefaultCommand( new RunCommand( () -> { @@ -183,8 +183,8 @@ private void configureButtonBindings() { climb)); // ---- DRIVETRAIN COMMANDS ---- - controller.x().whileTrue(Commands.runOnce(drive::stopWithX, drive)); - controller + driverController.x().whileTrue(Commands.runOnce(drive::stopWithX, drive)); + driverController .b() .whileTrue( Commands.runOnce( @@ -199,7 +199,7 @@ private void configureButtonBindings() { .ignoringDisable(true)); // ---- FEEDER COMMANDS ---- - controller + driverController .leftTrigger() .and(feeder::getSensorFeed) .whileTrue( @@ -207,7 +207,7 @@ private void configureButtonBindings() { .until(() -> !feeder.getSensorFeed())); // ---- INTAKE COMMANDS ---- - controller + driverController .leftBumper() // not a() .whileTrue( new RunCommand( @@ -216,7 +216,7 @@ private void configureButtonBindings() { intake.setRollerPercentage(0.75); }, intake)); - controller + driverController .rightBumper() .whileTrue( new RunCommand( @@ -227,13 +227,13 @@ private void configureButtonBindings() { intake)); // ---- SHOOTER COMMANDS ---- - controller + driverController .a() .whileTrue( Commands.startEnd( () -> shooter.runVelocity(flywheelSpeedInput.get()), shooter::stop, shooter)); - controller.rightTrigger().onTrue(new RunCommand(() -> shooter.runVolts(6.0), shooter)); - controller + driverController.rightTrigger().onTrue(new RunCommand(() -> shooter.runVolts(6.0), shooter)); + driverController .a() .whileTrue( Commands.startEnd( @@ -244,9 +244,9 @@ private void configureButtonBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); + () -> -driverController.getLeftY(), + () -> -driverController.getLeftX(), + () -> -driverController.getRightX())); var drivetrainDriveSysID = new SysIdRoutine( new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)), @@ -255,23 +255,23 @@ private void configureButtonBindings() { drive::populateDriveCharacterizationData, drive, "DrivetrainDriveMotors")); - controller + driverController .x() .whileTrue(drivetrainDriveSysID.dynamic(Direction.kForward)) .onFalse(Commands.runOnce(drive::stopWithX, drive)); - controller + driverController .y() .whileTrue(drivetrainDriveSysID.dynamic(Direction.kReverse)) .onFalse(Commands.runOnce(drive::stopWithX, drive)); - controller + driverController .a() .whileTrue(drivetrainDriveSysID.quasistatic(Direction.kForward).withTimeout(2.0)) .onFalse(Commands.runOnce(drive::stopWithX, drive)); - controller + driverController .b() .whileTrue(drivetrainDriveSysID.quasistatic(Direction.kReverse).withTimeout(2.0)) .onFalse(Commands.runOnce(drive::stopWithX, drive)); - controller + driverController .rightTrigger() .whileTrue( new RunCommand(() -> shooter.setTargetShooterAngleRad(new Rotation2d(-0.61))) From f7ee5546025d7041cdded3af31de3acd37094493 Mon Sep 17 00:00:00 2001 From: Everett Wilber <71281043+a1cd@users.noreply.github.com> Date: Thu, 22 Feb 2024 11:17:58 -0500 Subject: [PATCH 29/45] Update src/main/java/frc/robot/RobotContainer.java --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 40bd5c60..3b0ddafe 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,10 +80,10 @@ public RobotContainer() { drive = new Drive( new GyroIOPigeon2(), - new ModuleIOSparkMax(0) {}, - new ModuleIOSparkMax(1) {}, - new ModuleIOSparkMax(2) {}, - new ModuleIOSparkMax(3) {}); + new ModuleIOSparkMax(0), + new ModuleIOSparkMax(1), + new ModuleIOSparkMax(2), + new ModuleIOSparkMax(3)); shooter = new Shooter(new ShooterIO() {}); feeder = new Feeder(new FeederIO() {}); intake = new Intake(new IntakeIO() {}); From 506b98d4c3fb092f80354870fa94400f00400a64 Mon Sep 17 00:00:00 2001 From: Everett Wilber <71281043+a1cd@users.noreply.github.com> Date: Thu, 22 Feb 2024 11:18:27 -0500 Subject: [PATCH 30/45] Apply suggestions from code review --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3b0ddafe..d2025051 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -84,9 +84,9 @@ public RobotContainer() { new ModuleIOSparkMax(1), new ModuleIOSparkMax(2), new ModuleIOSparkMax(3)); - shooter = new Shooter(new ShooterIO() {}); + shooter = new Shooter(new ShooterIOSparkMax()); feeder = new Feeder(new FeederIO() {}); - intake = new Intake(new IntakeIO() {}); + intake = new Intake(new IntakeIOSparkMax()); climb = new Climb(new ClimbIOSparkMax()); // drive = new Drive( // new GyroIOPigeon2(), From 6a0c856b0a1114d4d1b3ed4ddfca571e73dfc339 Mon Sep 17 00:00:00 2001 From: Everett Wilber <71281043+a1cd@users.noreply.github.com> Date: Fri, 23 Feb 2024 13:47:01 -0500 Subject: [PATCH 31/45] Apply suggestions from code review --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0d68a253..63d24297 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,7 +78,7 @@ public RobotContainer() { new ModuleIOSparkMax(3)); shooter = new Shooter(new ShooterIOTalonFX()); feeder = new Feeder(new FeederIO() {}); - intake = new Intake(new IntakeIOSim()); + intake = new Intake(new IntakeIOSparkMax()); // drive = new Drive( // new GyroIOPigeon2(), // new ModuleIOTalonFX(0), From 19f0a5b4b8a231f21fd3808462f8ee8e39e3623a Mon Sep 17 00:00:00 2001 From: Everett Wilber <71281043+a1cd@users.noreply.github.com> Date: Fri, 23 Feb 2024 14:34:21 -0500 Subject: [PATCH 32/45] Apply suggestions from code review --- src/main/java/frc/robot/RobotContainer.java | 5 ++--- .../java/frc/robot/subsystems/shooter/HoodIO.java | 12 ------------ .../frc/robot/subsystems/shooter/HoodIOSparkMax.java | 5 ----- 3 files changed, 2 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a1b4e2c7..f732f37d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -206,7 +206,7 @@ private void configureButtonBindings() { .leftTrigger() .and(feeder::getSensorFeed) .whileTrue( - new RunCommand(() -> feeder.runVolts(0.0), feeder) + new RunCommand(() -> feeder.runVolts(10.0), feeder) .until(() -> !feeder.getSensorFeed())); // prepare the shooter for dumping into the amp @@ -311,8 +311,7 @@ private void configureButtonBindings() { .andThen( (new RunCommand( () -> - shooter.shooterRunVelocity( - 5000) /*THIS NUMBER NEEDS TO BE CALIBRATED*/, + shooter.shooterRunVelocity(5000), //THIS NUMBER NEEDS TO BE CALIBRATED intake)))); break; case EverythingElse: diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index b91fe680..bab06994 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -1,15 +1,3 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. package frc.robot.subsystems.shooter; diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java index 3868e58c..4a7b154a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOSparkMax.java @@ -40,9 +40,4 @@ public void setVoltage(double volts) { Logger.recordOutput("HoodVoltage", volts); leader.setVoltage(MathUtil.clamp(volts, -5.0, 5.0)); } - - @Override - public void wristStop() { - leader.stopMotor(); - } } From 761b3c3d4af06e3e6e8da0aafc344713266cf622 Mon Sep 17 00:00:00 2001 From: KP <86213869+Kanishk-Pandey@users.noreply.github.com> Date: Fri, 23 Feb 2024 15:05:52 -0500 Subject: [PATCH 33/45] fixes Signed-off-by: KP <86213869+Kanishk-Pandey@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 35 ++++++++++--------- .../subsystems/intake/IntakeIOSparkMax.java | 4 +-- 2 files changed, 21 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b8ca07aa..08ed247e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,9 @@ package frc.robot; +import static edu.wpi.first.units.BaseUnits.Voltage; +import static edu.wpi.first.units.Units.Seconds; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.MathUtil; @@ -40,15 +43,14 @@ import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; +import frc.robot.subsystems.intake.IntakeIOSparkMax; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOSim; +import frc.robot.subsystems.shooter.ShooterIOTalonFX; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; -import static edu.wpi.first.units.BaseUnits.Voltage; -import static edu.wpi.first.units.Units.Seconds; - /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -161,9 +163,9 @@ private void configureButtonBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -driverController.getLeftY(), - () -> -driverController.getLeftX(), - () -> -driverController.getRightX())); + () -> -driverController.getLeftY(), + () -> -driverController.getLeftX(), + () -> -driverController.getRightX())); intake.setDefaultCommand( new RunCommand( () -> { @@ -171,7 +173,7 @@ private void configureButtonBindings() { intake.setRollerPercentage(0.0); }, intake)); - feeder.setDefaultCommand(new RunCommand(feeder::stop, feeder)); + feeder.setDefaultCommand(new RunCommand(() -> feeder.runVolts(6.0), feeder)); shooter.setDefaultCommand(new RunCommand(shooter::stop, shooter)); // CLIMB DEFAULT COMMAND climb.setDefaultCommand( @@ -232,30 +234,31 @@ private void configureButtonBindings() { .rightTrigger() .whileTrue( Commands.run( - () -> shooter.runVolts(controller.getRightTriggerAxis() * 12.0), shooter)); - driverController - .leftTrigger() - .and(controller.rightTrigger().negate()) + () -> shooter.runVolts(driverController.getRightTriggerAxis() * 12.0), + shooter)); + driverController.leftTrigger().and(driverController.rightTrigger().negate()); driverController .a() .whileTrue( Commands.startEnd( () -> shooter.runVelocity(flywheelSpeedInput.get()), shooter::stop, shooter)); - driverController.rightTrigger().onTrue(new RunCommand(() -> shooter.runVolts(6.0), shooter)); + driverController + .rightTrigger() + .onTrue(new RunCommand(() -> shooter.runVolts(6.0), shooter)); driverController .a() .whileTrue( new RunCommand( - () -> shooter.runVolts(controller.getLeftTriggerAxis() * 12.0), shooter)); + () -> shooter.runVolts(driverController.getLeftTriggerAxis() * 12.0), shooter)); break; case DriveMotors: drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -driverController.getLeftY(), - () -> -driverController.getLeftX(), - () -> -driverController.getRightX())); + () -> -driverController.getLeftY(), + () -> -driverController.getLeftX(), + () -> -driverController.getRightX())); var drivetrainDriveSysID = new SysIdRoutine( new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)), diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index 11c567fe..c3b65e78 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -25,8 +25,8 @@ public class IntakeIOSparkMax implements IntakeIO { private static final double ARM_GEAR_RATIO = 100.0; private static final double ROLLER_GEAR_RATIO = 3.0; - private final CANSparkMax arm = new CANSparkMax(0, MotorType.kBrushless); - private final CANSparkMax roller = new CANSparkMax(1, MotorType.kBrushless); + private final CANSparkMax arm = new CANSparkMax(9, MotorType.kBrushless); + private final CANSparkMax roller = new CANSparkMax(31, MotorType.kBrushless); private final AbsoluteEncoder encoder = arm.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); private final SparkPIDController pid = arm.getPIDController(); From 9c2218b2d429afe153efba7adf8c39003fc64b31 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Sat, 24 Feb 2024 16:05:53 -0500 Subject: [PATCH 34/45] added characterization code that was removed Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- .../frc/robot/subsystems/shooter/Shooter.java | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index edf9b7c3..a4f234af 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -18,6 +18,9 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import org.littletonrobotics.junction.AutoLogOutput; @@ -94,12 +97,21 @@ public double getVelocityRPM() { return Units.radiansPerSecondToRotationsPerMinute(inputs.flywheelVelocityRadPerSec); } - /** Returns the current velocity in radians per second. */ - public double getCharacterizationVelocity() { - return inputs.flywheelVelocityRadPerSec; + /** + * Returns the current velocity in radians per second. + */ + public Measure> getCharacterizationVelocity() { + return edu.wpi.first.units.Units.RadiansPerSecond.of(inputs.flywheelVelocityRadPerSec); } - public void setTargetShooterAngleRad(Rotation2d anglediff) { + /** + * Returns the current velocity in radians per second. + */ + public Measure getCharacterizationPosition() { + return edu.wpi.first.units.Units.Radians.of(inputs.flywheelPositionRad); + } + + public void setTargetShooterAngle(Rotation2d anglediff) { targetShooterAngleRad = anglediff.getRadians() * TURN_SHOOTER_RATIO; } } From 9bc7e42c470b5422d9bc6a4fdbaeddae5bb62c76 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Sat, 24 Feb 2024 16:14:45 -0500 Subject: [PATCH 35/45] fixed shooter characterization stuff Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 27 ++++++++++++++ .../frc/robot/subsystems/shooter/Shooter.java | 36 ++++++++++++++++--- 2 files changed, 58 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 08ed247e..2185d3b8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -293,6 +293,33 @@ private void configureButtonBindings() { intake)))); break; case EverythingElse: + var shooterSysId = + new SysIdRoutine( + new Config(Voltage.per(Units.Second).of(.1), Voltage.of(9.0), Seconds.of(120)), + new Mechanism( + shooter::runVoltage, + (log) -> { + var motor = log.motor("Shooter"); + motor.voltage(shooter.getCharacterizationVoltage()); + motor.angularPosition(shooter.getCharacterizationPosition()); + motor.angularVelocity(shooter.getCharacterizationVelocity()); + motor.current(shooter.getCharacterizationCurrent()); + }, + shooter, + "FlywheelMotors")); + driverController + .a() + .onTrue( + shooterSysId.dynamic(Direction.kForward).withTimeout(5) + .andThen( + new WaitCommand(5), + shooterSysId.dynamic(Direction.kReverse).withTimeout(5), + new WaitCommand(5), + shooterSysId.quasistatic(Direction.kForward).withTimeout(120), + new WaitCommand(5), + shooterSysId.quasistatic(Direction.kReverse).withTimeout(120) + ) + ); break; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index a4f234af..9fd7f575 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -18,14 +18,14 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.Angle; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.*; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import static edu.wpi.first.units.Units.*; + public class Shooter extends SubsystemBase { private final ShooterIO io; private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); @@ -76,6 +76,13 @@ public void runVolts(double volts) { io.setFlywheelVoltage(volts); } + /** + * Run open loop at the specified voltage. + */ + public void runVoltage(Measure voltage) { + runVolts(voltage.in(Volts)); + } + /** Run closed loop at the specified velocity. */ public void runVelocity(double velocityRPM) { var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); @@ -101,14 +108,33 @@ public double getVelocityRPM() { * Returns the current velocity in radians per second. */ public Measure> getCharacterizationVelocity() { - return edu.wpi.first.units.Units.RadiansPerSecond.of(inputs.flywheelVelocityRadPerSec); + return RadiansPerSecond.of(inputs.flywheelVelocityRadPerSec); } /** * Returns the current velocity in radians per second. */ public Measure getCharacterizationPosition() { - return edu.wpi.first.units.Units.Radians.of(inputs.flywheelPositionRad); + return Radians.of(inputs.flywheelPositionRad); + } + + + /** + * Returns the current velocity in radians per second. + */ + public Measure getCharacterizationVoltage() { + return Volts.of(inputs.flywheelAppliedVolts); + } + + /** + * Returns the current velocity in radians per second. + */ + public Measure getCharacterizationCurrent() { + var sum = 0.0; + for (double flywheelCurrentAmp : inputs.flywheelCurrentAmps) sum += flywheelCurrentAmp; + + sum = (inputs.flywheelCurrentAmps.length > 0) ? sum / inputs.flywheelCurrentAmps.length : 0.0; + return Amps.of(sum); } public void setTargetShooterAngle(Rotation2d anglediff) { From c5145d1b2eced43d815b9cdd4c07bc4e782bcbce Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Sat, 24 Feb 2024 18:40:09 -0500 Subject: [PATCH 36/45] oops Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2185d3b8..25de5574 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,9 +13,6 @@ package frc.robot; -import static edu.wpi.first.units.BaseUnits.Voltage; -import static edu.wpi.first.units.Units.Seconds; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.MathUtil; @@ -27,6 +24,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; @@ -51,6 +49,9 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; +import static edu.wpi.first.units.BaseUnits.Voltage; +import static edu.wpi.first.units.Units.Seconds; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -286,7 +287,7 @@ private void configureButtonBindings() { driverController .rightTrigger() .whileTrue( - new RunCommand(() -> shooter.setTargetShooterAngleRad(new Rotation2d(-0.61))) + new RunCommand(() -> shooter.setTargetShooterAngle(new Rotation2d(-0.61))) .andThen( (new RunCommand( () -> shooter.runVelocity(5000) /*THIS NUMBER NEEDS TO BE CALIBRATED*/, From 8648c074aeb6cff6dda43e735ff97fc871d9a777 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Mon, 26 Feb 2024 10:23:38 -0500 Subject: [PATCH 37/45] cleanup Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- .../frc/robot/commands/IntakeCommand.java | 58 ------------------- 1 file changed, 58 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/IntakeCommand.java diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java deleted file mode 100644 index 8e40efa7..00000000 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ /dev/null @@ -1,58 +0,0 @@ -// package frc.robot.commands; -// -// import com.revrobotics.CANSparkLowLevel; -// import com.revrobotics.CANSparkMax; -// -// -// public class IntakeCommand { -// //private static double DEADBAND = 0.1; -// private CANSparkMax motor; -// CANSparkMax extend = new CANSparkMax(1, CANSparkLowLevel.MotorType.kBrushed); -// CANSparkMax spin = new CANSparkMax(0, CANSparkLowLevel.MotorType.kBrushless); -// public void spin(){ -// -// -// CANSparkMax motor = new CANSparkMax(0, CANSparkLowLevel.MotorType.kBrushless); -// motor.set(0.5); //maybe not 1.0 -// while(true) { //just spin indefinitely -// try{ -// Thread.sleep(100);// pause for a bit -// -// }catch (InterruptedException e){ -// e.printStackTrace(); -// } -// -// -// } -// -// -// } -// public void extend() { -// CANSparkMax motor = new CANSparkMax(1, CANSparkLowLevel.MotorType.kBrushed); -// //motor id and what type of motor it is -// extendMotor(motor); -// while(true) { -// try{ -// Thread.sleep(100); -// }catch(InterruptedException e){ -// e.printStackTrace(); -// -// } -// } -// -// } -// private static void extendMotor(CANSparkMax motor){ -// double motorSpeed = 0.5; -// motor.set(motorSpeed); -// } -// -// -// -// public void periodic(){ //motor constant (extend, spin) -// //idk how to periodic -// } -// -// -// -// -// } From 3003ef7c20db9c8f27fd35c6009735459a5c6658 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Mon, 26 Feb 2024 16:09:34 -0500 Subject: [PATCH 38/45] moved commands to different files Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 75 ++++++++----------- .../frc/robot/commands/IntakeCommands.java | 26 +++++++ .../frc/robot/subsystems/shooter/Shooter.java | 17 +---- 3 files changed, 61 insertions(+), 57 deletions(-) create mode 100644 src/main/java/frc/robot/commands/IntakeCommands.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 25de5574..f551a338 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,6 +31,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.commands.DriveCommands; +import frc.robot.commands.IntakeCommands; import frc.robot.subsystems.climb.Climb; import frc.robot.subsystems.climb.ClimbIO; import frc.robot.subsystems.climb.ClimbIOSparkMax; @@ -212,23 +213,9 @@ private void configureButtonBindings() { // ---- INTAKE COMMANDS ---- driverController - .leftBumper() // not a() - .whileTrue( - new RunCommand( - () -> { - intake.setIntakePosition(Rotation2d.fromDegrees(0.0)); - intake.setRollerPercentage(0.75); - }, - intake)); - driverController - .rightBumper() - .whileTrue( - new RunCommand( - () -> { - intake.setIntakePosition(Rotation2d.fromDegrees(-90.0)); - intake.setRollerPercentage(0.0); - }, - intake)); + .leftTrigger() // not a() + .whileTrue(IntakeCommands.intake(intake)); + driverController.rightBumper().whileTrue(IntakeCommands.idle(intake)); // ---- SHOOTER COMMANDS ---- driverController @@ -294,33 +281,33 @@ private void configureButtonBindings() { intake)))); break; case EverythingElse: - var shooterSysId = - new SysIdRoutine( - new Config(Voltage.per(Units.Second).of(.1), Voltage.of(9.0), Seconds.of(120)), - new Mechanism( - shooter::runVoltage, - (log) -> { - var motor = log.motor("Shooter"); - motor.voltage(shooter.getCharacterizationVoltage()); - motor.angularPosition(shooter.getCharacterizationPosition()); - motor.angularVelocity(shooter.getCharacterizationVelocity()); - motor.current(shooter.getCharacterizationCurrent()); - }, - shooter, - "FlywheelMotors")); - driverController - .a() - .onTrue( - shooterSysId.dynamic(Direction.kForward).withTimeout(5) - .andThen( - new WaitCommand(5), - shooterSysId.dynamic(Direction.kReverse).withTimeout(5), - new WaitCommand(5), - shooterSysId.quasistatic(Direction.kForward).withTimeout(120), - new WaitCommand(5), - shooterSysId.quasistatic(Direction.kReverse).withTimeout(120) - ) - ); + var shooterSysId = + new SysIdRoutine( + new Config(Voltage.per(Units.Second).of(.1), Voltage.of(9.0), Seconds.of(120)), + new Mechanism( + shooter::runVoltage, + (log) -> { + var motor = log.motor("Shooter"); + motor.voltage(shooter.getCharacterizationVoltage()); + motor.angularPosition(shooter.getCharacterizationPosition()); + motor.angularVelocity(shooter.getCharacterizationVelocity()); + motor.current(shooter.getCharacterizationCurrent()); + }, + shooter, + "FlywheelMotors")); + driverController + .a() + .onTrue( + shooterSysId + .dynamic(Direction.kForward) + .withTimeout(5) + .andThen( + new WaitCommand(5), + shooterSysId.dynamic(Direction.kReverse).withTimeout(5), + new WaitCommand(5), + shooterSysId.quasistatic(Direction.kForward).withTimeout(120), + new WaitCommand(5), + shooterSysId.quasistatic(Direction.kReverse).withTimeout(120))); break; } } diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java new file mode 100644 index 00000000..9f250ae9 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -0,0 +1,26 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.subsystems.intake.Intake; + +public class IntakeCommands { + public static Command intake(Intake intake) { + return new RunCommand( + () -> { + intake.setIntakePosition(Rotation2d.fromDegrees(0.0)); + intake.setRollerPercentage(0.75); + }, + intake); + } + + public static Command idle(Intake intake) { + return new RunCommand( + () -> { + intake.setIntakePosition(Rotation2d.fromDegrees(-90.0)); + intake.setRollerPercentage(0.0); + }, + intake); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 9fd7f575..c2efb61b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -104,31 +104,22 @@ public double getVelocityRPM() { return Units.radiansPerSecondToRotationsPerMinute(inputs.flywheelVelocityRadPerSec); } - /** - * Returns the current velocity in radians per second. - */ + /** Returns the current velocity in radians per second. */ public Measure> getCharacterizationVelocity() { return RadiansPerSecond.of(inputs.flywheelVelocityRadPerSec); } - /** - * Returns the current velocity in radians per second. - */ + /** Returns the current velocity in radians per second. */ public Measure getCharacterizationPosition() { return Radians.of(inputs.flywheelPositionRad); } - - /** - * Returns the current velocity in radians per second. - */ + /** Returns the current velocity in radians per second. */ public Measure getCharacterizationVoltage() { return Volts.of(inputs.flywheelAppliedVolts); } - /** - * Returns the current velocity in radians per second. - */ + /** Returns the current velocity in radians per second. */ public Measure getCharacterizationCurrent() { var sum = 0.0; for (double flywheelCurrentAmp : inputs.flywheelCurrentAmps) sum += flywheelCurrentAmp; From 7cd67f6c879b233ae7c7d3bbe53dc0137b0cb24c Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Mon, 26 Feb 2024 16:49:09 -0500 Subject: [PATCH 39/45] feedToBeamBreak Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 5 +++-- src/main/java/frc/robot/commands/IntakeCommands.java | 11 +++++++++-- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f551a338..303d6984 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -214,8 +214,9 @@ private void configureButtonBindings() { // ---- INTAKE COMMANDS ---- driverController .leftTrigger() // not a() - .whileTrue(IntakeCommands.intake(intake)); - driverController.rightBumper().whileTrue(IntakeCommands.idle(intake)); + .whileTrue(IntakeCommands.intakeCommand(intake).alongWith(IntakeCommands.feedToBeamBreak(feeder))) + .onFalse(IntakeCommands.feedToBeamBreak(feeder).withTimeout(5)); + driverController.rightBumper().whileTrue(IntakeCommands.idleCommand(intake)); // ---- SHOOTER COMMANDS ---- driverController diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java index 9f250ae9..28ff440c 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands.java +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -3,10 +3,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.subsystems.feeder.Feeder; import frc.robot.subsystems.intake.Intake; public class IntakeCommands { - public static Command intake(Intake intake) { + public static Command intakeCommand(Intake intake) { return new RunCommand( () -> { intake.setIntakePosition(Rotation2d.fromDegrees(0.0)); @@ -15,7 +16,7 @@ public static Command intake(Intake intake) { intake); } - public static Command idle(Intake intake) { + public static Command idleCommand(Intake intake) { return new RunCommand( () -> { intake.setIntakePosition(Rotation2d.fromDegrees(-90.0)); @@ -23,4 +24,10 @@ public static Command idle(Intake intake) { }, intake); } + + public static Command feedToBeamBreak(Feeder feeder) { + return new RunCommand(() -> { + feeder.runVolts(8); + }, feeder).until(feeder::getSensorFeed); + } } From 44aca1de1f74774249791ba6395068ae3b2fec29 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Mon, 26 Feb 2024 17:17:52 -0500 Subject: [PATCH 40/45] stuff Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 15 ++++----------- .../java/frc/robot/commands/DriveCommands.java | 4 +++- .../java/frc/robot/commands/IntakeCommands.java | 9 ++++++--- 3 files changed, 13 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 303d6984..c0d47914 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -202,19 +202,12 @@ private void configureButtonBindings() { }, drive) .ignoringDisable(true)); - - // ---- FEEDER COMMANDS ---- - driverController - .leftTrigger() - .and(feeder::getSensorFeed) - .whileTrue( - new RunCommand(() -> feeder.runVolts(6.0), feeder) - .until(() -> !feeder.getSensorFeed())); - // ---- INTAKE COMMANDS ---- driverController - .leftTrigger() // not a() - .whileTrue(IntakeCommands.intakeCommand(intake).alongWith(IntakeCommands.feedToBeamBreak(feeder))) + .leftTrigger() + .whileTrue( + IntakeCommands.intakeCommand(intake) + .alongWith(IntakeCommands.feedToBeamBreak(feeder))) .onFalse(IntakeCommands.feedToBeamBreak(feeder).withTimeout(5)); driverController.rightBumper().whileTrue(IntakeCommands.idleCommand(intake)); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index e8d4faa5..91a0658c 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -22,10 +22,12 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.drive.Drive; + import java.util.function.DoubleSupplier; public class DriveCommands { - private static final double DEADBAND = 0.1; + + private static final double DEADBAND = 0.1; private DriveCommands() {} diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java index 28ff440c..f30cf709 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands.java +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -26,8 +26,11 @@ public static Command idleCommand(Intake intake) { } public static Command feedToBeamBreak(Feeder feeder) { - return new RunCommand(() -> { - feeder.runVolts(8); - }, feeder).until(feeder::getSensorFeed); + return new RunCommand( + () -> { + feeder.runVolts(8); + }, + feeder) + .until(feeder::getSensorFeed); } } From f5a7ddf235a8a06d1d4f3403d9dd44a179abfefe Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Mon, 26 Feb 2024 17:28:00 -0500 Subject: [PATCH 41/45] ahhhhhhhh Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 11 +++------ .../frc/robot/subsystems/shooter/Shooter.java | 24 +++++-------------- 2 files changed, 9 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fdc77994..0765a433 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,7 +22,6 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.*; -import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; @@ -42,10 +41,6 @@ import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.intake.IntakeIOSparkMax; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterIO; -import frc.robot.subsystems.shooter.ShooterIOSim; -import frc.robot.subsystems.intake.IntakeIOSparkMax; import frc.robot.subsystems.shooter.*; import frc.robot.util.Mode; import frc.robot.util.ModeHelper; @@ -191,7 +186,7 @@ private void configureButtonBindings() { }, intake)); feeder.setDefaultCommand(new RunCommand(() -> feeder.runVolts(6.0), feeder)); - shooter.setDefaultCommand(new RunCommand(shooter::stop, shooter)); + shooter.setDefaultCommand(new RunCommand(() -> shooter.shooterRunVolts(0.0), shooter)); // CLIMB DEFAULT COMMAND climb.setDefaultCommand( Commands.run( @@ -231,7 +226,7 @@ private void configureButtonBindings() { .rightTrigger() .whileTrue( Commands.run( - () -> shooter.runVolts(driverController.getRightTriggerAxis() * 12.0), + () -> shooter.shooterRunVolts(driverController.getRightTriggerAxis() * 12.0), shooter)); driverController.leftTrigger().and(driverController.rightTrigger().negate()); driverController @@ -245,7 +240,7 @@ private void configureButtonBindings() { .rightTrigger() .whileTrue( new StartEndCommand( - () -> shooter.shooterRunVolts(12.0 * controller.getRightTriggerAxis()), + () -> shooter.shooterRunVolts(12.0 * driverController.getRightTriggerAxis()), () -> { shooter.stopShooter(); shooter.shooterRunVolts(0.0); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index c54c0d6d..fe2442ec 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -135,18 +135,6 @@ public void shooterRunVolts(double volts) { shooterIO.setFlywheelVoltage(volts); } - public Measure getCharacterizationPosition() { - return Radians.of(this.shooterInputs.flywheelPositionRad); - } - - public Measure> getCharacterizationVelocity() { - return RadiansPerSecond.of(this.shooterInputs.flywheelVelocityRadPerSec); - } - - public Measure getCharacterizationCurrent() { - return Amps.of(this.shooterInputs.flywheelCurrentAmps[0]); - } - public Measure getCharacterizationAppliedVolts() { return Volts.of(this.shooterInputs.flywheelAppliedVolts); } @@ -160,7 +148,7 @@ public void hoodRunVolts(double volts) { * Run open loop at the specified voltage. */ public void runVoltage(Measure voltage) { - runVolts(voltage.in(Volts)); + shooterRunVolts(voltage.in(Volts)); } /** Run closed loop at the specified velocity. @@ -195,25 +183,25 @@ public double getShooterVelocityRPM() { /** Returns the current velocity in radians per second. */ public Measure> getCharacterizationVelocity() { - return RadiansPerSecond.of(inputs.flywheelVelocityRadPerSec); + return RadiansPerSecond.of(shooterInputs.flywheelVelocityRadPerSec); } /** Returns the current velocity in radians per second. */ public Measure getCharacterizationPosition() { - return Radians.of(inputs.flywheelPositionRad); + return Radians.of(shooterInputs.flywheelPositionRad); } /** Returns the current velocity in radians per second. */ public Measure getCharacterizationVoltage() { - return Volts.of(inputs.flywheelAppliedVolts); + return Volts.of(shooterInputs.flywheelAppliedVolts); } /** Returns the current velocity in radians per second. */ public Measure getCharacterizationCurrent() { var sum = 0.0; - for (double flywheelCurrentAmp : inputs.flywheelCurrentAmps) sum += flywheelCurrentAmp; + for (double flywheelCurrentAmp : shooterInputs.flywheelCurrentAmps) sum += flywheelCurrentAmp; - sum = (inputs.flywheelCurrentAmps.length > 0) ? sum / inputs.flywheelCurrentAmps.length : 0.0; + sum = (shooterInputs.flywheelCurrentAmps.length > 0) ? sum / shooterInputs.flywheelCurrentAmps.length : 0.0; return Amps.of(sum); } From aaa09574b97b685063da5b8048972332864932d8 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Tue, 27 Feb 2024 14:25:23 -0500 Subject: [PATCH 42/45] stuff Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/commands/IntakeCommands.java | 2 +- .../frc/robot/subsystems/feeder/Feeder.java | 10 ++---- .../frc/robot/subsystems/feeder/FeederIO.java | 12 ++++--- .../subsystems/feeder/FeederIOSparkMax.java | 12 +++++-- .../subsystems/feeder/FeederIOTalonFX.java | 34 +++++++++++-------- 6 files changed, 42 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0765a433..10caae99 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -185,7 +185,7 @@ private void configureButtonBindings() { intake.setRollerPercentage(0.0); }, intake)); - feeder.setDefaultCommand(new RunCommand(() -> feeder.runVolts(6.0), feeder)); + feeder.setDefaultCommand(new RunCommand(() -> feeder.runVolts(6.0), feeder).onlyWhile(() -> !feeder.getBeamBroken())); shooter.setDefaultCommand(new RunCommand(() -> shooter.shooterRunVolts(0.0), shooter)); // CLIMB DEFAULT COMMAND climb.setDefaultCommand( diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java index f30cf709..95ea0a87 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands.java +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -31,6 +31,6 @@ public static Command feedToBeamBreak(Feeder feeder) { feeder.runVolts(8); }, feeder) - .until(feeder::getSensorFeed); + .until(feeder::getBeamBroken); } } diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index b7b404ad..61344916 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import org.littletonrobotics.junction.AutoLogOutput; @@ -17,16 +16,11 @@ public class Feeder extends SubsystemBase { private final ProfiledPIDController pidController; double offset = 0.0; - private DigitalInput conveyorSensor; - - private static final int conveyorSensorNum = 0; - - public boolean getSensorFeed() { - return conveyorSensor.get(); + public boolean getBeamBroken() { + return !inputs.beamUnobstructed; } public Feeder(FeederIO io) { - conveyorSensor = new DigitalInput(conveyorSensorNum); this.io = io; // Switch constants based on mode (the physics simulator is treated as a diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java index 7f73f34f..c98b0d1e 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java @@ -5,16 +5,20 @@ public interface FeederIO { /** Updates the set of loggable inputs. */ - public default void updateInputs(FeederIOInputs inputs) {} + default void updateInputs(FeederIOInputs inputs) { + } /** Run open loop at the specified voltage. */ - public default void setVoltage(double volts) {} + default void setVoltage(double volts) { + } /** Stop in open loop. */ - public default void stop() {} + default void stop() { + } @AutoLog - public static class FeederIOInputs { + class FeederIOInputs { + public boolean beamUnobstructed = false; public double positionRad = 0.0; public double velocityRadPerSec = 0.0; public double appliedVolts = 0.0; diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkMax.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkMax.java index d2472b84..23a371e6 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkMax.java @@ -1,13 +1,16 @@ package frc.robot.subsystems.feeder; -import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; - import com.revrobotics.CANSparkMax; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DigitalInput; + +import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; public class FeederIOSparkMax implements FeederIO { private static final double GEAR_RATIO = 1.0; CANSparkMax feeder = new CANSparkMax(0, kBrushless); + private static final int conveyorSensorNum = 9; + private DigitalInput conveyorSensor; public FeederIOSparkMax() { feeder.restoreFactoryDefaults(); @@ -20,6 +23,9 @@ public FeederIOSparkMax() { feeder.setSmartCurrentLimit(30); feeder.burnFlash(); + + DigitalInput + conveyorSensor = new DigitalInput(conveyorSensorNum); } @Override @@ -30,6 +36,8 @@ public void updateInputs(FeederIOInputs inputs) { inputs.appliedVolts = feeder.getAppliedOutput() * feeder.getBusVoltage(); inputs.currentAmps = new double[] {feeder.getOutputCurrent()}; inputs.temperature = new double[] {feeder.getMotorTemperature()}; + + inputs.beamUnobstructed = conveyorSensor.get(); } @Override diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java index ae71470b..b2d330eb 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java @@ -7,14 +7,17 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; -import frc.robot.subsystems.shooter.ShooterIO; +import edu.wpi.first.wpilibj.DigitalInput; + +import static edu.wpi.first.math.util.Units.rotationsToRadians; public class FeederIOTalonFX implements FeederIO { - @Override - public void updateInputs(FeederIOInputs inputs) { - FeederIO.super.updateInputs(inputs); - } + + private static final int conveyorSensorNum = 9; + private DigitalInput conveyorSensor; + + + private final TalonFX feedMotor = new TalonFX(43); private final StatusSignal feedMotorPosition = feedMotor.getPosition(); @@ -24,6 +27,7 @@ public void updateInputs(FeederIOInputs inputs) { private final StatusSignal feedMotorTemperature = feedMotor.getDeviceTemp(); public FeederIOTalonFX() { + conveyorSensor = new DigitalInput(conveyorSensorNum); var config = new TalonFXConfiguration(); config.CurrentLimits.StatorCurrentLimit = 50.0; config.CurrentLimits.StatorCurrentLimitEnable = true; @@ -37,23 +41,25 @@ public FeederIOTalonFX() { feedMotorVelocity, feedMotorAppliedVolts, feedMotorCurrent, - feedMotorCurrent); + feedMotorTemperature); feedMotor.optimizeBusUtilization(); } - public void updateInputs(ShooterIO.ShooterIOInputs inputs) { + public void updateInputs(FeederIO.FeederIOInputs inputs) { BaseStatusSignal.refreshAll( feedMotorPosition, feedMotorVelocity, feedMotorAppliedVolts, feedMotorCurrent, feedMotorTemperature); - inputs.flywheelPositionRad = Units.rotationsToRadians(feedMotorPosition.getValueAsDouble()); - inputs.flywheelVelocityRadPerSec = - Units.rotationsToRadians(feedMotorVelocity.getValueAsDouble()); - inputs.flywheelAppliedVolts = feedMotorAppliedVolts.getValueAsDouble(); - inputs.flywheelCurrentAmps = new double[] {feedMotorCurrent.getValueAsDouble()}; - inputs.flywheelTemperature = new double[] {feedMotorTemperature.getValueAsDouble()}; + inputs.positionRad = rotationsToRadians(feedMotorPosition.getValueAsDouble()); + inputs.velocityRadPerSec = + rotationsToRadians(feedMotorVelocity.getValueAsDouble()); + inputs.appliedVolts = feedMotorAppliedVolts.getValueAsDouble(); + inputs.currentAmps = new double[]{feedMotorCurrent.getValueAsDouble()}; + inputs.temperature = new double[]{feedMotorTemperature.getValueAsDouble()}; + + inputs.beamUnobstructed = conveyorSensor.get(); } @Override From 9376143679108c8545e53efe656598c3acdc749d Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Tue, 27 Feb 2024 17:13:45 -0500 Subject: [PATCH 43/45] changes Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 63 +++++++------------ .../frc/robot/commands/FeederCommands.java | 17 +++++ .../frc/robot/commands/IntakeCommands.java | 2 +- .../frc/robot/subsystems/feeder/Feeder.java | 13 ++-- .../subsystems/feeder/FeederIOTalonFX.java | 5 +- .../frc/robot/subsystems/shooter/Shooter.java | 38 +++++------ .../subsystems/shooter/ShooterIOTalonFX.java | 8 +-- 7 files changed, 76 insertions(+), 70 deletions(-) create mode 100644 src/main/java/frc/robot/commands/FeederCommands.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 10caae99..c90f6800 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.commands.DriveCommands; +import frc.robot.commands.FeederCommands; import frc.robot.commands.IntakeCommands; import frc.robot.subsystems.climb.Climb; import frc.robot.subsystems.climb.ClimbIO; @@ -84,7 +85,7 @@ public Command getExitCommand(Mode m) { private final LoggedDashboardNumber flywheelSpeedInput = new LoggedDashboardNumber("Flywheel Speed", 1500.0); - public static SysIDMode sysIDMode = SysIDMode.Disabled; + public static SysIDMode sysIDMode = SysIDMode.EverythingElse; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -185,8 +186,11 @@ private void configureButtonBindings() { intake.setRollerPercentage(0.0); }, intake)); - feeder.setDefaultCommand(new RunCommand(() -> feeder.runVolts(6.0), feeder).onlyWhile(() -> !feeder.getBeamBroken())); - shooter.setDefaultCommand(new RunCommand(() -> shooter.shooterRunVolts(0.0), shooter)); + feeder.setDefaultCommand(new RunCommand(() -> feeder.runVolts(0.0), feeder)); + shooter.setDefaultCommand(new RunCommand(() -> { + shooter.shooterRunVolts(0.0); + shooter.setTargetShooterAngle(Rotation2d.fromRadians(.5)); + }, shooter)); // CLIMB DEFAULT COMMAND climb.setDefaultCommand( Commands.run( @@ -222,38 +226,15 @@ private void configureButtonBindings() { driverController.rightBumper().whileTrue(IntakeCommands.idleCommand(intake)); // ---- SHOOTER COMMANDS ---- - driverController - .rightTrigger() + operatorController + .y() .whileTrue( Commands.run( - () -> shooter.shooterRunVolts(driverController.getRightTriggerAxis() * 12.0), - shooter)); - driverController.leftTrigger().and(driverController.rightTrigger().negate()); - driverController - .b() - .whileTrue( - Commands.startEnd( - () -> shooter.shooterRunVelocity(flywheelSpeedInput.get()), - shooter::stopShooter, - shooter)); - driverController - .rightTrigger() - .whileTrue( - new StartEndCommand( - () -> shooter.shooterRunVolts(12.0 * driverController.getRightTriggerAxis()), - () -> { - shooter.stopShooter(); - shooter.shooterRunVolts(0.0); - }, - shooter)); - driverController - .x() - .whileTrue( - Commands.startEnd( - () -> shooter.shooterRunVelocity(flywheelSpeedInput.get()), - shooter::stopShooter, - shooter)); - + () -> { + shooter.shooterRunVelocity(3000); + shooter.setTargetShooterAngle(Rotation2d.fromRadians(0.0)); + }, + shooter).andThen(new WaitUntilCommand(() -> (Math.abs(3000 - shooter.getShooterVelocityRPM()) < 50.0)).andThen(FeederCommands.feedToShooter(feeder)))); break; case DriveMotors: drive.setDefaultCommand( @@ -299,7 +280,7 @@ private void configureButtonBindings() { case EverythingElse: var shooterSysId = new SysIdRoutine( - new Config(Voltage.per(Units.Second).of(.1), Voltage.of(9.0), Seconds.of(120)), + new Config(Voltage.per(Units.Second).of(.25), Voltage.of(9.0), Seconds.of(36)), new Mechanism( shooter::shooterRunVolts, (log) -> { @@ -309,21 +290,25 @@ private void configureButtonBindings() { motor.angularVelocity(shooter.getCharacterizationVelocity()); motor.current(shooter.getCharacterizationCurrent()); }, - shooter, + climb, "FlywheelMotors")); driverController .a() .onTrue( shooterSysId .dynamic(Direction.kForward) - .withTimeout(5) + .withTimeout(3) .andThen( new WaitCommand(5), - shooterSysId.dynamic(Direction.kReverse).withTimeout(5), + shooterSysId.dynamic(Direction.kReverse).withTimeout(3), new WaitCommand(5), - shooterSysId.quasistatic(Direction.kForward).withTimeout(120), + shooterSysId.quasistatic(Direction.kForward).withTimeout(36), new WaitCommand(5), - shooterSysId.quasistatic(Direction.kReverse).withTimeout(120))); + shooterSysId.quasistatic(Direction.kReverse).withTimeout(36)) + .alongWith(new RunCommand(() -> { + shooter.setTargetShooterAngle(Rotation2d.fromRadians(0.0)); + shooter.setCharacterizeMode(true); + }))); break; } } diff --git a/src/main/java/frc/robot/commands/FeederCommands.java b/src/main/java/frc/robot/commands/FeederCommands.java new file mode 100644 index 00000000..f900e189 --- /dev/null +++ b/src/main/java/frc/robot/commands/FeederCommands.java @@ -0,0 +1,17 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.subsystems.feeder.Feeder; + +public class FeederCommands { + public static Command feedToShooter(Feeder feeder) { + return new RunCommand( + () -> { + feeder.runVolts(8); + }, + feeder) + .onlyIf(() -> feeder.getBeamBroken()) + .until(() -> !feeder.getBeamBroken()); + } +} diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java index 95ea0a87..c3f75a3b 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands.java +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -31,6 +31,6 @@ public static Command feedToBeamBreak(Feeder feeder) { feeder.runVolts(8); }, feeder) - .until(feeder::getBeamBroken); + .onlyWhile(() -> !feeder.getBeamBroken()); } } diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index 61344916..d0d6aea7 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -9,6 +10,8 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import static edu.wpi.first.math.filter.Debouncer.DebounceType.kBoth; + public class Feeder extends SubsystemBase { private final FeederIO io; private final FeederIOInputsAutoLogged inputs = new FeederIOInputsAutoLogged(); @@ -16,8 +19,10 @@ public class Feeder extends SubsystemBase { private final ProfiledPIDController pidController; double offset = 0.0; + Debouncer debouncer = new Debouncer(.05, kBoth); + public boolean getBeamBroken() { - return !inputs.beamUnobstructed; + return !debouncer.calculate(inputs.beamUnobstructed); } public Feeder(FeederIO io) { @@ -57,9 +62,9 @@ public Feeder(FeederIO io) { @Override public void periodic() { io.updateInputs(inputs); - io.setVoltage( - pidController.calculate(inputs.positionRad) - + ffModel.calculate(pidController.getSetpoint().velocity)); +// io.setVoltage( +// pidController.calculate(inputs.positionRad) +// + ffModel.calculate(pidController.getSetpoint().velocity)); Logger.processInputs("Flywheel", inputs); } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java index b2d330eb..1e4cda1d 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java @@ -3,7 +3,6 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -29,7 +28,7 @@ public class FeederIOTalonFX implements FeederIO { public FeederIOTalonFX() { conveyorSensor = new DigitalInput(conveyorSensorNum); var config = new TalonFXConfiguration(); - config.CurrentLimits.StatorCurrentLimit = 50.0; + config.CurrentLimits.StatorCurrentLimit = 30.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.MotorOutput.withInverted(InvertedValue.Clockwise_Positive); config.MotorOutput.NeutralMode = NeutralModeValue.Coast; @@ -64,7 +63,7 @@ public void updateInputs(FeederIO.FeederIOInputs inputs) { @Override public void setVoltage(double volts) { - feedMotor.setControl(new VoltageOut(volts)); + feedMotor.setVoltage(volts); } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index fe2442ec..dcc67ac8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -30,8 +30,6 @@ import static edu.wpi.first.units.Units.*; public class Shooter extends SubsystemBase { - private static final double HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_RATIO = 1.0; - private static final double HOOD_ENCODER_ANGLE_TO_REAL_ANGLE_OFFSET = 0.0; // the ratio for turning the shooter // private static final double TURN_SHOOTER_RATIO = 5.4; private static double targetHoodAngleRad = 0.0; @@ -41,10 +39,11 @@ public class Shooter extends SubsystemBase { private final HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); private final ArmFeedforward hoodFF; private PIDController shooterVelocityFB; - private ProfiledPIDController shooterPositionFB; - private SimpleMotorFeedforward shooterVelocityFF; + double setpointRadPS = 0; private ProfiledPIDController hoodFB; - private boolean characterizeMode; + // private ProfiledPIDController shooterPositionFB; + private SimpleMotorFeedforward shooterVelocityFF; + private boolean characterizeMode = false; /** * Creates a new Shooter. @@ -57,15 +56,15 @@ public Shooter(ShooterIO io, HoodIO hoodIO) { switch (Constants.currentMode) { case REAL: hoodFB = new ProfiledPIDController(2.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 2)); - if (shooterIO instanceof ShooterIOTalonFX) { +// if (shooterIO instanceof ShooterIOTalonFX) { shooterVelocityFB = - new PIDController(0.0050812, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); + new PIDController(0.0010812, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); shooterVelocityFB.setTolerance(25); - shooterPositionFB = - new ProfiledPIDController(20.817, 0.0, 1.7743, new TrapezoidProfile.Constraints(RadiansPerSecond.of(90), RadiansPerSecond.per(Second).of(27.8736842105))); - shooterPositionFB.setTolerance(0.26, 5); - shooterVelocityFF = new SimpleMotorFeedforward(0.10548, 0.11959, 0.066251); - } +// shooterPositionFB = +// new ProfiledPIDController(20.817, 0.0, 1.7743, new TrapezoidProfile.Constraints(RadiansPerSecond.of(90), RadiansPerSecond.per(Second).of(27.8736842105))); +// shooterPositionFB.setTolerance(0.26, 5); + shooterVelocityFF = new SimpleMotorFeedforward(.998, .0706 * .5, .16); +// } // FIXME: characterize real robot hoodFF = new ArmFeedforward(0.0, 0.0, 0); @@ -77,9 +76,9 @@ public Shooter(ShooterIO io, HoodIO hoodIO) { shooterVelocityFB = new PIDController(0.0050812, 0.0, 0.0 /*, new TrapezoidProfile.Constraints(0.5, 99)*/); shooterVelocityFB.setTolerance(25); - shooterPositionFB = - new ProfiledPIDController(20.817, 0.0, 1.7743, new TrapezoidProfile.Constraints(RadiansPerSecond.of(90), RadiansPerSecond.per(Second).of(27.8736842105))); - shooterPositionFB.setTolerance(0.26, 5); +// shooterPositionFB = +// new ProfiledPIDController(20.817, 0.0, 1.7743, new TrapezoidProfile.Constraints(RadiansPerSecond.of(90), RadiansPerSecond.per(Second).of(27.8736842105))); +// shooterPositionFB.setTolerance(0.26, 5); shooterVelocityFF = new SimpleMotorFeedforward(0.10548, 0.11959, 0.066251); break; case SIM: @@ -99,11 +98,12 @@ public Shooter(ShooterIO io, HoodIO hoodIO) { public void periodic() { shooterIO.updateInputs(shooterInputs); hoodIO.updateInputs(hoodInputs); - if (characterizeMode) { + if (!characterizeMode) { shooterIO.setFlywheelVoltage( - shooterVelocityFB.calculate(shooterInputs.flywheelVelocityRadPerSec) - + this.shooterVelocityFF.calculate(shooterInputs.flywheelVelocityRadPerSec)); + shooterVelocityFB.calculate(shooterInputs.flywheelVelocityRadPerSec, setpointRadPS) + + this.shooterVelocityFF.calculate(shooterVelocityFB.getSetpoint())); } + Logger.recordOutput("shooterSpeed", setpointRadPS); Logger.recordOutput("targetHoodAngle", targetHoodAngleRad); Logger.recordOutput("hoodInputs.hoodPositionRad", hoodInputs.hoodPositionRad); Logger.recordOutput("pidStuff", "" + hoodFB.getD() + " " + hoodFB.getI() + " " + hoodFB.getP()); @@ -156,7 +156,7 @@ public void runVoltage(Measure voltage) { public void shooterRunVelocity(double velocityRPM) { var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - shooterVelocityFB.setSetpoint(velocityRadPerSec); + setpointRadPS = velocityRadPerSec; // Log flywheel setpoint Logger.recordOutput("Shooter/SetpointRPM", velocityRPM); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 67b9e71a..06c10c79 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -22,7 +22,7 @@ import edu.wpi.first.math.util.Units; public class ShooterIOTalonFX implements ShooterIO { - private static final double GEAR_RATIO = 1.0; + private static final double GEAR_RATIO = 1.5; private final TalonFX leader = new TalonFX(40); private final TalonFX follower = new TalonFX(41); @@ -41,7 +41,7 @@ public class ShooterIOTalonFX implements ShooterIO { public ShooterIOTalonFX() { var config = new TalonFXConfiguration(); - config.CurrentLimits.StatorCurrentLimit = 50.0; + config.CurrentLimits.StatorCurrentLimit = 80.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.MotorOutput.NeutralMode = NeutralModeValue.Coast; leader.getConfigurator().apply(config); @@ -67,9 +67,9 @@ public void updateInputs(ShooterIOInputs inputs) { leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent, leaderDeviceTemp, leaderAncillaryDeviceTemp, leaderProcessorTemp, followerDeviceTemp, followerAncillaryDeviceTemp, followerProcessorTemp); - inputs.flywheelPositionRad = leaderPosition.getValueAsDouble(); + inputs.flywheelPositionRad = leaderPosition.getValueAsDouble() * GEAR_RATIO; inputs.flywheelVelocityRadPerSec = - Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / GEAR_RATIO; + Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) * GEAR_RATIO; inputs.flywheelAppliedVolts = leaderAppliedVolts.getValueAsDouble(); inputs.flywheelCurrentAmps = new double[] {leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()}; From 6a42d4c41ac2421c9954b1cdc1606afd5af0f20b Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Tue, 27 Feb 2024 19:01:24 -0500 Subject: [PATCH 44/45] added outline viewer stuff and settings for pathplanner Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- .gitignore | 3 ++- .outlineviewer/outlineviewer.json | 9 +++++++++ .pathplanner/settings.json | 14 ++++++++++++++ 3 files changed, 25 insertions(+), 1 deletion(-) create mode 100644 .outlineviewer/outlineviewer.json create mode 100644 .pathplanner/settings.json diff --git a/.gitignore b/.gitignore index 2f4f2e21..a1027524 100644 --- a/.gitignore +++ b/.gitignore @@ -177,4 +177,5 @@ src/main/java/frc/robot/BuildConstants.java # Network Tables and similar logs networktables.json logs/ -.sysid/ \ No newline at end of file +.sysid/ +/.outlineviewer/outlineviewer-window.json diff --git a/.outlineviewer/outlineviewer.json b/.outlineviewer/outlineviewer.json new file mode 100644 index 00000000..39fee8f3 --- /dev/null +++ b/.outlineviewer/outlineviewer.json @@ -0,0 +1,9 @@ +{ + "Connections": { + "open": true + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "6502" + } +} diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 00000000..22c0f6d6 --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,14 @@ +{ + "robotWidth": 0.85, + "robotLength": 0.85, + "holonomicMode": true, + "pathFolders": [ + "choreo" + ], + "autoFolders": [], + "defaultMaxVel": 4.0, + "defaultMaxAccel": 79.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.5 +} From dc7e386ba419055ba1d1615bad7d6e57e15e6fcf Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Tue, 27 Feb 2024 19:01:36 -0500 Subject: [PATCH 45/45] changes i think idk Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com> --- build.gradle | 2 +- src/main/java/frc/robot/RobotContainer.java | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/build.gradle b/build.gradle index 3ea2a9f8..2a33926c 100755 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.2.1" + id "edu.wpi.first.GradleRIO" version "2024.3.1" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.12.0" } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c90f6800..e69aa9d8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -85,7 +85,7 @@ public Command getExitCommand(Mode m) { private final LoggedDashboardNumber flywheelSpeedInput = new LoggedDashboardNumber("Flywheel Speed", 1500.0); - public static SysIDMode sysIDMode = SysIDMode.EverythingElse; + public static SysIDMode sysIDMode = SysIDMode.Shooter; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -156,13 +156,6 @@ public RobotContainer() { configureButtonBindings(); } - public enum SysIDMode { - Disabled, - DriveMotors, - TurnMotors, - EverythingElse - } - /** * Use this method to define your button->command mappings. Buttons can be created by * instantiating a {@link GenericHID} or one of its subclasses ({@link @@ -277,7 +270,7 @@ private void configureButtonBindings() { shooter.shooterRunVelocity(5000), //THIS NUMBER NEEDS TO BE CALIBRATED intake)))); break; - case EverythingElse: + case Shooter: var shooterSysId = new SysIdRoutine( new Config(Voltage.per(Units.Second).of(.25), Voltage.of(9.0), Seconds.of(36)), @@ -313,6 +306,13 @@ private void configureButtonBindings() { } } + public enum SysIDMode { + Disabled, + DriveMotors, + TurnMotors, + Shooter + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. *