diff --git a/.gitignore b/.gitignore index d1e3427..37c6ba8 100644 --- a/.gitignore +++ b/.gitignore @@ -41,6 +41,9 @@ # Log file *.log +*.wpilog +advantagekit/logs +logs # BlueJ files *.ctxt diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 0d4a452..33103bc 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2024", - "teamNumber": 6328 + "teamNumber": 422 } diff --git a/build.gradle b/build.gradle index 51a55b2..cf4fb97 100644 --- a/build.gradle +++ b/build.gradle @@ -1,8 +1,11 @@ +import java.text.SimpleDateFormat + plugins { id "java" id "edu.wpi.first.GradleRIO" version "2024.3.2" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.12.0" + id "org.ajoberstar.grgit" version "5.2.1" } java { @@ -184,3 +187,22 @@ spotless { endWithNewline() } } + +task (commitOnDeploy, dependsOn: "spotlessApply") { + description = "Commits changes when the robot code is deployed" + doLast { + def date = new Date() + def sdf = new SimpleDateFormat("MM/dd/yyyy HH:mm:ss") + grgit.add(patterns: [ + 'src/', + 'vendordeps/', + 'build.gradle', + 'logs', + 'gradle', + '.wpilib' + ]) + grgit.commit(message: "Automated commit on ${sdf.format(date)}") + } +} + +deployArtifact.dependsOn commitOnDeploy diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat new file mode 100644 index 0000000..ca8c308 Binary files /dev/null and b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat differ diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/src/main/java/frc/lib/advantagekit/LoggedIO.java b/src/main/java/frc/lib/advantagekit/LoggedIO.java new file mode 100644 index 0000000..a58057a --- /dev/null +++ b/src/main/java/frc/lib/advantagekit/LoggedIO.java @@ -0,0 +1,5 @@ +package frc.lib.advantagekit; + +public interface LoggedIO { + public void updateInputs(T inputs); +} diff --git a/src/main/java/frc/lib/advantagekit/LoggerUtil.java b/src/main/java/frc/lib/advantagekit/LoggerUtil.java new file mode 100644 index 0000000..5aab096 --- /dev/null +++ b/src/main/java/frc/lib/advantagekit/LoggerUtil.java @@ -0,0 +1,58 @@ +package frc.lib.advantagekit; + +import frc.robot.BuildConstants; +// import frc.robot.BuildConstants; +import frc.robot.Robot; +import java.io.File; +import java.nio.file.Path; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + +public class LoggerUtil { + public static final String kLogDirectory = "advantagekit/logs"; + + public static void initializeLogger() { + + // Record metadata + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + Logger.recordMetadata("GitDirty", getGitDirtyString()); + + // Set up data receivers & replay source + if (Robot.isSimulation()) { + Logger.addDataReceiver(new WPILOGWriter(getLogPath())); + Logger.addDataReceiver(new NT4Publisher()); + } else { + Logger.addDataReceiver(new WPILOGWriter("/media/sda1/")); + Logger.addDataReceiver(new NT4Publisher()); + } + + // Start AdvantageKit logger + Logger.start(); + } + + private static String getGitDirtyString() { + switch (BuildConstants.DIRTY) { + case 0: + return "All changes committed"; + case 1: + return "Uncomitted changes"; + default: + return "Unknown"; + } + } + + private static String getLogPath() { + File file = Path.of(kLogDirectory).toFile(); + + if (!file.exists()) { + file.mkdirs(); + } + + return file.getAbsolutePath(); + } +} diff --git a/src/main/java/frc/lib/utils/LoggedTunableNumber.java b/src/main/java/frc/lib/utils/LoggedTunableNumber.java new file mode 100644 index 0000000..9dfea6e --- /dev/null +++ b/src/main/java/frc/lib/utils/LoggedTunableNumber.java @@ -0,0 +1,122 @@ +package frc.lib.utils; + +import frc.robot.Constants; +// FROM 6328 Mechanical Advantage +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Consumer; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class LoggedTunableNumber { + private static final String tableKey = "TunableNumbers"; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedDashboardNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey, String tableKey) { + this.key = tableKey + "/" + dashboardKey; + // this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey, tableKey); + initDefault(defaultValue); + } + + public LoggedTunableNumber(String dashboardKey, double defaultValue, String tableKey) { + this(dashboardKey, tableKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (Constants.tuningMode) { + dashboardNumber = new LoggedDashboardNumber(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return Constants.tuningMode ? dashboardNumber.get() : defaultValue; + } + } + + public void set(double val) { + dashboardNumber.set(val); + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + if (!Constants.tuningMode) return false; + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } + + /** + * Runs action if any of the tunableNumbers have changed + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * + * objects. Recommended approach is to pass the result of "hashCode()" + * @param action Callback to run when any of the tunable numbers have changed. Access tunable + * numbers in order inputted in method + * @param tunableNumbers All tunable numbers to check + */ + public static void ifChanged( + int id, Consumer action, LoggedTunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { + action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); + } + } + + /** Runs action if any of the tunableNumbers have changed */ + public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { + ifChanged(id, values -> action.run(), tunableNumbers); + } +} diff --git a/src/main/java/frc/lib/utils/TunableNumber.java b/src/main/java/frc/lib/utils/TunableNumber.java new file mode 100644 index 0000000..9b7431f --- /dev/null +++ b/src/main/java/frc/lib/utils/TunableNumber.java @@ -0,0 +1,90 @@ +package frc.lib.utils; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class TunableNumber { + private static final String tableKey = "TunableNumbers"; + + private String key; + private double defaultValue; + private double lastHasChangedValue = defaultValue; + + /** + * Create a new TunableNumber + * + * @param dashboardKey Key on dashboard + */ + public TunableNumber(String tableString, String dashboardKey) { + this.key = tableString + "/" + dashboardKey; + } + + /** + * Create a new TunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public TunableNumber(String dashboardKey, double defaultValue) { + this(tableKey, dashboardKey); + setDefault(defaultValue); + } + + public TunableNumber(String dashboardKey, double defaultValue, String tableKey) { + this(tableKey, dashboardKey); + setDefault(defaultValue); + } + + /** + * Get the default value for the number that has been set + * + * @return The default value + */ + public double getDefault() { + return defaultValue; + } + + /** + * Set the default value of the number + * + * @param defaultValue The default value + */ + public void setDefault(double defaultValue) { + this.defaultValue = defaultValue; + if (Constants.tuningMode) { + // This makes sure the data is on NetworkTables but will not change it + SmartDashboard.putNumber(key, SmartDashboard.getNumber(key, defaultValue)); + } else { + SmartDashboard.clearPersistent(key); + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode + * + * @return The current value + */ + public double get() { + return Constants.tuningMode ? SmartDashboard.getNumber(key, defaultValue) : defaultValue; + } + + /** + * Checks whether the number has changed since our last check + * + * @return True if the number has changed since the last time this method was called, false + * otherwise + */ + public boolean hasChanged() { + double currentValue = get(); + if (currentValue != lastHasChangedValue) { + lastHasChangedValue = currentValue; + return true; + } + + return false; + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a82210e..650ff96 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,6 +13,13 @@ package frc.robot; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.math.util.Units; +import frc.lib.utils.LoggedTunableNumber; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -22,16 +29,106 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final Mode currentMode = Mode.REAL; + public static final boolean tuningMode = true; + + public static final class ShooterConstants { + + // Shooter pivot + public static final double kPivotGearRatio = 46.722; + + public static final Rotation2d kMaxAngle = Rotation2d.fromDegrees(55); + public static final Rotation2d kMinAngle = Rotation2d.fromDegrees(15); + public static final Rotation2d kHomeAngle = Rotation2d.fromDegrees(33); + + public static final Rotation2d kPivotOffset = Rotation2d.fromDegrees(170); + + public static final LoggedTunableNumber kPivotP = + new LoggedTunableNumber("Shooter Pivot P", 4.0); + public static final LoggedTunableNumber kPivotI = + new LoggedTunableNumber("Shooter Pivot I", 0.0); + public static final LoggedTunableNumber kPivotD = + new LoggedTunableNumber("Shooter Pivot D", 0.0); + public static final double kPivotVelocity = 5.0; + public static final double kPivotAcceleration = 10.0; + public static final ProfiledPIDController kPivotController = + new ProfiledPIDController( + kPivotP.get(), + kPivotI.get(), + kPivotD.get(), + new Constraints(kPivotVelocity, kPivotAcceleration)); + + // Shooter flywheel + public static final double kFlywheelDiameter = Units.inchesToMeters(4.0); + + public static final LoggedTunableNumber kLeftFlywheelP = + new LoggedTunableNumber("Shooter Left Flywheel P", 1.0); + public static final LoggedTunableNumber kLeftFlywheelI = + new LoggedTunableNumber("Shooter Left Flywheel I", 0.0); + public static final LoggedTunableNumber kLeftFlywheelD = + new LoggedTunableNumber("Shooter Left Flywheel D", 0.0); + public static final double kLeftFlywheelVelocity = 10.0; + public static final double kLeftFlywheelAcceleration = 15.0; + public static final ProfiledPIDController kLeftFlywheelController = + new ProfiledPIDController( + kLeftFlywheelP.get(), + kLeftFlywheelI.get(), + kLeftFlywheelD.get(), + new Constraints(kLeftFlywheelVelocity, kLeftFlywheelAcceleration)); + + public static final LoggedTunableNumber kRightFlywheelP = + new LoggedTunableNumber("Shooter Right Flywheel P", 1.0); + public static final LoggedTunableNumber kRightFlywheelI = + new LoggedTunableNumber("Shooter Right Flywheel I", 0.0); + public static final LoggedTunableNumber kRightFlywheelD = + new LoggedTunableNumber("Shooter Right Flywheel D", 0.0); + public static final double kRightFlywheelVelocity = 10.0; + public static final double kRightFlywheelAcceleration = 15.0; + public static final ProfiledPIDController kRightFlywheelController = + new ProfiledPIDController( + kRightFlywheelP.get(), + kRightFlywheelI.get(), + kRightFlywheelD.get(), + new Constraints(kRightFlywheelVelocity, kRightFlywheelAcceleration)); + + public static final LoggedTunableNumber kRightFlywheelKs = + new LoggedTunableNumber("Shooter Right Flywheel kS", 0.38); + public static final LoggedTunableNumber kRightFlywheelKv = + new LoggedTunableNumber("Shooter Right Flywheel kV", 0.27); + public static final SimpleMotorFeedforward kRightFlywheelFeedforward = + new SimpleMotorFeedforward(kRightFlywheelKs.get(), kRightFlywheelKv.get()); + + public static final LoggedTunableNumber kLeftFlywheelKs = + new LoggedTunableNumber("Shooter Left Flywheel kS", 0.32); + public static final LoggedTunableNumber kLeftFlywheelKv = + new LoggedTunableNumber("Shooter Left Flywheel kV", 0.27); + public static final SimpleMotorFeedforward kLeftFlywheelFeedforward = + new SimpleMotorFeedforward(kLeftFlywheelKs.get(), kLeftFlywheelKv.get()); + + public static final LoggedTunableNumber kTestLeftFlywheelSpeed = + new LoggedTunableNumber("Left test flywheel speed", 0.0); + public static final LoggedTunableNumber kTestRightFlywheelSpeed = + new LoggedTunableNumber("Right test flywheel speed", 20.0); + } + + public static final class Ports { + public static final int kShooterPivotLeader = 39; + public static final int kShooterPivotFollower = 40; + public static final int kShooterPivotEncoder = 9; + + public static final int kFlywheelLeft = 35; + public static final int kFlywheelRight = 36; + + public static final int kKicker = 31; + public static final int kFeeder = 30; + public static final int kIndexerBeamBreakOne = 2; + public static final int kIndexerBeamBreakTwo = 3; - public static enum Mode { - /** Running on a real robot. */ - REAL, + public static final int kIntakeRoller = 43; + public static final int kIntakePivot = 33; - /** Running a physics simulator. */ - SIM, + public static final int kClimbUp = 26; + public static final int kClimbDown = 25; - /** Replaying from a log file. */ - REPLAY + public static final int kAmp = 54; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ea47d35..7a97f53 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,14 +13,11 @@ package frc.robot; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import org.littletonrobotics.junction.LogFileUtil; +import frc.lib.advantagekit.LoggerUtil; import org.littletonrobotics.junction.LoggedRobot; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.NT4Publisher; -import org.littletonrobotics.junction.wpilog.WPILOGReader; -import org.littletonrobotics.junction.wpilog.WPILOGWriter; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -38,52 +35,14 @@ public class Robot extends LoggedRobot { */ @Override public void robotInit() { - // Record metadata - Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); - Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); - Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); - Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); - switch (BuildConstants.DIRTY) { - case 0: - Logger.recordMetadata("GitDirty", "All changes committed"); - break; - case 1: - Logger.recordMetadata("GitDirty", "Uncomitted changes"); - break; - default: - Logger.recordMetadata("GitDirty", "Unknown"); - break; - } - // Set up data receivers & replay source - switch (Constants.currentMode) { - case REAL: - // Running on a real robot, log to a USB stick ("/U/logs") - Logger.addDataReceiver(new WPILOGWriter()); - Logger.addDataReceiver(new NT4Publisher()); - break; - - case SIM: - // Running a physics simulator, log to NT - Logger.addDataReceiver(new NT4Publisher()); - break; - - case REPLAY: - // Replaying a log, set up replay source - setUseTiming(false); // Run as fast as possible - String logPath = LogFileUtil.findReplayLog(); - Logger.setReplaySource(new WPILOGReader(logPath)); - Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); - break; - } + LoggerUtil.initializeLogger(); + + DataLogManager.logNetworkTables(true); // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. // Logger.disableDeterministicTimestamps() - // Start AdvantageKit logger - Logger.start(); - // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. robotContainer = new RobotContainer(); @@ -98,6 +57,8 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); + + robotContainer.updateRobotState(); } /** This function is called once when the robot is disabled. */ @@ -111,7 +72,7 @@ public void disabledPeriodic() {} /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - autonomousCommand = robotContainer.getAutonomousCommand(); + // autonomousCommand = robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) if (autonomousCommand != null) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9f92af4..af578b0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,29 +13,27 @@ package frc.robot; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants.Ports; +import frc.robot.Constants.ShooterConstants; import frc.robot.commands.DriveCommands; +import frc.robot.oi.DriverControls; +import frc.robot.oi.DriverControlsPS5; +import frc.robot.oi.DriverControlsXbox; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; -import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; -import frc.robot.subsystems.drive.ModuleIOSparkMax; -import frc.robot.subsystems.flywheel.Flywheel; -import frc.robot.subsystems.flywheel.FlywheelIO; -import frc.robot.subsystems.flywheel.FlywheelIOSim; -import frc.robot.subsystems.flywheel.FlywheelIOSparkMax; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; +import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.indexer.IndexerIOFalcon; +import frc.robot.subsystems.indexer.IndexerIOSim; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.flywheel.FlywheelIOKraken; +import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim; +import frc.robot.subsystems.shooter.pivot.ShooterPivotIOFalcon; +import frc.robot.subsystems.shooter.pivot.ShooterPivotIOSim; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -44,135 +42,122 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // Subsystems - private final Drive drive; - private final Flywheel flywheel; + private RobotState m_robotState; - // Controller - private final CommandXboxController controller = new CommandXboxController(0); + // Subsystems + private Drive m_drive; + private Shooter m_shooter; + private Indexer m_indexer; - // Dashboard inputs - private final LoggedDashboardChooser autoChooser; - private final LoggedDashboardNumber flywheelSpeedInput = - new LoggedDashboardNumber("Flywheel Speed", 1500.0); + // Controllers + private DriverControls m_driverControls; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - switch (Constants.currentMode) { - case REAL: - // Real robot, instantiate hardware IO implementations - drive = - new Drive( - new GyroIOPigeon2(false), - new ModuleIOSparkMax(0), - new ModuleIOSparkMax(1), - new ModuleIOSparkMax(2), - new ModuleIOSparkMax(3)); - flywheel = new Flywheel(new FlywheelIOSparkMax()); - // drive = new Drive( - // new GyroIOPigeon2(true), - // new ModuleIOTalonFX(0), - // new ModuleIOTalonFX(1), - // new ModuleIOTalonFX(2), - // new ModuleIOTalonFX(3)); - // flywheel = new Flywheel(new FlywheelIOTalonFX()); - break; - - case SIM: - // Sim robot, instantiate physics sim IO implementations - drive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); - flywheel = new Flywheel(new FlywheelIOSim()); - break; + configureSubsystems(); + configureControllers(); + configureButtonBindings(); + } - default: - // Replayed robot, disable IO implementations - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); - flywheel = new Flywheel(new FlywheelIO() {}); - break; + private void configureSubsystems() { + if (RobotBase.isReal()) { + m_drive = + new Drive( + new GyroIOPigeon2(true), + new ModuleIOTalonFX(0), + new ModuleIOTalonFX(1), + new ModuleIOTalonFX(2), + new ModuleIOTalonFX(3)); + m_shooter = + new Shooter( + new ShooterPivotIOFalcon( + Ports.kShooterPivotLeader, + Ports.kShooterPivotFollower, + Ports.kShooterPivotEncoder), + new FlywheelIOKraken(Ports.kFlywheelLeft, Ports.kFlywheelRight), + ShooterConstants.kPivotController, + ShooterConstants.kLeftFlywheelController, + ShooterConstants.kRightFlywheelController, + ShooterConstants.kLeftFlywheelFeedforward, + ShooterConstants.kRightFlywheelFeedforward); + m_indexer = + new Indexer( + new IndexerIOFalcon( + Ports.kKicker, + Ports.kFeeder, + Ports.kIndexerBeamBreakOne, + Ports.kIndexerBeamBreakTwo)); + } else { + m_drive = + new Drive( + new GyroIOPigeon2(true), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + m_shooter = + new Shooter( + new ShooterPivotIOSim(), + new FlywheelIOSim(), + ShooterConstants.kPivotController, + ShooterConstants.kLeftFlywheelController, + ShooterConstants.kRightFlywheelController, + ShooterConstants.kLeftFlywheelFeedforward, + ShooterConstants.kRightFlywheelFeedforward); + m_indexer = new Indexer(new IndexerIOSim()); } - // Set up auto routines - NamedCommands.registerCommand( - "Run Flywheel", - Commands.startEnd( - () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel) - .withTimeout(5.0)); - autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + m_shooter.setPivotAngle(ShooterConstants.kHomeAngle); - // Set up SysId routines - autoChooser.addOption( - "Drive SysId (Quasistatic Forward)", - drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooser.addOption( - "Drive SysId (Quasistatic Reverse)", - drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooser.addOption( - "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooser.addOption( - "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - autoChooser.addOption( - "Flywheel SysId (Quasistatic Forward)", - flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooser.addOption( - "Flywheel SysId (Quasistatic Reverse)", - flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooser.addOption( - "Flywheel SysId (Dynamic Forward)", flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooser.addOption( - "Flywheel SysId (Dynamic Reverse)", flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + m_robotState = RobotState.startInstance(m_drive, m_shooter, m_indexer); + } - // Configure the button bindings - configureButtonBindings(); + private void configureControllers() { + // usually use ps5 on actual robot but xbox is better for keyboard testing + if (RobotBase.isReal()) { + m_driverControls = new DriverControlsPS5(1); + } else { + m_driverControls = new DriverControlsXbox(1); + } } - /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ private void configureButtonBindings() { - drive.setDefaultCommand( + m_drive.setDefaultCommand( DriveCommands.joystickDrive( - drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - controller - .b() + m_drive, + m_driverControls::getDriveForward, + m_driverControls::getDriveLeft, + m_driverControls::getDriveRotation)); + + m_driverControls + .testShooter() .onTrue( Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), - drive) - .ignoringDisable(true)); - controller - .a() - .whileTrue( - Commands.startEnd( - () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)); + () -> { + m_shooter.setFlywheelVelocity( + ShooterConstants.kTestLeftFlywheelSpeed.get(), + ShooterConstants.kTestRightFlywheelSpeed.get()); + m_shooter.setPivotAngle(Rotation2d.fromDegrees(50)); + })) + .onFalse( + Commands.runOnce( + () -> { + m_shooter.setFlywheelVelocity(0.0, 0.0); + m_shooter.setPivotAngle(ShooterConstants.kHomeAngle); + })); + + m_driverControls + .testKicker() + .onTrue(m_indexer.runKicker(6.0)) + .onFalse(m_indexer.runKicker(0.0)); + + m_driverControls + .testFeeder() + .onTrue(m_indexer.runFeeder(6.0)) + .onFalse(m_indexer.runFeeder(0.0)); } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return autoChooser.get(); + public void updateRobotState() { + m_robotState.updateRobotState(); } } diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java new file mode 100644 index 0000000..48e5261 --- /dev/null +++ b/src/main/java/frc/robot/RobotState.java @@ -0,0 +1,110 @@ +package frc.robot; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.shooter.Shooter; +import org.littletonrobotics.junction.Logger; + +public class RobotState { + + private static RobotState instance = null; + + // subsystems + private Drive m_drive; + private Shooter m_shooter; + private Indexer m_indexer; + + // mechanism + private final boolean kMechanismEnabled = true; + Mechanism2d m_mechanism; + + MechanismLigament2d m_shooterPivotMechLower; + MechanismLigament2d m_shooterPivotMechUpper; + + // advantagescope components + private final boolean kComponentsEnabled = true; + private final Translation3d kShooterZeroTranslation = new Translation3d(0.017, 0.0, 0.415); + + private RobotState(Drive drive, Shooter shooter, Indexer indexer) { + // subsystems + m_drive = drive; + m_shooter = shooter; + + // mechanism set up + m_mechanism = new Mechanism2d(1.5, 1); + MechanismRoot2d m_root = m_mechanism.getRoot("shooter", 1.0, 0.5); + + // have as two ligaments so it can rotate around center + m_shooterPivotMechLower = + m_root.append( + new MechanismLigament2d( + "pivotLower", + 0.5, + m_shooter.getPivotAngle().getDegrees() + 180, + 6, + new Color8Bit(Color.kPurple))); + + m_shooterPivotMechUpper = + m_root.append( + new MechanismLigament2d( + "pivotUpper", + 0.5, + m_shooter.getPivotAngle().getDegrees(), + 6, + new Color8Bit(Color.kPurple))); + + // post to dashboard + SmartDashboard.putData("Mech2d", m_mechanism); + } + + public static RobotState startInstance(Drive drive, Shooter shooter, Indexer indexer) { + if (instance == null) { + instance = new RobotState(drive, shooter, indexer); + } else { + throw new IllegalStateException("RobotState instance already started"); + } + return instance; + } + + public static RobotState getInstance() { + if (instance == null) { + throw new IllegalStateException("RobotState instance not started"); + } + return instance; + } + + public void updateMechanism() { + m_shooterPivotMechLower.setAngle(m_shooter.getPivotAngle().getDegrees() + 180); + m_shooterPivotMechUpper.setAngle(m_shooter.getPivotAngle().getDegrees()); + + Logger.recordOutput("Mechanism", m_mechanism); + } + + public void updateComponents() { + Pose3d shooterPose = + new Pose3d( + kShooterZeroTranslation, new Rotation3d(0, -m_shooter.getPivotAngle().getRadians(), 0)); + + Logger.recordOutput("Components/ShooterPose", shooterPose); + Logger.recordOutput("Components/ZeroPose", new Pose3d()); // for tuning config + } + + public void updateRobotState() { + if (kMechanismEnabled) { + updateMechanism(); + } + + if (kComponentsEnabled) { + updateComponents(); + } + } +} diff --git a/src/main/java/frc/robot/oi/DriverControls.java b/src/main/java/frc/robot/oi/DriverControls.java new file mode 100644 index 0000000..d9dbaee --- /dev/null +++ b/src/main/java/frc/robot/oi/DriverControls.java @@ -0,0 +1,17 @@ +package frc.robot.oi; + +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public interface DriverControls { + public double getDriveForward(); + + public double getDriveLeft(); + + public double getDriveRotation(); + + public Trigger testShooter(); + + public Trigger testKicker(); + + public Trigger testFeeder(); +} diff --git a/src/main/java/frc/robot/oi/DriverControlsPS5.java b/src/main/java/frc/robot/oi/DriverControlsPS5.java new file mode 100644 index 0000000..5ad1f1a --- /dev/null +++ b/src/main/java/frc/robot/oi/DriverControlsPS5.java @@ -0,0 +1,42 @@ +package frc.robot.oi; + +import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class DriverControlsPS5 implements DriverControls { + private CommandPS5Controller m_controller; + + public DriverControlsPS5(int port) { + m_controller = new CommandPS5Controller(port); + } + + @Override + public double getDriveForward() { + return m_controller.getLeftY(); + } + + @Override + public double getDriveLeft() { + return m_controller.getLeftX(); + } + + @Override + public double getDriveRotation() { + return m_controller.getRightX(); + } + + @Override + public Trigger testShooter() { + return m_controller.L2(); + } + + @Override + public Trigger testKicker() { + return m_controller.R1(); + } + + @Override + public Trigger testFeeder() { + return m_controller.L1(); + } +} diff --git a/src/main/java/frc/robot/oi/DriverControlsXbox.java b/src/main/java/frc/robot/oi/DriverControlsXbox.java new file mode 100644 index 0000000..f11189a --- /dev/null +++ b/src/main/java/frc/robot/oi/DriverControlsXbox.java @@ -0,0 +1,42 @@ +package frc.robot.oi; + +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class DriverControlsXbox implements DriverControls { + private CommandXboxController m_controller; + + public DriverControlsXbox(int port) { + m_controller = new CommandXboxController(port); + } + + @Override + public double getDriveForward() { + return m_controller.getLeftY(); + } + + @Override + public double getDriveLeft() { + return m_controller.getLeftX(); + } + + @Override + public double getDriveRotation() { + return m_controller.getRightX(); + } + + @Override + public Trigger testShooter() { + return m_controller.button(1); + } + + @Override + public Trigger testKicker() { + return m_controller.button(2); + } + + @Override + public Trigger testFeeder() { + return m_controller.button(3); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 51d02c0..7ac4936 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -32,6 +32,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -167,7 +168,7 @@ public void periodic() { } // Update gyro angle - if (gyroInputs.connected) { + if (gyroInputs.connected && RobotBase.isReal()) { // Use the real gyro angle rawGyroRotation = gyroInputs.odometryYawPositions[i]; } else { diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 65016c4..7488261 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -19,7 +19,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import frc.robot.Constants; +import edu.wpi.first.wpilibj.RobotBase; import org.littletonrobotics.junction.Logger; public class Module { @@ -44,23 +44,14 @@ public Module(ModuleIO io, int index) { // Switch constants based on mode (the physics simulator is treated as a // separate robot with different tuning) - switch (Constants.currentMode) { - case REAL: - case REPLAY: - driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - driveFeedback = new PIDController(0.05, 0.0, 0.0); - turnFeedback = new PIDController(7.0, 0.0, 0.0); - break; - case SIM: - driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); - driveFeedback = new PIDController(0.1, 0.0, 0.0); - turnFeedback = new PIDController(10.0, 0.0, 0.0); - break; - default: - driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - driveFeedback = new PIDController(0.0, 0.0, 0.0); - turnFeedback = new PIDController(0.0, 0.0, 0.0); - break; + if (RobotBase.isReal()) { + driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + driveFeedback = new PIDController(0.05, 0.0, 0.0); + turnFeedback = new PIDController(7.0, 0.0, 0.0); + } else { + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); + driveFeedback = new PIDController(0.1, 0.0, 0.0); + turnFeedback = new PIDController(10.0, 0.0, 0.0); } turnFeedback.enableContinuousInput(-Math.PI, Math.PI); @@ -94,7 +85,7 @@ public void periodic() { if (speedSetpoint != null) { // Scale velocity based on turn error // - // When the error is 90°, the velocity setpoint should be 0. As the wheel turns + // When the error is 90 degrees, the velocity setpoint should be 0. As the wheel turns // towards the setpoint, its velocity should increase. This is achieved by // taking the component of the velocity in the direction of the setpoint. double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index b41202e..132e9f2 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -69,28 +69,28 @@ public class ModuleIOTalonFX implements ModuleIO { public ModuleIOTalonFX(int index) { switch (index) { case 0: - driveTalon = new TalonFX(0); - turnTalon = new TalonFX(1); - cancoder = new CANcoder(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + driveTalon = new TalonFX(1, "Drivetrain"); + turnTalon = new TalonFX(2, "Drivetrain"); + cancoder = new CANcoder(3, "Drivetrain"); + absoluteEncoderOffset = new Rotation2d(1.226); // MUST BE CALIBRATED break; case 1: - driveTalon = new TalonFX(3); - turnTalon = new TalonFX(4); - cancoder = new CANcoder(5); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + driveTalon = new TalonFX(4, "Drivetrain"); + turnTalon = new TalonFX(5, "Drivetrain"); + cancoder = new CANcoder(6, "Drivetrain"); + absoluteEncoderOffset = new Rotation2d(0.175); // MUST BE CALIBRATED break; case 2: - driveTalon = new TalonFX(6); - turnTalon = new TalonFX(7); - cancoder = new CANcoder(8); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + driveTalon = new TalonFX(7, "Drivetrain"); + turnTalon = new TalonFX(8, "Drivetrain"); + cancoder = new CANcoder(9, "Drivetrain"); + absoluteEncoderOffset = new Rotation2d(-2.945); // MUST BE CALIBRATED break; case 3: - driveTalon = new TalonFX(9); - turnTalon = new TalonFX(10); - cancoder = new CANcoder(11); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + driveTalon = new TalonFX(10, "Drivetrain"); + turnTalon = new TalonFX(11, "Drivetrain"); + cancoder = new CANcoder(12, "Drivetrain"); + absoluteEncoderOffset = new Rotation2d(2.827); // MUST BE CALIBRATED break; default: throw new RuntimeException("Invalid module index"); diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java deleted file mode 100644 index 6056b24..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.flywheel; - -import static edu.wpi.first.units.Units.*; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -public class Flywheel extends SubsystemBase { - private final FlywheelIO io; - private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); - private final SimpleMotorFeedforward ffModel; - private final SysIdRoutine sysId; - - /** Creates a new Flywheel. */ - public Flywheel(FlywheelIO io) { - this.io = io; - - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.currentMode) { - case REAL: - case REPLAY: - ffModel = new SimpleMotorFeedforward(0.1, 0.05); - io.configurePID(1.0, 0.0, 0.0); - break; - case SIM: - ffModel = new SimpleMotorFeedforward(0.0, 0.03); - io.configurePID(0.5, 0.0, 0.0); - break; - default: - ffModel = new SimpleMotorFeedforward(0.0, 0.0); - break; - } - - // Configure SysId - sysId = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - null, - null, - (state) -> Logger.recordOutput("Flywheel/SysIdState", state.toString())), - new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this)); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Flywheel", inputs); - } - - /** Run open loop at the specified voltage. */ - public void runVolts(double volts) { - io.setVoltage(volts); - } - - /** Run closed loop at the specified velocity. */ - public void runVelocity(double velocityRPM) { - var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); - - // Log flywheel setpoint - Logger.recordOutput("Flywheel/SetpointRPM", velocityRPM); - } - - /** Stops the flywheel. */ - public void stop() { - io.stop(); - } - - /** Returns a command to run a quasistatic test in the specified direction. */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysId.quasistatic(direction); - } - - /** Returns a command to run a dynamic test in the specified direction. */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysId.dynamic(direction); - } - - /** Returns the current velocity in RPM. */ - @AutoLogOutput - public double getVelocityRPM() { - return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); - } - - /** Returns the current velocity in radians per second. */ - public double getCharacterizationVelocity() { - return inputs.velocityRadPerSec; - } -} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java deleted file mode 100644 index 98f7624..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.flywheel; - -import org.littletonrobotics.junction.AutoLog; - -public interface FlywheelIO { - @AutoLog - public static class FlywheelIOInputs { - public double positionRad = 0.0; - public double velocityRadPerSec = 0.0; - public double appliedVolts = 0.0; - public double[] currentAmps = new double[] {}; - } - - /** Updates the set of loggable inputs. */ - public default void updateInputs(FlywheelIOInputs inputs) {} - - /** Run open loop at the specified voltage. */ - public default void setVoltage(double volts) {} - - /** Run closed loop at the specified velocity. */ - public default void setVelocity(double velocityRadPerSec, double ffVolts) {} - - /** Stop in open loop. */ - public default void stop() {} - - /** Set velocity PID constants. */ - public default void configurePID(double kP, double kI, double kD) {} -} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java deleted file mode 100644 index 32ffa6f..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.flywheel; - -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 FlywheelIOSim implements FlywheelIO { - private FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), 1.5, 0.004); - private PIDController pid = new PIDController(0.0, 0.0, 0.0); - - private boolean closedLoop = false; - private double ffVolts = 0.0; - private double appliedVolts = 0.0; - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - if (closedLoop) { - appliedVolts = - MathUtil.clamp(pid.calculate(sim.getAngularVelocityRadPerSec()) + ffVolts, -12.0, 12.0); - sim.setInputVoltage(appliedVolts); - } - - sim.update(0.02); - - inputs.positionRad = 0.0; - inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); - inputs.appliedVolts = appliedVolts; - inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()}; - } - - @Override - public void setVoltage(double volts) { - closedLoop = false; - appliedVolts = volts; - sim.setInputVoltage(volts); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - closedLoop = true; - pid.setSetpoint(velocityRadPerSec); - this.ffVolts = ffVolts; - } - - @Override - public void stop() { - setVoltage(0.0); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - pid.setPID(kP, kI, kD); - } -} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java deleted file mode 100644 index 787f369..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.flywheel; - -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkPIDController.ArbFFUnits; -import edu.wpi.first.math.util.Units; - -/** - * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with - * "CANSparkFlex". - */ -public class FlywheelIOSparkMax implements FlywheelIO { - private static final double GEAR_RATIO = 1.5; - - private final CANSparkMax leader = new CANSparkMax(0, MotorType.kBrushless); - private final CANSparkMax follower = new CANSparkMax(1, MotorType.kBrushless); - private final RelativeEncoder encoder = leader.getEncoder(); - private final SparkPIDController pid = leader.getPIDController(); - - public FlywheelIOSparkMax() { - leader.restoreFactoryDefaults(); - follower.restoreFactoryDefaults(); - - leader.setCANTimeout(250); - follower.setCANTimeout(250); - - leader.setInverted(false); - follower.follow(leader, false); - - leader.enableVoltageCompensation(12.0); - leader.setSmartCurrentLimit(30); - - leader.burnFlash(); - follower.burnFlash(); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / GEAR_RATIO); - inputs.velocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); - inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); - inputs.currentAmps = new double[] {leader.getOutputCurrent(), follower.getOutputCurrent()}; - } - - @Override - public void setVoltage(double volts) { - leader.setVoltage(volts); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - pid.setReference( - Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, - ControlType.kVelocity, - 0, - ffVolts, - ArbFFUnits.kVoltage); - } - - @Override - public void stop() { - leader.stopMotor(); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - pid.setP(kP, 0); - pid.setI(kI, 0); - pid.setD(kD, 0); - pid.setFF(0, 0); - } -} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java deleted file mode 100644 index 3f53512..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.flywheel; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; - -public class FlywheelIOTalonFX implements FlywheelIO { - private static final double GEAR_RATIO = 1.5; - - private final TalonFX leader = new TalonFX(0); - private final TalonFX follower = new TalonFX(1); - - private final StatusSignal leaderPosition = leader.getPosition(); - private final StatusSignal leaderVelocity = leader.getVelocity(); - private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage(); - private final StatusSignal leaderCurrent = leader.getSupplyCurrent(); - private final StatusSignal followerCurrent = follower.getSupplyCurrent(); - - public FlywheelIOTalonFX() { - var config = new TalonFXConfiguration(); - config.CurrentLimits.SupplyCurrentLimit = 30.0; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - config.MotorOutput.NeutralMode = NeutralModeValue.Coast; - leader.getConfigurator().apply(config); - follower.getConfigurator().apply(config); - follower.setControl(new Follower(leader.getDeviceID(), false)); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); - leader.optimizeBusUtilization(); - follower.optimizeBusUtilization(); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - BaseStatusSignal.refreshAll( - leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); - inputs.positionRad = Units.rotationsToRadians(leaderPosition.getValueAsDouble()) / GEAR_RATIO; - inputs.velocityRadPerSec = - Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / GEAR_RATIO; - inputs.appliedVolts = leaderAppliedVolts.getValueAsDouble(); - inputs.currentAmps = - new double[] {leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()}; - } - - @Override - public void setVoltage(double volts) { - leader.setControl(new VoltageOut(volts)); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - leader.setControl( - new VelocityVoltage( - Units.radiansToRotations(velocityRadPerSec), - 0.0, - true, - ffVolts, - 0, - false, - false, - false)); - } - - @Override - public void stop() { - leader.stopMotor(); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - var config = new Slot0Configs(); - config.kP = kP; - config.kI = kI; - config.kD = kD; - leader.getConfigurator().apply(config); - } -} diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java new file mode 100644 index 0000000..fea79a1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems.indexer; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Indexer extends SubsystemBase { + private IndexerIO m_io; + public final IndexerInputsAutoLogged m_inputs; + + public Indexer(IndexerIO io) { + m_io = io; + m_inputs = new IndexerInputsAutoLogged(); + } + + @Override + public void periodic() { + m_io.updateInputs(m_inputs); + + Logger.processInputs("Indexer", m_inputs); + } + + public Command runKicker(double voltage) { + return Commands.runOnce(() -> m_io.setKickerVoltage(voltage)); + } + + public Command runFeeder(double voltage) { + return Commands.runOnce(() -> m_io.setFeederVoltage(voltage)); + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java new file mode 100644 index 0000000..fbcd937 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.indexer; + +import frc.lib.advantagekit.LoggedIO; +import org.littletonrobotics.junction.AutoLog; + +public interface IndexerIO extends LoggedIO { + @AutoLog + public static class IndexerInputs { + public double curKickerVelocity; + public double curKickerVoltage; + public double curKickerAmps; + public double curFeederVelocity; + public double curFeederVoltage; + public double curFeederAmps; + public boolean beamBreakOneBroken; + public boolean beamBreakTwoBroken; + } + + public void setKickerVoltage(double voltage); + + public void setFeederVoltage(double voltage); +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOFalcon.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOFalcon.java new file mode 100644 index 0000000..11613a3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOFalcon.java @@ -0,0 +1,69 @@ +package frc.robot.subsystems.indexer; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DigitalInput; + +public class IndexerIOFalcon implements IndexerIO { + private TalonFX m_kickerMotor; + private TalonFX m_feederMotor; + private DigitalInput m_beamBreakOne; + private DigitalInput m_beamBreakTwo; + + private StatusSignal m_curKickerVelocity; + private StatusSignal m_curKickerVoltage; + private StatusSignal m_curKickerAmps; + private StatusSignal m_curFeederVelocity; + private StatusSignal m_curFeederVoltage; + private StatusSignal m_curFeederAmps; + + public IndexerIOFalcon( + int kickerPort, int feederPort, int beamBreakOnePort, int beamBreakTwoPort) { + m_kickerMotor = new TalonFX(kickerPort); + m_feederMotor = new TalonFX(feederPort); + m_beamBreakOne = new DigitalInput(beamBreakOnePort); + m_beamBreakTwo = new DigitalInput(beamBreakTwoPort); + + m_curKickerVelocity = m_kickerMotor.getVelocity(); + m_curKickerVoltage = m_kickerMotor.getMotorVoltage(); + m_curKickerAmps = m_kickerMotor.getSupplyCurrent(); + + m_curFeederVelocity = m_feederMotor.getVelocity(); + m_curFeederVoltage = m_feederMotor.getMotorVoltage(); + m_curFeederAmps = m_feederMotor.getSupplyCurrent(); + } + + @Override + public void updateInputs(IndexerInputs inputs) { + BaseStatusSignal.refreshAll( + m_curKickerVelocity, + m_curKickerVoltage, + m_curKickerAmps, + m_curFeederVelocity, + m_curFeederVoltage, + m_curFeederAmps); + + inputs.curKickerVelocity = Units.rotationsToRadians(m_curKickerVelocity.getValue()); + inputs.curKickerVoltage = m_curKickerVoltage.getValue(); + inputs.curKickerAmps = m_curKickerAmps.getValue(); + + inputs.curFeederVelocity = Units.rotationsToRadians(m_curFeederVelocity.getValue()); + inputs.curFeederVoltage = m_curFeederVoltage.getValue(); + inputs.curFeederAmps = m_curFeederAmps.getValue(); + + inputs.beamBreakOneBroken = m_beamBreakOne.get(); + inputs.beamBreakTwoBroken = m_beamBreakTwo.get(); + } + + @Override + public void setKickerVoltage(double voltage) { + m_kickerMotor.setVoltage(voltage); + } + + @Override + public void setFeederVoltage(double voltage) { + m_feederMotor.setVoltage(voltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java new file mode 100644 index 0000000..5e66140 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java @@ -0,0 +1,57 @@ +package frc.robot.subsystems.indexer; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.DIOSim; +import frc.robot.Constants.Ports; + +public class IndexerIOSim implements IndexerIO { + private DCMotorSim m_kickerSim; + private DCMotorSim m_feederSim; + private double m_curKickerVoltage; + private double m_curFeederVoltage; + private DIOSim m_beamBreakOne; + private DIOSim m_beamBreakTwo; + + public IndexerIOSim() { + double simGearing = 1.0; + double simJkGMetersSquared = 0.01; + m_kickerSim = new DCMotorSim(DCMotor.getFalcon500(1), simGearing, simJkGMetersSquared); + m_feederSim = new DCMotorSim(DCMotor.getFalcon500(1), simGearing, simJkGMetersSquared); + + m_beamBreakOne = new DIOSim(Ports.kIndexerBeamBreakOne); + m_beamBreakTwo = new DIOSim(Ports.kIndexerBeamBreakTwo); + + m_curKickerVoltage = 0.0; + m_curFeederVoltage = 0.0; + } + + @Override + public void updateInputs(IndexerInputs inputs) { + m_kickerSim.update(0.02); + m_feederSim.update(0.02); + + inputs.curKickerVoltage = m_curKickerVoltage; + inputs.curKickerVelocity = m_kickerSim.getAngularVelocityRadPerSec(); + inputs.curKickerAmps = m_kickerSim.getCurrentDrawAmps(); + + inputs.curFeederVoltage = m_curFeederVoltage; + inputs.curFeederVelocity = m_feederSim.getAngularVelocityRadPerSec(); + inputs.curFeederAmps = m_feederSim.getCurrentDrawAmps(); + + inputs.beamBreakOneBroken = m_beamBreakOne.getValue(); + inputs.beamBreakTwoBroken = m_beamBreakTwo.getValue(); + } + + @Override + public void setKickerVoltage(double voltage) { + m_kickerSim.setInputVoltage(voltage); + m_curKickerVoltage = voltage; + } + + @Override + public void setFeederVoltage(double voltage) { + m_feederSim.setInputVoltage(voltage); + m_curFeederVoltage = voltage; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java new file mode 100644 index 0000000..c683d15 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -0,0 +1,210 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.lib.utils.LoggedTunableNumber; +import frc.robot.Constants.ShooterConstants; +import frc.robot.subsystems.shooter.flywheel.FlywheelIO; +import frc.robot.subsystems.shooter.flywheel.FlywheelInputsAutoLogged; +import frc.robot.subsystems.shooter.pivot.ShooterPivotIO; +import frc.robot.subsystems.shooter.pivot.ShooterPivotInputsAutoLogged; +import org.littletonrobotics.junction.Logger; + +public class Shooter extends SubsystemBase { + /** Represents the desired state of the shooter, including flywheel and pivot measurements */ + public static class ShooterPosition { + public Rotation2d pivotAngle; + public double leftFlywheelVelocity; + public double rightFlywheelVelocity; + + public ShooterPosition( + Rotation2d pivotAngle, double leftFlywheelVelocity, double rightFlywheelVelocity) { + this.pivotAngle = pivotAngle; + this.leftFlywheelVelocity = leftFlywheelVelocity; + this.rightFlywheelVelocity = rightFlywheelVelocity; + } + } + + private ShooterPivotIO m_pivotIO; + private FlywheelIO m_flywheelIO; + + public final ShooterPivotInputsAutoLogged m_pivotInputs; + public final FlywheelInputsAutoLogged m_flywheelInputs; + + private ProfiledPIDController m_pivotController; + private ProfiledPIDController m_leftFlywheelController; + private ProfiledPIDController m_rightFlywheelController; + + private SimpleMotorFeedforward m_rightFlywheelFeedforward; + private SimpleMotorFeedforward m_leftFlywheelFeedforward; + + public Shooter( + ShooterPivotIO pivotIO, + FlywheelIO flywheelIO, + ProfiledPIDController pivotController, + ProfiledPIDController leftFlywheelController, + ProfiledPIDController rightFlywheelController, + SimpleMotorFeedforward leftFlywheelFeedforward, + SimpleMotorFeedforward rightFlywheelFeedforward) { + + m_pivotIO = pivotIO; + m_flywheelIO = flywheelIO; + + m_pivotController = pivotController; + m_leftFlywheelController = leftFlywheelController; + m_rightFlywheelController = rightFlywheelController; + + m_rightFlywheelFeedforward = rightFlywheelFeedforward; + m_leftFlywheelFeedforward = leftFlywheelFeedforward; + + m_pivotInputs = new ShooterPivotInputsAutoLogged(); + m_flywheelInputs = new FlywheelInputsAutoLogged(); + } + + @Override + public void periodic() { + // update pid if changed + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + m_pivotController.setP(ShooterConstants.kPivotP.get()); + m_pivotController.setI(ShooterConstants.kPivotI.get()); + m_pivotController.setD(ShooterConstants.kPivotD.get()); + }, + ShooterConstants.kPivotP, + ShooterConstants.kPivotI, + ShooterConstants.kPivotD); + + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + m_leftFlywheelController.setP(ShooterConstants.kLeftFlywheelP.get()); + m_leftFlywheelController.setI(ShooterConstants.kLeftFlywheelI.get()); + m_leftFlywheelController.setD(ShooterConstants.kLeftFlywheelD.get()); + }, + ShooterConstants.kLeftFlywheelP, + ShooterConstants.kLeftFlywheelI, + ShooterConstants.kLeftFlywheelD); + + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + m_rightFlywheelController.setP(ShooterConstants.kRightFlywheelP.get()); + m_rightFlywheelController.setI(ShooterConstants.kRightFlywheelI.get()); + m_rightFlywheelController.setD(ShooterConstants.kRightFlywheelD.get()); + }, + ShooterConstants.kRightFlywheelP, + ShooterConstants.kRightFlywheelI, + ShooterConstants.kRightFlywheelD); + + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + m_rightFlywheelFeedforward = + new SimpleMotorFeedforward( + ShooterConstants.kRightFlywheelKs.get(), ShooterConstants.kRightFlywheelKv.get()); + }, + ShooterConstants.kRightFlywheelKs, + ShooterConstants.kRightFlywheelKv); + + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + m_leftFlywheelFeedforward = + new SimpleMotorFeedforward( + ShooterConstants.kLeftFlywheelKs.get(), ShooterConstants.kLeftFlywheelKv.get()); + }, + ShooterConstants.kLeftFlywheelKs, + ShooterConstants.kLeftFlywheelKv); + + m_pivotIO.updateInputs(m_pivotInputs); + m_flywheelIO.updateInputs(m_flywheelInputs); + + double pivotPidVoltage = m_pivotController.calculate(m_pivotInputs.curAngle); + m_pivotIO.setVoltage(pivotPidVoltage); + + double leftFlywheelPidVoltage = + m_leftFlywheelController.calculate(m_flywheelInputs.linearVelocity[0]); + double rightFlywheelPidVoltage = + m_rightFlywheelController.calculate(m_flywheelInputs.linearVelocity[1]); + + double leftFlywheelFeedforwardVoltage = + m_leftFlywheelFeedforward.calculate(m_leftFlywheelController.getGoal().position); + double rightFlywheelFeedforwardVoltage = + m_rightFlywheelFeedforward.calculate(m_rightFlywheelController.getGoal().position); + + m_flywheelIO.setVoltage( + leftFlywheelPidVoltage + leftFlywheelFeedforwardVoltage, + rightFlywheelPidVoltage + rightFlywheelFeedforwardVoltage); + + Logger.processInputs("Shooter Pivot", m_pivotInputs); + Logger.processInputs("Shooter Flywheel", m_flywheelInputs); + + Logger.recordOutput("Shooter/Pivot/PivotPIDVoltage", pivotPidVoltage); + Logger.recordOutput("Shooter/Pivot/PivotAngle", Units.radiansToDegrees(m_pivotInputs.curAngle)); + Logger.recordOutput( + "Shooter/Pivot/PivotDesiredAngle", + Units.radiansToDegrees(m_pivotController.getGoal().position)); + + Logger.recordOutput("Shooter/Flywheel/LeftFlywheelPIDVoltage", leftFlywheelPidVoltage); + Logger.recordOutput("Shooter/Flywheel/RightFlywheelPIDVoltage", rightFlywheelPidVoltage); + + Logger.recordOutput( + "Shooter/Flywheel/LeftFlywheelFeedforwardVoltage", leftFlywheelFeedforwardVoltage); + Logger.recordOutput( + "Shooter/Flywheel/RightFlywheelFeedforwardVoltage", rightFlywheelFeedforwardVoltage); + + Logger.recordOutput( + "Shooter/Flywheel/LeftFlywheelSetVoltage", leftFlywheelPidVoltage + leftFlywheelFeedforwardVoltage); + Logger.recordOutput( + "Shooter/Flywheel/RightFlywheelSetVoltage", rightFlywheelPidVoltage + rightFlywheelFeedforwardVoltage); + + Logger.recordOutput( + "Shooter/Flywheel/LeftFlywheelVelocity", m_flywheelInputs.linearVelocity[0]); + Logger.recordOutput( + "Shooter/Flywheel/RightFlywheelVelocity", m_flywheelInputs.linearVelocity[1]); + Logger.recordOutput( + "Shooter/Flywheel/LeftFlywheelDesiredVelocity", + m_leftFlywheelController.getGoal().position); + Logger.recordOutput( + "Shooter/Flywheel/RightFlywheelDesiredVelocity", + m_rightFlywheelController.getGoal().position); + } + + public void setPivotAngle(Rotation2d angle) { + m_pivotController.setGoal(angle.getRadians()); + } + + /** Set the flywheel velocity in meters per second */ + public void setFlywheelVelocity(double leftVelocity, double rightVelocity) { + m_leftFlywheelController.setGoal(leftVelocity); + m_rightFlywheelController.setGoal(rightVelocity); + } + + public Command setPivotAngleCommand(Rotation2d angle) { + return Commands.runOnce(() -> setPivotAngle(angle)); + } + + /** Set the flywheel velocity in meters per second */ + public Command setFlywheelVelocityCommand(double leftVelocity, double rightVelocity) { + return Commands.runOnce(() -> setFlywheelVelocity(leftVelocity, rightVelocity)); + } + + public void setShooter(ShooterPosition state) { + setPivotAngle(state.pivotAngle); + setFlywheelVelocity(state.leftFlywheelVelocity, state.rightFlywheelVelocity); + } + + public Command setShooterCommand(ShooterPosition state) { + return Commands.runOnce(() -> setShooter(state)); + } + + public Rotation2d getPivotAngle() { + return Rotation2d.fromRadians(m_pivotInputs.curAngle); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java new file mode 100644 index 0000000..da73cd0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java @@ -0,0 +1,17 @@ +package frc.robot.subsystems.shooter.flywheel; + +import frc.lib.advantagekit.LoggedIO; +import org.littletonrobotics.junction.AutoLog; + +public interface FlywheelIO extends LoggedIO { + @AutoLog + public static class FlywheelInputs { + public double[] angularVelocityRad; + public double[] angularVelocityRPS; + public double[] linearVelocity; + public double[] curVoltage; + public double[] curAmps; + } + + public void setVoltage(double leftVoltage, double rightVoltage); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOKraken.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOKraken.java new file mode 100644 index 0000000..89c24b7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOKraken.java @@ -0,0 +1,82 @@ +package frc.robot.subsystems.shooter.flywheel; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants.ShooterConstants; + +public class FlywheelIOKraken implements FlywheelIO { + private TalonFX m_leftMotor; + private TalonFX m_rightMotor; + + private StatusSignal m_leftVoltageSignal; + private StatusSignal m_leftCurrentSignal; + private StatusSignal m_leftVelocitySignal; + private StatusSignal m_rightVoltageSignal; + private StatusSignal m_rightCurrentSignal; + private StatusSignal m_rightVelocitySignal; + + public FlywheelIOKraken(int leftPort, int rightPort) { + m_leftMotor = new TalonFX(leftPort); + m_rightMotor = new TalonFX(rightPort); + + // status signals + m_leftVoltageSignal = m_leftMotor.getMotorVoltage(); + m_leftCurrentSignal = m_leftMotor.getSupplyCurrent(); + m_leftVelocitySignal = m_leftMotor.getVelocity(); + m_rightVoltageSignal = m_rightMotor.getMotorVoltage(); + m_rightCurrentSignal = m_rightMotor.getSupplyCurrent(); + m_rightVelocitySignal = m_rightMotor.getVelocity(); + } + + @Override + public void updateInputs(FlywheelInputs inputs) { + BaseStatusSignal.refreshAll( + m_leftVoltageSignal, + m_leftCurrentSignal, + m_leftVelocitySignal, + m_rightVoltageSignal, + m_rightCurrentSignal, + m_rightVelocitySignal); + + inputs.angularVelocityRad = + new double[] { + Units.rotationsToRadians(m_leftVelocitySignal.getValueAsDouble()), + Units.rotationsToRadians(m_rightVelocitySignal.getValueAsDouble()) + }; + + inputs.angularVelocityRPS = + new double[] { + m_leftVelocitySignal.getValueAsDouble(), m_rightVelocitySignal.getValueAsDouble() + }; + + inputs.linearVelocity = + new double[] { + rotationsPerSecondToMeterPerSecond(m_leftVelocitySignal.getValueAsDouble()), + rotationsPerSecondToMeterPerSecond(m_rightVelocitySignal.getValueAsDouble()) + }; + + inputs.curVoltage = + new double[] { + m_leftVoltageSignal.getValueAsDouble(), m_rightVoltageSignal.getValueAsDouble() + }; + + inputs.curAmps = + new double[] { + m_leftCurrentSignal.getValueAsDouble(), m_rightCurrentSignal.getValueAsDouble() + }; + } + + private double rotationsPerSecondToMeterPerSecond(double rotationsPerSecond) { + double radius = ShooterConstants.kFlywheelDiameter / 2; + double radPerSec = Units.rotationsToRadians(rotationsPerSecond); + return radius * radPerSec; + } + + @Override + public void setVoltage(double leftVoltage, double rightVoltage) { + m_leftMotor.setVoltage(leftVoltage); + m_rightMotor.setVoltage(rightVoltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java new file mode 100644 index 0000000..53609c7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -0,0 +1,53 @@ +package frc.robot.subsystems.shooter.flywheel; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.robot.Constants.ShooterConstants; + +public class FlywheelIOSim implements FlywheelIO { + private FlywheelSim m_leftSim; + private FlywheelSim m_rightSim; + private double m_leftVoltage; + private double m_rightVoltage; + + public FlywheelIOSim() { + m_leftSim = new FlywheelSim(DCMotor.getKrakenX60(1), 1 / 1.5, 1.0); + m_rightSim = new FlywheelSim(DCMotor.getKrakenX60(1), 1 / 1.5, 1.0); + m_leftVoltage = 0.0; + m_rightVoltage = 0.0; + } + + @Override + public void updateInputs(FlywheelInputs inputs) { + m_leftSim.update(0.02); + m_rightSim.update(0.02); + + inputs.curVoltage = new double[] {m_leftVoltage, m_rightVoltage}; + + inputs.angularVelocityRad = + new double[] { + m_leftSim.getAngularVelocityRadPerSec(), m_leftSim.getAngularVelocityRadPerSec() + }; + + inputs.angularVelocityRPS = + new double[] { + m_leftSim.getAngularVelocityRPM() / 60.0, m_rightSim.getAngularVelocityRPM() / 60.0 + }; + + inputs.linearVelocity = + new double[] { + m_leftSim.getAngularVelocityRadPerSec() * ShooterConstants.kFlywheelDiameter / 2.0, + m_rightSim.getAngularVelocityRadPerSec() * ShooterConstants.kFlywheelDiameter / 2.0 + }; + + inputs.curAmps = new double[] {m_leftSim.getCurrentDrawAmps(), m_rightSim.getCurrentDrawAmps()}; + } + + @Override + public void setVoltage(double leftVoltage, double rightVoltage) { + m_leftSim.setInputVoltage(leftVoltage); + m_rightSim.setInputVoltage(rightVoltage); + m_leftVoltage = leftVoltage; + m_rightVoltage = rightVoltage; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/pivot/ShooterPivotIO.java b/src/main/java/frc/robot/subsystems/shooter/pivot/ShooterPivotIO.java new file mode 100644 index 0000000..79dae57 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/pivot/ShooterPivotIO.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.shooter.pivot; + +import edu.wpi.first.math.geometry.Rotation2d; +import frc.lib.advantagekit.LoggedIO; +import org.littletonrobotics.junction.AutoLog; + +public interface ShooterPivotIO extends LoggedIO { + @AutoLog + public static class ShooterPivotInputs { + public double curAngle; + public double[] curVelocity; + public double[] curVoltage; + public double[] curAmps; + } + + public void setVoltage(double voltage); + + public Rotation2d getCurrentAngle(); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/pivot/ShooterPivotIOFalcon.java b/src/main/java/frc/robot/subsystems/shooter/pivot/ShooterPivotIOFalcon.java new file mode 100644 index 0000000..8a37f75 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/pivot/ShooterPivotIOFalcon.java @@ -0,0 +1,76 @@ +package frc.robot.subsystems.shooter.pivot; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import frc.robot.Constants.ShooterConstants; + +public class ShooterPivotIOFalcon implements ShooterPivotIO { + private TalonFX m_leader; + private TalonFX m_follower; + private DutyCycleEncoder m_absoluteEncoder; + + private StatusSignal m_leaderVoltageSignal; + private StatusSignal m_leaderCurrentSignal; + private StatusSignal m_leaderVelocitySignal; + private StatusSignal m_followerVoltageSignal; + private StatusSignal m_followerCurrentSignal; + private StatusSignal m_followerVelocitySignal; + + public ShooterPivotIOFalcon(int leaderPort, int followerPort, int absoluteEncoderPort) { + m_leader = new TalonFX(leaderPort); + m_follower = new TalonFX(followerPort); + m_absoluteEncoder = new DutyCycleEncoder(absoluteEncoderPort); + + // follow leader + m_follower.setControl(new Follower(leaderPort, false)); + + // status signals + m_leaderVoltageSignal = m_leader.getMotorVoltage(); + m_leaderCurrentSignal = m_leader.getSupplyCurrent(); + m_leaderVelocitySignal = m_leader.getVelocity(); + m_followerVoltageSignal = m_follower.getMotorVoltage(); + m_followerCurrentSignal = m_follower.getSupplyCurrent(); + m_followerVelocitySignal = m_follower.getVelocity(); + + m_absoluteEncoder.setPositionOffset(ShooterConstants.kPivotOffset.getRotations()); + } + + @Override + public void updateInputs(ShooterPivotInputs inputs) { + BaseStatusSignal.refreshAll( + m_leaderVoltageSignal, + m_leaderCurrentSignal, + m_leaderVelocitySignal, + m_followerVoltageSignal, + m_followerCurrentSignal, + m_followerVelocitySignal); + + inputs.curAngle = getCurrentAngle().getRadians(); + inputs.curVelocity = + new double[] { + m_leaderVelocitySignal.getValueAsDouble(), m_followerVelocitySignal.getValueAsDouble() + }; + inputs.curVoltage = + new double[] { + m_leaderVoltageSignal.getValueAsDouble(), m_followerVoltageSignal.getValueAsDouble() + }; + inputs.curAmps = + new double[] { + m_leaderCurrentSignal.getValueAsDouble(), m_followerCurrentSignal.getValueAsDouble() + }; + } + + @Override + public void setVoltage(double voltage) { + m_leader.setVoltage(voltage); + } + + @Override + public Rotation2d getCurrentAngle() { + return Rotation2d.fromRotations(m_absoluteEncoder.get()); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/pivot/ShooterPivotIOSim.java b/src/main/java/frc/robot/subsystems/shooter/pivot/ShooterPivotIOSim.java new file mode 100644 index 0000000..f8b630d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/pivot/ShooterPivotIOSim.java @@ -0,0 +1,47 @@ +package frc.robot.subsystems.shooter.pivot; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.Constants.ShooterConstants; + +public class ShooterPivotIOSim implements ShooterPivotIO { + private SingleJointedArmSim m_sim; + private double m_voltage; + + public ShooterPivotIOSim() { + m_sim = + new SingleJointedArmSim( + DCMotor.getFalcon500(2), + 46.722, + 10, + .24, + ShooterConstants.kMinAngle.getRadians(), + ShooterConstants.kMaxAngle.getRadians(), + false, + ShooterConstants.kHomeAngle.getRadians()); + m_voltage = 0.0; + } + + @Override + public void updateInputs(ShooterPivotInputs inputs) { + m_sim.update(0.02); + + // distinction between left and right doesnt matter in sim + inputs.curAngle = getCurrentAngle().getRadians(); + inputs.curVoltage = new double[] {m_voltage}; + inputs.curVelocity = new double[] {m_sim.getVelocityRadPerSec()}; + inputs.curAmps = new double[] {m_sim.getCurrentDrawAmps()}; + } + + @Override + public void setVoltage(double voltage) { + m_sim.setInputVoltage(voltage); + m_voltage = voltage; + } + + @Override + public Rotation2d getCurrentAngle() { + return Rotation2d.fromRadians(m_sim.getAngleRads()); + } +} diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index 5309b85..191b54f 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -129,7 +129,8 @@ public void fromLog(LogTable table) { List pathPoints = new ArrayList<>(); for (int i = 0; i < pointsLogged.length; i += 2) { - pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + pathPoints.add( + new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); } currentPathPoints = pathPoints; diff --git a/src/main/java/frc/robot/util/shooter/ShooterMath.java b/src/main/java/frc/robot/util/shooter/ShooterMath.java new file mode 100644 index 0000000..c559082 --- /dev/null +++ b/src/main/java/frc/robot/util/shooter/ShooterMath.java @@ -0,0 +1,13 @@ +package frc.robot.util.shooter; + +import frc.robot.subsystems.shooter.Shooter.ShooterPosition; + +public interface ShooterMath { + public ShooterPosition calculateShooterPosition(double distance); + + public double calculateLeftFlywheelVelocity(double distance); + + public double calculateRightFlywheelVelocity(double distance); + + public double calculatePivotAngle(double distance); +} diff --git a/src/main/java/frc/robot/util/shooter/ShooterMathTreeMap.java b/src/main/java/frc/robot/util/shooter/ShooterMathTreeMap.java new file mode 100644 index 0000000..c6bf746 --- /dev/null +++ b/src/main/java/frc/robot/util/shooter/ShooterMathTreeMap.java @@ -0,0 +1,55 @@ +package frc.robot.util.shooter; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import frc.robot.subsystems.shooter.Shooter.ShooterPosition; + +public class ShooterMathTreeMap implements ShooterMath { + private InterpolatingDoubleTreeMap m_shootSpeedLeft; + private InterpolatingDoubleTreeMap m_shootSpeedRight; + // in degrees + private InterpolatingDoubleTreeMap m_shootAngle; + + private final double kMeasurementOffset = 0.0; + // when calibrating, this is the x distance from 0.0 on the tape measure to the speaker hole + + public ShooterMathTreeMap() { + m_shootSpeedLeft = new InterpolatingDoubleTreeMap(); + m_shootSpeedRight = new InterpolatingDoubleTreeMap(); + m_shootAngle = new InterpolatingDoubleTreeMap(); + + // Left data + + // Right data + + // Angle data + + } + + @Override + public ShooterPosition calculateShooterPosition(double distance) { + distance += kMeasurementOffset; + return new ShooterPosition( + Rotation2d.fromDegrees(m_shootAngle.get(distance)), + m_shootSpeedLeft.get(distance), + m_shootSpeedRight.get(distance)); + } + + @Override + public double calculateLeftFlywheelVelocity(double distance) { + distance += kMeasurementOffset; + return m_shootSpeedLeft.get(distance); + } + + @Override + public double calculateRightFlywheelVelocity(double distance) { + distance += kMeasurementOffset; + return m_shootSpeedRight.get(distance); + } + + @Override + public double calculatePivotAngle(double distance) { + distance += kMeasurementOffset; + return m_shootAngle.get(distance); + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index b35a53a..55a634d 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,42 +1,42 @@ { - "fileName": "AdvantageKit.json", - "name": "AdvantageKit", - "version": "3.2.0", - "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2024", - "mavenUrls": [], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "wpilib-shim", - "version": "3.2.0" - }, - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "junction-core", - "version": "3.2.0" - }, - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-api", - "version": "3.2.0" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-wpilibio", - "version": "3.2.0", - "skipInvalidPlatforms": false, - "isJar": false, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [] -} \ No newline at end of file + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "3.2.0", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "wpilib-shim", + "version": "3.2.0" + }, + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "junction-core", + "version": "3.2.0" + }, + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-api", + "version": "3.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-wpilibio", + "version": "3.2.0", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [] +}