From b6e03f3dd4885abbaf7c011e5ec54cc0f07f5d33 Mon Sep 17 00:00:00 2001 From: Otto Date: Sat, 3 Aug 2024 22:19:54 -0400 Subject: [PATCH 1/6] PP wrapper --- src/main/java/frc/robot/Constants.java | 463 +++++++------ src/main/java/frc/robot/Main.java | 18 +- src/main/java/frc/robot/Robot.java | 278 ++++---- .../java/frc/robot/commands/AutoCommands.java | 68 +- .../java/frc/robot/subsystems/Subsystem.java | 152 ++--- .../frc/robot/subsystems/SubsystemStates.java | 2 +- .../java/frc/robot/subsystems/Trigger.java | 24 +- .../frc/robot/subsystems/ampBar/AmpBar.java | 104 ++- .../frc/robot/subsystems/ampBar/AmpBarIO.java | 52 +- .../robot/subsystems/ampBar/AmpBarIOReal.java | 192 +++--- .../robot/subsystems/ampBar/AmpBarIOSim.java | 194 +++--- .../robot/subsystems/ampBar/AmpBarStates.java | 44 +- .../frc/robot/subsystems/drive/Drive.java | 640 ++++++++---------- .../robot/subsystems/drive/DriveStates.java | 58 +- .../frc/robot/subsystems/drive/GyroIO.java | 18 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 88 +-- .../frc/robot/subsystems/drive/Module.java | 402 ++++++----- .../frc/robot/subsystems/drive/ModuleIO.java | 50 +- .../subsystems/drive/ModuleIOHybrid.java | 357 +++++----- .../robot/subsystems/drive/ModuleIOSim.java | 88 ++- .../subsystems/drive/ModuleIOSparkMax.java | 325 +++++---- .../subsystems/drive/ModuleIOTalonFX.java | 357 +++++----- .../subsystems/drive/PPDriveWrapper.java | 53 ++ .../drive/PhoenixOdometryThread.java | 198 +++--- .../drive/SparkMaxOdometryThread.java | 144 ++-- .../frc/robot/subsystems/intake/Intake.java | 102 ++- .../frc/robot/subsystems/intake/IntakeIO.java | 43 +- .../robot/subsystems/intake/IntakeIOSim.java | 181 +++-- .../subsystems/intake/IntakeIOSparkMax.java | 120 ++-- .../robot/subsystems/intake/IntakeStates.java | 70 +- .../frc/robot/subsystems/manager/Manager.java | 265 ++++---- .../subsystems/manager/ManagerStates.java | 85 ++- .../frc/robot/subsystems/shooter/Shooter.java | 103 ++- .../robot/subsystems/shooter/ShooterIO.java | 36 +- .../subsystems/shooter/ShooterIOSim.java | 94 ++- .../subsystems/shooter/ShooterIOTalonFX.java | 110 ++- .../subsystems/shooter/ShooterStates.java | 32 +- src/main/java/frc/robot/util/FFConstants.java | 12 +- .../java/frc/robot/util/LocalADStarAK.java | 270 ++++---- .../java/frc/robot/util/NoteSimulator.java | 202 +++--- 40 files changed, 2967 insertions(+), 3127 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4a130bc..aaf9a61 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -32,292 +32,283 @@ */ public final class Constants { - public static final double SIM_UPDATE_TIME = 0.05; + public static final double SIM_UPDATE_TIME = 0.05; - // Conversion Factors - public static final double RADIAN_CF = (Math.PI * 2); - public static final double RPM_TO_RPS_CF = 60; - public static final double DIAM_TO_RADIUS_CF = 2.0; - public static final double AVG_TWO_ITEM_F = 2.0; + // Conversion Factors + public static final double RADIAN_CF = (Math.PI * 2); + public static final double RPM_TO_RPS_CF = 60; + public static final double DIAM_TO_RADIUS_CF = 2.0; + public static final double AVG_TWO_ITEM_F = 2.0; - public static final Mode currentMode = Mode.SIM; + public static final Mode currentMode = Mode.SIM; - public static final double MAX_VOLTS = 12.0; - public static final double MIN_VOLTS = -12.0; + public static final double MAX_VOLTS = 12.0; + public static final double MIN_VOLTS = -12.0; - public static final XboxController controller = new XboxController(0); - public static final XboxController operatorController = new XboxController(1); + public static final XboxController controller = new XboxController(0); + public static final XboxController operatorController = new XboxController(1); - public static enum Mode { - /** Running on a real robot. */ - REAL, + public static enum Mode { + /** Running on a real robot. */ + REAL, - /** Running a physics simulator. */ - SIM, + /** Running a physics simulator. */ + SIM, - /** Replaying from a log file. */ - REPLAY, - } + /** Replaying from a log file. */ + REPLAY, + } - public static final class Intake { + public static final class Intake { - public static final Translation3d ZEROED_PIVOT_TRANSLATION = new Translation3d( - 0.31, - 0, - 0.24 - ); + public static final Translation3d ZEROED_PIVOT_TRANSLATION = new Translation3d(0.31, 0, 0.24); - // CAN IDs - public static final int PIVOT_ID = 32; - public static final int SPINNER_ID = 20; + // CAN IDs + public static final int PIVOT_ID = 32; + public static final int SPINNER_ID = 20; - // PID - public static final PIDConstants SIM_IN_PID = new PIDConstants(1, 0, 0); - public static final PIDConstants SIM_OUT_PID = new PIDConstants(1, 0, 0); + // PID + public static final PIDConstants SIM_IN_PID = new PIDConstants(1, 0, 0); + public static final PIDConstants SIM_OUT_PID = new PIDConstants(1, 0, 0); - public static final PIDConstants REAL_IN_PID = new PIDConstants(1, 0, 0); - public static final PIDConstants REAL_OUT_PID = new PIDConstants(1, 0, 0); + public static final PIDConstants REAL_IN_PID = new PIDConstants(1, 0, 0); + public static final PIDConstants REAL_OUT_PID = new PIDConstants(1, 0, 0); - // Sim Configs + // Sim Configs - // Pivot - public static final int NUM_PIVOT_MOTORS = 1; - public static final double PIVOT_GEARING = 67.5; - public static final double MAX_PIVOT_POSITION = Units.degreesToRadians(180.0); - public static final double PIVOT_MOI = 0.192383865; - public static final double PIVOT_LENGTH_METERS = 0.3; + // Pivot + public static final int NUM_PIVOT_MOTORS = 1; + public static final double PIVOT_GEARING = 67.5; + public static final double MAX_PIVOT_POSITION = Units.degreesToRadians(180.0); + public static final double PIVOT_MOI = 0.192383865; + public static final double PIVOT_LENGTH_METERS = 0.3; - // Spinner - public static final int NUM_SPINNER_MOTORS = 1; - public static final double SPINNER_MOI = 0.01; - public static final double SPINNER_GEARING = 1.0; - public static final double OFF = 0.0; + // Spinner + public static final int NUM_SPINNER_MOTORS = 1; + public static final double SPINNER_MOI = 0.01; + public static final double SPINNER_GEARING = 1.0; + public static final double OFF = 0.0; - // In Rads (Pivot setpoints) - public static final double DOWN = Units.degreesToRadians(180.0); - public static final double IN = Units.degreesToRadians(0.0); + // In Rads (Pivot setpoints) + public static final double DOWN = Units.degreesToRadians(180.0); + public static final double IN = Units.degreesToRadians(0.0); - // In RPS (Spinner Setpoints) - public static final double REVERSE = -10.0; - public static final double ON = 10.0; - } + // In RPS (Spinner Setpoints) + public static final double REVERSE = -10.0; + public static final double ON = 10.0; + } - public static final class Shooter { + public static final class Shooter { - public static final double ERROR_OF_MARGIN = 2.0; + public static final double ERROR_OF_MARGIN = 2.0; - // PID - public static final PIDConstants SIM_PID = new PIDConstants(1, 0, 0); - public static final PIDConstants REAL_PID = new PIDConstants(1, 0, 0); + // PID + public static final PIDConstants SIM_PID = new PIDConstants(1, 0, 0); + public static final PIDConstants REAL_PID = new PIDConstants(1, 0, 0); - // CAN IDs - public static final int LEFT_SHOOTER_ID = 15; - public static final int RIGHT_SHOOTER_ID = 14; + // CAN IDs + public static final int LEFT_SHOOTER_ID = 15; + public static final int RIGHT_SHOOTER_ID = 14; - // Shooter Setpoints (RPS) - public static final double OFF = 0.0; - public static final double FEEDING_AMP = 25.0; - public static final double SHOOTING = 50.0; + // Shooter Setpoints (RPS) + public static final double OFF = 0.0; + public static final double FEEDING_AMP = 25.0; + public static final double SHOOTING = 50.0; - // Sim Configs - public static final int NUM_MOTORS = 2; - public static final double SHOOTER_GEARING = 1.5; - public static final double SHOOTER_MOI = 0.004; + // Sim Configs + public static final int NUM_MOTORS = 2; + public static final double SHOOTER_GEARING = 1.5; + public static final double SHOOTER_MOI = 0.004; - // spinner circumfrence need to check with mech - public static final double CIRCUMFRENCE_OF_SHOOTER_SPINNER = 4; - } + // spinner circumfrence need to check with mech + public static final double CIRCUMFRENCE_OF_SHOOTER_SPINNER = 4; + } - public static final class AmpBar { + public static final class AmpBar { - public static final Translation3d ZEROED_PIVOT_TRANSLATION = new Translation3d( - -0.317, - 0, - 0.525 - ); + public static final Translation3d ZEROED_PIVOT_TRANSLATION = + new Translation3d(-0.317, 0, 0.525); - public static final double ERROR_OF_MARGIN = 0.1; + public static final double ERROR_OF_MARGIN = 0.1; - // PID - public static final PIDConstants SIM_PID = new PIDConstants(3, 0, 1.5); - public static final PIDConstants REAL_PID = new PIDConstants(1, 0, 0); + // PID + public static final PIDConstants SIM_PID = new PIDConstants(3, 0, 1.5); + public static final PIDConstants REAL_PID = new PIDConstants(1, 0, 0); - // Motor CAN IDs - public static final int LEFT_PIVOT_ID = 31; - public static final int RIGHT_PIVOT_ID = 30; - public static final int SPINNER_ID = 38; + // Motor CAN IDs + public static final int LEFT_PIVOT_ID = 31; + public static final int RIGHT_PIVOT_ID = 30; + public static final int SPINNER_ID = 38; - // Sim Configs + // Sim Configs - // Spinner - public static final double SPINNER_GEARING = 0.5; - public static final double SPINNER_MOI = 0.5; - public static final int NUM_SPINNER_MOTORS = 1; + // Spinner + public static final double SPINNER_GEARING = 0.5; + public static final double SPINNER_MOI = 0.5; + public static final int NUM_SPINNER_MOTORS = 1; - // Pivot - public static final int NUM_PIVOT_MOTORS = 2; - public static final double PIVOT_GEARING = 0.05; - public static final double PIVOT_MOI = 0.05; - public static final double PIVOT_LENGTH_METERS = 0.378; - public static final double MIN_PIVOT_POSITION = -Units.degreesToRadians(114.163329); - public static final double MAX_PIVOT_POSITION = Units.degreesToRadians(0); - - // Pivot and Spinner Setpoints + // Pivot + public static final int NUM_PIVOT_MOTORS = 2; + public static final double PIVOT_GEARING = 0.05; + public static final double PIVOT_MOI = 0.05; + public static final double PIVOT_LENGTH_METERS = 0.378; + public static final double MIN_PIVOT_POSITION = -Units.degreesToRadians(114.163329); + public static final double MAX_PIVOT_POSITION = Units.degreesToRadians(0); - // In RPS (Spinner Setpoints) - public static final double SHOOTING = -0.5; - public static final double FEEDING = -0.1; - public static final double OFF = 0.0; + // Pivot and Spinner Setpoints - // IN Rads (Pivot Setpoints) - public static final double OUT = -Units.degreesToRadians(100.0); - public static final double FEEDING_POSITION = -Units.degreesToRadians(93.0); - public static final double IN = Units.degreesToRadians(0.0); - } + // In RPS (Spinner Setpoints) + public static final double SHOOTING = -0.5; + public static final double FEEDING = -0.1; + public static final double OFF = 0.0; - public static final class Drive { + // IN Rads (Pivot Setpoints) + public static final double OUT = -Units.degreesToRadians(100.0); + public static final double FEEDING_POSITION = -Units.degreesToRadians(93.0); + public static final double IN = Units.degreesToRadians(0.0); + } - public static final double DISCRETIZE_TIME_SECONDS = 0.02; - public static final double CONTROLLER_DEADBAND = 0.1; - public static final int NUM_MODULES = 4; - - /* Rotation and Translation Modifers + public static final class Drive { + + public static final double DISCRETIZE_TIME_SECONDS = 0.02; + public static final double CONTROLLER_DEADBAND = 0.1; + public static final int NUM_MODULES = 4; + + /* Rotation and Translation Modifers rm = rotation multiplier tm = translation multipliers aa = auto align */ - public static final double REGULAR_RM = 1.0; - public static final double REGULAR_TM = 1.0; - public static final double SLOW_RM = 0.5; - public static final double SLOW_TM = 0.2; - public static final double AA_RM = 0.8; - public static final double AA_TM = 0.8; - public static final double FAST_RM = 1.5; - public static final double FAST_TM = 2.0; - - // Auto Config - public static final PIDConstants TRANSLATION_PID = new PIDConstants(5.0, 0.0, 0.0); - public static final PIDConstants ROTATION_PID = new PIDConstants(5.0, 0.0, 0.0); - public static final double MAX_MODULE_SPEED = 6.0; + public static final double REGULAR_RM = 1.0; + public static final double REGULAR_TM = 1.0; + public static final double SLOW_RM = 0.5; + public static final double SLOW_TM = 0.2; + public static final double AA_RM = 0.8; + public static final double AA_TM = 0.8; + public static final double FAST_RM = 1.5; + public static final double FAST_TM = 2.0; + + // Auto Config + public static final PIDConstants TRANSLATION_PID = new PIDConstants(5.0, 0.0, 0.0); + public static final PIDConstants ROTATION_PID = new PIDConstants(5.0, 0.0, 0.0); + public static final double MAX_MODULE_SPEED = 6.0; + + // Configs + public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + public static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); + public static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); + public static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); + public static final double DRIVE_BASE_RADIUS = + Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); + public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; - // Configs - public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); - public static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); - public static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); - public static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); - public static final double DRIVE_BASE_RADIUS = Math.hypot( - TRACK_WIDTH_X / 2.0, - TRACK_WIDTH_Y / 2.0 - ); - public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; - - public static final class Pidgeon2 { + public static final class Pidgeon2 { - public static final int DEVICE_ID = 20; - public static final double UPDATE_FREQUENCY = 100.0; - } + public static final int DEVICE_ID = 20; + public static final double UPDATE_FREQUENCY = 100.0; + } - public static final class Module { - - public static final double ODOMETRY_FREQUENCY = 250.0; - - // There isnt one for real its just all 0 so idk whats good with that - public static final FFConstants REPLAY_FF = new FFConstants(0.1, 0.13); - public static final PIDConstants REPLAY_DRIVE_PID = new PIDConstants(0.05, 0.0, 0.0); - public static final PIDConstants REPLAY_TURN_PID = new PIDConstants(7.0, 0.0, 0.0); - - public static final FFConstants SIM_FF = new FFConstants(0.0, 0.13); - public static final PIDConstants SIM_DRIVE_PID = new PIDConstants(0.1, 0.0, 0.0); - public static final PIDConstants SIM_TURN_PID = new PIDConstants(10.0, 0.0, 0.0); - - public static final int NUM_TURN_MOTORS = 1; - public static final int NUM_DRIVE_MOTORS = 1; - - public static final class Sim { - - public static final double LOOP_PERIOD_SECS = 0.02; - - // Configs - public static final double DRIVE_GEARING = 6.75; - public static final double DRIVE_MOI = 0.025; - - public static final double TURN_GEARING = 150.0 / 7.0; - public static final double TURN_MOI = 0.004; - } - - // TODO: Put constants from those abstractions in here - public static final class SparkMax {} - - public static final class TalonFX {} - - public static final class Hybrid { - - // These are for l2 Mk4i mods, should be L3 plus - public static final double DRIVE_GEAR_RATIO = - (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - public static final double TURN_GEAR_RATIO = 150.0 / 7.0; - - public static final double DRIVE_CURRENT_LIMIT = 40.0; - public static final int TURN_CURRENT_LIMIT = 30; - // Stuff - public static final double TALON_UPDATE_FREQUENCY_HZ = 50.0; - - public static final int SPARK_TIMEOUT_MS = 250; - public static final int SPARK_MEASURMENT_PERIOD_MS = 10; - public static final int SPARK_AVG_DEPTH = 2; - public static final double SPARK_FRAME_PERIOD = 1000.0 / ODOMETRY_FREQUENCY; - - // CAN/Device IDS and offsets - public static final int DRIVE0_ID = 0; - public static final int TURN0_ID = 1; - public static final int CANCODER0_ID = 2; - public static final double OFFSET0 = 0.0; - - public static final int DRIVE1_ID = 0; - public static final int TURN1_ID = 1; - public static final int CANCODER1_ID = 2; - public static final double OFFSET1 = 0.0; - - public static final int DRIVE2_ID = 0; - public static final int TURN2_ID = 1; - public static final int CANCODER2_ID = 2; - public static final double OFFSET2 = 0.0; - - public static final int DRIVE3_ID = 0; - public static final int TURN3_ID = 1; - public static final int CANCODER3_ID = 2; - public static final double OFFSET3 = 0.0; - } - } + public static final class Module { - public static final class OdoThread { + public static final double ODOMETRY_FREQUENCY = 250.0; + + // There isnt one for real its just all 0 so idk whats good with that + public static final FFConstants REPLAY_FF = new FFConstants(0.1, 0.13); + public static final PIDConstants REPLAY_DRIVE_PID = new PIDConstants(0.05, 0.0, 0.0); + public static final PIDConstants REPLAY_TURN_PID = new PIDConstants(7.0, 0.0, 0.0); + + public static final FFConstants SIM_FF = new FFConstants(0.0, 0.13); + public static final PIDConstants SIM_DRIVE_PID = new PIDConstants(0.1, 0.0, 0.0); + public static final PIDConstants SIM_TURN_PID = new PIDConstants(10.0, 0.0, 0.0); + + public static final int NUM_TURN_MOTORS = 1; + public static final int NUM_DRIVE_MOTORS = 1; + + public static final class Sim { + + public static final double LOOP_PERIOD_SECS = 0.02; + + // Configs + public static final double DRIVE_GEARING = 6.75; + public static final double DRIVE_MOI = 0.025; + + public static final double TURN_GEARING = 150.0 / 7.0; + public static final double TURN_MOI = 0.004; + } + + // TODO: Put constants from those abstractions in here + public static final class SparkMax {} + + public static final class TalonFX {} + + public static final class Hybrid { + + // These are for l2 Mk4i mods, should be L3 plus + public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + public static final double TURN_GEAR_RATIO = 150.0 / 7.0; + + public static final double DRIVE_CURRENT_LIMIT = 40.0; + public static final int TURN_CURRENT_LIMIT = 30; + // Stuff + public static final double TALON_UPDATE_FREQUENCY_HZ = 50.0; + + public static final int SPARK_TIMEOUT_MS = 250; + public static final int SPARK_MEASURMENT_PERIOD_MS = 10; + public static final int SPARK_AVG_DEPTH = 2; + public static final double SPARK_FRAME_PERIOD = 1000.0 / ODOMETRY_FREQUENCY; + + // CAN/Device IDS and offsets + public static final int DRIVE0_ID = 0; + public static final int TURN0_ID = 1; + public static final int CANCODER0_ID = 2; + public static final double OFFSET0 = 0.0; + + public static final int DRIVE1_ID = 0; + public static final int TURN1_ID = 1; + public static final int CANCODER1_ID = 2; + public static final double OFFSET1 = 0.0; + + public static final int DRIVE2_ID = 0; + public static final int TURN2_ID = 1; + public static final int CANCODER2_ID = 2; + public static final double OFFSET2 = 0.0; + + public static final int DRIVE3_ID = 0; + public static final int TURN3_ID = 1; + public static final int CANCODER3_ID = 2; + public static final double OFFSET3 = 0.0; + } + } + + public static final class OdoThread { - public static final class Phoenix { + public static final class Phoenix { - public static final int QUE_CAPACITY = 20; - public static final double SLEEP_TIME = 1000.0; - } + public static final int QUE_CAPACITY = 20; + public static final double SLEEP_TIME = 1000.0; + } - public static final class SparkMax { + public static final class SparkMax { - public static final int QUE_CAPACITY = 20; - public static final double PERIOD = 1.0; - } - } - } + public static final int QUE_CAPACITY = 20; + public static final double PERIOD = 1.0; + } + } + } - public static final class NoteSim { + public static final class NoteSim { - public static final double AIR_DENSITY = 1.225; - public static final double DRAG_COEFFICIENT = 0.45; - public static final double CROSSECTION_AREA = 0.11; - public static final double MASS = 0.235; + public static final double AIR_DENSITY = 1.225; + public static final double DRAG_COEFFICIENT = 0.45; + public static final double CROSSECTION_AREA = 0.11; + public static final double MASS = 0.235; - public static final Pose3d SHOOTER_POSE3D = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); - public static final Translation2d FIELD_SIZE = new Translation2d(16.54, 8.21); // stolen from 3015 constants + public static final Pose3d SHOOTER_POSE3D = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); + public static final Translation2d FIELD_SIZE = + new Translation2d(16.54, 8.21); // stolen from 3015 constants - public static final double dt = 0.2; // change in time for note sim - public static final Translation3d GRAVITY_TRANSLATION3D = new Translation3d(0, 0, 9.8); - public static final double OUT_OF_FIELD_MARGIN = .025; - } + public static final double dt = 0.2; // change in time for note sim + public static final Translation3d GRAVITY_TRANSLATION3D = new Translation3d(0, 0, 9.8); + public static final double OUT_OF_FIELD_MARGIN = .025; + } } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 40d8330..f638f55 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -22,14 +22,14 @@ */ public final class Main { - private Main() {} + private Main() {} - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 34958f8..5592fbe 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -37,143 +37,143 @@ */ public class Robot extends LoggedRobot { - public Manager managerSubsystem; - private SendableChooser autoChooser; - - private AutoCommands autoCommands = new AutoCommands(this); - - private Command autonomousCommand; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - @Override - public void robotInit() { - managerSubsystem = new Manager(); - - NamedCommands.registerCommand("Intaking", autoCommands.intaking()); - NamedCommands.registerCommand("Shooting", autoCommands.shooting()); - NamedCommands.registerCommand("Return To Idle", autoCommands.returnToIdle()); - NamedCommands.registerCommand("Speeding Up", autoCommands.startSpinningUp()); - NamedCommands.registerCommand("Spin and Intake", autoCommands.spinAndIntake()); - - // 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; - } - - // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. - // Logger.disableDeterministicTimestamps() - - // Start AdvantageKit logger - Logger.start(); - - autoChooser = AutoBuilder.buildAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); - } - - /** This function is called periodically during all modes. */ - @Override - public void robotPeriodic() { - managerSubsystem.periodic(); - - // Logs note sim logging and updating sims - NoteSimulator.update(); - NoteSimulator.logNoteInfo(); - - CommandScheduler.getInstance().run(); - } - - /** This function is called once when the robot is disabled. */ - @Override - public void disabledInit() { - managerSubsystem.stop(); - } - - /** This function is called periodically when disabled. */ - @Override - public void disabledPeriodic() {} - - /** This function is called once the robot enters Auto. */ - @Override - public void autonomousInit() { - autonomousCommand = getAutonomousCommand(); - - // schedule the autonomous command (example) - if (autonomousCommand != null) { - autonomousCommand.schedule(); - } - } - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() {} - - /** This function is called once when teleop is enabled. */ - @Override - public void teleopInit() { - if (autonomousCommand != null) { - autonomousCommand.cancel(); - } - } - - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() {} - - /** This function is called once when test mode is enabled. */ - @Override - public void testInit() {} - - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() {} - - /** This function is called once when the robot is first started up. */ - @Override - public void simulationInit() {} - - /** This function is called periodically whilst in simulation. */ - @Override - public void simulationPeriodic() {} - - public Command getAutonomousCommand() { - return autoChooser.getSelected(); - } + public Manager managerSubsystem; + private SendableChooser autoChooser; + + private AutoCommands autoCommands = new AutoCommands(this); + + private Command autonomousCommand; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + managerSubsystem = new Manager(); + + NamedCommands.registerCommand("Intaking", autoCommands.intaking()); + NamedCommands.registerCommand("Shooting", autoCommands.shooting()); + NamedCommands.registerCommand("Return To Idle", autoCommands.returnToIdle()); + NamedCommands.registerCommand("Speeding Up", autoCommands.startSpinningUp()); + NamedCommands.registerCommand("Spin and Intake", autoCommands.spinAndIntake()); + + // 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; + } + + // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. + // Logger.disableDeterministicTimestamps() + + // Start AdvantageKit logger + Logger.start(); + + autoChooser = AutoBuilder.buildAutoChooser("Example Auto"); + SmartDashboard.putData("Auto Chooser", autoChooser); + } + + /** This function is called periodically during all modes. */ + @Override + public void robotPeriodic() { + managerSubsystem.periodic(); + + // Logs note sim logging and updating sims + NoteSimulator.update(); + NoteSimulator.logNoteInfo(); + + CommandScheduler.getInstance().run(); + } + + /** This function is called once when the robot is disabled. */ + @Override + public void disabledInit() { + managerSubsystem.stop(); + } + + /** This function is called periodically when disabled. */ + @Override + public void disabledPeriodic() {} + + /** This function is called once the robot enters Auto. */ + @Override + public void autonomousInit() { + autonomousCommand = getAutonomousCommand(); + + // schedule the autonomous command (example) + System.out.println(":( womp womp"); + if (autonomousCommand != null) { + autonomousCommand.schedule(); + System.out.println("hi"); + } + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() {} + + /** This function is called once when teleop is enabled. */ + @Override + public void teleopInit() { + if (autonomousCommand != null) { + autonomousCommand.cancel(); + } + } + + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() {} + + /** This function is called once when test mode is enabled. */ + @Override + public void testInit() {} + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} + + /** This function is called once when the robot is first started up. */ + @Override + public void simulationInit() {} + + /** This function is called periodically whilst in simulation. */ + @Override + public void simulationPeriodic() {} + + public Command getAutonomousCommand() { + return autoChooser.getSelected(); + } } diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index a21dc3b..cee064c 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -8,41 +8,35 @@ public class AutoCommands { - Robot robot = null; - - public AutoCommands(Robot robot) { - this.robot = robot; - } - - public Command intaking() { - return Commands.sequence( - new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.INTAKING)) - ); - } - - public Command shooting() { - return Commands.sequence( - new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SHOOTING)) - ); - } - - public Command returnToIdle() { - return Commands.sequence( - new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.IDLE)) - ); - } - - public Command startSpinningUp() { - return Commands.sequence( - new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SPINNING_UP)) - ); - } - - public Command spinAndIntake() { - return Commands.sequence( - new InstantCommand(() -> - robot.managerSubsystem.setState(ManagerStates.SPINNING_AND_INTAKING) - ) - ); - } + Robot robot = null; + + public AutoCommands(Robot robot) { + this.robot = robot; + } + + public Command intaking() { + return Commands.sequence( + new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.INTAKING))); + } + + public Command shooting() { + return Commands.sequence( + new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SHOOTING))); + } + + public Command returnToIdle() { + return Commands.sequence( + new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.IDLE))); + } + + public Command startSpinningUp() { + return Commands.sequence( + new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SPINNING_UP))); + } + + public Command spinAndIntake() { + return Commands.sequence( + new InstantCommand( + () -> robot.managerSubsystem.setState(ManagerStates.SPINNING_AND_INTAKING))); + } } diff --git a/src/main/java/frc/robot/subsystems/Subsystem.java b/src/main/java/frc/robot/subsystems/Subsystem.java index 3b2cb1d..95470e0 100644 --- a/src/main/java/frc/robot/subsystems/Subsystem.java +++ b/src/main/java/frc/robot/subsystems/Subsystem.java @@ -12,81 +12,79 @@ public abstract class Subsystem { - private Map>> triggerMap = new HashMap< - StateType, - ArrayList> - >(); - - private StateType state = null; - private Timer stateTimer = new Timer(); - private String subsystemName; - - public Subsystem(String subsystemName, StateType defaultState) { - if (defaultState == null) { - throw new RuntimeException("Default state cannot be null!"); - } - this.subsystemName = subsystemName; - this.state = defaultState; - - stateTimer.start(); - } - - // State operation - public void periodic() { - Logger.recordOutput(subsystemName + "/state", state.getStateString()); - if (!DriverStation.isEnabled()) return; - - runState(); - - checkTriggers(); - } - - protected abstract void runState(); - - // SmartDashboard utils - protected void putSmartDashboard(String key, String value) { - SmartDashboard.putString("[" + subsystemName + "] " + key, value); - } - - protected void putSmartDashboard(String key, double value) { - SmartDashboard.putNumber("[" + subsystemName + "] " + key, value); - } - - // Triggers for state transitions - protected void addTrigger(StateType startType, StateType endType, BooleanSupplier check) { - if (triggerMap.get(startType) == null) { - triggerMap.put(startType, new ArrayList>()); - } - triggerMap.get(startType).add(new Trigger(check, endType)); - } - - private void checkTriggers() { - List> triggers = triggerMap.get(state); - if (triggers == null) return; - for (var trigger : triggers) { - if (trigger.isTriggered()) { - setState(trigger.getResultState()); - return; - } - } - } - - // Other utilities - public StateType getState() { - return state; - } - - public void setState(StateType state) { - if (this.state != state) stateTimer.reset(); - this.state = state; - } - - /** - * Gets amount of time the state machine has been in the current state. - * - * @return time in seconds. - */ - protected double getStateTime() { - return stateTimer.get(); - } + private Map>> triggerMap = + new HashMap>>(); + + private StateType state = null; + private Timer stateTimer = new Timer(); + private String subsystemName; + + public Subsystem(String subsystemName, StateType defaultState) { + if (defaultState == null) { + throw new RuntimeException("Default state cannot be null!"); + } + this.subsystemName = subsystemName; + this.state = defaultState; + + stateTimer.start(); + } + + // State operation + public void periodic() { + Logger.recordOutput(subsystemName + "/state", state.getStateString()); + if (!DriverStation.isEnabled()) return; + + runState(); + + checkTriggers(); + } + + protected abstract void runState(); + + // SmartDashboard utils + protected void putSmartDashboard(String key, String value) { + SmartDashboard.putString("[" + subsystemName + "] " + key, value); + } + + protected void putSmartDashboard(String key, double value) { + SmartDashboard.putNumber("[" + subsystemName + "] " + key, value); + } + + // Triggers for state transitions + protected void addTrigger(StateType startType, StateType endType, BooleanSupplier check) { + if (triggerMap.get(startType) == null) { + triggerMap.put(startType, new ArrayList>()); + } + triggerMap.get(startType).add(new Trigger(check, endType)); + } + + private void checkTriggers() { + List> triggers = triggerMap.get(state); + if (triggers == null) return; + for (var trigger : triggers) { + if (trigger.isTriggered()) { + setState(trigger.getResultState()); + return; + } + } + } + + // Other utilities + public StateType getState() { + return state; + } + + public void setState(StateType state) { + if (this.state != state) stateTimer.reset(); + this.state = state; + } + + /** + * Gets amount of time the state machine has been in the current state. + * + * @return time in seconds. + */ + protected double getStateTime() { + return stateTimer.get(); + } } diff --git a/src/main/java/frc/robot/subsystems/SubsystemStates.java b/src/main/java/frc/robot/subsystems/SubsystemStates.java index 7664a70..d2ffcaf 100644 --- a/src/main/java/frc/robot/subsystems/SubsystemStates.java +++ b/src/main/java/frc/robot/subsystems/SubsystemStates.java @@ -1,5 +1,5 @@ package frc.robot.subsystems; public interface SubsystemStates { - String getStateString(); + String getStateString(); } diff --git a/src/main/java/frc/robot/subsystems/Trigger.java b/src/main/java/frc/robot/subsystems/Trigger.java index cd358ff..d94c75e 100644 --- a/src/main/java/frc/robot/subsystems/Trigger.java +++ b/src/main/java/frc/robot/subsystems/Trigger.java @@ -4,19 +4,19 @@ public class Trigger { - BooleanSupplier supplier; - StateType state; + BooleanSupplier supplier; + StateType state; - public Trigger(BooleanSupplier supplier, StateType state) { - this.supplier = supplier; - this.state = state; - } + public Trigger(BooleanSupplier supplier, StateType state) { + this.supplier = supplier; + this.state = state; + } - public boolean isTriggered() { - return supplier.getAsBoolean(); - } + public boolean isTriggered() { + return supplier.getAsBoolean(); + } - public StateType getResultState() { - return state; - } + public StateType getResultState() { + return state; + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBar.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBar.java index ba55f54..05328ce 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBar.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBar.java @@ -8,59 +8,53 @@ public class AmpBar extends Subsystem { - AmpBarIO io; - AmpBarIOInputsAutoLogged inputs; - - public AmpBar(AmpBarIO io) { - super("Amp Bar", AmpBarStates.OFF); - this.io = io; - inputs = new AmpBarIOInputsAutoLogged(); - - switch (Constants.currentMode) { - case REAL: - io.configurePID( - Constants.AmpBar.REAL_PID.kP, - Constants.AmpBar.REAL_PID.kI, - Constants.AmpBar.REAL_PID.kD - ); - break; - case SIM: - io.configurePID( - Constants.AmpBar.SIM_PID.kP, - Constants.AmpBar.SIM_PID.kP, - Constants.AmpBar.SIM_PID.kP - ); - break; - case REPLAY: - io.configurePID(0, 0, 0); - break; - default: - break; - } - } - - @Override - protected void runState() { - io.setSpinnerSpeedpoint(getState().getMotorSpeedpoint()); - io.setPivotPosition(getState().getPivotPositionSetpoint()); - } - - public void stop() { - io.stop(); - } - - @Override - public void periodic() { - super.periodic(); - - Logger.recordOutput( - "Amp Bar/Amp Bar Pose3d", - new Pose3d( - Constants.AmpBar.ZEROED_PIVOT_TRANSLATION, - new Rotation3d(0, io.getPivotPosition(), 0) - ) - ); - Logger.processInputs("Amp Bar", inputs); - io.updateInput(inputs); - } + AmpBarIO io; + AmpBarIOInputsAutoLogged inputs; + + public AmpBar(AmpBarIO io) { + super("Amp Bar", AmpBarStates.OFF); + this.io = io; + inputs = new AmpBarIOInputsAutoLogged(); + + switch (Constants.currentMode) { + case REAL: + io.configurePID( + Constants.AmpBar.REAL_PID.kP, + Constants.AmpBar.REAL_PID.kI, + Constants.AmpBar.REAL_PID.kD); + break; + case SIM: + io.configurePID( + Constants.AmpBar.SIM_PID.kP, Constants.AmpBar.SIM_PID.kP, Constants.AmpBar.SIM_PID.kP); + break; + case REPLAY: + io.configurePID(0, 0, 0); + break; + default: + break; + } + } + + @Override + protected void runState() { + io.setSpinnerSpeedpoint(getState().getMotorSpeedpoint()); + io.setPivotPosition(getState().getPivotPositionSetpoint()); + } + + public void stop() { + io.stop(); + } + + @Override + public void periodic() { + super.periodic(); + + Logger.recordOutput( + "Amp Bar/Amp Bar Pose3d", + new Pose3d( + Constants.AmpBar.ZEROED_PIVOT_TRANSLATION, + new Rotation3d(0, io.getPivotPosition(), 0))); + Logger.processInputs("Amp Bar", inputs); + io.updateInput(inputs); + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIO.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIO.java index 5560918..93f0012 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIO.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIO.java @@ -3,40 +3,40 @@ import org.littletonrobotics.junction.AutoLog; public interface AmpBarIO { - @AutoLog - public static class AmpBarIOInputs { + @AutoLog + public static class AmpBarIOInputs { - public String ampBarState; + public String ampBarState; - // Pivot - public double pivotPosition; - public double pivotSetpoint; - public double pivotAppliedVoltage; - // Spinners - public double spinnerSpeed; - public double spinnerSetpoint; - public double spinnerAppliedVoltage; - } + // Pivot + public double pivotPosition; + public double pivotSetpoint; + public double pivotAppliedVoltage; + // Spinners + public double spinnerSpeed; + public double spinnerSetpoint; + public double spinnerAppliedVoltage; + } - public default void updateInput(AmpBarIOInputs inputs) {} + public default void updateInput(AmpBarIOInputs inputs) {} - public default void setPivotPosition(double position) {} + public default void setPivotPosition(double position) {} - public default void setSpinnerSpeedpoint(double speed) {} + public default void setSpinnerSpeedpoint(double speed) {} - public default void stop() {} + public default void stop() {} - public default double getPivotPosition() { - return 0.0; - } + public default double getPivotPosition() { + return 0.0; + } - public default double getSpinnerSpeed() { - return 0.0; - } + public default double getSpinnerSpeed() { + return 0.0; + } - public default void configurePID(double kP, double kI, double kD) {} + public default void configurePID(double kP, double kI, double kD) {} - public default boolean atSetPoint() { - return false; - } + public default boolean atSetPoint() { + return false; + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOReal.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOReal.java index 11f76fe..876cc81 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOReal.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOReal.java @@ -10,101 +10,99 @@ public class AmpBarIOReal implements AmpBarIO { - private final CANSparkMax leftMotor; - private final CANSparkMax rightMotor; - private final TalonFX spinnerMotor; - - private RelativeEncoder pivotEncoder; - private String stateString; - private PIDController controller; - - private double pivotMotorAppliedVoltage; - private double pivotPositionSetpoint; - - private double spinnerSpeedpoint; - private double spinnerAppliedVoltage; - - public AmpBarIOReal() { - leftMotor = new CANSparkMax(Constants.AmpBar.LEFT_PIVOT_ID, MotorType.kBrushless); - rightMotor = new CANSparkMax(Constants.AmpBar.RIGHT_PIVOT_ID, MotorType.kBrushless); - spinnerMotor = new TalonFX(Constants.AmpBar.SPINNER_ID); - - leftMotor.setInverted(false); - rightMotor.follow(leftMotor, true); - leftMotor.setIdleMode(IdleMode.kCoast); - rightMotor.setIdleMode(IdleMode.kCoast); - - pivotEncoder = leftMotor.getEncoder(); - - pivotEncoder.setPosition(0); - pivotEncoder.setPositionConversionFactor(Constants.RADIAN_CF); - pivotEncoder.setVelocityConversionFactor(Constants.RADIAN_CF); - - controller = new PIDController(0, 0, 0); - - stateString = ""; - pivotMotorAppliedVoltage = 0; - pivotPositionSetpoint = 0; - spinnerSpeedpoint = 0; - spinnerAppliedVoltage = 0; - } - - @Override - public void updateInput(AmpBarIOInputs inputs) { - inputs.ampBarState = stateString; - - inputs.pivotPosition = pivotEncoder.getPosition(); - inputs.pivotSetpoint = pivotPositionSetpoint; - inputs.pivotAppliedVoltage = pivotMotorAppliedVoltage; - - inputs.spinnerSpeed = getSpinnerSpeed(); - inputs.spinnerSetpoint = spinnerSpeedpoint; - inputs.spinnerAppliedVoltage = spinnerAppliedVoltage; - } - - @Override - public void setPivotPosition(double position) { - pivotPositionSetpoint = position; - pivotMotorAppliedVoltage = controller.calculate( - pivotEncoder.getPosition(), - pivotPositionSetpoint - ); - leftMotor.setVoltage(pivotMotorAppliedVoltage); - } - - public void stop() { - spinnerAppliedVoltage = 0; - pivotMotorAppliedVoltage = 0; - leftMotor.stopMotor(); - rightMotor.stopMotor(); - spinnerMotor.stopMotor(); - } - - @Override - public void setSpinnerSpeedpoint(double speed) { - spinnerSpeedpoint = speed; - spinnerAppliedVoltage = speed; - spinnerMotor.setVoltage(speed); - } - - @Override - public double getPivotPosition() { - return pivotEncoder.getPosition(); - } - - @Override - public double getSpinnerSpeed() { - return spinnerMotor.getVelocity().getValueAsDouble(); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - controller.setPID(kP, kI, kD); - } - - @Override - public boolean atSetPoint() { - double motorPosition = getPivotPosition(); - return Math.abs(motorPosition - pivotPositionSetpoint) <= Constants.AmpBar.ERROR_OF_MARGIN; - } + private final CANSparkMax leftMotor; + private final CANSparkMax rightMotor; + private final TalonFX spinnerMotor; + + private RelativeEncoder pivotEncoder; + private String stateString; + private PIDController controller; + + private double pivotMotorAppliedVoltage; + private double pivotPositionSetpoint; + + private double spinnerSpeedpoint; + private double spinnerAppliedVoltage; + + public AmpBarIOReal() { + leftMotor = new CANSparkMax(Constants.AmpBar.LEFT_PIVOT_ID, MotorType.kBrushless); + rightMotor = new CANSparkMax(Constants.AmpBar.RIGHT_PIVOT_ID, MotorType.kBrushless); + spinnerMotor = new TalonFX(Constants.AmpBar.SPINNER_ID); + + leftMotor.setInverted(false); + rightMotor.follow(leftMotor, true); + leftMotor.setIdleMode(IdleMode.kCoast); + rightMotor.setIdleMode(IdleMode.kCoast); + + pivotEncoder = leftMotor.getEncoder(); + + pivotEncoder.setPosition(0); + pivotEncoder.setPositionConversionFactor(Constants.RADIAN_CF); + pivotEncoder.setVelocityConversionFactor(Constants.RADIAN_CF); + + controller = new PIDController(0, 0, 0); + + stateString = ""; + pivotMotorAppliedVoltage = 0; + pivotPositionSetpoint = 0; + spinnerSpeedpoint = 0; + spinnerAppliedVoltage = 0; + } + + @Override + public void updateInput(AmpBarIOInputs inputs) { + inputs.ampBarState = stateString; + + inputs.pivotPosition = pivotEncoder.getPosition(); + inputs.pivotSetpoint = pivotPositionSetpoint; + inputs.pivotAppliedVoltage = pivotMotorAppliedVoltage; + + inputs.spinnerSpeed = getSpinnerSpeed(); + inputs.spinnerSetpoint = spinnerSpeedpoint; + inputs.spinnerAppliedVoltage = spinnerAppliedVoltage; + } + + @Override + public void setPivotPosition(double position) { + pivotPositionSetpoint = position; + pivotMotorAppliedVoltage = + controller.calculate(pivotEncoder.getPosition(), pivotPositionSetpoint); + leftMotor.setVoltage(pivotMotorAppliedVoltage); + } + + public void stop() { + spinnerAppliedVoltage = 0; + pivotMotorAppliedVoltage = 0; + leftMotor.stopMotor(); + rightMotor.stopMotor(); + spinnerMotor.stopMotor(); + } + + @Override + public void setSpinnerSpeedpoint(double speed) { + spinnerSpeedpoint = speed; + spinnerAppliedVoltage = speed; + spinnerMotor.setVoltage(speed); + } + + @Override + public double getPivotPosition() { + return pivotEncoder.getPosition(); + } + + @Override + public double getSpinnerSpeed() { + return spinnerMotor.getVelocity().getValueAsDouble(); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + controller.setPID(kP, kI, kD); + } + + @Override + public boolean atSetPoint() { + double motorPosition = getPivotPosition(); + return Math.abs(motorPosition - pivotPositionSetpoint) <= Constants.AmpBar.ERROR_OF_MARGIN; + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOSim.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOSim.java index c70e781..a191383 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOSim.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOSim.java @@ -8,101 +8,101 @@ public class AmpBarIOSim implements AmpBarIO { - SingleJointedArmSim pivotSim; - DCMotorSim spinnerSim; - PIDController controller; - - String stateString; - - double pivotAppliedVoltage; - double pivotSetpoint; - - double spinnerAppliedVoltage; - double spinnerSpeedpoint; - - public AmpBarIOSim() { - // the sim values are random - pivotSim = new SingleJointedArmSim( - DCMotor.getNEO(Constants.AmpBar.NUM_PIVOT_MOTORS), - Constants.AmpBar.PIVOT_GEARING, - Constants.AmpBar.PIVOT_MOI, - Constants.AmpBar.PIVOT_LENGTH_METERS, - Constants.AmpBar.MIN_PIVOT_POSITION, - Constants.AmpBar.MAX_PIVOT_POSITION, - false, - 0 - ); - spinnerSim = new DCMotorSim( - DCMotor.getKrakenX60(Constants.AmpBar.NUM_SPINNER_MOTORS), - Constants.AmpBar.SPINNER_GEARING, - Constants.AmpBar.SPINNER_MOI - ); - - controller = new PIDController(0, 0, 0); - stateString = ""; - - pivotAppliedVoltage = 0; - pivotSetpoint = 0; - spinnerAppliedVoltage = 0; - spinnerSpeedpoint = 0; - } - - @Override - public void updateInput(AmpBarIOInputs inputs) { - inputs.ampBarState = stateString; - - inputs.pivotPosition = getPivotPosition(); - inputs.pivotSetpoint = pivotSetpoint; - inputs.pivotAppliedVoltage = pivotAppliedVoltage; - - inputs.spinnerSpeed = getSpinnerSpeed(); - inputs.spinnerSetpoint = spinnerSpeedpoint; - inputs.spinnerAppliedVoltage = spinnerAppliedVoltage; - - pivotSim.update(Constants.SIM_UPDATE_TIME); - spinnerSim.update(Constants.SIM_UPDATE_TIME); - } - - @Override - public void setPivotPosition(double position) { - pivotSetpoint = position; - pivotAppliedVoltage = controller.calculate(pivotSim.getAngleRads(), pivotSetpoint); - pivotSim.setInputVoltage(pivotAppliedVoltage); - } - - @Override - public void setSpinnerSpeedpoint(double speed) { - spinnerSpeedpoint = speed; - spinnerAppliedVoltage = speed; - spinnerSim.setInputVoltage(speed); - } - - @Override - public void stop() { - pivotAppliedVoltage = 0; - spinnerAppliedVoltage = 0; - spinnerSim.setInputVoltage(0); - pivotSim.setInputVoltage(0); - } - - @Override - public double getPivotPosition() { - return pivotSim.getAngleRads(); - } - - @Override - public double getSpinnerSpeed() { - return spinnerSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - } - - @Override - public void configurePID(double kP, double kI, double kD) { - controller.setPID(kP, kI, kD); - } - - @Override - public boolean atSetPoint() { - double motorPosition = getPivotPosition(); - return Math.abs(motorPosition - pivotSetpoint) <= Constants.AmpBar.ERROR_OF_MARGIN; - } + SingleJointedArmSim pivotSim; + DCMotorSim spinnerSim; + PIDController controller; + + String stateString; + + double pivotAppliedVoltage; + double pivotSetpoint; + + double spinnerAppliedVoltage; + double spinnerSpeedpoint; + + public AmpBarIOSim() { + // the sim values are random + pivotSim = + new SingleJointedArmSim( + DCMotor.getNEO(Constants.AmpBar.NUM_PIVOT_MOTORS), + Constants.AmpBar.PIVOT_GEARING, + Constants.AmpBar.PIVOT_MOI, + Constants.AmpBar.PIVOT_LENGTH_METERS, + Constants.AmpBar.MIN_PIVOT_POSITION, + Constants.AmpBar.MAX_PIVOT_POSITION, + false, + 0); + spinnerSim = + new DCMotorSim( + DCMotor.getKrakenX60(Constants.AmpBar.NUM_SPINNER_MOTORS), + Constants.AmpBar.SPINNER_GEARING, + Constants.AmpBar.SPINNER_MOI); + + controller = new PIDController(0, 0, 0); + stateString = ""; + + pivotAppliedVoltage = 0; + pivotSetpoint = 0; + spinnerAppliedVoltage = 0; + spinnerSpeedpoint = 0; + } + + @Override + public void updateInput(AmpBarIOInputs inputs) { + inputs.ampBarState = stateString; + + inputs.pivotPosition = getPivotPosition(); + inputs.pivotSetpoint = pivotSetpoint; + inputs.pivotAppliedVoltage = pivotAppliedVoltage; + + inputs.spinnerSpeed = getSpinnerSpeed(); + inputs.spinnerSetpoint = spinnerSpeedpoint; + inputs.spinnerAppliedVoltage = spinnerAppliedVoltage; + + pivotSim.update(Constants.SIM_UPDATE_TIME); + spinnerSim.update(Constants.SIM_UPDATE_TIME); + } + + @Override + public void setPivotPosition(double position) { + pivotSetpoint = position; + pivotAppliedVoltage = controller.calculate(pivotSim.getAngleRads(), pivotSetpoint); + pivotSim.setInputVoltage(pivotAppliedVoltage); + } + + @Override + public void setSpinnerSpeedpoint(double speed) { + spinnerSpeedpoint = speed; + spinnerAppliedVoltage = speed; + spinnerSim.setInputVoltage(speed); + } + + @Override + public void stop() { + pivotAppliedVoltage = 0; + spinnerAppliedVoltage = 0; + spinnerSim.setInputVoltage(0); + pivotSim.setInputVoltage(0); + } + + @Override + public double getPivotPosition() { + return pivotSim.getAngleRads(); + } + + @Override + public double getSpinnerSpeed() { + return spinnerSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + } + + @Override + public void configurePID(double kP, double kI, double kD) { + controller.setPID(kP, kI, kD); + } + + @Override + public boolean atSetPoint() { + double motorPosition = getPivotPosition(); + return Math.abs(motorPosition - pivotSetpoint) <= Constants.AmpBar.ERROR_OF_MARGIN; + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBarStates.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBarStates.java index f468c30..4dadaef 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBarStates.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBarStates.java @@ -4,31 +4,31 @@ import frc.robot.subsystems.SubsystemStates; public enum AmpBarStates implements SubsystemStates { - OFF(Constants.AmpBar.IN, Constants.AmpBar.OFF, "Amp Bar Off"), - SHOOTING(Constants.AmpBar.OUT, Constants.AmpBar.SHOOTING, "Shooting Amp"), - FEEDING(Constants.AmpBar.FEEDING_POSITION, Constants.AmpBar.FEEDING, "Getting Fed"), - HOLDING_NOTE(Constants.AmpBar.OUT, Constants.AmpBar.OFF, "Holding a Note"); + OFF(Constants.AmpBar.IN, Constants.AmpBar.OFF, "Amp Bar Off"), + SHOOTING(Constants.AmpBar.OUT, Constants.AmpBar.SHOOTING, "Shooting Amp"), + FEEDING(Constants.AmpBar.FEEDING_POSITION, Constants.AmpBar.FEEDING, "Getting Fed"), + HOLDING_NOTE(Constants.AmpBar.OUT, Constants.AmpBar.OFF, "Holding a Note"); - private double pivotPositionSetpoint; - private double spinnerMotorSpeedpoint; - private String stateString; + private double pivotPositionSetpoint; + private double spinnerMotorSpeedpoint; + private String stateString; - AmpBarStates(double pivotMotorSetpoint, double spinnerMotorSpeedpoint, String stateString) { - this.pivotPositionSetpoint = pivotMotorSetpoint; - this.spinnerMotorSpeedpoint = spinnerMotorSpeedpoint; - this.stateString = stateString; - } + AmpBarStates(double pivotMotorSetpoint, double spinnerMotorSpeedpoint, String stateString) { + this.pivotPositionSetpoint = pivotMotorSetpoint; + this.spinnerMotorSpeedpoint = spinnerMotorSpeedpoint; + this.stateString = stateString; + } - public double getPivotPositionSetpoint() { - return pivotPositionSetpoint; - } + public double getPivotPositionSetpoint() { + return pivotPositionSetpoint; + } - public double getMotorSpeedpoint() { - return spinnerMotorSpeedpoint; - } + public double getMotorSpeedpoint() { + return spinnerMotorSpeedpoint; + } - @Override - public String getStateString() { - return stateString; - } + @Override + public String getStateString() { + return stateString; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 8fc9490..18a3f43 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -13,9 +13,6 @@ package frc.robot.subsystems.drive; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -29,370 +26,289 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.subsystems.Subsystem; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class Drive extends SubsystemBase { - - static final Lock odometryLock = new ReentrantLock(); - private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); - private final Module[] modules = new Module[Constants.Drive.NUM_MODULES]; // FL, FR, BL, BR - - private DriveStates state; - private String subsystemName; - - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); - private Rotation2d rawGyroRotation = new Rotation2d(); - private SwerveModulePosition[] lastModulePositions = // For delta tracking - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - }; - private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator( - kinematics, - rawGyroRotation, - lastModulePositions, - new Pose2d() - ); - - public Drive( - GyroIO gyroIO, - ModuleIO flModuleIO, - ModuleIO frModuleIO, - ModuleIO blModuleIO, - ModuleIO brModuleIO - ) { - subsystemName = "Drive"; - state = DriveStates.REGULAR_DRIVE; - - this.gyroIO = gyroIO; - modules[0] = new Module(flModuleIO, 0); - modules[1] = new Module(frModuleIO, 1); - modules[2] = new Module(blModuleIO, 2); - modules[3] = new Module(brModuleIO, 3); - - // Start threads (no-op for each if no signals have been created) - PhoenixOdometryThread.getInstance().start(); - SparkMaxOdometryThread.getInstance().start(); - - pathPlannerInit(); - } - - public void runState() { - drive( - this, - () -> Constants.controller.getLeftY(), - () -> Constants.controller.getLeftX(), - () -> -Constants.controller.getRightX(), - getState().getRotationModifier(), - getState().getTranslationModifier() - ); - } - - public DriveStates getState() { - return state; - } - - public void setState(DriveStates state) { - this.state = state; - } - - public void drive( - Drive drive, - DoubleSupplier xSupplier, - DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier, - double rotationMultiplier, - double translationMultiplier - ) { - // Apply deadband - double linearMagnitude = MathUtil.applyDeadband( - Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), - Constants.Drive.CONTROLLER_DEADBAND - ); - Rotation2d linearDirection = new Rotation2d( - xSupplier.getAsDouble(), - ySupplier.getAsDouble() - ); - double omega = MathUtil.applyDeadband( - omegaSupplier.getAsDouble(), - Constants.Drive.CONTROLLER_DEADBAND - ); - - // Square values - linearMagnitude = linearMagnitude * linearMagnitude; - omega = Math.copySign(omega * omega, omega); - - // Calcaulate new linear velocity - Translation2d linearVelocity = new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); - - // Convert to field relative speeds & send command - boolean isFlipped = - DriverStation.getAlliance().isPresent() && - DriverStation.getAlliance().get() == Alliance.Red; - drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * - drive.getMaxLinearSpeedMetersPerSec() * - translationMultiplier, - linearVelocity.getY() * - drive.getMaxLinearSpeedMetersPerSec() * - translationMultiplier, - omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier, - isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation() - ) - ); - } - - @Override - public void periodic() { - super.periodic(); - odometryLock.lock(); // Prevents odometry updates while reading data - gyroIO.updateInputs(gyroInputs); - for (var module : modules) { - module.updateInputs(); - } - odometryLock.unlock(); - Logger.processInputs("Drive/Gyro", gyroInputs); - for (var module : modules) { - module.periodic(); - } - - // Stop moving when disabled - if (DriverStation.isDisabled()) { - for (var module : modules) { - module.stop(); - } - } - // Log empty setpoint states when disabled - if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); - } - - // Update odometry - double[] sampleTimestamps = modules[0].getOdometryTimestamps(); // All signals are sampled together - int sampleCount = sampleTimestamps.length; - for (int i = 0; i < sampleCount; i++) { - // Read wheel positions and deltas from each module - SwerveModulePosition[] modulePositions = - new SwerveModulePosition[Constants.Drive.NUM_MODULES]; - SwerveModulePosition[] moduleDeltas = - new SwerveModulePosition[Constants.Drive.NUM_MODULES]; - for (int moduleIndex = 0; moduleIndex < Constants.Drive.NUM_MODULES; moduleIndex++) { - modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; - moduleDeltas[moduleIndex] = new SwerveModulePosition( - modulePositions[moduleIndex].distanceMeters - - lastModulePositions[moduleIndex].distanceMeters, - modulePositions[moduleIndex].angle - ); - lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; - } - - // Update gyro angle - if (gyroInputs.connected) { - // Use the real gyro angle - rawGyroRotation = gyroInputs.odometryYawPositions[i]; - } else { - // Use the angle delta from the kinematics and module deltas - Twist2d twist = kinematics.toTwist2d(moduleDeltas); - rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); - } - - // Apply update - poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); - } - - if (Constants.controller.getRightBumper() && getState() == DriveStates.REGULAR_DRIVE) { - setState(DriveStates.SLOW_MODE); - } else if ( - Constants.controller.getLeftBumper() && getState() == DriveStates.REGULAR_DRIVE - ) { - setState(DriveStates.SPEED_MAXXING); - } else if ( - !Constants.controller.getRightBumper() && !Constants.controller.getLeftBumper() - ) { - setState(DriveStates.REGULAR_DRIVE); - } - } - - /** - * Runs the drive at the desired velocity. - * - * @param speeds Speeds in meters/sec - */ - public void runVelocity(ChassisSpeeds speeds) { - // Calculate module setpoints - ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize( - speeds, - Constants.Drive.DISCRETIZE_TIME_SECONDS - ); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds( - setpointStates, - Constants.Drive.MAX_LINEAR_SPEED - ); - - // Send setpoints to modules - SwerveModuleState[] optimizedSetpointStates = - new SwerveModuleState[Constants.Drive.NUM_MODULES]; - for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { - // The module returns the optimized state, useful for logging - optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); - } - - // Log setpoint states - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); - } - - /** Stops the drive. */ - public void stop() { - runVelocity(new ChassisSpeeds()); - } - - /** - * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will - * return to their normal orientations the next time a nonzero velocity is requested. - */ - public void stopWithX() { - Rotation2d[] headings = new Rotation2d[Constants.Drive.NUM_MODULES]; - for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { - headings[i] = getModuleTranslations()[i].getAngle(); - } - kinematics.resetHeadings(headings); - stop(); - } - - /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveStates/Measured") - private SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[Constants.Drive.NUM_MODULES]; - for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { - states[i] = modules[i].getState(); - } - return states; - } - - /** Returns the module positions (turn angles and drive positions) for all of the modules. */ - private SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] states = new SwerveModulePosition[Constants.Drive.NUM_MODULES]; - for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { - states[i] = modules[i].getPosition(); - } - return states; - } - - /** Returns the current odometry pose. */ - @AutoLogOutput(key = "Odometry/Robot") - public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); - } - - public ChassisSpeeds getChassisSpeed() { - ChassisSpeeds robotChassisSpeed = kinematics.toChassisSpeeds(getModuleStates()); - return robotChassisSpeed; - } - - public double calculateVelocity() { - double robotSpeed = Math.sqrt( - Math.pow(getChassisSpeed().vxMetersPerSecond, 2) + - Math.pow(getChassisSpeed().vyMetersPerSecond, 2) - ); - return robotSpeed; - } - - /** Returns the current odometry rotation. */ - public Rotation2d getRotation() { - return getPose().getRotation(); - } - - /** Resets the current odometry pose. */ - public void setPose(Pose2d pose) { - poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); - } - - /** - * Adds a vision measurement to the pose estimator. - * - * @param visionPose The pose of the robot as measured by the vision camera. - * @param timestamp The timestamp of the vision measurement in seconds. - */ - public void addVisionMeasurement(Pose2d visionPose, double timestamp) { - poseEstimator.addVisionMeasurement(visionPose, timestamp); - } - - /** Returns the maximum linear speed in meters per sec. */ - public double getMaxLinearSpeedMetersPerSec() { - return Constants.Drive.MAX_LINEAR_SPEED; - } - - /** Returns the maximum angular speed in radians per sec. */ - public double getMaxAngularSpeedRadPerSec() { - return Constants.Drive.MAX_ANGULAR_SPEED; - } - - /** Returns an array of module translations. */ - public static Translation2d[] getModuleTranslations() { - return new Translation2d[] { - new Translation2d( - Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, - Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF - ), - new Translation2d( - Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, - -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF - ), - new Translation2d( - -Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, - Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF - ), - new Translation2d( - -Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, - -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF - ), - }; - } - - public void pathPlannerInit() { - AutoBuilder.configureHolonomic( - this::getPose, // Robot pose supplier - this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) - this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds - new HolonomicPathFollowerConfig( - // HolonomicPathFollowerConfig, this should likely live in - // your Constants class - Constants.Drive.TRANSLATION_PID, // Translation PID constants - Constants.Drive.ROTATION_PID, // Rotation PID constants - Constants.Drive.MAX_MODULE_SPEED, // Max module speed, in m/s - Constants.Drive.DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to - // furthest module. - new ReplanningConfig() // Default path replanning config. See the API for the options - // here - ), - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - ); - } +public class Drive extends Subsystem { + + static final Lock odometryLock = new ReentrantLock(); + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[Constants.Drive.NUM_MODULES]; // FL, FR, BL, BR + private PPDriveWrapper ppConfig; + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); + private Rotation2d rawGyroRotation = new Rotation2d(); + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + }; + private SwerveDrivePoseEstimator poseEstimator = + new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO) { + super("Drive", DriveStates.REGULAR_DRIVE); + + ppConfig = new PPDriveWrapper(this); + + this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); + + // Start threads (no-op for each if no signals have been created) + PhoenixOdometryThread.getInstance().start(); + SparkMaxOdometryThread.getInstance().start(); + } + + public void runState() { + drive( + this, + () -> Constants.controller.getLeftY(), + () -> Constants.controller.getLeftX(), + () -> -Constants.controller.getRightX(), + getState().getRotationModifier(), + getState().getTranslationModifier()); + } + + public void drive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier, + double rotationMultiplier, + double translationMultiplier) { + // Apply deadband + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), + Constants.Drive.CONTROLLER_DEADBAND); + Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = + MathUtil.applyDeadband(omegaSupplier.getAsDouble(), Constants.Drive.CONTROLLER_DEADBAND); + + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + omega = Math.copySign(omega * omega, omega); + + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + + // Convert to field relative speeds & send command + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec() * translationMultiplier, + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec() * translationMultiplier, + omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier, + isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation())); + } + + @Override + public void periodic() { + super.periodic(); + odometryLock.lock(); // Prevents odometry updates while reading data + gyroIO.updateInputs(gyroInputs); + for (var module : modules) { + module.updateInputs(); + } + odometryLock.unlock(); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } + + // Update odometry + double[] sampleTimestamps = + modules[0].getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = + new SwerveModulePosition[Constants.Drive.NUM_MODULES]; + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[Constants.Drive.NUM_MODULES]; + for (int moduleIndex = 0; moduleIndex < Constants.Drive.NUM_MODULES; moduleIndex++) { + modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; + moduleDeltas[moduleIndex] = + new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.odometryYawPositions[i]; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + + // Apply update + poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + } + } + + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = + ChassisSpeeds.discretize(speeds, Constants.Drive.DISCRETIZE_TIME_SECONDS); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, Constants.Drive.MAX_LINEAR_SPEED); + + // Send setpoints to modules + SwerveModuleState[] optimizedSetpointStates = + new SwerveModuleState[Constants.Drive.NUM_MODULES]; + for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { + // The module returns the optimized state, useful for logging + optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + } + + // Log setpoint states + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + } + + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[Constants.Drive.NUM_MODULES]; + for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { + headings[i] = getModuleTranslations()[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); + } + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[Constants.Drive.NUM_MODULES]; + for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { + states[i] = modules[i].getState(); + } + return states; + } + + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[Constants.Drive.NUM_MODULES]; + for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { + states[i] = modules[i].getPosition(); + } + return states; + } + + /** Returns the current odometry pose. */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + public ChassisSpeeds getChassisSpeed() { + ChassisSpeeds robotChassisSpeed = kinematics.toChassisSpeeds(getModuleStates()); + return robotChassisSpeed; + } + + public double calculateVelocity() { + double robotSpeed = + Math.sqrt( + Math.pow(getChassisSpeed().vxMetersPerSecond, 2) + + Math.pow(getChassisSpeed().vyMetersPerSecond, 2)); + return robotSpeed; + } + + /** Returns the current odometry rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** Resets the current odometry pose. */ + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + /** + * Adds a vision measurement to the pose estimator. + * + * @param visionPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. + */ + public void addVisionMeasurement(Pose2d visionPose, double timestamp) { + poseEstimator.addVisionMeasurement(visionPose, timestamp); + } + + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return Constants.Drive.MAX_LINEAR_SPEED; + } + + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return Constants.Drive.MAX_ANGULAR_SPEED; + } + + /** Returns an array of module translations. */ + public static Translation2d[] getModuleTranslations() { + return new Translation2d[] { + new Translation2d( + Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, + Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF), + new Translation2d( + Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, + -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF), + new Translation2d( + -Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, + Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF), + new Translation2d( + -Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, + -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF), + }; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveStates.java b/src/main/java/frc/robot/subsystems/drive/DriveStates.java index 5f07146..d3e6446 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveStates.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveStates.java @@ -4,33 +4,33 @@ import frc.robot.subsystems.SubsystemStates; public enum DriveStates implements SubsystemStates { - REGULAR_DRIVE("Regular Drive", Constants.Drive.REGULAR_TM, Constants.Drive.REGULAR_RM), - SLOW_MODE("Slow Mode", Constants.Drive.SLOW_TM, Constants.Drive.SLOW_RM), - AUTO_ALIGN("Auto Aligning", Constants.Drive.AA_TM, Constants.Drive.AA_RM), - SPEED_MAXXING("Speed Maxxing", Constants.Drive.FAST_TM, Constants.Drive.FAST_RM); - - // Someone make auto align so that state is useful please - - DriveStates(String stateString, double translationModifier, double rotationModifier) { - this.stateString = stateString; - this.translationModifier = translationModifier; - this.rotationModifier = rotationModifier; - } - - String stateString; - double translationModifier; - double rotationModifier; - - @Override - public String getStateString() { - return stateString; - } - - public double getTranslationModifier() { - return translationModifier; - } - - public double getRotationModifier() { - return rotationModifier; - } + REGULAR_DRIVE("Regular Drive", Constants.Drive.REGULAR_TM, Constants.Drive.REGULAR_RM), + SLOW_MODE("Slow Mode", Constants.Drive.SLOW_TM, Constants.Drive.SLOW_RM), + AUTO_ALIGN("Auto Aligning", Constants.Drive.AA_TM, Constants.Drive.AA_RM), + SPEED_MAXXING("Speed Maxxing", Constants.Drive.FAST_TM, Constants.Drive.FAST_RM); + + // Someone make auto align so that state is useful please + + DriveStates(String stateString, double translationModifier, double rotationModifier) { + this.stateString = stateString; + this.translationModifier = translationModifier; + this.rotationModifier = rotationModifier; + } + + String stateString; + double translationModifier; + double rotationModifier; + + @Override + public String getStateString() { + return stateString; + } + + public double getTranslationModifier() { + return translationModifier; + } + + public double getRotationModifier() { + return rotationModifier; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 8efbde0..9550c02 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -17,15 +17,15 @@ import org.littletonrobotics.junction.AutoLog; public interface GyroIO { - @AutoLog - public static class GyroIOInputs { + @AutoLog + public static class GyroIOInputs { - public boolean connected = false; - public Rotation2d yawPosition = new Rotation2d(); - public double[] odometryYawTimestamps = new double[] {}; - public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; - public double yawVelocityRadPerSec = 0.0; - } + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + public double yawVelocityRadPerSec = 0.0; + } - public default void updateInputs(GyroIOInputs inputs) {} + public default void updateInputs(GyroIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 391357c..5b29f8f 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -27,51 +27,51 @@ /** IO implementation for Pigeon2 */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(Constants.Drive.Pidgeon2.DEVICE_ID); - private final StatusSignal yaw = pigeon.getYaw(); - private final Queue yawPositionQueue; - private final Queue yawTimestampQueue; - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + private final Pigeon2 pigeon = new Pigeon2(Constants.Drive.Pidgeon2.DEVICE_ID); + private final StatusSignal yaw = pigeon.getYaw(); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - public GyroIOPigeon2(boolean phoenixDrive) { - pigeon.getConfigurator().apply(new Pigeon2Configuration()); - pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(Constants.Drive.Module.ODOMETRY_FREQUENCY); - yawVelocity.setUpdateFrequency(Constants.Drive.Pidgeon2.UPDATE_FREQUENCY); - pigeon.optimizeBusUtilization(); - if (phoenixDrive) { - yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = PhoenixOdometryThread.getInstance() - .registerSignal(pigeon, pigeon.getYaw()); - } else { - yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = SparkMaxOdometryThread.getInstance() - .registerSignal(() -> { - boolean valid = yaw.refresh().getStatus().isOK(); - if (valid) { - return OptionalDouble.of(yaw.getValueAsDouble()); - } else { - return OptionalDouble.empty(); - } - }); - } - } + public GyroIOPigeon2(boolean phoenixDrive) { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(Constants.Drive.Module.ODOMETRY_FREQUENCY); + yawVelocity.setUpdateFrequency(Constants.Drive.Pidgeon2.UPDATE_FREQUENCY); + pigeon.optimizeBusUtilization(); + if (phoenixDrive) { + yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw()); + } else { + yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + boolean valid = yaw.refresh().getStatus().isOK(); + if (valid) { + return OptionalDouble.of(yaw.getValueAsDouble()); + } else { + return OptionalDouble.empty(); + } + }); + } + } - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); - inputs.odometryYawTimestamps = yawTimestampQueue - .stream() - .mapToDouble((Double value) -> value) - .toArray(); - inputs.odometryYawPositions = yawPositionQueue - .stream() - .map((Double value) -> Rotation2d.fromDegrees(value)) - .toArray(Rotation2d[]::new); - yawTimestampQueue.clear(); - yawPositionQueue.clear(); - } + inputs.odometryYawTimestamps = + yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryYawPositions = + yawPositionQueue.stream() + .map((Double value) -> Rotation2d.fromDegrees(value)) + .toArray(Rotation2d[]::new); + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index cd764c7..93d1015 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -24,208 +24,202 @@ public class Module { - private static final double WHEEL_RADIUS = Units.inchesToMeters(Constants.Drive.WHEEL_RADIUS); - - private final ModuleIO io; - private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private final int index; - - private final SimpleMotorFeedforward driveFeedforward; - private final PIDController driveFeedback; - private final PIDController turnFeedback; - private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop - private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop - private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute - private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; - - public Module(ModuleIO io, int index) { - this.io = io; - this.index = index; - - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.currentMode) { - // TODO: Configure PID in rela mabye? idk why its off - case REAL: - case REPLAY: - driveFeedforward = new SimpleMotorFeedforward( - Constants.Drive.Module.REPLAY_FF.kS, - Constants.Drive.Module.REPLAY_FF.kV - ); - driveFeedback = new PIDController( - Constants.Drive.Module.REPLAY_DRIVE_PID.kP, - Constants.Drive.Module.REPLAY_DRIVE_PID.kI, - Constants.Drive.Module.REPLAY_DRIVE_PID.kD - ); - turnFeedback = new PIDController( - Constants.Drive.Module.REPLAY_TURN_PID.kP, - Constants.Drive.Module.REPLAY_TURN_PID.kI, - Constants.Drive.Module.REPLAY_TURN_PID.kD - ); - break; - case SIM: - driveFeedforward = new SimpleMotorFeedforward( - Constants.Drive.Module.SIM_FF.kS, - Constants.Drive.Module.SIM_FF.kV - ); - driveFeedback = new PIDController( - Constants.Drive.Module.SIM_DRIVE_PID.kP, - Constants.Drive.Module.SIM_DRIVE_PID.kI, - Constants.Drive.Module.SIM_DRIVE_PID.kD - ); - turnFeedback = new PIDController( - Constants.Drive.Module.SIM_TURN_PID.kP, - Constants.Drive.Module.SIM_TURN_PID.kI, - Constants.Drive.Module.SIM_TURN_PID.kD - ); - 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; - } - - turnFeedback.enableContinuousInput(-Math.PI, Math.PI); - setBrakeMode(true); - } - - /** - * Update inputs without running the rest of the periodic logic. This is useful since these - * updates need to be properly thread-locked. - */ - public void updateInputs() { - io.updateInputs(inputs); - } - - public void periodic() { - Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); - - // On first cycle, reset relative turn encoder - // Wait until absolute angle is nonzero in case it wasn't initialized yet - if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { - turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); - } - - // Run closed loop turn control - if (angleSetpoint != null) { - io.setTurnVoltage( - turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians()) - ); - - // Run closed loop drive control - // Only allowed if closed loop turn control is running - if (speedSetpoint != null) { - // Scale velocity based on turn error - // - // When the error is 90°, 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()); - - // Run drive controller - double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; - io.setDriveVoltage( - driveFeedforward.calculate(velocityRadPerSec) + - driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec) - ); - } - } - - // Calculate positions for odometry - int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together - odometryPositions = new SwerveModulePosition[sampleCount]; - for (int i = 0; i < sampleCount; i++) { - double positionMeters = inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS; - Rotation2d angle = - inputs.odometryTurnPositions[i].plus( - turnRelativeOffset != null ? turnRelativeOffset : new Rotation2d() - ); - odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); - } - } - - /** Runs the module with the specified setpoint state. Returns the optimized state. */ - public SwerveModuleState runSetpoint(SwerveModuleState state) { - // Optimize state based on current angle - // Controllers run in "periodic" when the setpoint is not null - var optimizedState = SwerveModuleState.optimize(state, getAngle()); - - // Update setpoints, controllers run in "periodic" - angleSetpoint = optimizedState.angle; - speedSetpoint = optimizedState.speedMetersPerSecond; - - return optimizedState; - } - - /** Runs the module with the specified voltage while controlling to zero degrees. */ - public void runCharacterization(double volts) { - // Closed loop turn control - angleSetpoint = new Rotation2d(); - - // Open loop drive control - io.setDriveVoltage(volts); - speedSetpoint = null; - } - - /** Disables all outputs to motors. */ - public void stop() { - io.setTurnVoltage(0.0); - io.setDriveVoltage(0.0); - - // Disable closed loop control for turn and drive - angleSetpoint = null; - speedSetpoint = null; - } - - /** Sets whether brake mode is enabled. */ - public void setBrakeMode(boolean enabled) { - io.setDriveBrakeMode(enabled); - io.setTurnBrakeMode(enabled); - } - - /** Returns the current turn angle of the module. */ - public Rotation2d getAngle() { - if (turnRelativeOffset == null) { - return new Rotation2d(); - } else { - return inputs.turnPosition.plus(turnRelativeOffset); - } - } - - /** Returns the current drive position of the module in meters. */ - public double getPositionMeters() { - return inputs.drivePositionRad * WHEEL_RADIUS; - } - - /** Returns the current drive velocity of the module in meters per second. */ - public double getVelocityMetersPerSec() { - return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; - } - - /** Returns the module position (turn angle and drive position). */ - public SwerveModulePosition getPosition() { - return new SwerveModulePosition(getPositionMeters(), getAngle()); - } - - /** Returns the module state (turn angle and drive velocity). */ - public SwerveModuleState getState() { - return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); - } - - /** Returns the module positions received this cycle. */ - public SwerveModulePosition[] getOdometryPositions() { - return odometryPositions; - } - - /** Returns the timestamps of the samples received this cycle. */ - public double[] getOdometryTimestamps() { - return inputs.odometryTimestamps; - } - - /** Returns the drive velocity in radians/sec. */ - public double getCharacterizationVelocity() { - return inputs.driveVelocityRadPerSec; - } + private static final double WHEEL_RADIUS = Units.inchesToMeters(Constants.Drive.WHEEL_RADIUS); + + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final int index; + + private final SimpleMotorFeedforward driveFeedforward; + private final PIDController driveFeedback; + private final PIDController turnFeedback; + private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop + private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop + private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + + public Module(ModuleIO io, int index) { + this.io = io; + this.index = index; + + // Switch constants based on mode (the physics simulator is treated as a + // separate robot with different tuning) + switch (Constants.currentMode) { + // TODO: Configure PID in rela mabye? idk why its off + case REAL: + case REPLAY: + driveFeedforward = + new SimpleMotorFeedforward( + Constants.Drive.Module.REPLAY_FF.kS, Constants.Drive.Module.REPLAY_FF.kV); + driveFeedback = + new PIDController( + Constants.Drive.Module.REPLAY_DRIVE_PID.kP, + Constants.Drive.Module.REPLAY_DRIVE_PID.kI, + Constants.Drive.Module.REPLAY_DRIVE_PID.kD); + turnFeedback = + new PIDController( + Constants.Drive.Module.REPLAY_TURN_PID.kP, + Constants.Drive.Module.REPLAY_TURN_PID.kI, + Constants.Drive.Module.REPLAY_TURN_PID.kD); + break; + case SIM: + driveFeedforward = + new SimpleMotorFeedforward( + Constants.Drive.Module.SIM_FF.kS, Constants.Drive.Module.SIM_FF.kV); + driveFeedback = + new PIDController( + Constants.Drive.Module.SIM_DRIVE_PID.kP, + Constants.Drive.Module.SIM_DRIVE_PID.kI, + Constants.Drive.Module.SIM_DRIVE_PID.kD); + turnFeedback = + new PIDController( + Constants.Drive.Module.SIM_TURN_PID.kP, + Constants.Drive.Module.SIM_TURN_PID.kI, + Constants.Drive.Module.SIM_TURN_PID.kD); + 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; + } + + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + setBrakeMode(true); + } + + /** + * Update inputs without running the rest of the periodic logic. This is useful since these + * updates need to be properly thread-locked. + */ + public void updateInputs() { + io.updateInputs(inputs); + } + + public void periodic() { + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + + // On first cycle, reset relative turn encoder + // Wait until absolute angle is nonzero in case it wasn't initialized yet + if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { + turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); + } + + // Run closed loop turn control + if (angleSetpoint != null) { + io.setTurnVoltage( + turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); + + // Run closed loop drive control + // Only allowed if closed loop turn control is running + if (speedSetpoint != null) { + // Scale velocity based on turn error + // + // When the error is 90°, 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()); + + // Run drive controller + double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; + io.setDriveVoltage( + driveFeedforward.calculate(velocityRadPerSec) + + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); + } + } + + // Calculate positions for odometry + int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together + odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS; + Rotation2d angle = + inputs.odometryTurnPositions[i].plus( + turnRelativeOffset != null ? turnRelativeOffset : new Rotation2d()); + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + } + + /** Runs the module with the specified setpoint state. Returns the optimized state. */ + public SwerveModuleState runSetpoint(SwerveModuleState state) { + // Optimize state based on current angle + // Controllers run in "periodic" when the setpoint is not null + var optimizedState = SwerveModuleState.optimize(state, getAngle()); + + // Update setpoints, controllers run in "periodic" + angleSetpoint = optimizedState.angle; + speedSetpoint = optimizedState.speedMetersPerSecond; + + return optimizedState; + } + + /** Runs the module with the specified voltage while controlling to zero degrees. */ + public void runCharacterization(double volts) { + // Closed loop turn control + angleSetpoint = new Rotation2d(); + + // Open loop drive control + io.setDriveVoltage(volts); + speedSetpoint = null; + } + + /** Disables all outputs to motors. */ + public void stop() { + io.setTurnVoltage(0.0); + io.setDriveVoltage(0.0); + + // Disable closed loop control for turn and drive + angleSetpoint = null; + speedSetpoint = null; + } + + /** Sets whether brake mode is enabled. */ + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + if (turnRelativeOffset == null) { + return new Rotation2d(); + } else { + return inputs.turnPosition.plus(turnRelativeOffset); + } + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return inputs.drivePositionRad * WHEEL_RADIUS; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; + } + + /** Returns the module position (turn angle and drive position). */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the module positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return inputs.odometryTimestamps; + } + + /** Returns the drive velocity in radians/sec. */ + public double getCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 91ba0e3..1506441 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -17,37 +17,37 @@ import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { - @AutoLog - public static class ModuleIOInputs { + @AutoLog + public static class ModuleIOInputs { - public double drivePositionRad = 0.0; - public double driveVelocityRadPerSec = 0.0; - public double driveAppliedVolts = 0.0; - public double[] driveCurrentAmps = new double[] {}; + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double[] driveCurrentAmps = new double[] {}; - public Rotation2d turnAbsolutePosition = new Rotation2d(); - public Rotation2d turnPosition = new Rotation2d(); - public double turnVelocityRadPerSec = 0.0; - public double turnAppliedVolts = 0.0; - public double[] turnCurrentAmps = new double[] {}; + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double[] turnCurrentAmps = new double[] {}; - public double[] odometryTimestamps = new double[] {}; - public double[] odometryDrivePositionsRad = new double[] {}; - public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; - } + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } - /** Updates the set of loggable inputs. */ - public default void updateInputs(ModuleIOInputs inputs) {} + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} - /** Run the drive motor at the specified voltage. */ - public default void setDriveVoltage(double volts) {} + /** Run the drive motor at the specified voltage. */ + public default void setDriveVoltage(double volts) {} - /** Run the turn motor at the specified voltage. */ - public default void setTurnVoltage(double volts) {} + /** Run the turn motor at the specified voltage. */ + public default void setTurnVoltage(double volts) {} - /** Enable or disable brake mode on the drive motor. */ - public default void setDriveBrakeMode(boolean enable) {} + /** Enable or disable brake mode on the drive motor. */ + public default void setDriveBrakeMode(boolean enable) {} - /** Enable or disable brake mode on the turn motor. */ - public default void setTurnBrakeMode(boolean enable) {} + /** Enable or disable brake mode on the turn motor. */ + public default void setTurnBrakeMode(boolean enable) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOHybrid.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOHybrid.java index 0c902f1..614c7a4 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOHybrid.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOHybrid.java @@ -28,190 +28,175 @@ */ public class ModuleIOHybrid implements ModuleIO { - private final TalonFX driveTalon; - private final CANSparkMax turnSparkMax; - private final CANcoder cancoder; - - private final Queue timestampQueue; - - private final StatusSignal drivePosition; - private final Queue drivePositionQueue; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - private final RelativeEncoder turnRelativeEncoder; - private final Queue turnPositionQueue; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOHybrid(int index) { - switch (index) { - case 0: - driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE0_ID); - turnSparkMax = new CANSparkMax( - Constants.Drive.Module.Hybrid.TURN0_ID, - MotorType.kBrushless - ); - cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER0_ID); - absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET0); - break; - case 1: - driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE1_ID); - turnSparkMax = new CANSparkMax( - Constants.Drive.Module.Hybrid.TURN1_ID, - MotorType.kBrushless - ); - cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER1_ID); - absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET1); - break; - case 2: - driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE2_ID); - turnSparkMax = new CANSparkMax( - Constants.Drive.Module.Hybrid.TURN2_ID, - MotorType.kBrushless - ); - cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER2_ID); - absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET2); - break; - case 3: - driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE3_ID); - turnSparkMax = new CANSparkMax( - Constants.Drive.Module.Hybrid.TURN3_ID, - MotorType.kBrushless - ); - cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER3_ID); - absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET3); - break; - default: - throw new RuntimeException("Invalid module index"); - } - - // TalonFX configuration - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.SupplyCurrentLimit = - Constants.Drive.Module.Hybrid.DRIVE_CURRENT_LIMIT; - driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - driveTalon.getConfigurator().apply(driveConfig); - setDriveBrakeMode(true); - - cancoder.getConfigurator().apply(new CANcoderConfiguration()); - - timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - - drivePosition = driveTalon.getPosition(); - drivePositionQueue = PhoenixOdometryThread.getInstance() - .registerSignal(driveTalon, driveTalon.getPosition()); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getSupplyCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - Constants.Drive.Module.ODOMETRY_FREQUENCY, - drivePosition - ); - BaseStatusSignal.setUpdateFrequencyForAll( - Constants.Drive.Module.Hybrid.TALON_UPDATE_FREQUENCY_HZ, - driveVelocity, - driveAppliedVolts, - driveCurrent - ); - driveTalon.optimizeBusUtilization(); - - // SparkMax configuration - turnSparkMax.restoreFactoryDefaults(); - turnSparkMax.setCANTimeout(Constants.Drive.Module.Hybrid.SPARK_TIMEOUT_MS); - turnRelativeEncoder = turnSparkMax.getEncoder(); - - turnSparkMax.setInverted(isTurnMotorInverted); - turnSparkMax.setSmartCurrentLimit(Constants.Drive.Module.Hybrid.TURN_CURRENT_LIMIT); - turnSparkMax.enableVoltageCompensation(Constants.MAX_VOLTS); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod( - Constants.Drive.Module.Hybrid.SPARK_MEASURMENT_PERIOD_MS - ); - turnRelativeEncoder.setAverageDepth(Constants.Drive.Module.Hybrid.SPARK_AVG_DEPTH); - - turnSparkMax.setCANTimeout(0); - turnSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, - (int) (Constants.Drive.Module.Hybrid.SPARK_FRAME_PERIOD) - ); - turnPositionQueue = SparkMaxOdometryThread.getInstance() - .registerSignal(() -> { - double value = turnRelativeEncoder.getPosition(); - if (turnSparkMax.getLastError() == REVLibError.kOk) { - return OptionalDouble.of(value); - } else { - return OptionalDouble.empty(); - } - }); - - turnSparkMax.burnFlash(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); - - // Turn Stuff - inputs.turnAbsolutePosition = Rotation2d.fromRotations( - cancoder.getAbsolutePosition().getValueAsDouble() - ).minus(absoluteEncoderOffset); - inputs.turnPosition = Rotation2d.fromRotations( - turnRelativeEncoder.getPosition() / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO - ); - inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( - turnRelativeEncoder.getVelocity() - ) / - Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); - inputs.turnCurrentAmps = new double[] { turnSparkMax.getOutputCurrent() }; - - // Other stuff - inputs.odometryTimestamps = timestampQueue - .stream() - .mapToDouble((Double value) -> value) - .toArray(); - inputs.odometryDrivePositionsRad = drivePositionQueue - .stream() - .mapToDouble( - (Double value) -> - Units.rotationsToRadians(value) / Constants.Drive.Module.Hybrid.DRIVE_GEAR_RATIO - ) - .toArray(); - inputs.odometryTurnPositions = turnPositionQueue - .stream() - .map((Double value) -> - Rotation2d.fromRotations(value / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO) - ) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setTurnVoltage(double volts) { - turnSparkMax.setVoltage(volts); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - driveTalon.getConfigurator().apply(config); - } - - public void setTurnBrakeMode(boolean enable) { - turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } + private final TalonFX driveTalon; + private final CANSparkMax turnSparkMax; + private final CANcoder cancoder; + + private final Queue timestampQueue; + + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + private final RelativeEncoder turnRelativeEncoder; + private final Queue turnPositionQueue; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOHybrid(int index) { + switch (index) { + case 0: + driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE0_ID); + turnSparkMax = + new CANSparkMax(Constants.Drive.Module.Hybrid.TURN0_ID, MotorType.kBrushless); + cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER0_ID); + absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET0); + break; + case 1: + driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE1_ID); + turnSparkMax = + new CANSparkMax(Constants.Drive.Module.Hybrid.TURN1_ID, MotorType.kBrushless); + cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER1_ID); + absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET1); + break; + case 2: + driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE2_ID); + turnSparkMax = + new CANSparkMax(Constants.Drive.Module.Hybrid.TURN2_ID, MotorType.kBrushless); + cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER2_ID); + absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET2); + break; + case 3: + driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE3_ID); + turnSparkMax = + new CANSparkMax(Constants.Drive.Module.Hybrid.TURN3_ID, MotorType.kBrushless); + cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER3_ID); + absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET3); + break; + default: + throw new RuntimeException("Invalid module index"); + } + + // TalonFX configuration + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = + Constants.Drive.Module.Hybrid.DRIVE_CURRENT_LIMIT; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveTalon.getConfigurator().apply(driveConfig); + setDriveBrakeMode(true); + + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + drivePosition = driveTalon.getPosition(); + drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getSupplyCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + Constants.Drive.Module.ODOMETRY_FREQUENCY, drivePosition); + BaseStatusSignal.setUpdateFrequencyForAll( + Constants.Drive.Module.Hybrid.TALON_UPDATE_FREQUENCY_HZ, + driveVelocity, + driveAppliedVolts, + driveCurrent); + driveTalon.optimizeBusUtilization(); + + // SparkMax configuration + turnSparkMax.restoreFactoryDefaults(); + turnSparkMax.setCANTimeout(Constants.Drive.Module.Hybrid.SPARK_TIMEOUT_MS); + turnRelativeEncoder = turnSparkMax.getEncoder(); + + turnSparkMax.setInverted(isTurnMotorInverted); + turnSparkMax.setSmartCurrentLimit(Constants.Drive.Module.Hybrid.TURN_CURRENT_LIMIT); + turnSparkMax.enableVoltageCompensation(Constants.MAX_VOLTS); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod( + Constants.Drive.Module.Hybrid.SPARK_MEASURMENT_PERIOD_MS); + turnRelativeEncoder.setAverageDepth(Constants.Drive.Module.Hybrid.SPARK_AVG_DEPTH); + + turnSparkMax.setCANTimeout(0); + turnSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, (int) (Constants.Drive.Module.Hybrid.SPARK_FRAME_PERIOD)); + turnPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + double value = turnRelativeEncoder.getPosition(); + if (turnSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + + turnSparkMax.burnFlash(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + + // Turn Stuff + inputs.turnAbsolutePosition = + Rotation2d.fromRotations(cancoder.getAbsolutePosition().getValueAsDouble()) + .minus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations( + turnRelativeEncoder.getPosition() / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) + / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + + // Other stuff + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream() + .mapToDouble( + (Double value) -> + Units.rotationsToRadians(value) + / Constants.Drive.Module.Hybrid.DRIVE_GEAR_RATIO) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map( + (Double value) -> + Rotation2d.fromRotations(value / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); + } + + public void setTurnBrakeMode(boolean enable) { + turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index e552626..e5f64b7 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -29,56 +29,54 @@ */ public class ModuleIOSim implements ModuleIO { - private DCMotorSim driveSim = new DCMotorSim( - DCMotor.getNEO(Constants.Drive.Module.NUM_DRIVE_MOTORS), - Constants.Drive.Module.Sim.DRIVE_GEARING, - Constants.Drive.Module.Sim.DRIVE_MOI - ); - private DCMotorSim turnSim = new DCMotorSim( - DCMotor.getNEO(Constants.Drive.Module.NUM_TURN_MOTORS), - Constants.Drive.Module.Sim.TURN_GEARING, - Constants.Drive.Module.Sim.TURN_MOI - ); + private DCMotorSim driveSim = + new DCMotorSim( + DCMotor.getNEO(Constants.Drive.Module.NUM_DRIVE_MOTORS), + Constants.Drive.Module.Sim.DRIVE_GEARING, + Constants.Drive.Module.Sim.DRIVE_MOI); + private DCMotorSim turnSim = + new DCMotorSim( + DCMotor.getNEO(Constants.Drive.Module.NUM_TURN_MOTORS), + Constants.Drive.Module.Sim.TURN_GEARING, + Constants.Drive.Module.Sim.TURN_MOI); - private final Rotation2d turnAbsoluteInitPosition = new Rotation2d( - Math.random() * Constants.RADIAN_CF - ); - private double driveAppliedVolts = 0.0; - private double turnAppliedVolts = 0.0; + private final Rotation2d turnAbsoluteInitPosition = + new Rotation2d(Math.random() * Constants.RADIAN_CF); + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; - @Override - public void updateInputs(ModuleIOInputs inputs) { - driveSim.update(Constants.Drive.Module.Sim.LOOP_PERIOD_SECS); - turnSim.update(Constants.Drive.Module.Sim.LOOP_PERIOD_SECS); + @Override + public void updateInputs(ModuleIOInputs inputs) { + driveSim.update(Constants.Drive.Module.Sim.LOOP_PERIOD_SECS); + turnSim.update(Constants.Drive.Module.Sim.LOOP_PERIOD_SECS); - inputs.drivePositionRad = driveSim.getAngularPositionRad(); - inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); - inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = new double[] { Math.abs(driveSim.getCurrentDrawAmps()) }; + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; - inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus( - turnAbsoluteInitPosition - ); - inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); - inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); - inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = new double[] { Math.abs(turnSim.getCurrentDrawAmps()) }; + inputs.turnAbsolutePosition = + new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); + inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; - inputs.odometryTimestamps = new double[] { Timer.getFPGATimestamp() }; - inputs.odometryDrivePositionsRad = new double[] { inputs.drivePositionRad }; - inputs.odometryTurnPositions = new Rotation2d[] { inputs.turnPosition }; - } + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; + } - @Override - public void setDriveVoltage(double volts) { - // :( it doesent work if you clamp volts regularly idk. It really just stopped working - driveAppliedVolts = MathUtil.clamp(volts, Constants.MIN_VOLTS, Constants.MAX_VOLTS); - driveSim.setInputVoltage(volts); - } + @Override + public void setDriveVoltage(double volts) { + // :( it doesent work if you clamp volts regularly idk. It really just stopped working + driveAppliedVolts = MathUtil.clamp(volts, Constants.MIN_VOLTS, Constants.MAX_VOLTS); + driveSim.setInputVoltage(volts); + } - @Override - public void setTurnVoltage(double volts) { - turnAppliedVolts = MathUtil.clamp(volts, Constants.MIN_VOLTS, Constants.MAX_VOLTS); - turnSim.setInputVoltage(turnAppliedVolts); - } + @Override + public void setTurnVoltage(double volts) { + turnAppliedVolts = MathUtil.clamp(volts, Constants.MIN_VOLTS, Constants.MAX_VOLTS); + turnSim.setInputVoltage(turnAppliedVolts); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index 52272ed..a1eeafe 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -41,169 +41,164 @@ */ public class ModuleIOSparkMax implements ModuleIO { - // Gear ratios for SDS MK4i L2, adjust as necessary - private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private static final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final CANSparkMax driveSparkMax; - private final CANSparkMax turnSparkMax; - - private final RelativeEncoder driveEncoder; - private final RelativeEncoder turnRelativeEncoder; - private final AnalogInput turnAbsoluteEncoder; - private final Queue timestampQueue; - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOSparkMax(int index) { - switch (index) { - case 0: - driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(2, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(0); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 1: - driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(1); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 2: - driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(6, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 3: - driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(8, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(3); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - default: - throw new RuntimeException("Invalid module index"); - } - - driveSparkMax.restoreFactoryDefaults(); - turnSparkMax.restoreFactoryDefaults(); - - driveSparkMax.setCANTimeout(250); - turnSparkMax.setCANTimeout(250); - - driveEncoder = driveSparkMax.getEncoder(); - turnRelativeEncoder = turnSparkMax.getEncoder(); - - turnSparkMax.setInverted(isTurnMotorInverted); - driveSparkMax.setSmartCurrentLimit(40); - turnSparkMax.setSmartCurrentLimit(30); - driveSparkMax.enableVoltageCompensation(12.0); - turnSparkMax.enableVoltageCompensation(12.0); - - driveEncoder.setPosition(0.0); - driveEncoder.setMeasurementPeriod(10); - driveEncoder.setAverageDepth(2); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod(10); - turnRelativeEncoder.setAverageDepth(2); - - driveSparkMax.setCANTimeout(0); - turnSparkMax.setCANTimeout(0); - - driveSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, - (int) (1000.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY) - ); - turnSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, - (int) (1000.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY) - ); - timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); - drivePositionQueue = SparkMaxOdometryThread.getInstance() - .registerSignal(() -> { - double value = driveEncoder.getPosition(); - if (driveSparkMax.getLastError() == REVLibError.kOk) { - return OptionalDouble.of(value); - } else { - return OptionalDouble.empty(); - } - }); - turnPositionQueue = SparkMaxOdometryThread.getInstance() - .registerSignal(() -> { - double value = turnRelativeEncoder.getPosition(); - if (turnSparkMax.getLastError() == REVLibError.kOk) { - return OptionalDouble.of(value); - } else { - return OptionalDouble.empty(); - } - }); - - driveSparkMax.burnFlash(); - turnSparkMax.burnFlash(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = Units.rotationsToRadians(driveEncoder.getPosition()) / - DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( - driveEncoder.getVelocity() - ) / - DRIVE_GEAR_RATIO; - inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); - inputs.driveCurrentAmps = new double[] { driveSparkMax.getOutputCurrent() }; - - inputs.turnAbsolutePosition = new Rotation2d( - (turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) * 2.0 * Math.PI - ).minus(absoluteEncoderOffset); - inputs.turnPosition = Rotation2d.fromRotations( - turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO - ); - inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( - turnRelativeEncoder.getVelocity() - ) / - TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); - inputs.turnCurrentAmps = new double[] { turnSparkMax.getOutputCurrent() }; - - inputs.odometryTimestamps = timestampQueue - .stream() - .mapToDouble((Double value) -> value) - .toArray(); - inputs.odometryDrivePositionsRad = drivePositionQueue - .stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = turnPositionQueue - .stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveSparkMax.setVoltage(volts); - } - - @Override - public void setTurnVoltage(double volts) { - turnSparkMax.setVoltage(volts); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } + // Gear ratios for SDS MK4i L2, adjust as necessary + private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private static final double TURN_GEAR_RATIO = 150.0 / 7.0; + + private final CANSparkMax driveSparkMax; + private final CANSparkMax turnSparkMax; + + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnRelativeEncoder; + private final AnalogInput turnAbsoluteEncoder; + private final Queue timestampQueue; + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOSparkMax(int index) { + switch (index) { + case 0: + driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(2, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(0); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 1: + driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(1); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 2: + driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(6, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(2); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 3: + driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(8, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(3); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + default: + throw new RuntimeException("Invalid module index"); + } + + driveSparkMax.restoreFactoryDefaults(); + turnSparkMax.restoreFactoryDefaults(); + + driveSparkMax.setCANTimeout(250); + turnSparkMax.setCANTimeout(250); + + driveEncoder = driveSparkMax.getEncoder(); + turnRelativeEncoder = turnSparkMax.getEncoder(); + + turnSparkMax.setInverted(isTurnMotorInverted); + driveSparkMax.setSmartCurrentLimit(40); + turnSparkMax.setSmartCurrentLimit(30); + driveSparkMax.enableVoltageCompensation(12.0); + turnSparkMax.enableVoltageCompensation(12.0); + + driveEncoder.setPosition(0.0); + driveEncoder.setMeasurementPeriod(10); + driveEncoder.setAverageDepth(2); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + + driveSparkMax.setCANTimeout(0); + turnSparkMax.setCANTimeout(0); + + driveSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, (int) (1000.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY)); + turnSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, (int) (1000.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY)); + timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); + drivePositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + double value = driveEncoder.getPosition(); + if (driveSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + turnPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + double value = turnRelativeEncoder.getPosition(); + if (turnSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + + driveSparkMax.burnFlash(); + turnSparkMax.burnFlash(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + inputs.drivePositionRad = + Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); + inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; + + inputs.turnAbsolutePosition = + new Rotation2d( + (turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) * 2.0 * Math.PI) + .minus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) + / TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveSparkMax.setVoltage(volts); + } + + @Override + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 7b1caa4..152f7e9 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -42,186 +42,179 @@ */ public class ModuleIOTalonFX implements ModuleIO { - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final CANcoder cancoder; - - private final Queue timestampQueue; - - private final StatusSignal drivePosition; - private final Queue drivePositionQueue; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; - private final Queue turnPositionQueue; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - - // Gear ratios for SDS MK4i L2, adjust as necessary - private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - 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 - break; - case 1: - driveTalon = new TalonFX(3); - turnTalon = new TalonFX(4); - cancoder = new CANcoder(5); - absoluteEncoderOffset = new Rotation2d(0.0); // 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 - break; - case 3: - driveTalon = new TalonFX(9); - turnTalon = new TalonFX(10); - cancoder = new CANcoder(11); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - default: - throw new RuntimeException("Invalid module index"); - } - - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; - driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - driveTalon.getConfigurator().apply(driveConfig); - setDriveBrakeMode(true); - - var turnConfig = new TalonFXConfiguration(); - turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; - turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - turnTalon.getConfigurator().apply(turnConfig); - setTurnBrakeMode(true); - - cancoder.getConfigurator().apply(new CANcoderConfiguration()); - - timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - - drivePosition = driveTalon.getPosition(); - drivePositionQueue = PhoenixOdometryThread.getInstance() - .registerSignal(driveTalon, driveTalon.getPosition()); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getSupplyCurrent(); - - turnAbsolutePosition = cancoder.getAbsolutePosition(); - turnPosition = turnTalon.getPosition(); - turnPositionQueue = PhoenixOdometryThread.getInstance() - .registerSignal(turnTalon, turnTalon.getPosition()); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getSupplyCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - Constants.Drive.Module.ODOMETRY_FREQUENCY, - drivePosition, - turnPosition - ); - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent - ); - driveTalon.optimizeBusUtilization(); - turnTalon.optimizeBusUtilization(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnCurrent - ); - - inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()) / - DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / - DRIVE_GEAR_RATIO; - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = new double[] { driveCurrent.getValueAsDouble() }; - - inputs.turnAbsolutePosition = Rotation2d.fromRotations( - turnAbsolutePosition.getValueAsDouble() - ).minus(absoluteEncoderOffset); - inputs.turnPosition = Rotation2d.fromRotations( - turnPosition.getValueAsDouble() / TURN_GEAR_RATIO - ); - inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / - TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = new double[] { turnCurrent.getValueAsDouble() }; - - inputs.odometryTimestamps = timestampQueue - .stream() - .mapToDouble((Double value) -> value) - .toArray(); - inputs.odometryDrivePositionsRad = drivePositionQueue - .stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = turnPositionQueue - .stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setTurnVoltage(double volts) { - turnTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - driveTalon.getConfigurator().apply(config); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = isTurnMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - turnTalon.getConfigurator().apply(config); - } + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final CANcoder cancoder; + + private final Queue timestampQueue; + + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final Queue turnPositionQueue; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Gear ratios for SDS MK4i L2, adjust as necessary + private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private final double TURN_GEAR_RATIO = 150.0 / 7.0; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + 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 + break; + case 1: + driveTalon = new TalonFX(3); + turnTalon = new TalonFX(4); + cancoder = new CANcoder(5); + absoluteEncoderOffset = new Rotation2d(0.0); // 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 + break; + case 3: + driveTalon = new TalonFX(9); + turnTalon = new TalonFX(10); + cancoder = new CANcoder(11); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + default: + throw new RuntimeException("Invalid module index"); + } + + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveTalon.getConfigurator().apply(driveConfig); + setDriveBrakeMode(true); + + var turnConfig = new TalonFXConfiguration(); + turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + turnTalon.getConfigurator().apply(turnConfig); + setTurnBrakeMode(true); + + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + drivePosition = driveTalon.getPosition(); + drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getSupplyCurrent(); + + turnAbsolutePosition = cancoder.getAbsolutePosition(); + turnPosition = turnTalon.getPosition(); + turnPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getSupplyCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + Constants.Drive.Module.ODOMETRY_FREQUENCY, drivePosition, turnPosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + driveTalon.optimizeBusUtilization(); + turnTalon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + inputs.drivePositionRad = + Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = + Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; + + inputs.turnAbsolutePosition = + Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) + .minus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); + inputs.turnVelocityRadPerSec = + Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; + + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setTurnVoltage(double volts) { + turnTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = + isTurnMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + turnTalon.getConfigurator().apply(config); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java b/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java new file mode 100644 index 0000000..0082d58 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java @@ -0,0 +1,53 @@ +package frc.robot.subsystems.drive; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class PPDriveWrapper extends SubsystemBase { + Drive drive; + + PPDriveWrapper(Drive drive) { + this.drive = drive; + pathPlannerInit(); + } + + public void pathPlannerInit() { + AutoBuilder.configureHolonomic( + drive::getPose, // Robot pose supplier + drive + ::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + drive::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + drive::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + new HolonomicPathFollowerConfig( + // HolonomicPathFollowerConfig, this should likely live in + // your Constants class + Constants.Drive.TRANSLATION_PID, // Translation PID constants + Constants.Drive.ROTATION_PID, // Rotation PID constants + Constants.Drive.MAX_MODULE_SPEED, // Max module speed, in m/s + Constants.Drive + .DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to + // furthest module. + // Replans path if vision or odo detects errors (S tier) + new ReplanningConfig( + true, true) // Default path replanning config. See the API for the options + // here + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index bf21e7a..5677c81 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -36,114 +36,108 @@ */ public class PhoenixOdometryThread extends Thread { - private final Lock signalsLock = new ReentrantLock(); // Prevents conflicts when registering signals - private BaseStatusSignal[] signals = new BaseStatusSignal[0]; - private final List> queues = new ArrayList<>(); - private final List> timestampQueues = new ArrayList<>(); - private boolean isCANFD = false; + private final Lock signalsLock = + new ReentrantLock(); // Prevents conflicts when registering signals + private BaseStatusSignal[] signals = new BaseStatusSignal[0]; + private final List> queues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); + private boolean isCANFD = false; - private static PhoenixOdometryThread instance = null; + private static PhoenixOdometryThread instance = null; - public static PhoenixOdometryThread getInstance() { - if (instance == null) { - instance = new PhoenixOdometryThread(); - } - return instance; - } + public static PhoenixOdometryThread getInstance() { + if (instance == null) { + instance = new PhoenixOdometryThread(); + } + return instance; + } - private PhoenixOdometryThread() { - setName("PhoenixOdometryThread"); - setDaemon(true); - } + private PhoenixOdometryThread() { + setName("PhoenixOdometryThread"); + setDaemon(true); + } - @Override - public void start() { - if (timestampQueues.size() > 0) { - super.start(); - } - } + @Override + public void start() { + if (timestampQueues.size() > 0) { + super.start(); + } + } - public Queue registerSignal(ParentDevice device, StatusSignal signal) { - Queue queue = new ArrayBlockingQueue<>( - Constants.Drive.OdoThread.Phoenix.QUE_CAPACITY - ); - signalsLock.lock(); - Drive.odometryLock.lock(); - try { - isCANFD = CANBus.isNetworkFD(device.getNetwork()); - BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; - System.arraycopy(signals, 0, newSignals, 0, signals.length); - newSignals[signals.length] = signal; - signals = newSignals; - queues.add(queue); - } finally { - signalsLock.unlock(); - Drive.odometryLock.unlock(); - } - return queue; - } + public Queue registerSignal(ParentDevice device, StatusSignal signal) { + Queue queue = new ArrayBlockingQueue<>(Constants.Drive.OdoThread.Phoenix.QUE_CAPACITY); + signalsLock.lock(); + Drive.odometryLock.lock(); + try { + isCANFD = CANBus.isNetworkFD(device.getNetwork()); + BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; + System.arraycopy(signals, 0, newSignals, 0, signals.length); + newSignals[signals.length] = signal; + signals = newSignals; + queues.add(queue); + } finally { + signalsLock.unlock(); + Drive.odometryLock.unlock(); + } + return queue; + } - public Queue makeTimestampQueue() { - Queue queue = new ArrayBlockingQueue<>( - Constants.Drive.OdoThread.Phoenix.QUE_CAPACITY - ); - Drive.odometryLock.lock(); - try { - timestampQueues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(Constants.Drive.OdoThread.Phoenix.QUE_CAPACITY); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } - @Override - public void run() { - while (true) { - // Wait for updates from all signals - signalsLock.lock(); - try { - if (isCANFD) { - BaseStatusSignal.waitForAll( - 2.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY, - signals - ); - } else { - // "waitForAll" does not support blocking on multiple - // signals with a bus that is not CAN FD, regardless - // of Pro licensing. No reasoning for this behavior - // is provided by the documentation. - Thread.sleep( - (long) (Constants.Drive.OdoThread.Phoenix.SLEEP_TIME / - Constants.Drive.Module.ODOMETRY_FREQUENCY) - ); - if (signals.length > 0) BaseStatusSignal.refreshAll(signals); - } - } catch (InterruptedException e) { - e.printStackTrace(); - } finally { - signalsLock.unlock(); - } + @Override + public void run() { + while (true) { + // Wait for updates from all signals + signalsLock.lock(); + try { + if (isCANFD) { + BaseStatusSignal.waitForAll(2.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY, signals); + } else { + // "waitForAll" does not support blocking on multiple + // signals with a bus that is not CAN FD, regardless + // of Pro licensing. No reasoning for this behavior + // is provided by the documentation. + Thread.sleep( + (long) + (Constants.Drive.OdoThread.Phoenix.SLEEP_TIME + / Constants.Drive.Module.ODOMETRY_FREQUENCY)); + if (signals.length > 0) BaseStatusSignal.refreshAll(signals); + } + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + signalsLock.unlock(); + } - // Save new data to queues - Drive.odometryLock.lock(); - try { - double timestamp = Logger.getRealTimestamp() / 1e6; - double totalLatency = 0.0; - for (BaseStatusSignal signal : signals) { - totalLatency += signal.getTimestamp().getLatency(); - } - if (signals.length > 0) { - timestamp -= totalLatency / signals.length; - } - for (int i = 0; i < signals.length; i++) { - queues.get(i).offer(signals[i].getValueAsDouble()); - } - for (int i = 0; i < timestampQueues.size(); i++) { - timestampQueues.get(i).offer(timestamp); - } - } finally { - Drive.odometryLock.unlock(); - } - } - } + // Save new data to queues + Drive.odometryLock.lock(); + try { + double timestamp = Logger.getRealTimestamp() / 1e6; + double totalLatency = 0.0; + for (BaseStatusSignal signal : signals) { + totalLatency += signal.getTimestamp().getLatency(); + } + if (signals.length > 0) { + timestamp -= totalLatency / signals.length; + } + for (int i = 0; i < signals.length; i++) { + queues.get(i).offer(signals[i].getValueAsDouble()); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } finally { + Drive.odometryLock.unlock(); + } + } + } } diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index fea8279..cad718f 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -31,86 +31,80 @@ */ public class SparkMaxOdometryThread { - private List> signals = new ArrayList<>(); - private List> queues = new ArrayList<>(); - private List> timestampQueues = new ArrayList<>(); + private List> signals = new ArrayList<>(); + private List> queues = new ArrayList<>(); + private List> timestampQueues = new ArrayList<>(); - private final Notifier notifier; - private static SparkMaxOdometryThread instance = null; + private final Notifier notifier; + private static SparkMaxOdometryThread instance = null; - public static SparkMaxOdometryThread getInstance() { - if (instance == null) { - instance = new SparkMaxOdometryThread(); - } - return instance; - } + public static SparkMaxOdometryThread getInstance() { + if (instance == null) { + instance = new SparkMaxOdometryThread(); + } + return instance; + } - private SparkMaxOdometryThread() { - notifier = new Notifier(this::periodic); - notifier.setName("SparkMaxOdometryThread"); - } + private SparkMaxOdometryThread() { + notifier = new Notifier(this::periodic); + notifier.setName("SparkMaxOdometryThread"); + } - public void start() { - if (timestampQueues.size() > 0) { - notifier.startPeriodic( - Constants.Drive.OdoThread.SparkMax.PERIOD / - Constants.Drive.Module.ODOMETRY_FREQUENCY - ); - } - } + public void start() { + if (timestampQueues.size() > 0) { + notifier.startPeriodic( + Constants.Drive.OdoThread.SparkMax.PERIOD / Constants.Drive.Module.ODOMETRY_FREQUENCY); + } + } - public Queue registerSignal(Supplier signal) { - Queue queue = new ArrayBlockingQueue<>( - Constants.Drive.OdoThread.SparkMax.QUE_CAPACITY - ); - Drive.odometryLock.lock(); - try { - signals.add(signal); - queues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } + public Queue registerSignal(Supplier signal) { + Queue queue = new ArrayBlockingQueue<>(Constants.Drive.OdoThread.SparkMax.QUE_CAPACITY); + Drive.odometryLock.lock(); + try { + signals.add(signal); + queues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } - public Queue makeTimestampQueue() { - Queue queue = new ArrayBlockingQueue<>( - Constants.Drive.OdoThread.SparkMax.QUE_CAPACITY - ); - Drive.odometryLock.lock(); - try { - timestampQueues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(Constants.Drive.OdoThread.SparkMax.QUE_CAPACITY); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } - private void periodic() { - Drive.odometryLock.lock(); - double timestamp = Logger.getRealTimestamp() / 1e6; - try { - double[] values = new double[signals.size()]; - boolean isValid = true; - for (int i = 0; i < signals.size(); i++) { - OptionalDouble value = signals.get(i).get(); - if (value.isPresent()) { - values[i] = value.getAsDouble(); - } else { - isValid = false; - break; - } - } - if (isValid) { - for (int i = 0; i < queues.size(); i++) { - queues.get(i).offer(values[i]); - } - for (int i = 0; i < timestampQueues.size(); i++) { - timestampQueues.get(i).offer(timestamp); - } - } - } finally { - Drive.odometryLock.unlock(); - } - } + private void periodic() { + Drive.odometryLock.lock(); + double timestamp = Logger.getRealTimestamp() / 1e6; + try { + double[] values = new double[signals.size()]; + boolean isValid = true; + for (int i = 0; i < signals.size(); i++) { + OptionalDouble value = signals.get(i).get(); + if (value.isPresent()) { + values[i] = value.getAsDouble(); + } else { + isValid = false; + break; + } + } + if (isValid) { + for (int i = 0; i < queues.size(); i++) { + queues.get(i).offer(values[i]); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } + } finally { + Drive.odometryLock.unlock(); + } + } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 0bae12c..c37455c 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -10,58 +10,52 @@ public class Intake extends Subsystem { - IntakeIO io; - IntakeIOInputsAutoLogged inputs; - - public Intake(IntakeIO io) { - super("Intake", IntakeStates.OFF); - this.io = io; - inputs = new IntakeIOInputsAutoLogged(); - - // Configure PIDs here - switch (Constants.currentMode) { - case REAL: - io.configurePID(Constants.Intake.REAL_OUT_PID, Constants.Intake.REAL_IN_PID); - break; - case REPLAY: - io.configurePID(new PIDConstants(0, 0, 0), new PIDConstants(0, 0, 0)); - break; - case SIM: - io.configurePID(Constants.Intake.REAL_OUT_PID, Constants.Intake.REAL_IN_PID); - break; - default: - break; - } - } - - protected void runState() { - io.setSetpoints( - getState().getPivotSetPoint(), - getState().getMotorSetPoint(), - getState().getUsingPID() - ); - if (getState() == IntakeStates.INTAKING) { - NoteSimulator.attachToShooter(); - } - } - - public void stop() { - io.stop(); - } - - @Override - public void periodic() { - super.periodic(); - - Logger.recordOutput( - "Intake/Intake Pose", - new Pose3d( - Constants.Intake.ZEROED_PIVOT_TRANSLATION, - new Rotation3d(0, io.getPosition(), 0) - ) - ); - - Logger.processInputs("Intake", inputs); - io.updateInputs(inputs); - } + IntakeIO io; + IntakeIOInputsAutoLogged inputs; + + public Intake(IntakeIO io) { + super("Intake", IntakeStates.OFF); + this.io = io; + inputs = new IntakeIOInputsAutoLogged(); + + // Configure PIDs here + switch (Constants.currentMode) { + case REAL: + io.configurePID(Constants.Intake.REAL_OUT_PID, Constants.Intake.REAL_IN_PID); + break; + case REPLAY: + io.configurePID(new PIDConstants(0, 0, 0), new PIDConstants(0, 0, 0)); + break; + case SIM: + io.configurePID(Constants.Intake.REAL_OUT_PID, Constants.Intake.REAL_IN_PID); + break; + default: + break; + } + } + + protected void runState() { + io.setSetpoints( + getState().getPivotSetPoint(), getState().getMotorSetPoint(), getState().getUsingPID()); + if (getState() == IntakeStates.INTAKING) { + NoteSimulator.attachToShooter(); + } + } + + public void stop() { + io.stop(); + } + + @Override + public void periodic() { + super.periodic(); + + Logger.recordOutput( + "Intake/Intake Pose", + new Pose3d( + Constants.Intake.ZEROED_PIVOT_TRANSLATION, new Rotation3d(0, io.getPosition(), 0))); + + Logger.processInputs("Intake", inputs); + io.updateInputs(inputs); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 119e7ca..1363c5b 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -5,36 +5,33 @@ import org.littletonrobotics.junction.AutoLog; public interface IntakeIO { - @AutoLog - public static class IntakeIOInputs { + @AutoLog + public static class IntakeIOInputs { - public Pose3d position; + public Pose3d position; - public double wheelSpeed; - public double wheelAppliedVoltage; - public double wheelSpeedpoint; + public double wheelSpeed; + public double wheelAppliedVoltage; + public double wheelSpeedpoint; - public double pivotPosition; - public double pivotAppliedVoltage; - public double pivotSetpoint; - public double pivotSetpointError; + public double pivotPosition; + public double pivotAppliedVoltage; + public double pivotSetpoint; + public double pivotSetpointError; - public boolean usingInPID; - } + public boolean usingInPID; + } - public default void updateInputs(IntakeIOInputs inputs) {} + public default void updateInputs(IntakeIOInputs inputs) {} - public default void setSetpoints( - double pivotMotorSetpoint, - double intakeMotorSetpoint, - boolean useIn - ) {} + public default void setSetpoints( + double pivotMotorSetpoint, double intakeMotorSetpoint, boolean useIn) {} - public default void stop() {} + public default void stop() {} - public default double getPosition() { - return 0.0; - } + public default double getPosition() { + return 0.0; + } - public default void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) {} + public default void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index fcf00f5..dcdde34 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -9,97 +9,92 @@ public class IntakeIOSim implements IntakeIO { - SingleJointedArmSim pivotSimModel; - DCMotorSim spinnerWheelSim; - - PIDController outPivotController; - PIDController inPIDController; - - PIDController pivotController; - - double wheelAppliedVoltage; - double pivotAppliedVoltage; - - double wheelSpeedpoint; - double pivotSetpoint; - - boolean usingInPID; - - public IntakeIOSim() { - pivotSimModel = new SingleJointedArmSim( - DCMotor.getKrakenX60(Constants.Intake.NUM_PIVOT_MOTORS), - Constants.Intake.PIVOT_GEARING, // gearing - Constants.Intake.PIVOT_MOI, // MOI - Constants.Intake.PIVOT_LENGTH_METERS, // arm length - 0, // min angle -- hard stop - Constants.Intake.MAX_PIVOT_POSITION, // max angle -- floor - false, - 0 - ); - - spinnerWheelSim = new DCMotorSim( - DCMotor.getFalcon500(Constants.Intake.NUM_SPINNER_MOTORS), - Constants.Intake.SPINNER_GEARING, - Constants.Intake.SPINNER_MOI - ); - - outPivotController = new PIDController(0, 0, 0); - inPIDController = new PIDController(0, 0, 0); - } - - public void setSetpoints( - double pivotMotorSetPoint, - double intakeMotorSetpoint, - boolean useInPID - ) { - usingInPID = useInPID; - - if (useInPID) pivotController = inPIDController; - else pivotController = outPivotController; - - pivotAppliedVoltage = pivotController.calculate( - pivotSimModel.getAngleRads(), - pivotMotorSetPoint - ); - - wheelAppliedVoltage = intakeMotorSetpoint; - - pivotSimModel.setInputVoltage(pivotAppliedVoltage); - spinnerWheelSim.setInputVoltage(intakeMotorSetpoint); - - wheelSpeedpoint = intakeMotorSetpoint; - pivotSetpoint = pivotMotorSetPoint; - } - - public void updateInputs(IntakeIOInputs inputs) { - inputs.wheelSpeed = spinnerWheelSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - inputs.wheelAppliedVoltage = wheelAppliedVoltage; - inputs.wheelSpeedpoint = wheelSpeedpoint; - - inputs.pivotPosition = pivotSimModel.getAngleRads(); - inputs.pivotAppliedVoltage = pivotAppliedVoltage; - inputs.pivotSetpoint = pivotSetpoint; - inputs.pivotSetpointError = Math.abs(pivotSimModel.getAngleRads() - pivotSetpoint); - - inputs.usingInPID = usingInPID; - - pivotSimModel.update(Constants.SIM_UPDATE_TIME); - spinnerWheelSim.update(Constants.SIM_UPDATE_TIME); - } - - public double getPosition() { - return pivotSimModel.getAngleRads(); - } - - public void stop() { - wheelAppliedVoltage = 0.0; - pivotAppliedVoltage = 0.0; - pivotSimModel.setInputVoltage(0); - spinnerWheelSim.setInputVoltage(0); - } - - public void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) { - outPivotController.setPID(outPIDConst.kP, outPIDConst.kI, outPIDConst.kD); - inPIDController.setPID(inPIPidConst.kP, inPIPidConst.kI, inPIPidConst.kD); - } + SingleJointedArmSim pivotSimModel; + DCMotorSim spinnerWheelSim; + + PIDController outPivotController; + PIDController inPIDController; + + PIDController pivotController; + + double wheelAppliedVoltage; + double pivotAppliedVoltage; + + double wheelSpeedpoint; + double pivotSetpoint; + + boolean usingInPID; + + public IntakeIOSim() { + pivotSimModel = + new SingleJointedArmSim( + DCMotor.getKrakenX60(Constants.Intake.NUM_PIVOT_MOTORS), + Constants.Intake.PIVOT_GEARING, // gearing + Constants.Intake.PIVOT_MOI, // MOI + Constants.Intake.PIVOT_LENGTH_METERS, // arm length + 0, // min angle -- hard stop + Constants.Intake.MAX_PIVOT_POSITION, // max angle -- floor + false, + 0); + + spinnerWheelSim = + new DCMotorSim( + DCMotor.getFalcon500(Constants.Intake.NUM_SPINNER_MOTORS), + Constants.Intake.SPINNER_GEARING, + Constants.Intake.SPINNER_MOI); + + outPivotController = new PIDController(0, 0, 0); + inPIDController = new PIDController(0, 0, 0); + } + + public void setSetpoints( + double pivotMotorSetPoint, double intakeMotorSetpoint, boolean useInPID) { + usingInPID = useInPID; + + if (useInPID) pivotController = inPIDController; + else pivotController = outPivotController; + + pivotAppliedVoltage = + pivotController.calculate(pivotSimModel.getAngleRads(), pivotMotorSetPoint); + + wheelAppliedVoltage = intakeMotorSetpoint; + + pivotSimModel.setInputVoltage(pivotAppliedVoltage); + spinnerWheelSim.setInputVoltage(intakeMotorSetpoint); + + wheelSpeedpoint = intakeMotorSetpoint; + pivotSetpoint = pivotMotorSetPoint; + } + + public void updateInputs(IntakeIOInputs inputs) { + inputs.wheelSpeed = spinnerWheelSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + inputs.wheelAppliedVoltage = wheelAppliedVoltage; + inputs.wheelSpeedpoint = wheelSpeedpoint; + + inputs.pivotPosition = pivotSimModel.getAngleRads(); + inputs.pivotAppliedVoltage = pivotAppliedVoltage; + inputs.pivotSetpoint = pivotSetpoint; + inputs.pivotSetpointError = Math.abs(pivotSimModel.getAngleRads() - pivotSetpoint); + + inputs.usingInPID = usingInPID; + + pivotSimModel.update(Constants.SIM_UPDATE_TIME); + spinnerWheelSim.update(Constants.SIM_UPDATE_TIME); + } + + public double getPosition() { + return pivotSimModel.getAngleRads(); + } + + public void stop() { + wheelAppliedVoltage = 0.0; + pivotAppliedVoltage = 0.0; + pivotSimModel.setInputVoltage(0); + spinnerWheelSim.setInputVoltage(0); + } + + public void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) { + outPivotController.setPID(outPIDConst.kP, outPIDConst.kI, outPIDConst.kD); + inPIDController.setPID(inPIPidConst.kP, inPIPidConst.kI, inPIPidConst.kD); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index f09ec0e..73c4f47 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -10,87 +10,81 @@ public class IntakeIOSparkMax implements IntakeIO { - private CANSparkMax pivotMotor; - public TalonFX intakeMotor; - private RelativeEncoder pivotEncoder; + private CANSparkMax pivotMotor; + public TalonFX intakeMotor; + private RelativeEncoder pivotEncoder; - PIDController outPivotController; - PIDController inPIDController; + PIDController outPivotController; + PIDController inPIDController; - PIDController pivotController; + PIDController pivotController; - double pivotMotorSetpoint = 0.0; - double intakeMotorSetpoint = 0.0; + double pivotMotorSetpoint = 0.0; + double intakeMotorSetpoint = 0.0; - double wheelAppliedVoltage; - double pivotAppliedVoltage; + double wheelAppliedVoltage; + double pivotAppliedVoltage; - double wheelSpeedpoint; - double pivotSetpoint; + double wheelSpeedpoint; + double pivotSetpoint; - boolean usingInPID; + boolean usingInPID; - public IntakeIOSparkMax() { - intakeMotor = new TalonFX(Constants.Intake.SPINNER_ID); - pivotMotor = new CANSparkMax(Constants.Intake.PIVOT_ID, MotorType.kBrushless); - pivotEncoder = pivotMotor.getEncoder(); + public IntakeIOSparkMax() { + intakeMotor = new TalonFX(Constants.Intake.SPINNER_ID); + pivotMotor = new CANSparkMax(Constants.Intake.PIVOT_ID, MotorType.kBrushless); + pivotEncoder = pivotMotor.getEncoder(); - outPivotController = new PIDController(0, 0, 0); - inPIDController = new PIDController(0, 0, 0); + outPivotController = new PIDController(0, 0, 0); + inPIDController = new PIDController(0, 0, 0); - pivotEncoder.setPositionConversionFactor(Constants.RADIAN_CF); - pivotEncoder.setVelocityConversionFactor(Constants.RADIAN_CF); - } + pivotEncoder.setPositionConversionFactor(Constants.RADIAN_CF); + pivotEncoder.setVelocityConversionFactor(Constants.RADIAN_CF); + } - public void setSetpoints( - double pivotMotorSetpoint, - double intakeMotorSetpoint, - boolean useInPID - ) { - usingInPID = useInPID; + public void setSetpoints( + double pivotMotorSetpoint, double intakeMotorSetpoint, boolean useInPID) { + usingInPID = useInPID; - if (useInPID) pivotController = inPIDController; - else pivotController = outPivotController; + if (useInPID) pivotController = inPIDController; + else pivotController = outPivotController; - pivotAppliedVoltage = pivotController.calculate( - pivotEncoder.getPosition(), - pivotMotorSetpoint - ); - wheelAppliedVoltage = intakeMotorSetpoint; + pivotAppliedVoltage = pivotController.calculate(pivotEncoder.getPosition(), pivotMotorSetpoint); + wheelAppliedVoltage = intakeMotorSetpoint; - pivotMotor.setVoltage(pivotAppliedVoltage); - intakeMotor.setVoltage(wheelAppliedVoltage); + pivotMotor.setVoltage(pivotAppliedVoltage); + intakeMotor.setVoltage(wheelAppliedVoltage); - wheelSpeedpoint = intakeMotorSetpoint; - pivotSetpoint = pivotMotorSetpoint; - } + wheelSpeedpoint = intakeMotorSetpoint; + pivotSetpoint = pivotMotorSetpoint; + } - public void updateInputs(IntakeIOInputs inputs) { - inputs.wheelSpeed = intakeMotor.getVelocity().getValueAsDouble(); - inputs.wheelAppliedVoltage = wheelAppliedVoltage; - inputs.wheelSpeedpoint = wheelSpeedpoint; + public void updateInputs(IntakeIOInputs inputs) { + inputs.wheelSpeed = intakeMotor.getVelocity().getValueAsDouble(); + inputs.wheelAppliedVoltage = wheelAppliedVoltage; + inputs.wheelSpeedpoint = wheelSpeedpoint; - inputs.pivotPosition = pivotEncoder.getPosition(); - inputs.pivotAppliedVoltage = pivotAppliedVoltage; - inputs.pivotSetpoint = pivotSetpoint; - inputs.pivotSetpointError = Math.abs(pivotEncoder.getPosition() - pivotSetpoint); + inputs.pivotPosition = pivotEncoder.getPosition(); + inputs.pivotAppliedVoltage = pivotAppliedVoltage; + inputs.pivotSetpoint = pivotSetpoint; + inputs.pivotSetpointError = Math.abs(pivotEncoder.getPosition() - pivotSetpoint); - inputs.usingInPID = usingInPID; - } + inputs.usingInPID = usingInPID; + } - public double getPosition() { - return pivotEncoder.getPosition(); - } + public double getPosition() { + return pivotEncoder.getPosition(); + } - public void stop() { - wheelAppliedVoltage = 0.0; - pivotAppliedVoltage = 0.0; - pivotMotor.stopMotor(); - intakeMotor.stopMotor(); - } + public void stop() { + wheelAppliedVoltage = 0.0; + pivotAppliedVoltage = 0.0; + pivotMotor.stopMotor(); + intakeMotor.stopMotor(); + } - public void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) { - outPivotController.setPID(outPIDConst.kP, outPIDConst.kI, outPIDConst.kD); - inPIDController.setPID(inPIPidConst.kP, inPIPidConst.kI, inPIPidConst.kD); - } + public void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) { + outPivotController.setPID(outPIDConst.kP, outPIDConst.kI, outPIDConst.kD); + inPIDController.setPID(inPIPidConst.kP, inPIPidConst.kI, inPIPidConst.kD); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeStates.java b/src/main/java/frc/robot/subsystems/intake/IntakeStates.java index 0537920..9c2a950 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeStates.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeStates.java @@ -4,41 +4,37 @@ import frc.robot.subsystems.*; public enum IntakeStates implements SubsystemStates { - OFF("Off", Constants.Intake.IN, Constants.Intake.OFF, true), - INTAKING("Intaking", Constants.Intake.DOWN, Constants.Intake.ON, false), - FEEDING("Feeding", Constants.Intake.IN, Constants.Intake.REVERSE, true), - OUTTAKING("Outtaking", Constants.Intake.DOWN, Constants.Intake.REVERSE, false); - - private String stateString; - private double pivotMotorSetpoint; - private double intakeMotorSetpoint; - private boolean useIn; - - IntakeStates( - String stateString, - double pivotMotorSetpoint, - double intakeMotorSetpoint, - boolean useIn - ) { - this.stateString = stateString; - this.pivotMotorSetpoint = pivotMotorSetpoint; - this.intakeMotorSetpoint = intakeMotorSetpoint; - this.useIn = useIn; - } - - public String getStateString() { - return this.stateString; - } - - public double getPivotSetPoint() { - return pivotMotorSetpoint; - } - - public double getMotorSetPoint() { - return intakeMotorSetpoint; - } - - public boolean getUsingPID() { - return useIn; - } + OFF("Off", Constants.Intake.IN, Constants.Intake.OFF, true), + INTAKING("Intaking", Constants.Intake.DOWN, Constants.Intake.ON, false), + FEEDING("Feeding", Constants.Intake.IN, Constants.Intake.REVERSE, true), + OUTTAKING("Outtaking", Constants.Intake.DOWN, Constants.Intake.REVERSE, false); + + private String stateString; + private double pivotMotorSetpoint; + private double intakeMotorSetpoint; + private boolean useIn; + + IntakeStates( + String stateString, double pivotMotorSetpoint, double intakeMotorSetpoint, boolean useIn) { + this.stateString = stateString; + this.pivotMotorSetpoint = pivotMotorSetpoint; + this.intakeMotorSetpoint = intakeMotorSetpoint; + this.useIn = useIn; + } + + public String getStateString() { + return this.stateString; + } + + public double getPivotSetPoint() { + return pivotMotorSetpoint; + } + + public double getMotorSetPoint() { + return intakeMotorSetpoint; + } + + public boolean getUsingPID() { + return useIn; + } } diff --git a/src/main/java/frc/robot/subsystems/manager/Manager.java b/src/main/java/frc/robot/subsystems/manager/Manager.java index abb210f..acfb22b 100644 --- a/src/main/java/frc/robot/subsystems/manager/Manager.java +++ b/src/main/java/frc/robot/subsystems/manager/Manager.java @@ -15,140 +15,141 @@ public class Manager extends Subsystem { - Intake intakeSubsystem; - AmpBar ampBarSubsystem; - Shooter shooterSubsystem; - Drive driveSubsystem; - - public Manager() { - super("Manager", ManagerStates.IDLE); - NoteSimulator.setDrive(driveSubsystem); - - switch (Constants.currentMode) { - case REAL: - intakeSubsystem = new Intake(new IntakeIOSparkMax()); - ampBarSubsystem = new AmpBar(new AmpBarIOReal()); - shooterSubsystem = new Shooter(new ShooterIOTalonFX()); - driveSubsystem = new Drive( - new GyroIOPigeon2(false), - new ModuleIOHybrid(0), - new ModuleIOHybrid(1), - new ModuleIOHybrid(2), - new ModuleIOHybrid(3) - ); - break; - case REPLAY: - intakeSubsystem = new Intake(new IntakeIO() {}); - ampBarSubsystem = new AmpBar(new AmpBarIO() {}); - shooterSubsystem = new Shooter(new ShooterIO() {}); - driveSubsystem = new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {} - ); - break; - case SIM: - intakeSubsystem = new Intake(new IntakeIOSim()); - ampBarSubsystem = new AmpBar(new AmpBarIOSim()); - shooterSubsystem = new Shooter(new ShooterIOSim()); - driveSubsystem = new Drive( - new GyroIO() {}, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim() - ); - break; - default: - break; - } - - NoteSimulator.setDrive(driveSubsystem); - - // State Transitions (Nothing Automatic YET) - - /* Generally each action has a specific button, in intermediary states X will return to idle and you press + Intake intakeSubsystem; + AmpBar ampBarSubsystem; + Shooter shooterSubsystem; + Drive driveSubsystem; + + public Manager() { + super("Manager", ManagerStates.IDLE); + NoteSimulator.setDrive(driveSubsystem); + + switch (Constants.currentMode) { + case REAL: + intakeSubsystem = new Intake(new IntakeIOSparkMax()); + ampBarSubsystem = new AmpBar(new AmpBarIOReal()); + shooterSubsystem = new Shooter(new ShooterIOTalonFX()); + driveSubsystem = + new Drive( + new GyroIOPigeon2(false), + new ModuleIOHybrid(0), + new ModuleIOHybrid(1), + new ModuleIOHybrid(2), + new ModuleIOHybrid(3)); + break; + case REPLAY: + intakeSubsystem = new Intake(new IntakeIO() {}); + ampBarSubsystem = new AmpBar(new AmpBarIO() {}); + shooterSubsystem = new Shooter(new ShooterIO() {}); + driveSubsystem = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); + break; + case SIM: + intakeSubsystem = new Intake(new IntakeIOSim()); + ampBarSubsystem = new AmpBar(new AmpBarIOSim()); + shooterSubsystem = new Shooter(new ShooterIOSim()); + driveSubsystem = + new Drive( + new GyroIO() {}, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + break; + default: + break; + } + + NoteSimulator.setDrive(driveSubsystem); + + // State Transitions (Nothing Automatic YET) + + /* Generally each action has a specific button, in intermediary states X will return to idle and you press the specific button to go through the states. Right now you can go through states without them being finished which should not be the case. Some state transitions that can be automated are also not automated. Most of the functions required for the TODOs are already build into the IOs!*/ - // TODO: Automatically return to idle after scoring amp (using a timer) - // TODO: Automatically current sense notes to return to idle after intaking - // TODO: Automatically go to the shooting state after spinning up if the shooting state is - // entered from main controller input - - // Intaking (B) - addTrigger(ManagerStates.IDLE, ManagerStates.INTAKING, () -> - Constants.controller.getBButtonPressed() - ); - addTrigger(ManagerStates.INTAKING, ManagerStates.IDLE, () -> - Constants.controller.getBButtonPressed() - ); - - // Amping (Y) - addTrigger(ManagerStates.IDLE, ManagerStates.FEED_AMP, () -> - Constants.controller.getYButtonPressed() - ); - addTrigger(ManagerStates.FEED_AMP, ManagerStates.SCORE_AMP, () -> - Constants.controller.getYButtonPressed() - ); - addTrigger(ManagerStates.SCORE_AMP, ManagerStates.IDLE, () -> - Constants.controller.getYButtonPressed() - ); - addTrigger(ManagerStates.FEED_AMP, ManagerStates.IDLE, () -> - Constants.controller.getXButtonPressed() - ); - - // Shooting (A) - addTrigger(ManagerStates.IDLE, ManagerStates.SPINNING_UP, () -> - Constants.controller.getAButtonPressed() - ); - addTrigger(ManagerStates.IDLE, ManagerStates.SPINNING_UP, () -> - Constants.operatorController.getAButtonPressed() - ); - addTrigger(ManagerStates.SPINNING_UP, ManagerStates.IDLE, () -> - Constants.controller.getXButtonPressed() - ); - addTrigger(ManagerStates.SPINNING_UP, ManagerStates.SHOOTING, () -> - Constants.controller.getAButtonPressed() - ); - addTrigger(ManagerStates.SHOOTING, ManagerStates.IDLE, () -> - Constants.controller.getAButtonPressed() - ); - } - - @Override - public void periodic() { - super.periodic(); - - intakeSubsystem.setState(getState().getIntakeState()); - ampBarSubsystem.setState(getState().getAmpBarState()); - shooterSubsystem.setState(getState().getShooterState()); - shooterSubsystem.setManagerState(getState()); - - intakeSubsystem.periodic(); - ampBarSubsystem.periodic(); - shooterSubsystem.periodic(); - driveSubsystem.periodic(); - - // Cancel all actions regardless of whats happening - if (Constants.operatorController.getXButtonPressed()) { - setState(ManagerStates.IDLE); - } - } - - public void stop() { - intakeSubsystem.stop(); - ampBarSubsystem.stop(); - shooterSubsystem.stop(); - driveSubsystem.stop(); - } - - @Override - protected void runState() { - NoteSimulator.update(); - NoteSimulator.logNoteInfo(); - } + // TODO: Automatically return to idle after scoring amp (using a timer) + // TODO: Automatically current sense notes to return to idle after intaking + // TODO: Automatically go to the shooting state after spinning up if the shooting state is + // entered from main controller input + + // Intaking (B) + addTrigger( + ManagerStates.IDLE, ManagerStates.INTAKING, () -> Constants.controller.getBButtonPressed()); + addTrigger( + ManagerStates.INTAKING, ManagerStates.IDLE, () -> Constants.controller.getBButtonPressed()); + + // Amping (Y) + addTrigger( + ManagerStates.IDLE, ManagerStates.FEED_AMP, () -> Constants.controller.getYButtonPressed()); + addTrigger( + ManagerStates.FEED_AMP, + ManagerStates.SCORE_AMP, + () -> Constants.controller.getYButtonPressed()); + addTrigger( + ManagerStates.SCORE_AMP, + ManagerStates.IDLE, + () -> Constants.controller.getYButtonPressed()); + addTrigger( + ManagerStates.FEED_AMP, ManagerStates.IDLE, () -> Constants.controller.getXButtonPressed()); + + // Shooting (A) + addTrigger( + ManagerStates.IDLE, + ManagerStates.SPINNING_UP, + () -> Constants.controller.getAButtonPressed()); + addTrigger( + ManagerStates.IDLE, + ManagerStates.SPINNING_UP, + () -> Constants.operatorController.getAButtonPressed()); + addTrigger( + ManagerStates.SPINNING_UP, + ManagerStates.IDLE, + () -> Constants.controller.getXButtonPressed()); + addTrigger( + ManagerStates.SPINNING_UP, + ManagerStates.SHOOTING, + () -> Constants.controller.getAButtonPressed()); + addTrigger( + ManagerStates.SHOOTING, ManagerStates.IDLE, () -> Constants.controller.getAButtonPressed()); + } + + @Override + public void periodic() { + super.periodic(); + + intakeSubsystem.setState(getState().getIntakeState()); + ampBarSubsystem.setState(getState().getAmpBarState()); + shooterSubsystem.setState(getState().getShooterState()); + shooterSubsystem.setManagerState(getState()); + + intakeSubsystem.periodic(); + ampBarSubsystem.periodic(); + shooterSubsystem.periodic(); + driveSubsystem.periodic(); + + // Cancel all actions regardless of whats happening + if (Constants.operatorController.getXButtonPressed()) { + setState(ManagerStates.IDLE); + } + } + + public void stop() { + intakeSubsystem.stop(); + ampBarSubsystem.stop(); + shooterSubsystem.stop(); + driveSubsystem.stop(); + } + + @Override + protected void runState() { + NoteSimulator.update(); + NoteSimulator.logNoteInfo(); + } } diff --git a/src/main/java/frc/robot/subsystems/manager/ManagerStates.java b/src/main/java/frc/robot/subsystems/manager/ManagerStates.java index 12a5114..f85b0f2 100644 --- a/src/main/java/frc/robot/subsystems/manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/manager/ManagerStates.java @@ -6,49 +6,44 @@ import frc.robot.subsystems.shooter.ShooterStates; public enum ManagerStates implements SubsystemStates { - IDLE("Idle", IntakeStates.OFF, AmpBarStates.OFF, ShooterStates.OFF), - INTAKING("Intaking", IntakeStates.INTAKING, AmpBarStates.OFF, ShooterStates.OFF), - SPINNING_UP("Spinning Up", IntakeStates.OFF, AmpBarStates.OFF, ShooterStates.SHOOTING), - SPINNING_AND_INTAKING( - "Spin and Intake", - IntakeStates.INTAKING, - AmpBarStates.OFF, - ShooterStates.SHOOTING - ), - SHOOTING("Shooting", IntakeStates.FEEDING, AmpBarStates.OFF, ShooterStates.SHOOTING), - FEED_AMP("Feed Amp", IntakeStates.FEEDING, AmpBarStates.FEEDING, ShooterStates.PASSING_AMP), - SCORE_AMP("Score Amp", IntakeStates.OFF, AmpBarStates.SHOOTING, ShooterStates.OFF); - - String stateString; - IntakeStates intakeState; - AmpBarStates ampBarState; - ShooterStates shooterState; - - ManagerStates( - String stateString, - IntakeStates intakeState, - AmpBarStates ampBarState, - ShooterStates shooterState - ) { - this.stateString = stateString; - this.intakeState = intakeState; - this.ampBarState = ampBarState; - this.shooterState = shooterState; - } - - public String getStateString() { - return this.stateString; - } - - public IntakeStates getIntakeState() { - return intakeState; - } - - public AmpBarStates getAmpBarState() { - return ampBarState; - } - - public ShooterStates getShooterState() { - return shooterState; - } + IDLE("Idle", IntakeStates.OFF, AmpBarStates.OFF, ShooterStates.OFF), + INTAKING("Intaking", IntakeStates.INTAKING, AmpBarStates.OFF, ShooterStates.OFF), + SPINNING_UP("Spinning Up", IntakeStates.OFF, AmpBarStates.OFF, ShooterStates.SHOOTING), + SPINNING_AND_INTAKING( + "Spin and Intake", IntakeStates.INTAKING, AmpBarStates.OFF, ShooterStates.SHOOTING), + SHOOTING("Shooting", IntakeStates.FEEDING, AmpBarStates.OFF, ShooterStates.SHOOTING), + FEED_AMP("Feed Amp", IntakeStates.FEEDING, AmpBarStates.FEEDING, ShooterStates.PASSING_AMP), + SCORE_AMP("Score Amp", IntakeStates.OFF, AmpBarStates.SHOOTING, ShooterStates.OFF); + + String stateString; + IntakeStates intakeState; + AmpBarStates ampBarState; + ShooterStates shooterState; + + ManagerStates( + String stateString, + IntakeStates intakeState, + AmpBarStates ampBarState, + ShooterStates shooterState) { + this.stateString = stateString; + this.intakeState = intakeState; + this.ampBarState = ampBarState; + this.shooterState = shooterState; + } + + public String getStateString() { + return this.stateString; + } + + public IntakeStates getIntakeState() { + return intakeState; + } + + public AmpBarStates getAmpBarState() { + return ampBarState; + } + + public ShooterStates getShooterState() { + return shooterState; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 77cb7c2..a19f2f7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -8,65 +8,62 @@ public class Shooter extends Subsystem { - private ShooterIO io; - private ManagerStates managerState; - ShooterIOInputsAutoLogged inputs; + private ShooterIO io; + private ManagerStates managerState; + ShooterIOInputsAutoLogged inputs; - public Shooter(ShooterIO io) { - super("Shooter", ShooterStates.OFF); - this.io = io; - inputs = new ShooterIOInputsAutoLogged(); + public Shooter(ShooterIO io) { + super("Shooter", ShooterStates.OFF); + this.io = io; + inputs = new ShooterIOInputsAutoLogged(); - switch (Constants.currentMode) { - case REAL: - io.configurePID( - Constants.Shooter.REAL_PID.kP, - Constants.Shooter.REAL_PID.kI, - Constants.Shooter.REAL_PID.kD - ); - break; - case REPLAY: - io.configurePID(0.0, 0.0, 0.0); - break; - case SIM: - io.configurePID( - Constants.Shooter.SIM_PID.kP, - Constants.Shooter.SIM_PID.kI, - Constants.Shooter.SIM_PID.kD - ); - break; - default: - break; - } - } + switch (Constants.currentMode) { + case REAL: + io.configurePID( + Constants.Shooter.REAL_PID.kP, + Constants.Shooter.REAL_PID.kI, + Constants.Shooter.REAL_PID.kD); + break; + case REPLAY: + io.configurePID(0.0, 0.0, 0.0); + break; + case SIM: + io.configurePID( + Constants.Shooter.SIM_PID.kP, + Constants.Shooter.SIM_PID.kI, + Constants.Shooter.SIM_PID.kD); + break; + default: + break; + } + } - @Override - protected void runState() { - io.setSpeed(getState().getMotorSpeedpoint()); + @Override + protected void runState() { + io.setSpeed(getState().getMotorSpeedpoint()); - if (managerState == ManagerStates.SHOOTING) { - NoteSimulator.launch( - io.getAverageSpeed() * Constants.Shooter.CIRCUMFRENCE_OF_SHOOTER_SPINNER - ); - } - } + if (managerState == ManagerStates.SHOOTING) { + NoteSimulator.launch( + io.getAverageSpeed() * Constants.Shooter.CIRCUMFRENCE_OF_SHOOTER_SPINNER); + } + } - public boolean nearSpeedPoint() { - return io.nearSpeedPoint(); - } + public boolean nearSpeedPoint() { + return io.nearSpeedPoint(); + } - public void stop() { - io.stop(); - } + public void stop() { + io.stop(); + } - @Override - public void periodic() { - super.periodic(); - io.updateInputs(inputs); - Logger.processInputs("Shooter", inputs); - } + @Override + public void periodic() { + super.periodic(); + io.updateInputs(inputs); + Logger.processInputs("Shooter", inputs); + } - public void setManagerState(ManagerStates state) { - managerState = state; - } + public void setManagerState(ManagerStates state) { + managerState = state; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index f396916..17a445f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -3,29 +3,29 @@ import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { - @AutoLog - public static class ShooterIOInputs { + @AutoLog + public static class ShooterIOInputs { - public double rightShooterSpeed = 0.0; - public double leftShooterSpeed = 0.0; - public double leftShooterAppliedVolts = 0.0; - public double rightShooterAppliedVolts = 0.0; - public double shooterSpeedPoint = 0.0; - } + public double rightShooterSpeed = 0.0; + public double leftShooterSpeed = 0.0; + public double leftShooterAppliedVolts = 0.0; + public double rightShooterAppliedVolts = 0.0; + public double shooterSpeedPoint = 0.0; + } - public default void updateInputs(ShooterIOInputs inputs) {} + public default void updateInputs(ShooterIOInputs inputs) {} - public default void setSpeed(double rps) {} + public default void setSpeed(double rps) {} - public default void stop() {} + public default void stop() {} - public default void configurePID(double kP, double kI, double kD) {} + public default void configurePID(double kP, double kI, double kD) {} - public default boolean nearSpeedPoint() { - return false; - } + public default boolean nearSpeedPoint() { + return false; + } - public default double getAverageSpeed() { - return 0.0; - } + public default double getAverageSpeed() { + return 0.0; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index 2f9690d..5f8f4c8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -7,60 +7,58 @@ public class ShooterIOSim implements ShooterIO { - FlywheelSim sim; - PIDController pid; - private double speedPoint; - private double appliedVolts; + FlywheelSim sim; + PIDController pid; + private double speedPoint; + private double appliedVolts; - public ShooterIOSim() { - sim = new FlywheelSim( - DCMotor.getFalcon500(Constants.Shooter.NUM_MOTORS), - Constants.Shooter.SHOOTER_GEARING, - Constants.Shooter.SHOOTER_MOI - ); - pid = new PIDController(0.0, 0.0, 0.0); - speedPoint = 0.0; - } + public ShooterIOSim() { + sim = + new FlywheelSim( + DCMotor.getFalcon500(Constants.Shooter.NUM_MOTORS), + Constants.Shooter.SHOOTER_GEARING, + Constants.Shooter.SHOOTER_MOI); + pid = new PIDController(0.0, 0.0, 0.0); + speedPoint = 0.0; + } - @Override - public void updateInputs(ShooterIOInputs inputs) { - sim.update(Constants.SIM_UPDATE_TIME); + @Override + public void updateInputs(ShooterIOInputs inputs) { + sim.update(Constants.SIM_UPDATE_TIME); - inputs.leftShooterSpeed = sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - inputs.rightShooterSpeed = sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - inputs.shooterSpeedPoint = speedPoint; - inputs.leftShooterAppliedVolts = appliedVolts; - inputs.rightShooterAppliedVolts = appliedVolts; - } + inputs.leftShooterSpeed = sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + inputs.rightShooterSpeed = sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + inputs.shooterSpeedPoint = speedPoint; + inputs.leftShooterAppliedVolts = appliedVolts; + inputs.rightShooterAppliedVolts = appliedVolts; + } - @Override - public void stop() { - appliedVolts = 0; - sim.setInputVoltage(appliedVolts); - } + @Override + public void stop() { + appliedVolts = 0; + sim.setInputVoltage(appliedVolts); + } - @Override - public void configurePID(double kP, double kI, double kD) { - pid.setPID(kP, kI, kD); - } + @Override + public void configurePID(double kP, double kI, double kD) { + pid.setPID(kP, kI, kD); + } - @Override - public void setSpeed(double rps) { - speedPoint = rps; - appliedVolts = pid.calculate(sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF, rps); - sim.setInputVoltage(appliedVolts); - } + @Override + public void setSpeed(double rps) { + speedPoint = rps; + appliedVolts = pid.calculate(sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF, rps); + sim.setInputVoltage(appliedVolts); + } - @Override - public boolean nearSpeedPoint() { - return ( - Math.abs((sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF) - speedPoint) > - Constants.Shooter.ERROR_OF_MARGIN - ); - } + @Override + public boolean nearSpeedPoint() { + return (Math.abs((sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF) - speedPoint) + > Constants.Shooter.ERROR_OF_MARGIN); + } - @Override - public double getAverageSpeed() { - return sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - } + @Override + public double getAverageSpeed() { + return sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index a32bad8..396d323 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -6,71 +6,63 @@ public class ShooterIOTalonFX implements ShooterIO { - private TalonFX leftMotor; - private TalonFX rightMotor; - private PIDController feedbackController; - private double speedPoint; - private double leftAppliedVolts; - private double rightAppliedVolts; + private TalonFX leftMotor; + private TalonFX rightMotor; + private PIDController feedbackController; + private double speedPoint; + private double leftAppliedVolts; + private double rightAppliedVolts; - public ShooterIOTalonFX() { - feedbackController = new PIDController(0, 0, 0); - leftMotor = new TalonFX(Constants.Shooter.LEFT_SHOOTER_ID); - rightMotor = new TalonFX(Constants.Shooter.RIGHT_SHOOTER_ID); - speedPoint = 0.0; - leftMotor.setInverted(false); - rightMotor.setInverted(true); - } + public ShooterIOTalonFX() { + feedbackController = new PIDController(0, 0, 0); + leftMotor = new TalonFX(Constants.Shooter.LEFT_SHOOTER_ID); + rightMotor = new TalonFX(Constants.Shooter.RIGHT_SHOOTER_ID); + speedPoint = 0.0; + leftMotor.setInverted(false); + rightMotor.setInverted(true); + } - public void updateInputs(ShooterIOInputs inputs) { - inputs.leftShooterAppliedVolts = leftAppliedVolts; - inputs.rightShooterAppliedVolts = rightAppliedVolts; - inputs.leftShooterSpeed = leftMotor.getVelocity().getValueAsDouble(); - inputs.rightShooterSpeed = rightMotor.getVelocity().getValueAsDouble(); - inputs.shooterSpeedPoint = speedPoint; - } + public void updateInputs(ShooterIOInputs inputs) { + inputs.leftShooterAppliedVolts = leftAppliedVolts; + inputs.rightShooterAppliedVolts = rightAppliedVolts; + inputs.leftShooterSpeed = leftMotor.getVelocity().getValueAsDouble(); + inputs.rightShooterSpeed = rightMotor.getVelocity().getValueAsDouble(); + inputs.shooterSpeedPoint = speedPoint; + } - public void setSpeed(double rps) { - speedPoint = rps; - leftAppliedVolts = feedbackController.calculate( - leftMotor.getRotorVelocity().getValueAsDouble(), - rps - ); - rightAppliedVolts = feedbackController.calculate( - rightMotor.getRotorVelocity().getValueAsDouble(), - rps - ); + public void setSpeed(double rps) { + speedPoint = rps; + leftAppliedVolts = + feedbackController.calculate(leftMotor.getRotorVelocity().getValueAsDouble(), rps); + rightAppliedVolts = + feedbackController.calculate(rightMotor.getRotorVelocity().getValueAsDouble(), rps); - leftMotor.setVoltage(leftAppliedVolts); - rightMotor.setVoltage(rightAppliedVolts); - } + leftMotor.setVoltage(leftAppliedVolts); + rightMotor.setVoltage(rightAppliedVolts); + } - public void stop() { - leftAppliedVolts = 0.0; - rightAppliedVolts = 0.0; - leftMotor.stopMotor(); - rightMotor.stopMotor(); - } + public void stop() { + leftAppliedVolts = 0.0; + rightAppliedVolts = 0.0; + leftMotor.stopMotor(); + rightMotor.stopMotor(); + } - public void configurePID(double kP, double kI, double kD) { - feedbackController.setPID(kP, kI, kD); - } + public void configurePID(double kP, double kI, double kD) { + feedbackController.setPID(kP, kI, kD); + } - public boolean nearSpeedPoint() { - return ( - Math.abs(speedPoint - leftMotor.getVelocity().getValueAsDouble()) < - Constants.Shooter.ERROR_OF_MARGIN && - Math.abs(speedPoint - rightMotor.getVelocity().getValueAsDouble()) < - Constants.Shooter.ERROR_OF_MARGIN - ); - } + public boolean nearSpeedPoint() { + return (Math.abs(speedPoint - leftMotor.getVelocity().getValueAsDouble()) + < Constants.Shooter.ERROR_OF_MARGIN + && Math.abs(speedPoint - rightMotor.getVelocity().getValueAsDouble()) + < Constants.Shooter.ERROR_OF_MARGIN); + } - @Override - public double getAverageSpeed() { - return ( - (leftMotor.getVelocity().getValueAsDouble() + - rightMotor.getVelocity().getValueAsDouble()) / - Constants.AVG_TWO_ITEM_F - ); - } + @Override + public double getAverageSpeed() { + return ((leftMotor.getVelocity().getValueAsDouble() + + rightMotor.getVelocity().getValueAsDouble()) + / Constants.AVG_TWO_ITEM_F); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterStates.java b/src/main/java/frc/robot/subsystems/shooter/ShooterStates.java index 17aceeb..4a775a0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterStates.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterStates.java @@ -4,24 +4,24 @@ import frc.robot.subsystems.SubsystemStates; public enum ShooterStates implements SubsystemStates { - OFF(Constants.Shooter.OFF, "Shooter Off"), - SHOOTING(Constants.Shooter.SHOOTING, "Shooting Full"), - PASSING_AMP(Constants.Shooter.FEEDING_AMP, "Passing To Amp"); + OFF(Constants.Shooter.OFF, "Shooter Off"), + SHOOTING(Constants.Shooter.SHOOTING, "Shooting Full"), + PASSING_AMP(Constants.Shooter.FEEDING_AMP, "Passing To Amp"); - private double speedPoint; - private String stateString; + private double speedPoint; + private String stateString; - ShooterStates(double speedPoint, String stateString) { - this.speedPoint = speedPoint; - this.stateString = stateString; - } + ShooterStates(double speedPoint, String stateString) { + this.speedPoint = speedPoint; + this.stateString = stateString; + } - public double getMotorSpeedpoint() { - return speedPoint; - } + public double getMotorSpeedpoint() { + return speedPoint; + } - @Override - public String getStateString() { - return stateString; - } + @Override + public String getStateString() { + return stateString; + } } diff --git a/src/main/java/frc/robot/util/FFConstants.java b/src/main/java/frc/robot/util/FFConstants.java index 6ab4eb2..7c72086 100644 --- a/src/main/java/frc/robot/util/FFConstants.java +++ b/src/main/java/frc/robot/util/FFConstants.java @@ -2,11 +2,11 @@ public class FFConstants { - public double kS; - public double kV; + public double kS; + public double kV; - public FFConstants(double ks, double kv) { - this.kS = ks; - this.kV = kv; - } + public FFConstants(double ks, double kv) { + this.kS = ks; + this.kV = kv; + } } diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index 8694cb9..5100a26 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -20,142 +20,136 @@ public class LocalADStarAK implements Pathfinder { - private final ADStarIO io = new ADStarIO(); - - /** - * Get if a new path has been calculated since the last time a path was retrieved - * - * @return True if a new path is available - */ - @Override - public boolean isNewPathAvailable() { - if (!Logger.hasReplaySource()) { - io.updateIsNewPathAvailable(); - } - - Logger.processInputs("LocalADStarAK", io); - - return io.isNewPathAvailable; - } - - /** - * Get the most recently calculated path - * - * @param constraints The path constraints to use when creating the path - * @param goalEndState The goal end state to use when creating the path - * @return The PathPlannerPath created from the points calculated by the pathfinder - */ - @Override - public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { - if (!Logger.hasReplaySource()) { - io.updateCurrentPathPoints(constraints, goalEndState); - } - - Logger.processInputs("LocalADStarAK", io); - - if (io.currentPathPoints.isEmpty()) { - return null; - } - - return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); - } - - /** - * Set the start position to pathfind from - * - * @param startPosition Start position on the field. If this is within an obstacle it will be - * moved to the nearest non-obstacle node. - */ - @Override - public void setStartPosition(Translation2d startPosition) { - if (!Logger.hasReplaySource()) { - io.adStar.setStartPosition(startPosition); - } - } - - /** - * Set the goal position to pathfind to - * - * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved - * to the nearest non-obstacle node. - */ - @Override - public void setGoalPosition(Translation2d goalPosition) { - if (!Logger.hasReplaySource()) { - io.adStar.setGoalPosition(goalPosition); - } - } - - /** - * Set the dynamic obstacles that should be avoided while pathfinding. - * - * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents - * opposite corners of a bounding box. - * @param currentRobotPos The current position of the robot. This is needed to change the start - * position of the path to properly avoid obstacles - */ - @Override - public void setDynamicObstacles( - List> obs, - Translation2d currentRobotPos - ) { - if (!Logger.hasReplaySource()) { - io.adStar.setDynamicObstacles(obs, currentRobotPos); - } - } - - private static class ADStarIO implements LoggableInputs { - - public LocalADStar adStar = new LocalADStar(); - public boolean isNewPathAvailable = false; - public List currentPathPoints = Collections.emptyList(); - - @Override - public void toLog(LogTable table) { - table.put("IsNewPathAvailable", isNewPathAvailable); - - double[] pointsLogged = new double[currentPathPoints.size() * 2]; - int idx = 0; - for (PathPoint point : currentPathPoints) { - pointsLogged[idx] = point.position.getX(); - pointsLogged[idx + 1] = point.position.getY(); - idx += 2; - } - - table.put("CurrentPathPoints", pointsLogged); - } - - @Override - public void fromLog(LogTable table) { - isNewPathAvailable = table.get("IsNewPathAvailable", false); - - double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); - - 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) - ); - } - - currentPathPoints = pathPoints; - } - - public void updateIsNewPathAvailable() { - isNewPathAvailable = adStar.isNewPathAvailable(); - } - - public void updateCurrentPathPoints( - PathConstraints constraints, - GoalEndState goalEndState - ) { - PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); - - if (currentPath != null) { - currentPathPoints = currentPath.getAllPathPoints(); - } else { - currentPathPoints = Collections.emptyList(); - } - } - } + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + 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)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } } diff --git a/src/main/java/frc/robot/util/NoteSimulator.java b/src/main/java/frc/robot/util/NoteSimulator.java index 0361c1c..ccebc59 100644 --- a/src/main/java/frc/robot/util/NoteSimulator.java +++ b/src/main/java/frc/robot/util/NoteSimulator.java @@ -12,110 +12,100 @@ public class NoteSimulator { - private static Drive drive; - - private static Pose3d currentFieldPose = new Pose3d(); - private static Translation3d fieldVelocity = new Translation3d(); - private static boolean inShooter = false; - private static List noteTrajectory = new ArrayList<>(); - - public static void setDrive(Drive drivesystem) { - drive = drivesystem; - } - - public static void attachToShooter() { - inShooter = true; - noteTrajectory.clear(); - } - - public static boolean isAttached() { - return inShooter; - } - - public static List getNoteTrajectory() { - return noteTrajectory; - } - - public static void launch(double velocity) { - if (!inShooter) { - return; - } - - Logger.recordOutput("Launch Velocity", velocity); - - currentFieldPose = getFieldPose(Constants.NoteSim.SHOOTER_POSE3D); - inShooter = false; - - fieldVelocity = new Translation3d(velocity, currentFieldPose.getRotation()); - - ChassisSpeeds robotVel = drive.getChassisSpeed(); - ChassisSpeeds fieldRel = ChassisSpeeds.fromRobotRelativeSpeeds( - robotVel, - drive.getRotation() - ); - - fieldVelocity = fieldVelocity.plus( - new Translation3d(fieldRel.vxMetersPerSecond, fieldRel.vyMetersPerSecond, 0.0) - ); - } - - public static Pose3d getFieldPose(Pose3d shooterPose) { - if (inShooter) { - return new Pose3d(drive.getPose()).transformBy( - new Transform3d(shooterPose.getTranslation(), shooterPose.getRotation()) - ); - } - - return currentFieldPose; - } - - public static void update() { - if (inShooter) { - return; - } - - Translation3d posDelta = fieldVelocity.times(Constants.NoteSim.dt); - - currentFieldPose = new Pose3d( - currentFieldPose.getTranslation().plus(posDelta), - currentFieldPose.getRotation() - ); - - if ( - currentFieldPose.getX() <= -Constants.NoteSim.OUT_OF_FIELD_MARGIN || - currentFieldPose.getX() >= - Constants.NoteSim.FIELD_SIZE.getX() + Constants.NoteSim.OUT_OF_FIELD_MARGIN || - currentFieldPose.getY() <= -Constants.NoteSim.OUT_OF_FIELD_MARGIN || - currentFieldPose.getY() >= - Constants.NoteSim.FIELD_SIZE.getY() + Constants.NoteSim.OUT_OF_FIELD_MARGIN || - currentFieldPose.getZ() <= 0.0 - ) { - fieldVelocity = new Translation3d(); - } else { - fieldVelocity = fieldVelocity.minus( - Constants.NoteSim.GRAVITY_TRANSLATION3D.times(Constants.NoteSim.dt) - ); - double norm = fieldVelocity.getNorm(); - - double fDrag = - (Constants.NoteSim.AIR_DENSITY * - Math.pow(norm, 2) * - Constants.NoteSim.DRAG_COEFFICIENT * - Constants.NoteSim.CROSSECTION_AREA) / - Constants.AVG_TWO_ITEM_F; - double deltaV = (Constants.NoteSim.MASS * fDrag) * Constants.NoteSim.dt; - - double t = (norm - deltaV) / norm; - fieldVelocity = fieldVelocity.times(t); - noteTrajectory.add(currentFieldPose.getTranslation()); - } - } - - public static void logNoteInfo() { - Logger.recordOutput( - "SimNoteTrajectory", - NoteSimulator.getNoteTrajectory().toArray(new Translation3d[0]) - ); - Logger.recordOutput("SimNotePose3d", getFieldPose(Constants.NoteSim.SHOOTER_POSE3D)); - } + private static Drive drive; + + private static Pose3d currentFieldPose = new Pose3d(); + private static Translation3d fieldVelocity = new Translation3d(); + private static boolean inShooter = false; + private static List noteTrajectory = new ArrayList<>(); + + public static void setDrive(Drive drivesystem) { + drive = drivesystem; + } + + public static void attachToShooter() { + inShooter = true; + noteTrajectory.clear(); + } + + public static boolean isAttached() { + return inShooter; + } + + public static List getNoteTrajectory() { + return noteTrajectory; + } + + public static void launch(double velocity) { + if (!inShooter) { + return; + } + + Logger.recordOutput("Launch Velocity", velocity); + + currentFieldPose = getFieldPose(Constants.NoteSim.SHOOTER_POSE3D); + inShooter = false; + + fieldVelocity = new Translation3d(velocity, currentFieldPose.getRotation()); + + ChassisSpeeds robotVel = drive.getChassisSpeed(); + ChassisSpeeds fieldRel = ChassisSpeeds.fromRobotRelativeSpeeds(robotVel, drive.getRotation()); + + fieldVelocity = + fieldVelocity.plus( + new Translation3d(fieldRel.vxMetersPerSecond, fieldRel.vyMetersPerSecond, 0.0)); + } + + public static Pose3d getFieldPose(Pose3d shooterPose) { + if (inShooter) { + return new Pose3d(drive.getPose()) + .transformBy(new Transform3d(shooterPose.getTranslation(), shooterPose.getRotation())); + } + + return currentFieldPose; + } + + public static void update() { + if (inShooter) { + return; + } + + Translation3d posDelta = fieldVelocity.times(Constants.NoteSim.dt); + + currentFieldPose = + new Pose3d( + currentFieldPose.getTranslation().plus(posDelta), currentFieldPose.getRotation()); + + if (currentFieldPose.getX() <= -Constants.NoteSim.OUT_OF_FIELD_MARGIN + || currentFieldPose.getX() + >= Constants.NoteSim.FIELD_SIZE.getX() + Constants.NoteSim.OUT_OF_FIELD_MARGIN + || currentFieldPose.getY() <= -Constants.NoteSim.OUT_OF_FIELD_MARGIN + || currentFieldPose.getY() + >= Constants.NoteSim.FIELD_SIZE.getY() + Constants.NoteSim.OUT_OF_FIELD_MARGIN + || currentFieldPose.getZ() <= 0.0) { + fieldVelocity = new Translation3d(); + } else { + fieldVelocity = + fieldVelocity.minus(Constants.NoteSim.GRAVITY_TRANSLATION3D.times(Constants.NoteSim.dt)); + double norm = fieldVelocity.getNorm(); + + double fDrag = + (Constants.NoteSim.AIR_DENSITY + * Math.pow(norm, 2) + * Constants.NoteSim.DRAG_COEFFICIENT + * Constants.NoteSim.CROSSECTION_AREA) + / Constants.AVG_TWO_ITEM_F; + double deltaV = (Constants.NoteSim.MASS * fDrag) * Constants.NoteSim.dt; + + double t = (norm - deltaV) / norm; + fieldVelocity = fieldVelocity.times(t); + noteTrajectory.add(currentFieldPose.getTranslation()); + } + } + + public static void logNoteInfo() { + Logger.recordOutput( + "SimNoteTrajectory", NoteSimulator.getNoteTrajectory().toArray(new Translation3d[0])); + Logger.recordOutput("SimNotePose3d", getFieldPose(Constants.NoteSim.SHOOTER_POSE3D)); + } } From b601bae4d94dead0d85cbf412eac254e4c3bda15 Mon Sep 17 00:00:00 2001 From: Otto Date: Sat, 3 Aug 2024 23:52:22 -0400 Subject: [PATCH 2/6] Working --- src/main/java/frc/robot/Robot.java | 2 + .../frc/robot/subsystems/drive/Drive.java | 39 ++++++++++++------- .../subsystems/drive/PPDriveWrapper.java | 29 +++++++++++--- 3 files changed, 52 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5592fbe..47a9e18 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -86,6 +86,8 @@ public void robotInit() { case SIM: // Running a physics simulator, log to NT Logger.addDataReceiver(new NT4Publisher()); + // Logger.addDataReceiver(new WPILOGWriter("C:\\Dev\\Robotics + // Related\\Controls\\2024-Rewrite\\logs", defaultPeriodSecs)); break; case REPLAY: // Replaying a log, set up replay source diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 18a3f43..169838c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -35,12 +35,11 @@ import org.littletonrobotics.junction.Logger; public class Drive extends Subsystem { - static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[Constants.Drive.NUM_MODULES]; // FL, FR, BL, BR - private PPDriveWrapper ppConfig; + private PPDriveWrapper autoConfig; private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); @@ -49,7 +48,7 @@ public class Drive extends Subsystem { new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition(), - new SwerveModulePosition(), + new SwerveModulePosition() }; private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); @@ -60,9 +59,9 @@ public Drive( ModuleIO frModuleIO, ModuleIO blModuleIO, ModuleIO brModuleIO) { - super("Drive", DriveStates.REGULAR_DRIVE); - ppConfig = new PPDriveWrapper(this); + super("Drive", DriveStates.REGULAR_DRIVE); + autoConfig = new PPDriveWrapper(this); this.gyroIO = gyroIO; modules[0] = new Module(flModuleIO, 0); @@ -73,16 +72,30 @@ public Drive( // Start threads (no-op for each if no signals have been created) PhoenixOdometryThread.getInstance().start(); SparkMaxOdometryThread.getInstance().start(); + + // Triggers + addTrigger( + DriveStates.REGULAR_DRIVE, + DriveStates.SLOW_MODE, + () -> Constants.controller.getRightBumper()); + addTrigger( + DriveStates.REGULAR_DRIVE, + DriveStates.SPEED_MAXXING, + () -> Constants.controller.getLeftBumper()); } + @Override public void runState() { - drive( - this, - () -> Constants.controller.getLeftY(), - () -> Constants.controller.getLeftX(), - () -> -Constants.controller.getRightX(), - getState().getRotationModifier(), - getState().getTranslationModifier()); + // Can't run in auto otherwise it will constantly tell drive not to drive in auto (and thats not good) + if (DriverStation.isTeleop()) { + drive( + this, + () -> Constants.controller.getLeftY(), + () -> Constants.controller.getLeftX(), + () -> -Constants.controller.getRightX(), + getState().getRotationModifier(), + getState().getTranslationModifier()); + } } public void drive( @@ -308,7 +321,7 @@ public static Translation2d[] getModuleTranslations() { Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF), new Translation2d( -Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, - -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF), + -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF) }; } } diff --git a/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java b/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java index 0082d58..216939e 100644 --- a/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java +++ b/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java @@ -3,6 +3,8 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -15,13 +17,30 @@ public class PPDriveWrapper extends SubsystemBase { pathPlannerInit(); } + private Pose2d getPose() { + return drive.getPose(); + } + + private void setPose(Pose2d pose) { + drive.setPose(pose); + } + + private ChassisSpeeds getChassisSpeed() { + return drive.getChassisSpeed(); + } + + private void runVelocity(ChassisSpeeds chassisSpeeds) { + drive.runVelocity(chassisSpeeds); + } + + public void periodic() {} + public void pathPlannerInit() { AutoBuilder.configureHolonomic( - drive::getPose, // Robot pose supplier - drive - ::setPose, // Method to reset odometry (will be called if your auto has a starting pose) - drive::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - drive::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + this::getPose, // Robot pose supplier + this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in // your Constants class From 0e76109d39bbde09801d56ec03df23fd4fc41742 Mon Sep 17 00:00:00 2001 From: github-actions Date: Sun, 4 Aug 2024 03:53:34 +0000 Subject: [PATCH 3/6] Apply Prettier format --- src/main/java/frc/robot/Constants.java | 463 +++++++------- src/main/java/frc/robot/Main.java | 18 +- src/main/java/frc/robot/Robot.java | 284 +++++---- .../java/frc/robot/commands/AutoCommands.java | 68 +- .../java/frc/robot/subsystems/Subsystem.java | 152 ++--- .../frc/robot/subsystems/SubsystemStates.java | 2 +- .../java/frc/robot/subsystems/Trigger.java | 24 +- .../frc/robot/subsystems/ampBar/AmpBar.java | 104 +-- .../frc/robot/subsystems/ampBar/AmpBarIO.java | 52 +- .../robot/subsystems/ampBar/AmpBarIOReal.java | 192 +++--- .../robot/subsystems/ampBar/AmpBarIOSim.java | 194 +++--- .../robot/subsystems/ampBar/AmpBarStates.java | 44 +- .../frc/robot/subsystems/drive/Drive.java | 602 +++++++++--------- .../robot/subsystems/drive/DriveStates.java | 58 +- .../frc/robot/subsystems/drive/GyroIO.java | 18 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 88 +-- .../frc/robot/subsystems/drive/Module.java | 402 ++++++------ .../frc/robot/subsystems/drive/ModuleIO.java | 50 +- .../subsystems/drive/ModuleIOHybrid.java | 357 ++++++----- .../robot/subsystems/drive/ModuleIOSim.java | 88 +-- .../subsystems/drive/ModuleIOSparkMax.java | 325 +++++----- .../subsystems/drive/ModuleIOTalonFX.java | 357 ++++++----- .../subsystems/drive/PPDriveWrapper.java | 101 ++- .../drive/PhoenixOdometryThread.java | 198 +++--- .../drive/SparkMaxOdometryThread.java | 144 +++-- .../frc/robot/subsystems/intake/Intake.java | 102 +-- .../frc/robot/subsystems/intake/IntakeIO.java | 43 +- .../robot/subsystems/intake/IntakeIOSim.java | 181 +++--- .../subsystems/intake/IntakeIOSparkMax.java | 120 ++-- .../robot/subsystems/intake/IntakeStates.java | 70 +- .../frc/robot/subsystems/manager/Manager.java | 265 ++++---- .../subsystems/manager/ManagerStates.java | 85 +-- .../frc/robot/subsystems/shooter/Shooter.java | 103 +-- .../robot/subsystems/shooter/ShooterIO.java | 36 +- .../subsystems/shooter/ShooterIOSim.java | 94 +-- .../subsystems/shooter/ShooterIOTalonFX.java | 110 ++-- .../subsystems/shooter/ShooterStates.java | 32 +- src/main/java/frc/robot/util/FFConstants.java | 12 +- .../java/frc/robot/util/LocalADStarAK.java | 270 ++++---- .../java/frc/robot/util/NoteSimulator.java | 202 +++--- 40 files changed, 3132 insertions(+), 2978 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index aaf9a61..4a130bc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -32,283 +32,292 @@ */ public final class Constants { - public static final double SIM_UPDATE_TIME = 0.05; + public static final double SIM_UPDATE_TIME = 0.05; - // Conversion Factors - public static final double RADIAN_CF = (Math.PI * 2); - public static final double RPM_TO_RPS_CF = 60; - public static final double DIAM_TO_RADIUS_CF = 2.0; - public static final double AVG_TWO_ITEM_F = 2.0; + // Conversion Factors + public static final double RADIAN_CF = (Math.PI * 2); + public static final double RPM_TO_RPS_CF = 60; + public static final double DIAM_TO_RADIUS_CF = 2.0; + public static final double AVG_TWO_ITEM_F = 2.0; - public static final Mode currentMode = Mode.SIM; + public static final Mode currentMode = Mode.SIM; - public static final double MAX_VOLTS = 12.0; - public static final double MIN_VOLTS = -12.0; + public static final double MAX_VOLTS = 12.0; + public static final double MIN_VOLTS = -12.0; - public static final XboxController controller = new XboxController(0); - public static final XboxController operatorController = new XboxController(1); + public static final XboxController controller = new XboxController(0); + public static final XboxController operatorController = new XboxController(1); - public static enum Mode { - /** Running on a real robot. */ - REAL, + public static enum Mode { + /** Running on a real robot. */ + REAL, - /** Running a physics simulator. */ - SIM, + /** Running a physics simulator. */ + SIM, - /** Replaying from a log file. */ - REPLAY, - } + /** Replaying from a log file. */ + REPLAY, + } - public static final class Intake { + public static final class Intake { - public static final Translation3d ZEROED_PIVOT_TRANSLATION = new Translation3d(0.31, 0, 0.24); + public static final Translation3d ZEROED_PIVOT_TRANSLATION = new Translation3d( + 0.31, + 0, + 0.24 + ); - // CAN IDs - public static final int PIVOT_ID = 32; - public static final int SPINNER_ID = 20; + // CAN IDs + public static final int PIVOT_ID = 32; + public static final int SPINNER_ID = 20; - // PID - public static final PIDConstants SIM_IN_PID = new PIDConstants(1, 0, 0); - public static final PIDConstants SIM_OUT_PID = new PIDConstants(1, 0, 0); + // PID + public static final PIDConstants SIM_IN_PID = new PIDConstants(1, 0, 0); + public static final PIDConstants SIM_OUT_PID = new PIDConstants(1, 0, 0); - public static final PIDConstants REAL_IN_PID = new PIDConstants(1, 0, 0); - public static final PIDConstants REAL_OUT_PID = new PIDConstants(1, 0, 0); + public static final PIDConstants REAL_IN_PID = new PIDConstants(1, 0, 0); + public static final PIDConstants REAL_OUT_PID = new PIDConstants(1, 0, 0); - // Sim Configs + // Sim Configs - // Pivot - public static final int NUM_PIVOT_MOTORS = 1; - public static final double PIVOT_GEARING = 67.5; - public static final double MAX_PIVOT_POSITION = Units.degreesToRadians(180.0); - public static final double PIVOT_MOI = 0.192383865; - public static final double PIVOT_LENGTH_METERS = 0.3; + // Pivot + public static final int NUM_PIVOT_MOTORS = 1; + public static final double PIVOT_GEARING = 67.5; + public static final double MAX_PIVOT_POSITION = Units.degreesToRadians(180.0); + public static final double PIVOT_MOI = 0.192383865; + public static final double PIVOT_LENGTH_METERS = 0.3; - // Spinner - public static final int NUM_SPINNER_MOTORS = 1; - public static final double SPINNER_MOI = 0.01; - public static final double SPINNER_GEARING = 1.0; - public static final double OFF = 0.0; + // Spinner + public static final int NUM_SPINNER_MOTORS = 1; + public static final double SPINNER_MOI = 0.01; + public static final double SPINNER_GEARING = 1.0; + public static final double OFF = 0.0; - // In Rads (Pivot setpoints) - public static final double DOWN = Units.degreesToRadians(180.0); - public static final double IN = Units.degreesToRadians(0.0); + // In Rads (Pivot setpoints) + public static final double DOWN = Units.degreesToRadians(180.0); + public static final double IN = Units.degreesToRadians(0.0); - // In RPS (Spinner Setpoints) - public static final double REVERSE = -10.0; - public static final double ON = 10.0; - } + // In RPS (Spinner Setpoints) + public static final double REVERSE = -10.0; + public static final double ON = 10.0; + } - public static final class Shooter { + public static final class Shooter { - public static final double ERROR_OF_MARGIN = 2.0; + public static final double ERROR_OF_MARGIN = 2.0; - // PID - public static final PIDConstants SIM_PID = new PIDConstants(1, 0, 0); - public static final PIDConstants REAL_PID = new PIDConstants(1, 0, 0); + // PID + public static final PIDConstants SIM_PID = new PIDConstants(1, 0, 0); + public static final PIDConstants REAL_PID = new PIDConstants(1, 0, 0); - // CAN IDs - public static final int LEFT_SHOOTER_ID = 15; - public static final int RIGHT_SHOOTER_ID = 14; + // CAN IDs + public static final int LEFT_SHOOTER_ID = 15; + public static final int RIGHT_SHOOTER_ID = 14; - // Shooter Setpoints (RPS) - public static final double OFF = 0.0; - public static final double FEEDING_AMP = 25.0; - public static final double SHOOTING = 50.0; + // Shooter Setpoints (RPS) + public static final double OFF = 0.0; + public static final double FEEDING_AMP = 25.0; + public static final double SHOOTING = 50.0; - // Sim Configs - public static final int NUM_MOTORS = 2; - public static final double SHOOTER_GEARING = 1.5; - public static final double SHOOTER_MOI = 0.004; + // Sim Configs + public static final int NUM_MOTORS = 2; + public static final double SHOOTER_GEARING = 1.5; + public static final double SHOOTER_MOI = 0.004; - // spinner circumfrence need to check with mech - public static final double CIRCUMFRENCE_OF_SHOOTER_SPINNER = 4; - } + // spinner circumfrence need to check with mech + public static final double CIRCUMFRENCE_OF_SHOOTER_SPINNER = 4; + } - public static final class AmpBar { + public static final class AmpBar { - public static final Translation3d ZEROED_PIVOT_TRANSLATION = - new Translation3d(-0.317, 0, 0.525); + public static final Translation3d ZEROED_PIVOT_TRANSLATION = new Translation3d( + -0.317, + 0, + 0.525 + ); - public static final double ERROR_OF_MARGIN = 0.1; + public static final double ERROR_OF_MARGIN = 0.1; - // PID - public static final PIDConstants SIM_PID = new PIDConstants(3, 0, 1.5); - public static final PIDConstants REAL_PID = new PIDConstants(1, 0, 0); + // PID + public static final PIDConstants SIM_PID = new PIDConstants(3, 0, 1.5); + public static final PIDConstants REAL_PID = new PIDConstants(1, 0, 0); - // Motor CAN IDs - public static final int LEFT_PIVOT_ID = 31; - public static final int RIGHT_PIVOT_ID = 30; - public static final int SPINNER_ID = 38; + // Motor CAN IDs + public static final int LEFT_PIVOT_ID = 31; + public static final int RIGHT_PIVOT_ID = 30; + public static final int SPINNER_ID = 38; - // Sim Configs + // Sim Configs - // Spinner - public static final double SPINNER_GEARING = 0.5; - public static final double SPINNER_MOI = 0.5; - public static final int NUM_SPINNER_MOTORS = 1; + // Spinner + public static final double SPINNER_GEARING = 0.5; + public static final double SPINNER_MOI = 0.5; + public static final int NUM_SPINNER_MOTORS = 1; - // Pivot - public static final int NUM_PIVOT_MOTORS = 2; - public static final double PIVOT_GEARING = 0.05; - public static final double PIVOT_MOI = 0.05; - public static final double PIVOT_LENGTH_METERS = 0.378; - public static final double MIN_PIVOT_POSITION = -Units.degreesToRadians(114.163329); - public static final double MAX_PIVOT_POSITION = Units.degreesToRadians(0); + // Pivot + public static final int NUM_PIVOT_MOTORS = 2; + public static final double PIVOT_GEARING = 0.05; + public static final double PIVOT_MOI = 0.05; + public static final double PIVOT_LENGTH_METERS = 0.378; + public static final double MIN_PIVOT_POSITION = -Units.degreesToRadians(114.163329); + public static final double MAX_PIVOT_POSITION = Units.degreesToRadians(0); + + // Pivot and Spinner Setpoints - // Pivot and Spinner Setpoints + // In RPS (Spinner Setpoints) + public static final double SHOOTING = -0.5; + public static final double FEEDING = -0.1; + public static final double OFF = 0.0; - // In RPS (Spinner Setpoints) - public static final double SHOOTING = -0.5; - public static final double FEEDING = -0.1; - public static final double OFF = 0.0; + // IN Rads (Pivot Setpoints) + public static final double OUT = -Units.degreesToRadians(100.0); + public static final double FEEDING_POSITION = -Units.degreesToRadians(93.0); + public static final double IN = Units.degreesToRadians(0.0); + } - // IN Rads (Pivot Setpoints) - public static final double OUT = -Units.degreesToRadians(100.0); - public static final double FEEDING_POSITION = -Units.degreesToRadians(93.0); - public static final double IN = Units.degreesToRadians(0.0); - } + public static final class Drive { - public static final class Drive { - - public static final double DISCRETIZE_TIME_SECONDS = 0.02; - public static final double CONTROLLER_DEADBAND = 0.1; - public static final int NUM_MODULES = 4; - - /* Rotation and Translation Modifers + public static final double DISCRETIZE_TIME_SECONDS = 0.02; + public static final double CONTROLLER_DEADBAND = 0.1; + public static final int NUM_MODULES = 4; + + /* Rotation and Translation Modifers rm = rotation multiplier tm = translation multipliers aa = auto align */ - public static final double REGULAR_RM = 1.0; - public static final double REGULAR_TM = 1.0; - public static final double SLOW_RM = 0.5; - public static final double SLOW_TM = 0.2; - public static final double AA_RM = 0.8; - public static final double AA_TM = 0.8; - public static final double FAST_RM = 1.5; - public static final double FAST_TM = 2.0; - - // Auto Config - public static final PIDConstants TRANSLATION_PID = new PIDConstants(5.0, 0.0, 0.0); - public static final PIDConstants ROTATION_PID = new PIDConstants(5.0, 0.0, 0.0); - public static final double MAX_MODULE_SPEED = 6.0; - - // Configs - public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); - public static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); - public static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); - public static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); - public static final double DRIVE_BASE_RADIUS = - Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); - public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + public static final double REGULAR_RM = 1.0; + public static final double REGULAR_TM = 1.0; + public static final double SLOW_RM = 0.5; + public static final double SLOW_TM = 0.2; + public static final double AA_RM = 0.8; + public static final double AA_TM = 0.8; + public static final double FAST_RM = 1.5; + public static final double FAST_TM = 2.0; + + // Auto Config + public static final PIDConstants TRANSLATION_PID = new PIDConstants(5.0, 0.0, 0.0); + public static final PIDConstants ROTATION_PID = new PIDConstants(5.0, 0.0, 0.0); + public static final double MAX_MODULE_SPEED = 6.0; - public static final class Pidgeon2 { + // Configs + public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + public static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); + public static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); + public static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); + public static final double DRIVE_BASE_RADIUS = Math.hypot( + TRACK_WIDTH_X / 2.0, + TRACK_WIDTH_Y / 2.0 + ); + public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + + public static final class Pidgeon2 { - public static final int DEVICE_ID = 20; - public static final double UPDATE_FREQUENCY = 100.0; - } + public static final int DEVICE_ID = 20; + public static final double UPDATE_FREQUENCY = 100.0; + } - public static final class Module { + public static final class Module { + + public static final double ODOMETRY_FREQUENCY = 250.0; + + // There isnt one for real its just all 0 so idk whats good with that + public static final FFConstants REPLAY_FF = new FFConstants(0.1, 0.13); + public static final PIDConstants REPLAY_DRIVE_PID = new PIDConstants(0.05, 0.0, 0.0); + public static final PIDConstants REPLAY_TURN_PID = new PIDConstants(7.0, 0.0, 0.0); + + public static final FFConstants SIM_FF = new FFConstants(0.0, 0.13); + public static final PIDConstants SIM_DRIVE_PID = new PIDConstants(0.1, 0.0, 0.0); + public static final PIDConstants SIM_TURN_PID = new PIDConstants(10.0, 0.0, 0.0); + + public static final int NUM_TURN_MOTORS = 1; + public static final int NUM_DRIVE_MOTORS = 1; + + public static final class Sim { + + public static final double LOOP_PERIOD_SECS = 0.02; + + // Configs + public static final double DRIVE_GEARING = 6.75; + public static final double DRIVE_MOI = 0.025; + + public static final double TURN_GEARING = 150.0 / 7.0; + public static final double TURN_MOI = 0.004; + } + + // TODO: Put constants from those abstractions in here + public static final class SparkMax {} + + public static final class TalonFX {} + + public static final class Hybrid { + + // These are for l2 Mk4i mods, should be L3 plus + public static final double DRIVE_GEAR_RATIO = + (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + public static final double TURN_GEAR_RATIO = 150.0 / 7.0; + + public static final double DRIVE_CURRENT_LIMIT = 40.0; + public static final int TURN_CURRENT_LIMIT = 30; + // Stuff + public static final double TALON_UPDATE_FREQUENCY_HZ = 50.0; + + public static final int SPARK_TIMEOUT_MS = 250; + public static final int SPARK_MEASURMENT_PERIOD_MS = 10; + public static final int SPARK_AVG_DEPTH = 2; + public static final double SPARK_FRAME_PERIOD = 1000.0 / ODOMETRY_FREQUENCY; + + // CAN/Device IDS and offsets + public static final int DRIVE0_ID = 0; + public static final int TURN0_ID = 1; + public static final int CANCODER0_ID = 2; + public static final double OFFSET0 = 0.0; + + public static final int DRIVE1_ID = 0; + public static final int TURN1_ID = 1; + public static final int CANCODER1_ID = 2; + public static final double OFFSET1 = 0.0; + + public static final int DRIVE2_ID = 0; + public static final int TURN2_ID = 1; + public static final int CANCODER2_ID = 2; + public static final double OFFSET2 = 0.0; + + public static final int DRIVE3_ID = 0; + public static final int TURN3_ID = 1; + public static final int CANCODER3_ID = 2; + public static final double OFFSET3 = 0.0; + } + } - public static final double ODOMETRY_FREQUENCY = 250.0; - - // There isnt one for real its just all 0 so idk whats good with that - public static final FFConstants REPLAY_FF = new FFConstants(0.1, 0.13); - public static final PIDConstants REPLAY_DRIVE_PID = new PIDConstants(0.05, 0.0, 0.0); - public static final PIDConstants REPLAY_TURN_PID = new PIDConstants(7.0, 0.0, 0.0); - - public static final FFConstants SIM_FF = new FFConstants(0.0, 0.13); - public static final PIDConstants SIM_DRIVE_PID = new PIDConstants(0.1, 0.0, 0.0); - public static final PIDConstants SIM_TURN_PID = new PIDConstants(10.0, 0.0, 0.0); - - public static final int NUM_TURN_MOTORS = 1; - public static final int NUM_DRIVE_MOTORS = 1; - - public static final class Sim { - - public static final double LOOP_PERIOD_SECS = 0.02; - - // Configs - public static final double DRIVE_GEARING = 6.75; - public static final double DRIVE_MOI = 0.025; - - public static final double TURN_GEARING = 150.0 / 7.0; - public static final double TURN_MOI = 0.004; - } - - // TODO: Put constants from those abstractions in here - public static final class SparkMax {} - - public static final class TalonFX {} - - public static final class Hybrid { - - // These are for l2 Mk4i mods, should be L3 plus - public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - public static final double TURN_GEAR_RATIO = 150.0 / 7.0; - - public static final double DRIVE_CURRENT_LIMIT = 40.0; - public static final int TURN_CURRENT_LIMIT = 30; - // Stuff - public static final double TALON_UPDATE_FREQUENCY_HZ = 50.0; - - public static final int SPARK_TIMEOUT_MS = 250; - public static final int SPARK_MEASURMENT_PERIOD_MS = 10; - public static final int SPARK_AVG_DEPTH = 2; - public static final double SPARK_FRAME_PERIOD = 1000.0 / ODOMETRY_FREQUENCY; - - // CAN/Device IDS and offsets - public static final int DRIVE0_ID = 0; - public static final int TURN0_ID = 1; - public static final int CANCODER0_ID = 2; - public static final double OFFSET0 = 0.0; - - public static final int DRIVE1_ID = 0; - public static final int TURN1_ID = 1; - public static final int CANCODER1_ID = 2; - public static final double OFFSET1 = 0.0; - - public static final int DRIVE2_ID = 0; - public static final int TURN2_ID = 1; - public static final int CANCODER2_ID = 2; - public static final double OFFSET2 = 0.0; - - public static final int DRIVE3_ID = 0; - public static final int TURN3_ID = 1; - public static final int CANCODER3_ID = 2; - public static final double OFFSET3 = 0.0; - } - } - - public static final class OdoThread { + public static final class OdoThread { - public static final class Phoenix { + public static final class Phoenix { - public static final int QUE_CAPACITY = 20; - public static final double SLEEP_TIME = 1000.0; - } + public static final int QUE_CAPACITY = 20; + public static final double SLEEP_TIME = 1000.0; + } - public static final class SparkMax { + public static final class SparkMax { - public static final int QUE_CAPACITY = 20; - public static final double PERIOD = 1.0; - } - } - } + public static final int QUE_CAPACITY = 20; + public static final double PERIOD = 1.0; + } + } + } - public static final class NoteSim { + public static final class NoteSim { - public static final double AIR_DENSITY = 1.225; - public static final double DRAG_COEFFICIENT = 0.45; - public static final double CROSSECTION_AREA = 0.11; - public static final double MASS = 0.235; + public static final double AIR_DENSITY = 1.225; + public static final double DRAG_COEFFICIENT = 0.45; + public static final double CROSSECTION_AREA = 0.11; + public static final double MASS = 0.235; - public static final Pose3d SHOOTER_POSE3D = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); - public static final Translation2d FIELD_SIZE = - new Translation2d(16.54, 8.21); // stolen from 3015 constants + public static final Pose3d SHOOTER_POSE3D = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); + public static final Translation2d FIELD_SIZE = new Translation2d(16.54, 8.21); // stolen from 3015 constants - public static final double dt = 0.2; // change in time for note sim - public static final Translation3d GRAVITY_TRANSLATION3D = new Translation3d(0, 0, 9.8); - public static final double OUT_OF_FIELD_MARGIN = .025; - } + public static final double dt = 0.2; // change in time for note sim + public static final Translation3d GRAVITY_TRANSLATION3D = new Translation3d(0, 0, 9.8); + public static final double OUT_OF_FIELD_MARGIN = .025; + } } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index f638f55..40d8330 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -22,14 +22,14 @@ */ public final class Main { - private Main() {} + private Main() {} - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 47a9e18..e804e5b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -37,145 +37,147 @@ */ public class Robot extends LoggedRobot { - public Manager managerSubsystem; - private SendableChooser autoChooser; - - private AutoCommands autoCommands = new AutoCommands(this); - - private Command autonomousCommand; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - @Override - public void robotInit() { - managerSubsystem = new Manager(); - - NamedCommands.registerCommand("Intaking", autoCommands.intaking()); - NamedCommands.registerCommand("Shooting", autoCommands.shooting()); - NamedCommands.registerCommand("Return To Idle", autoCommands.returnToIdle()); - NamedCommands.registerCommand("Speeding Up", autoCommands.startSpinningUp()); - NamedCommands.registerCommand("Spin and Intake", autoCommands.spinAndIntake()); - - // 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()); - // Logger.addDataReceiver(new WPILOGWriter("C:\\Dev\\Robotics - // Related\\Controls\\2024-Rewrite\\logs", defaultPeriodSecs)); - 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; - } - - // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. - // Logger.disableDeterministicTimestamps() - - // Start AdvantageKit logger - Logger.start(); - - autoChooser = AutoBuilder.buildAutoChooser("Example Auto"); - SmartDashboard.putData("Auto Chooser", autoChooser); - } - - /** This function is called periodically during all modes. */ - @Override - public void robotPeriodic() { - managerSubsystem.periodic(); - - // Logs note sim logging and updating sims - NoteSimulator.update(); - NoteSimulator.logNoteInfo(); - - CommandScheduler.getInstance().run(); - } - - /** This function is called once when the robot is disabled. */ - @Override - public void disabledInit() { - managerSubsystem.stop(); - } - - /** This function is called periodically when disabled. */ - @Override - public void disabledPeriodic() {} - - /** This function is called once the robot enters Auto. */ - @Override - public void autonomousInit() { - autonomousCommand = getAutonomousCommand(); - - // schedule the autonomous command (example) - System.out.println(":( womp womp"); - if (autonomousCommand != null) { - autonomousCommand.schedule(); - System.out.println("hi"); - } - } - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() {} - - /** This function is called once when teleop is enabled. */ - @Override - public void teleopInit() { - if (autonomousCommand != null) { - autonomousCommand.cancel(); - } - } - - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() {} - - /** This function is called once when test mode is enabled. */ - @Override - public void testInit() {} - - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() {} - - /** This function is called once when the robot is first started up. */ - @Override - public void simulationInit() {} - - /** This function is called periodically whilst in simulation. */ - @Override - public void simulationPeriodic() {} - - public Command getAutonomousCommand() { - return autoChooser.getSelected(); - } + public Manager managerSubsystem; + private SendableChooser autoChooser; + + private AutoCommands autoCommands = new AutoCommands(this); + + private Command autonomousCommand; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + managerSubsystem = new Manager(); + + NamedCommands.registerCommand("Intaking", autoCommands.intaking()); + NamedCommands.registerCommand("Shooting", autoCommands.shooting()); + NamedCommands.registerCommand("Return To Idle", autoCommands.returnToIdle()); + NamedCommands.registerCommand("Speeding Up", autoCommands.startSpinningUp()); + NamedCommands.registerCommand("Spin and Intake", autoCommands.spinAndIntake()); + + // 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()); + // Logger.addDataReceiver(new WPILOGWriter("C:\\Dev\\Robotics + // Related\\Controls\\2024-Rewrite\\logs", defaultPeriodSecs)); + 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; + } + + // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. + // Logger.disableDeterministicTimestamps() + + // Start AdvantageKit logger + Logger.start(); + + autoChooser = AutoBuilder.buildAutoChooser("Example Auto"); + SmartDashboard.putData("Auto Chooser", autoChooser); + } + + /** This function is called periodically during all modes. */ + @Override + public void robotPeriodic() { + managerSubsystem.periodic(); + + // Logs note sim logging and updating sims + NoteSimulator.update(); + NoteSimulator.logNoteInfo(); + + CommandScheduler.getInstance().run(); + } + + /** This function is called once when the robot is disabled. */ + @Override + public void disabledInit() { + managerSubsystem.stop(); + } + + /** This function is called periodically when disabled. */ + @Override + public void disabledPeriodic() {} + + /** This function is called once the robot enters Auto. */ + @Override + public void autonomousInit() { + autonomousCommand = getAutonomousCommand(); + + // schedule the autonomous command (example) + System.out.println(":( womp womp"); + if (autonomousCommand != null) { + autonomousCommand.schedule(); + System.out.println("hi"); + } + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() {} + + /** This function is called once when teleop is enabled. */ + @Override + public void teleopInit() { + if (autonomousCommand != null) { + autonomousCommand.cancel(); + } + } + + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() {} + + /** This function is called once when test mode is enabled. */ + @Override + public void testInit() {} + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} + + /** This function is called once when the robot is first started up. */ + @Override + public void simulationInit() {} + + /** This function is called periodically whilst in simulation. */ + @Override + public void simulationPeriodic() {} + + public Command getAutonomousCommand() { + return autoChooser.getSelected(); + } } diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index cee064c..a21dc3b 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -8,35 +8,41 @@ public class AutoCommands { - Robot robot = null; - - public AutoCommands(Robot robot) { - this.robot = robot; - } - - public Command intaking() { - return Commands.sequence( - new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.INTAKING))); - } - - public Command shooting() { - return Commands.sequence( - new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SHOOTING))); - } - - public Command returnToIdle() { - return Commands.sequence( - new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.IDLE))); - } - - public Command startSpinningUp() { - return Commands.sequence( - new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SPINNING_UP))); - } - - public Command spinAndIntake() { - return Commands.sequence( - new InstantCommand( - () -> robot.managerSubsystem.setState(ManagerStates.SPINNING_AND_INTAKING))); - } + Robot robot = null; + + public AutoCommands(Robot robot) { + this.robot = robot; + } + + public Command intaking() { + return Commands.sequence( + new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.INTAKING)) + ); + } + + public Command shooting() { + return Commands.sequence( + new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SHOOTING)) + ); + } + + public Command returnToIdle() { + return Commands.sequence( + new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.IDLE)) + ); + } + + public Command startSpinningUp() { + return Commands.sequence( + new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SPINNING_UP)) + ); + } + + public Command spinAndIntake() { + return Commands.sequence( + new InstantCommand(() -> + robot.managerSubsystem.setState(ManagerStates.SPINNING_AND_INTAKING) + ) + ); + } } diff --git a/src/main/java/frc/robot/subsystems/Subsystem.java b/src/main/java/frc/robot/subsystems/Subsystem.java index 95470e0..3b2cb1d 100644 --- a/src/main/java/frc/robot/subsystems/Subsystem.java +++ b/src/main/java/frc/robot/subsystems/Subsystem.java @@ -12,79 +12,81 @@ public abstract class Subsystem { - private Map>> triggerMap = - new HashMap>>(); - - private StateType state = null; - private Timer stateTimer = new Timer(); - private String subsystemName; - - public Subsystem(String subsystemName, StateType defaultState) { - if (defaultState == null) { - throw new RuntimeException("Default state cannot be null!"); - } - this.subsystemName = subsystemName; - this.state = defaultState; - - stateTimer.start(); - } - - // State operation - public void periodic() { - Logger.recordOutput(subsystemName + "/state", state.getStateString()); - if (!DriverStation.isEnabled()) return; - - runState(); - - checkTriggers(); - } - - protected abstract void runState(); - - // SmartDashboard utils - protected void putSmartDashboard(String key, String value) { - SmartDashboard.putString("[" + subsystemName + "] " + key, value); - } - - protected void putSmartDashboard(String key, double value) { - SmartDashboard.putNumber("[" + subsystemName + "] " + key, value); - } - - // Triggers for state transitions - protected void addTrigger(StateType startType, StateType endType, BooleanSupplier check) { - if (triggerMap.get(startType) == null) { - triggerMap.put(startType, new ArrayList>()); - } - triggerMap.get(startType).add(new Trigger(check, endType)); - } - - private void checkTriggers() { - List> triggers = triggerMap.get(state); - if (triggers == null) return; - for (var trigger : triggers) { - if (trigger.isTriggered()) { - setState(trigger.getResultState()); - return; - } - } - } - - // Other utilities - public StateType getState() { - return state; - } - - public void setState(StateType state) { - if (this.state != state) stateTimer.reset(); - this.state = state; - } - - /** - * Gets amount of time the state machine has been in the current state. - * - * @return time in seconds. - */ - protected double getStateTime() { - return stateTimer.get(); - } + private Map>> triggerMap = new HashMap< + StateType, + ArrayList> + >(); + + private StateType state = null; + private Timer stateTimer = new Timer(); + private String subsystemName; + + public Subsystem(String subsystemName, StateType defaultState) { + if (defaultState == null) { + throw new RuntimeException("Default state cannot be null!"); + } + this.subsystemName = subsystemName; + this.state = defaultState; + + stateTimer.start(); + } + + // State operation + public void periodic() { + Logger.recordOutput(subsystemName + "/state", state.getStateString()); + if (!DriverStation.isEnabled()) return; + + runState(); + + checkTriggers(); + } + + protected abstract void runState(); + + // SmartDashboard utils + protected void putSmartDashboard(String key, String value) { + SmartDashboard.putString("[" + subsystemName + "] " + key, value); + } + + protected void putSmartDashboard(String key, double value) { + SmartDashboard.putNumber("[" + subsystemName + "] " + key, value); + } + + // Triggers for state transitions + protected void addTrigger(StateType startType, StateType endType, BooleanSupplier check) { + if (triggerMap.get(startType) == null) { + triggerMap.put(startType, new ArrayList>()); + } + triggerMap.get(startType).add(new Trigger(check, endType)); + } + + private void checkTriggers() { + List> triggers = triggerMap.get(state); + if (triggers == null) return; + for (var trigger : triggers) { + if (trigger.isTriggered()) { + setState(trigger.getResultState()); + return; + } + } + } + + // Other utilities + public StateType getState() { + return state; + } + + public void setState(StateType state) { + if (this.state != state) stateTimer.reset(); + this.state = state; + } + + /** + * Gets amount of time the state machine has been in the current state. + * + * @return time in seconds. + */ + protected double getStateTime() { + return stateTimer.get(); + } } diff --git a/src/main/java/frc/robot/subsystems/SubsystemStates.java b/src/main/java/frc/robot/subsystems/SubsystemStates.java index d2ffcaf..7664a70 100644 --- a/src/main/java/frc/robot/subsystems/SubsystemStates.java +++ b/src/main/java/frc/robot/subsystems/SubsystemStates.java @@ -1,5 +1,5 @@ package frc.robot.subsystems; public interface SubsystemStates { - String getStateString(); + String getStateString(); } diff --git a/src/main/java/frc/robot/subsystems/Trigger.java b/src/main/java/frc/robot/subsystems/Trigger.java index d94c75e..cd358ff 100644 --- a/src/main/java/frc/robot/subsystems/Trigger.java +++ b/src/main/java/frc/robot/subsystems/Trigger.java @@ -4,19 +4,19 @@ public class Trigger { - BooleanSupplier supplier; - StateType state; + BooleanSupplier supplier; + StateType state; - public Trigger(BooleanSupplier supplier, StateType state) { - this.supplier = supplier; - this.state = state; - } + public Trigger(BooleanSupplier supplier, StateType state) { + this.supplier = supplier; + this.state = state; + } - public boolean isTriggered() { - return supplier.getAsBoolean(); - } + public boolean isTriggered() { + return supplier.getAsBoolean(); + } - public StateType getResultState() { - return state; - } + public StateType getResultState() { + return state; + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBar.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBar.java index 05328ce..ba55f54 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBar.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBar.java @@ -8,53 +8,59 @@ public class AmpBar extends Subsystem { - AmpBarIO io; - AmpBarIOInputsAutoLogged inputs; - - public AmpBar(AmpBarIO io) { - super("Amp Bar", AmpBarStates.OFF); - this.io = io; - inputs = new AmpBarIOInputsAutoLogged(); - - switch (Constants.currentMode) { - case REAL: - io.configurePID( - Constants.AmpBar.REAL_PID.kP, - Constants.AmpBar.REAL_PID.kI, - Constants.AmpBar.REAL_PID.kD); - break; - case SIM: - io.configurePID( - Constants.AmpBar.SIM_PID.kP, Constants.AmpBar.SIM_PID.kP, Constants.AmpBar.SIM_PID.kP); - break; - case REPLAY: - io.configurePID(0, 0, 0); - break; - default: - break; - } - } - - @Override - protected void runState() { - io.setSpinnerSpeedpoint(getState().getMotorSpeedpoint()); - io.setPivotPosition(getState().getPivotPositionSetpoint()); - } - - public void stop() { - io.stop(); - } - - @Override - public void periodic() { - super.periodic(); - - Logger.recordOutput( - "Amp Bar/Amp Bar Pose3d", - new Pose3d( - Constants.AmpBar.ZEROED_PIVOT_TRANSLATION, - new Rotation3d(0, io.getPivotPosition(), 0))); - Logger.processInputs("Amp Bar", inputs); - io.updateInput(inputs); - } + AmpBarIO io; + AmpBarIOInputsAutoLogged inputs; + + public AmpBar(AmpBarIO io) { + super("Amp Bar", AmpBarStates.OFF); + this.io = io; + inputs = new AmpBarIOInputsAutoLogged(); + + switch (Constants.currentMode) { + case REAL: + io.configurePID( + Constants.AmpBar.REAL_PID.kP, + Constants.AmpBar.REAL_PID.kI, + Constants.AmpBar.REAL_PID.kD + ); + break; + case SIM: + io.configurePID( + Constants.AmpBar.SIM_PID.kP, + Constants.AmpBar.SIM_PID.kP, + Constants.AmpBar.SIM_PID.kP + ); + break; + case REPLAY: + io.configurePID(0, 0, 0); + break; + default: + break; + } + } + + @Override + protected void runState() { + io.setSpinnerSpeedpoint(getState().getMotorSpeedpoint()); + io.setPivotPosition(getState().getPivotPositionSetpoint()); + } + + public void stop() { + io.stop(); + } + + @Override + public void periodic() { + super.periodic(); + + Logger.recordOutput( + "Amp Bar/Amp Bar Pose3d", + new Pose3d( + Constants.AmpBar.ZEROED_PIVOT_TRANSLATION, + new Rotation3d(0, io.getPivotPosition(), 0) + ) + ); + Logger.processInputs("Amp Bar", inputs); + io.updateInput(inputs); + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIO.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIO.java index 93f0012..5560918 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIO.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIO.java @@ -3,40 +3,40 @@ import org.littletonrobotics.junction.AutoLog; public interface AmpBarIO { - @AutoLog - public static class AmpBarIOInputs { + @AutoLog + public static class AmpBarIOInputs { - public String ampBarState; + public String ampBarState; - // Pivot - public double pivotPosition; - public double pivotSetpoint; - public double pivotAppliedVoltage; - // Spinners - public double spinnerSpeed; - public double spinnerSetpoint; - public double spinnerAppliedVoltage; - } + // Pivot + public double pivotPosition; + public double pivotSetpoint; + public double pivotAppliedVoltage; + // Spinners + public double spinnerSpeed; + public double spinnerSetpoint; + public double spinnerAppliedVoltage; + } - public default void updateInput(AmpBarIOInputs inputs) {} + public default void updateInput(AmpBarIOInputs inputs) {} - public default void setPivotPosition(double position) {} + public default void setPivotPosition(double position) {} - public default void setSpinnerSpeedpoint(double speed) {} + public default void setSpinnerSpeedpoint(double speed) {} - public default void stop() {} + public default void stop() {} - public default double getPivotPosition() { - return 0.0; - } + public default double getPivotPosition() { + return 0.0; + } - public default double getSpinnerSpeed() { - return 0.0; - } + public default double getSpinnerSpeed() { + return 0.0; + } - public default void configurePID(double kP, double kI, double kD) {} + public default void configurePID(double kP, double kI, double kD) {} - public default boolean atSetPoint() { - return false; - } + public default boolean atSetPoint() { + return false; + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOReal.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOReal.java index 876cc81..11f76fe 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOReal.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOReal.java @@ -10,99 +10,101 @@ public class AmpBarIOReal implements AmpBarIO { - private final CANSparkMax leftMotor; - private final CANSparkMax rightMotor; - private final TalonFX spinnerMotor; - - private RelativeEncoder pivotEncoder; - private String stateString; - private PIDController controller; - - private double pivotMotorAppliedVoltage; - private double pivotPositionSetpoint; - - private double spinnerSpeedpoint; - private double spinnerAppliedVoltage; - - public AmpBarIOReal() { - leftMotor = new CANSparkMax(Constants.AmpBar.LEFT_PIVOT_ID, MotorType.kBrushless); - rightMotor = new CANSparkMax(Constants.AmpBar.RIGHT_PIVOT_ID, MotorType.kBrushless); - spinnerMotor = new TalonFX(Constants.AmpBar.SPINNER_ID); - - leftMotor.setInverted(false); - rightMotor.follow(leftMotor, true); - leftMotor.setIdleMode(IdleMode.kCoast); - rightMotor.setIdleMode(IdleMode.kCoast); - - pivotEncoder = leftMotor.getEncoder(); - - pivotEncoder.setPosition(0); - pivotEncoder.setPositionConversionFactor(Constants.RADIAN_CF); - pivotEncoder.setVelocityConversionFactor(Constants.RADIAN_CF); - - controller = new PIDController(0, 0, 0); - - stateString = ""; - pivotMotorAppliedVoltage = 0; - pivotPositionSetpoint = 0; - spinnerSpeedpoint = 0; - spinnerAppliedVoltage = 0; - } - - @Override - public void updateInput(AmpBarIOInputs inputs) { - inputs.ampBarState = stateString; - - inputs.pivotPosition = pivotEncoder.getPosition(); - inputs.pivotSetpoint = pivotPositionSetpoint; - inputs.pivotAppliedVoltage = pivotMotorAppliedVoltage; - - inputs.spinnerSpeed = getSpinnerSpeed(); - inputs.spinnerSetpoint = spinnerSpeedpoint; - inputs.spinnerAppliedVoltage = spinnerAppliedVoltage; - } - - @Override - public void setPivotPosition(double position) { - pivotPositionSetpoint = position; - pivotMotorAppliedVoltage = - controller.calculate(pivotEncoder.getPosition(), pivotPositionSetpoint); - leftMotor.setVoltage(pivotMotorAppliedVoltage); - } - - public void stop() { - spinnerAppliedVoltage = 0; - pivotMotorAppliedVoltage = 0; - leftMotor.stopMotor(); - rightMotor.stopMotor(); - spinnerMotor.stopMotor(); - } - - @Override - public void setSpinnerSpeedpoint(double speed) { - spinnerSpeedpoint = speed; - spinnerAppliedVoltage = speed; - spinnerMotor.setVoltage(speed); - } - - @Override - public double getPivotPosition() { - return pivotEncoder.getPosition(); - } - - @Override - public double getSpinnerSpeed() { - return spinnerMotor.getVelocity().getValueAsDouble(); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - controller.setPID(kP, kI, kD); - } - - @Override - public boolean atSetPoint() { - double motorPosition = getPivotPosition(); - return Math.abs(motorPosition - pivotPositionSetpoint) <= Constants.AmpBar.ERROR_OF_MARGIN; - } + private final CANSparkMax leftMotor; + private final CANSparkMax rightMotor; + private final TalonFX spinnerMotor; + + private RelativeEncoder pivotEncoder; + private String stateString; + private PIDController controller; + + private double pivotMotorAppliedVoltage; + private double pivotPositionSetpoint; + + private double spinnerSpeedpoint; + private double spinnerAppliedVoltage; + + public AmpBarIOReal() { + leftMotor = new CANSparkMax(Constants.AmpBar.LEFT_PIVOT_ID, MotorType.kBrushless); + rightMotor = new CANSparkMax(Constants.AmpBar.RIGHT_PIVOT_ID, MotorType.kBrushless); + spinnerMotor = new TalonFX(Constants.AmpBar.SPINNER_ID); + + leftMotor.setInverted(false); + rightMotor.follow(leftMotor, true); + leftMotor.setIdleMode(IdleMode.kCoast); + rightMotor.setIdleMode(IdleMode.kCoast); + + pivotEncoder = leftMotor.getEncoder(); + + pivotEncoder.setPosition(0); + pivotEncoder.setPositionConversionFactor(Constants.RADIAN_CF); + pivotEncoder.setVelocityConversionFactor(Constants.RADIAN_CF); + + controller = new PIDController(0, 0, 0); + + stateString = ""; + pivotMotorAppliedVoltage = 0; + pivotPositionSetpoint = 0; + spinnerSpeedpoint = 0; + spinnerAppliedVoltage = 0; + } + + @Override + public void updateInput(AmpBarIOInputs inputs) { + inputs.ampBarState = stateString; + + inputs.pivotPosition = pivotEncoder.getPosition(); + inputs.pivotSetpoint = pivotPositionSetpoint; + inputs.pivotAppliedVoltage = pivotMotorAppliedVoltage; + + inputs.spinnerSpeed = getSpinnerSpeed(); + inputs.spinnerSetpoint = spinnerSpeedpoint; + inputs.spinnerAppliedVoltage = spinnerAppliedVoltage; + } + + @Override + public void setPivotPosition(double position) { + pivotPositionSetpoint = position; + pivotMotorAppliedVoltage = controller.calculate( + pivotEncoder.getPosition(), + pivotPositionSetpoint + ); + leftMotor.setVoltage(pivotMotorAppliedVoltage); + } + + public void stop() { + spinnerAppliedVoltage = 0; + pivotMotorAppliedVoltage = 0; + leftMotor.stopMotor(); + rightMotor.stopMotor(); + spinnerMotor.stopMotor(); + } + + @Override + public void setSpinnerSpeedpoint(double speed) { + spinnerSpeedpoint = speed; + spinnerAppliedVoltage = speed; + spinnerMotor.setVoltage(speed); + } + + @Override + public double getPivotPosition() { + return pivotEncoder.getPosition(); + } + + @Override + public double getSpinnerSpeed() { + return spinnerMotor.getVelocity().getValueAsDouble(); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + controller.setPID(kP, kI, kD); + } + + @Override + public boolean atSetPoint() { + double motorPosition = getPivotPosition(); + return Math.abs(motorPosition - pivotPositionSetpoint) <= Constants.AmpBar.ERROR_OF_MARGIN; + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOSim.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOSim.java index a191383..c70e781 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOSim.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOSim.java @@ -8,101 +8,101 @@ public class AmpBarIOSim implements AmpBarIO { - SingleJointedArmSim pivotSim; - DCMotorSim spinnerSim; - PIDController controller; - - String stateString; - - double pivotAppliedVoltage; - double pivotSetpoint; - - double spinnerAppliedVoltage; - double spinnerSpeedpoint; - - public AmpBarIOSim() { - // the sim values are random - pivotSim = - new SingleJointedArmSim( - DCMotor.getNEO(Constants.AmpBar.NUM_PIVOT_MOTORS), - Constants.AmpBar.PIVOT_GEARING, - Constants.AmpBar.PIVOT_MOI, - Constants.AmpBar.PIVOT_LENGTH_METERS, - Constants.AmpBar.MIN_PIVOT_POSITION, - Constants.AmpBar.MAX_PIVOT_POSITION, - false, - 0); - spinnerSim = - new DCMotorSim( - DCMotor.getKrakenX60(Constants.AmpBar.NUM_SPINNER_MOTORS), - Constants.AmpBar.SPINNER_GEARING, - Constants.AmpBar.SPINNER_MOI); - - controller = new PIDController(0, 0, 0); - stateString = ""; - - pivotAppliedVoltage = 0; - pivotSetpoint = 0; - spinnerAppliedVoltage = 0; - spinnerSpeedpoint = 0; - } - - @Override - public void updateInput(AmpBarIOInputs inputs) { - inputs.ampBarState = stateString; - - inputs.pivotPosition = getPivotPosition(); - inputs.pivotSetpoint = pivotSetpoint; - inputs.pivotAppliedVoltage = pivotAppliedVoltage; - - inputs.spinnerSpeed = getSpinnerSpeed(); - inputs.spinnerSetpoint = spinnerSpeedpoint; - inputs.spinnerAppliedVoltage = spinnerAppliedVoltage; - - pivotSim.update(Constants.SIM_UPDATE_TIME); - spinnerSim.update(Constants.SIM_UPDATE_TIME); - } - - @Override - public void setPivotPosition(double position) { - pivotSetpoint = position; - pivotAppliedVoltage = controller.calculate(pivotSim.getAngleRads(), pivotSetpoint); - pivotSim.setInputVoltage(pivotAppliedVoltage); - } - - @Override - public void setSpinnerSpeedpoint(double speed) { - spinnerSpeedpoint = speed; - spinnerAppliedVoltage = speed; - spinnerSim.setInputVoltage(speed); - } - - @Override - public void stop() { - pivotAppliedVoltage = 0; - spinnerAppliedVoltage = 0; - spinnerSim.setInputVoltage(0); - pivotSim.setInputVoltage(0); - } - - @Override - public double getPivotPosition() { - return pivotSim.getAngleRads(); - } - - @Override - public double getSpinnerSpeed() { - return spinnerSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - } - - @Override - public void configurePID(double kP, double kI, double kD) { - controller.setPID(kP, kI, kD); - } - - @Override - public boolean atSetPoint() { - double motorPosition = getPivotPosition(); - return Math.abs(motorPosition - pivotSetpoint) <= Constants.AmpBar.ERROR_OF_MARGIN; - } + SingleJointedArmSim pivotSim; + DCMotorSim spinnerSim; + PIDController controller; + + String stateString; + + double pivotAppliedVoltage; + double pivotSetpoint; + + double spinnerAppliedVoltage; + double spinnerSpeedpoint; + + public AmpBarIOSim() { + // the sim values are random + pivotSim = new SingleJointedArmSim( + DCMotor.getNEO(Constants.AmpBar.NUM_PIVOT_MOTORS), + Constants.AmpBar.PIVOT_GEARING, + Constants.AmpBar.PIVOT_MOI, + Constants.AmpBar.PIVOT_LENGTH_METERS, + Constants.AmpBar.MIN_PIVOT_POSITION, + Constants.AmpBar.MAX_PIVOT_POSITION, + false, + 0 + ); + spinnerSim = new DCMotorSim( + DCMotor.getKrakenX60(Constants.AmpBar.NUM_SPINNER_MOTORS), + Constants.AmpBar.SPINNER_GEARING, + Constants.AmpBar.SPINNER_MOI + ); + + controller = new PIDController(0, 0, 0); + stateString = ""; + + pivotAppliedVoltage = 0; + pivotSetpoint = 0; + spinnerAppliedVoltage = 0; + spinnerSpeedpoint = 0; + } + + @Override + public void updateInput(AmpBarIOInputs inputs) { + inputs.ampBarState = stateString; + + inputs.pivotPosition = getPivotPosition(); + inputs.pivotSetpoint = pivotSetpoint; + inputs.pivotAppliedVoltage = pivotAppliedVoltage; + + inputs.spinnerSpeed = getSpinnerSpeed(); + inputs.spinnerSetpoint = spinnerSpeedpoint; + inputs.spinnerAppliedVoltage = spinnerAppliedVoltage; + + pivotSim.update(Constants.SIM_UPDATE_TIME); + spinnerSim.update(Constants.SIM_UPDATE_TIME); + } + + @Override + public void setPivotPosition(double position) { + pivotSetpoint = position; + pivotAppliedVoltage = controller.calculate(pivotSim.getAngleRads(), pivotSetpoint); + pivotSim.setInputVoltage(pivotAppliedVoltage); + } + + @Override + public void setSpinnerSpeedpoint(double speed) { + spinnerSpeedpoint = speed; + spinnerAppliedVoltage = speed; + spinnerSim.setInputVoltage(speed); + } + + @Override + public void stop() { + pivotAppliedVoltage = 0; + spinnerAppliedVoltage = 0; + spinnerSim.setInputVoltage(0); + pivotSim.setInputVoltage(0); + } + + @Override + public double getPivotPosition() { + return pivotSim.getAngleRads(); + } + + @Override + public double getSpinnerSpeed() { + return spinnerSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + } + + @Override + public void configurePID(double kP, double kI, double kD) { + controller.setPID(kP, kI, kD); + } + + @Override + public boolean atSetPoint() { + double motorPosition = getPivotPosition(); + return Math.abs(motorPosition - pivotSetpoint) <= Constants.AmpBar.ERROR_OF_MARGIN; + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBarStates.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBarStates.java index 4dadaef..f468c30 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBarStates.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBarStates.java @@ -4,31 +4,31 @@ import frc.robot.subsystems.SubsystemStates; public enum AmpBarStates implements SubsystemStates { - OFF(Constants.AmpBar.IN, Constants.AmpBar.OFF, "Amp Bar Off"), - SHOOTING(Constants.AmpBar.OUT, Constants.AmpBar.SHOOTING, "Shooting Amp"), - FEEDING(Constants.AmpBar.FEEDING_POSITION, Constants.AmpBar.FEEDING, "Getting Fed"), - HOLDING_NOTE(Constants.AmpBar.OUT, Constants.AmpBar.OFF, "Holding a Note"); + OFF(Constants.AmpBar.IN, Constants.AmpBar.OFF, "Amp Bar Off"), + SHOOTING(Constants.AmpBar.OUT, Constants.AmpBar.SHOOTING, "Shooting Amp"), + FEEDING(Constants.AmpBar.FEEDING_POSITION, Constants.AmpBar.FEEDING, "Getting Fed"), + HOLDING_NOTE(Constants.AmpBar.OUT, Constants.AmpBar.OFF, "Holding a Note"); - private double pivotPositionSetpoint; - private double spinnerMotorSpeedpoint; - private String stateString; + private double pivotPositionSetpoint; + private double spinnerMotorSpeedpoint; + private String stateString; - AmpBarStates(double pivotMotorSetpoint, double spinnerMotorSpeedpoint, String stateString) { - this.pivotPositionSetpoint = pivotMotorSetpoint; - this.spinnerMotorSpeedpoint = spinnerMotorSpeedpoint; - this.stateString = stateString; - } + AmpBarStates(double pivotMotorSetpoint, double spinnerMotorSpeedpoint, String stateString) { + this.pivotPositionSetpoint = pivotMotorSetpoint; + this.spinnerMotorSpeedpoint = spinnerMotorSpeedpoint; + this.stateString = stateString; + } - public double getPivotPositionSetpoint() { - return pivotPositionSetpoint; - } + public double getPivotPositionSetpoint() { + return pivotPositionSetpoint; + } - public double getMotorSpeedpoint() { - return spinnerMotorSpeedpoint; - } + public double getMotorSpeedpoint() { + return spinnerMotorSpeedpoint; + } - @Override - public String getStateString() { - return stateString; - } + @Override + public String getStateString() { + return stateString; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 169838c..c2b767c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -35,293 +35,317 @@ import org.littletonrobotics.junction.Logger; public class Drive extends Subsystem { - static final Lock odometryLock = new ReentrantLock(); - private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); - private final Module[] modules = new Module[Constants.Drive.NUM_MODULES]; // FL, FR, BL, BR - private PPDriveWrapper autoConfig; - - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); - private Rotation2d rawGyroRotation = new Rotation2d(); - private SwerveModulePosition[] lastModulePositions = // For delta tracking - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; - private SwerveDrivePoseEstimator poseEstimator = - new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); - - public Drive( - GyroIO gyroIO, - ModuleIO flModuleIO, - ModuleIO frModuleIO, - ModuleIO blModuleIO, - ModuleIO brModuleIO) { - - super("Drive", DriveStates.REGULAR_DRIVE); - autoConfig = new PPDriveWrapper(this); - - this.gyroIO = gyroIO; - modules[0] = new Module(flModuleIO, 0); - modules[1] = new Module(frModuleIO, 1); - modules[2] = new Module(blModuleIO, 2); - modules[3] = new Module(brModuleIO, 3); - - // Start threads (no-op for each if no signals have been created) - PhoenixOdometryThread.getInstance().start(); - SparkMaxOdometryThread.getInstance().start(); - - // Triggers - addTrigger( - DriveStates.REGULAR_DRIVE, - DriveStates.SLOW_MODE, - () -> Constants.controller.getRightBumper()); - addTrigger( - DriveStates.REGULAR_DRIVE, - DriveStates.SPEED_MAXXING, - () -> Constants.controller.getLeftBumper()); - } - - @Override - public void runState() { - // Can't run in auto otherwise it will constantly tell drive not to drive in auto (and thats not good) - if (DriverStation.isTeleop()) { - drive( - this, - () -> Constants.controller.getLeftY(), - () -> Constants.controller.getLeftX(), - () -> -Constants.controller.getRightX(), - getState().getRotationModifier(), - getState().getTranslationModifier()); - } - } - - public void drive( - Drive drive, - DoubleSupplier xSupplier, - DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier, - double rotationMultiplier, - double translationMultiplier) { - // Apply deadband - double linearMagnitude = - MathUtil.applyDeadband( - Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), - Constants.Drive.CONTROLLER_DEADBAND); - Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); - double omega = - MathUtil.applyDeadband(omegaSupplier.getAsDouble(), Constants.Drive.CONTROLLER_DEADBAND); - - // Square values - linearMagnitude = linearMagnitude * linearMagnitude; - omega = Math.copySign(omega * omega, omega); - - // Calcaulate new linear velocity - Translation2d linearVelocity = - new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); - - // Convert to field relative speeds & send command - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; - drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec() * translationMultiplier, - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec() * translationMultiplier, - omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier, - isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation())); - } - - @Override - public void periodic() { - super.periodic(); - odometryLock.lock(); // Prevents odometry updates while reading data - gyroIO.updateInputs(gyroInputs); - for (var module : modules) { - module.updateInputs(); - } - odometryLock.unlock(); - Logger.processInputs("Drive/Gyro", gyroInputs); - for (var module : modules) { - module.periodic(); - } - - // Stop moving when disabled - if (DriverStation.isDisabled()) { - for (var module : modules) { - module.stop(); - } - } - // Log empty setpoint states when disabled - if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); - } - - // Update odometry - double[] sampleTimestamps = - modules[0].getOdometryTimestamps(); // All signals are sampled together - int sampleCount = sampleTimestamps.length; - for (int i = 0; i < sampleCount; i++) { - // Read wheel positions and deltas from each module - SwerveModulePosition[] modulePositions = - new SwerveModulePosition[Constants.Drive.NUM_MODULES]; - SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[Constants.Drive.NUM_MODULES]; - for (int moduleIndex = 0; moduleIndex < Constants.Drive.NUM_MODULES; moduleIndex++) { - modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; - moduleDeltas[moduleIndex] = - new SwerveModulePosition( - modulePositions[moduleIndex].distanceMeters - - lastModulePositions[moduleIndex].distanceMeters, - modulePositions[moduleIndex].angle); - lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; - } - - // Update gyro angle - if (gyroInputs.connected) { - // Use the real gyro angle - rawGyroRotation = gyroInputs.odometryYawPositions[i]; - } else { - // Use the angle delta from the kinematics and module deltas - Twist2d twist = kinematics.toTwist2d(moduleDeltas); - rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); - } - - // Apply update - poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); - } - } - - /** - * Runs the drive at the desired velocity. - * - * @param speeds Speeds in meters/sec - */ - public void runVelocity(ChassisSpeeds speeds) { - // Calculate module setpoints - ChassisSpeeds discreteSpeeds = - ChassisSpeeds.discretize(speeds, Constants.Drive.DISCRETIZE_TIME_SECONDS); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, Constants.Drive.MAX_LINEAR_SPEED); - - // Send setpoints to modules - SwerveModuleState[] optimizedSetpointStates = - new SwerveModuleState[Constants.Drive.NUM_MODULES]; - for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { - // The module returns the optimized state, useful for logging - optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); - } - - // Log setpoint states - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); - } - - /** Stops the drive. */ - public void stop() { - runVelocity(new ChassisSpeeds()); - } - - /** - * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will - * return to their normal orientations the next time a nonzero velocity is requested. - */ - public void stopWithX() { - Rotation2d[] headings = new Rotation2d[Constants.Drive.NUM_MODULES]; - for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { - headings[i] = getModuleTranslations()[i].getAngle(); - } - kinematics.resetHeadings(headings); - stop(); - } - - /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveStates/Measured") - private SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[Constants.Drive.NUM_MODULES]; - for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { - states[i] = modules[i].getState(); - } - return states; - } - - /** Returns the module positions (turn angles and drive positions) for all of the modules. */ - private SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] states = new SwerveModulePosition[Constants.Drive.NUM_MODULES]; - for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { - states[i] = modules[i].getPosition(); - } - return states; - } - - /** Returns the current odometry pose. */ - @AutoLogOutput(key = "Odometry/Robot") - public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); - } - - public ChassisSpeeds getChassisSpeed() { - ChassisSpeeds robotChassisSpeed = kinematics.toChassisSpeeds(getModuleStates()); - return robotChassisSpeed; - } - - public double calculateVelocity() { - double robotSpeed = - Math.sqrt( - Math.pow(getChassisSpeed().vxMetersPerSecond, 2) - + Math.pow(getChassisSpeed().vyMetersPerSecond, 2)); - return robotSpeed; - } - - /** Returns the current odometry rotation. */ - public Rotation2d getRotation() { - return getPose().getRotation(); - } - - /** Resets the current odometry pose. */ - public void setPose(Pose2d pose) { - poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); - } - - /** - * Adds a vision measurement to the pose estimator. - * - * @param visionPose The pose of the robot as measured by the vision camera. - * @param timestamp The timestamp of the vision measurement in seconds. - */ - public void addVisionMeasurement(Pose2d visionPose, double timestamp) { - poseEstimator.addVisionMeasurement(visionPose, timestamp); - } - - /** Returns the maximum linear speed in meters per sec. */ - public double getMaxLinearSpeedMetersPerSec() { - return Constants.Drive.MAX_LINEAR_SPEED; - } - - /** Returns the maximum angular speed in radians per sec. */ - public double getMaxAngularSpeedRadPerSec() { - return Constants.Drive.MAX_ANGULAR_SPEED; - } - - /** Returns an array of module translations. */ - public static Translation2d[] getModuleTranslations() { - return new Translation2d[] { - new Translation2d( - Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, - Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF), - new Translation2d( - Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, - -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF), - new Translation2d( - -Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, - Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF), - new Translation2d( - -Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, - -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF) - }; - } + + static final Lock odometryLock = new ReentrantLock(); + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[Constants.Drive.NUM_MODULES]; // FL, FR, BL, BR + private PPDriveWrapper autoConfig; + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); + private Rotation2d rawGyroRotation = new Rotation2d(); + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + }; + private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator( + kinematics, + rawGyroRotation, + lastModulePositions, + new Pose2d() + ); + + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO + ) { + super("Drive", DriveStates.REGULAR_DRIVE); + autoConfig = new PPDriveWrapper(this); + + this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); + + // Start threads (no-op for each if no signals have been created) + PhoenixOdometryThread.getInstance().start(); + SparkMaxOdometryThread.getInstance().start(); + + // Triggers + addTrigger(DriveStates.REGULAR_DRIVE, DriveStates.SLOW_MODE, () -> + Constants.controller.getRightBumper() + ); + addTrigger(DriveStates.REGULAR_DRIVE, DriveStates.SPEED_MAXXING, () -> + Constants.controller.getLeftBumper() + ); + } + + @Override + public void runState() { + // Can't run in auto otherwise it will constantly tell drive not to drive in auto (and thats not good) + if (DriverStation.isTeleop()) { + drive( + this, + () -> Constants.controller.getLeftY(), + () -> Constants.controller.getLeftX(), + () -> -Constants.controller.getRightX(), + getState().getRotationModifier(), + getState().getTranslationModifier() + ); + } + } + + public void drive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier, + double rotationMultiplier, + double translationMultiplier + ) { + // Apply deadband + double linearMagnitude = MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), + Constants.Drive.CONTROLLER_DEADBAND + ); + Rotation2d linearDirection = new Rotation2d( + xSupplier.getAsDouble(), + ySupplier.getAsDouble() + ); + double omega = MathUtil.applyDeadband( + omegaSupplier.getAsDouble(), + Constants.Drive.CONTROLLER_DEADBAND + ); + + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + omega = Math.copySign(omega * omega, omega); + + // Calcaulate new linear velocity + Translation2d linearVelocity = new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + + // Convert to field relative speeds & send command + boolean isFlipped = + DriverStation.getAlliance().isPresent() && + DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + linearVelocity.getY() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier, + isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation() + ) + ); + } + + @Override + public void periodic() { + super.periodic(); + odometryLock.lock(); // Prevents odometry updates while reading data + gyroIO.updateInputs(gyroInputs); + for (var module : modules) { + module.updateInputs(); + } + odometryLock.unlock(); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } + + // Update odometry + double[] sampleTimestamps = modules[0].getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = + new SwerveModulePosition[Constants.Drive.NUM_MODULES]; + SwerveModulePosition[] moduleDeltas = + new SwerveModulePosition[Constants.Drive.NUM_MODULES]; + for (int moduleIndex = 0; moduleIndex < Constants.Drive.NUM_MODULES; moduleIndex++) { + modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; + moduleDeltas[moduleIndex] = new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters - + lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle + ); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.odometryYawPositions[i]; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + + // Apply update + poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + } + } + + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize( + speeds, + Constants.Drive.DISCRETIZE_TIME_SECONDS + ); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds( + setpointStates, + Constants.Drive.MAX_LINEAR_SPEED + ); + + // Send setpoints to modules + SwerveModuleState[] optimizedSetpointStates = + new SwerveModuleState[Constants.Drive.NUM_MODULES]; + for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { + // The module returns the optimized state, useful for logging + optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + } + + // Log setpoint states + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + } + + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[Constants.Drive.NUM_MODULES]; + for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { + headings[i] = getModuleTranslations()[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); + } + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[Constants.Drive.NUM_MODULES]; + for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { + states[i] = modules[i].getState(); + } + return states; + } + + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[Constants.Drive.NUM_MODULES]; + for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { + states[i] = modules[i].getPosition(); + } + return states; + } + + /** Returns the current odometry pose. */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + public ChassisSpeeds getChassisSpeed() { + ChassisSpeeds robotChassisSpeed = kinematics.toChassisSpeeds(getModuleStates()); + return robotChassisSpeed; + } + + public double calculateVelocity() { + double robotSpeed = Math.sqrt( + Math.pow(getChassisSpeed().vxMetersPerSecond, 2) + + Math.pow(getChassisSpeed().vyMetersPerSecond, 2) + ); + return robotSpeed; + } + + /** Returns the current odometry rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** Resets the current odometry pose. */ + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + /** + * Adds a vision measurement to the pose estimator. + * + * @param visionPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. + */ + public void addVisionMeasurement(Pose2d visionPose, double timestamp) { + poseEstimator.addVisionMeasurement(visionPose, timestamp); + } + + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return Constants.Drive.MAX_LINEAR_SPEED; + } + + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return Constants.Drive.MAX_ANGULAR_SPEED; + } + + /** Returns an array of module translations. */ + public static Translation2d[] getModuleTranslations() { + return new Translation2d[] { + new Translation2d( + Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, + Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF + ), + new Translation2d( + Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, + -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF + ), + new Translation2d( + -Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, + Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF + ), + new Translation2d( + -Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, + -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF + ), + }; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveStates.java b/src/main/java/frc/robot/subsystems/drive/DriveStates.java index d3e6446..5f07146 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveStates.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveStates.java @@ -4,33 +4,33 @@ import frc.robot.subsystems.SubsystemStates; public enum DriveStates implements SubsystemStates { - REGULAR_DRIVE("Regular Drive", Constants.Drive.REGULAR_TM, Constants.Drive.REGULAR_RM), - SLOW_MODE("Slow Mode", Constants.Drive.SLOW_TM, Constants.Drive.SLOW_RM), - AUTO_ALIGN("Auto Aligning", Constants.Drive.AA_TM, Constants.Drive.AA_RM), - SPEED_MAXXING("Speed Maxxing", Constants.Drive.FAST_TM, Constants.Drive.FAST_RM); - - // Someone make auto align so that state is useful please - - DriveStates(String stateString, double translationModifier, double rotationModifier) { - this.stateString = stateString; - this.translationModifier = translationModifier; - this.rotationModifier = rotationModifier; - } - - String stateString; - double translationModifier; - double rotationModifier; - - @Override - public String getStateString() { - return stateString; - } - - public double getTranslationModifier() { - return translationModifier; - } - - public double getRotationModifier() { - return rotationModifier; - } + REGULAR_DRIVE("Regular Drive", Constants.Drive.REGULAR_TM, Constants.Drive.REGULAR_RM), + SLOW_MODE("Slow Mode", Constants.Drive.SLOW_TM, Constants.Drive.SLOW_RM), + AUTO_ALIGN("Auto Aligning", Constants.Drive.AA_TM, Constants.Drive.AA_RM), + SPEED_MAXXING("Speed Maxxing", Constants.Drive.FAST_TM, Constants.Drive.FAST_RM); + + // Someone make auto align so that state is useful please + + DriveStates(String stateString, double translationModifier, double rotationModifier) { + this.stateString = stateString; + this.translationModifier = translationModifier; + this.rotationModifier = rotationModifier; + } + + String stateString; + double translationModifier; + double rotationModifier; + + @Override + public String getStateString() { + return stateString; + } + + public double getTranslationModifier() { + return translationModifier; + } + + public double getRotationModifier() { + return rotationModifier; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 9550c02..8efbde0 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -17,15 +17,15 @@ import org.littletonrobotics.junction.AutoLog; public interface GyroIO { - @AutoLog - public static class GyroIOInputs { + @AutoLog + public static class GyroIOInputs { - public boolean connected = false; - public Rotation2d yawPosition = new Rotation2d(); - public double[] odometryYawTimestamps = new double[] {}; - public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; - public double yawVelocityRadPerSec = 0.0; - } + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + public double yawVelocityRadPerSec = 0.0; + } - public default void updateInputs(GyroIOInputs inputs) {} + public default void updateInputs(GyroIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 5b29f8f..391357c 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -27,51 +27,51 @@ /** IO implementation for Pigeon2 */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(Constants.Drive.Pidgeon2.DEVICE_ID); - private final StatusSignal yaw = pigeon.getYaw(); - private final Queue yawPositionQueue; - private final Queue yawTimestampQueue; - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + private final Pigeon2 pigeon = new Pigeon2(Constants.Drive.Pidgeon2.DEVICE_ID); + private final StatusSignal yaw = pigeon.getYaw(); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - public GyroIOPigeon2(boolean phoenixDrive) { - pigeon.getConfigurator().apply(new Pigeon2Configuration()); - pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(Constants.Drive.Module.ODOMETRY_FREQUENCY); - yawVelocity.setUpdateFrequency(Constants.Drive.Pidgeon2.UPDATE_FREQUENCY); - pigeon.optimizeBusUtilization(); - if (phoenixDrive) { - yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw()); - } else { - yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - boolean valid = yaw.refresh().getStatus().isOK(); - if (valid) { - return OptionalDouble.of(yaw.getValueAsDouble()); - } else { - return OptionalDouble.empty(); - } - }); - } - } + public GyroIOPigeon2(boolean phoenixDrive) { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(Constants.Drive.Module.ODOMETRY_FREQUENCY); + yawVelocity.setUpdateFrequency(Constants.Drive.Pidgeon2.UPDATE_FREQUENCY); + pigeon.optimizeBusUtilization(); + if (phoenixDrive) { + yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = PhoenixOdometryThread.getInstance() + .registerSignal(pigeon, pigeon.getYaw()); + } else { + yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = SparkMaxOdometryThread.getInstance() + .registerSignal(() -> { + boolean valid = yaw.refresh().getStatus().isOK(); + if (valid) { + return OptionalDouble.of(yaw.getValueAsDouble()); + } else { + return OptionalDouble.empty(); + } + }); + } + } - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); - inputs.odometryYawTimestamps = - yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryYawPositions = - yawPositionQueue.stream() - .map((Double value) -> Rotation2d.fromDegrees(value)) - .toArray(Rotation2d[]::new); - yawTimestampQueue.clear(); - yawPositionQueue.clear(); - } + inputs.odometryYawTimestamps = yawTimestampQueue + .stream() + .mapToDouble((Double value) -> value) + .toArray(); + inputs.odometryYawPositions = yawPositionQueue + .stream() + .map((Double value) -> Rotation2d.fromDegrees(value)) + .toArray(Rotation2d[]::new); + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 93d1015..cd764c7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -24,202 +24,208 @@ public class Module { - private static final double WHEEL_RADIUS = Units.inchesToMeters(Constants.Drive.WHEEL_RADIUS); - - private final ModuleIO io; - private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private final int index; - - private final SimpleMotorFeedforward driveFeedforward; - private final PIDController driveFeedback; - private final PIDController turnFeedback; - private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop - private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop - private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute - private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; - - public Module(ModuleIO io, int index) { - this.io = io; - this.index = index; - - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.currentMode) { - // TODO: Configure PID in rela mabye? idk why its off - case REAL: - case REPLAY: - driveFeedforward = - new SimpleMotorFeedforward( - Constants.Drive.Module.REPLAY_FF.kS, Constants.Drive.Module.REPLAY_FF.kV); - driveFeedback = - new PIDController( - Constants.Drive.Module.REPLAY_DRIVE_PID.kP, - Constants.Drive.Module.REPLAY_DRIVE_PID.kI, - Constants.Drive.Module.REPLAY_DRIVE_PID.kD); - turnFeedback = - new PIDController( - Constants.Drive.Module.REPLAY_TURN_PID.kP, - Constants.Drive.Module.REPLAY_TURN_PID.kI, - Constants.Drive.Module.REPLAY_TURN_PID.kD); - break; - case SIM: - driveFeedforward = - new SimpleMotorFeedforward( - Constants.Drive.Module.SIM_FF.kS, Constants.Drive.Module.SIM_FF.kV); - driveFeedback = - new PIDController( - Constants.Drive.Module.SIM_DRIVE_PID.kP, - Constants.Drive.Module.SIM_DRIVE_PID.kI, - Constants.Drive.Module.SIM_DRIVE_PID.kD); - turnFeedback = - new PIDController( - Constants.Drive.Module.SIM_TURN_PID.kP, - Constants.Drive.Module.SIM_TURN_PID.kI, - Constants.Drive.Module.SIM_TURN_PID.kD); - 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; - } - - turnFeedback.enableContinuousInput(-Math.PI, Math.PI); - setBrakeMode(true); - } - - /** - * Update inputs without running the rest of the periodic logic. This is useful since these - * updates need to be properly thread-locked. - */ - public void updateInputs() { - io.updateInputs(inputs); - } - - public void periodic() { - Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); - - // On first cycle, reset relative turn encoder - // Wait until absolute angle is nonzero in case it wasn't initialized yet - if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { - turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); - } - - // Run closed loop turn control - if (angleSetpoint != null) { - io.setTurnVoltage( - turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); - - // Run closed loop drive control - // Only allowed if closed loop turn control is running - if (speedSetpoint != null) { - // Scale velocity based on turn error - // - // When the error is 90°, 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()); - - // Run drive controller - double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; - io.setDriveVoltage( - driveFeedforward.calculate(velocityRadPerSec) - + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); - } - } - - // Calculate positions for odometry - int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together - odometryPositions = new SwerveModulePosition[sampleCount]; - for (int i = 0; i < sampleCount; i++) { - double positionMeters = inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS; - Rotation2d angle = - inputs.odometryTurnPositions[i].plus( - turnRelativeOffset != null ? turnRelativeOffset : new Rotation2d()); - odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); - } - } - - /** Runs the module with the specified setpoint state. Returns the optimized state. */ - public SwerveModuleState runSetpoint(SwerveModuleState state) { - // Optimize state based on current angle - // Controllers run in "periodic" when the setpoint is not null - var optimizedState = SwerveModuleState.optimize(state, getAngle()); - - // Update setpoints, controllers run in "periodic" - angleSetpoint = optimizedState.angle; - speedSetpoint = optimizedState.speedMetersPerSecond; - - return optimizedState; - } - - /** Runs the module with the specified voltage while controlling to zero degrees. */ - public void runCharacterization(double volts) { - // Closed loop turn control - angleSetpoint = new Rotation2d(); - - // Open loop drive control - io.setDriveVoltage(volts); - speedSetpoint = null; - } - - /** Disables all outputs to motors. */ - public void stop() { - io.setTurnVoltage(0.0); - io.setDriveVoltage(0.0); - - // Disable closed loop control for turn and drive - angleSetpoint = null; - speedSetpoint = null; - } - - /** Sets whether brake mode is enabled. */ - public void setBrakeMode(boolean enabled) { - io.setDriveBrakeMode(enabled); - io.setTurnBrakeMode(enabled); - } - - /** Returns the current turn angle of the module. */ - public Rotation2d getAngle() { - if (turnRelativeOffset == null) { - return new Rotation2d(); - } else { - return inputs.turnPosition.plus(turnRelativeOffset); - } - } - - /** Returns the current drive position of the module in meters. */ - public double getPositionMeters() { - return inputs.drivePositionRad * WHEEL_RADIUS; - } - - /** Returns the current drive velocity of the module in meters per second. */ - public double getVelocityMetersPerSec() { - return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; - } - - /** Returns the module position (turn angle and drive position). */ - public SwerveModulePosition getPosition() { - return new SwerveModulePosition(getPositionMeters(), getAngle()); - } - - /** Returns the module state (turn angle and drive velocity). */ - public SwerveModuleState getState() { - return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); - } - - /** Returns the module positions received this cycle. */ - public SwerveModulePosition[] getOdometryPositions() { - return odometryPositions; - } - - /** Returns the timestamps of the samples received this cycle. */ - public double[] getOdometryTimestamps() { - return inputs.odometryTimestamps; - } - - /** Returns the drive velocity in radians/sec. */ - public double getCharacterizationVelocity() { - return inputs.driveVelocityRadPerSec; - } + private static final double WHEEL_RADIUS = Units.inchesToMeters(Constants.Drive.WHEEL_RADIUS); + + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final int index; + + private final SimpleMotorFeedforward driveFeedforward; + private final PIDController driveFeedback; + private final PIDController turnFeedback; + private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop + private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop + private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + + public Module(ModuleIO io, int index) { + this.io = io; + this.index = index; + + // Switch constants based on mode (the physics simulator is treated as a + // separate robot with different tuning) + switch (Constants.currentMode) { + // TODO: Configure PID in rela mabye? idk why its off + case REAL: + case REPLAY: + driveFeedforward = new SimpleMotorFeedforward( + Constants.Drive.Module.REPLAY_FF.kS, + Constants.Drive.Module.REPLAY_FF.kV + ); + driveFeedback = new PIDController( + Constants.Drive.Module.REPLAY_DRIVE_PID.kP, + Constants.Drive.Module.REPLAY_DRIVE_PID.kI, + Constants.Drive.Module.REPLAY_DRIVE_PID.kD + ); + turnFeedback = new PIDController( + Constants.Drive.Module.REPLAY_TURN_PID.kP, + Constants.Drive.Module.REPLAY_TURN_PID.kI, + Constants.Drive.Module.REPLAY_TURN_PID.kD + ); + break; + case SIM: + driveFeedforward = new SimpleMotorFeedforward( + Constants.Drive.Module.SIM_FF.kS, + Constants.Drive.Module.SIM_FF.kV + ); + driveFeedback = new PIDController( + Constants.Drive.Module.SIM_DRIVE_PID.kP, + Constants.Drive.Module.SIM_DRIVE_PID.kI, + Constants.Drive.Module.SIM_DRIVE_PID.kD + ); + turnFeedback = new PIDController( + Constants.Drive.Module.SIM_TURN_PID.kP, + Constants.Drive.Module.SIM_TURN_PID.kI, + Constants.Drive.Module.SIM_TURN_PID.kD + ); + 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; + } + + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + setBrakeMode(true); + } + + /** + * Update inputs without running the rest of the periodic logic. This is useful since these + * updates need to be properly thread-locked. + */ + public void updateInputs() { + io.updateInputs(inputs); + } + + public void periodic() { + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + + // On first cycle, reset relative turn encoder + // Wait until absolute angle is nonzero in case it wasn't initialized yet + if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { + turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); + } + + // Run closed loop turn control + if (angleSetpoint != null) { + io.setTurnVoltage( + turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians()) + ); + + // Run closed loop drive control + // Only allowed if closed loop turn control is running + if (speedSetpoint != null) { + // Scale velocity based on turn error + // + // When the error is 90°, 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()); + + // Run drive controller + double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; + io.setDriveVoltage( + driveFeedforward.calculate(velocityRadPerSec) + + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec) + ); + } + } + + // Calculate positions for odometry + int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together + odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS; + Rotation2d angle = + inputs.odometryTurnPositions[i].plus( + turnRelativeOffset != null ? turnRelativeOffset : new Rotation2d() + ); + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + } + + /** Runs the module with the specified setpoint state. Returns the optimized state. */ + public SwerveModuleState runSetpoint(SwerveModuleState state) { + // Optimize state based on current angle + // Controllers run in "periodic" when the setpoint is not null + var optimizedState = SwerveModuleState.optimize(state, getAngle()); + + // Update setpoints, controllers run in "periodic" + angleSetpoint = optimizedState.angle; + speedSetpoint = optimizedState.speedMetersPerSecond; + + return optimizedState; + } + + /** Runs the module with the specified voltage while controlling to zero degrees. */ + public void runCharacterization(double volts) { + // Closed loop turn control + angleSetpoint = new Rotation2d(); + + // Open loop drive control + io.setDriveVoltage(volts); + speedSetpoint = null; + } + + /** Disables all outputs to motors. */ + public void stop() { + io.setTurnVoltage(0.0); + io.setDriveVoltage(0.0); + + // Disable closed loop control for turn and drive + angleSetpoint = null; + speedSetpoint = null; + } + + /** Sets whether brake mode is enabled. */ + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + if (turnRelativeOffset == null) { + return new Rotation2d(); + } else { + return inputs.turnPosition.plus(turnRelativeOffset); + } + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return inputs.drivePositionRad * WHEEL_RADIUS; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; + } + + /** Returns the module position (turn angle and drive position). */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the module positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return inputs.odometryTimestamps; + } + + /** Returns the drive velocity in radians/sec. */ + public double getCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 1506441..91ba0e3 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -17,37 +17,37 @@ import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { - @AutoLog - public static class ModuleIOInputs { + @AutoLog + public static class ModuleIOInputs { - public double drivePositionRad = 0.0; - public double driveVelocityRadPerSec = 0.0; - public double driveAppliedVolts = 0.0; - public double[] driveCurrentAmps = new double[] {}; + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double[] driveCurrentAmps = new double[] {}; - public Rotation2d turnAbsolutePosition = new Rotation2d(); - public Rotation2d turnPosition = new Rotation2d(); - public double turnVelocityRadPerSec = 0.0; - public double turnAppliedVolts = 0.0; - public double[] turnCurrentAmps = new double[] {}; + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double[] turnCurrentAmps = new double[] {}; - public double[] odometryTimestamps = new double[] {}; - public double[] odometryDrivePositionsRad = new double[] {}; - public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; - } + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } - /** Updates the set of loggable inputs. */ - public default void updateInputs(ModuleIOInputs inputs) {} + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} - /** Run the drive motor at the specified voltage. */ - public default void setDriveVoltage(double volts) {} + /** Run the drive motor at the specified voltage. */ + public default void setDriveVoltage(double volts) {} - /** Run the turn motor at the specified voltage. */ - public default void setTurnVoltage(double volts) {} + /** Run the turn motor at the specified voltage. */ + public default void setTurnVoltage(double volts) {} - /** Enable or disable brake mode on the drive motor. */ - public default void setDriveBrakeMode(boolean enable) {} + /** Enable or disable brake mode on the drive motor. */ + public default void setDriveBrakeMode(boolean enable) {} - /** Enable or disable brake mode on the turn motor. */ - public default void setTurnBrakeMode(boolean enable) {} + /** Enable or disable brake mode on the turn motor. */ + public default void setTurnBrakeMode(boolean enable) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOHybrid.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOHybrid.java index 614c7a4..0c902f1 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOHybrid.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOHybrid.java @@ -28,175 +28,190 @@ */ public class ModuleIOHybrid implements ModuleIO { - private final TalonFX driveTalon; - private final CANSparkMax turnSparkMax; - private final CANcoder cancoder; - - private final Queue timestampQueue; - - private final StatusSignal drivePosition; - private final Queue drivePositionQueue; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - private final RelativeEncoder turnRelativeEncoder; - private final Queue turnPositionQueue; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOHybrid(int index) { - switch (index) { - case 0: - driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE0_ID); - turnSparkMax = - new CANSparkMax(Constants.Drive.Module.Hybrid.TURN0_ID, MotorType.kBrushless); - cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER0_ID); - absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET0); - break; - case 1: - driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE1_ID); - turnSparkMax = - new CANSparkMax(Constants.Drive.Module.Hybrid.TURN1_ID, MotorType.kBrushless); - cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER1_ID); - absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET1); - break; - case 2: - driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE2_ID); - turnSparkMax = - new CANSparkMax(Constants.Drive.Module.Hybrid.TURN2_ID, MotorType.kBrushless); - cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER2_ID); - absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET2); - break; - case 3: - driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE3_ID); - turnSparkMax = - new CANSparkMax(Constants.Drive.Module.Hybrid.TURN3_ID, MotorType.kBrushless); - cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER3_ID); - absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET3); - break; - default: - throw new RuntimeException("Invalid module index"); - } - - // TalonFX configuration - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.SupplyCurrentLimit = - Constants.Drive.Module.Hybrid.DRIVE_CURRENT_LIMIT; - driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - driveTalon.getConfigurator().apply(driveConfig); - setDriveBrakeMode(true); - - cancoder.getConfigurator().apply(new CANcoderConfiguration()); - - timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - - drivePosition = driveTalon.getPosition(); - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getSupplyCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - Constants.Drive.Module.ODOMETRY_FREQUENCY, drivePosition); - BaseStatusSignal.setUpdateFrequencyForAll( - Constants.Drive.Module.Hybrid.TALON_UPDATE_FREQUENCY_HZ, - driveVelocity, - driveAppliedVolts, - driveCurrent); - driveTalon.optimizeBusUtilization(); - - // SparkMax configuration - turnSparkMax.restoreFactoryDefaults(); - turnSparkMax.setCANTimeout(Constants.Drive.Module.Hybrid.SPARK_TIMEOUT_MS); - turnRelativeEncoder = turnSparkMax.getEncoder(); - - turnSparkMax.setInverted(isTurnMotorInverted); - turnSparkMax.setSmartCurrentLimit(Constants.Drive.Module.Hybrid.TURN_CURRENT_LIMIT); - turnSparkMax.enableVoltageCompensation(Constants.MAX_VOLTS); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod( - Constants.Drive.Module.Hybrid.SPARK_MEASURMENT_PERIOD_MS); - turnRelativeEncoder.setAverageDepth(Constants.Drive.Module.Hybrid.SPARK_AVG_DEPTH); - - turnSparkMax.setCANTimeout(0); - turnSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (Constants.Drive.Module.Hybrid.SPARK_FRAME_PERIOD)); - turnPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - double value = turnRelativeEncoder.getPosition(); - if (turnSparkMax.getLastError() == REVLibError.kOk) { - return OptionalDouble.of(value); - } else { - return OptionalDouble.empty(); - } - }); - - turnSparkMax.burnFlash(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); - - // Turn Stuff - inputs.turnAbsolutePosition = - Rotation2d.fromRotations(cancoder.getAbsolutePosition().getValueAsDouble()) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations( - turnRelativeEncoder.getPosition() / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); - inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; - - // Other stuff - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble( - (Double value) -> - Units.rotationsToRadians(value) - / Constants.Drive.Module.Hybrid.DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map( - (Double value) -> - Rotation2d.fromRotations(value / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setTurnVoltage(double volts) { - turnSparkMax.setVoltage(volts); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - driveTalon.getConfigurator().apply(config); - } - - public void setTurnBrakeMode(boolean enable) { - turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } + private final TalonFX driveTalon; + private final CANSparkMax turnSparkMax; + private final CANcoder cancoder; + + private final Queue timestampQueue; + + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + private final RelativeEncoder turnRelativeEncoder; + private final Queue turnPositionQueue; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOHybrid(int index) { + switch (index) { + case 0: + driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE0_ID); + turnSparkMax = new CANSparkMax( + Constants.Drive.Module.Hybrid.TURN0_ID, + MotorType.kBrushless + ); + cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER0_ID); + absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET0); + break; + case 1: + driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE1_ID); + turnSparkMax = new CANSparkMax( + Constants.Drive.Module.Hybrid.TURN1_ID, + MotorType.kBrushless + ); + cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER1_ID); + absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET1); + break; + case 2: + driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE2_ID); + turnSparkMax = new CANSparkMax( + Constants.Drive.Module.Hybrid.TURN2_ID, + MotorType.kBrushless + ); + cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER2_ID); + absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET2); + break; + case 3: + driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE3_ID); + turnSparkMax = new CANSparkMax( + Constants.Drive.Module.Hybrid.TURN3_ID, + MotorType.kBrushless + ); + cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER3_ID); + absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET3); + break; + default: + throw new RuntimeException("Invalid module index"); + } + + // TalonFX configuration + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = + Constants.Drive.Module.Hybrid.DRIVE_CURRENT_LIMIT; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveTalon.getConfigurator().apply(driveConfig); + setDriveBrakeMode(true); + + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + drivePosition = driveTalon.getPosition(); + drivePositionQueue = PhoenixOdometryThread.getInstance() + .registerSignal(driveTalon, driveTalon.getPosition()); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getSupplyCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + Constants.Drive.Module.ODOMETRY_FREQUENCY, + drivePosition + ); + BaseStatusSignal.setUpdateFrequencyForAll( + Constants.Drive.Module.Hybrid.TALON_UPDATE_FREQUENCY_HZ, + driveVelocity, + driveAppliedVolts, + driveCurrent + ); + driveTalon.optimizeBusUtilization(); + + // SparkMax configuration + turnSparkMax.restoreFactoryDefaults(); + turnSparkMax.setCANTimeout(Constants.Drive.Module.Hybrid.SPARK_TIMEOUT_MS); + turnRelativeEncoder = turnSparkMax.getEncoder(); + + turnSparkMax.setInverted(isTurnMotorInverted); + turnSparkMax.setSmartCurrentLimit(Constants.Drive.Module.Hybrid.TURN_CURRENT_LIMIT); + turnSparkMax.enableVoltageCompensation(Constants.MAX_VOLTS); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod( + Constants.Drive.Module.Hybrid.SPARK_MEASURMENT_PERIOD_MS + ); + turnRelativeEncoder.setAverageDepth(Constants.Drive.Module.Hybrid.SPARK_AVG_DEPTH); + + turnSparkMax.setCANTimeout(0); + turnSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, + (int) (Constants.Drive.Module.Hybrid.SPARK_FRAME_PERIOD) + ); + turnPositionQueue = SparkMaxOdometryThread.getInstance() + .registerSignal(() -> { + double value = turnRelativeEncoder.getPosition(); + if (turnSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + + turnSparkMax.burnFlash(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + + // Turn Stuff + inputs.turnAbsolutePosition = Rotation2d.fromRotations( + cancoder.getAbsolutePosition().getValueAsDouble() + ).minus(absoluteEncoderOffset); + inputs.turnPosition = Rotation2d.fromRotations( + turnRelativeEncoder.getPosition() / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO + ); + inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( + turnRelativeEncoder.getVelocity() + ) / + Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] { turnSparkMax.getOutputCurrent() }; + + // Other stuff + inputs.odometryTimestamps = timestampQueue + .stream() + .mapToDouble((Double value) -> value) + .toArray(); + inputs.odometryDrivePositionsRad = drivePositionQueue + .stream() + .mapToDouble( + (Double value) -> + Units.rotationsToRadians(value) / Constants.Drive.Module.Hybrid.DRIVE_GEAR_RATIO + ) + .toArray(); + inputs.odometryTurnPositions = turnPositionQueue + .stream() + .map((Double value) -> + Rotation2d.fromRotations(value / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO) + ) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); + } + + public void setTurnBrakeMode(boolean enable) { + turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index e5f64b7..e552626 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -29,54 +29,56 @@ */ public class ModuleIOSim implements ModuleIO { - private DCMotorSim driveSim = - new DCMotorSim( - DCMotor.getNEO(Constants.Drive.Module.NUM_DRIVE_MOTORS), - Constants.Drive.Module.Sim.DRIVE_GEARING, - Constants.Drive.Module.Sim.DRIVE_MOI); - private DCMotorSim turnSim = - new DCMotorSim( - DCMotor.getNEO(Constants.Drive.Module.NUM_TURN_MOTORS), - Constants.Drive.Module.Sim.TURN_GEARING, - Constants.Drive.Module.Sim.TURN_MOI); + private DCMotorSim driveSim = new DCMotorSim( + DCMotor.getNEO(Constants.Drive.Module.NUM_DRIVE_MOTORS), + Constants.Drive.Module.Sim.DRIVE_GEARING, + Constants.Drive.Module.Sim.DRIVE_MOI + ); + private DCMotorSim turnSim = new DCMotorSim( + DCMotor.getNEO(Constants.Drive.Module.NUM_TURN_MOTORS), + Constants.Drive.Module.Sim.TURN_GEARING, + Constants.Drive.Module.Sim.TURN_MOI + ); - private final Rotation2d turnAbsoluteInitPosition = - new Rotation2d(Math.random() * Constants.RADIAN_CF); - private double driveAppliedVolts = 0.0; - private double turnAppliedVolts = 0.0; + private final Rotation2d turnAbsoluteInitPosition = new Rotation2d( + Math.random() * Constants.RADIAN_CF + ); + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; - @Override - public void updateInputs(ModuleIOInputs inputs) { - driveSim.update(Constants.Drive.Module.Sim.LOOP_PERIOD_SECS); - turnSim.update(Constants.Drive.Module.Sim.LOOP_PERIOD_SECS); + @Override + public void updateInputs(ModuleIOInputs inputs) { + driveSim.update(Constants.Drive.Module.Sim.LOOP_PERIOD_SECS); + turnSim.update(Constants.Drive.Module.Sim.LOOP_PERIOD_SECS); - inputs.drivePositionRad = driveSim.getAngularPositionRad(); - inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); - inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = new double[] { Math.abs(driveSim.getCurrentDrawAmps()) }; - inputs.turnAbsolutePosition = - new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); - inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); - inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); - inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus( + turnAbsoluteInitPosition + ); + inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = new double[] { Math.abs(turnSim.getCurrentDrawAmps()) }; - inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; - inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; - inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; - } + inputs.odometryTimestamps = new double[] { Timer.getFPGATimestamp() }; + inputs.odometryDrivePositionsRad = new double[] { inputs.drivePositionRad }; + inputs.odometryTurnPositions = new Rotation2d[] { inputs.turnPosition }; + } - @Override - public void setDriveVoltage(double volts) { - // :( it doesent work if you clamp volts regularly idk. It really just stopped working - driveAppliedVolts = MathUtil.clamp(volts, Constants.MIN_VOLTS, Constants.MAX_VOLTS); - driveSim.setInputVoltage(volts); - } + @Override + public void setDriveVoltage(double volts) { + // :( it doesent work if you clamp volts regularly idk. It really just stopped working + driveAppliedVolts = MathUtil.clamp(volts, Constants.MIN_VOLTS, Constants.MAX_VOLTS); + driveSim.setInputVoltage(volts); + } - @Override - public void setTurnVoltage(double volts) { - turnAppliedVolts = MathUtil.clamp(volts, Constants.MIN_VOLTS, Constants.MAX_VOLTS); - turnSim.setInputVoltage(turnAppliedVolts); - } + @Override + public void setTurnVoltage(double volts) { + turnAppliedVolts = MathUtil.clamp(volts, Constants.MIN_VOLTS, Constants.MAX_VOLTS); + turnSim.setInputVoltage(turnAppliedVolts); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index a1eeafe..52272ed 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -41,164 +41,169 @@ */ public class ModuleIOSparkMax implements ModuleIO { - // Gear ratios for SDS MK4i L2, adjust as necessary - private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private static final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final CANSparkMax driveSparkMax; - private final CANSparkMax turnSparkMax; - - private final RelativeEncoder driveEncoder; - private final RelativeEncoder turnRelativeEncoder; - private final AnalogInput turnAbsoluteEncoder; - private final Queue timestampQueue; - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOSparkMax(int index) { - switch (index) { - case 0: - driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(2, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(0); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 1: - driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(1); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 2: - driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(6, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 3: - driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(8, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(3); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - default: - throw new RuntimeException("Invalid module index"); - } - - driveSparkMax.restoreFactoryDefaults(); - turnSparkMax.restoreFactoryDefaults(); - - driveSparkMax.setCANTimeout(250); - turnSparkMax.setCANTimeout(250); - - driveEncoder = driveSparkMax.getEncoder(); - turnRelativeEncoder = turnSparkMax.getEncoder(); - - turnSparkMax.setInverted(isTurnMotorInverted); - driveSparkMax.setSmartCurrentLimit(40); - turnSparkMax.setSmartCurrentLimit(30); - driveSparkMax.enableVoltageCompensation(12.0); - turnSparkMax.enableVoltageCompensation(12.0); - - driveEncoder.setPosition(0.0); - driveEncoder.setMeasurementPeriod(10); - driveEncoder.setAverageDepth(2); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod(10); - turnRelativeEncoder.setAverageDepth(2); - - driveSparkMax.setCANTimeout(0); - turnSparkMax.setCANTimeout(0); - - driveSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (1000.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY)); - turnSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (1000.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY)); - timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); - drivePositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - double value = driveEncoder.getPosition(); - if (driveSparkMax.getLastError() == REVLibError.kOk) { - return OptionalDouble.of(value); - } else { - return OptionalDouble.empty(); - } - }); - turnPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - double value = turnRelativeEncoder.getPosition(); - if (turnSparkMax.getLastError() == REVLibError.kOk) { - return OptionalDouble.of(value); - } else { - return OptionalDouble.empty(); - } - }); - - driveSparkMax.burnFlash(); - turnSparkMax.burnFlash(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO; - inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); - inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; - - inputs.turnAbsolutePosition = - new Rotation2d( - (turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) * 2.0 * Math.PI) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); - inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; - - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveSparkMax.setVoltage(volts); - } - - @Override - public void setTurnVoltage(double volts) { - turnSparkMax.setVoltage(volts); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } + // Gear ratios for SDS MK4i L2, adjust as necessary + private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private static final double TURN_GEAR_RATIO = 150.0 / 7.0; + + private final CANSparkMax driveSparkMax; + private final CANSparkMax turnSparkMax; + + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnRelativeEncoder; + private final AnalogInput turnAbsoluteEncoder; + private final Queue timestampQueue; + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOSparkMax(int index) { + switch (index) { + case 0: + driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(2, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(0); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 1: + driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(1); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 2: + driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(6, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(2); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 3: + driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(8, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(3); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + default: + throw new RuntimeException("Invalid module index"); + } + + driveSparkMax.restoreFactoryDefaults(); + turnSparkMax.restoreFactoryDefaults(); + + driveSparkMax.setCANTimeout(250); + turnSparkMax.setCANTimeout(250); + + driveEncoder = driveSparkMax.getEncoder(); + turnRelativeEncoder = turnSparkMax.getEncoder(); + + turnSparkMax.setInverted(isTurnMotorInverted); + driveSparkMax.setSmartCurrentLimit(40); + turnSparkMax.setSmartCurrentLimit(30); + driveSparkMax.enableVoltageCompensation(12.0); + turnSparkMax.enableVoltageCompensation(12.0); + + driveEncoder.setPosition(0.0); + driveEncoder.setMeasurementPeriod(10); + driveEncoder.setAverageDepth(2); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + + driveSparkMax.setCANTimeout(0); + turnSparkMax.setCANTimeout(0); + + driveSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, + (int) (1000.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY) + ); + turnSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, + (int) (1000.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY) + ); + timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); + drivePositionQueue = SparkMaxOdometryThread.getInstance() + .registerSignal(() -> { + double value = driveEncoder.getPosition(); + if (driveSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + turnPositionQueue = SparkMaxOdometryThread.getInstance() + .registerSignal(() -> { + double value = turnRelativeEncoder.getPosition(); + if (turnSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + + driveSparkMax.burnFlash(); + turnSparkMax.burnFlash(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + inputs.drivePositionRad = Units.rotationsToRadians(driveEncoder.getPosition()) / + DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( + driveEncoder.getVelocity() + ) / + DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); + inputs.driveCurrentAmps = new double[] { driveSparkMax.getOutputCurrent() }; + + inputs.turnAbsolutePosition = new Rotation2d( + (turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) * 2.0 * Math.PI + ).minus(absoluteEncoderOffset); + inputs.turnPosition = Rotation2d.fromRotations( + turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO + ); + inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( + turnRelativeEncoder.getVelocity() + ) / + TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] { turnSparkMax.getOutputCurrent() }; + + inputs.odometryTimestamps = timestampQueue + .stream() + .mapToDouble((Double value) -> value) + .toArray(); + inputs.odometryDrivePositionsRad = drivePositionQueue + .stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) + .toArray(); + inputs.odometryTurnPositions = turnPositionQueue + .stream() + .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveSparkMax.setVoltage(volts); + } + + @Override + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 152f7e9..7b1caa4 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -42,179 +42,186 @@ */ public class ModuleIOTalonFX implements ModuleIO { - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final CANcoder cancoder; - - private final Queue timestampQueue; - - private final StatusSignal drivePosition; - private final Queue drivePositionQueue; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; - private final Queue turnPositionQueue; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - - // Gear ratios for SDS MK4i L2, adjust as necessary - private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - 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 - break; - case 1: - driveTalon = new TalonFX(3); - turnTalon = new TalonFX(4); - cancoder = new CANcoder(5); - absoluteEncoderOffset = new Rotation2d(0.0); // 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 - break; - case 3: - driveTalon = new TalonFX(9); - turnTalon = new TalonFX(10); - cancoder = new CANcoder(11); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - default: - throw new RuntimeException("Invalid module index"); - } - - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; - driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - driveTalon.getConfigurator().apply(driveConfig); - setDriveBrakeMode(true); - - var turnConfig = new TalonFXConfiguration(); - turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; - turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - turnTalon.getConfigurator().apply(turnConfig); - setTurnBrakeMode(true); - - cancoder.getConfigurator().apply(new CANcoderConfiguration()); - - timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - - drivePosition = driveTalon.getPosition(); - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getSupplyCurrent(); - - turnAbsolutePosition = cancoder.getAbsolutePosition(); - turnPosition = turnTalon.getPosition(); - turnPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getSupplyCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - Constants.Drive.Module.ODOMETRY_FREQUENCY, drivePosition, turnPosition); - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - driveTalon.optimizeBusUtilization(); - turnTalon.optimizeBusUtilization(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - inputs.drivePositionRad = - Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; - - inputs.turnAbsolutePosition = - Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; - - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setTurnVoltage(double volts) { - turnTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - driveTalon.getConfigurator().apply(config); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = - isTurnMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - turnTalon.getConfigurator().apply(config); - } + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final CANcoder cancoder; + + private final Queue timestampQueue; + + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final Queue turnPositionQueue; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Gear ratios for SDS MK4i L2, adjust as necessary + private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private final double TURN_GEAR_RATIO = 150.0 / 7.0; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + 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 + break; + case 1: + driveTalon = new TalonFX(3); + turnTalon = new TalonFX(4); + cancoder = new CANcoder(5); + absoluteEncoderOffset = new Rotation2d(0.0); // 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 + break; + case 3: + driveTalon = new TalonFX(9); + turnTalon = new TalonFX(10); + cancoder = new CANcoder(11); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + default: + throw new RuntimeException("Invalid module index"); + } + + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveTalon.getConfigurator().apply(driveConfig); + setDriveBrakeMode(true); + + var turnConfig = new TalonFXConfiguration(); + turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + turnTalon.getConfigurator().apply(turnConfig); + setTurnBrakeMode(true); + + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + drivePosition = driveTalon.getPosition(); + drivePositionQueue = PhoenixOdometryThread.getInstance() + .registerSignal(driveTalon, driveTalon.getPosition()); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getSupplyCurrent(); + + turnAbsolutePosition = cancoder.getAbsolutePosition(); + turnPosition = turnTalon.getPosition(); + turnPositionQueue = PhoenixOdometryThread.getInstance() + .registerSignal(turnTalon, turnTalon.getPosition()); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getSupplyCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + Constants.Drive.Module.ODOMETRY_FREQUENCY, + drivePosition, + turnPosition + ); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent + ); + driveTalon.optimizeBusUtilization(); + turnTalon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnCurrent + ); + + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()) / + DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / + DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = new double[] { driveCurrent.getValueAsDouble() }; + + inputs.turnAbsolutePosition = Rotation2d.fromRotations( + turnAbsolutePosition.getValueAsDouble() + ).minus(absoluteEncoderOffset); + inputs.turnPosition = Rotation2d.fromRotations( + turnPosition.getValueAsDouble() / TURN_GEAR_RATIO + ); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / + TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = new double[] { turnCurrent.getValueAsDouble() }; + + inputs.odometryTimestamps = timestampQueue + .stream() + .mapToDouble((Double value) -> value) + .toArray(); + inputs.odometryDrivePositionsRad = drivePositionQueue + .stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) + .toArray(); + inputs.odometryTurnPositions = turnPositionQueue + .stream() + .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setTurnVoltage(double volts) { + turnTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = isTurnMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + turnTalon.getConfigurator().apply(config); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java b/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java index 216939e..7609aac 100644 --- a/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java +++ b/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java @@ -10,63 +10,62 @@ import frc.robot.Constants; public class PPDriveWrapper extends SubsystemBase { - Drive drive; - PPDriveWrapper(Drive drive) { - this.drive = drive; - pathPlannerInit(); - } + Drive drive; - private Pose2d getPose() { - return drive.getPose(); - } + PPDriveWrapper(Drive drive) { + this.drive = drive; + pathPlannerInit(); + } - private void setPose(Pose2d pose) { - drive.setPose(pose); - } + private Pose2d getPose() { + return drive.getPose(); + } - private ChassisSpeeds getChassisSpeed() { - return drive.getChassisSpeed(); - } + private void setPose(Pose2d pose) { + drive.setPose(pose); + } - private void runVelocity(ChassisSpeeds chassisSpeeds) { - drive.runVelocity(chassisSpeeds); - } + private ChassisSpeeds getChassisSpeed() { + return drive.getChassisSpeed(); + } - public void periodic() {} + private void runVelocity(ChassisSpeeds chassisSpeeds) { + drive.runVelocity(chassisSpeeds); + } - public void pathPlannerInit() { - AutoBuilder.configureHolonomic( - this::getPose, // Robot pose supplier - this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) - this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds - new HolonomicPathFollowerConfig( - // HolonomicPathFollowerConfig, this should likely live in - // your Constants class - Constants.Drive.TRANSLATION_PID, // Translation PID constants - Constants.Drive.ROTATION_PID, // Rotation PID constants - Constants.Drive.MAX_MODULE_SPEED, // Max module speed, in m/s - Constants.Drive - .DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to - // furthest module. - // Replans path if vision or odo detects errors (S tier) - new ReplanningConfig( - true, true) // Default path replanning config. See the API for the options - // here - ), - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + public void periodic() {} - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - ); - } + public void pathPlannerInit() { + AutoBuilder.configureHolonomic( + this::getPose, // Robot pose supplier + this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + new HolonomicPathFollowerConfig( + // HolonomicPathFollowerConfig, this should likely live in + // your Constants class + Constants.Drive.TRANSLATION_PID, // Translation PID constants + Constants.Drive.ROTATION_PID, // Rotation PID constants + Constants.Drive.MAX_MODULE_SPEED, // Max module speed, in m/s + Constants.Drive.DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to + // furthest module. + // Replans path if vision or odo detects errors (S tier) + new ReplanningConfig(true, true) // Default path replanning config. See the API for the options + // here + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 5677c81..bf21e7a 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -36,108 +36,114 @@ */ public class PhoenixOdometryThread extends Thread { - private final Lock signalsLock = - new ReentrantLock(); // Prevents conflicts when registering signals - private BaseStatusSignal[] signals = new BaseStatusSignal[0]; - private final List> queues = new ArrayList<>(); - private final List> timestampQueues = new ArrayList<>(); - private boolean isCANFD = false; + private final Lock signalsLock = new ReentrantLock(); // Prevents conflicts when registering signals + private BaseStatusSignal[] signals = new BaseStatusSignal[0]; + private final List> queues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); + private boolean isCANFD = false; - private static PhoenixOdometryThread instance = null; + private static PhoenixOdometryThread instance = null; - public static PhoenixOdometryThread getInstance() { - if (instance == null) { - instance = new PhoenixOdometryThread(); - } - return instance; - } + public static PhoenixOdometryThread getInstance() { + if (instance == null) { + instance = new PhoenixOdometryThread(); + } + return instance; + } - private PhoenixOdometryThread() { - setName("PhoenixOdometryThread"); - setDaemon(true); - } + private PhoenixOdometryThread() { + setName("PhoenixOdometryThread"); + setDaemon(true); + } - @Override - public void start() { - if (timestampQueues.size() > 0) { - super.start(); - } - } + @Override + public void start() { + if (timestampQueues.size() > 0) { + super.start(); + } + } - public Queue registerSignal(ParentDevice device, StatusSignal signal) { - Queue queue = new ArrayBlockingQueue<>(Constants.Drive.OdoThread.Phoenix.QUE_CAPACITY); - signalsLock.lock(); - Drive.odometryLock.lock(); - try { - isCANFD = CANBus.isNetworkFD(device.getNetwork()); - BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; - System.arraycopy(signals, 0, newSignals, 0, signals.length); - newSignals[signals.length] = signal; - signals = newSignals; - queues.add(queue); - } finally { - signalsLock.unlock(); - Drive.odometryLock.unlock(); - } - return queue; - } + public Queue registerSignal(ParentDevice device, StatusSignal signal) { + Queue queue = new ArrayBlockingQueue<>( + Constants.Drive.OdoThread.Phoenix.QUE_CAPACITY + ); + signalsLock.lock(); + Drive.odometryLock.lock(); + try { + isCANFD = CANBus.isNetworkFD(device.getNetwork()); + BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; + System.arraycopy(signals, 0, newSignals, 0, signals.length); + newSignals[signals.length] = signal; + signals = newSignals; + queues.add(queue); + } finally { + signalsLock.unlock(); + Drive.odometryLock.unlock(); + } + return queue; + } - public Queue makeTimestampQueue() { - Queue queue = new ArrayBlockingQueue<>(Constants.Drive.OdoThread.Phoenix.QUE_CAPACITY); - Drive.odometryLock.lock(); - try { - timestampQueues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>( + Constants.Drive.OdoThread.Phoenix.QUE_CAPACITY + ); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } - @Override - public void run() { - while (true) { - // Wait for updates from all signals - signalsLock.lock(); - try { - if (isCANFD) { - BaseStatusSignal.waitForAll(2.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY, signals); - } else { - // "waitForAll" does not support blocking on multiple - // signals with a bus that is not CAN FD, regardless - // of Pro licensing. No reasoning for this behavior - // is provided by the documentation. - Thread.sleep( - (long) - (Constants.Drive.OdoThread.Phoenix.SLEEP_TIME - / Constants.Drive.Module.ODOMETRY_FREQUENCY)); - if (signals.length > 0) BaseStatusSignal.refreshAll(signals); - } - } catch (InterruptedException e) { - e.printStackTrace(); - } finally { - signalsLock.unlock(); - } + @Override + public void run() { + while (true) { + // Wait for updates from all signals + signalsLock.lock(); + try { + if (isCANFD) { + BaseStatusSignal.waitForAll( + 2.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY, + signals + ); + } else { + // "waitForAll" does not support blocking on multiple + // signals with a bus that is not CAN FD, regardless + // of Pro licensing. No reasoning for this behavior + // is provided by the documentation. + Thread.sleep( + (long) (Constants.Drive.OdoThread.Phoenix.SLEEP_TIME / + Constants.Drive.Module.ODOMETRY_FREQUENCY) + ); + if (signals.length > 0) BaseStatusSignal.refreshAll(signals); + } + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + signalsLock.unlock(); + } - // Save new data to queues - Drive.odometryLock.lock(); - try { - double timestamp = Logger.getRealTimestamp() / 1e6; - double totalLatency = 0.0; - for (BaseStatusSignal signal : signals) { - totalLatency += signal.getTimestamp().getLatency(); - } - if (signals.length > 0) { - timestamp -= totalLatency / signals.length; - } - for (int i = 0; i < signals.length; i++) { - queues.get(i).offer(signals[i].getValueAsDouble()); - } - for (int i = 0; i < timestampQueues.size(); i++) { - timestampQueues.get(i).offer(timestamp); - } - } finally { - Drive.odometryLock.unlock(); - } - } - } + // Save new data to queues + Drive.odometryLock.lock(); + try { + double timestamp = Logger.getRealTimestamp() / 1e6; + double totalLatency = 0.0; + for (BaseStatusSignal signal : signals) { + totalLatency += signal.getTimestamp().getLatency(); + } + if (signals.length > 0) { + timestamp -= totalLatency / signals.length; + } + for (int i = 0; i < signals.length; i++) { + queues.get(i).offer(signals[i].getValueAsDouble()); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } finally { + Drive.odometryLock.unlock(); + } + } + } } diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index cad718f..fea8279 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -31,80 +31,86 @@ */ public class SparkMaxOdometryThread { - private List> signals = new ArrayList<>(); - private List> queues = new ArrayList<>(); - private List> timestampQueues = new ArrayList<>(); + private List> signals = new ArrayList<>(); + private List> queues = new ArrayList<>(); + private List> timestampQueues = new ArrayList<>(); - private final Notifier notifier; - private static SparkMaxOdometryThread instance = null; + private final Notifier notifier; + private static SparkMaxOdometryThread instance = null; - public static SparkMaxOdometryThread getInstance() { - if (instance == null) { - instance = new SparkMaxOdometryThread(); - } - return instance; - } + public static SparkMaxOdometryThread getInstance() { + if (instance == null) { + instance = new SparkMaxOdometryThread(); + } + return instance; + } - private SparkMaxOdometryThread() { - notifier = new Notifier(this::periodic); - notifier.setName("SparkMaxOdometryThread"); - } + private SparkMaxOdometryThread() { + notifier = new Notifier(this::periodic); + notifier.setName("SparkMaxOdometryThread"); + } - public void start() { - if (timestampQueues.size() > 0) { - notifier.startPeriodic( - Constants.Drive.OdoThread.SparkMax.PERIOD / Constants.Drive.Module.ODOMETRY_FREQUENCY); - } - } + public void start() { + if (timestampQueues.size() > 0) { + notifier.startPeriodic( + Constants.Drive.OdoThread.SparkMax.PERIOD / + Constants.Drive.Module.ODOMETRY_FREQUENCY + ); + } + } - public Queue registerSignal(Supplier signal) { - Queue queue = new ArrayBlockingQueue<>(Constants.Drive.OdoThread.SparkMax.QUE_CAPACITY); - Drive.odometryLock.lock(); - try { - signals.add(signal); - queues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } + public Queue registerSignal(Supplier signal) { + Queue queue = new ArrayBlockingQueue<>( + Constants.Drive.OdoThread.SparkMax.QUE_CAPACITY + ); + Drive.odometryLock.lock(); + try { + signals.add(signal); + queues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } - public Queue makeTimestampQueue() { - Queue queue = new ArrayBlockingQueue<>(Constants.Drive.OdoThread.SparkMax.QUE_CAPACITY); - Drive.odometryLock.lock(); - try { - timestampQueues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>( + Constants.Drive.OdoThread.SparkMax.QUE_CAPACITY + ); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } - private void periodic() { - Drive.odometryLock.lock(); - double timestamp = Logger.getRealTimestamp() / 1e6; - try { - double[] values = new double[signals.size()]; - boolean isValid = true; - for (int i = 0; i < signals.size(); i++) { - OptionalDouble value = signals.get(i).get(); - if (value.isPresent()) { - values[i] = value.getAsDouble(); - } else { - isValid = false; - break; - } - } - if (isValid) { - for (int i = 0; i < queues.size(); i++) { - queues.get(i).offer(values[i]); - } - for (int i = 0; i < timestampQueues.size(); i++) { - timestampQueues.get(i).offer(timestamp); - } - } - } finally { - Drive.odometryLock.unlock(); - } - } + private void periodic() { + Drive.odometryLock.lock(); + double timestamp = Logger.getRealTimestamp() / 1e6; + try { + double[] values = new double[signals.size()]; + boolean isValid = true; + for (int i = 0; i < signals.size(); i++) { + OptionalDouble value = signals.get(i).get(); + if (value.isPresent()) { + values[i] = value.getAsDouble(); + } else { + isValid = false; + break; + } + } + if (isValid) { + for (int i = 0; i < queues.size(); i++) { + queues.get(i).offer(values[i]); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } + } finally { + Drive.odometryLock.unlock(); + } + } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index c37455c..0bae12c 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -10,52 +10,58 @@ public class Intake extends Subsystem { - IntakeIO io; - IntakeIOInputsAutoLogged inputs; - - public Intake(IntakeIO io) { - super("Intake", IntakeStates.OFF); - this.io = io; - inputs = new IntakeIOInputsAutoLogged(); - - // Configure PIDs here - switch (Constants.currentMode) { - case REAL: - io.configurePID(Constants.Intake.REAL_OUT_PID, Constants.Intake.REAL_IN_PID); - break; - case REPLAY: - io.configurePID(new PIDConstants(0, 0, 0), new PIDConstants(0, 0, 0)); - break; - case SIM: - io.configurePID(Constants.Intake.REAL_OUT_PID, Constants.Intake.REAL_IN_PID); - break; - default: - break; - } - } - - protected void runState() { - io.setSetpoints( - getState().getPivotSetPoint(), getState().getMotorSetPoint(), getState().getUsingPID()); - if (getState() == IntakeStates.INTAKING) { - NoteSimulator.attachToShooter(); - } - } - - public void stop() { - io.stop(); - } - - @Override - public void periodic() { - super.periodic(); - - Logger.recordOutput( - "Intake/Intake Pose", - new Pose3d( - Constants.Intake.ZEROED_PIVOT_TRANSLATION, new Rotation3d(0, io.getPosition(), 0))); - - Logger.processInputs("Intake", inputs); - io.updateInputs(inputs); - } + IntakeIO io; + IntakeIOInputsAutoLogged inputs; + + public Intake(IntakeIO io) { + super("Intake", IntakeStates.OFF); + this.io = io; + inputs = new IntakeIOInputsAutoLogged(); + + // Configure PIDs here + switch (Constants.currentMode) { + case REAL: + io.configurePID(Constants.Intake.REAL_OUT_PID, Constants.Intake.REAL_IN_PID); + break; + case REPLAY: + io.configurePID(new PIDConstants(0, 0, 0), new PIDConstants(0, 0, 0)); + break; + case SIM: + io.configurePID(Constants.Intake.REAL_OUT_PID, Constants.Intake.REAL_IN_PID); + break; + default: + break; + } + } + + protected void runState() { + io.setSetpoints( + getState().getPivotSetPoint(), + getState().getMotorSetPoint(), + getState().getUsingPID() + ); + if (getState() == IntakeStates.INTAKING) { + NoteSimulator.attachToShooter(); + } + } + + public void stop() { + io.stop(); + } + + @Override + public void periodic() { + super.periodic(); + + Logger.recordOutput( + "Intake/Intake Pose", + new Pose3d( + Constants.Intake.ZEROED_PIVOT_TRANSLATION, + new Rotation3d(0, io.getPosition(), 0) + ) + ); + + Logger.processInputs("Intake", inputs); + io.updateInputs(inputs); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 1363c5b..119e7ca 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -5,33 +5,36 @@ import org.littletonrobotics.junction.AutoLog; public interface IntakeIO { - @AutoLog - public static class IntakeIOInputs { + @AutoLog + public static class IntakeIOInputs { - public Pose3d position; + public Pose3d position; - public double wheelSpeed; - public double wheelAppliedVoltage; - public double wheelSpeedpoint; + public double wheelSpeed; + public double wheelAppliedVoltage; + public double wheelSpeedpoint; - public double pivotPosition; - public double pivotAppliedVoltage; - public double pivotSetpoint; - public double pivotSetpointError; + public double pivotPosition; + public double pivotAppliedVoltage; + public double pivotSetpoint; + public double pivotSetpointError; - public boolean usingInPID; - } + public boolean usingInPID; + } - public default void updateInputs(IntakeIOInputs inputs) {} + public default void updateInputs(IntakeIOInputs inputs) {} - public default void setSetpoints( - double pivotMotorSetpoint, double intakeMotorSetpoint, boolean useIn) {} + public default void setSetpoints( + double pivotMotorSetpoint, + double intakeMotorSetpoint, + boolean useIn + ) {} - public default void stop() {} + public default void stop() {} - public default double getPosition() { - return 0.0; - } + public default double getPosition() { + return 0.0; + } - public default void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) {} + public default void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index dcdde34..fcf00f5 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -9,92 +9,97 @@ public class IntakeIOSim implements IntakeIO { - SingleJointedArmSim pivotSimModel; - DCMotorSim spinnerWheelSim; - - PIDController outPivotController; - PIDController inPIDController; - - PIDController pivotController; - - double wheelAppliedVoltage; - double pivotAppliedVoltage; - - double wheelSpeedpoint; - double pivotSetpoint; - - boolean usingInPID; - - public IntakeIOSim() { - pivotSimModel = - new SingleJointedArmSim( - DCMotor.getKrakenX60(Constants.Intake.NUM_PIVOT_MOTORS), - Constants.Intake.PIVOT_GEARING, // gearing - Constants.Intake.PIVOT_MOI, // MOI - Constants.Intake.PIVOT_LENGTH_METERS, // arm length - 0, // min angle -- hard stop - Constants.Intake.MAX_PIVOT_POSITION, // max angle -- floor - false, - 0); - - spinnerWheelSim = - new DCMotorSim( - DCMotor.getFalcon500(Constants.Intake.NUM_SPINNER_MOTORS), - Constants.Intake.SPINNER_GEARING, - Constants.Intake.SPINNER_MOI); - - outPivotController = new PIDController(0, 0, 0); - inPIDController = new PIDController(0, 0, 0); - } - - public void setSetpoints( - double pivotMotorSetPoint, double intakeMotorSetpoint, boolean useInPID) { - usingInPID = useInPID; - - if (useInPID) pivotController = inPIDController; - else pivotController = outPivotController; - - pivotAppliedVoltage = - pivotController.calculate(pivotSimModel.getAngleRads(), pivotMotorSetPoint); - - wheelAppliedVoltage = intakeMotorSetpoint; - - pivotSimModel.setInputVoltage(pivotAppliedVoltage); - spinnerWheelSim.setInputVoltage(intakeMotorSetpoint); - - wheelSpeedpoint = intakeMotorSetpoint; - pivotSetpoint = pivotMotorSetPoint; - } - - public void updateInputs(IntakeIOInputs inputs) { - inputs.wheelSpeed = spinnerWheelSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - inputs.wheelAppliedVoltage = wheelAppliedVoltage; - inputs.wheelSpeedpoint = wheelSpeedpoint; - - inputs.pivotPosition = pivotSimModel.getAngleRads(); - inputs.pivotAppliedVoltage = pivotAppliedVoltage; - inputs.pivotSetpoint = pivotSetpoint; - inputs.pivotSetpointError = Math.abs(pivotSimModel.getAngleRads() - pivotSetpoint); - - inputs.usingInPID = usingInPID; - - pivotSimModel.update(Constants.SIM_UPDATE_TIME); - spinnerWheelSim.update(Constants.SIM_UPDATE_TIME); - } - - public double getPosition() { - return pivotSimModel.getAngleRads(); - } - - public void stop() { - wheelAppliedVoltage = 0.0; - pivotAppliedVoltage = 0.0; - pivotSimModel.setInputVoltage(0); - spinnerWheelSim.setInputVoltage(0); - } - - public void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) { - outPivotController.setPID(outPIDConst.kP, outPIDConst.kI, outPIDConst.kD); - inPIDController.setPID(inPIPidConst.kP, inPIPidConst.kI, inPIPidConst.kD); - } + SingleJointedArmSim pivotSimModel; + DCMotorSim spinnerWheelSim; + + PIDController outPivotController; + PIDController inPIDController; + + PIDController pivotController; + + double wheelAppliedVoltage; + double pivotAppliedVoltage; + + double wheelSpeedpoint; + double pivotSetpoint; + + boolean usingInPID; + + public IntakeIOSim() { + pivotSimModel = new SingleJointedArmSim( + DCMotor.getKrakenX60(Constants.Intake.NUM_PIVOT_MOTORS), + Constants.Intake.PIVOT_GEARING, // gearing + Constants.Intake.PIVOT_MOI, // MOI + Constants.Intake.PIVOT_LENGTH_METERS, // arm length + 0, // min angle -- hard stop + Constants.Intake.MAX_PIVOT_POSITION, // max angle -- floor + false, + 0 + ); + + spinnerWheelSim = new DCMotorSim( + DCMotor.getFalcon500(Constants.Intake.NUM_SPINNER_MOTORS), + Constants.Intake.SPINNER_GEARING, + Constants.Intake.SPINNER_MOI + ); + + outPivotController = new PIDController(0, 0, 0); + inPIDController = new PIDController(0, 0, 0); + } + + public void setSetpoints( + double pivotMotorSetPoint, + double intakeMotorSetpoint, + boolean useInPID + ) { + usingInPID = useInPID; + + if (useInPID) pivotController = inPIDController; + else pivotController = outPivotController; + + pivotAppliedVoltage = pivotController.calculate( + pivotSimModel.getAngleRads(), + pivotMotorSetPoint + ); + + wheelAppliedVoltage = intakeMotorSetpoint; + + pivotSimModel.setInputVoltage(pivotAppliedVoltage); + spinnerWheelSim.setInputVoltage(intakeMotorSetpoint); + + wheelSpeedpoint = intakeMotorSetpoint; + pivotSetpoint = pivotMotorSetPoint; + } + + public void updateInputs(IntakeIOInputs inputs) { + inputs.wheelSpeed = spinnerWheelSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + inputs.wheelAppliedVoltage = wheelAppliedVoltage; + inputs.wheelSpeedpoint = wheelSpeedpoint; + + inputs.pivotPosition = pivotSimModel.getAngleRads(); + inputs.pivotAppliedVoltage = pivotAppliedVoltage; + inputs.pivotSetpoint = pivotSetpoint; + inputs.pivotSetpointError = Math.abs(pivotSimModel.getAngleRads() - pivotSetpoint); + + inputs.usingInPID = usingInPID; + + pivotSimModel.update(Constants.SIM_UPDATE_TIME); + spinnerWheelSim.update(Constants.SIM_UPDATE_TIME); + } + + public double getPosition() { + return pivotSimModel.getAngleRads(); + } + + public void stop() { + wheelAppliedVoltage = 0.0; + pivotAppliedVoltage = 0.0; + pivotSimModel.setInputVoltage(0); + spinnerWheelSim.setInputVoltage(0); + } + + public void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) { + outPivotController.setPID(outPIDConst.kP, outPIDConst.kI, outPIDConst.kD); + inPIDController.setPID(inPIPidConst.kP, inPIPidConst.kI, inPIPidConst.kD); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index 73c4f47..f09ec0e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -10,81 +10,87 @@ public class IntakeIOSparkMax implements IntakeIO { - private CANSparkMax pivotMotor; - public TalonFX intakeMotor; - private RelativeEncoder pivotEncoder; + private CANSparkMax pivotMotor; + public TalonFX intakeMotor; + private RelativeEncoder pivotEncoder; - PIDController outPivotController; - PIDController inPIDController; + PIDController outPivotController; + PIDController inPIDController; - PIDController pivotController; + PIDController pivotController; - double pivotMotorSetpoint = 0.0; - double intakeMotorSetpoint = 0.0; + double pivotMotorSetpoint = 0.0; + double intakeMotorSetpoint = 0.0; - double wheelAppliedVoltage; - double pivotAppliedVoltage; + double wheelAppliedVoltage; + double pivotAppliedVoltage; - double wheelSpeedpoint; - double pivotSetpoint; + double wheelSpeedpoint; + double pivotSetpoint; - boolean usingInPID; + boolean usingInPID; - public IntakeIOSparkMax() { - intakeMotor = new TalonFX(Constants.Intake.SPINNER_ID); - pivotMotor = new CANSparkMax(Constants.Intake.PIVOT_ID, MotorType.kBrushless); - pivotEncoder = pivotMotor.getEncoder(); + public IntakeIOSparkMax() { + intakeMotor = new TalonFX(Constants.Intake.SPINNER_ID); + pivotMotor = new CANSparkMax(Constants.Intake.PIVOT_ID, MotorType.kBrushless); + pivotEncoder = pivotMotor.getEncoder(); - outPivotController = new PIDController(0, 0, 0); - inPIDController = new PIDController(0, 0, 0); + outPivotController = new PIDController(0, 0, 0); + inPIDController = new PIDController(0, 0, 0); - pivotEncoder.setPositionConversionFactor(Constants.RADIAN_CF); - pivotEncoder.setVelocityConversionFactor(Constants.RADIAN_CF); - } + pivotEncoder.setPositionConversionFactor(Constants.RADIAN_CF); + pivotEncoder.setVelocityConversionFactor(Constants.RADIAN_CF); + } - public void setSetpoints( - double pivotMotorSetpoint, double intakeMotorSetpoint, boolean useInPID) { - usingInPID = useInPID; + public void setSetpoints( + double pivotMotorSetpoint, + double intakeMotorSetpoint, + boolean useInPID + ) { + usingInPID = useInPID; - if (useInPID) pivotController = inPIDController; - else pivotController = outPivotController; + if (useInPID) pivotController = inPIDController; + else pivotController = outPivotController; - pivotAppliedVoltage = pivotController.calculate(pivotEncoder.getPosition(), pivotMotorSetpoint); - wheelAppliedVoltage = intakeMotorSetpoint; + pivotAppliedVoltage = pivotController.calculate( + pivotEncoder.getPosition(), + pivotMotorSetpoint + ); + wheelAppliedVoltage = intakeMotorSetpoint; - pivotMotor.setVoltage(pivotAppliedVoltage); - intakeMotor.setVoltage(wheelAppliedVoltage); + pivotMotor.setVoltage(pivotAppliedVoltage); + intakeMotor.setVoltage(wheelAppliedVoltage); - wheelSpeedpoint = intakeMotorSetpoint; - pivotSetpoint = pivotMotorSetpoint; - } + wheelSpeedpoint = intakeMotorSetpoint; + pivotSetpoint = pivotMotorSetpoint; + } - public void updateInputs(IntakeIOInputs inputs) { - inputs.wheelSpeed = intakeMotor.getVelocity().getValueAsDouble(); - inputs.wheelAppliedVoltage = wheelAppliedVoltage; - inputs.wheelSpeedpoint = wheelSpeedpoint; + public void updateInputs(IntakeIOInputs inputs) { + inputs.wheelSpeed = intakeMotor.getVelocity().getValueAsDouble(); + inputs.wheelAppliedVoltage = wheelAppliedVoltage; + inputs.wheelSpeedpoint = wheelSpeedpoint; - inputs.pivotPosition = pivotEncoder.getPosition(); - inputs.pivotAppliedVoltage = pivotAppliedVoltage; - inputs.pivotSetpoint = pivotSetpoint; - inputs.pivotSetpointError = Math.abs(pivotEncoder.getPosition() - pivotSetpoint); + inputs.pivotPosition = pivotEncoder.getPosition(); + inputs.pivotAppliedVoltage = pivotAppliedVoltage; + inputs.pivotSetpoint = pivotSetpoint; + inputs.pivotSetpointError = Math.abs(pivotEncoder.getPosition() - pivotSetpoint); - inputs.usingInPID = usingInPID; - } + inputs.usingInPID = usingInPID; + } - public double getPosition() { - return pivotEncoder.getPosition(); - } + public double getPosition() { + return pivotEncoder.getPosition(); + } - public void stop() { - wheelAppliedVoltage = 0.0; - pivotAppliedVoltage = 0.0; - pivotMotor.stopMotor(); - intakeMotor.stopMotor(); - } + public void stop() { + wheelAppliedVoltage = 0.0; + pivotAppliedVoltage = 0.0; + pivotMotor.stopMotor(); + intakeMotor.stopMotor(); + } - public void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) { - outPivotController.setPID(outPIDConst.kP, outPIDConst.kI, outPIDConst.kD); - inPIDController.setPID(inPIPidConst.kP, inPIPidConst.kI, inPIPidConst.kD); - } + public void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) { + outPivotController.setPID(outPIDConst.kP, outPIDConst.kI, outPIDConst.kD); + inPIDController.setPID(inPIPidConst.kP, inPIPidConst.kI, inPIPidConst.kD); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeStates.java b/src/main/java/frc/robot/subsystems/intake/IntakeStates.java index 9c2a950..0537920 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeStates.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeStates.java @@ -4,37 +4,41 @@ import frc.robot.subsystems.*; public enum IntakeStates implements SubsystemStates { - OFF("Off", Constants.Intake.IN, Constants.Intake.OFF, true), - INTAKING("Intaking", Constants.Intake.DOWN, Constants.Intake.ON, false), - FEEDING("Feeding", Constants.Intake.IN, Constants.Intake.REVERSE, true), - OUTTAKING("Outtaking", Constants.Intake.DOWN, Constants.Intake.REVERSE, false); - - private String stateString; - private double pivotMotorSetpoint; - private double intakeMotorSetpoint; - private boolean useIn; - - IntakeStates( - String stateString, double pivotMotorSetpoint, double intakeMotorSetpoint, boolean useIn) { - this.stateString = stateString; - this.pivotMotorSetpoint = pivotMotorSetpoint; - this.intakeMotorSetpoint = intakeMotorSetpoint; - this.useIn = useIn; - } - - public String getStateString() { - return this.stateString; - } - - public double getPivotSetPoint() { - return pivotMotorSetpoint; - } - - public double getMotorSetPoint() { - return intakeMotorSetpoint; - } - - public boolean getUsingPID() { - return useIn; - } + OFF("Off", Constants.Intake.IN, Constants.Intake.OFF, true), + INTAKING("Intaking", Constants.Intake.DOWN, Constants.Intake.ON, false), + FEEDING("Feeding", Constants.Intake.IN, Constants.Intake.REVERSE, true), + OUTTAKING("Outtaking", Constants.Intake.DOWN, Constants.Intake.REVERSE, false); + + private String stateString; + private double pivotMotorSetpoint; + private double intakeMotorSetpoint; + private boolean useIn; + + IntakeStates( + String stateString, + double pivotMotorSetpoint, + double intakeMotorSetpoint, + boolean useIn + ) { + this.stateString = stateString; + this.pivotMotorSetpoint = pivotMotorSetpoint; + this.intakeMotorSetpoint = intakeMotorSetpoint; + this.useIn = useIn; + } + + public String getStateString() { + return this.stateString; + } + + public double getPivotSetPoint() { + return pivotMotorSetpoint; + } + + public double getMotorSetPoint() { + return intakeMotorSetpoint; + } + + public boolean getUsingPID() { + return useIn; + } } diff --git a/src/main/java/frc/robot/subsystems/manager/Manager.java b/src/main/java/frc/robot/subsystems/manager/Manager.java index acfb22b..abb210f 100644 --- a/src/main/java/frc/robot/subsystems/manager/Manager.java +++ b/src/main/java/frc/robot/subsystems/manager/Manager.java @@ -15,141 +15,140 @@ public class Manager extends Subsystem { - Intake intakeSubsystem; - AmpBar ampBarSubsystem; - Shooter shooterSubsystem; - Drive driveSubsystem; - - public Manager() { - super("Manager", ManagerStates.IDLE); - NoteSimulator.setDrive(driveSubsystem); - - switch (Constants.currentMode) { - case REAL: - intakeSubsystem = new Intake(new IntakeIOSparkMax()); - ampBarSubsystem = new AmpBar(new AmpBarIOReal()); - shooterSubsystem = new Shooter(new ShooterIOTalonFX()); - driveSubsystem = - new Drive( - new GyroIOPigeon2(false), - new ModuleIOHybrid(0), - new ModuleIOHybrid(1), - new ModuleIOHybrid(2), - new ModuleIOHybrid(3)); - break; - case REPLAY: - intakeSubsystem = new Intake(new IntakeIO() {}); - ampBarSubsystem = new AmpBar(new AmpBarIO() {}); - shooterSubsystem = new Shooter(new ShooterIO() {}); - driveSubsystem = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); - break; - case SIM: - intakeSubsystem = new Intake(new IntakeIOSim()); - ampBarSubsystem = new AmpBar(new AmpBarIOSim()); - shooterSubsystem = new Shooter(new ShooterIOSim()); - driveSubsystem = - new Drive( - new GyroIO() {}, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); - break; - default: - break; - } - - NoteSimulator.setDrive(driveSubsystem); - - // State Transitions (Nothing Automatic YET) - - /* Generally each action has a specific button, in intermediary states X will return to idle and you press + Intake intakeSubsystem; + AmpBar ampBarSubsystem; + Shooter shooterSubsystem; + Drive driveSubsystem; + + public Manager() { + super("Manager", ManagerStates.IDLE); + NoteSimulator.setDrive(driveSubsystem); + + switch (Constants.currentMode) { + case REAL: + intakeSubsystem = new Intake(new IntakeIOSparkMax()); + ampBarSubsystem = new AmpBar(new AmpBarIOReal()); + shooterSubsystem = new Shooter(new ShooterIOTalonFX()); + driveSubsystem = new Drive( + new GyroIOPigeon2(false), + new ModuleIOHybrid(0), + new ModuleIOHybrid(1), + new ModuleIOHybrid(2), + new ModuleIOHybrid(3) + ); + break; + case REPLAY: + intakeSubsystem = new Intake(new IntakeIO() {}); + ampBarSubsystem = new AmpBar(new AmpBarIO() {}); + shooterSubsystem = new Shooter(new ShooterIO() {}); + driveSubsystem = new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {} + ); + break; + case SIM: + intakeSubsystem = new Intake(new IntakeIOSim()); + ampBarSubsystem = new AmpBar(new AmpBarIOSim()); + shooterSubsystem = new Shooter(new ShooterIOSim()); + driveSubsystem = new Drive( + new GyroIO() {}, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim() + ); + break; + default: + break; + } + + NoteSimulator.setDrive(driveSubsystem); + + // State Transitions (Nothing Automatic YET) + + /* Generally each action has a specific button, in intermediary states X will return to idle and you press the specific button to go through the states. Right now you can go through states without them being finished which should not be the case. Some state transitions that can be automated are also not automated. Most of the functions required for the TODOs are already build into the IOs!*/ - // TODO: Automatically return to idle after scoring amp (using a timer) - // TODO: Automatically current sense notes to return to idle after intaking - // TODO: Automatically go to the shooting state after spinning up if the shooting state is - // entered from main controller input - - // Intaking (B) - addTrigger( - ManagerStates.IDLE, ManagerStates.INTAKING, () -> Constants.controller.getBButtonPressed()); - addTrigger( - ManagerStates.INTAKING, ManagerStates.IDLE, () -> Constants.controller.getBButtonPressed()); - - // Amping (Y) - addTrigger( - ManagerStates.IDLE, ManagerStates.FEED_AMP, () -> Constants.controller.getYButtonPressed()); - addTrigger( - ManagerStates.FEED_AMP, - ManagerStates.SCORE_AMP, - () -> Constants.controller.getYButtonPressed()); - addTrigger( - ManagerStates.SCORE_AMP, - ManagerStates.IDLE, - () -> Constants.controller.getYButtonPressed()); - addTrigger( - ManagerStates.FEED_AMP, ManagerStates.IDLE, () -> Constants.controller.getXButtonPressed()); - - // Shooting (A) - addTrigger( - ManagerStates.IDLE, - ManagerStates.SPINNING_UP, - () -> Constants.controller.getAButtonPressed()); - addTrigger( - ManagerStates.IDLE, - ManagerStates.SPINNING_UP, - () -> Constants.operatorController.getAButtonPressed()); - addTrigger( - ManagerStates.SPINNING_UP, - ManagerStates.IDLE, - () -> Constants.controller.getXButtonPressed()); - addTrigger( - ManagerStates.SPINNING_UP, - ManagerStates.SHOOTING, - () -> Constants.controller.getAButtonPressed()); - addTrigger( - ManagerStates.SHOOTING, ManagerStates.IDLE, () -> Constants.controller.getAButtonPressed()); - } - - @Override - public void periodic() { - super.periodic(); - - intakeSubsystem.setState(getState().getIntakeState()); - ampBarSubsystem.setState(getState().getAmpBarState()); - shooterSubsystem.setState(getState().getShooterState()); - shooterSubsystem.setManagerState(getState()); - - intakeSubsystem.periodic(); - ampBarSubsystem.periodic(); - shooterSubsystem.periodic(); - driveSubsystem.periodic(); - - // Cancel all actions regardless of whats happening - if (Constants.operatorController.getXButtonPressed()) { - setState(ManagerStates.IDLE); - } - } - - public void stop() { - intakeSubsystem.stop(); - ampBarSubsystem.stop(); - shooterSubsystem.stop(); - driveSubsystem.stop(); - } - - @Override - protected void runState() { - NoteSimulator.update(); - NoteSimulator.logNoteInfo(); - } + // TODO: Automatically return to idle after scoring amp (using a timer) + // TODO: Automatically current sense notes to return to idle after intaking + // TODO: Automatically go to the shooting state after spinning up if the shooting state is + // entered from main controller input + + // Intaking (B) + addTrigger(ManagerStates.IDLE, ManagerStates.INTAKING, () -> + Constants.controller.getBButtonPressed() + ); + addTrigger(ManagerStates.INTAKING, ManagerStates.IDLE, () -> + Constants.controller.getBButtonPressed() + ); + + // Amping (Y) + addTrigger(ManagerStates.IDLE, ManagerStates.FEED_AMP, () -> + Constants.controller.getYButtonPressed() + ); + addTrigger(ManagerStates.FEED_AMP, ManagerStates.SCORE_AMP, () -> + Constants.controller.getYButtonPressed() + ); + addTrigger(ManagerStates.SCORE_AMP, ManagerStates.IDLE, () -> + Constants.controller.getYButtonPressed() + ); + addTrigger(ManagerStates.FEED_AMP, ManagerStates.IDLE, () -> + Constants.controller.getXButtonPressed() + ); + + // Shooting (A) + addTrigger(ManagerStates.IDLE, ManagerStates.SPINNING_UP, () -> + Constants.controller.getAButtonPressed() + ); + addTrigger(ManagerStates.IDLE, ManagerStates.SPINNING_UP, () -> + Constants.operatorController.getAButtonPressed() + ); + addTrigger(ManagerStates.SPINNING_UP, ManagerStates.IDLE, () -> + Constants.controller.getXButtonPressed() + ); + addTrigger(ManagerStates.SPINNING_UP, ManagerStates.SHOOTING, () -> + Constants.controller.getAButtonPressed() + ); + addTrigger(ManagerStates.SHOOTING, ManagerStates.IDLE, () -> + Constants.controller.getAButtonPressed() + ); + } + + @Override + public void periodic() { + super.periodic(); + + intakeSubsystem.setState(getState().getIntakeState()); + ampBarSubsystem.setState(getState().getAmpBarState()); + shooterSubsystem.setState(getState().getShooterState()); + shooterSubsystem.setManagerState(getState()); + + intakeSubsystem.periodic(); + ampBarSubsystem.periodic(); + shooterSubsystem.periodic(); + driveSubsystem.periodic(); + + // Cancel all actions regardless of whats happening + if (Constants.operatorController.getXButtonPressed()) { + setState(ManagerStates.IDLE); + } + } + + public void stop() { + intakeSubsystem.stop(); + ampBarSubsystem.stop(); + shooterSubsystem.stop(); + driveSubsystem.stop(); + } + + @Override + protected void runState() { + NoteSimulator.update(); + NoteSimulator.logNoteInfo(); + } } diff --git a/src/main/java/frc/robot/subsystems/manager/ManagerStates.java b/src/main/java/frc/robot/subsystems/manager/ManagerStates.java index f85b0f2..12a5114 100644 --- a/src/main/java/frc/robot/subsystems/manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/manager/ManagerStates.java @@ -6,44 +6,49 @@ import frc.robot.subsystems.shooter.ShooterStates; public enum ManagerStates implements SubsystemStates { - IDLE("Idle", IntakeStates.OFF, AmpBarStates.OFF, ShooterStates.OFF), - INTAKING("Intaking", IntakeStates.INTAKING, AmpBarStates.OFF, ShooterStates.OFF), - SPINNING_UP("Spinning Up", IntakeStates.OFF, AmpBarStates.OFF, ShooterStates.SHOOTING), - SPINNING_AND_INTAKING( - "Spin and Intake", IntakeStates.INTAKING, AmpBarStates.OFF, ShooterStates.SHOOTING), - SHOOTING("Shooting", IntakeStates.FEEDING, AmpBarStates.OFF, ShooterStates.SHOOTING), - FEED_AMP("Feed Amp", IntakeStates.FEEDING, AmpBarStates.FEEDING, ShooterStates.PASSING_AMP), - SCORE_AMP("Score Amp", IntakeStates.OFF, AmpBarStates.SHOOTING, ShooterStates.OFF); - - String stateString; - IntakeStates intakeState; - AmpBarStates ampBarState; - ShooterStates shooterState; - - ManagerStates( - String stateString, - IntakeStates intakeState, - AmpBarStates ampBarState, - ShooterStates shooterState) { - this.stateString = stateString; - this.intakeState = intakeState; - this.ampBarState = ampBarState; - this.shooterState = shooterState; - } - - public String getStateString() { - return this.stateString; - } - - public IntakeStates getIntakeState() { - return intakeState; - } - - public AmpBarStates getAmpBarState() { - return ampBarState; - } - - public ShooterStates getShooterState() { - return shooterState; - } + IDLE("Idle", IntakeStates.OFF, AmpBarStates.OFF, ShooterStates.OFF), + INTAKING("Intaking", IntakeStates.INTAKING, AmpBarStates.OFF, ShooterStates.OFF), + SPINNING_UP("Spinning Up", IntakeStates.OFF, AmpBarStates.OFF, ShooterStates.SHOOTING), + SPINNING_AND_INTAKING( + "Spin and Intake", + IntakeStates.INTAKING, + AmpBarStates.OFF, + ShooterStates.SHOOTING + ), + SHOOTING("Shooting", IntakeStates.FEEDING, AmpBarStates.OFF, ShooterStates.SHOOTING), + FEED_AMP("Feed Amp", IntakeStates.FEEDING, AmpBarStates.FEEDING, ShooterStates.PASSING_AMP), + SCORE_AMP("Score Amp", IntakeStates.OFF, AmpBarStates.SHOOTING, ShooterStates.OFF); + + String stateString; + IntakeStates intakeState; + AmpBarStates ampBarState; + ShooterStates shooterState; + + ManagerStates( + String stateString, + IntakeStates intakeState, + AmpBarStates ampBarState, + ShooterStates shooterState + ) { + this.stateString = stateString; + this.intakeState = intakeState; + this.ampBarState = ampBarState; + this.shooterState = shooterState; + } + + public String getStateString() { + return this.stateString; + } + + public IntakeStates getIntakeState() { + return intakeState; + } + + public AmpBarStates getAmpBarState() { + return ampBarState; + } + + public ShooterStates getShooterState() { + return shooterState; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index a19f2f7..77cb7c2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -8,62 +8,65 @@ public class Shooter extends Subsystem { - private ShooterIO io; - private ManagerStates managerState; - ShooterIOInputsAutoLogged inputs; + private ShooterIO io; + private ManagerStates managerState; + ShooterIOInputsAutoLogged inputs; - public Shooter(ShooterIO io) { - super("Shooter", ShooterStates.OFF); - this.io = io; - inputs = new ShooterIOInputsAutoLogged(); + public Shooter(ShooterIO io) { + super("Shooter", ShooterStates.OFF); + this.io = io; + inputs = new ShooterIOInputsAutoLogged(); - switch (Constants.currentMode) { - case REAL: - io.configurePID( - Constants.Shooter.REAL_PID.kP, - Constants.Shooter.REAL_PID.kI, - Constants.Shooter.REAL_PID.kD); - break; - case REPLAY: - io.configurePID(0.0, 0.0, 0.0); - break; - case SIM: - io.configurePID( - Constants.Shooter.SIM_PID.kP, - Constants.Shooter.SIM_PID.kI, - Constants.Shooter.SIM_PID.kD); - break; - default: - break; - } - } + switch (Constants.currentMode) { + case REAL: + io.configurePID( + Constants.Shooter.REAL_PID.kP, + Constants.Shooter.REAL_PID.kI, + Constants.Shooter.REAL_PID.kD + ); + break; + case REPLAY: + io.configurePID(0.0, 0.0, 0.0); + break; + case SIM: + io.configurePID( + Constants.Shooter.SIM_PID.kP, + Constants.Shooter.SIM_PID.kI, + Constants.Shooter.SIM_PID.kD + ); + break; + default: + break; + } + } - @Override - protected void runState() { - io.setSpeed(getState().getMotorSpeedpoint()); + @Override + protected void runState() { + io.setSpeed(getState().getMotorSpeedpoint()); - if (managerState == ManagerStates.SHOOTING) { - NoteSimulator.launch( - io.getAverageSpeed() * Constants.Shooter.CIRCUMFRENCE_OF_SHOOTER_SPINNER); - } - } + if (managerState == ManagerStates.SHOOTING) { + NoteSimulator.launch( + io.getAverageSpeed() * Constants.Shooter.CIRCUMFRENCE_OF_SHOOTER_SPINNER + ); + } + } - public boolean nearSpeedPoint() { - return io.nearSpeedPoint(); - } + public boolean nearSpeedPoint() { + return io.nearSpeedPoint(); + } - public void stop() { - io.stop(); - } + public void stop() { + io.stop(); + } - @Override - public void periodic() { - super.periodic(); - io.updateInputs(inputs); - Logger.processInputs("Shooter", inputs); - } + @Override + public void periodic() { + super.periodic(); + io.updateInputs(inputs); + Logger.processInputs("Shooter", inputs); + } - public void setManagerState(ManagerStates state) { - managerState = state; - } + public void setManagerState(ManagerStates state) { + managerState = state; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 17a445f..f396916 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -3,29 +3,29 @@ import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { - @AutoLog - public static class ShooterIOInputs { + @AutoLog + public static class ShooterIOInputs { - public double rightShooterSpeed = 0.0; - public double leftShooterSpeed = 0.0; - public double leftShooterAppliedVolts = 0.0; - public double rightShooterAppliedVolts = 0.0; - public double shooterSpeedPoint = 0.0; - } + public double rightShooterSpeed = 0.0; + public double leftShooterSpeed = 0.0; + public double leftShooterAppliedVolts = 0.0; + public double rightShooterAppliedVolts = 0.0; + public double shooterSpeedPoint = 0.0; + } - public default void updateInputs(ShooterIOInputs inputs) {} + public default void updateInputs(ShooterIOInputs inputs) {} - public default void setSpeed(double rps) {} + public default void setSpeed(double rps) {} - public default void stop() {} + public default void stop() {} - public default void configurePID(double kP, double kI, double kD) {} + public default void configurePID(double kP, double kI, double kD) {} - public default boolean nearSpeedPoint() { - return false; - } + public default boolean nearSpeedPoint() { + return false; + } - public default double getAverageSpeed() { - return 0.0; - } + public default double getAverageSpeed() { + return 0.0; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index 5f8f4c8..2f9690d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -7,58 +7,60 @@ public class ShooterIOSim implements ShooterIO { - FlywheelSim sim; - PIDController pid; - private double speedPoint; - private double appliedVolts; + FlywheelSim sim; + PIDController pid; + private double speedPoint; + private double appliedVolts; - public ShooterIOSim() { - sim = - new FlywheelSim( - DCMotor.getFalcon500(Constants.Shooter.NUM_MOTORS), - Constants.Shooter.SHOOTER_GEARING, - Constants.Shooter.SHOOTER_MOI); - pid = new PIDController(0.0, 0.0, 0.0); - speedPoint = 0.0; - } + public ShooterIOSim() { + sim = new FlywheelSim( + DCMotor.getFalcon500(Constants.Shooter.NUM_MOTORS), + Constants.Shooter.SHOOTER_GEARING, + Constants.Shooter.SHOOTER_MOI + ); + pid = new PIDController(0.0, 0.0, 0.0); + speedPoint = 0.0; + } - @Override - public void updateInputs(ShooterIOInputs inputs) { - sim.update(Constants.SIM_UPDATE_TIME); + @Override + public void updateInputs(ShooterIOInputs inputs) { + sim.update(Constants.SIM_UPDATE_TIME); - inputs.leftShooterSpeed = sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - inputs.rightShooterSpeed = sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - inputs.shooterSpeedPoint = speedPoint; - inputs.leftShooterAppliedVolts = appliedVolts; - inputs.rightShooterAppliedVolts = appliedVolts; - } + inputs.leftShooterSpeed = sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + inputs.rightShooterSpeed = sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + inputs.shooterSpeedPoint = speedPoint; + inputs.leftShooterAppliedVolts = appliedVolts; + inputs.rightShooterAppliedVolts = appliedVolts; + } - @Override - public void stop() { - appliedVolts = 0; - sim.setInputVoltage(appliedVolts); - } + @Override + public void stop() { + appliedVolts = 0; + sim.setInputVoltage(appliedVolts); + } - @Override - public void configurePID(double kP, double kI, double kD) { - pid.setPID(kP, kI, kD); - } + @Override + public void configurePID(double kP, double kI, double kD) { + pid.setPID(kP, kI, kD); + } - @Override - public void setSpeed(double rps) { - speedPoint = rps; - appliedVolts = pid.calculate(sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF, rps); - sim.setInputVoltage(appliedVolts); - } + @Override + public void setSpeed(double rps) { + speedPoint = rps; + appliedVolts = pid.calculate(sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF, rps); + sim.setInputVoltage(appliedVolts); + } - @Override - public boolean nearSpeedPoint() { - return (Math.abs((sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF) - speedPoint) - > Constants.Shooter.ERROR_OF_MARGIN); - } + @Override + public boolean nearSpeedPoint() { + return ( + Math.abs((sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF) - speedPoint) > + Constants.Shooter.ERROR_OF_MARGIN + ); + } - @Override - public double getAverageSpeed() { - return sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - } + @Override + public double getAverageSpeed() { + return sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 396d323..a32bad8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -6,63 +6,71 @@ public class ShooterIOTalonFX implements ShooterIO { - private TalonFX leftMotor; - private TalonFX rightMotor; - private PIDController feedbackController; - private double speedPoint; - private double leftAppliedVolts; - private double rightAppliedVolts; + private TalonFX leftMotor; + private TalonFX rightMotor; + private PIDController feedbackController; + private double speedPoint; + private double leftAppliedVolts; + private double rightAppliedVolts; - public ShooterIOTalonFX() { - feedbackController = new PIDController(0, 0, 0); - leftMotor = new TalonFX(Constants.Shooter.LEFT_SHOOTER_ID); - rightMotor = new TalonFX(Constants.Shooter.RIGHT_SHOOTER_ID); - speedPoint = 0.0; - leftMotor.setInverted(false); - rightMotor.setInverted(true); - } + public ShooterIOTalonFX() { + feedbackController = new PIDController(0, 0, 0); + leftMotor = new TalonFX(Constants.Shooter.LEFT_SHOOTER_ID); + rightMotor = new TalonFX(Constants.Shooter.RIGHT_SHOOTER_ID); + speedPoint = 0.0; + leftMotor.setInverted(false); + rightMotor.setInverted(true); + } - public void updateInputs(ShooterIOInputs inputs) { - inputs.leftShooterAppliedVolts = leftAppliedVolts; - inputs.rightShooterAppliedVolts = rightAppliedVolts; - inputs.leftShooterSpeed = leftMotor.getVelocity().getValueAsDouble(); - inputs.rightShooterSpeed = rightMotor.getVelocity().getValueAsDouble(); - inputs.shooterSpeedPoint = speedPoint; - } + public void updateInputs(ShooterIOInputs inputs) { + inputs.leftShooterAppliedVolts = leftAppliedVolts; + inputs.rightShooterAppliedVolts = rightAppliedVolts; + inputs.leftShooterSpeed = leftMotor.getVelocity().getValueAsDouble(); + inputs.rightShooterSpeed = rightMotor.getVelocity().getValueAsDouble(); + inputs.shooterSpeedPoint = speedPoint; + } - public void setSpeed(double rps) { - speedPoint = rps; - leftAppliedVolts = - feedbackController.calculate(leftMotor.getRotorVelocity().getValueAsDouble(), rps); - rightAppliedVolts = - feedbackController.calculate(rightMotor.getRotorVelocity().getValueAsDouble(), rps); + public void setSpeed(double rps) { + speedPoint = rps; + leftAppliedVolts = feedbackController.calculate( + leftMotor.getRotorVelocity().getValueAsDouble(), + rps + ); + rightAppliedVolts = feedbackController.calculate( + rightMotor.getRotorVelocity().getValueAsDouble(), + rps + ); - leftMotor.setVoltage(leftAppliedVolts); - rightMotor.setVoltage(rightAppliedVolts); - } + leftMotor.setVoltage(leftAppliedVolts); + rightMotor.setVoltage(rightAppliedVolts); + } - public void stop() { - leftAppliedVolts = 0.0; - rightAppliedVolts = 0.0; - leftMotor.stopMotor(); - rightMotor.stopMotor(); - } + public void stop() { + leftAppliedVolts = 0.0; + rightAppliedVolts = 0.0; + leftMotor.stopMotor(); + rightMotor.stopMotor(); + } - public void configurePID(double kP, double kI, double kD) { - feedbackController.setPID(kP, kI, kD); - } + public void configurePID(double kP, double kI, double kD) { + feedbackController.setPID(kP, kI, kD); + } - public boolean nearSpeedPoint() { - return (Math.abs(speedPoint - leftMotor.getVelocity().getValueAsDouble()) - < Constants.Shooter.ERROR_OF_MARGIN - && Math.abs(speedPoint - rightMotor.getVelocity().getValueAsDouble()) - < Constants.Shooter.ERROR_OF_MARGIN); - } + public boolean nearSpeedPoint() { + return ( + Math.abs(speedPoint - leftMotor.getVelocity().getValueAsDouble()) < + Constants.Shooter.ERROR_OF_MARGIN && + Math.abs(speedPoint - rightMotor.getVelocity().getValueAsDouble()) < + Constants.Shooter.ERROR_OF_MARGIN + ); + } - @Override - public double getAverageSpeed() { - return ((leftMotor.getVelocity().getValueAsDouble() - + rightMotor.getVelocity().getValueAsDouble()) - / Constants.AVG_TWO_ITEM_F); - } + @Override + public double getAverageSpeed() { + return ( + (leftMotor.getVelocity().getValueAsDouble() + + rightMotor.getVelocity().getValueAsDouble()) / + Constants.AVG_TWO_ITEM_F + ); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterStates.java b/src/main/java/frc/robot/subsystems/shooter/ShooterStates.java index 4a775a0..17aceeb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterStates.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterStates.java @@ -4,24 +4,24 @@ import frc.robot.subsystems.SubsystemStates; public enum ShooterStates implements SubsystemStates { - OFF(Constants.Shooter.OFF, "Shooter Off"), - SHOOTING(Constants.Shooter.SHOOTING, "Shooting Full"), - PASSING_AMP(Constants.Shooter.FEEDING_AMP, "Passing To Amp"); + OFF(Constants.Shooter.OFF, "Shooter Off"), + SHOOTING(Constants.Shooter.SHOOTING, "Shooting Full"), + PASSING_AMP(Constants.Shooter.FEEDING_AMP, "Passing To Amp"); - private double speedPoint; - private String stateString; + private double speedPoint; + private String stateString; - ShooterStates(double speedPoint, String stateString) { - this.speedPoint = speedPoint; - this.stateString = stateString; - } + ShooterStates(double speedPoint, String stateString) { + this.speedPoint = speedPoint; + this.stateString = stateString; + } - public double getMotorSpeedpoint() { - return speedPoint; - } + public double getMotorSpeedpoint() { + return speedPoint; + } - @Override - public String getStateString() { - return stateString; - } + @Override + public String getStateString() { + return stateString; + } } diff --git a/src/main/java/frc/robot/util/FFConstants.java b/src/main/java/frc/robot/util/FFConstants.java index 7c72086..6ab4eb2 100644 --- a/src/main/java/frc/robot/util/FFConstants.java +++ b/src/main/java/frc/robot/util/FFConstants.java @@ -2,11 +2,11 @@ public class FFConstants { - public double kS; - public double kV; + public double kS; + public double kV; - public FFConstants(double ks, double kv) { - this.kS = ks; - this.kV = kv; - } + public FFConstants(double ks, double kv) { + this.kS = ks; + this.kV = kv; + } } diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index 5100a26..8694cb9 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -20,136 +20,142 @@ public class LocalADStarAK implements Pathfinder { - private final ADStarIO io = new ADStarIO(); - - /** - * Get if a new path has been calculated since the last time a path was retrieved - * - * @return True if a new path is available - */ - @Override - public boolean isNewPathAvailable() { - if (!Logger.hasReplaySource()) { - io.updateIsNewPathAvailable(); - } - - Logger.processInputs("LocalADStarAK", io); - - return io.isNewPathAvailable; - } - - /** - * Get the most recently calculated path - * - * @param constraints The path constraints to use when creating the path - * @param goalEndState The goal end state to use when creating the path - * @return The PathPlannerPath created from the points calculated by the pathfinder - */ - @Override - public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { - if (!Logger.hasReplaySource()) { - io.updateCurrentPathPoints(constraints, goalEndState); - } - - Logger.processInputs("LocalADStarAK", io); - - if (io.currentPathPoints.isEmpty()) { - return null; - } - - return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); - } - - /** - * Set the start position to pathfind from - * - * @param startPosition Start position on the field. If this is within an obstacle it will be - * moved to the nearest non-obstacle node. - */ - @Override - public void setStartPosition(Translation2d startPosition) { - if (!Logger.hasReplaySource()) { - io.adStar.setStartPosition(startPosition); - } - } - - /** - * Set the goal position to pathfind to - * - * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved - * to the nearest non-obstacle node. - */ - @Override - public void setGoalPosition(Translation2d goalPosition) { - if (!Logger.hasReplaySource()) { - io.adStar.setGoalPosition(goalPosition); - } - } - - /** - * Set the dynamic obstacles that should be avoided while pathfinding. - * - * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents - * opposite corners of a bounding box. - * @param currentRobotPos The current position of the robot. This is needed to change the start - * position of the path to properly avoid obstacles - */ - @Override - public void setDynamicObstacles( - List> obs, Translation2d currentRobotPos) { - if (!Logger.hasReplaySource()) { - io.adStar.setDynamicObstacles(obs, currentRobotPos); - } - } - - private static class ADStarIO implements LoggableInputs { - - public LocalADStar adStar = new LocalADStar(); - public boolean isNewPathAvailable = false; - public List currentPathPoints = Collections.emptyList(); - - @Override - public void toLog(LogTable table) { - table.put("IsNewPathAvailable", isNewPathAvailable); - - double[] pointsLogged = new double[currentPathPoints.size() * 2]; - int idx = 0; - for (PathPoint point : currentPathPoints) { - pointsLogged[idx] = point.position.getX(); - pointsLogged[idx + 1] = point.position.getY(); - idx += 2; - } - - table.put("CurrentPathPoints", pointsLogged); - } - - @Override - public void fromLog(LogTable table) { - isNewPathAvailable = table.get("IsNewPathAvailable", false); - - double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); - - 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)); - } - - currentPathPoints = pathPoints; - } - - public void updateIsNewPathAvailable() { - isNewPathAvailable = adStar.isNewPathAvailable(); - } - - public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { - PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); - - if (currentPath != null) { - currentPathPoints = currentPath.getAllPathPoints(); - } else { - currentPathPoints = Collections.emptyList(); - } - } - } + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, + Translation2d currentRobotPos + ) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + 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) + ); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints( + PathConstraints constraints, + GoalEndState goalEndState + ) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } } diff --git a/src/main/java/frc/robot/util/NoteSimulator.java b/src/main/java/frc/robot/util/NoteSimulator.java index ccebc59..0361c1c 100644 --- a/src/main/java/frc/robot/util/NoteSimulator.java +++ b/src/main/java/frc/robot/util/NoteSimulator.java @@ -12,100 +12,110 @@ public class NoteSimulator { - private static Drive drive; - - private static Pose3d currentFieldPose = new Pose3d(); - private static Translation3d fieldVelocity = new Translation3d(); - private static boolean inShooter = false; - private static List noteTrajectory = new ArrayList<>(); - - public static void setDrive(Drive drivesystem) { - drive = drivesystem; - } - - public static void attachToShooter() { - inShooter = true; - noteTrajectory.clear(); - } - - public static boolean isAttached() { - return inShooter; - } - - public static List getNoteTrajectory() { - return noteTrajectory; - } - - public static void launch(double velocity) { - if (!inShooter) { - return; - } - - Logger.recordOutput("Launch Velocity", velocity); - - currentFieldPose = getFieldPose(Constants.NoteSim.SHOOTER_POSE3D); - inShooter = false; - - fieldVelocity = new Translation3d(velocity, currentFieldPose.getRotation()); - - ChassisSpeeds robotVel = drive.getChassisSpeed(); - ChassisSpeeds fieldRel = ChassisSpeeds.fromRobotRelativeSpeeds(robotVel, drive.getRotation()); - - fieldVelocity = - fieldVelocity.plus( - new Translation3d(fieldRel.vxMetersPerSecond, fieldRel.vyMetersPerSecond, 0.0)); - } - - public static Pose3d getFieldPose(Pose3d shooterPose) { - if (inShooter) { - return new Pose3d(drive.getPose()) - .transformBy(new Transform3d(shooterPose.getTranslation(), shooterPose.getRotation())); - } - - return currentFieldPose; - } - - public static void update() { - if (inShooter) { - return; - } - - Translation3d posDelta = fieldVelocity.times(Constants.NoteSim.dt); - - currentFieldPose = - new Pose3d( - currentFieldPose.getTranslation().plus(posDelta), currentFieldPose.getRotation()); - - if (currentFieldPose.getX() <= -Constants.NoteSim.OUT_OF_FIELD_MARGIN - || currentFieldPose.getX() - >= Constants.NoteSim.FIELD_SIZE.getX() + Constants.NoteSim.OUT_OF_FIELD_MARGIN - || currentFieldPose.getY() <= -Constants.NoteSim.OUT_OF_FIELD_MARGIN - || currentFieldPose.getY() - >= Constants.NoteSim.FIELD_SIZE.getY() + Constants.NoteSim.OUT_OF_FIELD_MARGIN - || currentFieldPose.getZ() <= 0.0) { - fieldVelocity = new Translation3d(); - } else { - fieldVelocity = - fieldVelocity.minus(Constants.NoteSim.GRAVITY_TRANSLATION3D.times(Constants.NoteSim.dt)); - double norm = fieldVelocity.getNorm(); - - double fDrag = - (Constants.NoteSim.AIR_DENSITY - * Math.pow(norm, 2) - * Constants.NoteSim.DRAG_COEFFICIENT - * Constants.NoteSim.CROSSECTION_AREA) - / Constants.AVG_TWO_ITEM_F; - double deltaV = (Constants.NoteSim.MASS * fDrag) * Constants.NoteSim.dt; - - double t = (norm - deltaV) / norm; - fieldVelocity = fieldVelocity.times(t); - noteTrajectory.add(currentFieldPose.getTranslation()); - } - } - - public static void logNoteInfo() { - Logger.recordOutput( - "SimNoteTrajectory", NoteSimulator.getNoteTrajectory().toArray(new Translation3d[0])); - Logger.recordOutput("SimNotePose3d", getFieldPose(Constants.NoteSim.SHOOTER_POSE3D)); - } + private static Drive drive; + + private static Pose3d currentFieldPose = new Pose3d(); + private static Translation3d fieldVelocity = new Translation3d(); + private static boolean inShooter = false; + private static List noteTrajectory = new ArrayList<>(); + + public static void setDrive(Drive drivesystem) { + drive = drivesystem; + } + + public static void attachToShooter() { + inShooter = true; + noteTrajectory.clear(); + } + + public static boolean isAttached() { + return inShooter; + } + + public static List getNoteTrajectory() { + return noteTrajectory; + } + + public static void launch(double velocity) { + if (!inShooter) { + return; + } + + Logger.recordOutput("Launch Velocity", velocity); + + currentFieldPose = getFieldPose(Constants.NoteSim.SHOOTER_POSE3D); + inShooter = false; + + fieldVelocity = new Translation3d(velocity, currentFieldPose.getRotation()); + + ChassisSpeeds robotVel = drive.getChassisSpeed(); + ChassisSpeeds fieldRel = ChassisSpeeds.fromRobotRelativeSpeeds( + robotVel, + drive.getRotation() + ); + + fieldVelocity = fieldVelocity.plus( + new Translation3d(fieldRel.vxMetersPerSecond, fieldRel.vyMetersPerSecond, 0.0) + ); + } + + public static Pose3d getFieldPose(Pose3d shooterPose) { + if (inShooter) { + return new Pose3d(drive.getPose()).transformBy( + new Transform3d(shooterPose.getTranslation(), shooterPose.getRotation()) + ); + } + + return currentFieldPose; + } + + public static void update() { + if (inShooter) { + return; + } + + Translation3d posDelta = fieldVelocity.times(Constants.NoteSim.dt); + + currentFieldPose = new Pose3d( + currentFieldPose.getTranslation().plus(posDelta), + currentFieldPose.getRotation() + ); + + if ( + currentFieldPose.getX() <= -Constants.NoteSim.OUT_OF_FIELD_MARGIN || + currentFieldPose.getX() >= + Constants.NoteSim.FIELD_SIZE.getX() + Constants.NoteSim.OUT_OF_FIELD_MARGIN || + currentFieldPose.getY() <= -Constants.NoteSim.OUT_OF_FIELD_MARGIN || + currentFieldPose.getY() >= + Constants.NoteSim.FIELD_SIZE.getY() + Constants.NoteSim.OUT_OF_FIELD_MARGIN || + currentFieldPose.getZ() <= 0.0 + ) { + fieldVelocity = new Translation3d(); + } else { + fieldVelocity = fieldVelocity.minus( + Constants.NoteSim.GRAVITY_TRANSLATION3D.times(Constants.NoteSim.dt) + ); + double norm = fieldVelocity.getNorm(); + + double fDrag = + (Constants.NoteSim.AIR_DENSITY * + Math.pow(norm, 2) * + Constants.NoteSim.DRAG_COEFFICIENT * + Constants.NoteSim.CROSSECTION_AREA) / + Constants.AVG_TWO_ITEM_F; + double deltaV = (Constants.NoteSim.MASS * fDrag) * Constants.NoteSim.dt; + + double t = (norm - deltaV) / norm; + fieldVelocity = fieldVelocity.times(t); + noteTrajectory.add(currentFieldPose.getTranslation()); + } + } + + public static void logNoteInfo() { + Logger.recordOutput( + "SimNoteTrajectory", + NoteSimulator.getNoteTrajectory().toArray(new Translation3d[0]) + ); + Logger.recordOutput("SimNotePose3d", getFieldPose(Constants.NoteSim.SHOOTER_POSE3D)); + } } From 2a28d205ab1d7e2be1dc4a5304ce57e5833933ba Mon Sep 17 00:00:00 2001 From: Otto Date: Sat, 3 Aug 2024 23:59:42 -0400 Subject: [PATCH 4/6] Remove un-used debug prints --- src/main/java/frc/robot/Robot.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e804e5b..761e7d2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -105,7 +105,7 @@ public void robotInit() { // Start AdvantageKit logger Logger.start(); - + // TODO: Make default auto a "cross line" auto autoChooser = AutoBuilder.buildAutoChooser("Example Auto"); SmartDashboard.putData("Auto Chooser", autoChooser); } @@ -138,10 +138,8 @@ public void autonomousInit() { autonomousCommand = getAutonomousCommand(); // schedule the autonomous command (example) - System.out.println(":( womp womp"); if (autonomousCommand != null) { autonomousCommand.schedule(); - System.out.println("hi"); } } From 0a525a6ae28f87de382c08a882a6857efeca3415 Mon Sep 17 00:00:00 2001 From: github-actions Date: Sun, 4 Aug 2024 04:00:09 +0000 Subject: [PATCH 5/6] Apply Prettier format --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 761e7d2..d3c6f8e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -105,7 +105,7 @@ public void robotInit() { // Start AdvantageKit logger Logger.start(); - // TODO: Make default auto a "cross line" auto + // TODO: Make default auto a "cross line" auto autoChooser = AutoBuilder.buildAutoChooser("Example Auto"); SmartDashboard.putData("Auto Chooser", autoChooser); } From c03844e2ec5bd6c166a2d5175df0e0dbcd518ad1 Mon Sep 17 00:00:00 2001 From: Otto Date: Sun, 4 Aug 2024 12:16:28 -0400 Subject: [PATCH 6/6] Add back to off triggers --- src/main/java/frc/robot/subsystems/drive/Drive.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index c2b767c..12a648d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -85,6 +85,14 @@ public Drive( addTrigger(DriveStates.REGULAR_DRIVE, DriveStates.SPEED_MAXXING, () -> Constants.controller.getLeftBumper() ); + + // Back to Off + addTrigger(DriveStates.SPEED_MAXXING, DriveStates.SLOW_MODE, () -> + Constants.controller.getLeftBumperReleased() + ); + addTrigger(DriveStates.SLOW_MODE, DriveStates.REGULAR_DRIVE, () -> + Constants.controller.getRightBumperReleased() + ); } @Override