From d0fe848879b731ab10a3e68fc7c176bfb6f94dab Mon Sep 17 00:00:00 2001 From: natha Date: Mon, 26 Aug 2024 10:35:52 -0400 Subject: [PATCH 1/8] Created basic state machine for shooter/intake subsystem --- .../shooter_intake/ShooterIntakeIO.java | 11 ++++ .../ShooterIntakeSubsystem.java | 54 +++++++++++++++++++ 2 files changed, 65 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java new file mode 100644 index 0000000..0f69b19 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.shooter_intake; + + + +public interface ShooterIntakeIO { + + public void setVoltage(double volts); + + public double getVoltage(double volts); + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java new file mode 100644 index 0000000..2f01d65 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java @@ -0,0 +1,54 @@ +package frc.robot.subsystems.shooter_intake; + +public class ShooterIntakeSubsystem { + + State state = State.IDLE; + State newState = State.IDLE; + ShooterIntakeIO io; + + public ShooterIntakeSubsystem(ShooterIntakeIO io){ + this.io = io; + } + + public void periodic(){ + + state.periodic(this); + + } + + public void setNewState(State newState){ + this.newState = newState; + } + + private enum State{ + IDLE{ + @Override + void changeStates(ShooterIntakeSubsystem subsystem){ + subsystem.state = subsystem.newState; + } + + @Override + void periodic(ShooterIntakeSubsystem subsystem){ + super.periodic(subsystem); + subsystem.io.setVoltage(0); + } + }, + + INTAKING{ + //TODO add periodic for intake + }, + + SHOOTING{ + //TODO add periodic for shooting + }; + + void periodic(ShooterIntakeSubsystem subsystem){ + this.changeStates(subsystem); + } + + void changeStates(ShooterIntakeSubsystem subsystem){ + subsystem.state = subsystem.newState; + } + } + +} From 50102e49a9c88f7a1addebe99aa51bdd58a19694 Mon Sep 17 00:00:00 2001 From: natha Date: Mon, 26 Aug 2024 18:15:48 -0400 Subject: [PATCH 2/8] Improved implementation of State Machine --- .../shooter_intake/ShooterIntakeIO.java | 6 ++- .../ShooterIntakeSubsystem.java | 47 ++++++++++++------- 2 files changed, 35 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java index 0f69b19..df63bda 100644 --- a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java @@ -4,8 +4,10 @@ public interface ShooterIntakeIO { - public void setVoltage(double volts); + public static class ShooterIntakeIOInputs { + public double intakeVoltage = 0.0; + } - public double getVoltage(double volts); + public void setVoltage(double volts); } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java index 2f01d65..54d45c7 100644 --- a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java @@ -2,53 +2,68 @@ public class ShooterIntakeSubsystem { - State state = State.IDLE; - State newState = State.IDLE; + protected State currentState = State.IDLE; + protected State targetState = State.IDLE; ShooterIntakeIO io; public ShooterIntakeSubsystem(ShooterIntakeIO io){ this.io = io; + State.subsystem = this; } public void periodic(){ - state.periodic(this); + currentState.periodic(); } - public void setNewState(State newState){ - this.newState = newState; + public void setTargetState(State target){ + this.targetState = target; } - private enum State{ + public enum State{ IDLE{ @Override - void changeStates(ShooterIntakeSubsystem subsystem){ - subsystem.state = subsystem.newState; + void changeStates(){ + subsystem.currentState = subsystem.targetState; } @Override - void periodic(ShooterIntakeSubsystem subsystem){ - super.periodic(subsystem); + void onStart(){ subsystem.io.setVoltage(0); } }, INTAKING{ - //TODO add periodic for intake + @Override + void periodic(){ + super.periodic(); + subsystem.io.setVoltage(-1); + } }, SHOOTING{ - //TODO add periodic for shooting + @Override + void periodic(){ + super.periodic(); + subsystem.io.setVoltage(1); + } }; - void periodic(ShooterIntakeSubsystem subsystem){ - this.changeStates(subsystem); + static ShooterIntakeSubsystem subsystem; + + void periodic(){ + this.changeStates(); } - void changeStates(ShooterIntakeSubsystem subsystem){ - subsystem.state = subsystem.newState; + void changeStates(){ + if (subsystem.currentState != subsystem.targetState){ + subsystem.currentState = State.IDLE; + } } + + void onStart() {}; + void onEnd() {}; } } From eb8341657c77228c5a1c906110f96d8b507a5001 Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Mon, 26 Aug 2024 19:34:18 -0400 Subject: [PATCH 3/8] Fixed Shooter and Intake --- build.gradle | 2 +- .../shooter_intake/ShooterIntakeIO.java | 5 +-- .../ShooterIntakeSubsystem.java | 43 +++++++++++-------- vendordeps/AdvantageKit.json | 10 ++--- 4 files changed, 33 insertions(+), 27 deletions(-) diff --git a/build.gradle b/build.gradle index d5dc6c0..9d72dc9 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" id "com.diffplug.spotless" version "6.24.0" } diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java index df63bda..8d396dd 100644 --- a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.shooter_intake; - - public interface ShooterIntakeIO { public static class ShooterIntakeIOInputs { @@ -9,5 +7,4 @@ public static class ShooterIntakeIOInputs { } public void setVoltage(double volts); - -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java index 54d45c7..5866eb2 100644 --- a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java @@ -6,45 +6,46 @@ public class ShooterIntakeSubsystem { protected State targetState = State.IDLE; ShooterIntakeIO io; - public ShooterIntakeSubsystem(ShooterIntakeIO io){ + public ShooterIntakeSubsystem(ShooterIntakeIO io) { this.io = io; State.subsystem = this; } - public void periodic(){ + public void periodic() { currentState.periodic(); - } - public void setTargetState(State target){ + public void setTargetState(State target) { this.targetState = target; } - public enum State{ - IDLE{ + // spotless:off + public enum State { + IDLE { + @Override - void changeStates(){ - subsystem.currentState = subsystem.targetState; + void changeStates() { + this.changeState(subsystem.targetState); } @Override - void onStart(){ + void onStart() { subsystem.io.setVoltage(0); } }, - INTAKING{ + INTAKING { @Override - void periodic(){ + void periodic() { super.periodic(); subsystem.io.setVoltage(-1); } }, - SHOOTING{ + SHOOTING { @Override - void periodic(){ + void periodic() { super.periodic(); subsystem.io.setVoltage(1); } @@ -52,18 +53,26 @@ void periodic(){ static ShooterIntakeSubsystem subsystem; - void periodic(){ + void periodic() { this.changeStates(); } - void changeStates(){ - if (subsystem.currentState != subsystem.targetState){ - subsystem.currentState = State.IDLE; + void changeStates() { + if (this != subsystem.targetState) { + this.changeState(IDLE); } } void onStart() {}; + void onEnd() {}; + + protected void changeState(State newState){ + this.onEnd(); + subsystem.currentState = newState; + newState.onStart(); + } } + // spotless:on } diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index cca06ae..63dacbb 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.0.0", + "version": "3.2.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.0.0" + "version": "3.2.1" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.0.0" + "version": "3.2.1" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.0.0" + "version": "3.2.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.0.0", + "version": "3.2.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ From 85d944c937cd0bb312d25e10e1c16ef5395fd060 Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Tue, 27 Aug 2024 22:04:53 -0400 Subject: [PATCH 4/8] Simulation testing implementation --- simgui.json | 83 +++++++++++++++++++++ src/main/java/frc/robot/Robot.java | 18 +++++ src/main/java/frc/robot/RobotContainer.java | 13 ++++ 3 files changed, 114 insertions(+) diff --git a/simgui.json b/simgui.json index ffee927..f96f70d 100644 --- a/simgui.json +++ b/simgui.json @@ -1,4 +1,11 @@ { + "HALProvider": { + "Timing": { + "window": { + "visible": false + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", @@ -6,7 +13,25 @@ } }, "NetworkTables": { + "Retained Values": { + "open": false + }, "transitory": { + "AdvantageKit": { + "DriverStation": { + "Joystick0": { + "open": true + }, + "Joystick1": { + "open": true + }, + "Joystick5": { + "open": true + }, + "open": true + }, + "open": true + }, "CameraPublisher": { "open": true }, @@ -19,6 +44,64 @@ } }, "NetworkTables Info": { + "Server": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + } + }, "visible": true + }, + "NetworkTables View": { + "visible": false + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 151, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/AdvantageKit/DriverStation/Joystick0/ButtonCount" + }, + { + "color": [ + 0.8666667342185974, + 0.5176470875740051, + 0.32156863808631897, + 1.0 + ], + "id": "NT:/AdvantageKit/Timestamp" + } + ] + }, + { + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 151 + } + ], + "window": { + "visible": false + } + } } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 66b2471..9188293 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,10 @@ package frc.robot; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import org.littletonrobotics.junction.LogFileUtil; @@ -24,6 +28,11 @@ public class Robot extends LoggedRobot { private RobotContainer robotContainer; + Pose2d pose2d = new Pose2d(0.0, 0.0, new Rotation2d(0.0, 0.0)); + Transform2d transform2d = + new Transform2d(new Translation2d(0.02, 0.0), new Rotation2d(0.0, 0.0)); + int count = 0; + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -52,6 +61,8 @@ public void robotInit() { } Logger.start(); + Logger.recordOutput("randomTesting", new Pose2d(0.0, 0.0, new Rotation2d(0.0, 0.0))); + robotContainer = new RobotContainer(); } @@ -68,7 +79,14 @@ public void robotPeriodic() { // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + Logger.recordOutput("randomCount", pose2d); + Transform2d modifier = transform2d.times(Math.sin(count / 100.0)); + pose2d = pose2d.transformBy(modifier); + Logger.recordOutput("joystickExpected", Math.sin(count / 100.0)); + + count++; } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 91ff416..0aa1317 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.OperatorConstants; +import frc.robot.subsystems.shooter_intake.ShooterIntakeIO; +import frc.robot.subsystems.shooter_intake.ShooterIntakeSubsystem; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -19,6 +21,17 @@ public class RobotContainer { private DoubleSupplier rightDistanceSupplier; private Supplier gyroSupplier; private Supplier simulatedPoseSupplier; + private ShooterIntakeSubsystem intakeSubsystem = + new ShooterIntakeSubsystem( + new ShooterIntakeIO() { + + @Override + public void setVoltage(double volts) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException( + "Unimplemented method 'setVoltage'"); + } + }); private final CommandXboxController driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); From d21c79c1bcd5e3825a01ec9963e7d4b2e74cca65 Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Thu, 5 Sep 2024 23:56:37 -0400 Subject: [PATCH 5/8] Simulation IO and cleanup --- .Glass/glass.json | 36 ++++++ simgui-ds.json | 7 ++ simgui.json | 13 ++ src/main/java/frc/robot/Robot.java | 14 +-- src/main/java/frc/robot/RobotContainer.java | 18 +-- .../shooter_intake/ShooterIntakeIO.java | 19 ++- .../shooter_intake/ShooterIntakeIOSim.java | 57 +++++++++ .../ShooterIntakeSubsystem.java | 117 ++++++++++++++---- 8 files changed, 228 insertions(+), 53 deletions(-) create mode 100644 .Glass/glass.json create mode 100644 src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java diff --git a/.Glass/glass.json b/.Glass/glass.json new file mode 100644 index 0000000..cc09156 --- /dev/null +++ b/.Glass/glass.json @@ -0,0 +1,36 @@ +{ + "NetworkTables": { + "transitory": { + "LiveWindow": { + ".status": { + "open": true + }, + "open": true + } + }, + "types": { + "/FMSInfo": "FMSInfo", + "/LiveWindow/ShooterIntakeSubsystem/testMaxMotor": "Motor Controller", + "/LiveWindow/Ungrouped/Scheduler": "Scheduler" + }, + "windows": { + "/FMSInfo": { + "window": { + "visible": true + } + }, + "/LiveWindow/ShooterIntakeSubsystem/testMaxMotor": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables Info": { + "visible": true + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "127.0.0.1" + } +} diff --git a/simgui-ds.json b/simgui-ds.json index 2bf389e..c79569a 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -97,6 +97,13 @@ "robotJoysticks": [ { "guid": "Keyboard0" + }, + {}, + { + "guid": "Keyboard2" + }, + { + "guid": "Keyboard1" } ] } diff --git a/simgui.json b/simgui.json index f96f70d..185b996 100644 --- a/simgui.json +++ b/simgui.json @@ -9,7 +9,17 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", + "/LiveWindow/ShooterIntakeSubsystem": "Subsystem", + "/LiveWindow/ShooterIntakeSubsystem/testMaxMotor": "Motor Controller", + "/LiveWindow/Ungrouped/Scheduler": "Scheduler", "/SmartDashboard/VisionSystemSim-/Sim Field": "Field2d" + }, + "windows": { + "/FMSInfo": { + "window": { + "visible": true + } + } } }, "NetworkTables": { @@ -44,6 +54,9 @@ } }, "NetworkTables Info": { + "Clients": { + "open": true + }, "Server": { "Publishers": { "open": true diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9188293..e370be1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,9 +7,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.shooter_intake.ShooterIntakeSubsystem; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -28,17 +28,13 @@ public class Robot extends LoggedRobot { private RobotContainer robotContainer; - Pose2d pose2d = new Pose2d(0.0, 0.0, new Rotation2d(0.0, 0.0)); - Transform2d transform2d = - new Transform2d(new Translation2d(0.02, 0.0), new Rotation2d(0.0, 0.0)); - int count = 0; - /** * This function is run when the robot is first started up and should be used for any * initialization code. */ @Override public void robotInit() { + enableLiveWindowInTest(true); // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. Logger.recordMetadata("ProjectName", "2024 - 401 Comp Robot"); @@ -61,7 +57,6 @@ public void robotInit() { } Logger.start(); - Logger.recordOutput("randomTesting", new Pose2d(0.0, 0.0, new Rotation2d(0.0, 0.0))); robotContainer = new RobotContainer(); } @@ -81,12 +76,7 @@ public void robotPeriodic() { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - Logger.recordOutput("randomCount", pose2d); - Transform2d modifier = transform2d.times(Math.sin(count / 100.0)); - pose2d = pose2d.transformBy(modifier); - Logger.recordOutput("joystickExpected", Math.sin(count / 100.0)); - count++; } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0aa1317..9b5c4c9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.OperatorConstants; -import frc.robot.subsystems.shooter_intake.ShooterIntakeIO; +import frc.robot.subsystems.shooter_intake.ShooterIntakeIOSim; import frc.robot.subsystems.shooter_intake.ShooterIntakeSubsystem; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -21,23 +21,13 @@ public class RobotContainer { private DoubleSupplier rightDistanceSupplier; private Supplier gyroSupplier; private Supplier simulatedPoseSupplier; - private ShooterIntakeSubsystem intakeSubsystem = - new ShooterIntakeSubsystem( - new ShooterIntakeIO() { - - @Override - public void setVoltage(double volts) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException( - "Unimplemented method 'setVoltage'"); - } - }); + ShooterIntakeSubsystem intakeSubsystem = new ShooterIntakeSubsystem(new ShooterIntakeIOSim()); private final CommandXboxController driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); - private Joystick leftJoystick = new Joystick(0); - private Joystick rightJoystick = new Joystick(1); + Joystick leftJoystick = new Joystick(0); + Joystick rightJoystick = new Joystick(1); // private final LoggedDashboardChooser autoChooser; diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java index 8d396dd..bf74996 100644 --- a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java @@ -1,10 +1,25 @@ package frc.robot.subsystems.shooter_intake; +import org.littletonrobotics.junction.AutoLog; + public interface ShooterIntakeIO { + @AutoLog public static class ShooterIntakeIOInputs { - public double intakeVoltage = 0.0; + public double flywheelTargetSpeed = 0.0; + public double flywheelSpeed = 0.0; + public boolean flywheelPowered = false; } - public void setVoltage(double volts); + public void periodic(); + + public void setTargetSpeed(double rpm); + + public void setFlywheelPowered(boolean powered); + + public double getSpeed(); + + public boolean atSpeed(); + + public void setInputs(ShooterIntakeIOInputsAutoLogged shooterIntakeIOInputs); } diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java new file mode 100644 index 0000000..dd6b1cc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java @@ -0,0 +1,57 @@ +package frc.robot.subsystems.shooter_intake; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import org.littletonrobotics.junction.Logger; + +public class ShooterIntakeIOSim implements ShooterIntakeIO { + + FlywheelSim flywheelSim = new FlywheelSim(DCMotor.getCIM(2), 1.0, 1.0); + PIDController controller = new PIDController(0.75, 0.1, 0.0); + ShooterIntakeIOInputsAutoLogged shooterIntakeIOInputs; + + @Override + public void periodic() { + flywheelSim.update(0.02); + if (shooterIntakeIOInputs.flywheelPowered) { + controller.setSetpoint(shooterIntakeIOInputs.flywheelTargetSpeed); + } else { + controller.setSetpoint(0.0); + } + double calculatedVoltage = controller.calculate(getSpeed()); + calculatedVoltage = MathUtil.clamp(calculatedVoltage, -12, 12); + flywheelSim.setInputVoltage(calculatedVoltage); + Logger.recordOutput("Shooter.flyWheel.appliedVolts", calculatedVoltage); + shooterIntakeIOInputs.flywheelSpeed = flywheelSim.getAngularVelocityRPM(); + } + + @Override + public void setTargetSpeed(double rpm) { + shooterIntakeIOInputs.flywheelTargetSpeed = rpm; + } + + @Override + public void setFlywheelPowered(boolean powered) { + shooterIntakeIOInputs.flywheelPowered = powered; + } + + @Override + public double getSpeed() { + return flywheelSim.getAngularVelocityRPM(); + } + + @Override + public boolean atSpeed() { + if (shooterIntakeIOInputs.flywheelTargetSpeed > 0) { + return (shooterIntakeIOInputs.flywheelTargetSpeed <= getSpeed()); + } + return (shooterIntakeIOInputs.flywheelTargetSpeed >= getSpeed()); + } + + @Override + public void setInputs(ShooterIntakeIOInputsAutoLogged shooterIntakeIOInputs) { + this.shooterIntakeIOInputs = shooterIntakeIOInputs; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java index 5866eb2..14cf743 100644 --- a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java @@ -1,19 +1,31 @@ package frc.robot.subsystems.shooter_intake; -public class ShooterIntakeSubsystem { +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class ShooterIntakeSubsystem extends SubsystemBase { + + // Move to constants later + double intakeTargetRPM = -100.0; + double shootingTargetRPM = 300.0; + // protected State currentState = State.IDLE; protected State targetState = State.IDLE; ShooterIntakeIO io; + static ShooterIntakeIOInputsAutoLogged shooterIntakeIOInputs = + new ShooterIntakeIOInputsAutoLogged(); public ShooterIntakeSubsystem(ShooterIntakeIO io) { this.io = io; - State.subsystem = this; + io.setInputs(shooterIntakeIOInputs); } public void periodic() { - - currentState.periodic(); + currentState.periodic(this); + io.periodic(); + Logger.recordOutput("Shooter.flyWheel.CurrentState", currentState); + Logger.recordOutput("Shooter.flyWheel.speed", io.getSpeed()); } public void setTargetState(State target) { @@ -25,52 +37,107 @@ public enum State { IDLE { @Override - void changeStates() { - this.changeState(subsystem.targetState); + void handleStateChanges(ShooterIntakeSubsystem subsystem) { + if (subsystem.targetState == this) return; + switch (subsystem.targetState){ + case SHOOTING: + if (!SHOOTING_PREP.canStart(subsystem)) return; + this.transitionToState(subsystem, SHOOTING_PREP); + break; + default: + if (!subsystem.targetState.canStart(subsystem)) return; + this.transitionToState(subsystem, subsystem.targetState); + } } @Override - void onStart() { - subsystem.io.setVoltage(0); + void onStart(ShooterIntakeSubsystem subsystem) { + subsystem.io.setFlywheelPowered(false); } }, INTAKING { + + @Override + void onStart(ShooterIntakeSubsystem subsystem) { + subsystem.io.setFlywheelPowered(true); + } + + @Override + void periodic(ShooterIntakeSubsystem subsystem) { + super.periodic(subsystem); + subsystem.io.setTargetSpeed(subsystem.intakeTargetRPM); + } + }, + + SHOOTING_PREP { + @Override - void periodic() { - super.periodic(); - subsystem.io.setVoltage(-1); + void onStart(ShooterIntakeSubsystem subsystem) { + subsystem.io.setFlywheelPowered(true); + subsystem.io.setTargetSpeed(subsystem.shootingTargetRPM); + } + + @Override + void handleStateChanges(ShooterIntakeSubsystem subsystem) { + switch (subsystem.targetState){ + case SHOOTING: + if (!SHOOTING.canStart(subsystem)) return; + if (!subsystem.io.atSpeed()) return; + this.transitionToState(subsystem, SHOOTING); + break; + default: + if (!subsystem.targetState.canStart(subsystem)) return; + + this.transitionToState(subsystem, subsystem.targetState); + } + } + + @Override + void periodic(ShooterIntakeSubsystem subsystem) { + super.periodic(subsystem); + subsystem.io.setTargetSpeed(subsystem.shootingTargetRPM); } }, SHOOTING { + + @Override + void periodic(ShooterIntakeSubsystem subsystem) { + super.periodic(subsystem); + subsystem.io.setTargetSpeed(subsystem.shootingTargetRPM); + } + @Override - void periodic() { - super.periodic(); - subsystem.io.setVoltage(1); + boolean canStart(ShooterIntakeSubsystem subsystem){ + return (subsystem.currentState == SHOOTING_PREP); } }; - static ShooterIntakeSubsystem subsystem; - - void periodic() { - this.changeStates(); + void periodic(ShooterIntakeSubsystem subsystem) { + this.handleStateChanges(subsystem); + Logger.processInputs("shooterintake", shooterIntakeIOInputs); } - void changeStates() { + void handleStateChanges(ShooterIntakeSubsystem subsystem) { if (this != subsystem.targetState) { - this.changeState(IDLE); + if (!IDLE.canStart(subsystem)) return; + this.transitionToState(subsystem, IDLE); } } - void onStart() {}; + void onStart(ShooterIntakeSubsystem subsystem) {} - void onEnd() {}; + void onEnd(ShooterIntakeSubsystem subsystem) {} + + boolean canStart(ShooterIntakeSubsystem subsystem) { + return true; + } - protected void changeState(State newState){ - this.onEnd(); + protected void transitionToState(ShooterIntakeSubsystem subsystem, State newState){ + this.onEnd(subsystem); subsystem.currentState = newState; - newState.onStart(); + newState.onStart(subsystem); } } // spotless:on From ace6c95ea93168e05fd3f38e80ddd0045fafcb3d Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Thu, 12 Sep 2024 09:36:21 -0400 Subject: [PATCH 6/8] Fixed naming and logging --- src/main/java/frc/robot/Robot.java | 5 - .../shooter_intake/ShooterIntakeIO.java | 1 + .../shooter_intake/ShooterIntakeIOSim.java | 4 +- .../ShooterIntakeSubsystem.java | 112 +++++++++--------- 4 files changed, 56 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e370be1..2781110 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,12 +4,8 @@ package frc.robot; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.subsystems.shooter_intake.ShooterIntakeSubsystem; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -76,7 +72,6 @@ public void robotPeriodic() { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java index bf74996..10e3879 100644 --- a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java @@ -8,6 +8,7 @@ public interface ShooterIntakeIO { public static class ShooterIntakeIOInputs { public double flywheelTargetSpeed = 0.0; public double flywheelSpeed = 0.0; + public double flywheelMotorVoltage = 0.0; public boolean flywheelPowered = false; } diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java index dd6b1cc..5bb13c5 100644 --- a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.FlywheelSim; -import org.littletonrobotics.junction.Logger; public class ShooterIntakeIOSim implements ShooterIntakeIO { @@ -23,8 +22,9 @@ public void periodic() { double calculatedVoltage = controller.calculate(getSpeed()); calculatedVoltage = MathUtil.clamp(calculatedVoltage, -12, 12); flywheelSim.setInputVoltage(calculatedVoltage); - Logger.recordOutput("Shooter.flyWheel.appliedVolts", calculatedVoltage); + shooterIntakeIOInputs.flywheelMotorVoltage = calculatedVoltage; shooterIntakeIOInputs.flywheelSpeed = flywheelSim.getAngularVelocityRPM(); + } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java index 14cf743..b0fe7c1 100644 --- a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java @@ -6,26 +6,26 @@ public class ShooterIntakeSubsystem extends SubsystemBase { // Move to constants later - double intakeTargetRPM = -100.0; - double shootingTargetRPM = 300.0; + private double intakeTargetRPM = -100.0; + private double shootingTargetRPM = 300.0; // protected State currentState = State.IDLE; protected State targetState = State.IDLE; - ShooterIntakeIO io; + protected ShooterIntakeIO shooterIntakeIO; static ShooterIntakeIOInputsAutoLogged shooterIntakeIOInputs = new ShooterIntakeIOInputsAutoLogged(); - public ShooterIntakeSubsystem(ShooterIntakeIO io) { - this.io = io; - io.setInputs(shooterIntakeIOInputs); + public ShooterIntakeSubsystem(ShooterIntakeIO shooterIntakeIO) { + this.shooterIntakeIO = shooterIntakeIO; + shooterIntakeIO.setInputs(shooterIntakeIOInputs); } public void periodic() { currentState.periodic(this); - io.periodic(); - Logger.recordOutput("Shooter.flyWheel.CurrentState", currentState); - Logger.recordOutput("Shooter.flyWheel.speed", io.getSpeed()); + shooterIntakeIO.periodic(); + Logger.recordOutput("ShooterIntake.CurrentState", currentState); + Logger.processInputs("shooterIntake", shooterIntakeIOInputs); } public void setTargetState(State target) { @@ -35,102 +35,85 @@ public void setTargetState(State target) { // spotless:off public enum State { IDLE { - @Override - void handleStateChanges(ShooterIntakeSubsystem subsystem) { - if (subsystem.targetState == this) return; - switch (subsystem.targetState){ - case SHOOTING: - if (!SHOOTING_PREP.canStart(subsystem)) return; - this.transitionToState(subsystem, SHOOTING_PREP); - break; - default: - if (!subsystem.targetState.canStart(subsystem)) return; - this.transitionToState(subsystem, subsystem.targetState); - } + protected void periodic(ShooterIntakeSubsystem subsystem) { + subsystem.shooterIntakeIO.setFlywheelPowered(false); } @Override - void onStart(ShooterIntakeSubsystem subsystem) { - subsystem.io.setFlywheelPowered(false); + protected void handleStateChanges(ShooterIntakeSubsystem subsystem) { + if (subsystem.targetState == this) return; + if (subsystem.targetState == SHOOTING){ + if (!SHOOTING_PREP.canStart(subsystem)) return; + this.transitionToState(subsystem, SHOOTING_PREP); + return; + } + if (!subsystem.targetState.canStart(subsystem)) return; + this.transitionToState(subsystem, subsystem.targetState); } }, INTAKING { @Override - void onStart(ShooterIntakeSubsystem subsystem) { - subsystem.io.setFlywheelPowered(true); + protected void onStart(ShooterIntakeSubsystem subsystem) { + subsystem.shooterIntakeIO.setFlywheelPowered(true); } @Override - void periodic(ShooterIntakeSubsystem subsystem) { + protected void periodic(ShooterIntakeSubsystem subsystem) { super.periodic(subsystem); - subsystem.io.setTargetSpeed(subsystem.intakeTargetRPM); + subsystem.shooterIntakeIO.setTargetSpeed(subsystem.intakeTargetRPM); } }, SHOOTING_PREP { @Override - void onStart(ShooterIntakeSubsystem subsystem) { - subsystem.io.setFlywheelPowered(true); - subsystem.io.setTargetSpeed(subsystem.shootingTargetRPM); + protected void onStart(ShooterIntakeSubsystem subsystem) { + subsystem.shooterIntakeIO.setFlywheelPowered(true); + subsystem.shooterIntakeIO.setTargetSpeed(subsystem.shootingTargetRPM); } @Override - void handleStateChanges(ShooterIntakeSubsystem subsystem) { + protected void handleStateChanges(ShooterIntakeSubsystem subsystem) { switch (subsystem.targetState){ case SHOOTING: if (!SHOOTING.canStart(subsystem)) return; - if (!subsystem.io.atSpeed()) return; + if (!subsystem.shooterIntakeIO.atSpeed()) return; this.transitionToState(subsystem, SHOOTING); break; default: - if (!subsystem.targetState.canStart(subsystem)) return; - - this.transitionToState(subsystem, subsystem.targetState); + if (!IDLE.canStart(subsystem)) return; + this.transitionToState(subsystem, IDLE); } } @Override - void periodic(ShooterIntakeSubsystem subsystem) { + protected void periodic(ShooterIntakeSubsystem subsystem) { super.periodic(subsystem); - subsystem.io.setTargetSpeed(subsystem.shootingTargetRPM); + subsystem.shooterIntakeIO.setTargetSpeed(subsystem.shootingTargetRPM); } }, SHOOTING { - @Override - void periodic(ShooterIntakeSubsystem subsystem) { - super.periodic(subsystem); - subsystem.io.setTargetSpeed(subsystem.shootingTargetRPM); - } - - @Override - boolean canStart(ShooterIntakeSubsystem subsystem){ + protected boolean canStart(ShooterIntakeSubsystem subsystem){ return (subsystem.currentState == SHOOTING_PREP); } - }; - void periodic(ShooterIntakeSubsystem subsystem) { - this.handleStateChanges(subsystem); - Logger.processInputs("shooterintake", shooterIntakeIOInputs); - } - - void handleStateChanges(ShooterIntakeSubsystem subsystem) { - if (this != subsystem.targetState) { - if (!IDLE.canStart(subsystem)) return; - this.transitionToState(subsystem, IDLE); + @Override + protected void periodic(ShooterIntakeSubsystem subsystem) { + super.periodic(subsystem); + subsystem.shooterIntakeIO.setTargetSpeed(subsystem.shootingTargetRPM); } - } + }; - void onStart(ShooterIntakeSubsystem subsystem) {} + protected void onStart(ShooterIntakeSubsystem subsystem) {} - void onEnd(ShooterIntakeSubsystem subsystem) {} + protected void onEnd(ShooterIntakeSubsystem subsystem) {} - boolean canStart(ShooterIntakeSubsystem subsystem) { + protected boolean canStart(ShooterIntakeSubsystem subsystem) { return true; } @@ -139,6 +122,17 @@ protected void transitionToState(ShooterIntakeSubsystem subsystem, State newStat subsystem.currentState = newState; newState.onStart(subsystem); } + + protected void handleStateChanges(ShooterIntakeSubsystem subsystem) { + if (this != subsystem.targetState) { + if (!IDLE.canStart(subsystem)) return; + this.transitionToState(subsystem, IDLE); + } + } + + protected void periodic(ShooterIntakeSubsystem subsystem) { + this.handleStateChanges(subsystem); + } } // spotless:on From bd5dbecd2a0034dfd9c43d386ebb72354946b62f Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Thu, 12 Sep 2024 09:43:52 -0400 Subject: [PATCH 7/8] add missing imports --- src/main/java/frc/robot/RobotContainer.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d5a590..801a6ef 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,9 @@ package frc.robot; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.OperatorConstants; From e00a3ea6c5871186f9b5e5d2225016e7392037ce Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Thu, 12 Sep 2024 09:56:49 -0400 Subject: [PATCH 8/8] Moved Constants (Does not build) --- src/main/java/frc/robot/Constants.java | 15 +++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 5 ++--- .../shooter_intake/ShooterIntakeIOSim.java | 16 ++++++++++++---- .../shooter_intake/ShooterIntakeSubsystem.java | 14 +++++--------- 4 files changed, 34 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0512ba0..55bb5ce 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import java.io.IOException; @@ -65,6 +66,20 @@ public static class BotConstants { iMode.Stick; // my puny brain can't think of how to set inputMode automatically } + public static class ShooterIntakeConstants { + public static final double intakeTargetRPM = -100.0; + public static final double shootingTargetRPM = 300.0; + + public static class ShooterIntakeSimConstants { + public static final DCMotor FLYWHEEL_DC_MOTOR = DCMotor.getCIM(2); + public static final double FLYWHEEL_GEARING = 1.0; + public static final double FLYWHEEL_jKg_METERS_SQUARED = 1.0; + public static final double FLYWHEEL_KP = 0.75; + public static final double FLYWHEEL_KI = 0.01; + public static final double FLYWHEEL_KD = 0.0; + } + } + public static class VisionConstants { // ALL PLACEHOLDERS public static final String CAMERA_NAME = ""; public static final double CAMERA_HEIGHT_METERS = 0.7; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 801a6ef..0f2b5ae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,16 +6,15 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.DriveWithGamepad; +import frc.robot.subsystems.shooter_intake.ShooterIntakeIOSim; +import frc.robot.subsystems.shooter_intake.ShooterIntakeSubsystem; import frc.robot.subsystems.swerve.SwerveDriveSubsystem; import frc.robot.subsystems.swerve.SwerveHardwareIO; import frc.robot.subsystems.swerve.SwerveSimIO; -import frc.robot.subsystems.shooter_intake.ShooterIntakeIOSim; -import frc.robot.subsystems.shooter_intake.ShooterIntakeSubsystem; import java.util.function.DoubleSupplier; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java index 5bb13c5..9861273 100644 --- a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java @@ -1,14 +1,23 @@ package frc.robot.subsystems.shooter_intake; +import static frc.robot.Constants.ShooterIntakeConstants.ShooterIntakeSimConstants; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.FlywheelSim; public class ShooterIntakeIOSim implements ShooterIntakeIO { - FlywheelSim flywheelSim = new FlywheelSim(DCMotor.getCIM(2), 1.0, 1.0); - PIDController controller = new PIDController(0.75, 0.1, 0.0); + FlywheelSim flywheelSim = + new FlywheelSim( + ShooterIntakeSimConstants.FLYWHEEL_DC_MOTOR, + ShooterIntakeSimConstants.FLYWHEEL_GEARING, + ShooterIntakeSimConstants.FLYWHEEL_jKg_METERS_SQUARED); + PIDController controller = + new PIDController( + ShooterIntakeSimConstants.FLYWHEEL_KP, + ShooterIntakeSimConstants.FLYWHEEL_KI, + ShooterIntakeSimConstants.FLYWHEEL_KD); ShooterIntakeIOInputsAutoLogged shooterIntakeIOInputs; @Override @@ -24,7 +33,6 @@ public void periodic() { flywheelSim.setInputVoltage(calculatedVoltage); shooterIntakeIOInputs.flywheelMotorVoltage = calculatedVoltage; shooterIntakeIOInputs.flywheelSpeed = flywheelSim.getAngularVelocityRPM(); - } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java index b0fe7c1..67836a9 100644 --- a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java @@ -1,15 +1,11 @@ package frc.robot.subsystems.shooter_intake; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import org.littletonrobotics.junction.Logger; public class ShooterIntakeSubsystem extends SubsystemBase { - // Move to constants later - private double intakeTargetRPM = -100.0; - private double shootingTargetRPM = 300.0; - // - protected State currentState = State.IDLE; protected State targetState = State.IDLE; protected ShooterIntakeIO shooterIntakeIO; @@ -63,7 +59,7 @@ protected void onStart(ShooterIntakeSubsystem subsystem) { @Override protected void periodic(ShooterIntakeSubsystem subsystem) { super.periodic(subsystem); - subsystem.shooterIntakeIO.setTargetSpeed(subsystem.intakeTargetRPM); + subsystem.shooterIntakeIO.setTargetSpeed(Constants.ShooterIntakeConstants.intakeTargetRPM); } }, @@ -72,7 +68,7 @@ protected void periodic(ShooterIntakeSubsystem subsystem) { @Override protected void onStart(ShooterIntakeSubsystem subsystem) { subsystem.shooterIntakeIO.setFlywheelPowered(true); - subsystem.shooterIntakeIO.setTargetSpeed(subsystem.shootingTargetRPM); + subsystem.shooterIntakeIO.setTargetSpeed(Constants.ShooterIntakeConstants.shootingTargetRPM); } @Override @@ -92,7 +88,7 @@ protected void handleStateChanges(ShooterIntakeSubsystem subsystem) { @Override protected void periodic(ShooterIntakeSubsystem subsystem) { super.periodic(subsystem); - subsystem.shooterIntakeIO.setTargetSpeed(subsystem.shootingTargetRPM); + subsystem.shooterIntakeIO.setTargetSpeed(Constants.ShooterIntakeConstants.shootingTargetRPM); } }, @@ -105,7 +101,7 @@ protected boolean canStart(ShooterIntakeSubsystem subsystem){ @Override protected void periodic(ShooterIntakeSubsystem subsystem) { super.periodic(subsystem); - subsystem.shooterIntakeIO.setTargetSpeed(subsystem.shootingTargetRPM); + subsystem.shooterIntakeIO.setTargetSpeed(Constants.ShooterIntakeConstants.shootingTargetRPM); } };