diff --git a/src/main/java/first/robot/Robot.java b/src/main/java/first/robot/Robot.java index 8736aa6..70d4d50 100644 --- a/src/main/java/first/robot/Robot.java +++ b/src/main/java/first/robot/Robot.java @@ -6,8 +6,11 @@ package first.robot; import org.wpilib.command2.Command; import org.wpilib.command2.CommandScheduler; +import org.wpilib.epilogue.Epilogue; +import org.wpilib.epilogue.Logged; import org.wpilib.framework.TimedRobot; +@Logged public class Robot extends TimedRobot { private Command autonomousCommand; @@ -20,6 +23,7 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + Epilogue.update(this); } @Override diff --git a/src/main/java/first/robot/RobotContainer.java b/src/main/java/first/robot/RobotContainer.java index 5a79631..89062d0 100644 --- a/src/main/java/first/robot/RobotContainer.java +++ b/src/main/java/first/robot/RobotContainer.java @@ -6,7 +6,9 @@ package first.robot; import org.wpilib.command2.Command; import org.wpilib.command2.Commands; +import org.wpilib.epilogue.Logged; +@Logged public class RobotContainer { public RobotContainer() { configureBindings(); diff --git a/src/main/java/first/robot/constants/ClimberConstants.java b/src/main/java/first/robot/constants/ClimberConstants.java index 1f39665..730b157 100644 --- a/src/main/java/first/robot/constants/ClimberConstants.java +++ b/src/main/java/first/robot/constants/ClimberConstants.java @@ -1,6 +1,6 @@ package first.robot.constants; -import java.util.function.Function; +import org.wpilib.hardware.bus.CANPort; import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; @@ -24,6 +24,8 @@ public class ClimberConstants { } } + public static final CANPort kCANPort = CANPort.CAN_S0; + public static final int kMotorCANID = 0; public static final double kConversionFactor = 0; @@ -45,19 +47,17 @@ public class ClimberConstants { public static final SparkMaxConfig kMotorConfig = new SparkMaxConfig(); - public static final Function kPositionConversionFunction = (d) -> { - return d * kConversionFactor; - }; - - public static final Function kVelocityConversionFunction = (d) -> { - return kPositionConversionFunction.apply(d) / 60; - }; - static { kMotorConfig .inverted(kMotorInverted) .smartCurrentLimit(kCurrentLimit) .idleMode(kIdleMode); + kMotorConfig.encoder + // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing + // a solution for this. Issue is open and pending + // https://github.com/wpilibsuite/SystemcoreTesting/issues/401 + .positionConversionFactor(kConversionFactor) + .velocityConversionFactor(kConversionFactor / 60); kMotorConfig.closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pid(kP, kI, kD) diff --git a/src/main/java/first/robot/constants/CompetitionConstants.java b/src/main/java/first/robot/constants/CompetitionConstants.java new file mode 100644 index 0000000..86c9854 --- /dev/null +++ b/src/main/java/first/robot/constants/CompetitionConstants.java @@ -0,0 +1,36 @@ +package first.robot.constants; + +import org.wpilib.fields.Fields; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.util.Units; + +public class CompetitionConstants { + public static final Fields kTagLayout = Fields.FRC_2026_REBUILT_ANDY_MARK; + + public static final double kHubGoalHeightMeters = Units.inchesToMeters(72); + + // TODO Real Values + public static final Transform3d kRobotToShooter = new Transform3d(); + + public static final Pose2d kBlueHubLocation = new Pose2d( + Units.inchesToMeters(182.11), + Units.inchesToMeters(158.84), + Rotation2d.fromDegrees(0) + ); + + // TODO The origination value produced by the April Tag Field Layout object may + // influence what this value should actually be. See AprilTagFieldLayout.getOrigin + // For now, the X axis position (forward/backward) is calculated as though the blue + // alliance wall right hand side is the originiation point, so, the distance from + // the blue alliance wall, to the blue alliance hub center point, plus + // the distance between the center of the blue alliance hub and the center of + // the red alliance hub + public static final Pose2d kRedHubLocation = new Pose2d( + Units.inchesToMeters(182.11 + 143.5 * 2), + Units.inchesToMeters(158.84), + Rotation2d.fromDegrees(0) + ); + +} diff --git a/src/main/java/first/robot/constants/DrivetrainConstants.java b/src/main/java/first/robot/constants/DrivetrainConstants.java new file mode 100644 index 0000000..6e836e9 --- /dev/null +++ b/src/main/java/first/robot/constants/DrivetrainConstants.java @@ -0,0 +1,58 @@ +package first.robot.constants; + +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.kinematics.SwerveDriveKinematics; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N3; +import org.wpilib.math.util.Units; + +public class DrivetrainConstants { + public static final double kMaxSpeedMetersPerSecond = 4.663; + public static final double kMaxAngularSpeed = 2 * Math.PI; + + public static final double kTrackWidth = Units.inchesToMeters(23.75); + public static final double kWheelBase = Units.inchesToMeters(18.75); + + public static final double kFrontLeftMagEncoderOffset = 2.965; + public static final double kFrontRightMagEncoderOffset = 1.120; + public static final double kRearLeftMagEncoderOffset = 3.761; + public static final double kRearRightMagEncoderOffset = 2.573; + + public static final int kFrontLeftDrivingCANID = 4; + public static final int kFrontRightDrivingCANID = 3; + public static final int kRearLeftDrivingCANID = 1; + public static final int kRearRightDrivingCANID = 2; + + public static final int kFrontLeftTurningCANID = 7; // 8 + public static final int kFrontRightTurningCANID = 21; //9 + public static final int kRearLeftTurningCANID = 6; //7 + public static final int kRearRightTurningCANID = 8; //6 + + public static final int kFrontLeftAnalogInPort = 3; + public static final int kFrontRightAnalogInPort = 2; + public static final int kRearLeftAnalogInPort = 0; + public static final int kRearRightAnalogInPort = 1; + + public static final boolean kGyroReversed = true; + + // TODO Hold over from 2025, adjust? + public static final double kHeadingP = .65; + public static final double kXTranslationP = .5; + public static final double kYTranslationP = .5; + public static final double kYawPIDTolerance = Units.degreesToRadians(1); + + // TODO How much do we trust gyro and encoders vs vision estimates. + // NOTE: Bigger values indicate LESS trust. Generally all three values for a given matrix should be the same + public static final Matrix kSensorFusionOdometryStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); + public static final Matrix kVisionOdometryStdDevs = VecBuilder.fill(0.3, 0.3, 0.3); + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2) + ); +} diff --git a/src/main/java/first/robot/constants/HoodConstants.java b/src/main/java/first/robot/constants/HoodConstants.java new file mode 100644 index 0000000..48b2cc9 --- /dev/null +++ b/src/main/java/first/robot/constants/HoodConstants.java @@ -0,0 +1,90 @@ +package first.robot.constants; + +import java.util.Map; + +import org.wpilib.math.interpolation.InterpolatingDoubleTreeMap; +import org.wpilib.math.util.Units; + +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +import first.robot.constants.ShooterConstants.ShooterSpeeds; + +public class HoodConstants { + // TODO Real Values + public static final int kMotorCANID = 12; + + public static final double kConversionFactor = (1.0/3.0)*(8.0/147.0)*2*Math.PI; + + public static final double kP = 1.75; + public static final double kI = 0; + public static final double kD = 0; + public static final double kS = 0.435; + public static final double kV = 0; + public static final double kA = 0; + public static final double kStartupAngle = 0.0; + public static final double kMaxManualSpeedMultiplier = 0.1; + public static final double kTolerance = Math.toRadians(0.5); + + public static final double kAmpsToTriggerPositionReset = 10; + + // TODO This is just barely longer than the default frame time for output current information + // Should this be longer? + public static final double kTimeAboveThresholdToReset = .25; + + public static final int kCurrentLimit = 15; + + public static final boolean kInverted = true; + public static final boolean kUseInterpolatorForAngle = false; + + public static final IdleMode kIdleMode = IdleMode.kBrake; + + public static final Map kHoodInterpolators = Map.of( + ShooterSpeeds.kHubSpeed, new InterpolatingDoubleTreeMap() + ); + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final SparkMaxConfig kConfig = new SparkMaxConfig(); + + static { + kConfig + .idleMode(kIdleMode) + .inverted(kInverted) + .smartCurrentLimit(kCurrentLimit); + kConfig.encoder + // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing + // a solution for this. Issue is open and pending + // https://github.com/wpilibsuite/SystemcoreTesting/issues/401 + .positionConversionFactor(kConversionFactor) + .velocityConversionFactor(kConversionFactor / 60); + kConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(kP, kI, kD) + .outputRange(-1, 1) + .allowedClosedLoopError(kTolerance, ClosedLoopSlot.kSlot0) + .feedForward + .sva(kS, kV, kA); + + kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put( + Double.valueOf(Units.inchesToMeters(22.2 + 40)), + Double.valueOf(Units.degreesToRadians(9.5))); + + kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put( + Double.valueOf(Units.inchesToMeters(22.2 + 60)), + Double.valueOf(Units.degreesToRadians(12.5))); + + kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put( + Double.valueOf(Units.inchesToMeters(22.2 + 80)), + Double.valueOf(Units.degreesToRadians(16.25))); + + kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put( + Double.valueOf(Units.inchesToMeters(22.2 + 100)), + Double.valueOf(Units.degreesToRadians(20.5))); + + kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put( + Double.valueOf(Units.inchesToMeters(22.2 + 120)), + Double.valueOf(Units.degreesToRadians(23.5))); + } +} diff --git a/src/main/java/first/robot/constants/IntakePivotConstants.java b/src/main/java/first/robot/constants/IntakePivotConstants.java new file mode 100644 index 0000000..7b32bea --- /dev/null +++ b/src/main/java/first/robot/constants/IntakePivotConstants.java @@ -0,0 +1,89 @@ +package first.robot.constants; + +import org.wpilib.hardware.bus.CANPort; + +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class IntakePivotConstants { + public enum IntakePivotPosition { + kUp(Math.toRadians(116.0)), + kDown(Math.toRadians(0.0)); + + private double positionRadians; + private double positionRotations; + + private IntakePivotPosition(double positionRadians) { + this.positionRadians = positionRadians; + this.positionRotations = positionRadians / kConversionFactor; + } + + public double getPositionRadians() { + return positionRadians; + } + + public double getPositionRotations() { + return positionRotations; + } + } + + public static final CANPort kCANPort = CANPort.CAN_S0; + + public static final int kLeftMotorCANID = 16; + public static final int kRightMotorCANID = 9; + + public static final double kConversionFactor = 60.0/11.0*60.0/18.0*38.0/16.0; + + // Ultra conservative multiplier to prevent 1/8" lexan destruction, modify at your own peril + public static final double kMaxManualSpeedMultiplier = .3; + + public static final double kP = 0; + public static final double kI = 0; + public static final double kD = 0; + public static final double kS = 0; + public static final double kV = 5.26; + public static final double kA = 0.05; + public static final double kG = 0.25; + + public static final boolean kInvertMotors = false; + + public static final int kCurrentLimit = 30; + + public static final IdleMode kIdleMode = IdleMode.kCoast; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final SparkMaxConfig KLeftMotorConfig = new SparkMaxConfig(); + public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig(); + + static { + KLeftMotorConfig + .idleMode(kIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(false); + KLeftMotorConfig.encoder + // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing + // a solution for this. Issue is open and pending + // https://github.com/wpilibsuite/SystemcoreTesting/issues/401 + .positionConversionFactor(kConversionFactor) + .velocityConversionFactor(kConversionFactor / 60); + KLeftMotorConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(kP, kI, kD) + .outputRange(-1, 1) + .positionWrappingEnabled(true) + // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing + // a solution for this. Issue is open and pending + // https://github.com/wpilibsuite/SystemcoreTesting/issues/401 + .positionWrappingInputRange(0, 2 * Math.PI) + .feedForward + .svag(kS, kV, kA, kG); + + kRightMotorConfig + .idleMode(kIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(true) + ;//.follow(kLeftMotorCANID); + } +} diff --git a/src/main/java/first/robot/constants/IntakeRollerConstants.java b/src/main/java/first/robot/constants/IntakeRollerConstants.java new file mode 100644 index 0000000..378de16 --- /dev/null +++ b/src/main/java/first/robot/constants/IntakeRollerConstants.java @@ -0,0 +1,40 @@ +package first.robot.constants; + +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import org.wpilib.hardware.bus.CANPort; + +import com.revrobotics.spark.config.SparkMaxConfig; + +public class IntakeRollerConstants { + public static final CANPort kCANPort = CANPort.CAN_S0; + + public static final int kRightMotorCANID = 20; + public static final int kLeftMotorCANID = 1; + + public static final int kCurrentLimit = 65; + + public static final boolean kInvertLeftMotor = false; + public static final boolean kInvertRightMotor = true; + + public static final double kSpeed = 1; + + public static final IdleMode kIdleMode = IdleMode.kCoast; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final SparkMaxConfig leftMotorConfig = new SparkMaxConfig(); + public static final SparkMaxConfig rightMotorConfig = new SparkMaxConfig(); + + static { + leftMotorConfig + .idleMode(kIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(kInvertLeftMotor); + rightMotorConfig + .idleMode(kIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(kInvertRightMotor) + ; + } +} diff --git a/src/main/java/first/robot/constants/KrakenMotorConstants.java b/src/main/java/first/robot/constants/KrakenMotorConstants.java new file mode 100644 index 0000000..1ff8cd3 --- /dev/null +++ b/src/main/java/first/robot/constants/KrakenMotorConstants.java @@ -0,0 +1,5 @@ +package first.robot.constants; + +public class KrakenMotorConstants { + public static final double kFreeSpeedRPM = 6000; +} \ No newline at end of file diff --git a/src/main/java/first/robot/constants/ModuleConstants.java b/src/main/java/first/robot/constants/ModuleConstants.java new file mode 100644 index 0000000..c2540d0 --- /dev/null +++ b/src/main/java/first/robot/constants/ModuleConstants.java @@ -0,0 +1,139 @@ +package first.robot.constants; + +import org.wpilib.math.util.Units; + +import com.ctre.phoenix6.configs.AudioConfigs; +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class ModuleConstants { + public enum ModuleName { + kFrontLeft("FrontLeft"), + kFrontRight("FrontRight"), + kRearLeft("RearLeft"), + kRearRight("RearRight"); + + private String loggableName; + + private ModuleName(String loggableName) { + this.loggableName = loggableName; + } + + public String getLoggableName() { + return loggableName; + } + + } + + // DRIVING MOTOR CONFIG (Kraken) + public static final double kDrivingMotorReduction = (50 * 16 * 45)/(14.0 * 28.0 * 15.0); + + public static final double kDrivingMotorFeedSpeedRPS = KrakenMotorConstants.kFreeSpeedRPM / 60; + public static final double kWheelDiameterMeters = Units.inchesToMeters(4); + public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; + public static final double kDriveWheelFreeSpeedRPS = (kDrivingMotorFeedSpeedRPS * kWheelCircumferenceMeters) / + kDrivingMotorReduction; + public static final double kDrivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction; + public static final double kDrivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRPS; + + // TODO Hold over from 2025, adjust? + public static final double kDriveP = .06; + public static final double kDriveI = 0; + public static final double kDriveD = 0; + public static final double kDriveS = 0; + public static final double kDriveV = kDrivingVelocityFeedForward; + public static final double kDriveA = 0; + public static final double kClosedLoopRampRate = .01; + + // TODO Hold over from 2025, adjust? + public static final int kDriveMotorStatorCurrentLimit = 90; + public static final int kDriveMotorSupplyCurrentLimit = 40; + + // TODO Hold over from 2025, adjust? + public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive; + public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake; + + // TURNING MOTOR CONFIG (NEO) + public static final double kTurningMotorReduction = 150.0/7.0; + public static final double kTurningFactor = 2 * Math.PI / kTurningMotorReduction; + // TODO Adjust? Let over from 2025 + public static final double kTurnP = 12; + public static final double kTurnI = 0; + public static final double kTurnD = 0; + + public static final double kTurnTolerance = Math.toRadians(0.25); + + public static final boolean kIsEncoderInverted = false; + + // TODO How sensitive is too sensitive? + public static final double kAutoResetPositionDeadband = Units.degreesToRadians(.25); + + public static final int kTurnMotorCurrentLimit = 20; + + public static final IdleMode kTurnIdleMode = IdleMode.kBrake; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final SparkMaxConfig turningConfig = new SparkMaxConfig(); + + public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs(); + public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs(); + public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs(); + public static final AudioConfigs kAudioConfig = new AudioConfigs(); + public static final Slot0Configs kDriveSlot0Config = new Slot0Configs(); + public static final ClosedLoopRampsConfigs kDriveClosedLoopRampConfig = new ClosedLoopRampsConfigs(); + + static { + kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction; + + kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true; + kDriveCurrentLimitConfig.SupplyCurrentLimitEnable = true; + kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit; + kDriveCurrentLimitConfig.SupplyCurrentLimit = kDriveMotorSupplyCurrentLimit; + + kDriveMotorConfig.Inverted = kDriveInversionState; + kDriveMotorConfig.NeutralMode = kDriveIdleMode; + + kAudioConfig.AllowMusicDurDisable = true; + + kDriveSlot0Config.kP = kDriveP; + kDriveSlot0Config.kI = kDriveI; + kDriveSlot0Config.kD = kDriveD; + kDriveSlot0Config.kS = kDriveS; + kDriveSlot0Config.kV = kDriveV; + kDriveSlot0Config.kA = kDriveA; + + kDriveClosedLoopRampConfig.withVoltageClosedLoopRampPeriod(kClosedLoopRampRate); + + turningConfig + .idleMode(kTurnIdleMode) + .smartCurrentLimit(kTurnMotorCurrentLimit) + .inverted(true); + turningConfig.encoder + // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing + // a solution for this. Issue is open and pending + // https://github.com/wpilibsuite/SystemcoreTesting/issues/401 + .positionConversionFactor(kTurningFactor) + .velocityConversionFactor(kTurningFactor / 60.0); + turningConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(kTurnP, kTurnI, kTurnD) + .outputRange(-1, 1) + .allowedClosedLoopError(kTurnTolerance, ClosedLoopSlot.kSlot0) + .positionWrappingEnabled(true) + // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing + // a solution for this. Issue is open and pending + // https://github.com/wpilibsuite/SystemcoreTesting/issues/401 + .positionWrappingInputRange(0, 2 * Math.PI); + + } +} diff --git a/src/main/java/first/robot/constants/OIConstants.java b/src/main/java/first/robot/constants/OIConstants.java new file mode 100644 index 0000000..67c61d6 --- /dev/null +++ b/src/main/java/first/robot/constants/OIConstants.java @@ -0,0 +1,29 @@ +package first.robot.constants; + +import org.wpilib.util.Color; + +public class OIConstants { + public static final int kDriverControllerPort = 0; + public static final int kOperatorControllerPort = 1; + + public static final double kDriveDeadband = .01; + + public static final double kJoystickExponential = 3; + + public static final String kTeleopTab = "Teleoperated"; + + public static final String kCurrentActiveHub = "Alliance Hub Currently Active"; + + public static final String[] kRedBlueDisplay = { + Color.RED.toHexString(), + Color.BLUE.toHexString() + }; + + public static final String[] kRedDisplay = { + Color.RED.toHexString() + }; + + public static final String[] kBlueDisplay = { + Color.BLUE.toHexString() + }; +} diff --git a/src/main/java/first/robot/constants/PhotonConstants.java b/src/main/java/first/robot/constants/PhotonConstants.java new file mode 100644 index 0000000..e833fa7 --- /dev/null +++ b/src/main/java/first/robot/constants/PhotonConstants.java @@ -0,0 +1,48 @@ +package first.robot.constants; + +import java.util.List; + +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.util.Units; + +import first.robot.utilities.PhotonVisionConfig; + +public class PhotonConstants { + public static final String kCamera1Name = "CameraPV1"; + public static final String kCamera2Name = "CameraPV2"; + + // TODO Need actual values for all of this + public static final Transform3d kCamera1RobotToCam = new Transform3d( + Units.inchesToMeters(1.5), + Units.inchesToMeters(-8.5), + Units.inchesToMeters(28.5), + new Rotation3d( + Units.degreesToRadians(0), + Units.degreesToRadians(-24.0), + Units.degreesToRadians(30.0) + ) + ); + public static final Transform3d kCamera2RobotToCam = new Transform3d( + Units.inchesToMeters(1.5), + Units.inchesToMeters(-10.5), + Units.inchesToMeters(28.5), + new Rotation3d( + Units.degreesToRadians(0.0), + Units.degreesToRadians(-24.0), + Units.degreesToRadians(-10.0) + ) + ); + + public static final double kCamera1HeightMeters = 0; + public static final double kCamera1PitchRadians = 0; + public static final double kCamera2HeightMeters = 0; + public static final double kCamera2PitchRadians = 0; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final List configs = List.of( + new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians), + new PhotonVisionConfig(kCamera2Name, kCamera2RobotToCam, kCamera2HeightMeters, kCamera2PitchRadians) + ); +} diff --git a/src/main/java/first/robot/constants/RollerIndexerConstants.java b/src/main/java/first/robot/constants/RollerIndexerConstants.java new file mode 100644 index 0000000..23da80c --- /dev/null +++ b/src/main/java/first/robot/constants/RollerIndexerConstants.java @@ -0,0 +1,28 @@ +package first.robot.constants; + +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class RollerIndexerConstants { + public static final int kMotor1CANID = 50; + public static final int kMotor2CANID = 4; + + public static final int kCurrentLimit = 40; + + public static final double kMotorSpeed = 1; + + public static final boolean kMotorInverted = true; + + public static final IdleMode kIdleMode = IdleMode.kBrake; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final SparkMaxConfig kMotorConfig = new SparkMaxConfig(); + + static { + kMotorConfig + .inverted(kMotorInverted) + .smartCurrentLimit(kCurrentLimit) + .idleMode(kIdleMode); + } +} \ No newline at end of file diff --git a/src/main/java/first/robot/constants/ShooterConstants.java b/src/main/java/first/robot/constants/ShooterConstants.java new file mode 100644 index 0000000..a8e66c2 --- /dev/null +++ b/src/main/java/first/robot/constants/ShooterConstants.java @@ -0,0 +1,112 @@ +package first.robot.constants; + +import org.wpilib.math.util.Units; + +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class ShooterConstants { + public enum ShooterSpeeds { + kHubSpeed(3000.0), + kFeedSpeed(5000.0), + kIdleSpeed(750.0); + + private double speedMPS; + private double speedRPM; + + private ShooterSpeeds(double speedRPM) { + this.speedMPS = speedRPM * kWheelDiameter*Math.PI; + this.speedRPM = speedRPM; + } + + public double getSpeedMPS() { + return speedMPS * kWheelDiameter*Math.PI; + } + + public double getSpeedRPM(){ + return speedRPM; + } + } + + // TODO Conversion factor? + + public static final double kWheelDiameter = Units.inchesToMeters(4); + + // TODO Real values + public static final int kLeftShooterMotorCANID = 2; + public static final int kRightShooterMotorCANID = 5; + + public static final boolean kLeftShooterMotorInverted = true; + public static final boolean kRightShooterMotorInverted = false; + + public static final double kLeftP = 0.75;//0.01;//0.001; + public static final double kLeftI = 0; + public static final double kLeftD = 0;//0.1;//1.8; + public static final double kLeftS = 0; + public static final double kLeftV = 0.00129; + public static final double kLeftA = 0; + + public static final double kRightP = 0.75;//0.001;//0.001; + public static final double kRightI = 0; + public static final double kRightD = 0;//0.1; + public static final double kRightS = 0; + public static final double kRightV = 0.00125; + public static final double kRightA = 0; + + public static final double kMaxManualSpeedMultiplier = 1; + + public static final double kShooterHeightMeters = 0; + + // TODO Is this value sane? + public static final int kCurrentLimit = 60; + + public static final IdleMode kShooterIdleMode = IdleMode.kCoast; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final SparkMaxConfig kLeftMotorConfig = new SparkMaxConfig(); + public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig(); + + static { + kLeftMotorConfig + .idleMode(kShooterIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(kLeftShooterMotorInverted); + kLeftMotorConfig.absoluteEncoder + // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing + // a solution for this. Issue is open and pending + // https://github.com/wpilibsuite/SystemcoreTesting/issues/401 + .positionConversionFactor(1) + .velocityConversionFactor(60) + .averageDepth(8); // VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER + kLeftMotorConfig.closedLoop + .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .pid(kLeftP, kLeftI, kLeftD, ClosedLoopSlot.kSlot0) + .outputRange(-1, 1) + .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0) + .feedForward + .sva(kLeftS, kLeftV, kLeftA, ClosedLoopSlot.kSlot0); + + kRightMotorConfig + .idleMode(kShooterIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(kRightShooterMotorInverted); + kRightMotorConfig.absoluteEncoder + // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing + // a solution for this. Issue is open and pending + // https://github.com/wpilibsuite/SystemcoreTesting/issues/401 + .positionConversionFactor(1) + .velocityConversionFactor(60) + .averageDepth(8)// VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER + .inverted(true); + kRightMotorConfig.closedLoop + .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .pid(kRightP, kRightI, kRightD) + .outputRange(-1, 1) + .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0) + .feedForward + .sva(kRightS, kRightV, kRightA, ClosedLoopSlot.kSlot0); + } +} diff --git a/src/main/java/first/robot/constants/SpindexerConstants.java b/src/main/java/first/robot/constants/SpindexerConstants.java new file mode 100644 index 0000000..d8384de --- /dev/null +++ b/src/main/java/first/robot/constants/SpindexerConstants.java @@ -0,0 +1,50 @@ +package first.robot.constants; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class SpindexerConstants { + // TODO Real values + public static final int kSpindexerMotorCANID = 0; + public static final int kFeederMotorCANID = 4; + + public static final int kSpindexerStatorCurrentLimit = 95; + public static final int kSpindexerSupplyCurrentLimit = 50; + public static final int kFeederCurrentLimit = 30; + + public static final double kSpindexerSpeed = 1; + public static final double kFeederSpeed = 1; + + public static final boolean kFeederMotorInverted = false; + + public static final InvertedValue kSpindexerInversionState = InvertedValue.Clockwise_Positive; + public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Coast; + + public static final IdleMode kFeederIdleMode = IdleMode.kBrake; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final SparkMaxConfig kFeederConfig = new SparkMaxConfig(); + + public static final CurrentLimitsConfigs kSpindexerCurrentLimitConfig = new CurrentLimitsConfigs(); + public static final MotorOutputConfigs kSpindexerMotorConfig = new MotorOutputConfigs(); + + static { + kSpindexerCurrentLimitConfig.StatorCurrentLimitEnable = true; + kSpindexerCurrentLimitConfig.SupplyCurrentLimitEnable = true; + kSpindexerCurrentLimitConfig.StatorCurrentLimit = kSpindexerStatorCurrentLimit; + kSpindexerCurrentLimitConfig.SupplyCurrentLimit = kSpindexerSupplyCurrentLimit; + + kSpindexerMotorConfig.Inverted = kSpindexerInversionState; + kSpindexerMotorConfig.NeutralMode = kSpindexerIdleMode; + + kFeederConfig + .inverted(kFeederMotorInverted) + .smartCurrentLimit(kFeederCurrentLimit) + .idleMode(kFeederIdleMode); + } +} diff --git a/src/main/java/first/robot/interfaces/IIndexer.java b/src/main/java/first/robot/interfaces/IIndexer.java new file mode 100644 index 0000000..868bfee --- /dev/null +++ b/src/main/java/first/robot/interfaces/IIndexer.java @@ -0,0 +1,13 @@ +package first.robot.interfaces; + +import java.util.function.DoubleSupplier; + +import org.wpilib.command2.Command; + +public interface IIndexer { + public Command spinToShooter(); + public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM); + public Command spinToIntake(); + public Command stop(); + public Command instantaneousStop(); +} diff --git a/src/main/java/first/robot/subsystems/Climber.java b/src/main/java/first/robot/subsystems/Climber.java new file mode 100644 index 0000000..0323fec --- /dev/null +++ b/src/main/java/first/robot/subsystems/Climber.java @@ -0,0 +1,88 @@ +package first.robot.subsystems; + +import java.util.function.DoubleSupplier; + +import org.wpilib.command2.Command; +import org.wpilib.command2.SubsystemBase; +import org.wpilib.epilogue.Logged; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import first.robot.constants.ClimberConstants; +import first.robot.constants.ClimberConstants.ClimberPositions; + +@Logged +public class Climber extends SubsystemBase { + private SparkMax motor; + + private RelativeEncoder encoder; + + private SparkClosedLoopController controller; + + private ClimberPositions targetPosition; + + public Climber() { + motor = new SparkMax( + ClimberConstants.kCANPort, + ClimberConstants.kMotorCANID, + MotorType.kBrushless + ); + + motor.configure( + ClimberConstants.kMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + encoder = motor.getEncoder(); + + controller = motor.getClosedLoopController(); + + targetPosition = null; + } + + public double getTargetPositionMeters() { + if (targetPosition == null) { + return -1; + } else { + return targetPosition.getPositionMeters(); + } + } + + public double getCurrentPositionMeters() { + return encoder.getPosition().get(); + } + + public boolean isAtSetpoint() { + return controller.isAtSetpoint().get(); + } + + public Command maintainPosition(ClimberPositions position) { + return run(() -> { + targetPosition = position; + + controller.setSetpoint( + position.getPositionMeters(), + ControlType.kPosition + ); + }); + } + + public Command manualSpeed(DoubleSupplier speed) { + return run(() -> { + targetPosition = null; + + motor.setThrottle(speed.getAsDouble()); + }); + } + + public Command stop() { + return manualSpeed(() -> 0); + } +} diff --git a/src/main/java/first/robot/utilities/Elastic.java b/src/main/java/first/robot/utilities/Elastic.java new file mode 100644 index 0000000..7513dbd --- /dev/null +++ b/src/main/java/first/robot/utilities/Elastic.java @@ -0,0 +1,390 @@ +package first.robot.utilities; + +import org.wpilib.networktables.NetworkTableInstance; +import org.wpilib.networktables.PubSubOption; +import org.wpilib.networktables.StringPublisher; +import org.wpilib.networktables.StringTopic; + +import io.avaje.json.JsonException; +import io.avaje.jsonb.Json; +import io.avaje.jsonb.JsonType; +import io.avaje.jsonb.Jsonb; + +public final class Elastic { + private static final StringTopic notificationTopic = + NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications"); + private static final StringPublisher notificationPublisher = + notificationTopic.publish(PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES); + private static final StringTopic selectedTabTopic = + NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab"); + private static final StringPublisher selectedTabPublisher = + selectedTabTopic.publish(PubSubOption.KEEP_DUPLICATES); + private static final Jsonb jsonb = Jsonb.builder().build(); + private static final JsonType notificationJsonType = + jsonb.type(Notification.class); + + /** + * Represents the possible levels of notifications for the Elastic dashboard. These levels are + * used to indicate the severity or type of notification. + */ + public enum NotificationLevel { + /** Informational Message */ + INFO, + /** Warning message */ + WARNING, + /** Error message */ + ERROR + } + + /** + * Sends an notification to the Elastic dashboard. The notification is serialized as a JSON string + * before being published. + * + * @param notification the {@link Notification} object containing notification details + */ + public static void sendNotification(Notification notification) { + try { + notificationPublisher.set(notificationJsonType.toJson(notification)); + } catch (JsonException e) { + e.printStackTrace(); + } + } + + /** + * Selects the tab of the dashboard with the given name. If no tab matches the name, this will + * have no effect on the widgets or tabs in view. + * + *

If the given name is a number, Elastic will select the tab whose index equals the number + * provided. + * + * @param tabName the name of the tab to select + */ + public static void selectTab(String tabName) { + selectedTabPublisher.set(tabName); + } + + /** + * Selects the tab of the dashboard at the given index. If this index is greater than or equal to + * the number of tabs, this will have no effect. + * + * @param tabIndex the index of the tab to select. + */ + public static void selectTab(int tabIndex) { + selectTab(Integer.toString(tabIndex)); + } + + /** + * Represents an notification object to be sent to the Elastic dashboard. This object holds + * properties such as level, title, description, display time, and dimensions to control how the + * notification is displayed on the dashboard. + */ + @Json + public static class Notification { + @Json.Property("level") + private NotificationLevel level; + + @Json.Property("title") + private String title; + + @Json.Property("description") + private String description; + + @Json.Property("displayTime") + private int displayTimeMillis; + + @Json.Property("width") + private double width; + + @Json.Property("height") + private double height; + + /** + * Creates a new Notification with all default parameters. This constructor is intended to be + * used with the chainable decorator methods + * + *

Title and description fields are empty. + */ + public Notification() { + this(NotificationLevel.INFO, "", ""); + } + + /** + * Creates a new Notification with all properties specified. + * + * @param level the level of the notification (e.g., INFO, WARNING, ERROR) + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param displayTimeMillis the time in milliseconds for which the notification is displayed + * @param width the width of the notification display area + * @param height the height of the notification display area, inferred if below zero + */ + public Notification( + NotificationLevel level, + String title, + String description, + int displayTimeMillis, + double width, + double height) { + this.level = level; + this.title = title; + this.displayTimeMillis = displayTimeMillis; + this.description = description; + this.height = height; + this.width = width; + } + + /** + * Creates a new Notification with default display time and dimensions. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + */ + public Notification(NotificationLevel level, String title, String description) { + this(level, title, description, 3000, 350, -1); + } + + /** + * Creates a new Notification with a specified display time and default dimensions. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param displayTimeMillis the display time in milliseconds + */ + public Notification( + NotificationLevel level, String title, String description, int displayTimeMillis) { + this(level, title, description, displayTimeMillis, 350, -1); + } + + /** + * Creates a new Notification with specified dimensions and default display time. If the height + * is below zero, it is automatically inferred based on screen size. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param width the width of the notification display area + * @param height the height of the notification display area, inferred if below zero + */ + public Notification( + NotificationLevel level, String title, String description, double width, double height) { + this(level, title, description, 3000, width, height); + } + + /** + * Updates the level of this notification + * + * @param level the level to set the notification to + */ + public void setLevel(NotificationLevel level) { + this.level = level; + } + + /** + * @return the level of this notification + */ + public NotificationLevel getLevel() { + return level; + } + + /** + * Updates the title of this notification + * + * @param title the title to set the notification to + */ + public void setTitle(String title) { + this.title = title; + } + + /** + * Gets the title of this notification + * + * @return the title of this notification + */ + public String getTitle() { + return title; + } + + /** + * Updates the description of this notification + * + * @param description the description to set the notification to + */ + public void setDescription(String description) { + this.description = description; + } + + public String getDescription() { + return description; + } + + /** + * Updates the display time of the notification + * + * @param seconds the number of seconds to display the notification for + */ + public void setDisplayTimeSeconds(double seconds) { + setDisplayTimeMillis((int) Math.round(seconds * 1000)); + } + + /** + * Updates the display time of the notification in milliseconds + * + * @param displayTimeMillis the number of milliseconds to display the notification for + */ + public void setDisplayTimeMillis(int displayTimeMillis) { + this.displayTimeMillis = displayTimeMillis; + } + + /** + * Gets the display time of the notification in milliseconds + * + * @return the number of milliseconds the notification is displayed for + */ + public int getDisplayTimeMillis() { + return displayTimeMillis; + } + + /** + * Updates the width of the notification + * + * @param width the width to set the notification to + */ + public void setWidth(double width) { + this.width = width; + } + + /** + * Gets the width of the notification + * + * @return the width of the notification + */ + public double getWidth() { + return width; + } + + /** + * Updates the height of the notification + * + *

If the height is set to -1, the height will be determined automatically by the dashboard + * + * @param height the height to set the notification to + */ + public void setHeight(double height) { + this.height = height; + } + + /** + * Gets the height of the notification + * + * @return the height of the notification + */ + public double getHeight() { + return height; + } + + /** + * Modifies the notification's level and returns itself to allow for method chaining + * + * @param level the level to set the notification to + * @return the current notification + */ + public Notification withLevel(NotificationLevel level) { + this.level = level; + return this; + } + + /** + * Modifies the notification's title and returns itself to allow for method chaining + * + * @param title the title to set the notification to + * @return the current notification + */ + public Notification withTitle(String title) { + setTitle(title); + return this; + } + + /** + * Modifies the notification's description and returns itself to allow for method chaining + * + * @param description the description to set the notification to + * @return the current notification + */ + public Notification withDescription(String description) { + setDescription(description); + return this; + } + + /** + * Modifies the notification's display time and returns itself to allow for method chaining + * + * @param seconds the number of seconds to display the notification for + * @return the current notification + */ + public Notification withDisplaySeconds(double seconds) { + return withDisplayMilliseconds((int) Math.round(seconds * 1000)); + } + + /** + * Modifies the notification's display time and returns itself to allow for method chaining + * + * @param displayTimeMillis the number of milliseconds to display the notification for + * @return the current notification + */ + public Notification withDisplayMilliseconds(int displayTimeMillis) { + setDisplayTimeMillis(displayTimeMillis); + return this; + } + + /** + * Modifies the notification's width and returns itself to allow for method chaining + * + * @param width the width to set the notification to + * @return the current notification + */ + public Notification withWidth(double width) { + setWidth(width); + return this; + } + + /** + * Modifies the notification's height and returns itself to allow for method chaining + * + * @param height the height to set the notification to + * @return the current notification + */ + public Notification withHeight(double height) { + setHeight(height); + return this; + } + + /** + * Modifies the notification's height and returns itself to allow for method chaining + * + *

This will set the height to -1 to have it automatically determined by the dashboard + * + * @return the current notification + */ + public Notification withAutomaticHeight() { + setHeight(-1); + return this; + } + + /** + * Modifies the notification to disable the auto dismiss behavior + * + *

This sets the display time to 0 milliseconds + * + *

The auto dismiss behavior can be re-enabled by setting the display time to a number + * greater than 0 + * + * @return the current notification + */ + public Notification withNoAutoDismiss() { + setDisplayTimeMillis(0); + return this; + } + } +} \ No newline at end of file diff --git a/src/main/java/first/robot/utilities/PhotonVisionConfig.java b/src/main/java/first/robot/utilities/PhotonVisionConfig.java new file mode 100644 index 0000000..5230674 --- /dev/null +++ b/src/main/java/first/robot/utilities/PhotonVisionConfig.java @@ -0,0 +1,10 @@ +package first.robot.utilities; + +import org.wpilib.math.geometry.Transform3d; + +public record PhotonVisionConfig ( + String cameraName, + Transform3d robotToCamera, + double cameraHeightMeters, + double cameraPitchRadians +) {} diff --git a/src/main/java/first/robot/utilities/SparkMAXTester.java b/src/main/java/first/robot/utilities/SparkMAXTester.java new file mode 100644 index 0000000..056bea0 --- /dev/null +++ b/src/main/java/first/robot/utilities/SparkMAXTester.java @@ -0,0 +1,38 @@ +package first.robot.utilities; + +import java.util.function.DoubleSupplier; + +import org.wpilib.command2.Command; +import org.wpilib.command2.SubsystemBase; +import org.wpilib.hardware.bus.CANPort; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +/** + * A simple subsystem that can be used to test a single SparkMax and associated NEO motor + */ +public class SparkMAXTester extends SubsystemBase { + private SparkMax spark; + + /** + * Constructor + * + * @param deviceID The CAN ID of the SparkMAX that needs testing + */ + public SparkMAXTester(CANPort canPort, int deviceID) { + spark = new SparkMax(canPort, deviceID, MotorType.kBrushless); + } + + /** + * Sets the speed of the motor + * + * @param speed A method or lambda which returns a double between -1 and 1 + * @return A Command object that runs indefinitely to control motor speed + */ + public Command setSpeed(DoubleSupplier speed) { + return run(() -> { + spark.setThrottle(speed.getAsDouble()); + }); + } +} diff --git a/src/main/java/first/robot/utilities/SwerveModule.java b/src/main/java/first/robot/utilities/SwerveModule.java new file mode 100644 index 0000000..70275ca --- /dev/null +++ b/src/main/java/first/robot/utilities/SwerveModule.java @@ -0,0 +1,226 @@ +package first.robot.utilities; + +import org.wpilib.hardware.bus.CANPort; +import org.wpilib.hardware.rotation.AnalogEncoder; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleVelocity; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import first.robot.constants.ModuleConstants; +import first.robot.constants.ModuleConstants.ModuleName; + +/* + * This thread + * + * https://www.chiefdelphi.com/t/best-easiest-way-to-connect-wire-and-program-thrifty-absolute-magnetic-encoder-to-rev-spark-max-motor-controller/439040/30 + * + * implies that the best use of the thrifty absolute encoder is to use it as a reference for the Spark relative encoder and then + * used the closed loop control on the controller for turning + * + * IDK if that's really necessary, the read rate of the analog ports is 100HZ, I suppose the only benefit is the higher rate of + * the controller closed loop controller. + */ +public class SwerveModule { + private ModuleName moduleName; + + private TalonFX drive; + private SparkMax turning; + + private RelativeEncoder turningRelativeEncoder; + + private AnalogEncoder turningAbsoluteEncoder; + + private SparkClosedLoopController turningClosedLoopController; + + private VelocityVoltage driveVelocityRequest; + + private SwerveModuleVelocity lastTargetState; + private SwerveModuleVelocity lastTargetStateOptimized; + + private boolean isAbsoluteEncoderDisabled; + private boolean turningEncoderAutoRezeroEnabled; + + /** + * Builds the swerve module but with the Absolute Encoder disabled. + * + * This constructor assumes you zeroed the swerve modules (faced all the bevel gears to the left) + * before booting up the robot. + * + * @param canPort The CANPort enumeration that represents the CAN bus that the motors for this module live on + * @param moduleName The module name, Front Left, Front Right, etc. + * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel + * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel + */ + public SwerveModule(CANPort canPort, ModuleName moduleName, int drivingCANID, int turningCANID) { + this(canPort, moduleName, drivingCANID, turningCANID, -1, -1); + } + + /** + * Builds the swerve module with the normal features, disables automatic rezeroing of the turning encoder + * from the absolute encoder. + * + * @param canPort The CANPort enumeration that represents the CAN bus that the motors for this module live on + * @param moduleName The module name, Front Left, Front Right, etc. + * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel + * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel + * @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder + * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module + */ + public SwerveModule(CANPort canPort, ModuleName moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) { + this(canPort, moduleName, drivingCANID, turningCANID, analogEncoderID, analogEncoderOffset, false); + } + + /** + * Builds the swerve module with the normal features, and gives the option to enable automatic turning encoder rezeroing + * when the turning motor is not moving + * + * @param canPort The CANPort enumeration that represents the CAN bus that the motors for this module live on + * @param moduleName The module name, Front Left, Front Right, etc. + * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel + * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel + * @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder + * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module + * @param turningEncoderAutoRezeroEnabled Should the turning encoder in the NEO automatically rezero from the absolute encoder + */ + public SwerveModule(CANPort canPort, ModuleName moduleName, int drivingCANID, int turningCANID, + int analogEncoderID, double analogEncoderOffset, boolean turningEncoderAutoRezeroEnabled) { + isAbsoluteEncoderDisabled = (analogEncoderID == -1) || (analogEncoderOffset < 0); + + drive = new TalonFX(drivingCANID, new CANBus(canPort)); + turning = new SparkMax(canPort, turningCANID, MotorType.kBrushless); + + turningRelativeEncoder = turning.getEncoder(); + + if(!isAbsoluteEncoderDisabled) { + turningAbsoluteEncoder = new AnalogEncoder(analogEncoderID, 2 * Math.PI, analogEncoderOffset); + } + + turningClosedLoopController = turning.getClosedLoopController(); + + drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig); + drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig); + drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig); + drive.getConfigurator().apply(ModuleConstants.kAudioConfig); + drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config); + drive.getConfigurator().apply(ModuleConstants.kDriveClosedLoopRampConfig); + + turning.configure( + ModuleConstants.turningConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + driveVelocityRequest = new VelocityVoltage(0); + + if(isAbsoluteEncoderDisabled){ + turningRelativeEncoder.setPosition(0); + } else { + turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get()); + } + + drive.setPosition(0); + + this.lastTargetState = getState(); + this.lastTargetStateOptimized = getState(); + + this.turningEncoderAutoRezeroEnabled = turningEncoderAutoRezeroEnabled; + + this.moduleName = moduleName; + } + + public void periodic() { + + if(!isAbsoluteEncoderDisabled) { + Logger.recordOutput(moduleName.getLoggableName() + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get()); + } + + Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetState", lastTargetState); + Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetStateOptimized", lastTargetStateOptimized); + Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModuleState", getState()); + Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModulePosition", getPosition()); + Logger.recordOutput(moduleName.getLoggableName() + "/RelativeEncoderPosition", getTurningEncoderPosition()); + + // TODO Re-enable this? Was turned off when there was drivetrain issues + // Now that there aren't, do we try this again? + /* + if(!isAbsoluteEncoderDisabled && turningEncoderAutoRezeroEnabled) { + if(Math.abs(getState().angle.getRadians() - lastTargetState.angle.getRadians()) <= ModuleConstants.kAutoResetPositionDeadband) { + resetEncoders(); + } + }*/ + } + + public ModuleName getModuleName() { + return moduleName; + } + + public SwerveModuleVelocity getState() { + return new SwerveModuleVelocity( + drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, + new Rotation2d(getTurningEncoderPosition()) + ); + } + + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, + new Rotation2d(getTurningEncoderPosition()) + ); + } + + public void disableOutput() { + drive.disable(); + turning.disable(); + } + + public void setDesiredState(SwerveModuleVelocity desiredState) { + lastTargetState = new SwerveModuleVelocity(desiredState.velocity, desiredState.angle); + + desiredState.optimize(new Rotation2d(getTurningEncoderPosition())); + + lastTargetStateOptimized = desiredState; + + drive.setControl( + driveVelocityRequest.withVelocity( + desiredState.velocity / ModuleConstants.kWheelCircumferenceMeters + ).withFeedForward( + desiredState.velocity / ModuleConstants.kWheelCircumferenceMeters + ) + ); + + turningClosedLoopController.setSetpoint( + desiredState.angle.getRadians(), + ControlType.kPosition + ); + } + + public double getTurningEncoderPosition() { + return turningRelativeEncoder.getPosition().get() * (ModuleConstants.kIsEncoderInverted ? -1 : 1); + } + + public void resetEncoders() { + drive.setPosition(0); + + zeroTurningEncoder(); + } + + public void zeroTurningEncoder() { + if(isAbsoluteEncoderDisabled) { + turningRelativeEncoder.setPosition(0); + } else { + turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get()); + } + } +} + diff --git a/src/main/java/first/robot/utilities/Utilities.java b/src/main/java/first/robot/utilities/Utilities.java new file mode 100644 index 0000000..eda7ae3 --- /dev/null +++ b/src/main/java/first/robot/utilities/Utilities.java @@ -0,0 +1,119 @@ +package first.robot.utilities; + +import java.util.Optional; + +import org.wpilib.driverstation.Alliance; +import org.wpilib.driverstation.MatchState; +import org.wpilib.math.geometry.Pose2d; + +import first.robot.constants.CompetitionConstants; + +public class Utilities { + public static final double kG = -9.81; + + /** + * Returns the Alliance enumeration that indicates who will have the first + * shift. Returns null if the data is not available. + * + * @return The Alliance that will have the first shift, or null if game specific data + * is not present + */ + public static Alliance whoHasFirstShift() { + Optional gameData = MatchState.getGameData(); + + if(gameData.isPresent()) { + String gameDataString = gameData.get(); + + if(gameDataString.length() > 0) { + switch (gameDataString.charAt(0)) { + case 'B': + return Alliance.RED; + case 'R': + return Alliance.BLUE; + default: + return null; + } + } + } + return null; + } + + /** + * Returns the pose of the hub for the given alliance assigned to our robot. + * If no alliance is assigned (which is unlikely) this method returns + * the Blue hub pose, which is the closet to the field origin + * + * @return The Pose2d object which represents the appropriate pose of the Hub + */ + public static Pose2d getHubPose() { + Optional alliance = MatchState.getAlliance(); + + if(alliance.isEmpty() || alliance.get() == Alliance.BLUE) { + return CompetitionConstants.kBlueHubLocation; + } else { + return CompetitionConstants.kRedHubLocation; + } + } + + /** + * Returns the AprilTag ID of the tag that is in the center of the hub + * for the robot's assigned alliance. If no alliance is assigned (which is unlikely) + * the Blue hub's center tag is returned. + * + * @return The tag ID that is in the center of the assigned alliance's hub + */ + public static int getHubCenterAprilTagID() { + Optional alliance = MatchState.getAlliance(); + + if(alliance.isEmpty() || alliance.get() == Alliance.BLUE) { + return 26; + } else { + return 10; + } + } + + /** + * A ChatGPT possible hallucination related to calcuating whether a shot is possible + * for a given speed and change in X and Y position + * + * Note that X in this scenario is the physical distance from the shooter exit to + * target. Y is the change in height from the shooter exit to the target height + * + * @param targetVMPS The target velocity of the shooter in Meters per Second + * @param deltaXM The "as the crow flies" distance between the shooter exit and the target + * @param deltaYM The height difference between the shooter exit and the target + * @return A true or false value indicating whether the shot is possible + */ + public static boolean shotPossible(double targetVMPS, double deltaXM, double deltaYM) { + return Math.pow(targetVMPS, 4) >= + -9.81 * (-9.81 * Math.pow(deltaXM, 2) + 2 * deltaYM * Math.pow(targetVMPS, 2)); + } + + /** + * A ChatGPT possible hallucination that calculates the angle required to make a shot for + * a given speed and change in X and Y position + * + * Note that X in this scenario is the physical disatance from the shooter exit to + * target. Y is the change in height from the shooter exit to the target height + * + * Setting softerShot to true changes the angle of attack to a soft, long range shot. False + * makes the shot more of a lob + * + * @param targetVMPS The target velocity of the shooter in Meters per Second + * @param deltaXM The "as the crow flies" distance between the shooter exit and the target + * @param deltaYM The height difference between the shooter exit and the target + * @param softerShot True for a long range shot, false for a short range lob + * @return The angle required of the shooter to make the shot described by the input parameters + */ + public static double shotAngle(double targetVMPS, double deltaXM, double deltaYM, boolean softerShot) { + double vPow2 = Math.pow(targetVMPS, 2); + double vPow4 = Math.pow(targetVMPS, 4); + double sqrtPiece = Math.sqrt(vPow4 - kG * (kG * Math.pow(deltaXM, 2) + 2 * deltaYM * vPow2)); + + if(softerShot) { + return Math.atan((vPow2 - sqrtPiece) / (kG * deltaXM)); + } else { + return Math.atan((vPow2 + sqrtPiece) / (kG * deltaXM)); + } + } +} diff --git a/src/main/java/first/robot/utilities/VisualPose.java b/src/main/java/first/robot/utilities/VisualPose.java new file mode 100644 index 0000000..f314a6d --- /dev/null +++ b/src/main/java/first/robot/utilities/VisualPose.java @@ -0,0 +1,9 @@ +package first.robot.utilities; + +import org.wpilib.math.geometry.Pose2d; + +/** + * A record class which represents the source of a visual pose, the pose itself + * and the timestamp the pose was generated. + */ +public record VisualPose(String cameraName, Pose2d visualPose, double timestamp) {} diff --git a/vendordeps/PathplannerLibSystemCoreAlpha.json b/vendordeps/PathplannerLibSystemCoreAlpha.json index ae3ba59..03f3868 100644 --- a/vendordeps/PathplannerLibSystemCoreAlpha.json +++ b/vendordeps/PathplannerLibSystemCoreAlpha.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLibSystemCoreAlpha.json", "name": "PathplannerLib", - "version": "2027.0.0-alpha-4", + "version": "2027.0.0-alpha-4.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "wpilibYear": "2027_alpha7", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2027.0.0-alpha-4" + "version": "2027.0.0-alpha-4.1" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2027.0.0-alpha-4", + "version": "2027.0.0-alpha-4.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false,