diff --git a/advantagescope-configs/scope-layout/config.json b/advantagescope-configs/scope-layout/config.json index 7cd2df5d..4184c8ba 100644 --- a/advantagescope-configs/scope-layout/config.json +++ b/advantagescope-configs/scope-layout/config.json @@ -1,10 +1,10 @@ { "hubs": [ { - "x": 133, - "y": 35, + "x": 119, + "y": 14, "width": 1100, - "height": 650, + "height": 560, "state": { "sidebar": { "width": 300, @@ -12,13 +12,14 @@ "/AdvantageKit", "/AdvantageKit/RealOutputs", "/AdvantageKit/telemetry", - "/AdvantageKit/scoring", "/telemetry", "/RealOutputs", "/AdvantageKit/RealOutputs/scoring", "/telemetry/Pose3d", "/telemetry/Pose3d/translation", - "/AdvantageKit/RealOutputs/endgame" + "/AdvantageKit/RealOutputs/endgame", + "/AdvantageKit/intake", + "/AdvantageKit/RealOutputs/Scoring" ] }, "tabs": { @@ -37,7 +38,66 @@ "type": null, "factor": 1 }, - "fields": [] + "fields": [ + { + "key": "NT:/AdvantageKit/endgame/Position", + "color": "#2b66a2", + "show": true + } + ] + }, + "discrete": { + "fields": [ + { + "key": "NT:/AdvantageKit/RealOutputs/scoring/State", + "color": "#e5b31b", + "show": true + } + ] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/AdvantageKit/RealOutputs/Scoring/elevatorLimit", + "color": "#af2437", + "show": true + }, + { + "key": "NT:/AdvantageKit/scoring/aimer/AimAngleRad", + "color": "#80588e", + "show": true + } + ] + } + }, + "title": "Line Graph" + }, + { + "type": 1, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/AdvantageKit/scoring/aimer/AimAngleRad", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:/AdvantageKit/scoring/aimer/AimGoalAngleRad", + "color": "#e5b31b", + "show": true + } + ] }, "discrete": { "fields": [] @@ -53,6 +113,27 @@ }, "title": "Line Graph" }, + { + "type": 8, + "uuid": "h88lmidg3e3b5f0d0ni8y5fo0ud1a1zn", + "fields": [], + "listFields": [], + "options": { + "ids": [ + 0, + 1, + 2 + ], + "layouts": [ + "None", + "None", + "Xbox Controller (Blue)" + ] + }, + "configHidden": false, + "visualizer": null, + "title": "Joysticks" + }, { "type": 6, "uuid": "q06ytxmrkaenac0ijl343nag2sax99il", @@ -98,14 +179,14 @@ "cameraIndex": -1, "orbitFov": 50, "cameraPosition": [ - 1.4695761589768194e-15, - 5.999999999999987, - -11.999999999999964 + 10.102714356287965, + 0.26622930405943956, + -2.8352716372526316 ], "cameraTarget": [ - 0, - 0.5, - 0 + 1.619835703195438, + -0.10624782552698095, + -5.541832675475496 ] }, "title": "3D Field" diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3cb..536283a7 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -92,6 +92,13 @@ "robotJoysticks": [ { "guid": "Keyboard0" + }, + { + "guid": "Keyboard2" + }, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/simgui.json b/simgui.json index 89014be5..073dce1c 100644 --- a/simgui.json +++ b/simgui.json @@ -32,6 +32,7 @@ "/SmartDashboard/Module 2": "Mechanism2d", "/SmartDashboard/Module 3": "Mechanism2d", "/SmartDashboard/PhotonSimField": "Field2d", + "/SmartDashboard/Test Mode Chooser": "String Chooser", "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" }, "windows": { diff --git a/src/main/deploy/controllerconfig/SimJoysticks.json b/src/main/deploy/controllerconfig/SimJoysticks.json new file mode 100644 index 00000000..9d503f6f --- /dev/null +++ b/src/main/deploy/controllerconfig/SimJoysticks.json @@ -0,0 +1,81 @@ +{ + "controllers": [ + { + "port": 0, + "type": "joystick" + }, + { + "port": 1, + "type": "joystick" + } + ], + + "buttons": [ + { "command": "intake", + "controller": 0, + "button": "1" + }, + { + "command": "aimSpeaker", + "controller": 0, + "button": "2" + }, + { + "command": "aimAmp", + "controller": 0, + "button": "3" + }, + { + "command": "manualIdle", + "controller": 0, + "button": "4" + }, + { + "command": "align", + "controller": 1, + "button": "1" + }, + { + "command": "shoot", + "controller": 1, + "button": "2" + }, + { + + "command": "endgame", + "controller": 1, + "button": "3" + }, + { + "command": "toggleAlign", + "controller": 1, + "button": "4" + } + ], + + "axes": [ + { "command": "xDrive", + "controller": 0, + "axis": "yAxis", + "negate": true + }, + { + "command": "yDrive", + "controller": 0, + "axis": "xAxis", + "negate": true + }, + { + "command": "rotation", + "controller": 1, + "axis": "xAxis", + "negate": true + } + ], + + "pov": [ + { "command": "pov", + "controller": 0 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/controllerconfig/SingleController.json b/src/main/deploy/controllerconfig/SingleController.json new file mode 100644 index 00000000..bb75ee6d --- /dev/null +++ b/src/main/deploy/controllerconfig/SingleController.json @@ -0,0 +1,87 @@ +{ + "controllers": [ + { + "port": 2, + "type": "gamepad" + } + ], + + "buttons": [ + { "command": "intake", + "controller": 2, + "button": "a" + }, + { + "command": "aimSpeaker", + "controller": 2, + "button": "b" + }, + { + "command": "aimAmp", + "controller": 2, + "button": "back" + }, + { + "command": "stopIntake", + "controller": 2, + "button": "start" + }, + { + "command": "align", + "controller": 2, + "button": "rightBumper" + }, + { + "command": "shoot", + "controller": 2, + "button": "x" + }, + { + + "command": "endgame", + "controller": 2, + "button": "y" + }, + { + "command": "fieldRelative", + "controller": 2, + "button": "leftBumper" + }, + { + "command": "endgame1", + "controller": 2, + "button": "pov0" + }, + { + "command": "endgame2", + "controller": 2, + "button": "pov180" + } + ], + + "axes": [ + { "command": "xDrive", + "controller": 2, + "axis": "leftY", + "negate": false + }, + { + "command": "yDrive", + "controller": 2, + "axis": "leftX", + "negate": false + }, + { + "command": "rotation", + "controller": 2, + "axis": "rightX", + "negate": true + } + ], + + "pov": [ + { "command": "pov", + "controller": 2 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/controllerconfig/SingleControllerTestMode.json b/src/main/deploy/controllerconfig/SingleControllerTestMode.json new file mode 100644 index 00000000..a3bbdc07 --- /dev/null +++ b/src/main/deploy/controllerconfig/SingleControllerTestMode.json @@ -0,0 +1,77 @@ +{ + "controllers": [ + { + "port": 2, + "type": "gamepad" + } + ], + + "buttons": [ + { "command": "a", + "controller": 2, + "button": "a" + }, + { + "command": "b", + "controller": 2, + "button": "b" + }, + { + "command": "right", + "controller": 2, + "button": "rightBumper" + }, + { + "command": "x", + "controller": 2, + "button": "x" + }, + { + + "command": "y", + "controller": 2, + "button": "y" + }, + { + "command": "left", + "controller": 2, + "button": "leftBumper" + }, + { + "command": "povUp", + "controller": 2, + "button": "pov0" + } + ], + + "axes": [ + { "command": "xDrive", + "controller": 2, + "axis": "leftY", + "negate": false + }, + { + "command": "yDrive", + "controller": 2, + "axis": "leftX", + "negate": false + }, + { + "command": "rotation", + "controller": 2, + "axis": "rightX", + "negate": true + }, + { + "command": "povDown", + "controller": 2, + "button": "pov180" + } + ], + + "pov": [ + { "command": "pov", + "controller": 2 + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cbabb2ea..41428405 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -48,10 +48,10 @@ public static enum Mode { public static final class FeatureFlags { public static final boolean runVision = false; - public static final boolean runIntake = false; + public static final boolean runIntake = true; public static final boolean runScoring = true; public static final boolean runEndgame = false; - public static final boolean runDrive = false; + public static final boolean runDrive = true; public static final boolean enableLEDS = false; } @@ -70,7 +70,7 @@ public static final class ConversionConstants { public static final class CANDevices {} public static final class SensorConstants { - public static final int bannerPort = 1; // TODO: Change this + public static final int upperBannerPort = 1; } public static final class DriveConstants { @@ -195,24 +195,23 @@ private static AprilTagFieldLayout initLayout(String name) { } public static final class IntakeConstants { - public static final int leftIntakeMotorID = 0; - public static final int rightIntakeMotorID = 0; - public static final int frontBeltMotorID = 0; - public static final int backBeltMotorID = 0; + public static final int leftIntakeMotorID = 9; + public static final int rightIntakeMotorID = 10; + public static final int indexTwoMotorID = 14; - public static final int bannerSensorID = 0; + public static final int bannerSensorID = 2; - public static final double intakePower = 5.0; - public static final double beltPower = 5.0; + public static final double intakePower = 7.0; + public static final double beltPower = 7.0; } public static final class EndgameConstants { - public static final int leftMotorID = 1; - public static final int rightMotorID = 2; + public static final int leftMotorID = 18; + public static final int rightMotorID = 19; - public static final int smartCurrentLimit = 25; // TODO: Tune this + public static final int smartCurrentLimit = 50; - public static final double encoderToMeters = Math.PI * Units.inchesToMeters(1.7567) / 80; + public static final double encoderToMeters = Math.PI * Units.inchesToMeters(1.7567) / 20.0; } public static final class TunerConstants { @@ -382,12 +381,12 @@ public static final class TunerConstants { } public static final class ScoringConstants { - public static final double aimerkP = 10.0; - public static final double aimerkI = 0.0; + public static final double aimerkP = 15.0; + public static final double aimerkI = 5.0; public static final double aimerkD = 0.0; - public static final double aimerkS = 0.0; // TODO: Find Imperically - public static final double aimerkG = 0.17; + public static final double aimerkS = 0.265; + public static final double aimerkG = 0.095; public static final double aimerkV = 1.51; public static final double aimerkA = 0.01; @@ -406,36 +405,36 @@ public static final class ScoringConstants { public static final double hoodPositionTolerance = 0.01; - public static final double hoodEncoderToRad = 0.0; + public static final double hoodEncoderToRad = (0.3947368421) * (2.0 * Math.PI); - public static final int aimLeftMotorId = 9; - public static final int aimRightMotorId = 10; + public static final int aimLeftMotorId = 16; + public static final int aimRightMotorId = 15; public static final int shooterLeftMotorId = 11; public static final int shooterRightMotorId = 12; public static final int kickerMotorId = 13; - public static final int hoodId = 14; + public static final int hoodId = 17; - public static final int aimEncoderPort = 0; // TODO: Change + public static final int aimEncoderPort = 0; + public static final double aimerEncoderOffset = 1.78; - public static final double aimAcceleration = 10.0; - public static final double aimCruiseVelocity = 10.0; + public static final double aimPositionTolerance = 0.015; - public static final double shooterAcceleration = 1; - public static final double shooterJerk = 1; + public static final double aimAcceleration = 5.0; // TODO: 15.0 + public static final double aimCruiseVelocity = 5.0; // TODO: 15.0 public static final double shooterVelocityMarginRPM = 10; public static final double aimAngleMarginRadians = Units.degreesToRadians(5); public static final double hoodAngleMarginRadians = Units.degreesToRadians(5); - public static final double intakeAngleToleranceRadians = - Math.PI / 2 - Units.degreesToRadians(40); + public static final double intakeAngleToleranceRadians = 0.0; + // Math.PI / 2 - Units.degreesToRadians(40); public static final double shooterAmpVelocityRPM = 10; - public static final double hoodHomeAmps = 10.0; // TODO: Find this + public static final double hoodHomeAmps = 40.0; // TODO: Find this public static final double hoodHomeAngleRad = Math.PI; public static final double aimMaxAngleRadians = Math.PI / 2; @@ -521,6 +520,21 @@ public static HashMap timeToPutAimDownMap() { // TODO: Find this return map; } + + // NOTE - This should be monotonically increasing + // Key - Elevator position in meters + // Value - Aimer angle in radians + public static HashMap aimerAvoidElevatorTable() { + HashMap map = new HashMap(); + map.put(0.0, 0.0); + map.put(0.01, Math.PI / 8); + map.put(0.05, Math.PI / 6); + map.put(0.1, Math.PI / 4); + map.put(0.2, Math.PI / 3); + map.put(0.4, 1.37); + + return map; + } } public static final class LEDConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2c7b7fa1..e181ecaa 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,7 +41,7 @@ public void robotInit() { if (Constants.currentMode == Constants.Mode.REAL) { // TODO: Log data to a USB drive on the RIO - // Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") + Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables pdh = new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging } else if (Constants.currentMode == Constants.Mode.SIM) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1883ac8d..eaa87ec6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,8 +10,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.CommandJoystick; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.FeatureFlags; import frc.robot.Constants.FieldConstants; @@ -37,8 +36,8 @@ import frc.robot.subsystems.localization.CameraContainerSim; import frc.robot.subsystems.localization.VisionLocalizer; import frc.robot.subsystems.scoring.AimerIO; +import frc.robot.subsystems.scoring.AimerIORoboRio; import frc.robot.subsystems.scoring.AimerIOSim; -import frc.robot.subsystems.scoring.AimerIOTalon; import frc.robot.subsystems.scoring.HoodIO; import frc.robot.subsystems.scoring.HoodIOSim; import frc.robot.subsystems.scoring.HoodIOSparkFlex; @@ -51,10 +50,14 @@ import frc.robot.telemetry.TelemetryIO; import frc.robot.telemetry.TelemetryIOLive; import frc.robot.telemetry.TelemetryIOSim; +import frc.robot.utils.ControllerJSONReader; import frc.robot.utils.FieldFinder; import frc.robot.utils.feedforward.TuneG; import frc.robot.utils.feedforward.TuneS; import frc.robot.utils.feedforward.TuneV; +import java.util.HashMap; +import java.util.function.DoubleSupplier; +import java.util.function.IntSupplier; import org.littletonrobotics.junction.Logger; public class RobotContainer { @@ -62,9 +65,9 @@ public class RobotContainer { IntakeSubsystem intakeSubsystem; EndgameSubsystem endgameSubsystem; - CommandJoystick leftJoystick = new CommandJoystick(0); - CommandJoystick rightJoystick = new CommandJoystick(1); - CommandXboxController controller = new CommandXboxController(2); + HashMap triggers; + HashMap axes; + HashMap pov; VisionLocalizer tagVision; @@ -80,8 +83,115 @@ public RobotContainer() { configureSubsystems(); configureModes(); configureAutonomous(); + configureBindings(); } + // spotless:off + // spotless:off + private void configureBindings() { + // Resets bindings + ControllerJSONReader.pullConfiguration("SingleController"); + triggers = ControllerJSONReader.getTriggers(); + axes = ControllerJSONReader.getAxes(); + pov = ControllerJSONReader.getPOVs(); + + if (FeatureFlags.runDrive) { + drivetrain.setDefaultCommand( + new DriveWithJoysticks( + drivetrain, + axes.get("xDrive"), + axes.get("yDrive"), + axes.get("rotation"), + pov.get("pov"), + () -> true, + () -> false, + triggers.get("fieldRelative"))); + + triggers.get("align") + .onTrue(new InstantCommand( + () -> drivetrain.setAlignState(AlignState.ALIGNING))) + .onFalse(new InstantCommand( + () -> drivetrain.setAlignState(AlignState.MANUAL))); + + triggers.get("fieldRelative") + .onTrue(new InstantCommand( + () -> drivetrain.seedFieldRelative(getPoseAgainstSpeaker())) + ); + + triggers.get("aimSpeaker") + .onTrue(new InstantCommand( + () -> drivetrain.setAlignTarget(AlignTarget.SPEAKER))); + + triggers.get("shoot") + .onTrue(new InstantCommand( + () -> drivetrain.setAlignTarget(AlignTarget.SPEAKER))); + + triggers.get("aimAmp") + .onTrue(new InstantCommand( + () -> drivetrain.setAlignTarget(AlignTarget.AMP))); + } + + if (FeatureFlags.runEndgame) { + endgameSubsystem.setAction(EndgameSubsystem.EndgameAction.OVERRIDE); + + triggers.get("endgame1") + .onTrue(new InstantCommand(() -> endgameSubsystem.setVolts(2.0, 0))) + .onFalse(new InstantCommand(() -> endgameSubsystem.setVolts(0.0, 0))); + + triggers.get("endgame2") + .onTrue(new InstantCommand(() -> endgameSubsystem.setVolts(-2.0, 0))) + .onFalse(new InstantCommand(() -> endgameSubsystem.setVolts(0.0, 0))); + } + + if (FeatureFlags.runIntake) { + triggers.get("intake") + .onTrue(new InstantCommand( + () -> intakeSubsystem.run(IntakeAction.INTAKE))); + + triggers.get("stopIntake") + .onTrue(new InstantCommand( + () -> intakeSubsystem.run(IntakeAction.NONE))); + } + + if (FeatureFlags.runScoring) { + triggers.get("intake") + .onTrue(new InstantCommand( + () -> scoringSubsystem.setAction( + ScoringSubsystem.ScoringAction.INTAKE))); + + triggers.get("aimSpeaker") + .onTrue(new InstantCommand( + () -> scoringSubsystem.setAction( + ScoringSubsystem.ScoringAction.AIM))); + + triggers.get("shoot") + .onTrue(new InstantCommand( + () -> scoringSubsystem.setAction( + ScoringSubsystem.ScoringAction.SHOOT))) + .onFalse(new InstantCommand( + () -> scoringSubsystem.setAction( + ScoringSubsystem.ScoringAction.AIM))); + + triggers.get("endgame") + .onTrue(new InstantCommand( + () -> scoringSubsystem.setAction( + ScoringSubsystem.ScoringAction.ENDGAME))) + .onFalse(new InstantCommand( + () -> scoringSubsystem.setAction( + ScoringSubsystem.ScoringAction.WAIT))); + + triggers.get("aimAmp") + .onTrue(new InstantCommand( + () -> scoringSubsystem.setAction( + ScoringSubsystem.ScoringAction.AMP_AIM))); + + triggers.get("stopIntake") + .onTrue(new InstantCommand( + () -> scoringSubsystem.setAction( + ScoringSubsystem.ScoringAction.WAIT))); + } + } // spotless:on + public void configureSubsystems() { switch (Constants.currentMode) { case REAL: @@ -94,7 +204,7 @@ public void configureSubsystems() { scoringSubsystem = new ScoringSubsystem( new ShooterIOTalon(), - new AimerIOTalon(), + new AimerIORoboRio(), new HoodIOSparkFlex()); } @@ -213,101 +323,12 @@ public void configureSubsystems() { if (FeatureFlags.enableLEDS) leds = new LED(scoringSubsystem); } - // spotless:off - private void configureBindings() { - // Resets bindings - controller = new CommandXboxController(2); - - if (FeatureFlags.runDrive) { - drivetrain.setDefaultCommand( - new DriveWithJoysticks( - drivetrain, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX(), - () -> controller.getHID().getPOV(), - () -> true, - () -> false, - () -> controller.getHID().getLeftBumper())); - - controller.rightBumper() - .onTrue(new InstantCommand( - () -> drivetrain.setAlignState(AlignState.ALIGNING))) - .onFalse(new InstantCommand( - () -> drivetrain.setAlignState(AlignState.MANUAL))); - - controller.leftBumper() - .onTrue(new InstantCommand( - () -> drivetrain.seedFieldRelative(getPoseAgainstSpeaker())) - ); - - controller.b() - .onTrue(new InstantCommand( - () -> drivetrain.setAlignTarget(AlignTarget.SPEAKER))); - - controller.x() - .onTrue(new InstantCommand( - () -> drivetrain.setAlignTarget(AlignTarget.SPEAKER))); - - controller.back() - .onTrue(new InstantCommand( - () -> drivetrain.setAlignTarget(AlignTarget.AMP))); - } - - if (FeatureFlags.runIntake) { - controller.a() - .onTrue(new InstantCommand( - () -> intakeSubsystem.run(IntakeAction.INTAKE))); - - controller.start() - .onTrue(new InstantCommand( - () -> intakeSubsystem.run(IntakeAction.NONE))); - } - - if (FeatureFlags.runScoring) { - controller.a() - .onTrue(new InstantCommand( - () -> scoringSubsystem.setAction( - ScoringSubsystem.ScoringAction.INTAKE))); - - controller.b() - .onTrue(new InstantCommand( - () -> scoringSubsystem.setAction( - ScoringSubsystem.ScoringAction.AIM))); - - controller.x() - .onTrue(new InstantCommand( - () -> scoringSubsystem.setAction( - ScoringSubsystem.ScoringAction.SHOOT))) - .onFalse(new InstantCommand( - () -> scoringSubsystem.setAction( - ScoringSubsystem.ScoringAction.AIM))); - - controller.y() - .onTrue(new InstantCommand( - () -> scoringSubsystem.setAction( - ScoringSubsystem.ScoringAction.ENDGAME))) - .onFalse(new InstantCommand( - () -> scoringSubsystem.setAction( - ScoringSubsystem.ScoringAction.WAIT))); - - controller.back() - .onTrue(new InstantCommand( - () -> scoringSubsystem.setAction( - ScoringSubsystem.ScoringAction.AMP_AIM))); - - controller.start() - .onTrue(new InstantCommand( - () -> scoringSubsystem.setAction( - ScoringSubsystem.ScoringAction.WAIT))); - } - } // spotless:on - private void configureModes() { testModeChooser.setDefaultOption("Blank", "tuning"); testModeChooser.addOption("Speaker Tuning", "calculate-speaker"); + testModeChooser.addOption("Intake Tuning", "tuning-intake"); testModeChooser.addOption("Aimer Tuning", "tuning-aimer"); testModeChooser.addOption("Hood Tuning", "tuning-hood"); testModeChooser.addOption("Shooter Tuning", "tuning-shooter"); @@ -317,10 +338,10 @@ private void configureModes() { } public void enabledInit() { - if (FeatureFlags.runScoring) { - scoringSubsystem.setAction(ScoringAction.WAIT); - scoringSubsystem.homeHood(); + if (FeatureFlags.runIntake) { + intakeSubsystem.run(IntakeAction.NONE); } + if (FeatureFlags.runEndgame) { endgameSubsystem.setAction(EndgameSubsystem.EndgameAction.CANCEL); } @@ -328,11 +349,13 @@ public void enabledInit() { public void testInit() { // Resets bindings - controller = new CommandXboxController(2); - if (FeatureFlags.runScoring) { - scoringSubsystem.homeHood(); - } + HashMap testTriggers; + HashMap testAxes; + + ControllerJSONReader.pullConfiguration("SingleControllerTestMode"); + testTriggers = ControllerJSONReader.getTriggers(); + testAxes = ControllerJSONReader.getAxes(); // spotless:off switch (testModeChooser.getSelected()) { @@ -341,41 +364,90 @@ public void testInit() { case "calculate-speaker": drivetrain.seedFieldRelative(); scoringSubsystem.setAction(ScoringAction.TUNING); - controller.leftBumper() + testTriggers.get("left") .onTrue(new InstantCommand( () -> scoringSubsystem.setTuningKickerVolts(5))) .onFalse(new InstantCommand( () -> scoringSubsystem.setTuningKickerVolts(0))); break; + case "tuning-intake": + if (FeatureFlags.runDrive) { + drivetrain.setDefaultCommand( + new DriveWithJoysticks( + drivetrain, + testAxes.get("xDrive"), + testAxes.get("yDrive"), + testAxes.get("rotation"), + () -> -1, + () -> true, + () -> false, + () -> false)); + } + + SmartDashboard.putNumber("Test-Mode/intake/intakeVolts", 1.0); + SmartDashboard.putNumber("Test-Mode/intake/beltVolts", 1.0); + + intakeSubsystem.run(IntakeSubsystem.IntakeAction.OVERRIDE); + + testTriggers.get("left") + .onTrue(new InstantCommand(() -> intakeSubsystem.setOverrideVolts( + SmartDashboard.getNumber("Test-Mode/intake/intakeVolts", 1.0), + SmartDashboard.getNumber("Test-Mode/intake/beltVolts", 1.0)))) + .onFalse(new InstantCommand(() -> intakeSubsystem.setOverrideVolts(0, 0))); + + testTriggers.get("right") + .onTrue(new InstantCommand(() -> intakeSubsystem.setOverrideVolts( + -SmartDashboard.getNumber("Test-Mode/intake/intakeVolts", 1.0), + -SmartDashboard.getNumber("Test-Mode/intake/beltVolts", 1.0)))) + .onFalse(new InstantCommand(() -> intakeSubsystem.setOverrideVolts(0, 0))); + break; case "tuning-aimer": SmartDashboard.putNumber("Test-Mode/aimer/kP", ScoringConstants.aimerkP); SmartDashboard.putNumber("Test-Mode/aimer/kI", ScoringConstants.aimerkI); SmartDashboard.putNumber("Test-Mode/aimer/kD", ScoringConstants.aimerkD); + SmartDashboard.putNumber("Test-Mode/aimer/profileMaxVelocity", ScoringConstants.aimCruiseVelocity); + SmartDashboard.putNumber("Test-Mode/aimer/profileMaxAcceleration", ScoringConstants.aimAcceleration); + + SmartDashboard.putNumber("Test-Mode/aimer/setpointPosition", 0.25); + SmartDashboard.putNumber("Test-Mode/aimer/volts", 2.0); + scoringSubsystem.setAction(ScoringAction.OVERRIDE); - controller.a() + testTriggers.get("a") .onTrue(new TuneS(scoringSubsystem, 0)); - controller.b() + testTriggers.get("b") .onTrue(new TuneG(scoringSubsystem, 0)); - controller.x() - .onTrue(new TuneV(scoringSubsystem, 1.0, 0)); - - controller.y() + testTriggers.get("y") .onTrue(new InstantCommand(() -> scoringSubsystem.setPID( SmartDashboard.getNumber("Test-Mode/aimer/kP", ScoringConstants.aimerkP), SmartDashboard.getNumber("Test-Mode/aimer/kI", ScoringConstants.aimerkI), SmartDashboard.getNumber("Test-Mode/aimer/kD", ScoringConstants.aimerkD), 0))) - .onTrue(new InstantCommand(() ->scoringSubsystem.runToPosition(0.25, 0))); + .onTrue(new InstantCommand(() -> scoringSubsystem.setMaxProfileProperties( + SmartDashboard.getNumber("Test-Mode/aimer/profileMaxVelocity", ScoringConstants.aimCruiseVelocity), + SmartDashboard.getNumber("Test-Mode/aimer/profileMaxAcceleration", ScoringConstants.aimAcceleration), 0))) + .onTrue(new InstantCommand(() -> scoringSubsystem.runToPosition(SmartDashboard.getNumber("Test-Mode/aimer/setpointPosition", 0.25), 0))) + .onTrue(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.TEMPORARY_SETPOINT))) + .onFalse(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.OVERRIDE))); + + testTriggers.get("povUp") + .onTrue(new InstantCommand(() -> scoringSubsystem.runToPosition(1.1, 0))) + .onTrue(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.TEMPORARY_SETPOINT))) + .onFalse(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.OVERRIDE))); + + testTriggers.get("povDown") + .onTrue(new InstantCommand(() -> scoringSubsystem.runToPosition(0.0, 0))) + .onTrue(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.TEMPORARY_SETPOINT))) + .onFalse(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.OVERRIDE))); - controller.leftBumper() - .onTrue(new InstantCommand(() -> scoringSubsystem.setVolts(1.0, 0))) + testTriggers.get("left") + .onTrue(new InstantCommand(() -> scoringSubsystem.setVolts(SmartDashboard.getNumber("Test-Mode/aimer/volts", 2.0), 0))) .onFalse(new InstantCommand(() -> scoringSubsystem.setVolts(0, 0))); - controller.rightBumper() - .onTrue(new InstantCommand(() -> scoringSubsystem.setVolts(-1.0, 0))) + testTriggers.get("right") + .onTrue(new InstantCommand(() -> scoringSubsystem.setVolts(-SmartDashboard.getNumber("Test-Mode/aimer/volts", 2.0), 0))) .onFalse(new InstantCommand(() -> scoringSubsystem.setVolts(0, 0))); break; case "tuning-hood": @@ -383,29 +455,31 @@ public void testInit() { SmartDashboard.putNumber("Test-Mode/hood/kI", ScoringConstants.hoodkI); SmartDashboard.putNumber("Test-Mode/hood/kD", ScoringConstants.hoodkD); - scoringSubsystem.setAction(ScoringAction.OVERRIDE); - - controller.a() - .onTrue(new TuneS(scoringSubsystem, 1)); + SmartDashboard.putNumber("Test-Mode/hood/setpointPosition", 0.75); - controller.b() - .onTrue(new TuneG(scoringSubsystem, 1)); + scoringSubsystem.setAction(ScoringAction.OVERRIDE); - controller.x() - .onTrue(new TuneV(scoringSubsystem, 1.0, 1)); + testTriggers.get("x").onTrue(new InstantCommand(() -> scoringSubsystem.homeHood())); - controller.y() + testTriggers.get("y") .onTrue(new InstantCommand(() -> scoringSubsystem.setPID( SmartDashboard.getNumber("Test-Mode/hood/kP", ScoringConstants.hoodkP), SmartDashboard.getNumber("Test-Mode/hood/kI", ScoringConstants.hoodkI), SmartDashboard.getNumber("Test-Mode/hood/kD", ScoringConstants.hoodkD), 1))) - .onTrue(new InstantCommand(() ->scoringSubsystem.runToPosition(0.25, 1))); + .onTrue(new InstantCommand(() ->scoringSubsystem.runToPosition(SmartDashboard.getNumber("Test-Mode/hood/setpointPosition", 0.75), 1))) + .onTrue(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.TEMPORARY_SETPOINT))) + .onFalse(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.OVERRIDE))); - controller.leftBumper() + testTriggers.get("povDown") + .onTrue(new InstantCommand(() -> scoringSubsystem.runToPosition(0.0, 1))) + .onTrue(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.TEMPORARY_SETPOINT))) + .onFalse(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.OVERRIDE))); + + testTriggers.get("left") .onTrue(new InstantCommand(() -> scoringSubsystem.setVolts(1.0, 1))) .onFalse(new InstantCommand(() -> scoringSubsystem.setVolts(0, 1))); - controller.rightBumper() + testTriggers.get("right") .onTrue(new InstantCommand(() -> scoringSubsystem.setVolts(-1.0, 1))) .onFalse(new InstantCommand(() -> scoringSubsystem.setVolts(0, 1))); break; @@ -414,27 +488,42 @@ public void testInit() { SmartDashboard.putNumber("Test-Mode/shooter/kI", ScoringConstants.shooterkI); SmartDashboard.putNumber("Test-Mode/shooter/kD", ScoringConstants.shooterkD); - scoringSubsystem.setAction(ScoringAction.OVERRIDE); + SmartDashboard.putNumber("Test-Mode/shooter/kS", ScoringConstants.shooterkS); + SmartDashboard.putNumber("Test-Mode/shooter/kV", ScoringConstants.shooterkV); + SmartDashboard.putNumber("Test-Mode/shooter/kA", ScoringConstants.shooterkA); - controller.a() - .onTrue(new TuneS(scoringSubsystem, 2)); + SmartDashboard.putNumber("Test-Mode/shooter/setpointRPM", 2000); - controller.x() - .onTrue(new TuneV(scoringSubsystem, 1.0, 2)); + scoringSubsystem.setAction(ScoringAction.OVERRIDE); - controller.y() + testTriggers.get("y") .onTrue(new InstantCommand(() -> scoringSubsystem.setPID( SmartDashboard.getNumber("Test-Mode/shooter/kP", ScoringConstants.shooterkP), SmartDashboard.getNumber("Test-Mode/shooter/kI", ScoringConstants.shooterkI), SmartDashboard.getNumber("Test-Mode/shooter/kD", ScoringConstants.shooterkD), 2))) - .onTrue(new InstantCommand(() ->scoringSubsystem.runToPosition(0.25, 2))); - - controller.leftBumper() - .onTrue(new InstantCommand(() -> scoringSubsystem.setVolts(1.0, 2))) + .onTrue(new InstantCommand(() -> scoringSubsystem.setFF( + SmartDashboard.getNumber("Test-Mode/shooter/kS", ScoringConstants.shooterkS), + SmartDashboard.getNumber("Test-Mode/shooter/kV", ScoringConstants.shooterkV), + SmartDashboard.getNumber("Test-Mode/shooter/kA", ScoringConstants.shooterkA), + 0.0, 2))) + .onTrue(new InstantCommand(() ->scoringSubsystem.runToPosition(SmartDashboard.getNumber("Test-Mode/shooter/setpointRPM", 2000), 2))) + .onTrue(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.TEMPORARY_SETPOINT))) + .onFalse(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.OVERRIDE))); + + testTriggers.get("left").onTrue(new InstantCommand(() -> scoringSubsystem.setTuningKickerVolts(2.0))).onFalse(new InstantCommand(() -> scoringSubsystem.setTuningKickerVolts(0.0))); + testTriggers.get("right").onTrue(new InstantCommand(() -> scoringSubsystem.setTuningKickerVolts(-2.0))).onFalse(new InstantCommand(() -> scoringSubsystem.setTuningKickerVolts(0.0))); + + testTriggers.get("povDown") + .onTrue(new InstantCommand(() -> scoringSubsystem.runToPosition(0.0, 2))) + .onTrue(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.TEMPORARY_SETPOINT))) + .onFalse(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.OVERRIDE))); + + testTriggers.get("left") + .onTrue(new InstantCommand(() -> scoringSubsystem.setVolts(10.0, 2))) .onFalse(new InstantCommand(() -> scoringSubsystem.setVolts(0, 2))); - controller.rightBumper() - .onTrue(new InstantCommand(() -> scoringSubsystem.setVolts(-1.0, 2))) + testTriggers.get("right") + .onTrue(new InstantCommand(() -> scoringSubsystem.setVolts(-10.0, 2))) .onFalse(new InstantCommand(() -> scoringSubsystem.setVolts(0, 2))); break; case "tuning-endgame": @@ -443,23 +532,25 @@ public void testInit() { // SmartDashboard.putNumber("Test-Mode/endgame/kI", EndgameConstants.kI); // SmartDashboard.putNumber("Test-Mode/endgame/kD", EndgameConstants.kD); + SmartDashboard.putNumber("Test-Mode/endgame/volts", 1.0); + endgameSubsystem.setAction(EndgameSubsystem.EndgameAction.OVERRIDE); - controller.a() + testTriggers.get("a") .onTrue(new TuneS(endgameSubsystem, 0)); - controller.b() + testTriggers.get("b") .onTrue(new TuneG(endgameSubsystem, 0)); - controller.x() + testTriggers.get("x") .onTrue(new TuneV(endgameSubsystem, 1.0, 0)); - controller.leftBumper() - .onTrue(new InstantCommand(() -> endgameSubsystem.setVolts(1.0, 0))) + testTriggers.get("left") + .onTrue(new InstantCommand(() -> endgameSubsystem.setVolts(SmartDashboard.getNumber("Test-Mode/endgame/volts", 1.0), 0))) .onFalse(new InstantCommand(() -> endgameSubsystem.setVolts(0, 0))); - controller.rightBumper() - .onTrue(new InstantCommand(() -> endgameSubsystem.setVolts(-1.0, 0))) + testTriggers.get("right") + .onTrue(new InstantCommand(() -> endgameSubsystem.setVolts(-SmartDashboard.getNumber("Test-Mode/endgame/volts", 1.0), 0))) .onFalse(new InstantCommand(() -> endgameSubsystem.setVolts(0, 0))); break; } @@ -584,6 +675,8 @@ public void teleopInit() { // This is in teleopInit to prevent it from wasting time in auto if (FeatureFlags.runScoring) { scoringSubsystem.homeHood(); + + scoringSubsystem.setAction(ScoringAction.WAIT); } } } diff --git a/src/main/java/frc/robot/subsystems/endgame/EndgameIO.java b/src/main/java/frc/robot/subsystems/endgame/EndgameIO.java index 5e83b377..cccda819 100644 --- a/src/main/java/frc/robot/subsystems/endgame/EndgameIO.java +++ b/src/main/java/frc/robot/subsystems/endgame/EndgameIO.java @@ -7,10 +7,12 @@ public interface EndgameIO { @AutoLog public static class EndgameIOInputs { public double endgameLeftAppliedVolts = 0.0; - public double endgameLeftCurrentAmps = 0.0; + public double endgameLeftStatorCurrentAmps = 0.0; + public double endgameLeftSupplyCurrentAmps = 0.0; public double endgameRightAppliedVolts = 0.0; - public double endgameRightCurrentAmps = 0.0; + public double endgameRightStatorCurrentAmps = 0.0; + public double endgameRightSupplyCurrentAmps = 0.0; public double position = 0.0; public double velocity = 0.0; diff --git a/src/main/java/frc/robot/subsystems/endgame/EndgameIOSim.java b/src/main/java/frc/robot/subsystems/endgame/EndgameIOSim.java index c6f06581..3d0ead17 100644 --- a/src/main/java/frc/robot/subsystems/endgame/EndgameIOSim.java +++ b/src/main/java/frc/robot/subsystems/endgame/EndgameIOSim.java @@ -21,10 +21,10 @@ public void updateInputs(EndgameIOInputs inputs) { elevatorSim.setInputVoltage(appliedVolts); inputs.endgameLeftAppliedVolts = appliedVolts; - inputs.endgameLeftCurrentAmps = elevatorSim.getCurrentDrawAmps(); + inputs.endgameLeftStatorCurrentAmps = elevatorSim.getCurrentDrawAmps(); inputs.endgameRightAppliedVolts = appliedVolts; - inputs.endgameRightCurrentAmps = elevatorSim.getCurrentDrawAmps(); + inputs.endgameRightStatorCurrentAmps = elevatorSim.getCurrentDrawAmps(); inputs.position = elevatorSim.getPositionMeters(); inputs.velocity = elevatorSim.getVelocityMetersPerSecond(); diff --git a/src/main/java/frc/robot/subsystems/endgame/EndgameIOSparkFlex.java b/src/main/java/frc/robot/subsystems/endgame/EndgameIOSparkFlex.java index 2daca0f3..bf8625aa 100644 --- a/src/main/java/frc/robot/subsystems/endgame/EndgameIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/endgame/EndgameIOSparkFlex.java @@ -18,7 +18,7 @@ public EndgameIOSparkFlex() { rightEndgameMotor.setIdleMode(IdleMode.kBrake); leftEndgameMotor.setIdleMode(IdleMode.kBrake); - rightEndgameMotor.follow(leftEndgameMotor, true); // TODO: check if this should be inverted + leftEndgameMotor.follow(rightEndgameMotor, true); leftEndgameMotor.getEncoder().setPositionConversionFactor(EndgameConstants.encoderToMeters); leftEndgameMotor.getEncoder().setPosition(0.0); @@ -26,16 +26,16 @@ public EndgameIOSparkFlex() { @Override public void setVolts(double volts) { - leftEndgameMotor.setVoltage(volts); + rightEndgameMotor.setVoltage(volts); } @Override public void updateInputs(EndgameIOInputs inputs) { inputs.endgameLeftAppliedVolts = leftEndgameMotor.getAppliedOutput(); - inputs.endgameLeftCurrentAmps = leftEndgameMotor.getOutputCurrent(); + inputs.endgameLeftStatorCurrentAmps = leftEndgameMotor.getOutputCurrent(); inputs.endgameRightAppliedVolts = rightEndgameMotor.getAppliedOutput(); - inputs.endgameRightCurrentAmps = rightEndgameMotor.getOutputCurrent(); + inputs.endgameRightStatorCurrentAmps = rightEndgameMotor.getOutputCurrent(); inputs.position = leftEndgameMotor.getEncoder().getPosition(); inputs.velocity = leftEndgameMotor.getEncoder().getVelocity(); diff --git a/src/main/java/frc/robot/subsystems/endgame/EndgameSubsystem.java b/src/main/java/frc/robot/subsystems/endgame/EndgameSubsystem.java index 346eb6ce..c2dde0f9 100644 --- a/src/main/java/frc/robot/subsystems/endgame/EndgameSubsystem.java +++ b/src/main/java/frc/robot/subsystems/endgame/EndgameSubsystem.java @@ -103,4 +103,14 @@ public void periodic() { "endgame/Elevator3d", new Pose3d(0.0, 0.0, endgameInputs.position + 0.1, new Rotation3d(0, 0, 0))); } + + @Override + public void setFF(double kS, double kV, double kA, double kG, int slot) { + throw new UnsupportedOperationException("Unimplemented method 'setFF'"); + } + + @Override + public void setMaxProfileProperties(double maxVelocity, double maxAcceleration, int slot) { + throw new UnsupportedOperationException("Unimplemented method 'setMaxProfileVelocity'"); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 35b1ed07..44a6888d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -7,13 +7,16 @@ public interface IntakeIO { @AutoLog public static class IntakeIOInputs { public double leftIntakeVoltage = 0.0; - public double leftIntakeCurrent = 0.0; + public double leftIntakeStatorCurrent = 0.0; + public double leftIntakeSupplyCurrent = 0.0; public double rightIntakeVoltage = 0.0; - public double rightIntakeCurrent = 0.0; + public double rightIntakeStatorCurrent = 0.0; + public double rightIntakeSupplyCurrent = 0.0; public double beltVoltage = 0.0; - public double beltCurrent = 0.0; + public double beltStatorCurrent = 0.0; + public double beltSupplyCurrent = 0.0; public boolean noteSensed = false; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 39998a44..e792e932 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -26,13 +26,13 @@ public void updateInputs(IntakeIOInputs inputs) { noteInBelts = SmartDashboard.getBoolean("noteInBelts", false); inputs.leftIntakeVoltage = intakeWheelsAppliedVolts; - inputs.leftIntakeCurrent = noteInIntakeWheels ? 100000 : 0; + inputs.leftIntakeStatorCurrent = noteInIntakeWheels ? 100000 : 0; inputs.rightIntakeVoltage = intakeWheelsAppliedVolts; - inputs.rightIntakeCurrent = noteInIntakeWheels ? 100000 : 0; + inputs.rightIntakeStatorCurrent = noteInIntakeWheels ? 100000 : 0; inputs.beltVoltage = beltAppliedVolts; - inputs.beltCurrent = noteInBelts ? 100000 : 0; + inputs.beltStatorCurrent = noteInBelts ? 100000 : 0; inputs.noteSensed = noteInBelts; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index bf56d71b..87940377 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.intake; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfigurator; @@ -9,7 +8,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.IntakeConstants; public class IntakeIOSparkMax implements IntakeIO { @@ -19,38 +18,43 @@ public class IntakeIOSparkMax implements IntakeIO { private CANSparkMax rightIntake = new CANSparkMax(IntakeConstants.rightIntakeMotorID, MotorType.kBrushless); - private TalonFX belt = new TalonFX(IntakeConstants.frontBeltMotorID); - private StatusSignal beltVoltage = belt.getMotorVoltage(); - private StatusSignal beltCurrent = belt.getStatorCurrent(); + private TalonFX belt = new TalonFX(IntakeConstants.indexTwoMotorID); - private DigitalInput bannerSensor = new DigitalInput(IntakeConstants.bannerSensorID); + // private DigitalInput bannerSensor = new DigitalInput(IntakeConstants.bannerSensorID); public IntakeIOSparkMax() { - leftIntake.setSmartCurrentLimit(20, 25); - rightIntake.setSmartCurrentLimit(20, 25); + SmartDashboard.putBoolean("noteInBelts", false); + + leftIntake.setSmartCurrentLimit(40, 40); + rightIntake.setSmartCurrentLimit(40, 40); + + leftIntake.setInverted(true); + rightIntake.setInverted(true); + + belt.setInverted(true); TalonFXConfigurator beltConfig = belt.getConfigurator(); beltConfig.apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake)); beltConfig.apply( new CurrentLimitsConfigs() - .withSupplyCurrentLimit(30) - .withStatorCurrentLimitEnable(true) - .withSupplyCurrentThreshold(35) - .withSupplyTimeThreshold(1)); + .withStatorCurrentLimit(60) + .withStatorCurrentLimitEnable(true)); } @Override public void updateInputs(IntakeIOInputs inputs) { inputs.leftIntakeVoltage = leftIntake.getAppliedOutput() * 12; - inputs.leftIntakeCurrent = leftIntake.getOutputCurrent(); + inputs.leftIntakeStatorCurrent = leftIntake.getOutputCurrent(); inputs.rightIntakeVoltage = rightIntake.getAppliedOutput() * 12; - inputs.rightIntakeCurrent = rightIntake.getOutputCurrent(); + inputs.rightIntakeStatorCurrent = rightIntake.getOutputCurrent(); - inputs.beltVoltage = beltVoltage.getValueAsDouble(); - inputs.beltCurrent = beltCurrent.getValueAsDouble(); + inputs.beltVoltage = belt.getMotorVoltage().getValueAsDouble(); + inputs.beltStatorCurrent = belt.getStatorCurrent().getValueAsDouble(); + inputs.beltSupplyCurrent = belt.getSupplyCurrent().getValueAsDouble(); - inputs.noteSensed = bannerSensor.get(); + // inputs.noteSensed = bannerSensor.get(); + inputs.noteSensed = SmartDashboard.getBoolean("noteInBelts", false); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index c2aa21f9..f394d239 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -16,6 +16,9 @@ public class IntakeSubsystem extends SubsystemBase { private IntakeAction action = IntakeAction.NONE; + private double intakeOverrideVolts = 0.0; + private double beltOverrideVolts = 0.0; + public IntakeSubsystem(IntakeIO io) { this.io = io; } @@ -38,6 +41,9 @@ public void periodic() { case REVERSING: reversing(); break; + case OVERRIDE: + override(); + break; } Logger.recordOutput("intake/running", inputs.leftIntakeVoltage != 0.0); @@ -75,6 +81,8 @@ private void idle() { state = State.REVERSING; io.setIntakeVoltage(-IntakeConstants.intakePower); io.setBeltVoltage(-IntakeConstants.beltPower); + } else if (action == IntakeAction.OVERRIDE) { + state = State.OVERRIDE; } } @@ -96,7 +104,7 @@ private void passing() { } else { io.setBeltVoltage(0); } - if (!inputs.noteSensed && inputs.beltCurrent < 2.0) { + if (!inputs.noteSensed && inputs.beltStatorCurrent < 2.0) { state = State.IDLE; io.setBeltVoltage(0); } @@ -116,16 +124,32 @@ private void reversing() { } } + private void override() { + if (action != IntakeAction.OVERRIDE) { + state = State.IDLE; + } + + io.setIntakeVoltage(intakeOverrideVolts); + io.setBeltVoltage(beltOverrideVolts); + } + + public void setOverrideVolts(double intake, double belt) { + intakeOverrideVolts = intake; + beltOverrideVolts = belt; + } + private enum State { IDLE, // do nothing SEEKING, // run intake wheels until a note is taken in PASSING, // move the belt to pass the note to the shooter when its ready - REVERSING // whole intake backwards + REVERSING, // whole intake backwards + OVERRIDE } public enum IntakeAction { NONE, // do nothing INTAKE, // Try to intake a note if you don't have one - REVERSE // run backwards + REVERSE, // run backwards + OVERRIDE } } diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIO.java b/src/main/java/frc/robot/subsystems/scoring/AimerIO.java index 9539ed0b..796e5f55 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIO.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIO.java @@ -7,17 +7,17 @@ public interface AimerIO { public static class AimerIOInputs { public double aimAngleRad = 0.0; public double aimGoalAngleRad = 0.0; + public double aimProfileGoalAngleRad = 0.0; public double aimAppliedVolts = 0.0; - public double aimCurrentAmps = 0.0; + public double aimStatorCurrentAmps = 0.0; + public double aimSupplyCurrentAmps = 0.0; public double aimVelocityRadPerSec = 0.0; } public default void updateInputs(AimerIOInputs inputs) {} - public default void setAimAngleRad(double angle, boolean newProfile) {} - - public default void controlAimAngleRad() {} + public default void setAimAngleRad(double angle) {} public default void setAngleClampsRad(double min, double max) {} @@ -26,4 +26,8 @@ public default void setOverrideMode(boolean override) {} public default void setOverrideVolts(double volts) {} public default void setPID(double p, double i, double d) {} + + public default void setMaxProfile(double maxVelocity, double maxAcceleration) {} + + public default void setFF(double kS, double kV, double kA, double kG) {} } diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java new file mode 100644 index 00000000..cb7972d4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java @@ -0,0 +1,202 @@ +package frc.robot.subsystems.scoring; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +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.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants.ConversionConstants; +import frc.robot.Constants.ScoringConstants; + +public class AimerIORoboRio implements AimerIO { + private final TalonFX aimerLeft = new TalonFX(ScoringConstants.aimLeftMotorId); + private final TalonFX aimerRight = new TalonFX(ScoringConstants.aimRightMotorId); + + private final PIDController controller = + new PIDController( + ScoringConstants.aimerkP, ScoringConstants.aimerkI, ScoringConstants.aimerkD); + private ArmFeedforward feedforward = + new ArmFeedforward( + ScoringConstants.aimerkS, + ScoringConstants.aimerkG, + ScoringConstants.aimerkV, + ScoringConstants.aimerkA); + private TrapezoidProfile profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + ScoringConstants.aimCruiseVelocity, ScoringConstants.aimAcceleration)); + + private final DutyCycleEncoder encoder = new DutyCycleEncoder(ScoringConstants.aimEncoderPort); + + private final Timer timer = new Timer(); + + private boolean override = false; + private double overrideVolts = 0.0; + + boolean useProfile = false; + double previousGoalAngle = 0.0; + + double minAngleClamp = 0.0; + double maxAngleClamp = 0.0; + + double goalAngleRad = 0.0; + double appliedVolts = 0.0; + + double initialAngle = 0.0; + double initialVelocity = 0.0; + + double velocity = 0.0; + + double lastPosition = 0.0; + double lastTime = Utils.getCurrentTimeSeconds(); + + public AimerIORoboRio() { + aimerLeft.setControl(new Follower(ScoringConstants.aimRightMotorId, true)); + + aimerLeft.setNeutralMode(NeutralModeValue.Brake); + aimerRight.setNeutralMode(NeutralModeValue.Brake); + + TalonFXConfigurator aimerLeftConfig = aimerLeft.getConfigurator(); + aimerLeftConfig.apply( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(60) + .withStatorCurrentLimitEnable(true)); + + TalonFXConfigurator aimerRightConfig = aimerRight.getConfigurator(); + aimerRightConfig.apply( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(60) + .withStatorCurrentLimitEnable(true)); + + aimerRight.setPosition(0.0); + + controller.setTolerance(0.015); + } + + @Override + public void setAimAngleRad(double goalAngleRad) { + this.goalAngleRad = goalAngleRad; + + useProfile = + (Math.abs(goalAngleRad - getEncoderPosition()) + > 10 * ConversionConstants.kDegreesToRadians); + + if (goalAngleRad != previousGoalAngle && useProfile) { + timer.reset(); + timer.start(); + + initialAngle = + MathUtil.clamp(getEncoderPosition(), 0.0, ScoringConstants.aimMaxAngleRadians); + initialVelocity = velocity; + + previousGoalAngle = goalAngleRad; + } + goalAngleRad = MathUtil.clamp(goalAngleRad, minAngleClamp, maxAngleClamp); + } + + @Override + public void setAngleClampsRad(double minAngleClamp, double maxAngleClamp) { + if (minAngleClamp > maxAngleClamp) { + return; + } + this.minAngleClamp = + MathUtil.clamp(minAngleClamp, 0.0, ScoringConstants.aimMaxAngleRadians); + this.maxAngleClamp = + MathUtil.clamp(maxAngleClamp, 0.0, ScoringConstants.aimMaxAngleRadians); + } + + @Override + public void setOverrideMode(boolean override) { + this.override = override; + } + + @Override + public void setOverrideVolts(double volts) { + overrideVolts = volts; + } + + @Override + public void setPID(double p, double i, double d) { + controller.setP(p); + controller.setI(i); + controller.setD(d); + } + + @Override + public void setMaxProfile(double maxVelocity, double maxAcceleration) { + profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints(maxVelocity, maxAcceleration)); + } + + @Override + public void setFF(double kS, double kV, double kA, double kG) { + feedforward = new ArmFeedforward(kS, kG, kV, kA); + } + + private double getEncoderPosition() { + return encoder.getAbsolutePosition() * 2.0 * Math.PI - ScoringConstants.aimerEncoderOffset; + } + + @Override + public void updateInputs(AimerIOInputs inputs) { + double controlSetpoint = goalAngleRad; + double velocitySetpoint = 0.0; + appliedVolts = 0.0; + + State trapezoidSetpoint = + profile.calculate( + timer.get(), + new State(initialAngle, initialVelocity), + new State(goalAngleRad, 0)); + + if (useProfile) { + controlSetpoint = + MathUtil.clamp( + trapezoidSetpoint.position, 0.0, ScoringConstants.aimMaxAngleRadians); + + velocitySetpoint = trapezoidSetpoint.velocity; + } + + if (override) { + appliedVolts = overrideVolts; + } else { + boolean atSetpoint = + MathUtil.isNear( + controlSetpoint, + getEncoderPosition(), + ScoringConstants.aimPositionTolerance); + double controllerVolts = + atSetpoint ? 0.0 : controller.calculate(getEncoderPosition(), controlSetpoint); + appliedVolts = + feedforward.calculate(controlSetpoint, velocitySetpoint) + controllerVolts; + } + + appliedVolts = MathUtil.clamp(appliedVolts, -12.0, 12.0); + aimerRight.setVoltage(appliedVolts); + + inputs.aimGoalAngleRad = goalAngleRad; + inputs.aimProfileGoalAngleRad = controlSetpoint; + inputs.aimAngleRad = getEncoderPosition(); + + double currentTime = Utils.getCurrentTimeSeconds(); + double diffTime = currentTime - lastTime; + lastTime = currentTime; + + inputs.aimVelocityRadPerSec = (getEncoderPosition() - lastPosition) / diffTime; + velocity = (getEncoderPosition() - lastPosition) / diffTime; + lastPosition = getEncoderPosition(); + + inputs.aimAppliedVolts = appliedVolts; + inputs.aimStatorCurrentAmps = aimerRight.getStatorCurrent().getValueAsDouble(); + inputs.aimSupplyCurrentAmps = aimerRight.getSupplyCurrent().getValueAsDouble(); + } +} diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java index 2121e3f2..7b36e005 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.robot.Constants; +import frc.robot.Constants.ConversionConstants; import frc.robot.Constants.ScoringConstants; public class AimerIOSim implements AimerIO { @@ -39,8 +40,9 @@ public class AimerIOSim implements AimerIO { private final Timer timer = new Timer(); private boolean override = false; + private double overrideVolts = 0.0; - boolean newProfile = false; + boolean useProfile = false; double previousGoalAngle = 0.0; double minAngleClamp = 0.0; @@ -53,14 +55,14 @@ public class AimerIOSim implements AimerIO { double initialVelocity = 0.0; @Override - public void setAimAngleRad(double goalAngleRad, boolean newProfile) { + public void setAimAngleRad(double goalAngleRad) { this.goalAngleRad = goalAngleRad; - this.newProfile = newProfile; - } - @Override - public void controlAimAngleRad() { - if (goalAngleRad != previousGoalAngle && newProfile) { + useProfile = + (Math.abs(goalAngleRad - sim.getAngleRads()) + > 10 * ConversionConstants.kDegreesToRadians); + + if (goalAngleRad != previousGoalAngle && useProfile) { timer.reset(); timer.start(); @@ -74,8 +76,13 @@ public void controlAimAngleRad() { @Override public void setAngleClampsRad(double minAngleClamp, double maxAngleClamp) { - this.minAngleClamp = minAngleClamp; - this.maxAngleClamp = maxAngleClamp; + if (minAngleClamp > maxAngleClamp) { + return; + } + this.minAngleClamp = + MathUtil.clamp(minAngleClamp, 0.0, ScoringConstants.aimMaxAngleRadians); + this.maxAngleClamp = + MathUtil.clamp(maxAngleClamp, 0.0, ScoringConstants.aimMaxAngleRadians); } @Override @@ -99,18 +106,30 @@ public void setPID(double p, double i, double d) { public void updateInputs(AimerIOInputs inputs) { sim.update(Constants.loopTime); + double controlSetpoint = goalAngleRad; + State trapezoidSetpoint = profile.calculate( timer.get(), new State(initialAngle, initialVelocity), new State(goalAngleRad, 0)); - if (!override) { + if (useProfile) { + controlSetpoint = + MathUtil.clamp( + trapezoidSetpoint.position, 0.0, ScoringConstants.aimMaxAngleRadians); + } + + if (override) { + sim.setInputVoltage(overrideVolts); + } else { appliedVolts = - feedforward.calculate(trapezoidSetpoint.position, trapezoidSetpoint.velocity) - + controller.calculate(sim.getAngleRads(), trapezoidSetpoint.position); + feedforward.calculate( + controlSetpoint, useProfile ? trapezoidSetpoint.velocity : 0.0) + + controller.calculate(sim.getAngleRads(), controlSetpoint); + appliedVolts = MathUtil.clamp(appliedVolts, -12.0, 12.0); + sim.setInputVoltage(appliedVolts); } - sim.setInputVoltage(appliedVolts); inputs.aimGoalAngleRad = goalAngleRad; inputs.aimAngleRad = sim.getAngleRads(); @@ -118,6 +137,6 @@ public void updateInputs(AimerIOInputs inputs) { inputs.aimVelocityRadPerSec = sim.getVelocityRadPerSec(); inputs.aimAppliedVolts = appliedVolts; - inputs.aimCurrentAmps = sim.getCurrentDrawAmps(); + inputs.aimStatorCurrentAmps = sim.getCurrentDrawAmps(); } } diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java index 5805c020..b98e0505 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java @@ -1,8 +1,10 @@ package frc.robot.subsystems.scoring; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -38,6 +40,21 @@ public AimerIOTalon() { aimerLeft.setNeutralMode(NeutralModeValue.Brake); aimerRight.setNeutralMode(NeutralModeValue.Brake); + aimerLeft.setInverted(true); + aimerRight.setInverted(false); + + TalonFXConfigurator aimerLeftConfig = aimerLeft.getConfigurator(); + aimerLeftConfig.apply( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(60) + .withStatorCurrentLimitEnable(true)); + + TalonFXConfigurator aimerRightConfig = aimerRight.getConfigurator(); + aimerRightConfig.apply( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(60) + .withStatorCurrentLimitEnable(true)); + slot0.withKP(ScoringConstants.aimerkP); slot0.withKI(ScoringConstants.aimerkI); slot0.withKD(ScoringConstants.aimerkD); @@ -57,16 +74,12 @@ public AimerIOTalon() { aimerRight.getConfigurator().apply(configs); encoder.setDistancePerRotation(2 * Math.PI); + encoder.setPositionOffset(ScoringConstants.aimerEncoderOffset); } @Override - public void setAimAngleRad(double goalAngleRad, boolean newProfile) { - this.goalAngleRad = goalAngleRad; - } - - @Override - public void controlAimAngleRad() { - goalAngleRad = MathUtil.clamp(goalAngleRad, minAngleClamp, maxAngleClamp); + public void setAimAngleRad(double goalAngleRad) { + this.goalAngleRad = MathUtil.clamp(goalAngleRad, minAngleClamp, maxAngleClamp); } @Override @@ -95,6 +108,17 @@ public void setPID(double p, double i, double d) { aimerRight.getConfigurator().apply(slot0); } + @Override + public void setFF(double kS, double kV, double kA, double kG) { + slot0.withKS(kS); + slot0.withKV(kV); + slot0.withKA(kA); + slot0.withKG(kG); + + aimerLeft.getConfigurator().apply(slot0); + aimerRight.getConfigurator().apply(slot0); + } + @Override public void updateInputs(AimerIOInputs inputs) { if (override) { @@ -105,6 +129,7 @@ public void updateInputs(AimerIOInputs inputs) { inputs.aimGoalAngleRad = goalAngleRad; inputs.aimAngleRad = encoder.getAbsolutePosition(); + inputs.aimAngleRad = 0.0; double currentTime = Utils.getCurrentTimeSeconds(); double diffTime = currentTime - lastTime; @@ -112,8 +137,10 @@ public void updateInputs(AimerIOInputs inputs) { inputs.aimVelocityRadPerSec = (encoder.getAbsolutePosition() - lastPosition) / diffTime; lastPosition = encoder.getAbsolutePosition(); + inputs.aimVelocityRadPerSec = 0.0; inputs.aimAppliedVolts = aimerLeft.getMotorVoltage().getValueAsDouble(); - inputs.aimCurrentAmps = aimerLeft.getSupplyCurrent().getValueAsDouble(); + inputs.aimStatorCurrentAmps = aimerLeft.getStatorCurrent().getValueAsDouble(); + inputs.aimSupplyCurrentAmps = aimerLeft.getSupplyCurrent().getValueAsDouble(); } } diff --git a/src/main/java/frc/robot/subsystems/scoring/HoodIO.java b/src/main/java/frc/robot/subsystems/scoring/HoodIO.java index 9e26d0de..97407d9c 100644 --- a/src/main/java/frc/robot/subsystems/scoring/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/scoring/HoodIO.java @@ -8,7 +8,8 @@ public static class HoodIOInputs { public double hoodAngleRad = 0.0; public double hoodGoalAngleRad = 0.0; public double hoodAppliedVolts = 0.0; - public double hoodCurrentAmps = 0.0; + public double hoodStatorCurrentAmps = 0.0; + public double hoodSupplyCurrentAmps = 0.0; public double hoodVelocityRadPerSec = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/scoring/HoodIOSim.java b/src/main/java/frc/robot/subsystems/scoring/HoodIOSim.java index a8d9c3a5..1826ee24 100644 --- a/src/main/java/frc/robot/subsystems/scoring/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/HoodIOSim.java @@ -66,6 +66,6 @@ public void updateInputs(HoodIOInputs inputs) { inputs.hoodVelocityRadPerSec = sim.getVelocityRadPerSec(); inputs.hoodAppliedVolts = appliedVolts; - inputs.hoodCurrentAmps = sim.getCurrentDrawAmps(); + inputs.hoodStatorCurrentAmps = sim.getCurrentDrawAmps(); } } diff --git a/src/main/java/frc/robot/subsystems/scoring/HoodIOSparkFlex.java b/src/main/java/frc/robot/subsystems/scoring/HoodIOSparkFlex.java index e26e6c52..dc2de182 100644 --- a/src/main/java/frc/robot/subsystems/scoring/HoodIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/scoring/HoodIOSparkFlex.java @@ -2,6 +2,7 @@ import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkFlex; +import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants.ScoringConstants; public class HoodIOSparkFlex implements HoodIO { @@ -15,8 +16,10 @@ public class HoodIOSparkFlex implements HoodIO { double goalAngleRad = 0.0; + private Timer homeTimer = new Timer(); + public HoodIOSparkFlex() { - hoodMotor.setSmartCurrentLimit(10); + hoodMotor.setSmartCurrentLimit(120); hoodMotor.getPIDController().setP(ScoringConstants.hoodkP); hoodMotor.getPIDController().setI(ScoringConstants.hoodkI); @@ -27,10 +30,9 @@ public HoodIOSparkFlex() { hoodMotor.getEncoder().setPosition(0.0); hoodMotor.getEncoder().setPositionConversionFactor(ScoringConstants.hoodEncoderToRad); + hoodMotor.getEncoder().setVelocityConversionFactor(ScoringConstants.hoodEncoderToRad); hoodMotor.setIdleMode(CANSparkFlex.IdleMode.kBrake); - hoodMotor.getEncoder().setPositionConversionFactor(15.0 / 38.0 * 2.0 * Math.PI); - hoodMotor.getEncoder().setVelocityConversionFactor(15.0 / 38.0 * 2.0 * Math.PI); } @Override @@ -51,7 +53,10 @@ public void setOverrideVolts(double volts) { @Override public void home() { homing = true; - hoodMotor.setVoltage(-4); + hoodMotor.setVoltage(1.5); + + homeTimer.reset(); + homeTimer.start(); } @Override @@ -64,10 +69,13 @@ public void setPID(double p, double i, double d) { @Override public void updateInputs(HoodIOInputs inputs) { if (homing) { - if (hoodMotor.getOutputCurrent() > ScoringConstants.hoodHomeAmps) { + if (hoodMotor.getOutputCurrent() > ScoringConstants.hoodHomeAmps + && homeTimer.get() > 0.25) { hoodMotor.setVoltage(0); hoodMotor.getEncoder().setPosition(ScoringConstants.hoodHomeAngleRad); homing = false; + + homeTimer.stop(); } } else if (override) { hoodMotor.setVoltage(overrideVolts); @@ -81,6 +89,6 @@ public void updateInputs(HoodIOInputs inputs) { inputs.hoodVelocityRadPerSec = hoodMotor.getEncoder().getVelocity(); inputs.hoodAppliedVolts = hoodMotor.getAppliedOutput(); - inputs.hoodCurrentAmps = hoodMotor.getOutputCurrent(); + inputs.hoodStatorCurrentAmps = hoodMotor.getOutputCurrent(); } } diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 645d10f4..f6cc10f0 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.scoring; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; @@ -21,6 +20,7 @@ import frc.robot.utils.FieldFinder.FieldLocations; import frc.robot.utils.InterpolateDouble; import frc.robot.utils.Tunable; +import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -39,12 +39,13 @@ public class ScoringSubsystem extends SubsystemBase implements Tunable { private Supplier poseSupplier = () -> new Pose2d(); private Supplier> velocitySupplier = () -> VecBuilder.fill(0.0, 0.0); private Supplier speakerSupplier = () -> new Translation2d(0, 0); - private Supplier elevatorPositionSupplier = () -> 0.0; + private DoubleSupplier elevatorPositionSupplier = () -> 0.0; private Supplier driveAllignedSupplier = () -> true; private final InterpolateDouble shooterInterpolated; private final InterpolateDouble aimerInterpolated; private final InterpolateDouble timeToPutAimDown; + private final InterpolateDouble aimerAvoidElevator; private double shooterGoalVelocityRPMTuning = 0.0; private double aimerGoalAngleRadTuning = 0.0; @@ -83,7 +84,8 @@ public enum ScoringAction { SHOOT, ENDGAME, TUNING, - OVERRIDE + OVERRIDE, + TEMPORARY_SETPOINT } private ScoringState state = ScoringState.IDLE; @@ -108,6 +110,13 @@ public ScoringSubsystem(ShooterIO shooterIo, AimerIO aimerIo, HoodIO hoodIo) { ScoringConstants.getAimerMap(), 0.0, ScoringConstants.aimMaxAngleRadians); timeToPutAimDown = new InterpolateDouble(ScoringConstants.timeToPutAimDownMap(), 0.0, 2.0); + + aimerAvoidElevator = + new InterpolateDouble( + ScoringConstants.aimerAvoidElevatorTable(), 0.0, Math.PI / 2.0); + + // This causes elevator avoidance to work upon enable + aimerIo.setAimAngleRad(0.01); } public void setAction(ScoringAction action) { @@ -115,7 +124,7 @@ public void setAction(ScoringAction action) { } private void idle() { - aimerIo.setAimAngleRad(0, true); + aimerIo.setAimAngleRad(0.0); shooterIo.setShooterVelocityRPM(0); shooterIo.setKickerVolts(0); hoodIo.setHoodAngleRad(0); @@ -126,7 +135,7 @@ private void idle() { state = ScoringState.INTAKE; } else if (action == ScoringAction.AIM || action == ScoringAction.SHOOT) { state = ScoringState.PRIME; - aimerIo.setAimAngleRad(Math.PI / 4, true); + aimerIo.setAimAngleRad(aimerInputs.aimAngleRad); } else if (action == ScoringAction.AMP_AIM) { state = ScoringState.AMP_PRIME; } else if (action == ScoringAction.ENDGAME) { @@ -142,7 +151,7 @@ private void idle() { private void intake() { if (!aimerAtIntakePosition()) { - aimerIo.setAimAngleRad(ScoringConstants.intakeAngleToleranceRadians, true); + aimerIo.setAimAngleRad(ScoringConstants.intakeAngleToleranceRadians); } shooterIo.setKickerVolts(0); hoodIo.setHoodAngleRad(0); @@ -156,7 +165,7 @@ private void prime() { double distancetoGoal = findDistanceToGoal(); Logger.recordOutput("scoring/aimGoal", aimerInterpolated.getValue(distancetoGoal)); shooterIo.setShooterVelocityRPM(shooterInterpolated.getValue(distancetoGoal)); - aimerIo.setAimAngleRad(aimerInterpolated.getValue(distancetoGoal), false); + aimerIo.setAimAngleRad(aimerInterpolated.getValue(distancetoGoal)); boolean shooterReady = Math.abs( @@ -169,8 +178,8 @@ private void prime() { boolean driveReady = driveAllignedSupplier.get(); boolean notePresent = hasNote(); - boolean primeReady = shooterReady && aimReady && driveReady && notePresent; - readyToShoot = primeReady; + boolean primeReady = shooterReady && aimReady && driveReady; + readyToShoot = primeReady && notePresent; if (action != ScoringAction.SHOOT && action != ScoringAction.AIM) { state = ScoringState.IDLE; @@ -185,7 +194,7 @@ private void prime() { private void ampPrime() { shooterIo.setShooterVelocityRPM(ScoringConstants.shooterAmpVelocityRPM); - aimerIo.setAimAngleRad(Math.PI / 2, true); + aimerIo.setAimAngleRad(Math.PI / 2); hoodIo.setHoodAngleRad(Math.PI); boolean shooterReady = @@ -219,7 +228,7 @@ private void ampPrime() { private void shoot() { double distancetoGoal = findDistanceToGoal(); shooterIo.setShooterVelocityRPM(shooterInterpolated.getValue(distancetoGoal)); - aimerIo.setAimAngleRad(aimerInterpolated.getValue(distancetoGoal), false); + aimerIo.setAimAngleRad(aimerInterpolated.getValue(distancetoGoal)); shooterIo.setKickerVolts(5); @@ -233,22 +242,29 @@ private void shoot() { private void ampShoot() { shooterIo.setKickerVolts(5); - if (shootTimer.get() > 0.5) { // TODO: Tune time + if (shootTimer.get() > 1.0) { // TODO: Tune time state = ScoringState.AMP_PRIME; shootTimer.stop(); } } - private void endgame() { // TODO: Later - state = ScoringState.IDLE; + private void endgame() { + aimerIo.setAimAngleRad(0.0); + shooterIo.setShooterVelocityRPM(0); + shooterIo.setKickerVolts(0); + hoodIo.setHoodAngleRad(0); + + if (action != ScoringAction.ENDGAME) { + state = ScoringState.IDLE; + } } private void tuning() { shooterGoalVelocityRPMTuning = SmartDashboard.getNumber("Test-Mode/ShooterGoal", 0.0); aimerGoalAngleRadTuning = SmartDashboard.getNumber("Test-Mode/AimerGoal", 0.0); shooterIo.setShooterVelocityRPM(shooterGoalVelocityRPMTuning); - aimerIo.setAimAngleRad(aimerGoalAngleRadTuning, false); + aimerIo.setAimAngleRad(aimerGoalAngleRadTuning); hoodIo.setHoodAngleRad(0.0); shooterIo.setKickerVolts(kickerVoltsTuning); @@ -258,26 +274,21 @@ private void tuning() { } private void override() { - aimerIo.setOverrideMode(true); - hoodIo.setOverrideMode(true); - shooterIo.setOverrideMode(true); + shooterIo.setKickerVolts(kickerVoltsTuning); + + if (action == ScoringAction.TEMPORARY_SETPOINT) { + state = ScoringState.TEMPORARY_SETPOINT; + } if (action != ScoringAction.OVERRIDE) { state = ScoringState.IDLE; - - aimerIo.setOverrideMode(false); - hoodIo.setOverrideMode(false); - shooterIo.setOverrideMode(false); } } private void temporarySetpoint() { - if (MathUtil.isNear(temporarySetpointPosition, getPosition(temporarySetpointSlot), 0.1) - && MathUtil.isNear(0.0, getVelocity(temporarySetpointSlot), 0.01)) { - state = ScoringState.OVERRIDE; - - setVolts(0.0, temporarySetpointSlot); - } else if (action != ScoringAction.OVERRIDE) { + // if (MathUtil.isNear(temporarySetpointPosition, getPosition(temporarySetpointSlot), 0.1) + // && MathUtil.isNear(0.0, getVelocity(temporarySetpointSlot), 0.01)) { + if (action != ScoringAction.TEMPORARY_SETPOINT) { state = ScoringState.OVERRIDE; setVolts(0.0, temporarySetpointSlot); @@ -323,7 +334,7 @@ public void setSpeakerSupplier(Supplier speakerSupplier) { this.speakerSupplier = speakerSupplier; } - public void setElevatorPositionSupplier(Supplier elevatorPositionSupplier) { + public void setElevatorPositionSupplier(DoubleSupplier elevatorPositionSupplier) { this.elevatorPositionSupplier = elevatorPositionSupplier; } @@ -331,45 +342,46 @@ public void setDriveAllignedSupplier(Supplier driveAllignedSupplier) { this.driveAllignedSupplier = driveAllignedSupplier; } + private boolean willHitStage() { + return (FieldFinder.willIHitThis( + poseSupplier.get().getX(), + poseSupplier.get().getY(), + velocitySupplier.get().get(0, 0) + * timeToPutAimDown.getValue(aimerInputs.aimAngleRad), + velocitySupplier.get().get(1, 0) + * timeToPutAimDown.getValue(aimerInputs.aimAngleRad), + FieldLocations.BLUE_STAGE) + || FieldFinder.willIHitThis( + poseSupplier.get().getX(), + poseSupplier.get().getY(), + velocitySupplier.get().get(0, 0) + * timeToPutAimDown.getValue(aimerInputs.aimAngleRad), + velocitySupplier.get().get(1, 0) + * timeToPutAimDown.getValue(aimerInputs.aimAngleRad), + FieldLocations.RED_STAGE)); + } + @Override public void periodic() { if (state == ScoringState.TEMPORARY_SETPOINT) { - aimerIo.setAngleClampsRad(0.0, Math.PI); + aimerIo.setAngleClampsRad(0.0, ScoringConstants.aimMaxAngleRadians); } else if (state != ScoringState.TUNING && state != ScoringState.ENDGAME && !overrideStageAvoidance - && (FieldFinder.willIHitThis( - poseSupplier.get().getX(), - poseSupplier.get().getY(), - velocitySupplier.get().get(0, 0) - * timeToPutAimDown.getValue(aimerInputs.aimAngleRad), - velocitySupplier.get().get(1, 0) - * timeToPutAimDown.getValue(aimerInputs.aimAngleRad), - FieldLocations.BLUE_STAGE) - || FieldFinder.willIHitThis( - poseSupplier.get().getX(), - poseSupplier.get().getY(), - velocitySupplier.get().get(0, 0) - * timeToPutAimDown.getValue(aimerInputs.aimAngleRad), - velocitySupplier.get().get(1, 0) - * timeToPutAimDown.getValue(aimerInputs.aimAngleRad), - FieldLocations.RED_STAGE))) { + && willHitStage()) { aimerIo.setAngleClampsRad(0, 0); } else { - // Linear equation in point-slope form to calculate the arm's limit based on the - // elevator position - double maxElevatorPosition = ScoringConstants.maxElevatorPosition; // x-intercept - double maxAimAngle = ScoringConstants.maxAimAngleElevatorLimit; // y-intercept - double elevatorLimit = - (maxAimAngle / maxElevatorPosition) - * (elevatorPositionSupplier.get() - maxElevatorPosition) - + maxAimAngle; + aimerAvoidElevator.getValue(elevatorPositionSupplier.getAsDouble()); + Logger.recordOutput("scoring/elevatorPosition", elevatorPositionSupplier.getAsDouble()); + Logger.recordOutput("scoring/elevatorLimit", elevatorLimit); aimerIo.setAngleClampsRad( Math.max(0.0, elevatorLimit), ScoringConstants.aimMaxAngleRadians); } - aimerIo.controlAimAngleRad(); + aimerIo.setOverrideMode(state == ScoringState.OVERRIDE); + hoodIo.setOverrideMode(state == ScoringState.OVERRIDE); + shooterIo.setOverrideMode(state == ScoringState.OVERRIDE); Logger.recordOutput("scoring/State", state.toString()); Logger.recordOutput("scoring/Action", action.toString()); @@ -491,13 +503,13 @@ public double getConversionFactor(int slot) { switch (slot) { // Aimer case 0: - return 2 * Math.PI; + return Math.PI / 2.0; // Hood case 1: - return 2 * Math.PI; + return Math.PI; // Shooter case 2: - return 1000.0; + return 1.0; default: throw new IllegalArgumentException("Invalid slot"); } @@ -543,6 +555,34 @@ public void setPID(double p, double i, double d, int slot) { } } + @Override + public void setMaxProfileProperties(double maxVelocity, double maxAcceleration, int slot) { + switch (slot) { + // Aimer + case 0: + aimerIo.setMaxProfile(maxVelocity, maxAcceleration); + break; + default: + throw new IllegalArgumentException("Invalid slot"); + } + } + + @Override + public void setFF(double kS, double kV, double kA, double kG, int slot) { + switch (slot) { + // Aimer + case 0: + aimerIo.setFF(kS, kV, kA, kG); + break; + // Shooter + case 2: + shooterIo.setFF(kS, kV, kA); + break; + default: + throw new IllegalArgumentException("Invalid slot"); + } + } + @Override public void runToPosition(double position, int slot) { state = ScoringState.TEMPORARY_SETPOINT; @@ -553,7 +593,7 @@ public void runToPosition(double position, int slot) { switch (slot) { // Aimer case 0: - aimerIo.setAimAngleRad(temporarySetpointPosition, true); + aimerIo.setAimAngleRad(temporarySetpointPosition); break; // Hood case 1: diff --git a/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java b/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java index 1fe227fb..dbc68d0c 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java @@ -8,15 +8,17 @@ public static class ShooterIOInputs { public double shooterLeftVelocityRPM = 0.0; public double shooterLeftGoalVelocityRPM = 0.0; public double shooterLeftAppliedVolts = 0.0; - public double shooterLeftCurrentAmps = 0.0; + public double shooterLeftStatorCurrentAmps = 0.0; + public double shooterLeftSupplyCurrentAmps = 0.0; public double shooterRightVelocityRPM = 0.0; public double shooterRightGoalVelocityRPM = 0.0; public double shooterRightAppliedVolts = 0.0; - public double shooterRightCurrentAmps = 0.0; + public double shooterRightStatorCurrentAmps = 0.0; + public double shooterRightSupplyCurrentAmps = 0.0; public double kickerAppliedVolts = 0.0; - public double kickerCurrentAmps = 0.0; + public double kickerStatorCurrentAmps = 0.0; public boolean bannerSensor = false; } @@ -32,4 +34,10 @@ public default void setOverrideMode(boolean override) {} public default void setOverrideVolts(double volts) {} public default void setPID(double p, double i, double d) {} + + public default void setMaxAcceleration(double maxAcceleration) {} + + public default void setMaxJerk(double maxJerk) {} + + public default void setFF(double kS, double kV, double kA) {} } diff --git a/src/main/java/frc/robot/subsystems/scoring/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/scoring/ShooterIOSim.java index 05e195aa..68719a16 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/ShooterIOSim.java @@ -35,7 +35,7 @@ public class ShooterIOSim implements ShooterIO { private boolean override = false; - DigitalInput bannerSensor = new DigitalInput(Constants.SensorConstants.bannerPort); + DigitalInput bannerSensor = new DigitalInput(Constants.SensorConstants.upperBannerPort); double shooterLeftGoalVelRPM = 0.0; double shooterLeftAppliedVolts = 0.0; @@ -101,17 +101,17 @@ public void updateInputs(ShooterIOInputs inputs) { * ConversionConstants.kRadiansPerSecondToRPM; inputs.shooterLeftGoalVelocityRPM = shooterLeftGoalVelRPM; inputs.shooterLeftAppliedVolts = shooterLeftAppliedVolts; - inputs.shooterLeftCurrentAmps = shooterLeftSim.getCurrentDrawAmps(); + inputs.shooterLeftStatorCurrentAmps = shooterLeftSim.getCurrentDrawAmps(); inputs.shooterRightVelocityRPM = shooterRightSim.getAngularVelocityRadPerSec() * ConversionConstants.kRadiansPerSecondToRPM; inputs.shooterRightGoalVelocityRPM = shooterRightGoalVelRPM; inputs.shooterRightAppliedVolts = shooterRightAppliedVolts; - inputs.shooterRightCurrentAmps = shooterRightSim.getCurrentDrawAmps(); + inputs.shooterRightStatorCurrentAmps = shooterRightSim.getCurrentDrawAmps(); inputs.kickerAppliedVolts = kickerVolts; - inputs.kickerCurrentAmps = 0.0; + inputs.kickerStatorCurrentAmps = 0.0; inputs.bannerSensor = bannerSensor.get(); } diff --git a/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalon.java b/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalon.java index e6f1359d..35f84cb9 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalon.java +++ b/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalon.java @@ -1,14 +1,15 @@ package frc.robot.subsystems.scoring; -import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj.DigitalInput; -import frc.robot.Constants; import frc.robot.Constants.ConversionConstants; import frc.robot.Constants.ScoringConstants; +import frc.robot.Constants.SensorConstants; public class ShooterIOTalon implements ShooterIO { private final TalonFX kicker = new TalonFX(ScoringConstants.kickerMotorId); @@ -16,13 +17,9 @@ public class ShooterIOTalon implements ShooterIO { private final TalonFX shooterLeft = new TalonFX(ScoringConstants.shooterLeftMotorId); private final TalonFX shooterRight = new TalonFX(ScoringConstants.shooterRightMotorId); - MotionMagicVelocityVoltage leftController = new MotionMagicVelocityVoltage(0).withSlot(0); - MotionMagicVelocityVoltage rightController = new MotionMagicVelocityVoltage(0).withSlot(0); - - private final MotionMagicConfigs configs = new MotionMagicConfigs(); private final Slot0Configs slot0 = new Slot0Configs(); - DigitalInput bannerSensor = new DigitalInput(Constants.SensorConstants.bannerPort); + DigitalInput bannerSensor = new DigitalInput(SensorConstants.upperBannerPort); private boolean override = false; private double overrideVolts = 0.0; @@ -31,11 +28,26 @@ public class ShooterIOTalon implements ShooterIO { double goalRightVelocityRPM = 0.0; public ShooterIOTalon() { - shooterRight.setInverted(true); + kicker.setInverted(true); + + shooterLeft.setInverted(true); + shooterRight.setInverted(false); shooterLeft.setNeutralMode(NeutralModeValue.Coast); shooterRight.setNeutralMode(NeutralModeValue.Coast); + TalonFXConfigurator shooterLeftConfig = shooterLeft.getConfigurator(); + shooterLeftConfig.apply( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(120) + .withStatorCurrentLimitEnable(true)); + + TalonFXConfigurator shooterRightConfig = shooterRight.getConfigurator(); + shooterRightConfig.apply( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(120) + .withStatorCurrentLimitEnable(true)); + slot0.withKP(ScoringConstants.shooterkP); slot0.withKI(ScoringConstants.shooterkI); slot0.withKD(ScoringConstants.shooterkD); @@ -44,14 +56,8 @@ public ShooterIOTalon() { slot0.withKV(ScoringConstants.shooterkV); slot0.withKA(ScoringConstants.shooterkA); - configs.withMotionMagicAcceleration(ScoringConstants.shooterAcceleration); - configs.withMotionMagicJerk(ScoringConstants.shooterJerk); - shooterLeft.getConfigurator().apply(slot0); shooterRight.getConfigurator().apply(slot0); - - shooterLeft.getConfigurator().apply(configs); - shooterRight.getConfigurator().apply(configs); } @Override @@ -85,36 +91,44 @@ public void setPID(double p, double i, double d) { shooterRight.getConfigurator().apply(slot0); } + @Override + public void setFF(double kS, double kV, double kA) { + slot0.withKS(kS); + slot0.withKV(kV); + slot0.withKA(kA); + + shooterLeft.getConfigurator().apply(slot0); + shooterRight.getConfigurator().apply(slot0); + } + @Override public void updateInputs(ShooterIOInputs inputs) { if (override) { shooterLeft.setVoltage(overrideVolts); shooterRight.setVoltage(overrideVolts); } else { - shooterLeft.setControl( - leftController.withVelocity( - goalLeftVelocityRPM * ConversionConstants.kMinutesToSeconds)); - shooterRight.setControl( - rightController.withVelocity( - goalRightVelocityRPM * ConversionConstants.kMinutesToSeconds)); + shooterLeft.setControl(new VelocityDutyCycle(goalLeftVelocityRPM)); + shooterRight.setControl(new VelocityDutyCycle(goalRightVelocityRPM)); } inputs.shooterLeftVelocityRPM = shooterLeft.getVelocity().getValueAsDouble() - * ConversionConstants.kSecondsToMinutes; + / ConversionConstants.kSecondsToMinutes; inputs.shooterLeftGoalVelocityRPM = goalLeftVelocityRPM; inputs.shooterLeftAppliedVolts = shooterLeft.getMotorVoltage().getValueAsDouble(); - inputs.shooterLeftCurrentAmps = shooterLeft.getSupplyCurrent().getValueAsDouble(); + inputs.shooterLeftStatorCurrentAmps = shooterLeft.getStatorCurrent().getValueAsDouble(); + inputs.shooterLeftSupplyCurrentAmps = shooterLeft.getSupplyCurrent().getValueAsDouble(); inputs.shooterRightVelocityRPM = shooterRight.getVelocity().getValueAsDouble() - * ConversionConstants.kSecondsToMinutes; + / ConversionConstants.kSecondsToMinutes; inputs.shooterRightGoalVelocityRPM = goalRightVelocityRPM; inputs.shooterRightAppliedVolts = shooterRight.getMotorVoltage().getValueAsDouble(); - inputs.shooterRightCurrentAmps = shooterRight.getSupplyCurrent().getValueAsDouble(); + inputs.shooterRightStatorCurrentAmps = shooterRight.getStatorCurrent().getValueAsDouble(); + inputs.shooterRightSupplyCurrentAmps = shooterRight.getSupplyCurrent().getValueAsDouble(); inputs.kickerAppliedVolts = kicker.getMotorVoltage().getValueAsDouble(); - inputs.kickerCurrentAmps = kicker.getSupplyCurrent().getValueAsDouble(); + inputs.kickerStatorCurrentAmps = kicker.getStatorCurrent().getValueAsDouble(); inputs.bannerSensor = bannerSensor.get(); } diff --git a/src/main/java/frc/robot/utils/ControllerJSONReader.java b/src/main/java/frc/robot/utils/ControllerJSONReader.java new file mode 100644 index 00000000..940c9de0 --- /dev/null +++ b/src/main/java/frc/robot/utils/ControllerJSONReader.java @@ -0,0 +1,280 @@ +package frc.robot.utils; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; +import edu.wpi.first.wpilibj2.command.button.CommandJoystick; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import java.io.File; +import java.io.FileReader; +import java.util.HashMap; +import java.util.Iterator; +import java.util.function.DoubleSupplier; +import java.util.function.IntSupplier; +import org.json.simple.JSONArray; +import org.json.simple.JSONObject; +import org.json.simple.parser.JSONParser; + +public class ControllerJSONReader { + + private static HashMap controllers; + + private static HashMap triggers; + private static HashMap axes; + private static HashMap pov; + + // gets json objects from chosen json file + public static void pullConfiguration(String configuration) { + JSONParser jsonParser = new JSONParser(); + JSONObject jsonObject; + try { + jsonObject = + (JSONObject) + jsonParser.parse( + (new FileReader( + new File( + Filesystem.getDeployDirectory(), + "controllerconfig/" + + configuration + + ".json")))); + + } catch (Exception e) { + throw new RuntimeException("Controller not found, please try again"); + } + setControllers((JSONArray) jsonObject.get("controllers")); + setTriggers((JSONArray) jsonObject.get("buttons")); + setAxes((JSONArray) jsonObject.get("axes")); + setPOV((JSONArray) jsonObject.get("pov")); + } + + // initializes controllers based on json file data + private static HashMap setControllers(JSONArray controllersJSON) { + HashMap controllersList = + new HashMap(); + Iterator iterator = controllersJSON.iterator(); + while (iterator.hasNext()) { + // initializes each controller + JSONObject controller = iterator.next(); + // joysticks + if (((String) controller.get("type")).equals("joystick")) { + controllersList.put( + ((Long) controller.get("port")).intValue(), + new CommandJoystick(((Long) controller.get("port")).intValue())); + } else { + // xbox controller + controllersList.put( + ((Long) controller.get("port")).intValue(), + new CommandXboxController(((Long) controller.get("port")).intValue())); + } + } + controllers = controllersList; + return controllersList; + } + + // assigns triggers to string keys + private static HashMap setTriggers(JSONArray triggersJSON) { + HashMap triggerList = new HashMap(); + Iterator iterator = triggersJSON.iterator(); + while (iterator.hasNext()) { + JSONObject trigger = iterator.next(); + Trigger t; + + int port = ((Long) trigger.get("controller")).intValue(); + + // first case is for xbox controllers, second case is for joysticks + switch (((String) trigger.get("button"))) { + case "a": + case "1": + t = controllers.get(port).button(1); + break; + case "b": + case "2": + t = controllers.get(port).button(2); + break; + case "x": + case "3": + t = controllers.get(port).button(3); + break; + case "y": + case "4": + t = controllers.get(port).button(4); + break; + case "leftBumper": + case "5": + t = controllers.get(port).button(5); + break; + case "rightBumper": + case "6": + t = controllers.get(port).button(6); + break; + case "back": + case "7": + t = controllers.get(port).button(7); + break; + case "start": + case "8": + t = controllers.get(port).button(8); + break; + case "leftJoystickPress": + case "9": + t = controllers.get(port).button(9); + break; + case "rightJoystickPress": + case "10": + t = controllers.get(port).button(10); + break; + case "pov0": + t = controllers.get(port).pov(0); + break; + case "pov45": + t = controllers.get(port).pov(45); + break; + case "pov90": + t = controllers.get(port).pov(90); + break; + case "pov135": + t = controllers.get(port).pov(135); + break; + case "pov180": + t = controllers.get(port).pov(180); + break; + case "pov225": + t = controllers.get(port).pov(225); + break; + case "pov270": + t = controllers.get(port).pov(270); + break; + case "pov315": + t = controllers.get(port).pov(315); + break; + case "povCenter": + t = controllers.get(port).pov(-1); + break; + default: + t = null; + DriverStation.reportError( + "Trigger" + (String) trigger.get("button") + "not found", false); + break; + } + + triggerList.put((String) trigger.get("command"), t); + } + triggers = triggerList; + return triggerList; + } + + // assigns axis doublesuppliers to string keys + private static HashMap setAxes(JSONArray axisJSON) { + HashMap axisList = new HashMap(); + Iterator iterator = axisJSON.iterator(); + while (iterator.hasNext()) { + JSONObject axis = iterator.next(); + DoubleSupplier t; + + int port = ((Long) axis.get("controller")).intValue(); + + // first case is for xbox controller, second case is for joysticks + switch (((String) axis.get("axis"))) { + case "leftX": + case "xAxis": + if ((boolean) axis.get("negate")) + t = () -> -controllers.get(port).getRawAxis(0); + else t = () -> controllers.get(port).getRawAxis(0); + SmartDashboard.putNumber("readaxis", t.getAsDouble()); + break; + case "leftY": + case "yAxis": + if ((boolean) axis.get("negate")) + t = () -> -controllers.get(port).getRawAxis(1); + else t = () -> controllers.get(port).getRawAxis(1); + break; + case "leftTrigger": + case "zRotate": + if ((boolean) axis.get("negate")) + t = () -> -controllers.get(port).getRawAxis(2); + else t = () -> controllers.get(port).getRawAxis(2); + break; + case "rightTrigger": + case "slider": + if ((boolean) axis.get("negate")) + t = () -> -controllers.get(port).getRawAxis(3); + else t = () -> controllers.get(port).getRawAxis(3); + break; + case "rightX": + if ((boolean) axis.get("negate")) + t = () -> -controllers.get(port).getRawAxis(4); + else t = () -> controllers.get(port).getRawAxis(4); + break; + case "rightY": + if ((boolean) axis.get("negate")) + t = () -> -controllers.get(port).getRawAxis(5); + else t = () -> controllers.get(port).getRawAxis(5); + break; + default: + t = null; + DriverStation.reportError( + "Axis" + (String) axis.get("button") + "not found", false); + break; + } + + axisList.put((String) axis.get("command"), t); + } + axes = axisList; + return axisList; + } + + // assigns pov intsuppliers to string keys + private static HashMap setPOV(JSONArray povJSON) { + HashMap povList = new HashMap(); + Iterator iterator = povJSON.iterator(); + while (iterator.hasNext()) { + JSONObject p = iterator.next(); + + if (controllers.get(p.get("controller")) == null) { + povList.put((String) p.get("command"), () -> 0); + DriverStation.reportError("POV" + (String) p.get("command") + "not found", false); + } else + povList.put( + (String) p.get("command"), + () -> controllers.get(p.get("controller")).getHID().getPOV()); + } + pov = povList; + return povList; + } + + // methods to get controllers, triggers, axes, povs for robotcontainer use + + public static HashMap getControllers() { + if (controllers == null) { + throw new RuntimeException("Controllers not yet generated, run pullConfiguration"); + } else { + return controllers; + } + } + + public static HashMap getTriggers() { + if (triggers == null) { + throw new RuntimeException("Triggers not yet generated, run pullConfiguration"); + } else { + return triggers; + } + } + + public static HashMap getAxes() { + if (axes == null) { + throw new RuntimeException("Axes not yet generated, run pullConfiguration"); + } else { + return axes; + } + } + + public static HashMap getPOVs() { + if (pov == null) { + throw new RuntimeException("POVs not yet generated, run pullConfiguration"); + } else { + return pov; + } + } +} diff --git a/src/main/java/frc/robot/utils/InterpolateDouble.java b/src/main/java/frc/robot/utils/InterpolateDouble.java index 6568b01d..ac093f9a 100644 --- a/src/main/java/frc/robot/utils/InterpolateDouble.java +++ b/src/main/java/frc/robot/utils/InterpolateDouble.java @@ -23,7 +23,10 @@ public InterpolateDouble(HashMap map, double minValue, double ma this.minValue = minValue; this.maxValue = maxValue; - sortedKeys = new ArrayList(map.keySet()); + sortedKeys = new ArrayList(); + for (Double k : map.keySet()) { + sortedKeys.add(k); + } Collections.sort(sortedKeys); // Get lowest and highest keys of the HashMap @@ -68,6 +71,11 @@ public double getValue(double key) { double lowerValue = map.get(lowerKey); double upperValue = map.get(upperKey); + // Edge case if keys equal each other + if (upperKey == lowerKey) { + upperKey += 0.01; + } + double t = (key - lowerKey) / (upperKey - lowerKey); double result = lowerValue * (1.0 - t) + t * upperValue; if (result < minValue) { diff --git a/src/main/java/frc/robot/utils/Tunable.java b/src/main/java/frc/robot/utils/Tunable.java index 32740bad..bec74b48 100644 --- a/src/main/java/frc/robot/utils/Tunable.java +++ b/src/main/java/frc/robot/utils/Tunable.java @@ -17,5 +17,9 @@ public interface Tunable { public void setPID(double p, double i, double d, int slot); + public void setFF(double kS, double kV, double kA, double kG, int slot); + + public void setMaxProfileProperties(double maxVelocity, double maxAcceleration, int slot); + public void runToPosition(double position, int slot); } diff --git a/src/main/java/frc/robot/utils/feedforward/TuneV.java b/src/main/java/frc/robot/utils/feedforward/TuneV.java index 5e3f0ad4..76d9f705 100644 --- a/src/main/java/frc/robot/utils/feedforward/TuneV.java +++ b/src/main/java/frc/robot/utils/feedforward/TuneV.java @@ -33,7 +33,7 @@ public TuneV(Tunable subsystem, double volts, int slot) { this.conversionFactor = subsystem.getConversionFactor(slot); - // this.withTimeout(5); TODO: Maybe add? + this.withTimeout(5); } @Override @@ -68,6 +68,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return Math.abs(subsystem.getPosition(slot)) > 1 * conversionFactor; + return Math.abs(subsystem.getPosition(slot)) > 0.9 * conversionFactor; } }