diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index db1ddb1..0cc2936 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -29,6 +29,7 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; import java.lang.reflect.Field; @@ -36,12 +37,17 @@ import edu.wpi.first.units.AngleUnit; import edu.wpi.first.units.Measure; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.DistanceUnit; +import edu.wpi.first.units.PerUnit; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Per; import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.Velocity; import frc.robot.utils.ShootingEntry; @@ -58,322 +64,367 @@ */ public final class Constants { - public static final class DriveConstants { - // Driving Parameters - Note that these are not the maximum capable speeds of - // the robot, rather the allowed maximum speeds - - public static final LinearVelocity kMaxSpeed = MetersPerSecond.of(2.4); - public static final LinearAcceleration kMaxAcceleration = MetersPerSecondPerSecond.of(10.0); - - public static final AngularVelocity kMaxAngularSpeed = RadiansPerSecond.of(2 * Math.PI); - public static final AngularAcceleration kMaxAngularAcceleration = RadiansPerSecondPerSecond.of(4 * Math.PI); - - // Chassis configuration - public static final double kTrackWidth = Units.inchesToMeters(26.5); - // Distance between centers of right and left wheels on robot - public static final double kWheelBase = Units.inchesToMeters(26.5); - // Distance between front and back wheels on robot - 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)); - - // Angular offsets of the modules relative to the chassis in radians - public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; - public static final double kFrontRightChassisAngularOffset = 0; - public static final double kBackLeftChassisAngularOffset = Math.PI; - public static final double kBackRightChassisAngularOffset = Math.PI / 2; - - // SPARK MAX CAN IDs Drive Motors - public static final int kFrontLeftDrivingCanId = 10; - public static final int kRearLeftDrivingCanId = 14; - public static final int kFrontRightDrivingCanId = 1; - public static final int kRearRightDrivingCanId = 2; - - // SPARK MAX CAN IDs Turning Motors - public static final int kFrontLeftTurningCanId = 11; - public static final int kRearLeftTurningCanId = 15; - public static final int kFrontRightTurningCanId = 62; - public static final int kRearRightTurningCanId = 3; - - // Auxiliary Device Can IDs - public static final int kPidgeyCanId = 13; - - public static final boolean kGyroReversed = false; - - public static final Time kPeriodicInterval = Seconds.of(0.02); - - public static final double kAutoRotationP = Robot.isReal() ? 3.6 : 3.0; - public static final double kAutoRotationI = 0.0; - public static final double kAutoRotationD = 0.0; - - public static enum RangeType { - Within, - CloseMin, - CloseMax + public static final class DriveConstants { + // Driving Parameters - Note that these are not the maximum capable speeds of + // the robot, rather the allowed maximum speeds + + public static final LinearVelocity kMaxSpeed = MetersPerSecond.of(2.4); + public static final LinearAcceleration kMaxAcceleration = MetersPerSecondPerSecond.of(10.0); + + public static final AngularVelocity kMaxAngularSpeed = RadiansPerSecond.of(2 * Math.PI); + public static final AngularAcceleration kMaxAngularAcceleration = RadiansPerSecondPerSecond + .of(4 * Math.PI); + + // Chassis configuration + public static final double kTrackWidth = Units.inchesToMeters(26.5); + // Distance between centers of right and left wheels on robot + public static final double kWheelBase = Units.inchesToMeters(26.5); + // Distance between front and back wheels on robot + 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)); + + // Angular offsets of the modules relative to the chassis in radians + public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; + public static final double kFrontRightChassisAngularOffset = 0; + public static final double kBackLeftChassisAngularOffset = Math.PI; + public static final double kBackRightChassisAngularOffset = Math.PI / 2; + + // SPARK MAX CAN IDs Drive Motors + public static final int kFrontLeftDrivingCanId = 10; + public static final int kRearLeftDrivingCanId = 14; + public static final int kFrontRightDrivingCanId = 1; + public static final int kRearRightDrivingCanId = 2; + + // SPARK MAX CAN IDs Turning Motors + public static final int kFrontLeftTurningCanId = 11; + public static final int kRearLeftTurningCanId = 15; + public static final int kFrontRightTurningCanId = 62; + public static final int kRearRightTurningCanId = 3; + + // Auxiliary Device Can IDs + public static final int kPidgeyCanId = 13; + + public static final boolean kGyroReversed = false; + + public static final Time kPeriodicInterval = Seconds.of(0.02); + + public static final double kAutoRotationP = Robot.isReal() ? 3.6 : 3.0; + public static final double kAutoRotationI = 0.0; + public static final double kAutoRotationD = 0.0; + + public static enum RangeType { + Within, + CloseMin, + CloseMax + } + + public static final Angle kTurnToAngleTolerance = Degrees.of(2); } - public static final Angle kTurnToAngleTolerance = Degrees.of(2); - } + public static final class ModuleConstants { + // The MAXSwerve module can be configured with one of three pinion gears: 12T, + // 13T, or 14T. This changes the drive speed of the module (a pinion gear with + // more teeth will result in a robot that drives faster). - public static final class ModuleConstants { - // The MAXSwerve module can be configured with one of three pinion gears: 12T, - // 13T, or 14T. This changes the drive speed of the module (a pinion gear with - // more teeth will result in a robot that drives faster). + public static final int kDrivingMotorPinionTeeth = 14; - public static final int kDrivingMotorPinionTeeth = 14; + // Calculations required for driving motor conversion factors and feed forward + public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; + public static final double kWheelDiameterMeters = 0.0762; + public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; + // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 + // teeth on the bevel pinion - // Calculations required for driving motor conversion factors and feed forward - public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; - public static final double kWheelDiameterMeters = 0.0762; - public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; - // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 - // teeth on the bevel pinion + public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); + public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps + * kWheelCircumferenceMeters) + / kDrivingMotorReduction; + } - public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); - public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) - / kDrivingMotorReduction; - } + public static final class OIConstants { - public static final class OIConstants { + public static final int kDriverControllerPort = 0; + public static final double kDriveDeadband = 0.05; + } - public static final int kDriverControllerPort = 0; - public static final double kDriveDeadband = 0.05; - } + public static final class AutoConstants { - public static final class AutoConstants { + public static final double kMaxSpeedMetersPerSecond = 3; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - public static final double kMaxSpeedMetersPerSecond = 3; - public static final double kMaxAccelerationMetersPerSecondSquared = 3; - public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + public static final double kPXController = 1; + public static final double kPYController = 1; + public static final double kPThetaController = 1; - public static final double kPXController = 1; - public static final double kPYController = 1; - public static final double kPThetaController = 1; + // Constraint for the motion profiled robot angle controller + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); - // Constraint for the motion profiled robot angle controller - public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( - kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); + // Whether pathplanner should actually reset odometry for pathplanner autos + public static final boolean kIgnoreResetOdometry = false; - // Whether pathplanner should actually reset odometry for pathplanner autos - public static final boolean kIgnoreResetOdometry = false; + // Auto Names + public static final String kExampleAutoName = "Example Auto"; + } - // Auto Names - public static final String kExampleAutoName = "Example Auto"; - } + public static final class NeoMotorConstants { - public static final class NeoMotorConstants { + public static final double kFreeSpeedRpm = 5676; + } - public static final double kFreeSpeedRpm = 5676; - } + public static final class VisionConstants { - public static final class VisionConstants { + public static final String kCameraName1 = "Photonvision"; + public static final String kCameraName2 = "Photonvision2"; - public static final String kCameraName1 = "Photonvision"; - public static final String kCameraName2 = "Photonvision2"; + // Distance to fill pose3d z value assuming robot is on the ground + public static Distance kEncoderZOffset = Inches.of(5.0); - // Distance to fill pose3d z value assuming robot is on the ground - public static Distance kEncoderZOffset = Inches.of(5.0); + // Confidence of encoder readings for vision; should be tuned + public static final double kEncoderConfidence = 0.15; - // Confidence of encoder readings for vision; should be tuned - public static final double kEncoderConfidence = 0.15; + public static final Transform3d kRobotToCamOne = new Transform3d(new Translation3d(0.5, 0.0, 0.5), + new Rotation3d(0, 0, 0)); - public static final Transform3d kRobotToCamOne = new Transform3d(new Translation3d(0.5, 0.0, 0.5), - new Rotation3d(0, 0, 0)); + // These are not final numbers + public static final Transform3d kRobotToCamTwo = new Transform3d( + new Translation3d(Inches.of(8.375), Inches.of(-2.16), Inches.of(-20.668)), + new Rotation3d(0, 0, 0)); - // These are not final numbers - public static final Transform3d kRobotToCamTwo = new Transform3d( - new Translation3d(Inches.of(8.375), Inches.of(-2.16), Inches.of(-20.668)), - new Rotation3d(0, 0, 0)); + public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout + .loadField(AprilTagFields.kDefaultField); - public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout - .loadField(AprilTagFields.kDefaultField); + // Placeholder numbers + public static final Distance kTurretCameraDistanceToCenter = Meters.of(0.13); + public static final Distance kCameraTwoZ = Inches.of(18.0); - // Placeholder numbers - public static final Distance kTurretCameraDistanceToCenter = Meters.of(0.13); - public static final Distance kCameraTwoZ = Inches.of(18.0); + public static final Translation3d kTurretCenterOfRotation = new Translation3d(Meters.of(0.0), + Meters.of(0.0), + Inches.of(18)); - public static final Translation3d kTurretCenterOfRotation = new Translation3d(Meters.of(0.0), Meters.of(0.0), - Inches.of(18)); + public static final Angle kCameraTwoPitch = Degrees.of(15.0); + public static final Angle kCameraTwoRoll = Degrees.of(0.0); + public static final Angle kCameraTwoYaw = Degrees.of(-21.0); - public static final Angle kCameraTwoPitch = Degrees.of(15.0); - public static final Angle kCameraTwoRoll = Degrees.of(0.0); - public static final Angle kCameraTwoYaw = Degrees.of(-21.0); + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); - public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); - public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + public static final Matrix kStateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); + public static final Matrix kVisionStdDevs = VecBuilder.fill(1, 1, 1); - public static final Matrix kStateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); - public static final Matrix kVisionStdDevs = VecBuilder.fill(1, 1, 1); + public static final AngularVelocity kMaxTurretVisionSpeed = RPM.of(30); + } - public static final AngularVelocity kMaxTurretVisionSpeed = RPM.of(30); - } + public static final class NumericalConstants { + public static final double kEpsilon = 1e-6; + public static final Angle kFullRotation = Radians.of(2.0 * Math.PI); + public static final Angle kNoRotation = Radians.of(0.0); + public static final LinearAcceleration kGravity = MetersPerSecondPerSecond.of(9.807); + } - public static final class NumericalConstants { - public static final double kEpsilon = 1e-6; - public static final Angle kFullRotation = Radians.of(2.0 * Math.PI); - public static final Angle kNoRotation = Radians.of(0.0); - public static final LinearAcceleration kGravity = MetersPerSecondPerSecond.of(9.807); - } + public static final class TurretConstants { + public static final int kMotorId = 18; // Was 20 + public static final Angle kMinAngle = Rotations.of(0.1); + public static final Angle kMaxAngle = Rotations.of(0.854); - public static final class TurretConstants { - public static final int kMotorId = 18; // Was 20 - public static final Angle kMinAngle = Radians.of(0.0); - public static final Angle kMaxAngle = Degrees.of(340); + public static final int kPositionBufferLength = 4000; + public static final Time kEncoderReadingDelay = Seconds.of(0.005); - public static final int kPositionBufferLength = 4000; - public static final Time kEncoderReadingDelay = Seconds.of(0.005); + public static final Time kEncoderReadInterval = Seconds.of(0.01); - public static final Time kEncoderReadInterval = Seconds.of(0.01); + public static final double kP = 1.5; + public static final double kI = 0.0; + public static final double kD = 0.0; - public static final double kP = 1.5; - public static final double kI = 0.0; - public static final double kD = 0.0; + public static final int kSmartCurrentLimit = 40; - public static final int kSmartCurrentLimit = 40; + public static final Angle kHubMinAngle1 = Degrees.of(311); + public static final Angle kHubMaxAngle1 = Degrees.of(351); - public static final Angle kHubMinAngle1 = Rotations.of(0.375); - public static final Angle kHubMaxAngle1 = Rotations.of(0.582); + public static final Angle kHubMinAngle2 = Degrees.of(180); + public static final Angle kHubMaxAngle2 = Degrees.of(224); - public static final Angle kHubMinAngle2 = Rotations.of(0.833); - public static final Angle kHubMaxAngle2 = Rotations.of(0.000); + public static final Angle kFeedMinAngle = Degrees.of(180); + public static final Angle kFeedMaxAngle = Degrees.of(224); - public static final Angle kFeedMinAngle = Rotations.of(0.375); - public static final Angle kFeedMaxAngle = Rotations.of(0.582); + public static final Angle kTurretCameraIdleViewMinAngle = Rotations.of(0.375); + public static final Angle kTurretCameraIdleViewMaxAngle = Rotations.of(0.582); + public static final Angle kTurretCameraMidPoint = kTurretCameraIdleViewMinAngle + .plus(kTurretCameraIdleViewMaxAngle).div(2.0); - public static final Angle kTurretCameraIdleViewMinAngle = Rotations.of(0.375); - public static final Angle kTurretCameraIdleViewMaxAngle = Rotations.of(0.582); - public static final Angle kTurretCameraMidPoint = kTurretCameraIdleViewMinAngle - .plus(kTurretCameraIdleViewMaxAngle).div(2.0); + public static final Angle[] kRestrictedAngles = new Angle[] { + kFeedMinAngle, kFeedMaxAngle + }; - public static final Angle[] kRestrictedAngles = new Angle[] { - kFeedMinAngle, kFeedMaxAngle - }; + public static final Angle[] kUnrestrictedAngles = new Angle[] { + kHubMinAngle1, kHubMaxAngle1, kHubMinAngle2, kHubMaxAngle2 + }; - public static final Angle[] kUnrestrictedAngles = new Angle[] { - kHubMinAngle1, kHubMaxAngle1, kHubMinAngle2, kHubMaxAngle2 - }; + public static final Angle kOvershootAmount = Degrees.of(10.0); - public static final Angle kOvershootAmount = Degrees.of(10.0); + public static final Translation2d kTurretOffset = new Translation2d(Meters.of(0.0), Meters.of(0.0)); - public static final Translation2d kTurretOffset = new Translation2d(Meters.of(0.0), Meters.of(0.0)); + public static final Angle kTurretAngleTolerance = Degrees.of(2.0); - public static final Angle kTurretAngleTolerance = Degrees.of(2.0); + // TODO: Change to real numbers + public static Angle kNonAimTurretAngle = Degrees.of(25.0); + public static int kTurretMotorAmpLimit = 10; - // TODO: Change to real numbers - public static Angle kNonAimTurretAngle = Degrees.of(25.0); - } + public static final Angle kAngularDistanceToFrontOfRobot = Rotations.of(0.605); + } - public static final class ShooterConstants { - public static final int kShooterMotorId = 70; - public static final int kHoodMotorId = 71; // Was 31 + public static final class ShooterConstants { + public static final int kShooterMotorId = 5; + public static final int kHoodMotorId = 4; // Was 31 + + public static final double kHoodP = 5.0; // Only use this high P when converion factor is 1. + public static final double kHoodI = 0.0; + public static final double kHoodD = 0.0; + + public static final double kShooterP = 0.1; + public static final double kShooterI = 0.0; + public static final double kShooterD = 0.0; + + // Teeth on encoder gear to teeth on shaft, teeth on shaft to teeth on hood part + // NOTE: Need to use 14D so the result is a double, otherwise you end up with + // zero. + // 2.48 * 0.0642201834862385 = 0.1592660550458716 + // 16.5 motor rotations to one absolute encoder rotation (roughly) + // NOTE: gear ration commented out for now is it isn't used + // public static final double kHoodGearRatio = (62D / 25) * (14D / 218); + public static final int kHoodSmartCurrentLimit = 20; + public static final Angle kFeedAngle = Degrees.of(90.0); + + public static final AngularVelocity kPlaceholderWheelVelocity = RPM.of(2000); + public static final LinearVelocity kMuzzleVelocity = MetersPerSecond.of(10); + + public static final LinearVelocity kMaxMuzzleVelocity = MetersPerSecond.of(10.0); + + public static final ShootingEntry[] kShootingEntries = { + new ShootingEntry(Meters.of(0.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, + Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(1.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, + Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(2.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, + Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(3.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, + Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(4.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, + Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(5.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, + Seconds.of(1.0), + kFeedAngle), + }; + + public static final Angle kHoodTolerence = Degrees.of(2.0); + + public static final LinearVelocity kMaxStationaryVelocity = MetersPerSecond.of(1e-1); + + public static final double kHoodMinAbsolutePosition = 0.0; + public static final double kHoodMaxAbsolutePosition = 0.55; + + public static final double kHoodDegreeConversionFactor = kHoodMaxAbsolutePosition / 30; + + // TODO: Change to real numbers + public static AngularVelocity kNonAimShooterVelocity = RPM.of(500); + public static Angle kNonAimHoodAngle = Degrees.of(15.0); + public static AngularVelocity kFeedingWheelVelocity = RPM.of(60); + public static Angle kHoodFeedingPosition = Degrees.of(25.0); + public static Measure kTurretAngleRestrictiveShooterAngle = Degrees.of(10); + } - public static final double kHoodP = 5.0; // Only use this high P when converion factor is 1. - public static final double kHoodI = 0.0; - public static final double kHoodD = 0.0; + public static final class StagingConstants { + public static int kFeedIntoHoodMotor = 16; + public static double kFeedIntoHoodSpeed = 0.10; - public static final double kShooterP = 0.1; - public static final double kShooterI = 0.0; - public static final double kShooterD = 0.0; + public static final int kAgitationMotorId = 9; + public static final double kAgitationSpeed = -0.15; - // Teeth on encoder gear to teeth on shaft, teeth on shaft to teeth on hood part - // NOTE: Need to use 14D so the result is a double, otherwise you end up with - // zero. - // 2.48 * 0.0642201834862385 = 0.1592660550458716 - // 16.5 motor rotations to one absolute encoder rotation (roughly) - // NOTE: gear ration commented out for now is it isn't used - // public static final double kHoodGearRatio = (62D / 25) * (14D / 218); - public static final int kHoodSmartCurrentLimit = 20; - public static final Angle kFeedAngle = Degrees.of(90.0); + public static final int kRollerMotorId = 12; + public static final double kRollerSpeed = 0.25; + } - public static final AngularVelocity kPlaceholderWheelVelocity = RPM.of(2000); - public static final LinearVelocity kMuzzleVelocity = MetersPerSecond.of(10); + public static final class Fixtures { + public static final Translation2d kBlueAllianceHub = new Translation2d(Inches.of(182.11), + Inches.of(154.84)); + public static final Translation2d kRedAllianceHub = new Translation2d(Inches.of(651.22 - 182.11), + Inches.of(158.84)); - public static final LinearVelocity kMaxMuzzleVelocity = MetersPerSecond.of(10.0); + // From a top down perspective of the field with the red alliance on the left + // side + public static final Translation2d kTopFeedPose = new Translation2d(); + public static final Translation2d kBottomFeedPose = new Translation2d(); - public static final ShootingEntry[] kShootingEntries = { - new ShootingEntry(Meters.of(0.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(1.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(2.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(3.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(4.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(5.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), - kFeedAngle), - }; + public static final Distance kFieldYMidpoint = Inches.of(158.84); - public static final Angle kHoodTolerence = Degrees.of(2.0); + public static final Distance kBlueSideNeutralBorder = Inches.of(182.11); + public static final Distance kRedSideNeutralBorder = Inches.of(651.22 - 182.11); - public static final LinearVelocity kMaxStationaryVelocity = MetersPerSecond.of(1e-1); - - public static final double kHoodMinAbsolutePosition = 0.0; - public static final double kHoodMaxAbsolutePosition = 0.55; + public static enum FieldLocations { + AllianceSide, + NeutralSide, + OpponentSide + } - public static final double kHoodDegreeConversionFactor = kHoodMaxAbsolutePosition / 30; + public static final HashMap kFieldLocationStringMap = new HashMap<>(); - // TODO: Change to real numbers - public static AngularVelocity kNonAimShooterVelocity = RPM.of(500); - public static Angle kNonAimHoodAngle = Degrees.of(15.0); - public static AngularVelocity kFeedingWheelVelocity = RPM.of(500); - public static Angle kHoodFeedingPosition = Degrees.of(25.0); - public static Measure kTurretAngleRestrictiveShooterAngle = Degrees.of(10); - } + static { + kFieldLocationStringMap.put(FieldLocations.AllianceSide, "Alliance Side"); + kFieldLocationStringMap.put(FieldLocations.NeutralSide, "Neutral Side"); + kFieldLocationStringMap.put(FieldLocations.OpponentSide, "Opponent Side"); + } - public static final class StagingConstants { - public static int kFeedIntoHoodMotor = 30; - public static double kFeedIntoHoodSpeed = 0.4; + // Placeholders + public static final Angle kBlueLeftSideFeedHeading = Degrees.of(40); + public static final Angle kBlueRightSideFeedHeading = Degrees.of(160); - public static final int kAgitationMotorId = 31; - public static final double kAgitationSpeed = 0.2; + // Placeholders + public static final Angle kRedLeftSideFeedHeading = Degrees.of(-40); + public static final Angle kRedRightSideFeedHeading = Degrees.of(-160); - public static final int kRollerMotorId = 32; - public static final double kRollerSpeed = 0.3; - } - - public static final class Fixtures { - public static final Translation2d kBlueAllianceHub = new Translation2d(Inches.of(182.11), Inches.of(154.84)); - public static final Translation2d kRedAllianceHub = new Translation2d(Inches.of(651.22 - 182.11), - Inches.of(158.84)); - - // From a top down perspective of the field with the red alliance on the left - // side - public static final Translation2d kTopFeedPose = new Translation2d(); - public static final Translation2d kBottomFeedPose = new Translation2d(); - - public static final Distance kFieldYMidpoint = Inches.of(158.84); - - public static final Distance kBlueSideNeutralBorder = Inches.of(182.11); - public static final Distance kRedSideNeutralBorder = Inches.of(651.22 - 182.11); - - public static enum FieldLocations { - AllianceSide, - NeutralSide, - OpponentSide + public static final Pose2d kRedHubAprilTag = AprilTagFieldLayout + .loadField(AprilTagFields.k2026RebuiltAndymark) + .getTagPose(3).get().toPose2d(); } - public static final HashMap kFieldLocationStringMap = new HashMap<>(); + public static final class IntakeConstants { + public static final double kP1 = 0.05; + public static final double kI1 = 0; + public static final double kD1 = 0; - static { - kFieldLocationStringMap.put(FieldLocations.AllianceSide, "Alliance Side"); - kFieldLocationStringMap.put(FieldLocations.NeutralSide, "Neutral Side"); - kFieldLocationStringMap.put(FieldLocations.OpponentSide, "Opponent Side"); - } + public static final double kP2 = 0.05; + public static final double kI2 = 0; + public static final double kD2 = 0; - // Placeholders - public static final Angle kBlueLeftSideFeedHeading = Degrees.of(40); - public static final Angle kBlueRightSideFeedHeading = Degrees.of(160); + public static final double kPIn = 0.0003; + public static final double kIIn = 0; + public static final double kDIn = 0.0005; + public static final double kFFIn = 0.00192; - // Placeholders - public static final Angle kRedLeftSideFeedHeading = Degrees.of(-40); - public static final Angle kRedRightSideFeedHeading = Degrees.of(-160); + public static final int kDeployMotor1Id = 13; + public static final int kDeployMotor2Id = 8; + public static final int kIntakeMotorId = 7; - public static final Pose2d kRedHubAprilTag = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark) - .getTagPose(3).get().toPose2d(); - } + // 10 teeth on pinion, 20 teeth on rack + public static final Angle kDeployRotations = Rotations.of(9.5); + public static final Angle kRetractRotations = Rotations.of(0.0); + + public static final Angle kMaxExtension = Rotations.of(9.5); + public static final Angle kMinExtension = Rotations.of(0.0); + + public static final int kDeployMotorCurrentLimit = 40; + public static final int kIntakeMotorCurrentLimit = 80; + + public static final AngularVelocity kDefaultIntakeSpeed = RPM.of(-2000); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index edd68e2..6e0b03c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; @@ -22,20 +23,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.Fixtures; +import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.OIConstants; +import frc.robot.Constants.ShooterConstants; import frc.robot.commands.AimCommandFactory; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.TurretSubsystem; -import frc.robot.utils.TargetSolution; -import frc.robot.utils.UtilityFunctions; +import frc.robot.utils.*; public class RobotContainer { + private final IntakeSubsystem m_intake = new IntakeSubsystem(); + private final CommandXboxController m_driverController = new CommandXboxController( OIConstants.kDriverControllerPort); @@ -57,9 +63,11 @@ public RobotContainer() { m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName); SmartDashboard.putData("Auto Choices", m_chooser); - SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); - SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0); - SmartDashboard.putNumber("Turret angle in degrees", 0.0); + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + + // SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); + // SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0); + // SmartDashboard.putNumber("Turret angle in degrees", 0.0); // Configure the button bindings configureBindings(); @@ -83,14 +91,32 @@ private void configureBindings() { // m_driverController.a() // .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); - m_driverController.b() - .whileTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle in degrees", 0.0)), - Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)))); + // m_driverController.b() + // .whileTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle + // in degrees", 0.0)), + // Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)))); + + // m_driverController.a().whileTrue(m_aimFactory.ShootCommand()); + + // System.out.println("Bindings configured"); + // m_driverController.x().whileTrue(m_aimFactory.PointAtHub(true)); + + // m_driverController.x().whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); - m_driverController.a().whileTrue(m_aimFactory.ShootCommand()); + // m_driverController.x().whileTrue( + // m_aimFactory.Shoot(ShooterConstants.kFeedingWheelVelocity) + // .finallyDo(() -> m_aimFactory.Shoot(RPM.of(0.0)))); + + // m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); + + m_driverController.x().onTrue(DeployIntake()); + + m_driverController.a().onTrue(retractIntake()); + + m_driverController.b().whileTrue(spinIntake()); + + m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); - System.out.println("Bindings configured"); - m_driverController.x().whileTrue(m_aimFactory.PointAtHub(true)); } public Command getAutonomousCommand() { @@ -113,6 +139,37 @@ public Command feedPosition(Alliance alliance) { }, m_drive, m_turret); } + public Command spinIntake() { + return new RunCommand(() -> { + m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed); + }).finallyDo(m_intake::stopIntake); + } + + public Command retractIntake() { + return new InstantCommand(() -> { + m_intake.retractIntake(); + }); + } + + public Command outTake() { + return new RunCommand(() -> { + m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed.times(-1)); + }); + } + + public Command DeployIntake() { + return new InstantCommand(() -> { + m_intake.deployIntake(); + }); + + } + + public Command SpinIntake() { + return new InstantCommand(() -> { + m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed); + }); + } + public void teleopPeriodic() { m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading())); diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 66ef6d3..d8397e2 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -17,6 +17,7 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Seconds; @@ -40,7 +41,7 @@ import frc.robot.Constants.TurretConstants; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.StagingSubsystem; +import frc.robot.subsystems.*; import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.ShootingEntry; import frc.robot.utils.TargetSolution; @@ -54,7 +55,7 @@ public class AimCommandFactory { private boolean m_isAiming = false; - private AngularVelocity m_wheelVelocity; + private AngularVelocity m_wheelVelocity = RPM.of(60); private Translation2d m_lockedTag; @@ -134,6 +135,18 @@ public Command ShootCommand() { }, m_stager)).finallyDo(m_shooter::Stop); } + public Command RunAllStager() { + return new RunCommand(() -> { + m_stager.Agitate(); + m_stager.Feed(); + m_stager.Roll(); + }, m_stager).finallyDo(() -> { + m_stager.StopAgitate(); + m_stager.StopFeed(); + m_stager.StopRoll(); + }); + } + private void Shoot() { m_shooter.Spin(m_wheelVelocity); } @@ -159,7 +172,7 @@ public TargetSolution GetHubAimSolution() { } public Command MoveTurretToHeadingCommand(Angle heading) { - return new InstantCommand(() -> { + return new RunCommand(() -> { MoveTurretToHeading(heading); }, m_turret); } @@ -191,7 +204,8 @@ public void MoveTurretToHeading(Angle heading) { Angle robotRelativeTurretAngle = UtilityFunctions.WrapAngle(heading.minus(robotHeading)); - Angle[] currentRange = getCurrentTurretRange(); + // Angle[] currentRange = getCurrentTurretRange(); + Angle[] currentRange = TurretConstants.kUnrestrictedAngles; if (withinAngles(currentRange, robotRelativeTurretAngle)) { m_turret.moveToAngle(robotRelativeTurretAngle); @@ -209,7 +223,7 @@ public void MoveTurretToHeading(Angle heading) { Angle driveTarget = heading.minus(closest); - System.out.println(); + // System.out.println(); m_drive.moveToAngle(driveTarget); m_turret.moveToAngle(closest); } @@ -304,6 +318,10 @@ private static Angle getClosestAngle(Angle a, Angle... others) { // System.out.print(a.in(Degrees) + " is robot heading"); // System.out.println(); + if (others.length == 0) { + return null; + } + Angle closest = UtilityFunctions.WrapAngle(others[0]); double closestDistance = UtilityFunctions.angleDiff(a, closest).abs(Degrees); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 9c8832c..bff4d06 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -15,6 +15,9 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.DriverStation; @@ -268,10 +271,10 @@ public void periodic() { m_vision.periodic(); - SmartDashboard.putData(m_field); - m_pidController.periodic(); + SmartDashboard.putData(m_field); + SmartDashboard.putBoolean("Is manual rotate", m_isManualRotate); } @@ -415,9 +418,9 @@ public void zeroHeading() { * @return the robot's heading in degrees, from -180 to 180 */ public Angle getHeading() { - // return pidgey.getYaw().getValue(); + return pidgey.getYaw().getValue(); // TODO: Don't use this code - return m_poseEstimator.getEstimatedPosition().getRotation().getMeasure(); + // return m_poseEstimator.getEstimatedPosition().getRotation().getMeasure(); } public Angle getGyroHeading() { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..ec9385c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,174 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.*; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkRelativeEncoder; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import dev.doglog.DogLog; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.IntakeConstants; +import com.revrobotics.spark.SparkLimitSwitch; + +public class IntakeSubsystem extends SubsystemBase { + private final SparkMax m_intakeMotor = new SparkMax(IntakeConstants.kIntakeMotorId, MotorType.kBrushless); + private final SparkMax m_deployMotor1 = new SparkMax(IntakeConstants.kDeployMotor1Id, MotorType.kBrushless); + private final SparkMax m_deployMotor2 = new SparkMax(IntakeConstants.kDeployMotor2Id, MotorType.kBrushless); + + private final SparkClosedLoopController m_intakeClosedLoopController = m_intakeMotor.getClosedLoopController(); + private final SparkClosedLoopController m_deploy1ClosedLoopController = m_deployMotor1.getClosedLoopController(); + private final SparkClosedLoopController m_deploy2ClosedLoopController = m_deployMotor2.getClosedLoopController(); + + private final RelativeEncoder m_deploy1RelativeEncoder = m_deployMotor1.getEncoder(); + private final RelativeEncoder m_deploy2RelativeEncoder = m_deployMotor2.getEncoder(); + + private final SparkLimitSwitch m_minLimitSwitch1 = m_deployMotor1.getReverseLimitSwitch(); + private final SparkLimitSwitch m_minLimitSwitch2 = m_deployMotor2.getReverseLimitSwitch(); + + private final SparkLimitSwitch m_maxLimitSwitch1 = m_deployMotor1.getForwardLimitSwitch(); + private final SparkLimitSwitch m_maxLimitSwitch2 = m_deployMotor2.getForwardLimitSwitch(); + + NetworkTableEntry m_entry = NetworkTableInstance.getDefault().getTable("Intake").getEntry("Intake"); + + private SparkMaxConfig m_deploy1Config = new SparkMaxConfig(); + private SparkMaxConfig m_deploy2Config = new SparkMaxConfig(); + private SparkMaxConfig m_intakeConfig = new SparkMaxConfig(); + + public IntakeSubsystem() { + m_deploy1Config.closedLoop.p(IntakeConstants.kP1).i(IntakeConstants.kI1) + .d(IntakeConstants.kD1); + m_deploy2Config.closedLoop.p(IntakeConstants.kP2).i(IntakeConstants.kI2) + .d(IntakeConstants.kD2); + m_intakeConfig.closedLoop.p(IntakeConstants.kPIn).i(IntakeConstants.kIIn).d(IntakeConstants.kDIn).feedForward + .kV(IntakeConstants.kFFIn); + // Apparently there is no absolute encoder :( + // m_deployConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + m_deploy1Config.idleMode(IdleMode.kCoast); + m_deploy2Config.idleMode(IdleMode.kCoast); // TODO: change to brake + m_intakeConfig.idleMode(IdleMode.kBrake); + + m_deploy1Config.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); + m_deploy2Config.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); + m_intakeConfig.smartCurrentLimit(IntakeConstants.kIntakeMotorCurrentLimit); + + m_deploy1Config.inverted(true); + m_deployMotor1.configure(m_deploy1Config, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + m_deployMotor2.configure(m_deploy2Config, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + m_intakeMotor.configure(m_intakeConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + } + + public void spinIntake(AngularVelocity velocity) { + m_intakeClosedLoopController.setSetpoint(velocity.in(RPM), ControlType.kVelocity); + } + + public void stopIntake() { + spinIntake(RPM.of(0)); + } + + public void deployIntake() { + Angle deploy1Position = Rotations.of(m_deploy1RelativeEncoder.getPosition()); + Angle deploy2Position = Rotations.of(m_deploy2RelativeEncoder.getPosition()); + + if (deploy1Position.lt(IntakeConstants.kMaxExtension)) { + // m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kDeployRotations.in(Rotations), + // ControlType.kPosition); + + m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kDeployRotations.in(Rotations), + ControlType.kPosition); + } else { + System.out.println("Attempting to extend when beyond limit hitting limit on deploy motor 1."); + } + + if (deploy2Position.lt(IntakeConstants.kMaxExtension)) { + // m_deploy2ClosedLoopController.setSetpoint(IntakeConstants.kDeployRotations.in(Rotations), + // ControlType.kPosition); + m_deploy2ClosedLoopController.setSetpoint(IntakeConstants.kDeployRotations.in(Rotations), + ControlType.kPosition); + } else { + System.out.println("Attempting to extend beyond limit when hitting limit on deploy motor 2."); + } + } + + public void retractIntake() { + Angle deploy1Position = Rotations.of(m_deploy1RelativeEncoder.getPosition()); + Angle deploy2Position = Rotations.of(m_deploy2RelativeEncoder.getPosition()); + + if (deploy1Position.gt(IntakeConstants.kMinExtension)) { + m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kRetractRotations.in(Rotations), + ControlType.kPosition); + } else { + System.out.println("Attempting to retract beyond limit when hitting limit on deploy motor 1."); + } + + if (deploy2Position.gt(IntakeConstants.kMinExtension)) { + m_deploy2ClosedLoopController.setSetpoint(IntakeConstants.kRetractRotations.in(Rotations), + ControlType.kPosition); + } else { + System.out.println("Attempting to retract beyond limit when hitting limit on deploy motor 2."); + } + } + + @Override + public void periodic() { + if (m_minLimitSwitch1.isPressed()) { + m_deploy1RelativeEncoder.setPosition(IntakeConstants.kMinExtension.in(Rotations)); + } + + if (m_minLimitSwitch2.isPressed()) { + m_deploy2RelativeEncoder.setPosition(IntakeConstants.kMinExtension.in(Rotations)); + } + + if (m_maxLimitSwitch1.isPressed()) { + m_deploy1RelativeEncoder.setPosition(IntakeConstants.kMaxExtension.in(Rotations)); + } + + if (m_maxLimitSwitch2.isPressed()) { + m_deploy2RelativeEncoder.setPosition(IntakeConstants.kMaxExtension.in(Rotations)); + } + + // if (m_minLimitSwitch1.isPressed()) + // System.out.println("Min One"); + + // if (m_minLimitSwitch2.isPressed()) + // System.out.println("Min Two"); + + // if (m_maxLimitSwitch1.isPressed()) + // System.out.println("Max One"); + + // if (m_maxLimitSwitch2.isPressed()) + // System.out.println("Max Two"); + + DogLog.log("Deploy motor 1: canID 13 position", + m_deploy1RelativeEncoder.getPosition()); + DogLog.log("Deploy motor 2: canID 8 position", + m_deploy2RelativeEncoder.getPosition()); + + DogLog.log("Deploy motor can ID 8 setpoint", m_deploy2ClosedLoopController.getSetpoint()); + DogLog.log("Deploy motor can ID 13 setpoint", m_deploy1ClosedLoopController.getSetpoint()); + + DogLog.log("Min limit one", m_minLimitSwitch1.isPressed()); + DogLog.log("Min limit two", m_minLimitSwitch2.isPressed()); + + DogLog.log("Max limit one", m_maxLimitSwitch1.isPressed()); + DogLog.log("Max limit two", m_maxLimitSwitch2.isPressed()); + } +} diff --git a/src/main/java/frc/robot/subsystems/StagingSubsystem.java b/src/main/java/frc/robot/subsystems/StagingSubsystem.java index 8535536..3087a1f 100644 --- a/src/main/java/frc/robot/subsystems/StagingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/StagingSubsystem.java @@ -9,7 +9,7 @@ public class StagingSubsystem extends SubsystemBase { SparkMax m_feedIntoHoodMotor = new SparkMax(StagingConstants.kFeedIntoHoodMotor, MotorType.kBrushless); SparkMax m_agitationMotor = new SparkMax(StagingConstants.kAgitationMotorId, MotorType.kBrushless); - SparkMax m_rollerMotor = new SparkMax(StagingConstants.kRollerMotorId, null); + SparkMax m_rollerMotor = new SparkMax(StagingConstants.kRollerMotorId, MotorType.kBrushless); public void Agitate() { m_agitationMotor.set(StagingConstants.kAgitationSpeed); @@ -25,4 +25,16 @@ public void Feed() { public void Roll() { m_rollerMotor.set(StagingConstants.kRollerSpeed); } + + public void StopAgitate() { + m_agitationMotor.stopMotor(); + } + + public void StopFeed() { + m_feedIntoHoodMotor.stopMotor(); + } + + public void StopRoll() { + m_rollerMotor.stopMotor(); + } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 8f610f0..e7d8101 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -10,6 +10,9 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import dev.doglog.DogLog; + import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.system.plant.DCMotor; @@ -27,6 +30,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; import frc.robot.Constants.TurretConstants; import frc.robot.utils.PositionBuffer; import frc.robot.utils.TurretPosition; @@ -52,6 +56,7 @@ public class TurretSubsystem extends SubsystemBase { private MechanismLigament2d m_max1 = new MechanismLigament2d("max1", 2, 0); private MechanismLigament2d m_min2 = new MechanismLigament2d("min2", 2, 0); private MechanismLigament2d m_max2 = new MechanismLigament2d("max2", 2, 0); + private MechanismLigament2d m_robotHeading = new MechanismLigament2d("robotHeading", 2, 0); private Angle robotRotation; @@ -65,6 +70,7 @@ public TurretSubsystem() { m_config.idleMode(IdleMode.kBrake); m_config.absoluteEncoder.inverted(true); m_config.inverted(true); + m_config.smartCurrentLimit(TurretConstants.kTurretMotorAmpLimit); m_turretMotor.configure(m_config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); m_positionBuffer.pushElement(UtilityFunctions.WrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), @@ -85,22 +91,27 @@ public TurretSubsystem() { m_max2 = m_mechRoot.append(m_max2); m_max2.setColor(new Color8Bit("#FF0000")); + m_robotHeading = m_mechRoot.append(m_robotHeading); + m_robotHeading.setColor(new Color8Bit("#32CD32")); + robotRotation = Degrees.of(0); } public void moveToAngle(Angle angle) { - + angle = angle.plus(TurretConstants.kAngularDistanceToFrontOfRobot); angle = UtilityFunctions.WrapAngle(angle); + System.out.println(angle + "is commanded angle for turret"); + if (angle.gt(TurretConstants.kMaxAngle)) { System.out - .println("Angle " + angle + "is bigger than maximum angle " + - TurretConstants.kMaxAngle + "."); + .println("Angle " + angle.in(Degrees) + "is bigger than maximum angle " + + TurretConstants.kMaxAngle.in(Degrees) + "."); return; } else if (angle.lt(TurretConstants.kMinAngle)) { System.out.println( - "Angle " + angle + "is to smaller than minimum angle " + - TurretConstants.kMinAngle + "."); + "Angle " + angle.in(Degrees) + "is to smaller than minimum angle " + + TurretConstants.kMinAngle.in(Degrees) + "."); return; } @@ -110,7 +121,12 @@ public void moveToAngle(Angle angle) { } public Angle getRotation() { - return UtilityFunctions.WrapAngle(Rotations.of(m_absoluteEncoder.getPosition())); + if (Robot.isReal()) + return UtilityFunctions.WrapAngle( + Rotations.of(m_absoluteEncoder.getPosition()) + .minus(TurretConstants.kAngularDistanceToFrontOfRobot)); + + return UtilityFunctions.WrapAngle(m_targetAngle.minus(TurretConstants.kAngularDistanceToFrontOfRobot)); } public void addDriveHeading(Angle angle) { @@ -125,9 +141,14 @@ public TurretPosition getRotationAtTime(double timestamp) { @Override public void periodic() { - m_positionBuffer.pushElement(UtilityFunctions.WrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), + m_positionBuffer.pushElement( + UtilityFunctions.WrapAngle(getRotation()), RPM.of(m_absoluteEncoder.getVelocity()), TurretConstants.kEncoderReadingDelay.in(Seconds)); + + DogLog.log("Turret rotation relative to front of robot", getRotation().in(Degrees)); + DogLog.log("Turret rotation relative to turret zero", UtilityFunctions + .WrapAngle(getRotation().minus(TurretConstants.kAngularDistanceToFrontOfRobot)).in(Degrees)); } // Connected to another periodic loop that runs quicker than 0.02 seconds @@ -143,11 +164,13 @@ public LinearVelocity getMuzzleVelocityAtHoodAngle() { @Override public void simulationPeriodic() { - m_simLigament.setAngle(m_targetAngle.plus(robotRotation).in(Degrees)); + m_simLigament.setAngle( + m_targetAngle.plus(robotRotation).minus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Degrees)); m_min1.setAngle(TurretConstants.kHubMinAngle1.plus(robotRotation).in(Degrees)); m_max1.setAngle(TurretConstants.kHubMaxAngle1.plus(robotRotation).in(Degrees)); m_min2.setAngle(TurretConstants.kHubMinAngle2.plus(robotRotation).in(Degrees)); m_max2.setAngle(TurretConstants.kHubMaxAngle2.plus(robotRotation).in(Degrees)); + m_robotHeading.setAngle(robotRotation.in(Degrees)); SmartDashboard.putData("Turret Rotation", m_simMech); } diff --git a/src/main/java/frc/robot/utils/ShootingEntry.java b/src/main/java/frc/robot/utils/ShootingEntry.java index 0017da2..14b19e1 100644 --- a/src/main/java/frc/robot/utils/ShootingEntry.java +++ b/src/main/java/frc/robot/utils/ShootingEntry.java @@ -7,7 +7,7 @@ import edu.wpi.first.units.measure.Time; public record ShootingEntry(Distance distance, AngularVelocity wheelVelocity, LinearVelocity muzzleVelocity, - Distance maxHeight, - Time timeOfFlight, Angle shooterAngle) { + Distance maxHeight, + Time timeOfFlight, Angle shooterAngle) { } diff --git a/src/main/java/frc/robot/utils/TargetSolution.java b/src/main/java/frc/robot/utils/TargetSolution.java index 2b6739f..039ee78 100644 --- a/src/main/java/frc/robot/utils/TargetSolution.java +++ b/src/main/java/frc/robot/utils/TargetSolution.java @@ -6,6 +6,6 @@ // Phi is how much the aiming heading should be adjusted to properly correct for velocity public record TargetSolution(Angle hoodAngle, AngularVelocity wheelSpeed, Angle phi, Distance distance, - Angle hubAngle) { + Angle hubAngle) { } diff --git a/src/main/java/frc/robot/utils/TurretPosition.java b/src/main/java/frc/robot/utils/TurretPosition.java index 24a41dd..38253d3 100644 --- a/src/main/java/frc/robot/utils/TurretPosition.java +++ b/src/main/java/frc/robot/utils/TurretPosition.java @@ -4,5 +4,5 @@ import edu.wpi.first.units.measure.AngularVelocity; public record TurretPosition(Angle angle, AngularVelocity velocity, - double timestamp) { + double timestamp) { } diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json new file mode 100644 index 0000000..7201ed0 --- /dev/null +++ b/vendordeps/DogLog.json @@ -0,0 +1,20 @@ +{ + "javaDependencies": [ + { + "groupId": "com.github.jonahsnider", + "artifactId": "doglog", + "version": "2026.5.0" + } + ], + "fileName": "DogLog.json", + "frcYear": "2026", + "jsonUrl": "https://doglog.dev/vendordep.json", + "name": "DogLog", + "jniDependencies": [], + "mavenUrls": [ + "https://jitpack.io" + ], + "cppDependencies": [], + "version": "2026.5.0", + "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" +} \ No newline at end of file