diff --git a/src/main/deploy/practiceswerve/controllerproperties.json b/src/main/deploy/practiceswerve/controllerproperties.json deleted file mode 100644 index 901c1a2b..00000000 --- a/src/main/deploy/practiceswerve/controllerproperties.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "angleJoystickRadiusDeadband": 0.5, - "heading": { - "p": 0.5, - "i": 0, - "d": 0.05 - } -} \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/backleft.json b/src/main/deploy/practiceswerve/modules/backleft.json deleted file mode 100644 index 1f195857..00000000 --- a/src/main/deploy/practiceswerve/modules/backleft.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "location": { - "front": -12.5, - "left": 12.5 - }, - "absoluteEncoderOffset": 221.47476562500003, - "drive": { - "type": "falcon", - "id": 10, - "canbus": "Drivebase" - }, - "angle": { - "type": "falcon", - "id": 11, - "canbus": "Drivebase" - }, - "encoder": { - "type": "cancoder", - "id": 12, - "canbus": "Drivebase" - }, - "inverted": { - "drive": false, - "angle": false - } -} \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/backright.json b/src/main/deploy/practiceswerve/modules/backright.json deleted file mode 100644 index 4f5c06c2..00000000 --- a/src/main/deploy/practiceswerve/modules/backright.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "location": { - "front": -12.5, - "left": -12.5 - }, - "absoluteEncoderOffset": 173.47109375000002, - "drive": { - "type": "falcon", - "id": 7, - "canbus": "Drivebase" - }, - "angle": { - "type": "falcon", - "id": 8, - "canbus": "Drivebase" - }, - "encoder": { - "type": "cancoder", - "id": 9, - "canbus": "Drivebase" - }, - "inverted": { - "drive": false, - "angle": false - } -} \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/frontleft.json b/src/main/deploy/practiceswerve/modules/frontleft.json deleted file mode 100644 index 07269218..00000000 --- a/src/main/deploy/practiceswerve/modules/frontleft.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "location": { - "front": 12.5, - "left": 12.5 - }, - "absoluteEncoderOffset": 340.01562500000003, - "drive": { - "type": "falcon", - "id": 1, - "canbus": "Drivebase" - }, - "angle": { - "type": "falcon", - "id": 2, - "canbus": "Drivebase" - }, - "encoder": { - "type": "cancoder", - "id": 3, - "canbus": "Drivebase" - }, - "inverted": { - "drive": false, - "angle": false - } -} \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/frontright.json b/src/main/deploy/practiceswerve/modules/frontright.json deleted file mode 100644 index 41841df2..00000000 --- a/src/main/deploy/practiceswerve/modules/frontright.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "location": { - "front": 12.5, - "left": -12.5 - }, - "absoluteEncoderOffset": 357.30859375, - "drive": { - "type": "falcon", - "id": 4, - "canbus": "Drivebase" - }, - "angle": { - "type": "falcon", - "id": 5, - "canbus": "Drivebase" - }, - "encoder": { - "type": "cancoder", - "id": 6, - "canbus": "Drivebase" - }, - "inverted": { - "drive": false, - "angle": false - } -} \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/modules/physicalproperties.json b/src/main/deploy/practiceswerve/modules/physicalproperties.json deleted file mode 100644 index 0d3837f1..00000000 --- a/src/main/deploy/practiceswerve/modules/physicalproperties.json +++ /dev/null @@ -1,16 +0,0 @@ -{ - "optimalVoltage": 12, - "wheelGripCoefficientOfFriction": 1.19, - "currentLimit": { - "drive": 60, - "angle": 24 - }, - "conversionFactor": { - "angle": 19.2, - "drive": 0.06459101163105705142 - }, - "rampRate": { - "drive": 0.25, - "angle": 0.25 - } -} \ No newline at end of file diff --git a/src/main/deploy/practiceswerve/swervedrive.json b/src/main/deploy/practiceswerve/swervedrive.json deleted file mode 100644 index 93a4e126..00000000 --- a/src/main/deploy/practiceswerve/swervedrive.json +++ /dev/null @@ -1,14 +0,0 @@ -{ - "imu": { - "type": "pigeon2", - "id": 13, - "canbus": "Drivebase" - }, - "invertedIMU": false, - "modules": [ - "frontleft.json", - "frontright.json", - "backleft.json", - "backright.json" - ] -} \ No newline at end of file diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json index c40df9f9..901c1a2b 100644 --- a/src/main/deploy/swerve/controllerproperties.json +++ b/src/main/deploy/swerve/controllerproperties.json @@ -1,8 +1,8 @@ { "angleJoystickRadiusDeadband": 0.5, "heading": { - "p": 0.4, + "p": 0.5, "i": 0, - "d": 0 + "d": 0.05 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/README.txt b/src/main/deploy/swerve/modules/README.txt new file mode 100644 index 00000000..88eed69b --- /dev/null +++ b/src/main/deploy/swerve/modules/README.txt @@ -0,0 +1,24 @@ +I modifed how PID, kS, kV, and kA values are passed to the modules. +Instead of using pidfproperties.json, pass the values into the module config josns themselves +Use: + +"driveTuning": { + "p": 0, + "i": 0, + "d": 0, + "kS": 0, + "kV": 0, + "kA": 0 +}, +"angleTuning": { + "p": 0, + "i": 0, + "d": 0, + "kS": 0, + "kV": 0, + "kA": 0 +} + +in the module json config files. If the pidfproperties.json file is missing or its p value is 0, then the Swerve Drive will use these tuning configuration values instead. + +- Jonah Kowal \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index a59f11fa..d368d2e7 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -1,30 +1,42 @@ { "location": { - "front": -8.5, - "left": 8.5 + "front": -12.5, + "left": 12.5 }, - "absoluteEncoderOffset": 206.191, + "absoluteEncoderOffset": 221.47476562500003, "drive": { "type": "falcon", - "id": 7, - "canbus": null + "id": 10, + "canbus": "Drivebase" }, "angle": { "type": "falcon", - "id": 8, - "canbus": null - }, - "conversionFactor": { - "angle": 0.01373291, - "drive": 0.000019146492 + "id": 11, + "canbus": "Drivebase" }, "encoder": { "type": "cancoder", - "id": 9, - "canbus": null + "id": 12, + "canbus": "Drivebase" }, "inverted": { "drive": false, "angle": false + }, + "driveTuning": { + "p": 0.0036753, + "i": 0, + "d": 0, + "kS": 0.18532, + "kV": 0.347198082, + "kA": 0.28447 + }, + "angleTuning": { + "p": 68.706, + "i": 0, + "d": 4.8261, + "kS": 0.17859, + "kV": 2.2193, + "kA": 0.13188 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index abcdbcc9..a3d7b30d 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -1,30 +1,42 @@ { "location": { - "front": -8.5, - "left": -8.5 + "front": -12.5, + "left": -12.5 }, - "absoluteEncoderOffset": 309.99, + "absoluteEncoderOffset": 173.47109375000002, "drive": { "type": "falcon", - "id": 10, - "canbus": null + "id": 7, + "canbus": "Drivebase" }, "angle": { "type": "falcon", - "id": 11, - "canbus": null - }, - "conversionFactor": { - "angle": 0.01373291, - "drive": 0.000019146492 + "id": 8, + "canbus": "Drivebase" }, "encoder": { "type": "cancoder", - "id": 12, - "canbus": null + "id": 9, + "canbus": "Drivebase" }, "inverted": { "drive": false, "angle": false + }, + "driveTuning": { + "p": 0.055241, + "i": 0, + "d": 0, + "kS": 0.15508, + "kV": 0.35947706641, + "kA": 0.26534 + }, + "angleTuning": { + "p": 68.04, + "i": 0, + "d": 4.2632, + "kS": 0.19684, + "kV": 2.2497, + "kA": 0.07772 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index 6c00fe7c..7e101bfb 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -1,30 +1,42 @@ { "location": { - "front": 8.5, - "left": 8.5 + "front": 12.5, + "left": 12.5 }, - "absoluteEncoderOffset": 524.971, + "absoluteEncoderOffset": 340.01562500000003, "drive": { "type": "falcon", "id": 1, - "canbus": null + "canbus": "Drivebase" }, "angle": { "type": "falcon", "id": 2, - "canbus": null - }, - "conversionFactor": { - "angle": 0.01373291, - "drive": 0.000019146492 + "canbus": "Drivebase" }, "encoder": { "type": "cancoder", "id": 3, - "canbus": null + "canbus": "Drivebase" }, "inverted": { "drive": false, "angle": false + }, + "driveTuning": { + "p": 0.087084, + "i": 0, + "d": 0, + "kS": 0.026162, + "kV": 0.3849384, + "kA": 0.39611 + }, + "angleTuning": { + "p": 69.289, + "i": 0, + "d": 6.5371, + "kS": 0.1762, + "kV": 2.2121, + "kA": 0.32267 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index 33abccf2..ed88758f 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -1,30 +1,42 @@ { "location": { - "front": 8.5, - "left": -8.5 + "front": 12.5, + "left": -12.5 }, - "absoluteEncoderOffset": 249.346, + "absoluteEncoderOffset": 357.30859375, "drive": { "type": "falcon", "id": 4, - "canbus": null + "canbus": "Drivebase" }, "angle": { "type": "falcon", "id": 5, - "canbus": null - }, - "conversionFactor": { - "angle": 0.01373291, - "drive": 0.000019146492 + "canbus": "Drivebase" }, "encoder": { "type": "cancoder", "id": 6, - "canbus": null + "canbus": "Drivebase" }, "inverted": { "drive": false, "angle": false + }, + "driveTuning": { + "p": 0.76249, + "i": 0, + "d": 0, + "kS": 0.12121, + "kV": 0.368676, + "kA": 0.33215 + }, + "angleTuning": { + "p": 67.31, + "i": 0, + "d": 3.8477, + "kS": 0.19439, + "kV": 2.188, + "kA": 0.051692 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index 288ab51e..d2bb71ac 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -2,12 +2,12 @@ "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, "currentLimit": { - "drive": 40, - "angle": 20 + "drive": 60, + "angle": 24 }, "conversionFactor": { - "angle": 0.01373291, - "drive": 0.000019146492 + "angle": 19.2, + "drive": 0.0696793458626792 }, "rampRate": { "drive": 0.25, diff --git a/src/main/deploy/practiceswerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.disabled similarity index 68% rename from src/main/deploy/practiceswerve/modules/pidfproperties.json rename to src/main/deploy/swerve/modules/pidfproperties.disabled index 445eb5cb..80ef54e7 100644 --- a/src/main/deploy/practiceswerve/modules/pidfproperties.json +++ b/src/main/deploy/swerve/modules/pidfproperties.disabled @@ -3,14 +3,12 @@ "p": 2.9044, "i": 0, "d": 0, - "f": 1.9083, - "iz": 0 + "kV": 1.9083 }, "angle": { "p": 25.815, "i": 0, "d": 1.1044, - "f": 2.2918, - "iz": 0 + "kV": 2.2918 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json index 3834a362..e158d84f 100644 --- a/src/main/deploy/swerve/modules/pidfproperties.json +++ b/src/main/deploy/swerve/modules/pidfproperties.json @@ -1,16 +1,12 @@ { - "drive": { - "p": 0.0020645, - "i": 0, - "d": 0, - "f": 0, - "iz": 0 - }, - "angle": { - "p": 0.0020645, - "i": 0, - "d": 0, - "f": 0, - "iz": 0 - } -} \ No newline at end of file + "drive": { + "p": 0, + "i": 0, + "d": 0 + }, + "angle": { + "p": 35, + "i": 0, + "d": 0 + } + } \ No newline at end of file diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json index 51e71a62..93a4e126 100644 --- a/src/main/deploy/swerve/swervedrive.json +++ b/src/main/deploy/swerve/swervedrive.json @@ -1,10 +1,10 @@ { "imu": { - "type": "navx", - "id": 0, - "canbus": null + "type": "pigeon2", + "id": 13, + "canbus": "Drivebase" }, - "invertedIMU": true, + "invertedIMU": false, "modules": [ "frontleft.json", "frontright.json", diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 40ef7e62..6135891e 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -5,6 +5,7 @@ import static frc.team2412.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; import static frc.team2412.robot.Subsystems.SubsystemConstants.INTAKE_ENABLED; import static frc.team2412.robot.Subsystems.SubsystemConstants.LAUNCHER_ENABLED; +import static frc.team2412.robot.Subsystems.SubsystemConstants.LED_ENABLED; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; @@ -15,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.team2412.robot.commands.LED.LightsCommand; import frc.team2412.robot.commands.intake.AllInCommand; import frc.team2412.robot.commands.intake.AllReverseCommand; import frc.team2412.robot.commands.intake.AllStopCommand; @@ -84,7 +86,7 @@ public Controls(Subsystems s) { codriveIntakeReverseButton = codriveController.povLeft(); codriveIntakeRejectButton = codriveController.povDown(); - if (Robot.isSysIdMode() && LAUNCHER_ENABLED) { + if (Robot.isSysIdMode()) { bindSysIdControls(); return; } @@ -103,6 +105,9 @@ public Controls(Subsystems s) { if (INTAKE_ENABLED) { bindIntakeControls(); } + if (LED_ENABLED) { + bindLEDControls(); + } if (DRIVEBASE_ENABLED && LAUNCHER_ENABLED && INTAKE_ENABLED) { // temporary controls, not sure what drive team wants driveController @@ -129,6 +134,14 @@ public Controls(Subsystems s) { // codriveController.leftBumper())); } } + // LED + private void bindLEDControls() { + CommandScheduler.getInstance() + .setDefaultCommand( + s.ledSubsystem, + new LightsCommand(s.ledSubsystem, s.intakeSubsystem, s.launcherSubsystem) + .withName("Lights123")); + } // drivebase private void bindDrivebaseControls() { @@ -225,18 +238,18 @@ private void bindLauncherControls() { private void bindSysIdControls() { // only one routine can be run in one robot log // switch these between arm and flywheel in code when tuning - driveController - .leftBumper() - .whileTrue(s.launcherSubsystem.flywheelSysIdQuasistatic(Direction.kForward)); - driveController - .rightBumper() - .whileTrue(s.launcherSubsystem.flywheelSysIdQuasistatic(Direction.kReverse)); - driveController - .leftTrigger() - .whileTrue(s.launcherSubsystem.flywheelSysIdDynamic(Direction.kForward)); - driveController - .rightTrigger() - .whileTrue(s.launcherSubsystem.flywheelSysIdDynamic(Direction.kReverse)); + // driveController + // .leftBumper() + // .whileTrue(s.launcherSubsystem.flywheelSysIdQuasistatic(Direction.kForward)); + // driveController + // .rightBumper() + // .whileTrue(s.launcherSubsystem.flywheelSysIdQuasistatic(Direction.kReverse)); + // driveController + // .leftTrigger() + // .whileTrue(s.launcherSubsystem.flywheelSysIdDynamic(Direction.kForward)); + // driveController + // .rightTrigger() + // .whileTrue(s.launcherSubsystem.flywheelSysIdDynamic(Direction.kReverse)); // switch these between angle and drive tests in code when tuning driveController.x().whileTrue(s.drivebaseSubsystem.angleSysIdQuasistatic(Direction.kForward)); driveController.y().whileTrue(s.drivebaseSubsystem.angleSysIdQuasistatic(Direction.kReverse)); diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 37e9f8c5..be286999 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -28,7 +28,6 @@ public class Robot extends TimedRobot { public enum RobotType { COMPETITION, - PRACTICE, CRANE, BONK; } @@ -43,6 +42,9 @@ public static Robot getInstance() { // Really dangerous to keep this enabled as it disables all other controls, use with caution private static final boolean sysIdMode = false; + // Turns on an off auto logic + private static final boolean autoEnabled = true; + private final RobotType robotType; private final PowerDistribution PDP; public Controls controls; @@ -61,13 +63,11 @@ protected Robot() { this(getTypeFromAddress()); } - public static final MACAddress COMPETITION_ADDRESS = MACAddress.of(0x00, 0x00, 0x00); - public static final MACAddress PRACTICE_ADDRESS = MACAddress.of(0x38, 0xd9, 0x9e); + public static final MACAddress COMPETITION_ADDRESS = MACAddress.of(0x38, 0xd9, 0x9e); public static final MACAddress BONK_ADDRESS = MACAddress.of(0x33, 0x9D, 0xE7); public static final MACAddress CRANE_ADDRESS = MACAddress.of(0x22, 0xB0, 0x92); private static RobotType getTypeFromAddress() { - if (PRACTICE_ADDRESS.exists()) return RobotType.PRACTICE; if (CRANE_ADDRESS.exists()) return RobotType.CRANE; if (BONK_ADDRESS.exists()) return RobotType.BONK; if (!COMPETITION_ADDRESS.exists()) @@ -85,10 +85,14 @@ public void robotInit() { controls = new Controls(subsystems); // TODO: might be a duplicate, keep until after comp - AutoLogic.registerCommands(); + if (autoEnabled) { + AutoLogic.registerCommands(); + } if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { - AutoLogic.initShuffleBoard(); + if (autoEnabled) { + AutoLogic.initShuffleBoard(); + } } SmartDashboard.putString("current bot", getTypeFromAddress().toString()); @@ -142,8 +146,10 @@ public void autonomousInit() { // Checks if FMS is attatched and enables joystick warning if true DriverStation.silenceJoystickConnectionWarning(!DriverStation.isFMSAttached()); // System.out.println(AutoLogic.getSelected() != null); - if (AutoLogic.getSelectedAuto() != null && SubsystemConstants.DRIVEBASE_ENABLED) { - AutoLogic.getSelectedAuto().schedule(); + if (autoEnabled) { + if (AutoLogic.getSelectedAuto() != null && SubsystemConstants.DRIVEBASE_ENABLED) { + AutoLogic.getSelectedAuto().schedule(); + } } } @@ -151,6 +157,7 @@ public void autonomousInit() { public void teleopInit() { Shuffleboard.startRecording(); SignalLogger.start(); + CommandScheduler.getInstance().cancelAll(); } @Override @@ -167,7 +174,6 @@ public void teleopExit() { @Override public void disabledInit() { Shuffleboard.stopRecording(); - Command coastCommand = new WaitCommand(5) .andThen( @@ -176,7 +182,8 @@ public void disabledInit() { if (DriverStation.isDisabled()) subsystems.drivebaseSubsystem.setMotorBrake(false); })) - .ignoringDisable(true); + .ignoringDisable(true) + .withName("drivebaseCoast"); coastCommand.schedule(); } diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index 5b4f182d..077cc3fb 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -5,6 +5,7 @@ import frc.team2412.robot.sensors.AprilTagsProcessor; import frc.team2412.robot.subsystems.DrivebaseSubsystem; import frc.team2412.robot.subsystems.IntakeSubsystem; +import frc.team2412.robot.subsystems.LEDSubsystem; import frc.team2412.robot.subsystems.LauncherSubsystem; import frc.team2412.robot.subsystems.LimelightSubsystem; import frc.team2412.robot.util.DrivebaseWrapper; @@ -19,7 +20,7 @@ public static class SubsystemConstants { public static final boolean LAUNCHER_ENABLED = true; public static final boolean INTAKE_ENABLED = true; public static final boolean DRIVEBASE_ENABLED = true; - + public static final boolean LED_ENABLED = true; public static final boolean USE_APRILTAGS_CORRECTION = true; } @@ -28,6 +29,7 @@ public static class SubsystemConstants { public final LauncherSubsystem launcherSubsystem; public final LimelightSubsystem limelightSubsystem; public final IntakeSubsystem intakeSubsystem; + public final LEDSubsystem ledSubsystem; public final AprilTagsProcessor apriltagsProcessor; public Subsystems() { @@ -59,5 +61,10 @@ public Subsystems() { } else { intakeSubsystem = null; } + if (LED_ENABLED) { + ledSubsystem = new LEDSubsystem(); + } else { + ledSubsystem = null; + } } } diff --git a/src/main/java/frc/team2412/robot/commands/LED/LightsCommand.java b/src/main/java/frc/team2412/robot/commands/LED/LightsCommand.java new file mode 100644 index 00000000..f99911fd --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/LED/LightsCommand.java @@ -0,0 +1,55 @@ +package frc.team2412.robot.commands.LED; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.subsystems.IntakeSubsystem; +import frc.team2412.robot.subsystems.LEDSubsystem; +import frc.team2412.robot.subsystems.LauncherSubsystem; + +public class LightsCommand extends Command { + private final LEDSubsystem ledSubsystem; + private final IntakeSubsystem intakeSubsystem; + private final LauncherSubsystem launcherSubsystem; + + public LightsCommand( + LEDSubsystem ledSubsystem, + IntakeSubsystem intakeSubsystem, + LauncherSubsystem launcherSubsystem) { + this.ledSubsystem = ledSubsystem; + this.intakeSubsystem = intakeSubsystem; + this.launcherSubsystem = launcherSubsystem; + addRequirements(ledSubsystem); + } + + @Override + public void initialize() { + ledSubsystem.setRED_LED(); + } + + @Override + public void execute() { + if (launcherSubsystem != null && intakeSubsystem != null) { + if (launcherSubsystem.isAtSpeed() + && intakeSubsystem.feederSensorHasNote()) { // Checks if launcher is ready + ledSubsystem.setGREEN_LED(); + } else if (intakeSubsystem.feederSensorHasNote()) { + ledSubsystem.setVIOLET_LED(); + } else if (intakeSubsystem.indexSensorHasNote()) { // Checks if note is in feeder + ledSubsystem.setBLUE_LED(); + } else if (intakeSubsystem.isIntakeOn()) { // Checks if intake is on + ledSubsystem.setYELLOW_LED(); + } else { // Everything else including intake off + ledSubsystem.setRED_LED(); + } + } + } + + @Override + public void end(boolean interrupted) { + ledSubsystem.disableLED(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 70d1924b..2a6e8f2e 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -48,7 +48,7 @@ public class DrivebaseSubsystem extends SubsystemBase { public static final double MAX_SPEED = Robot.getInstance().getRobotType() == RobotType.BONK ? 3.0 - : Robot.getInstance().getRobotType() == RobotType.PRACTICE + : Robot.getInstance().getRobotType() == RobotType.COMPETITION ? 6.0 : Robot.getInstance().getRobotType() == RobotType.CRANE ? 3.0 : 1.0; @@ -63,14 +63,14 @@ public class DrivebaseSubsystem extends SubsystemBase { ? 0.305328701 : Robot.getInstance().getRobotType() == RobotType.CRANE ? 0.3937 - : Robot.getInstance().getRobotType() == RobotType.PRACTICE ? 0.3 : 0.3; + : Robot.getInstance().getRobotType() == RobotType.COMPETITION ? 0.3 : 0.3; private static final double JOYSTICK_DEADBAND = 0.05; private static final double HEADING_CORRECTION_DEADBAND = 0.005; // AUTO CONSTANTS private static final PIDConstants AUTO_TRANSLATION_PID = - Robot.getInstance().getRobotType() == RobotType.PRACTICE + Robot.getInstance().getRobotType() == RobotType.COMPETITION ? new PIDConstants(5, 0, 0.5) // practice : Robot.getInstance().getRobotType() == RobotType.BONK ? new PIDConstants(6, 0, 0.1) // bonk @@ -100,10 +100,6 @@ public DrivebaseSubsystem() { File swerveJsonDirectory; switch (Robot.getInstance().getRobotType()) { - case PRACTICE: - swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "practiceswerve"); - System.out.println("Running practice swerve"); - break; case CRANE: swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "craneswerve"); System.out.println("Running crane swerve"); diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 696d75df..396be013 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -252,6 +252,13 @@ public boolean getRejectOverride() { return rejectOverride.getBoolean(false); } + public boolean isIntakeOn() { + return (intakeMotorFront.get() > 0 + || indexMotorUpper.get() > 0 + || ingestMotor.get() > 0 + || feederMotor.get() > 0); + } + // logging public void initShuffleboard() { if (Robot.isDebugMode()) { diff --git a/src/main/java/frc/team2412/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LEDSubsystem.java index 58f343f1..a1a64ca9 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LEDSubsystem.java @@ -10,6 +10,7 @@ public class LEDSubsystem extends SubsystemBase { private static final int GREEN_LED_COLOR = 1885; private static final int BLUE_LED_COLOR = 1935; private static final int YELLOW_LED_COLOR = 1845; + private static final int VIOLET_LED_COLOR = 1955; private final PWM blinkin; @@ -22,6 +23,14 @@ private void setLED(int color) { blinkin.setPulseTimeMicroseconds(color); } + public void disableLED() { + blinkin.setDisabled(); + } + + public void setRED_LED() { + setLED(RED_LED_COLOR); + } + public void setGREEN_LED() { setLED(GREEN_LED_COLOR); } @@ -33,4 +42,8 @@ public void setBLUE_LED() { public void setYELLOW_LED() { setLED(YELLOW_LED_COLOR); } + + public void setVIOLET_LED() { + setLED(VIOLET_LED_COLOR); + } } diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 6cc21cbc..6904362a 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -105,6 +105,8 @@ public class LauncherSubsystem extends SubsystemBase { private GenericEntry angleSetpointEntry; + private GenericEntry launcherFlywheelSetpointEntry; + // Constructors public LauncherSubsystem() { @@ -358,6 +360,8 @@ private void initShuffleboard() { angleSetpointEntry = Shuffleboard.getTab("Launcher").add("Angle Setpoint", 0).withPosition(2, 2).getEntry(); + launcherFlywheelSetpointEntry = + Shuffleboard.getTab("Launcher").add("Flywheel Setpoint", 0).withPosition(4, 5).getEntry(); } public void updateDistanceEntry(double distance) { @@ -372,6 +376,7 @@ public void periodic() { launcherIsAtSpeed.setBoolean(isAtSpeed()); launcherAngleManual.setDouble(manualAngleSetpoint); angleSetpointEntry.setDouble(angleSetpoint); + launcherFlywheelSetpointEntry.setDouble(rpmSetpoint); // sanity check the pivot encoder if (launcherAngleEncoder.getPosition() >= PIVOT_SOFTSTOP_FORWARD + PIVOT_DISABLE_OFFSET diff --git a/src/main/java/frc/team2412/robot/util/SparkPIDWidget.java b/src/main/java/frc/team2412/robot/util/SparkPIDWidget.java index 61d9d88b..d4cd2b25 100644 --- a/src/main/java/frc/team2412/robot/util/SparkPIDWidget.java +++ b/src/main/java/frc/team2412/robot/util/SparkPIDWidget.java @@ -17,7 +17,7 @@ public SparkPIDWidget(SparkPIDController controller, String name) { @Override public void initSendable(NTSendableBuilder builder) { - if (Robot.getInstance().getRobotType() != Robot.RobotType.PRACTICE) { + if (Robot.getInstance().getRobotType() != Robot.RobotType.COMPETITION) { // builder.setSmartDashboardType("PIDController"); // builder.addDoubleProperty("p", controller::getP, controller::setP); diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index a5f8025a..741c7aae 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -233,7 +233,7 @@ public void configurePIDF(PIDFConfig config) { configureSparkFlex(() -> pid.setP(config.p, pidSlot)); configureSparkFlex(() -> pid.setI(config.i, pidSlot)); configureSparkFlex(() -> pid.setD(config.d, pidSlot)); - configureSparkFlex(() -> pid.setFF(config.f, pidSlot)); + configureSparkFlex(() -> pid.setFF(config.kV, pidSlot)); configureSparkFlex(() -> pid.setIZone(config.iz, pidSlot)); configureSparkFlex(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); } diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index 879412bd..2582134d 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -267,7 +267,7 @@ public void configurePIDF(PIDFConfig config) { configureSparkMax(() -> pid.setP(config.p, pidSlot)); configureSparkMax(() -> pid.setI(config.i, pidSlot)); configureSparkMax(() -> pid.setD(config.d, pidSlot)); - configureSparkMax(() -> pid.setFF(config.f, pidSlot)); + configureSparkMax(() -> pid.setFF(config.kV, pidSlot)); configureSparkMax(() -> pid.setIZone(config.iz, pidSlot)); configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); } diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 88ecef47..d52b34bd 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -217,7 +217,7 @@ public void configurePIDF(PIDFConfig config) { configureSparkMax(() -> pid.setP(config.p)); configureSparkMax(() -> pid.setI(config.i)); configureSparkMax(() -> pid.setD(config.d)); - configureSparkMax(() -> pid.setFF(config.f)); + configureSparkMax(() -> pid.setFF(config.kV)); configureSparkMax(() -> pid.setIZone(config.iz)); configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max)); } diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index ef6328fb..ace788bd 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -213,7 +213,27 @@ public void configurePIDF(PIDFConfig config) { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.Slot0); cfg.apply( - configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f)); + configuration.Slot0.withKP(config.p) + .withKI(config.i) + .withKD(config.d) + .withKS(config.kS) + .withKV(config.kV) + .withKA(config.kA)); + System.out.println( + "Configured drivebase motor " + + motor.getDeviceID() + + " with P: " + + config.p + + ", I: " + + config.i + + ", D:" + + config.d + + ", kS: " + + config.kS + + ", kV: " + + config.kV + + ", kA: " + + config.kA); // configuration.slot0.integralZone = config.iz; // configuration.slot0.closedLoopPeakOutput = config.output.max; } diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java index 694cfd2a..060680b9 100644 --- a/src/main/java/swervelib/motors/TalonSRXSwerve.java +++ b/src/main/java/swervelib/motors/TalonSRXSwerve.java @@ -173,7 +173,7 @@ public void configurePIDF(PIDFConfig config) { configuration.slot0.kP = config.p; configuration.slot0.kI = config.i; configuration.slot0.kD = config.d; - configuration.slot0.kF = config.f; + configuration.slot0.kF = config.kS; configuration.slot0.integralZone = config.iz; configuration.slot0.closedLoopPeakOutput = config.output.max; configChanged = true; diff --git a/src/main/java/swervelib/parser/PIDFConfig.java b/src/main/java/swervelib/parser/PIDFConfig.java index b6451aa6..0f800fd8 100644 --- a/src/main/java/swervelib/parser/PIDFConfig.java +++ b/src/main/java/swervelib/parser/PIDFConfig.java @@ -12,8 +12,10 @@ public class PIDFConfig { public double i; /** Derivative Gain for PID. */ public double d; - /** Feedforward value for PID. */ - public double f; + + public double kS; + public double kV; + public double kA; /** Integral zone of the PID. */ public double iz; @@ -23,21 +25,31 @@ public class PIDFConfig { /** Used when parsing PIDF values from JSON. */ public PIDFConfig() {} + public PIDFConfig(double p, double i, double d, double kS, double kV, double kA, double iz) { + this.p = p; + this.i = i; + this.d = d; + this.kS = kS; + this.kV = kV; + this.kA = kA; + this.iz = iz; + } + + public PIDFConfig(double p, double i, double d, double kS, double kV, double kA) { + this(p, i, d, kS, kV, kA, 0); + } + /** * PIDF Config constructor to contain the values. * * @param p P gain. * @param i I gain. * @param d D gain. - * @param f F gain. + * @param kV kV gain * @param iz Intergral zone. */ - public PIDFConfig(double p, double i, double d, double f, double iz) { - this.p = p; - this.i = i; - this.d = d; - this.f = f; - this.iz = iz; + public PIDFConfig(double p, double i, double d, double kV, double iz) { + this(p, i, d, 0, kV, 0, iz); } /** @@ -46,10 +58,10 @@ public PIDFConfig(double p, double i, double d, double f, double iz) { * @param p P gain. * @param i I gain. * @param d D gain. - * @param f F gain. + * @param f kV gain. */ - public PIDFConfig(double p, double i, double d, double f) { - this(p, i, d, f, 0); + public PIDFConfig(double p, double i, double d, double kV) { + this(p, i, d, kV, 0); } /** diff --git a/src/main/java/swervelib/parser/SwerveParser.java b/src/main/java/swervelib/parser/SwerveParser.java index 3c4517f6..f4b7b743 100644 --- a/src/main/java/swervelib/parser/SwerveParser.java +++ b/src/main/java/swervelib/parser/SwerveParser.java @@ -187,8 +187,8 @@ public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, do ModuleJson module = moduleJsons[i]; moduleConfigurations[i] = module.createModuleConfiguration( - pidfPropertiesJson.angle, - pidfPropertiesJson.drive, + pidfPropertiesJson.angle.p == 0 ? module.angleTuning : pidfPropertiesJson.angle, + pidfPropertiesJson.drive.p == 0 ? module.driveTuning : pidfPropertiesJson.drive, physicalPropertiesJson.createPhysicalProperties(), swerveDriveJson.modules[i]); } diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index 10119615..2104672f 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -18,6 +18,10 @@ public class ModuleJson { public DeviceJson drive; /** Angle motor device configuration. */ public DeviceJson angle; + + public PIDFConfig driveTuning; + public PIDFConfig angleTuning; + /** * Conversion factor for the module, if different from the one in swervedrive.json * diff --git a/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java b/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java index 68ab9a99..2732ad1f 100644 --- a/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java +++ b/src/main/java/swervelib/parser/json/PIDFPropertiesJson.java @@ -3,6 +3,7 @@ import swervelib.parser.PIDFConfig; /** {@link swervelib.SwerveModule} PID with Feedforward for the drive motor and angle motor. */ +@Deprecated public class PIDFPropertiesJson { /** The PIDF with Integral Zone used for the drive motor. */