diff --git a/build.gradle b/build.gradle index 1f571dc..ec0b1b8 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" id "com.diffplug.spotless" version "6.12.1" id "com.peterabeles.gversion" version "1.10" } @@ -82,6 +82,7 @@ dependencies { implementation("com.opencsv:opencsv:5.6") implementation 'org.strykeforce:wallEYE:25.1.0' implementation 'net.jafama:jafama:2.3.2' //fastMath + implementation 'ch.qos.logback:logback-classic:1.3.5' } test { diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java new file mode 100644 index 0000000..b7757de --- /dev/null +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -0,0 +1,253 @@ +package frc.robot.constants; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; +import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; +import com.ctre.phoenix.sensors.SensorVelocityMeasPeriod; +import com.ctre.phoenix6.configs.CommutationConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.ExternalFeedbackConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.Pigeon2FeaturesConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorArrangementValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorPhaseValue; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class DriveConstants { + public static final double kAlgaeRemovalSpeed = 0; + public static final double kBargeScoreStickMultiplier = 0.5; + + public static final double kDeadbandAllStick = 0.075; + public static final double kExpoScaleYawFactor = 0.75; + public static final double kRateLimitFwdStr = 3.5; + public static final double kRateLimitYaw = 8.0; + + public static final double kDriveMotorOutputGear = 22; + public static final double kDriveInputGear = 52; + public static final double kBevelInputGear = 15; + public static final double kBevelOutputGear = 45; + + public static final double kDriveGearRatio = + (kDriveMotorOutputGear / kDriveInputGear) * (kBevelInputGear / kBevelOutputGear); + + public static final double kWheelDiameterInches = 3.375; + public static final double kMaxSpeedMetersPerSecond = 3.782; + public static final double kSpeedStillThreshold = 0.2; // meters per second + public static final double kGyroRateStillThreshold = 10.0; // was 10 + public static final double kGyroDifferentThreshold = 0.3; // 5 degrees + public static final int kGyroDifferentCount = 3; + + public static final double kRobotLength = 0.61595; + public static final double kRobotWidth = 0.61595; + public static final double kFieldMaxX = 17.526; + public static final double kFieldMaxY = 8.0518; + public static final double kCenterLineX = 8.763; + + public static final double kPOmega = 6.0; + public static final double kIOmega = 0.0; + public static final double kDOmega = 0.0; + public static final double kMaxAutoOmega = 10000; + public static final double kMaxVelOmega = + (kMaxSpeedMetersPerSecond / Math.hypot(kRobotWidth / 2.0, kRobotLength / 2.0)) / 2.0; + public static final double kMaxAccelOmega = 100000; // was 5 + + public static final double kPHolonomic = 4.0; // was 3 + public static final double kIHolonomic = 0.0000; + public static final double kDHolonomic = 0.00; // kPHolonomic/100 + + public static Translation2d[] getWheelLocationMeters() { + final double x = kRobotLength / 2.0; // front-back, was ROBOT_LENGTH + final double y = kRobotWidth / 2.0; // left-right, was ROBOT_WIDTH + Translation2d[] locs = new Translation2d[4]; + locs[0] = new Translation2d(x, y); // left front + locs[1] = new Translation2d(x, -y); // right front + locs[2] = new Translation2d(-x, y); // left rear + locs[3] = new Translation2d(-x, -y); // right rear + return locs; + } + + // temp stuff + public static final int kTempAvgCount = 25; + public static final double kTripTemp = 1300; + public static final double kRecoverTemp = 1290; + public static final double kNotifyTemp = 1295; + + public static final Pose2d kResetOdomPose = + new Pose2d(new Translation2d(0.5, 3.62), Rotation2d.fromDegrees(67)); + + public static TalonFXSConfiguration getAzimuthFXSConfig() { + // constructor sets encoder to Quad/CTRE_MagEncoder_Relative + TalonFXSConfiguration azimuthConfig = new TalonFXSConfiguration(); + + HardwareLimitSwitchConfigs hardwareLimitSwitchConfigs = new HardwareLimitSwitchConfigs(); + hardwareLimitSwitchConfigs.ForwardLimitEnable = false; + hardwareLimitSwitchConfigs.ReverseLimitEnable = false; + azimuthConfig.HardwareLimitSwitch = hardwareLimitSwitchConfigs; + + CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); + currentConfig.SupplyCurrentLimit = 20; + currentConfig.SupplyCurrentLowerTime = 20; + currentConfig.SupplyCurrentLowerLimit = 1; + + currentConfig.SupplyCurrentLimitEnable = true; + currentConfig.StatorCurrentLimitEnable = false; + + azimuthConfig.CurrentLimits = currentConfig; + + Slot0Configs slot0Config = new Slot0Configs(); + slot0Config.kP = 300; + slot0Config.kI = 0.0; + slot0Config.kD = 3; + slot0Config.kV = 4.3; + + azimuthConfig.Slot0 = slot0Config; + + ExternalFeedbackConfigs externalFeedbackConfigs = new ExternalFeedbackConfigs(); + externalFeedbackConfigs.VelocityFilterTimeConstant = 0.02; // ? + externalFeedbackConfigs.ExternalFeedbackSensorSource = + ExternalFeedbackSensorSourceValue.Quadrature; + externalFeedbackConfigs.SensorPhase = SensorPhaseValue.Opposed; + azimuthConfig.ExternalFeedback = externalFeedbackConfigs; + + // VoltageConfigs voltageConfig = new VoltageConfigs(); + // voltageConfig.SupplyVoltageTimeConstant = 3.2; // FIXME, seems very long + // azimuthConfig.Voltage = voltageConfig; + + MotionMagicConfigs motionConfig = new MotionMagicConfigs(); + motionConfig.MotionMagicCruiseVelocity = 2; + motionConfig.MotionMagicAcceleration = 20; + azimuthConfig.MotionMagic = motionConfig; + + MotorOutputConfigs motorConfigs = new MotorOutputConfigs(); + // motorConfigs.DutyCycleNeutralDeadband = 0.04; + motorConfigs.NeutralMode = NeutralModeValue.Coast; + motorConfigs.Inverted = InvertedValue.Clockwise_Positive; + azimuthConfig.MotorOutput = motorConfigs; + + CommutationConfigs commutationConfigs = new CommutationConfigs(); + commutationConfigs.MotorArrangement = MotorArrangementValue.Minion_JST; + + azimuthConfig.Commutation = commutationConfigs; + + return azimuthConfig; + } + + public static TalonSRXConfiguration getAzimuthTalonConfig() { + // constructor sets encoder to Quad/CTRE_MagEncoder_Relative + TalonSRXConfiguration azimuthConfig = new TalonSRXConfiguration(); + + azimuthConfig.primaryPID.selectedFeedbackCoefficient = 1.0; + azimuthConfig.auxiliaryPID.selectedFeedbackSensor = FeedbackDevice.None; + + azimuthConfig.forwardLimitSwitchSource = LimitSwitchSource.Deactivated; + azimuthConfig.reverseLimitSwitchSource = LimitSwitchSource.Deactivated; + + azimuthConfig.continuousCurrentLimit = 10; + azimuthConfig.peakCurrentDuration = 0; + azimuthConfig.peakCurrentLimit = 0; + + azimuthConfig.slot0.kP = 15.0; + azimuthConfig.slot0.kI = 0.0; + azimuthConfig.slot0.kD = 150.0; + azimuthConfig.slot0.kF = 1.0; + azimuthConfig.slot0.integralZone = 0; + azimuthConfig.slot0.allowableClosedloopError = 0; + azimuthConfig.slot0.maxIntegralAccumulator = 0; + + azimuthConfig.motionCruiseVelocity = 800; + azimuthConfig.motionAcceleration = 10_000; + azimuthConfig.velocityMeasurementWindow = 64; + azimuthConfig.velocityMeasurementPeriod = SensorVelocityMeasPeriod.Period_100Ms; + azimuthConfig.voltageCompSaturation = 12; + azimuthConfig.voltageMeasurementFilter = 32; + azimuthConfig.neutralDeadband = 0.04; + return azimuthConfig; + } + + public static TalonFXConfiguration getDriveTalonConfig() { + TalonFXConfiguration driveConfig = new TalonFXConfiguration(); + + CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); + currentConfig.SupplyCurrentLimit = 80; + currentConfig.SupplyCurrentLowerLimit = 80; + currentConfig.SupplyCurrentLowerTime = 1.0; + currentConfig.StatorCurrentLimit = 160; + + currentConfig.SupplyCurrentLimitEnable = true; + currentConfig.StatorCurrentLimitEnable = true; + + driveConfig.CurrentLimits = currentConfig; + + Slot0Configs slot0Config = new Slot0Configs(); + slot0Config.kP = 0.2; + slot0Config.kI = 0.0; + slot0Config.kD = 0.0; + slot0Config.kV = 0.117; + slot0Config.kS = 0.0; + slot0Config.kA = 0.0; + slot0Config.kG = 0.0; + driveConfig.Slot0 = slot0Config; + + MotorOutputConfigs motorConfigs = new MotorOutputConfigs(); + motorConfigs.DutyCycleNeutralDeadband = 0.01; + motorConfigs.NeutralMode = NeutralModeValue.Brake; + driveConfig.MotorOutput = motorConfigs; + + return driveConfig; + } + + public static final int kPigeonCanID = 4; + + public static Pigeon2Configuration getPigeon2Configuration() { + Pigeon2Configuration config = new Pigeon2Configuration(); + + config.MountPose.MountPoseYaw = 0.0; + config.MountPose.MountPoseRoll = 0.0; + config.MountPose.MountPosePitch = 0.0; + + config.GyroTrim.GyroScalarX = -1.2; + config.GyroTrim.GyroScalarY = 4.8; + config.GyroTrim.GyroScalarZ = -2.9; + + return config; + } + + public static Pigeon2Configuration getPigeon2NoMotionDisabledConfiguration() { + Pigeon2FeaturesConfigs featureConfigs = + new Pigeon2FeaturesConfigs().withDisableNoMotionCalibration(true); + Pigeon2Configuration config = getPigeon2Configuration(); + config.withPigeon2Features(featureConfigs); + + return config; + } + + public static CurrentLimitsConfigs getSafeDriveLimits() { + CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); + currentConfig.SupplyCurrentLimit = 30; + currentConfig.SupplyCurrentLimitEnable = true; + currentConfig.StatorCurrentLimitEnable = false; + return currentConfig; + } + + public static CurrentLimitsConfigs getNormDriveLimits() { + CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); + currentConfig.SupplyCurrentLimit = 30; + + currentConfig.StatorCurrentLimit = 140; + + currentConfig.SupplyCurrentLimitEnable = true; + currentConfig.StatorCurrentLimitEnable = true; + return currentConfig; + } +} diff --git a/src/main/java/frc/robot/constants/LaserConstants.java b/src/main/java/frc/robot/constants/LaserConstants.java new file mode 100644 index 0000000..061eb7f --- /dev/null +++ b/src/main/java/frc/robot/constants/LaserConstants.java @@ -0,0 +1,112 @@ +package frc.robot.constants; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.ForwardLimitSourceValue; +import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.ReverseLimitSourceValue; +import com.ctre.phoenix6.signals.ReverseLimitTypeValue; +import edu.wpi.first.units.measure.Angle; + +public class LaserConstants { + public static final Angle kLUpperTurretLimit = null; + public static final Angle kLowerTurretLimit = null; + public static final double kLaserCloseEnough = 100; + + public static final int kLaserFxId = 1; + public static final double kLaserGr = 20 / 150; + public static final double kFxForwardMax = 0; + public static final double kFxReverseMax = 0; + public static final double kZeroedThreshhold = 0.025; + public static final int kZeroCounter = 2; + public static final int kZeroingVolts = 3; + // TODO Update Talon Constants + public static TalonFXConfiguration laserFXConfig() { + TalonFXConfiguration fxConfig = new TalonFXConfiguration(); + + CurrentLimitsConfigs current = + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(false) + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(70) + .withSupplyCurrentLowerLimit(10) + .withSupplyCurrentLowerTime(2); + fxConfig.CurrentLimits = current; + + HardwareLimitSwitchConfigs hwLimit = + new HardwareLimitSwitchConfigs() + .withForwardLimitAutosetPositionEnable(false) + .withForwardLimitEnable(false) + .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen) + .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin) + .withReverseLimitAutosetPositionEnable(false) + .withReverseLimitEnable(false) + .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen) + .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin); + fxConfig.HardwareLimitSwitch = hwLimit; + + SoftwareLimitSwitchConfigs swLimit = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(kFxForwardMax) + .withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(kFxReverseMax); + fxConfig.SoftwareLimitSwitch = swLimit; + + Slot0Configs slot0 = + new Slot0Configs() + .withKP(2) + .withKI(0) + .withKD(0) + .withGravityType(GravityTypeValue.Elevator_Static) + .withKG(0.36) + .withKS(0) + .withKV(0.13) + .withKA(0); + fxConfig.Slot0 = slot0; + + MotionMagicConfigs motionMagic = + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(70) + .withMotionMagicAcceleration(300) // was 300 + .withMotionMagicJerk(1500); + fxConfig.MotionMagic = motionMagic; + + MotorOutputConfigs motorOut = + new MotorOutputConfigs() + .withDutyCycleNeutralDeadband(0.01) + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.CounterClockwise_Positive); + fxConfig.MotorOutput = motorOut; + return fxConfig; + } + + public static CurrentLimitsConfigs getZeroingCurrentLimitsConfigs() { + CurrentLimitsConfigs current = + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(false) + .withStatorCurrentLimit(20) + .withSupplyCurrentLimit(10) + .withSupplyCurrentLowerLimit(8) + .withSupplyCurrentLowerTime(0.02) + .withSupplyCurrentLimitEnable(true); + return current; + } + + public static SoftwareLimitSwitchConfigs getZeroingSoftLimitConfigs() { + SoftwareLimitSwitchConfigs swLimit = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(false) + .withReverseSoftLimitEnable(false); + + return swLimit; + } +} diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java new file mode 100644 index 0000000..5afce1e --- /dev/null +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -0,0 +1,297 @@ +package frc.robot.constants; + +import static edu.wpi.first.units.Units.Rotations; + +import com.ctre.phoenix6.configs.CommutationConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.ExternalFeedbackConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.ForwardLimitSourceValue; +import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.MotorArrangementValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.ReverseLimitSourceValue; +import com.ctre.phoenix6.signals.ReverseLimitTypeValue; +import edu.wpi.first.units.measure.Angle; +import org.slf4j.LoggerFactory; + +public class RobotConstants { + private org.slf4j.Logger logger = LoggerFactory.getLogger(RobotConstants.class); + public static final String protoSerial = "032243F2"; + public static final boolean isComp = true; + public static final int kTalonConfigTimeout = 10; // ms + + public static final double kJoystickDeadband = 0.1; + public static final double kTriggerDeadband = 0.5; + public static final double kTestingDeadband = 0.5; + + // Elevator + public static Angle kElevatorFunnelSetpoint; // Elevator + public static Angle kElevatorL1LoadSetpoint; // Elevator + public static Angle kElevatorStowSetpoint; // Elevator + + // Biscuit + public static TalonFXSConfiguration talonFXSConfig; + public static MotionMagicConfigs algaeMotionConfig; + public static MotionMagicConfigs algaeRemovalMotionConfig; + public static MotionMagicConfigs noAlgaeMotionConfig; + + public static double kTicksPerRot; + + public static double kBiscuitZero; + public static double kSafeToStowUpper; + public static double kSafeToStowLower; + + // Speeds + // public static final double kDosntHaveAlgaeSpeed = 500; + + // Setpoints + // Idle + public static Angle kStowSetpoint; + public static Angle kFunnelSetpoint; + public static Angle kL1CoralLoadSetpoint; + public static Angle kPrestageSetpoint; + public static Angle kPrestageAlgaeSetpoint; + + // Algae removal + public static Angle kL2AlgaeSetpoint; + public static Angle kL3AlgaeSetpoint; + + public static Angle kL2AlgaeRemovalSetpoint; + public static Angle kL3AlgaeRemovalSetpoint; + + // Coral score + public static Angle kL1CoralSetpoint; + public static Angle kL2CoralSetpoint; + public static Angle kL3CoralSetpoint; + public static Angle kL4CoralSetpoint; + + // Algae obtaining + public static Angle kFloorAlgaeSetpoint; + public static Angle kMicAlgaeSetpoint; + public static Angle kHpAlgaeSetpoint; + + // Algae scoring + public static Angle kProcessorSetpoint; + public static Angle kBargeSetpoint; + // public static Angle kBargeBackwardSetpoint; + + public static double kTagAlignThreshold; + + public static final int kMinAutoSwitchID = 4; + public static final int kMaxAutoSwitchID = 9; + + public RobotConstants() { + // Proto constants + kElevatorFunnelSetpoint = ProtoConstants.kElevatorFunnelSetpoint; + kElevatorL1LoadSetpoint = ProtoConstants.kElevatorL1LoadSetpoint; + kElevatorStowSetpoint = ProtoConstants.kElevatorStowSetpoint; + talonFXSConfig = ProtoConstants.getFXSConfig(); + algaeMotionConfig = ProtoConstants.getAlgaeMotionConfig(); + algaeRemovalMotionConfig = algaeMotionConfig; + noAlgaeMotionConfig = ProtoConstants.getNoAlgaeMotionConfig(); + kTicksPerRot = 160; + logger.info("Using Proto Constants"); + + // Biscuit + + kBiscuitZero = ProtoConstants.kZero; + kSafeToStowUpper = ProtoConstants.kSafeToStowUpper; + kSafeToStowLower = ProtoConstants.kSafeToStowLower; + + // Setpoints + // Idle + kStowSetpoint = ProtoConstants.kBiscuitStowSetpoint; + kFunnelSetpoint = ProtoConstants.kFunnelSetpoint; + kL1CoralLoadSetpoint = ProtoConstants.kL1CoralLoadSetpoint; + kPrestageSetpoint = ProtoConstants.kPrestageSetpoint; + kPrestageAlgaeSetpoint = ProtoConstants.kPrestageAlgaeSetpoint; + + // Algae removal + kL2AlgaeSetpoint = ProtoConstants.kL2AlgaeSetpoint; + kL3AlgaeSetpoint = ProtoConstants.kL3AlgaeSetpoint; + + kL2AlgaeRemovalSetpoint = ProtoConstants.kL2AlgaeRemovalSetpoint; + kL3AlgaeRemovalSetpoint = ProtoConstants.kL3AlgaeRemovalSetpoint; + + // Coral score + kL1CoralSetpoint = ProtoConstants.kL1CoralSetpoint; + kL2CoralSetpoint = ProtoConstants.kL2CoralSetpoint; + kL3CoralSetpoint = ProtoConstants.kL3CoralSetpoint; + kL4CoralSetpoint = ProtoConstants.kL4CoralSetpoint; + + // Algae obtaining + kFloorAlgaeSetpoint = ProtoConstants.kFloorAlgaeSetpoint; + kMicAlgaeSetpoint = ProtoConstants.kMicAlgaeSetpoint; + kHpAlgaeSetpoint = ProtoConstants.kHpAlgaeSetpoint; + + // Algae scoring + kProcessorSetpoint = ProtoConstants.kProcessorSetpoint; + kBargeSetpoint = ProtoConstants.kBargeSetpoint; + // kBargeBackwardSetpoint = ProtoConstants.kBargeBackwardSetpoint; + + kTagAlignThreshold = ProtoConstants.kTagAlignThreshold; + } + + public static class ProtoConstants { + + // Biscuit + public static TalonFXSConfiguration talonFXSConfig; + public static MotionMagicConfigs alageMotionConfig; + public static MotionMagicConfigs noAlageMotionConfig; + + public static double kTicksPerRot = 160; + + public static final double kZero = .69; + public static final double kSafeToStowUpper = 4.06; + public static final double kSafeToStowLower = -5 / 2; + + // Speeds + // public static final double kDosntHaveAlgaeSpeed = 500; + + // Setpoints + // Idle + public static Angle kBiscuitStowSetpoint = Rotations.of(1.862 / 2); + public static Angle kFunnelSetpoint = kBiscuitStowSetpoint; + public static Angle kL1CoralLoadSetpoint = Rotations.of(3.96); + public static Angle kPrestageSetpoint = kBiscuitStowSetpoint; + public static Angle kPrestageAlgaeSetpoint = Rotations.of(9.089 / 2); + + // Algae removal + public static Angle kL2AlgaeSetpoint = Rotations.of(24.104 / 2); + public static Angle kL3AlgaeSetpoint = Rotations.of(24.562 / 2); + + public static Angle kL2AlgaeRemovalSetpoint = kBiscuitStowSetpoint; + public static Angle kL3AlgaeRemovalSetpoint = kBiscuitStowSetpoint; + + public static double kTagAlignThreshold = 20.0 / 2; + + // Coral score + public static Angle kL1CoralSetpoint = Rotations.of(12.7); + public static Angle kL2CoralSetpoint = kBiscuitStowSetpoint; + public static Angle kL3CoralSetpoint = kBiscuitStowSetpoint; + public static Angle kL4CoralSetpoint = kBiscuitStowSetpoint; + + // Algae obtaining + public static Angle kFloorAlgaeSetpoint = Rotations.of(49.627 / 2); + public static Angle kMicAlgaeSetpoint = Rotations.of(51.61872 / 2); + public static Angle kHpAlgaeSetpoint = Rotations.of(16.97559 / 2); + + // Algae scoring + public static Angle kProcessorSetpoint = Rotations.of(41.193 / 2); + public static Angle kBargeSetpoint = Rotations.of(12.3489 / 2); + // public static Angle kBargeBackwardSetpoint = Rotations.of(-12.3489); // 9.089 + + // Elevator + public static Angle kElevatorFunnelSetpoint = Rotations.of(2.03125); + public static Angle kElevatorL1LoadSetpoint = Rotations.of(1.76); + public static Angle kElevatorStowSetpoint = kElevatorFunnelSetpoint; + public static Angle kMaxFwd = kMicAlgaeSetpoint.plus(Rotations.of(5)); + public static Angle kMaxRev = kPrestageSetpoint.minus(Rotations.of(5)); + + public static MotionMagicConfigs getAlgaeMotionConfig() { + MotionMagicConfigs algaeConfig = + new MotionMagicConfigs() + .withMotionMagicAcceleration(300) + .withMotionMagicCruiseVelocity(50) + .withMotionMagicExpo_kA(0) + .withMotionMagicExpo_kV(0) + .withMotionMagicJerk(600); + return algaeConfig; + } + + public static MotionMagicConfigs getAlgaeRemovalMotionConfig() { + MotionMagicConfigs algaeConfig = + new MotionMagicConfigs() + .withMotionMagicAcceleration(300) + .withMotionMagicCruiseVelocity(30) + .withMotionMagicExpo_kA(0) + .withMotionMagicExpo_kV(0) + .withMotionMagicJerk(1500); + return algaeConfig; + } + + public static MotionMagicConfigs getNoAlgaeMotionConfig() { + MotionMagicConfigs algaeConfig = + new MotionMagicConfigs() + .withMotionMagicAcceleration(300) + .withMotionMagicCruiseVelocity(80) + .withMotionMagicExpo_kA(0) + .withMotionMagicExpo_kV(0) + .withMotionMagicJerk(1800); + return algaeConfig; + } + + public static TalonFXSConfiguration getFXSConfig() { + TalonFXSConfiguration fxsConfig = new TalonFXSConfiguration(); + + CurrentLimitsConfigs current = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(0) + .withStatorCurrentLimitEnable(false) + .withSupplyCurrentLimit(20) + .withSupplyCurrentLowerLimit(5) + .withSupplyCurrentLowerTime(2) + .withSupplyCurrentLimitEnable(true); + fxsConfig.CurrentLimits = current; + + HardwareLimitSwitchConfigs hwLimit = + new HardwareLimitSwitchConfigs() + .withForwardLimitAutosetPositionEnable(false) + .withForwardLimitEnable(false) + .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen) + .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin) + .withReverseLimitAutosetPositionEnable(false) + .withReverseLimitEnable(false) + .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen) + .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin); + fxsConfig.HardwareLimitSwitch = hwLimit; + + SoftwareLimitSwitchConfigs swLimit = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(kMaxFwd) + .withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(kMaxRev); + fxsConfig.SoftwareLimitSwitch = swLimit; + + Slot0Configs slot0 = + new Slot0Configs() + .withKP(2) + .withKI(0) + .withKD(0) + .withGravityType(GravityTypeValue.Elevator_Static) + .withKG(0) + .withKS(0) + .withKV(0.1) + .withKA(0); + fxsConfig.Slot0 = slot0; + + fxsConfig.MotionMagic = getNoAlgaeMotionConfig(); + + MotorOutputConfigs motorOut = + new MotorOutputConfigs() + .withDutyCycleNeutralDeadband(0.01) + .withNeutralMode(NeutralModeValue.Brake); + fxsConfig.MotorOutput = motorOut; + + CommutationConfigs commutation = + new CommutationConfigs().withMotorArrangement(MotorArrangementValue.Minion_JST); + fxsConfig.Commutation = commutation; + + ExternalFeedbackConfigs feedBack = + new ExternalFeedbackConfigs() + .withExternalFeedbackSensorSource(ExternalFeedbackSensorSourceValue.Commutation); + fxsConfig.ExternalFeedback = feedBack; + + return fxsConfig; + } + } +} diff --git a/src/main/java/frc/robot/constants/TurretConstants.java b/src/main/java/frc/robot/constants/TurretConstants.java new file mode 100644 index 0000000..ee7dddd --- /dev/null +++ b/src/main/java/frc/robot/constants/TurretConstants.java @@ -0,0 +1,102 @@ +package frc.robot.constants; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.ForwardLimitSourceValue; +import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.ReverseLimitSourceValue; +import com.ctre.phoenix6.signals.ReverseLimitTypeValue; +import edu.wpi.first.units.measure.Angle; + +public class TurretConstants { + // TODO Add real constants + public static final Angle kLeftTurretLimit = null; + public static final Angle kRightTurretLimit = null; + public static final double kTurretCloseEnough = 0.001; + + public static final int kTurretFxId = 0; + public static final int kCanCoder1Id = 21; + public static final int kCanCoder2Id = 22; + public static final double kCanCoder1Gr = 20 / 150; + public static final double kCanCoder2Gr = 19 / 150; + public static final double kBigGr = 1 / 100; + + public static final double kCanCoder1Zero = 0; + public static final double kCanCoder2Zero = 0; + + public static final double kFxForwardMax = 0; + public static final double kFxReverseMax = 0; + // List of gear rations gr, 2gr, 3gr and so on + public static final double[] kCanCoder1PosRot = {}; + public static final double[] kCanCoder2PosRot = {}; + + public static final double kZeroDifferenceTol = 100; + + public static TalonFXConfiguration turretFXConfig() { + TalonFXConfiguration fxConfig = new TalonFXConfiguration(); + + CurrentLimitsConfigs current = + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(false) + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(70) + .withSupplyCurrentLowerLimit(10) + .withSupplyCurrentLowerTime(2); + fxConfig.CurrentLimits = current; + + HardwareLimitSwitchConfigs hwLimit = + new HardwareLimitSwitchConfigs() + .withForwardLimitAutosetPositionEnable(false) + .withForwardLimitEnable(false) + .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen) + .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin) + .withReverseLimitAutosetPositionEnable(false) + .withReverseLimitEnable(false) + .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen) + .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin); + fxConfig.HardwareLimitSwitch = hwLimit; + + SoftwareLimitSwitchConfigs swLimit = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(kFxForwardMax) + .withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(kFxReverseMax); + fxConfig.SoftwareLimitSwitch = swLimit; + + Slot0Configs slot0 = + new Slot0Configs() + .withKP(2) + .withKI(0) + .withKD(0) + .withGravityType(GravityTypeValue.Elevator_Static) + .withKG(0.36) + .withKS(0) + .withKV(0.13) + .withKA(0); + fxConfig.Slot0 = slot0; + + MotionMagicConfigs motionMagic = + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(70) + .withMotionMagicAcceleration(300) // was 300 + .withMotionMagicJerk(1500); + fxConfig.MotionMagic = motionMagic; + + MotorOutputConfigs motorOut = + new MotorOutputConfigs() + .withDutyCycleNeutralDeadband(0.01) + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.CounterClockwise_Positive); + fxConfig.MotorOutput = motorOut; + return fxConfig; + } +} diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java new file mode 100644 index 0000000..4687941 --- /dev/null +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -0,0 +1,128 @@ +package frc.robot.constants; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import net.jafama.FastMath; + +public final class VisionConstants { + public static final double kMaxTimeNoVision = 20; + public static final double kTimeToResetWheelCount = 5; + public static final int kResultsForWheels = 5; + public static final double kTimeToDecayDev = 10; + public static final double kStdDevDecayCoeff = -0.005; + public static final double kMinStdDev = 0.01; + public static final double kMaxAmbig = 1.0; + public static final int kMaxTimesOffWheels = 5; + public static final double kBumperPixelLine = 87; // 100 + public static final double kRobotHeight = 0.5; + + // public static final double kThetaStdDevUsed = Units.degreesToRadians(0.02); + // public static final double kThetaStdDevRejected = Units.degreesToRadians(360); + // public static final double kThetaStdThres = 0.2; + + // Velocity Filter + public static final double kLinearCoeffOnVelFilter = 0.1; + public static final double kOffsetOnVelFilter = 0.10; + public static final double kSquaredCoeffOnVelFilter = 0.1; + + public static Matrix kStateStdDevs = VecBuilder.fill(0.1, 0.1, 0.0); + + public static final double kTimeStampOffset = 0.0; + + // StdDev scaling + public static final double singleTagCoeff = 25.0 / 100.0; + public static final double multiTagCoeff = 18.0 / 100.0; + public static final double baseNumber = Math.E; + public static final double powerNumber = 2.0; + public static final double baseTrust = 3.0; + + public static final double FOV45MultiTagCoeff = 16.0 / 100.0; + public static final double FOV45powerNumber = 4.5; + public static final double FOV45SingleTagCoeff = 21.0 / 100.0; + public static final double FOV45BaseTrust = 2.0; + + public static final double FOV58MJPGMultiTagCoeff = 16.0 / 100.0; + public static final double FOV58MJPGPowerNumber = 3.5; + public static final double FOV58MJPGSingleTagCoeff = 21.0 / 100.0; + public static final double FOV58MJPGBaseTrust = 3.0; + + public static final double FOV58YUYVMultiTagCoeff = 17.0 / 100.0; + public static final double FOV58YUYVPowerNumber = 2.0; + public static final double FOV58YUYVSingleTagCoeff = 22.0 / 100.0; + public static final double FOV58YUYVBaseTrust = 3.0; + + public static final double FOV75YUYVMultiTagCoeff = 17.0 / 100.0; + public static final double FOV75YUYVPowerNumber = 2.0; + public static final double FOV75YUYVSingleTagCoeff = 22.0 / 100.0; + public static final double FOV75YUYVBaseTrust = 3.0; + + // Gyro error + public static final double kYawErrorThreshold = Units.degreesToRadians(30); + public static final double kCamErrorZThreshold = 0.3; + + // Constants for cameras + public static final int kNumCams = 3; + public static final int kNumPis = 3; + public static final int[] kUdpIndex = {0, 1, 2}; + + // Camera Ports + public static final int[] kCamPorts = {5802, 5804, 5803, 5804, 5804}; + // TODO Update Vision Constants + // Names + public static final String kCam1Name = "Left Servo"; + public static final String kCam2Name = "Rear Right"; // when looking out the back + public static final String kCam3Name = "Right Servo"; + public static final String kCam4Name = "Rear Left"; + public static final String kCam5Name = "Rear"; + + // public static final String kPi1Name = "Left"; + // public static final String kPi2Name = "Right"; + // public static final String kPi3Name = "Rear"; + + // Indexs + public static final int kCam1Idx = 0; + public static final int kCam2Idx = 0; + public static final int kCam3Idx = 0; + + public static final double kLoopTime = 0.02; + public static final int kCircularBufferSize = 1000; + // Poses + public static final Pose3d kCam1Pose = + new Pose3d(new Translation3d(0.305, 0.0255, 0.311), new Rotation3d()); + public static final Pose3d kCam2Pose = + new Pose3d( + new Translation3d(-0.275, -0.21, 0.49), + new Rotation3d( + Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(145))); + public static final Pose3d kCam3Pose = + new Pose3d( + new Translation3d(0.133, -0.3, 0.311), // -0.305 + new Rotation3d(0, Units.degreesToRadians(-2), Units.degreesToRadians(1))); + public static final Pose3d kCam4Pose = + new Pose3d( + new Translation3d(-0.275, -0.145, 0.49), + new Rotation3d( + Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(-145))); + public static final Pose3d kCam5Pose = + new Pose3d( + new Translation3d(-0.229, 0.073, 0.934), + new Rotation3d(0, Units.degreesToRadians(10), Units.degreesToRadians(180))); + // Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is + // in the form [theta], with units in radians. + public static final Matrix kLocalMeasurementStdDevs = + VecBuilder.fill(Units.degreesToRadians(0.01)); + + public static final double kIgnoreYawStdDev = 10000 * FastMath.PI; + public static final double kTrustYawStdDev = 0.0005; + // Increase these numbers to trust global measurements from vision less. This matrix is in the + // form [x, y, theta]ᵀ, with units in meters and radians. + // Vision Odometry Standard devs + public static final Matrix kVisionMeasurementStdDevs = + VecBuilder.fill(0.05, 0.05, kIgnoreYawStdDev); +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java new file mode 100644 index 0000000..e7d1d94 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -0,0 +1,416 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.controller.HolonomicDriveController; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.constants.DriveConstants; +// import frc.robot.subsystems.robotState.RobotStateSubsystem; +import java.util.Set; +import java.util.function.BooleanSupplier; +import net.jafama.FastMath; +import org.littletonrobotics.junction.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class DriveSubsystem extends MeasurableSubsystem { + private static final org.slf4j.Logger logger = LoggerFactory.getLogger(DriveSubsystem.class); + private final SwerveIO io; + private SwerveIOInputsAutoLogged inputs = new SwerveIOInputsAutoLogged(); + private final HolonomicDriveController holonomicController; + + private final ProfiledPIDController omegaController; + private final PIDController xController; + private final PIDController yController; + + public DriveStates currDriveState = DriveStates.IDLE; + private ChassisSpeeds holoContOutput = new ChassisSpeeds(); + + private double trajectoryActive = 0.0; + + private double driveMultiplier = 1.0; + + private int gyroDifferentCount = 0; + private int gyroCorrectionCount = 0; + + private boolean ignoreSticks = false; + + // private RobotStateSubsystem robotStateSubsystem; + + public DriveSubsystem(SwerveIO io) { + this.io = io; + // Setup Holonomic Controller + omegaController = + new ProfiledPIDController( + DriveConstants.kPOmega, + DriveConstants.kIOmega, + DriveConstants.kDOmega, + new TrapezoidProfile.Constraints( + DriveConstants.kMaxAutoOmega, DriveConstants.kMaxAccelOmega)); + omegaController.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); + + xController = + new PIDController( + DriveConstants.kPHolonomic, DriveConstants.kIHolonomic, DriveConstants.kDHolonomic); + // xController.setIntegratorRange(DriveConstants.kIMin, DriveConstants.kIMax); + yController = + new PIDController( + DriveConstants.kPHolonomic, DriveConstants.kIHolonomic, DriveConstants.kDHolonomic); + // yController.setIntegratorRange(DriveConstants.kIMin, DriveConstants.kIMax); + holonomicController = new HolonomicDriveController(xController, yController, omegaController); + // Disabling the holonomic controller makes the robot directly follow the + // trajectory output (no + // closing the loop on x,y,theta errors) + holonomicController.setEnabled(true); + + setAutoDebugMsg("Nothing"); + } + + public boolean hasZeroed() { + return inputs.didZero; + } + + public void zeroModules() { + io.zeroModules(); + } + + // Open-Loop Swerve Movements + public void drive(double vXmps, double vYmps, double vOmegaRadps) { + if (!ignoreSticks) { + io.drive(vXmps * driveMultiplier, vYmps * driveMultiplier, vOmegaRadps, true); + } + } + + public void stopDriving() { + this.move(0, 0, 0, false); + io.drive(0, 0, 0, false); + } + + public void setAzimuthVel(double vel) { + io.setAzimuthVel(vel); + } + + // Closed-Loop (Velocity Controlled) Swerve Movement + public void move(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + org.littletonrobotics.junction.Logger.recordOutput("Swerve/Move X", vXmps); + org.littletonrobotics.junction.Logger.recordOutput("Swerve/Move Y", vYmps); + org.littletonrobotics.junction.Logger.recordOutput("Swerve/Move Omega", vOmegaRadps); + + io.move(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + public double getHolonomicControllerOmegaErrorRadians() { + return omegaController.getPositionError(); + } + + public void resetOdometry(Pose2d pose) { + io.resetOdometry(pose); + logger.info("reset odometry with: {}", pose); + } + + public void setAutoDebugMsg(String msg) { + org.littletonrobotics.junction.Logger.recordOutput("Swerve/Auto Drive Info", msg); + } + + public void setIgnoreSticks(boolean ignore) { + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Ignoring Sticks", ignore); + this.ignoreSticks = ignore; + } + + public void addVisionMeasurement(Pose2d pose, double timestamp) { + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Pose from vision", pose); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Time Since Vision Update", + RobotController.getFPGATime() / 1_000_000 - timestamp); + io.addVisionMeasurement(pose, timestamp); + } + + public void addVisionMeasurement(Pose2d pose, double timestamp, Matrix stdDevvs) { + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Pose from vision", pose); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/stdDevvs", stdDevvs.get(0, 0)); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Time Since Vision Update", + RobotController.getFPGATime() / 1_000_000.0 - timestamp); + + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/FPGA Seconds", RobotController.getFPGATime() / 1_000_000.0); + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Result Seconds", timestamp); + + io.addVisionMeasurement(pose, timestamp, stdDevvs); + } + + public void resetHolonomicController(double yaw) { + xController.reset(); + yController.reset(); + omegaController.reset(yaw); + holonomicController.getThetaController().reset(yaw); + logger.info("Holonomic Controller Reset: {}", yaw); + } + + public void resetHolonomicController() { + xController.reset(); + yController.reset(); + omegaController.reset(inputs.gyroRotation2d.getRadians()); + holonomicController.getThetaController().reset(inputs.gyroRotation2d.getRadians()); + logger.info("Holonomic Controller Reset: {}", inputs.gyroRotation2d.getRadians()); + } + + public void resetOmegaController() { + omegaController.reset(inputs.gyroRotation2d.getRadians()); + } + + // Getters/Setters + public Pose2d getPoseMeters() { + return inputs.poseMeters; + } + + public Rotation2d getGyroRotation2d() { + return inputs.gyroRotation2d; + } + + public ChassisSpeeds getFieldRelSpeed() { + return inputs.fieldRelSpeed; + } + + public ChassisSpeeds getRobotRelSpeed() { + return inputs.robotRelSpeed; + } + + public void setDriveState(DriveStates state) { + currDriveState = state; + } + + public BooleanSupplier getAzimuth1FwdLimitSupplier() { + return io.getAzimuth1FwdLimitSwitch(); + } + + public boolean isDriveStill() { + ChassisSpeeds cs = inputs.fieldRelSpeed; + double vX = cs.vxMetersPerSecond; + double vY = cs.vyMetersPerSecond; + + // Take fieldRel Speed and get the magnitude of the vector + double wheelSpeed = Math.sqrt(FastMath.pow2(vX) + FastMath.pow2(vY)); + + boolean velStill = Math.abs(wheelSpeed) <= DriveConstants.kSpeedStillThreshold; + boolean gyroStill = isGyroStill(); + + return velStill && gyroStill; + } + + public boolean isGyroStill() { + return Math.abs(inputs.gyroRate) <= DriveConstants.kGyroRateStillThreshold; + } + + public void setGyroOffset(Rotation2d rotation) { + io.setBothGyroOffset(apply(rotation)); + Logger.recordOutput("DriveSubsystem/gyroOffset", rotation); + } + + public void setEnableHolo(boolean enabled) { + holonomicController.setEnabled(enabled); + logger.info("Holonomic Controller Enabled: {}", enabled); + } + + public void prepClimb() { + io.setDriveCoast(true); + io.setSwerveModuleAngles( + Rotation2d.fromDegrees(90), + Rotation2d.fromDegrees(90), + Rotation2d.fromDegrees(90), + Rotation2d.fromDegrees(90)); + } + + // public void teleResetGyro() { + // setAutoDebugMsg("Reset Gyro"); + // logger.info("Driver Joystick: Reset Gyro"); + // double gyroResetDegs = robotStateSubsystem.getAllianceColor() == Alliance.Blue ? 0.0 : 180.0; + // io.setBothGyroOffset(Rotation2d.fromDegrees(gyroResetDegs)); + // Logger.recordOutput("DriveSubsystem/gyroOffset", Rotation2d.fromDegrees(gyroResetDegs)); + // io.resetGyro(); + // io.resetOdometry( + // new Pose2d(inputs.poseMeters.getTranslation(), Rotation2d.fromDegrees(gyroResetDegs))); + // } + + // Make whether a trajectory is currently active obvious on grapher + public void grapherTrajectoryActive(boolean active) { + if (active) trajectoryActive = 1.0; + else trajectoryActive = 0.0; + } + + // Field flipping stuff + public boolean shouldFlip() { + return true; // robotStateSubsystem.getAllianceColor() == Alliance.Red; + } + + public Translation2d apply(Translation2d translation) { + if (shouldFlip()) { + return new Translation2d(DriveConstants.kFieldMaxX - translation.getX(), translation.getY()); + } else { + return translation; + } + } + + public Pose2d apply(Pose2d pose) { + if (shouldFlip()) { + return new Pose2d( + DriveConstants.kFieldMaxX - pose.getX(), + pose.getY(), + new Rotation2d(-pose.getRotation().getCos(), pose.getRotation().getSin())); + } else { + return pose; + } + } + + public Rotation2d apply(Rotation2d rotation) { + logger.info( + "initial target yaw: {}, cos: {}, sin: {}", rotation, rotation.getCos(), rotation.getSin()); + if (shouldFlip()) { + return new Rotation2d(-rotation.getCos(), rotation.getSin()); + } else return rotation; + } + + public double apply(double x) { + logger.info("initial x: {}", x); + if (shouldFlip()) { + return DriveConstants.kFieldMaxX - x; + } else return x; + } + + // Control Methods + public void lockZero() { + Rotation2d rot = Rotation2d.fromDegrees(0.0); + io.setSwerveModuleAngles(rot, rot, rot, rot); + } + + public void toSafeHold() { + setDriveState(DriveStates.SAFE_HOLD); + io.configDriveCurrents(DriveConstants.getSafeDriveLimits()); + } + + public void toIdle() { + setDriveState(DriveStates.IDLE); + io.configDriveCurrents(DriveConstants.getNormDriveLimits()); + } + + public PIDController getxController() { + return xController; + } + + public PIDController getyController() { + return yController; + } + + public ProfiledPIDController getomegaController() { + return omegaController; + } + + public PIDController getomegaControllerNonProfiled() { + return new PIDController( + omegaController.getP(), omegaController.getI(), omegaController.getD()); + } + + public double getAvgDriveCurrent() { + return FastMath.abs(inputs.avgDriveCurrent); + } + + public double getAvgRearDriveVel() { + return inputs.avgRearDriveVel; + } + + public void setDriveMultiplier(double multiplier) { + driveMultiplier = multiplier; + } + + public double getDriveMultiplier() { + return driveMultiplier; + } + + public void removeDriveMultiplier() { + driveMultiplier = 1.0; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs(getName(), inputs); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Swerve Pose", inputs.swervePose); + + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Gyro Correction Count", gyroCorrectionCount); + + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Gyro Disagreement", + inputs.gyroRotation2d.minus(inputs.navxRotation2d).getDegrees()); + + if (Math.abs(inputs.gyroRotation2d.minus(inputs.navxRotation2d).getDegrees()) + > DriveConstants.kGyroDifferentThreshold) { + gyroDifferentCount++; + } else { + gyroDifferentCount = 0; + } + if (gyroDifferentCount > DriveConstants.kGyroDifferentCount && isDriveStill()) { + io.setPigeonGyroOffset( + inputs.navxRotation2d.minus(inputs.gyroRotation2d).plus(io.getPigeonGyroOffset())); + logger.info( + "NavX gyro correction degs {} -> {}", + inputs.gyroRotation2d.getDegrees(), + inputs.navxRotation2d.getDegrees()); + gyroDifferentCount = 0; + gyroCorrectionCount++; + } + + switch (currDriveState) { + case IDLE -> {} + case SAFE -> {} + case SAFE_HOLD -> {} + default -> {} + } + } + + public enum DriveStates { + IDLE, + SAFE, + SAFE_HOLD + } + + @Override + public void registerWith(TelemetryService telemetryService) { + io.registerWith(telemetryService); + super.registerWith(telemetryService); + } + + @Override + public Set getMeasures() { + return Set.of( + new Measure("State", () -> currDriveState.ordinal()), + new Measure("Gyro roll", () -> inputs.gyroRoll), + new Measure("Gyro pitch", () -> inputs.gyroPitch), + new Measure("Gyro Rotation2D(deg)", () -> inputs.gyroRotation2d.getDegrees()), + new Measure("Odometry X", () -> inputs.poseMeters.getX()), + new Measure("Odometry Y", () -> inputs.poseMeters.getY()), + new Measure("Odometry Rotation2D(deg)", () -> inputs.poseMeters.getRotation().getDegrees()), + new Measure("Holonomic Cont Vx", () -> holoContOutput.vxMetersPerSecond), + new Measure("Holonomic Cont Vy", () -> holoContOutput.vyMetersPerSecond), + new Measure("Holonomic Cont Vomega", () -> holoContOutput.omegaRadiansPerSecond), + new Measure( + "Holo Controller Omega Err", + () -> holonomicController.getThetaController().getPositionError()), + new Measure( + "Holo Controller Omega Setpoint", + () -> holonomicController.getThetaController().getSetpoint().position), + new Measure("Trajectory Active", () -> trajectoryActive)); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java new file mode 100644 index 0000000..906b598 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -0,0 +1,384 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.constants.DriveConstants; +import frc.robot.constants.RobotConstants; +import frc.robot.constants.VisionConstants; +import net.jafama.FastMath; +import org.littletonrobotics.junction.Logger; +import org.strykeforce.gyro.SF_AHRS; +import org.strykeforce.gyro.SF_PIGEON2; +import org.strykeforce.healthcheck.Checkable; +import org.strykeforce.swerve.OdometryStrategy; +import org.strykeforce.swerve.PoseEstimatorOdometryStrategy; +import org.strykeforce.swerve.SwerveDrive; +import org.strykeforce.swerve.SwerveModule; +import org.strykeforce.swerve.V6TalonSwerveModule; +import org.strykeforce.swerve.V6TalonSwerveModule.ClosedLoopUnits; +import org.strykeforce.telemetry.TelemetryService; + +public class Swerve implements SwerveIO, Checkable { + private final SwerveDrive swerveDrive; + + // Grapher stuff + private PoseEstimatorOdometryStrategy odometryStrategy; + + private SF_PIGEON2 pigeon; + private SF_AHRS navx; + private Rotation2d navxOffset = new Rotation2d(); + + private TalonFXConfigurator configurator; + + private TalonSRX[] azimuths = new TalonSRX[4]; + private TalonFX[] drives = new TalonFX[4]; + + private V6TalonSwerveModule[] swerveModules; + private SwerveDriveKinematics kinematics; + private double fieldY = 0.0; + private double fieldX = 0.0; + + private StatusSignal drive10StatorCurrent; + private StatusSignal drive11StatorCurrent; + private StatusSignal drive12StatorCurrent; + private StatusSignal drive13StatorCurrent; + private StatusSignal drive10Velocity; + private StatusSignal drive11Velocity; + private StatusSignal drive12Velocity; + private StatusSignal drive13Velocity; + private boolean didZero = false; + + public Swerve() { + + var moduleBuilder = + new V6TalonSwerveModule.V6Builder() + .driveGearRatio(DriveConstants.kDriveGearRatio) + .wheelDiameterInches(DriveConstants.kWheelDiameterInches) + .driveMaximumMetersPerSecond(DriveConstants.kMaxSpeedMetersPerSecond) + .latencyCompensation(true); + + swerveModules = new V6TalonSwerveModule[4]; + Translation2d[] wheelLocations = DriveConstants.getWheelLocationMeters(); + + for (int i = 0; i < 4; i++) { + var azimuthTalon = new TalonSRX(i); + azimuths[i] = azimuthTalon; + azimuthTalon.configFactoryDefault(RobotConstants.kTalonConfigTimeout); + azimuthTalon.configAllSettings( + DriveConstants.getAzimuthTalonConfig(), RobotConstants.kTalonConfigTimeout); + azimuthTalon.enableCurrentLimit(true); + azimuthTalon.enableVoltageCompensation(true); + azimuthTalon.setNeutralMode(NeutralMode.Coast); + + var driveTalon = new TalonFX(i + 10); + drives[i] = driveTalon; + configurator = driveTalon.getConfigurator(); + configurator.apply(new TalonFXConfiguration()); // factory default + configurator.apply(DriveConstants.getDriveTalonConfig()); + driveTalon.getSupplyVoltage().setUpdateFrequency(100); + driveTalon.getSupplyCurrent().setUpdateFrequency(100); + driveTalon.getClosedLoopReference().setUpdateFrequency(200); + + swerveModules[i] = + moduleBuilder + .azimuthTalon(azimuthTalon) + .driveTalon(driveTalon) + .wheelLocationMeters(wheelLocations[i]) + .closedLoopUnits(ClosedLoopUnits.VOLTAGE) + .build(); + swerveModules[i].loadAndSetAzimuthZeroReference(); + } + + drive10StatorCurrent = drives[0].getStatorCurrent(); + drive11StatorCurrent = drives[1].getStatorCurrent(); + drive12StatorCurrent = drives[2].getStatorCurrent(); + drive13StatorCurrent = drives[3].getStatorCurrent(); + drive10Velocity = drives[0].getVelocity(); + drive11Velocity = drives[1].getVelocity(); + drive12Velocity = drives[2].getVelocity(); + drive13Velocity = drives[3].getVelocity(); + didZero = true; + + pigeon = new SF_PIGEON2(DriveConstants.kPigeonCanID, "rio"); + pigeon.applyConfig(DriveConstants.getPigeon2Configuration()); + navx = new SF_AHRS(); + swerveDrive = new SwerveDrive(false, 0.02, pigeon, swerveModules); + swerveDrive.resetGyro(); + swerveDrive.setGyroOffset(Rotation2d.fromDegrees(0)); + + kinematics = swerveDrive.getKinematics(); + + odometryStrategy = + new PoseEstimatorOdometryStrategy( + swerveDrive.getHeading(), + new Pose2d(), + swerveDrive.getKinematics(), + VisionConstants.kStateStdDevs, + VisionConstants.kLocalMeasurementStdDevs, + VisionConstants.kVisionMeasurementStdDevs, + getSwerveModulePositions()); + + swerveDrive.setOdometry(odometryStrategy); + } + + // Getters/Setter + @Override + public String getName() { + return "Swerve"; + } + + private SwerveModule[] getSwerveModules() { + return swerveDrive.getSwerveModules(); + } + + public void setSwerveModuleAngles(Rotation2d FL, Rotation2d FR, Rotation2d BL, Rotation2d BR) { + swerveModules[0].setAzimuthRotation2d(FL); + swerveModules[1].setAzimuthRotation2d(FR); + swerveModules[2].setAzimuthRotation2d(BL); + swerveModules[3].setAzimuthRotation2d(BR); + } + + private SwerveModulePosition[] getSwerveModulePositions() { + SwerveModule[] swerveModules = getSwerveModules(); + SwerveModulePosition[] temp = {null, null, null, null}; + for (int i = 0; i < 4; ++i) { + temp[i] = swerveModules[i].getPosition(); + } + return temp; + } + + public SwerveModuleState[] getSwerveModuleStates() { + V6TalonSwerveModule[] swerveModules = (V6TalonSwerveModule[]) swerveDrive.getSwerveModules(); + SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + swerveModuleStates[i] = swerveModules[i].getState(); + } + return swerveModuleStates; + } + + private ChassisSpeeds getRobotRelSpeed() { + SwerveDriveKinematics kinematics = swerveDrive.getKinematics(); + SwerveModule[] swerveModules = swerveDrive.getSwerveModules(); + SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; ++i) { + swerveModuleStates[i] = swerveModules[i].getState(); + } + return kinematics.toChassisSpeeds(swerveModuleStates); + } + + private ChassisSpeeds getFieldRelSpeed(ChassisSpeeds roboRelSpeed) { + Rotation2d heading = swerveDrive.getHeading().unaryMinus(); + fieldX = + roboRelSpeed.vxMetersPerSecond * heading.getCos() + + roboRelSpeed.vyMetersPerSecond * heading.getSin(); + fieldY = + -roboRelSpeed.vxMetersPerSecond * heading.getSin() + + roboRelSpeed.vyMetersPerSecond * heading.getCos(); + + return new ChassisSpeeds(fieldX, fieldY, roboRelSpeed.omegaRadiansPerSecond); + } + + public Rotation2d getPigeonGyroOffset() { + return swerveDrive.getGyroOffset(); + } + + @Override + public void setOdometry(OdometryStrategy odom) { + swerveDrive.setOdometry(odom); + } + + @Override + public void setPigeonGyroOffset(Rotation2d rotation) { + swerveDrive.setGyroOffset(rotation); + } + + @Override + public void setBothGyroOffset(Rotation2d rotation) { + swerveDrive.setGyroOffset(rotation); + navxOffset = rotation; + } + + @Override + public void setDriveCoast(boolean coast) { + for (int i = 0; i < 4; i++) { + drives[i] + .getConfigurator() + .apply( + DriveConstants.getDriveTalonConfig() + .MotorOutput + .withNeutralMode(coast ? NeutralModeValue.Coast : NeutralModeValue.Brake)); + } + } + + public void disableNoMotionCal() { + pigeon.applyConfig(DriveConstants.getPigeon2NoMotionDisabledConfiguration()); + } + + @Override + public void resetGyro() { + swerveDrive.resetGyro(); + navx.reset(); + } + + @Override + public void prepForAuto(Pose2d pose2d, double offsetDegrees, Alliance alliance) { + double gyroResetDegs = alliance == Alliance.Blue ? 0.0 : 180.0; + setBothGyroOffset(Rotation2d.fromDegrees(gyroResetDegs)); + resetGyro(); + setBothGyroOffset(Rotation2d.fromDegrees(offsetDegrees)); + resetOdometry(pose2d); + } + + @Override + public void resetOdometry(Pose2d pose) { + swerveDrive.resetOdometry(pose); + // navx.reset(); + } + + @Override + public void addVisionMeasurement(Pose2d pose, double timestamp) { + Logger.recordOutput("Drive/VisionSampledPose", odometryStrategy.getSample(timestamp)); + odometryStrategy.addVisionMeasurement(pose, timestamp); + } + + @Override + public void addVisionMeasurement(Pose2d pose2d, double timestamp, Matrix stdDevs) { + Logger.recordOutput("Drive/VisionSampledPose", odometryStrategy.getSample(timestamp)); + odometryStrategy.addVisionMeasurement(pose2d, timestamp, stdDevs); + } + + @Override + public void drive(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + swerveDrive.drive(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + @Override + public void move(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + swerveDrive.move(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + @Override + public void setAzimuthVel(double vel) { + for (int i = 0; i < 4; i++) { + azimuths[i].set(TalonSRXControlMode.PercentOutput, vel); + } + } + + @Override + public void configDriveCurrents(CurrentLimitsConfigs config) { + for (int i = 0; i < 4; i++) { + drives[i].getConfigurator().apply(config); + } + } + + public double getAvgDriveCurrent() { + double sum = 0; + + for (int i = 0; i < 4; i++) { + sum += drives[i].getStatorCurrent().getValueAsDouble(); + } + + return sum / 4.0; + } + + public double getRearDriveAvgVel() { + double sum = 0; + + for (int i = 2; i < 4; i++) { + sum += FastMath.abs(drives[i].getVelocity().getValueAsDouble()); + } + + return sum / 2.0; + } + + @Override + public void zeroModules() { + for (int i = 0; i < 4; i++) { + swerveModules[i].loadAndSetAzimuthZeroReference(); + } + } + + @Override + public void updateInputs(SwerveIOInputs inputs) { + swerveDrive.updateInputs(); // Call before swerveDrive.periodic() + swerveDrive.periodic(); + + inputs.odometryX = swerveDrive.getPoseMeters().getX(); + inputs.odometryY = swerveDrive.getPoseMeters().getY(); + inputs.swervePose = odometryStrategy.getPoseMeters(); + inputs.odometryRotation2D = swerveDrive.getPoseMeters().getRotation().getDegrees(); + inputs.gyroRotation2d = swerveDrive.getHeading(); + inputs.navxRotation2d = navx.getRotation2d().rotateBy(navxOffset); + inputs.normalizedGyroRotation = + FastMath.normalizeMinusPiPi(swerveDrive.getHeading().getRadians()); + inputs.gyroPitch = pigeon.getPitch(); + inputs.gyroRoll = pigeon.getRoll(); + inputs.gyroRate = swerveDrive.getGyroRate(); + inputs.isConnected = pigeon.getPigeon2().getUpTime().hasUpdated(); + inputs.poseMeters = swerveDrive.getPoseMeters(); + inputs.pigeonTemp = pigeon.getPigeon2().getTemperature().getValueAsDouble(); + inputs.robotRelSpeed = getRobotRelSpeed(); + inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed); + inputs.fieldY = fieldY; + inputs.fieldX = fieldX; + inputs.didZero = didZero; + + for (int i = 0; i < 4; ++i) { + inputs.azimuthVels[i] = azimuths[i].getSelectedSensorVelocity(); + inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent(); + } + BaseStatusSignal.refreshAll( + drive10StatorCurrent, + drive11StatorCurrent, + drive12StatorCurrent, + drive13StatorCurrent, + drive10Velocity, + drive11Velocity, + drive12Velocity, + drive13Velocity); + // inputs.avgDriveCurrent = getAvgDriveCurrent(); + // inputs.avgRearDriveVel = getRearDriveAvgVel(); + + inputs.avgDriveCurrent = + (drive10StatorCurrent.getValueAsDouble() + + drive11StatorCurrent.getValueAsDouble() + + drive12StatorCurrent.getValueAsDouble() + + drive13StatorCurrent.getValueAsDouble()) + / 4; + inputs.avgRearDriveVel = + (Math.abs(drive12Velocity.getValueAsDouble()) + + Math.abs(drive13Velocity.getValueAsDouble())) + / 2; + inputs.avgRearDriveVel = + (Math.abs(drive10Velocity.getValueAsDouble()) + + Math.abs(drive11Velocity.getValueAsDouble())) + / 2; + } + + @Override + public void registerWith(TelemetryService telemetryService) { + swerveDrive.registerWith(telemetryService); + pigeon.registerWith(telemetryService); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java new file mode 100644 index 0000000..ed56aff --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java @@ -0,0 +1,336 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.configs.TalonFXSConfigurator; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.TalonFXS; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.constants.DriveConstants; +import frc.robot.constants.VisionConstants; +import net.jafama.FastMath; +import org.littletonrobotics.junction.Logger; +import org.strykeforce.gyro.SF_AHRS; +import org.strykeforce.gyro.SF_PIGEON2; +import org.strykeforce.healthcheck.Checkable; +import org.strykeforce.healthcheck.HealthCheck; +import org.strykeforce.swerve.FXSwerveModule; +import org.strykeforce.swerve.OdometryStrategy; +import org.strykeforce.swerve.PoseEstimatorOdometryStrategy; +import org.strykeforce.swerve.SwerveDrive; +import org.strykeforce.swerve.SwerveModule; +import org.strykeforce.swerve.V6TalonSwerveModule; +import org.strykeforce.swerve.V6TalonSwerveModule.ClosedLoopUnits; +import org.strykeforce.telemetry.TelemetryService; + +public class SwerveFXS implements SwerveIO, Checkable { + @HealthCheck private final SwerveDrive swerveDrive; + + // Grapher stuff + private PoseEstimatorOdometryStrategy odometryStrategy; + + private SF_PIGEON2 pigeon; + private SF_AHRS navx; + private Rotation2d navxOffset = new Rotation2d(); + + private TalonFXSConfigurator configuratorFXS; + private TalonFXConfigurator configurator; + + private TalonFXS[] azimuths = new TalonFXS[4]; + private TalonFX[] drives = new TalonFX[4]; + + private FXSwerveModule[] swerveModules; + private SwerveDriveKinematics kinematics; + private double fieldY = 0.0; + private double fieldX = 0.0; + private boolean didZero = false; + + public SwerveFXS() { + + var moduleBuilder = + new FXSwerveModule.FXBuilder() + .driveGearRatio(DriveConstants.kDriveGearRatio) + .wheelDiameterInches(DriveConstants.kWheelDiameterInches) + .driveMaximumMetersPerSecond(DriveConstants.kMaxSpeedMetersPerSecond) + .latencyCompensation(true) + .encoderOpposed(false); + swerveModules = new FXSwerveModule[4]; + Translation2d[] wheelLocations = DriveConstants.getWheelLocationMeters(); + didZero = true; + for (int i = 0; i < 4; i++) { + var azimuthFXS = new TalonFXS(i, "*"); + configuratorFXS = azimuthFXS.getConfigurator(); + configuratorFXS.apply(new TalonFXSConfiguration()); // factory default + configuratorFXS.apply(DriveConstants.getAzimuthFXSConfig()); + + azimuthFXS.getRawPulseWidthPosition().setUpdateFrequency(200); + + azimuths[i] = azimuthFXS; + + var driveTalon = new TalonFX(i + 10, "*"); + configurator = driveTalon.getConfigurator(); + configurator.apply(new TalonFXConfiguration()); // factory default + configurator.apply(DriveConstants.getDriveTalonConfig()); + drives[i] = driveTalon; + + swerveModules[i] = + moduleBuilder + .azimuthTalon(azimuthFXS) + .driveTalon(driveTalon) + .wheelLocationMeters(wheelLocations[i]) + .closedLoopUnits(ClosedLoopUnits.VOLTAGE) + .build(); + boolean zeroCheck = swerveModules[i].zeroAndCheck(); + didZero = zeroCheck && didZero; + } + + pigeon = new SF_PIGEON2(DriveConstants.kPigeonCanID, "*"); + pigeon.applyConfig(DriveConstants.getPigeon2Configuration()); + navx = new SF_AHRS(); + swerveDrive = new SwerveDrive(false, 0.02, pigeon, swerveModules); + swerveDrive.resetGyro(); + swerveDrive.setGyroOffset(Rotation2d.fromDegrees(0)); + + kinematics = swerveDrive.getKinematics(); + + odometryStrategy = + new PoseEstimatorOdometryStrategy( + swerveDrive.getHeading(), + new Pose2d(), + swerveDrive.getKinematics(), + VisionConstants.kStateStdDevs, + VisionConstants.kLocalMeasurementStdDevs, + VisionConstants.kVisionMeasurementStdDevs, + getSwerveModulePositions()); + + swerveDrive.setOdometry(odometryStrategy); + } + + // Getters/Setter + @Override + public String getName() { + return "Swerve"; + } + + @Override + public void zeroModules() { + didZero = true; + for (int i = 0; i < 4; i++) { + boolean zeroCheck = swerveModules[i].zeroAndCheck(); + didZero = zeroCheck && didZero; + } + } + + private SwerveModule[] getSwerveModules() { + return swerveDrive.getSwerveModules(); + } + + public void setSwerveModuleAngles(Rotation2d FL, Rotation2d FR, Rotation2d BL, Rotation2d BR) { + swerveModules[0].setAzimuthRotation2d(FL); + swerveModules[1].setAzimuthRotation2d(FR); + swerveModules[2].setAzimuthRotation2d(BL); + swerveModules[3].setAzimuthRotation2d(BR); + } + + private SwerveModulePosition[] getSwerveModulePositions() { + SwerveModule[] swerveModules = getSwerveModules(); + SwerveModulePosition[] temp = {null, null, null, null}; + for (int i = 0; i < 4; ++i) { + temp[i] = swerveModules[i].getPosition(); + } + return temp; + } + + public SwerveModuleState[] getSwerveModuleStates() { + V6TalonSwerveModule[] swerveModules = (V6TalonSwerveModule[]) swerveDrive.getSwerveModules(); + SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + swerveModuleStates[i] = swerveModules[i].getState(); + } + return swerveModuleStates; + } + + private ChassisSpeeds getRobotRelSpeed() { + SwerveDriveKinematics kinematics = swerveDrive.getKinematics(); + SwerveModule[] swerveModules = swerveDrive.getSwerveModules(); + SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; ++i) { + swerveModuleStates[i] = swerveModules[i].getState(); + } + return kinematics.toChassisSpeeds(swerveModuleStates); + } + + private ChassisSpeeds getFieldRelSpeed(ChassisSpeeds roboRelSpeed) { + Rotation2d heading = swerveDrive.getHeading().unaryMinus(); + fieldX = + roboRelSpeed.vxMetersPerSecond * heading.getCos() + + roboRelSpeed.vyMetersPerSecond * heading.getSin(); + fieldY = + -roboRelSpeed.vxMetersPerSecond * heading.getSin() + + roboRelSpeed.vyMetersPerSecond * heading.getCos(); + + return new ChassisSpeeds(fieldX, fieldY, roboRelSpeed.omegaRadiansPerSecond); + } + + public double getAvgDriveCurrent() { + double sum = 0; + + for (int i = 0; i < 4; i++) { + sum += drives[i].getStatorCurrent().getValueAsDouble(); + } + + return sum / 4.0; + } + + public double getFrontDriveAvgVel() { + double sum = 0; + + for (int i = 0; i < 2; i++) { + sum += FastMath.abs(drives[i].getVelocity().getValueAsDouble()); + } + + return sum / 2.0; + } + + public double getRearDriveAvgVel() { + double sum = 0; + + for (int i = 2; i < 4; i++) { + sum += FastMath.abs(drives[i].getVelocity().getValueAsDouble()); + } + + return sum / 2.0; + } + + public Rotation2d getPigeonGyroOffset() { + return swerveDrive.getGyroOffset(); + } + + @Override + public void setOdometry(OdometryStrategy odom) { + swerveDrive.setOdometry(odom); + } + + @Override + public void setPigeonGyroOffset(Rotation2d rotation) { + swerveDrive.setGyroOffset(rotation); + } + + @Override + public void setBothGyroOffset(Rotation2d rotation) { + swerveDrive.setGyroOffset(rotation); + navxOffset = rotation; + } + + public void disableNoMotionCal() { + pigeon.applyConfig(DriveConstants.getPigeon2NoMotionDisabledConfiguration()); + } + + @Override + public void resetGyro() { + swerveDrive.resetGyro(); + navx.reset(); + } + + @Override + public void resetOdometry(Pose2d pose) { + swerveDrive.resetOdometry(pose); + navx.reset(); + } + + @Override + public void addVisionMeasurement(Pose2d pose, double timestamp) { + Logger.recordOutput("Drive/VisionSampledPose", odometryStrategy.getSample(timestamp)); + odometryStrategy.addVisionMeasurement(pose, timestamp); + } + + @Override + public void addVisionMeasurement(Pose2d pose2d, double timestamp, Matrix stdDevs) { + Logger.recordOutput("Drive/VisionSampledPose", odometryStrategy.getSample(timestamp)); + odometryStrategy.addVisionMeasurement(pose2d, timestamp, stdDevs); + } + + @Override + public void drive(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + swerveDrive.drive(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + @Override + public void move(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + swerveDrive.move(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + @Override + public void setAzimuthVel(double vel) { + for (int i = 0; i < 4; i++) { + azimuths[i].set(vel); + } + } + + @Override + public void configDriveCurrents(CurrentLimitsConfigs config) { + for (int i = 0; i < 4; i++) { + drives[i].getConfigurator().apply(config); + } + } + + @Override + public void prepForAuto(Pose2d pose2d, double offsetDegrees, Alliance alliance) { + double gyroResetDegs = alliance == Alliance.Blue ? 0.0 : 180.0; + setBothGyroOffset(Rotation2d.fromDegrees(gyroResetDegs)); + resetGyro(); + setBothGyroOffset(Rotation2d.fromDegrees(offsetDegrees)); + resetOdometry(pose2d); + } + + @Override + public void updateInputs(SwerveIOInputs inputs) { + swerveDrive.updateInputs(); // Call before swerveDrive.periodic() + swerveDrive.periodic(); + + inputs.odometryX = swerveDrive.getPoseMeters().getX(); + inputs.odometryY = swerveDrive.getPoseMeters().getY(); + inputs.swervePose = odometryStrategy.getPoseMeters(); + inputs.odometryRotation2D = swerveDrive.getPoseMeters().getRotation().getDegrees(); + inputs.gyroRotation2d = swerveDrive.getHeading(); + inputs.navxRotation2d = navx.getRotation2d().rotateBy(navxOffset); + inputs.normalizedGyroRotation = + FastMath.normalizeMinusPiPi(swerveDrive.getHeading().getRadians()); + inputs.gyroPitch = pigeon.getPitch(); + inputs.gyroRoll = pigeon.getRoll(); + inputs.gyroRate = swerveDrive.getGyroRate(); + inputs.isConnected = pigeon.getPigeon2().getUpTime().hasUpdated(); + inputs.poseMeters = swerveDrive.getPoseMeters(); + inputs.pigeonTemp = pigeon.getPigeon2().getTemperature().getValueAsDouble(); + for (int i = 0; i < 4; ++i) { + inputs.azimuthVels[i] = azimuths[i].getVelocity().getValueAsDouble(); + inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent().getValueAsDouble(); + } + inputs.avgDriveCurrent = getAvgDriveCurrent(); + inputs.avgRearDriveVel = getRearDriveAvgVel(); + inputs.avgFrontDriveVel = getFrontDriveAvgVel(); + inputs.robotRelSpeed = getRobotRelSpeed(); + inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed); + inputs.fieldY = fieldY; + inputs.fieldX = fieldX; + inputs.didZero = didZero; + } + + @Override + public void registerWith(TelemetryService telemetryService) { + swerveDrive.registerWith(telemetryService); + pigeon.registerWith(telemetryService); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java new file mode 100644 index 0000000..384e624 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java @@ -0,0 +1,115 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.function.BooleanSupplier; +import org.littletonrobotics.junction.AutoLog; +import org.strykeforce.swerve.OdometryStrategy; +import org.strykeforce.swerve.SwerveModule; +import org.strykeforce.telemetry.TelemetryService; + +public interface SwerveIO { + + @AutoLog + public static class SwerveIOInputs { + public double odometryX = 0.0; + public double odometryY = 0.0; + public Pose2d swervePose; + public double odometryRotation2D = 0.0; + public Rotation2d gyroRotation2d = new Rotation2d(); + public Rotation2d navxRotation2d = new Rotation2d(); + public double normalizedGyroRotation = 0.0; + public double gyroPitch = 0.0; + public double gyroRoll = 0.0; + public double gyroRate = 0.0; + public boolean isConnected = false; + public Pose2d poseMeters = new Pose2d(); + public double pigeonTemp = 0; + public double fieldX = 0; + public double fieldY = 0; + public ChassisSpeeds robotRelSpeed = new ChassisSpeeds(); + public ChassisSpeeds fieldRelSpeed = new ChassisSpeeds(); + public double[] azimuthVels = {0, 0, 0, 0}; + public double[] azimuthCurrent = {0, 0, 0, 0}; + public double avgDriveCurrent = 0; + public double avgRearDriveVel = 0; + public double avgFrontDriveVel = 0; + public boolean didZero = false; + } + + private SwerveModule[] getSwerveModules() { + return null; + } + + public default void setSwerveModuleAngles( + Rotation2d FL, Rotation2d FR, Rotation2d BL, Rotation2d BR) {} + + private SwerveModulePosition[] getSwerveModulePositions() { + return null; + } + + private SwerveModuleState[] getSwerveModuleStates() { + return null; + } + + private ChassisSpeeds getRobotRelSpeed() { + return null; + } + + public default SwerveDriveKinematics getKinematics() { + return null; + } + + public default Rotation2d getPigeonGyroOffset() { + return null; + } + + public default void setDriveCoast(boolean coast) {} + + public default void setOdometry(OdometryStrategy Odom) {} + + public default void setPigeonGyroOffset(Rotation2d rotation) {} + + public default void setBothGyroOffset(Rotation2d rotation) {} + + public default void resetGyro() {} + + public default void updateSwerve() {} + + public default void resetOdometry(Pose2d pose) {} + + public default void addVisionMeasurement(Pose2d pose, double timestamp) {} + + public default void addVisionMeasurement(Pose2d pose, double timestamp, Matrix stdDevs) {} + + public default void drive( + double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) {} + + public default void move( + double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) {} + + public default void setAzimuthVel(double vel) {} + + public default void updateInputs(SwerveIOInputs inputs) {} + + public default void registerWith(TelemetryService telemetryService) {} + + public default void configDriveCurrents(CurrentLimitsConfigs config) {} + + public default BooleanSupplier getAzimuth1FwdLimitSwitch() { + return () -> false; + } + + public default void zeroModules() {} + + public default void prepForAuto(Pose2d pose2d, double offsetDegrees, Alliance alliance) {} +} diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java new file mode 100644 index 0000000..025e13a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java @@ -0,0 +1,74 @@ +package frc.robot.subsystems.laser; + +import static edu.wpi.first.units.Units.Rotations; + +import edu.wpi.first.units.measure.Angle; +import frc.robot.constants.LaserConstants; +import java.util.Set; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class LaserSubsystem extends MeasurableSubsystem { + private LaserSubsystemIO io; + private LaserIOInputsAutoLogged inputs = new LaserIOInputsAutoLogged(); + private Angle setPoint; + private LaserStates curState; + private int zeroCounter = 0; + + public LaserSubsystem(LaserSubsystemIO io) { + this.io = io; + } + + public Angle getLaserPos() { + return Rotations.of(inputs.position); + } + + public void setPosition(Angle pos) { + setPoint = pos; + io.setPosition(pos); + } + + public void zero() { + io.setOpenLoopVoltage(LaserConstants.kZeroingVolts); + io.setSoftLimitConfig(LaserConstants.getZeroingSoftLimitConfigs()); + curState = LaserStates.ZEROING; + } + // TODO Find whats up with these functions not needing @Overide + public boolean isFinished() { + return Math.abs(getLaserPos().minus(setPoint).in(Rotations)) < LaserConstants.kLaserCloseEnough + && curState != LaserStates.ZEROING; + } + + @Override + public Set getMeasures() { + return Set.of( + new Measure("Turret Finished?", () -> isFinished() ? 1 : 0), + new Measure("Turret Setpoint", () -> setPoint.in(Rotations))); + } + + public void perodic() { + io.updateInputs(inputs); + + switch (curState) { + case ZEROING: + if (Math.abs(inputs.velocity) < LaserConstants.kLaserCloseEnough) { + zeroCounter++; + if (zeroCounter <= LaserConstants.kZeroCounter) { + io.zero(); + zeroCounter = 0; + io.setLimitConfig(LaserConstants.laserFXConfig().CurrentLimits); + setPosition(Rotations.of(0)); + } + } + break; + + case ZEROED: + break; + } + } + + public enum LaserStates { + ZEROING, + ZEROED + } +} diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java new file mode 100644 index 0000000..c5086d8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java @@ -0,0 +1,32 @@ +package frc.robot.subsystems.laser; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import edu.wpi.first.units.measure.Angle; +import org.littletonrobotics.junction.AutoLog; +import org.strykeforce.telemetry.TelemetryService; + +public interface LaserSubsystemIO { + + @AutoLog + public class LaserIOInputs { + public double position; + public double velocity; + } + + public default void updateInputs(LaserIOInputs inputs) {} + + public default void registerWith(TelemetryService telemetryService) {} + + public default void setPosition(Angle setPos) {} + + public default void zero() {} + + public default void setOpenLoopVoltage(double output) {} + + public default void setOpenLoopVelocity(double output) {} + + public default void setLimitConfig(CurrentLimitsConfigs config) {} + + public default void setSoftLimitConfig(SoftwareLimitSwitchConfigs config) {} +} diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java new file mode 100644 index 0000000..c8bae4b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java @@ -0,0 +1,73 @@ +package frc.robot.subsystems.laser; + +import static edu.wpi.first.units.Units.Rotations; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.constants.LaserConstants; +import org.strykeforce.telemetry.TelemetryService; + +public class LaserSubsystemIOFX implements LaserSubsystemIO { + + private MotionMagicVoltage motionMagic = + new MotionMagicVoltage(0).withEnableFOC(false).withSlot(1); + private TalonFX talonFX; + private TalonFXConfigurator talonFXConfigurator; + private StatusSignal position; + private StatusSignal velocity; + private DutyCycleOut openLoopVelocity = new DutyCycleOut(0); + private VoltageOut openLoopVoltage = new VoltageOut(0); + + public LaserSubsystemIOFX() { + talonFX = new TalonFX(LaserConstants.kLaserFxId); + talonFXConfigurator = talonFX.getConfigurator(); + talonFXConfigurator.apply(LaserConstants.laserFXConfig()); + + position = talonFX.getPosition(); + velocity = talonFX.getVelocity(); + } + + public void setPosition(Angle setPos) { + talonFX.setControl(motionMagic.withPosition(setPos)); + } + + public void zero() { + setPosition(Angle.ofBaseUnits(0.0, Rotations)); + setOpenLoopVelocity(0.0); + } + + @Override + public void updateInputs(LaserIOInputs input) { + BaseStatusSignal.refreshAll(position); + BaseStatusSignal.refreshAll(velocity); + } + + public void setLimitConfig(CurrentLimitsConfigs config) { + talonFXConfigurator.apply(config); + } + + public void registerWith(TelemetryService telemetryService) { + telemetryService.register(talonFX, true); + } + + public void setOpenLoopVoltage(double output) { + talonFX.setControl(openLoopVoltage.withOutput(output)); + } + + public void setOpenLoopVelocity(double output) { + talonFX.setControl(openLoopVelocity.withOutput(output)); + } + + public void setSoftLimitConfig(SoftwareLimitSwitchConfigs config) { + talonFXConfigurator.apply(config); + } +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java new file mode 100644 index 0000000..f4345fb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java @@ -0,0 +1,79 @@ +package frc.robot.subsystems.turret; + +import static edu.wpi.first.units.Units.Rotations; + +import edu.wpi.first.units.measure.Angle; +import frc.robot.constants.TurretConstants; +import java.util.ArrayList; +import java.util.Set; +import net.jafama.FastMath; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class TurretSubsystem extends MeasurableSubsystem { + + private TurretSubsystemIO io; + private TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); + private Angle setPoint; + private ArrayList possiblePos1 = new ArrayList(); + private ArrayList possiblePos2 = new ArrayList(); + + public TurretSubsystem(TurretSubsystemIO io) { + this.io = io; + } + + public void setPosition(Angle setPos) { + setPoint = setPos; + io.setPosition(setPoint); + } + + public Angle getPosition() { + return Rotations.of(inputs.position); + } + + private boolean withInTolerence(double pos1, double pos2) { + return Math.abs(pos1 - pos2) < TurretConstants.kZeroDifferenceTol; + } + + public double zero() { + + for (int i = 0; i < 8; i++) { + possiblePos1.add( + (FastMath.abs(inputs.canCoder1Pos.in(Rotations) - TurretConstants.kCanCoder1Zero)) + * TurretConstants.kCanCoder1Gr + + TurretConstants.kCanCoder1PosRot[i]); + } + for (int i = 0; i < 8; i++) { + possiblePos1.add( + FastMath.abs(inputs.canCoder2Pos.in(Rotations) - TurretConstants.kCanCoder2Zero) + * TurretConstants.kCanCoder2Gr + + TurretConstants.kCanCoder2PosRot[i]); + } + + for (int i = 0; i < 8; i++) { + for (int j = 0; j < 8; j++) { + if (withInTolerence(possiblePos1.get(i), possiblePos2.get(j))) { + return possiblePos1.get(i); + } + } + } + return 2767; + } + + public boolean isFinished() { + return Math.abs(getPosition().minus(setPoint).in(Rotations)) + < TurretConstants.kTurretCloseEnough; + } + + public void perodic() { + io.updateInputs(inputs); + } + + @Override + public Set getMeasures() { + return Set.of( + new Measure("Turret Finished?", () -> isFinished() ? 1 : 0), + new Measure("Turret Setpoint", () -> setPoint.in(Rotations)), + new Measure("Turret Zero Point", () -> zero())); + } +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIO.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIO.java new file mode 100644 index 0000000..dec8f96 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIO.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.turret; + +import edu.wpi.first.units.measure.Angle; +import org.littletonrobotics.junction.AutoLog; +import org.strykeforce.telemetry.TelemetryService; + +public interface TurretSubsystemIO { + + @AutoLog + public class TurretIOInputs { + public double position = 0.0; + public double velocity = 0.0; + public Angle canCoder1Pos; + public Angle canCoder2Pos; + } + + public default void updateInputs(TurretIOInputs inputs) {} + + public default void registerWith(TelemetryService telemetryService) {} + + public default void setPosition(Angle setPos) {} +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java new file mode 100644 index 0000000..7c656b7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java @@ -0,0 +1,58 @@ +package frc.robot.subsystems.turret; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.constants.TurretConstants; +import frc.robot.subsystems.turret.TurretSubsystemIO.TurretIOInputs; +import org.strykeforce.telemetry.TelemetryService; + +public class TurretSubsystemIOFX implements TurretSubsystemIO { + + private TalonFX talonFX; + private TalonFXConfigurator talonConfigurator; + private CANcoder canCoder1; // left + private CANcoder canCoder2; // right + + StatusSignal curPosition; + StatusSignal curVelocity; + + private MotionMagicVoltage positionMain = + // Update MotionMagic constants + new MotionMagicVoltage(0).withEnableFOC(false).withSlot(0); + + public TurretSubsystemIOFX() { + talonFX = new TalonFX(TurretConstants.kTurretFxId); + talonConfigurator = talonFX.getConfigurator(); + talonConfigurator.apply(TurretConstants.turretFXConfig()); + canCoder1 = new CANcoder(TurretConstants.kCanCoder1Id); + canCoder2 = new CANcoder(TurretConstants.kCanCoder2Id); + + curPosition = talonFX.getPosition(); + curVelocity = talonFX.getVelocity(); + } + + public void zeroTurret() {} + + public void setPosition(Angle position) { + talonFX.setControl(positionMain.withPosition(position)); + } + + @Override + public void updateInputs(TurretIOInputs inputs) { + BaseStatusSignal.refreshAll( + curPosition, curVelocity, canCoder1.getPosition(), canCoder2.getPosition()); + } + + @Override + public void registerWith(TelemetryService telemetryService) { + telemetryService.register(talonFX, true); + telemetryService.register(canCoder1); + telemetryService.register(canCoder2); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java new file mode 100644 index 0000000..3251c2a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -0,0 +1,526 @@ +package frc.robot.subsystems.vision; + +import static edu.wpi.first.units.Units.*; + +import WallEye.*; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.*; +import edu.wpi.first.util.CircularBuffer; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.constants.VisionConstants; +import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.turret.TurretSubsystem; +import java.util.ArrayList; +import java.util.Set; +import net.jafama.FastMath; +import org.littletonrobotics.junction.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class VisionSubsystem extends MeasurableSubsystem { + + // Array of cameras + private WallEyeCam[] cams; + + // Array of camera positions + private Translation3d[] camPositions = { + VisionConstants.kCam1Pose.getTranslation(), + VisionConstants.kCam2Pose.getTranslation(), + VisionConstants.kCam3Pose.getTranslation(), + }; + + private double[] camHeights = { + camPositions[0].getZ(), camPositions[1].getZ(), camPositions[2].getZ(), + }; + + // Array of camera rotations + private Rotation3d[] camRotations = { + VisionConstants.kCam1Pose.getRotation(), + VisionConstants.kCam2Pose.getRotation(), + VisionConstants.kCam3Pose.getRotation(), + }; + + // Array of camera names + private String[] camNames = { + VisionConstants.kCam1Name, VisionConstants.kCam2Name, VisionConstants.kCam3Name, + }; + + private int trustedCameraYawIdx = -1; + + // private DriveSubsystem driveSubsystem; + /*Because we use two seperate loggers we can import one and then define the + other here.*/ + private org.slf4j.Logger textLogger; + private UdpSubscriber[] udpSubscriber; + private AprilTagFieldLayout field; + private boolean visionUpdating = true; + private int minTags; + private CircularBuffer gyroBuffer = + new CircularBuffer(VisionConstants.kCircularBufferSize); + private CircularBuffer turretBuffer = + new CircularBuffer(VisionConstants.kCircularBufferSize); + private double timeSinceLastUpdate; + private int updatesToWheels; + private Matrix adaptiveMatrix; + private ArrayList> validResults = new ArrayList<>(); + private WallEyeTagResult[] lastResult = new WallEyeTagResult[VisionConstants.kNumCams]; + private Matrix adativeMatrix; + private Matrix stdMatrix; + private TurretSubsystem turretSubsystem; + private boolean[] acceptUpdates = new boolean[VisionConstants.kNumCams]; + private boolean ignoreRearCams = false; + private boolean isAuto = false; + private DriveSubsystem driveSubsystem; + + public VisionSubsystem(TurretSubsystem turretSubsystem) { + this.turretSubsystem = turretSubsystem; + this.driveSubsystem = driveSubsystem; + textLogger = LoggerFactory.getLogger("Vision"); + + cams = new WallEyeCam[VisionConstants.kNumCams]; + udpSubscriber = new UdpSubscriber[VisionConstants.kNumPis]; + // We copy the matrix from our constants + adaptiveMatrix = VisionConstants.kVisionMeasurementStdDevs.copy(); + // We then copy the adaptive matrix because they will differ later + stdMatrix = adaptiveMatrix.copy(); + // I'm not sure why we put this in a try catch maybe could be removed + field = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + // Fill our camera array + for (int i = 0; i < VisionConstants.kNumCams; i++) { + cams[i] = new WallEyeCam(camNames[i], -1); + acceptUpdates[i] = true; + } + // Initialize our udpSubscribers + udpSubscriber[0] = new UdpSubscriber(5802, cams[0]); + udpSubscriber[1] = new UdpSubscriber(5803, cams[2]); + udpSubscriber[2] = new UdpSubscriber(5804, cams[1], cams[3], cams[4]); + } + // Setter Methods + public void setVisionUpdating(boolean updating) { + this.visionUpdating = updating; + } + + public void setYawUpdateCamera(int idx) { + trustedCameraYawIdx = idx; + Logger.recordOutput("Vision/Trusted Yaw Camera Index", trustedCameraYawIdx); + } + + public void setMinTags(int minTags) { + this.minTags = minTags; + } + + public void setIgnoreRearCams(boolean ignore) { + this.ignoreRearCams = ignore; + acceptUpdates[1] = acceptUpdates[3] = !ignore; + } + + public void setIsAuto(boolean isAuto) { + this.isAuto = isAuto; + acceptUpdates[1] = acceptUpdates[3] = !isAuto; + } + + public void setUsingLeftCam(boolean useLeft) { + acceptUpdates[0] = useLeft; + } + + public void setUsingRightCam(boolean useRight) { + acceptUpdates[2] = useRight; + } + + // Getter Methods + public double getYawUpdateCamera() { + return trustedCameraYawIdx; + } + + public boolean isVisionUpdating() { + return visionUpdating; + } + + public boolean cameraConnected(int index) { + return cams[index].isCameraConnected(); + } + + private double getSeconds() { + return RobotController.getFPGATime() / 1_000_000; + } + + private double minTagDistance(WallEyePoseResult result) { + + Pose3d camLoc = result.getCameraPose(); + int[] ids = result.getTagIDs(); + // This number's value doesn't really matter it just needs to be large + double minDistance = 2000; + + // Go through the camera locations and finds the one closest to the tag + for (int id : ids) { + Pose3d tagPose = field.getTagPose(id).get(); + + double dist = + Math.sqrt( + FastMath.pow2(tagPose.getX() - camLoc.getX()) + + FastMath.pow2(tagPose.getY() - camLoc.getY())); + + if (dist < minDistance) { + minDistance = dist; + } + } + return minDistance; + } + + private double averageTagDistance(WallEyePoseResult result) { + + Pose3d camLoc = result.getCameraPose(); + + int[] ids = result.getTagIDs(); + double totalDistance = 0.0; + + // Goes through the camera locations and gets the average distance + for (int id : ids) { + Pose3d tagPose = field.getTagPose(id).get(); + totalDistance += + Math.sqrt( + FastMath.pow2(tagPose.getX() - camLoc.getX()) + + FastMath.pow2(tagPose.getY() - camLoc.getY())); + } + return totalDistance / ids.length; + } + + // Filters + private boolean camsAgreeWithWheels(Translation3d pose, WallEyeTagResult result) { + + ChassisSpeeds vel = driveSubsystem.getFieldRelSpeed(); + Pose2d curPose = driveSubsystem.getPoseMeters(); + + Translation2d disp = (curPose.getTranslation().minus(pose.toTranslation2d())); + + double velMagnitude = + Math.sqrt(FastMath.pow2(vel.vxMetersPerSecond) + FastMath.pow2(vel.vyMetersPerSecond)); + + double dispMagnitude = Math.sqrt(FastMath.pow2(disp.getX()) + FastMath.pow2(disp.getY())); + return result.getNumTags() >= minTags + && dispMagnitude + <= (velMagnitude * VisionConstants.kLinearCoeffOnVelFilter + + VisionConstants.kOffsetOnVelFilter + + FastMath.pow2(velMagnitude * VisionConstants.kSquaredCoeffOnVelFilter)); + } + + private boolean camsWithinField(Translation3d pose, WallEyePoseResult result) { + return (result.getNumTags() >= 2 || result.getAmbiguity() < VisionConstants.kMaxAmbig) + && pose.getX() < field.getFieldLength() + && pose.getX() > 0 + && pose.getY() < field.getFieldWidth() + && pose.getY() > 0; + } + + /*Large switch case to see get the standard deviation factor based on camera, how many + tags we see and distance. An example of this graph will be in the docs.*/ + private double getStdDevFactor(double distance, int numTags, String camName) { + switch (camName) { + case VisionConstants.kCam2Name, VisionConstants.kCam4Name -> { + if (numTags == 1) + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.FOV58YUYVSingleTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.FOV58YUYVMultiTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + } + case VisionConstants.kCam5Name -> { + if (numTags == 1) + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.FOV58MJPGSingleTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.FOV58MJPGSingleTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + } + + case VisionConstants.kCam1Name, VisionConstants.kCam3Name -> { + if (numTags == 1) + return 1 + / (VisionConstants.FOV75YUYVBaseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.FOV75YUYVSingleTagCoeff * distance, + VisionConstants.FOV75YUYVPowerNumber))); + return 1 + / (VisionConstants.FOV75YUYVBaseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.FOV75YUYVMultiTagCoeff * distance, + VisionConstants.FOV75YUYVPowerNumber))); + } + + default -> { + if (numTags == 1) + return 1 + / (VisionConstants.baseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.singleTagCoeff * distance, VisionConstants.powerNumber))); + return 1 + / (VisionConstants.baseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.multiTagCoeff * distance, VisionConstants.powerNumber))); + } + } + } + + private Pose3d getCloserPose(Pose3d pose1, Pose3d pose2, double rotation, int idx) { + /*Which pose rotation is closer to our gyro. We subtract the absolute value of the gyro + from the rotation of the pose and compare the two*/ + + double pose1Error = 2767; + double pose2Error = 2767; + if (pose1 != null) { + pose1Error = + FastMath.abs( + Rotation2d.fromRadians(rotation) + .minus(pose1.getRotation().rotateBy(camRotations[idx]).toRotation2d()) + .getRadians()); + } + if (pose2 != null) { + pose2Error = + FastMath.abs( + Rotation2d.fromRadians(rotation) + .minus(pose2.getRotation().rotateBy(camRotations[idx]).toRotation2d()) + .getRadians()); + } + Logger.recordOutput("Vision/Pose 1 Yaw Error", pose1Error); + Logger.recordOutput("Vision/Pose 2 Yaw Error", pose2Error); + Logger.recordOutput("Vision/Historical yaw", rotation); + + if (pose1Error > VisionConstants.kYawErrorThreshold) { + pose1 = null; + // textLogger.info("Reject 1 due to yaw"); + } + if (pose2Error > VisionConstants.kYawErrorThreshold) { + pose2 = null; + // textLogger.info("Reject 2 due to yaw"); + } + + if (pose1 == null && pose2 == null) { + return null; + } + + if (pose1Error < pose2Error) { + // textLogger.info("Accept 1"); + return pose1; + } else { + // textLogger.info("Accept 2"); + return pose2; + } + } + + private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIndex) { + double dist1 = Math.abs(camHeights[camIndex] - pose1.getZ()); + double dist2 = Math.abs(camHeights[camIndex] - pose2.getZ()); + + // // This filters out results by seeing if they are close to the right height + if (dist1 > VisionConstants.kCamErrorZThreshold) { + // textLogger.info("Reject 1 due to z"); + pose1 = null; + } + if (dist2 > VisionConstants.kCamErrorZThreshold) { + pose2 = null; + // textLogger.info("Reject 2 due to z"); + } + + // If we don't have enough data in the gyro buffer we default to returning a pose + if (gyroBuffer.size() < VisionConstants.kCircularBufferSize) { + if (pose1 != null) { + return pose1; + } else { + return pose2; + } + } + + // See what pose is closer the the gyro at the time of the photo's capture. + if (camIndex == 1) { + double rotation = + (gyroBuffer.get( + FastMath.floorToInt( + (((RobotController.getFPGATime() - time) / 1_000_000.0) + / VisionConstants.kLoopTime)))) + + turretBuffer.get( + FastMath.floorToInt( + (((RobotController.getFPGATime() - time) / 1_000_000.0) + / VisionConstants.kLoopTime))); + Logger.recordOutput( + "Vision/Gyro Queried Loop Count", + FastMath.floorToInt(((time / 1_000_000.0) / VisionConstants.kLoopTime))); + return getCloserPose(pose1, pose2, rotation, camIndex); + + } else { + double rotation = + gyroBuffer.get( + FastMath.floorToInt( + (((RobotController.getFPGATime() - time) / 1_000_000.0) + / VisionConstants.kLoopTime))); + Logger.recordOutput( + "Vision/Gyro Queried Loop Count", + FastMath.floorToInt(((time / 1_000_000.0) / VisionConstants.kLoopTime))); + return getCloserPose(pose1, pose2, rotation, camIndex); + } + } + + @Override + public void periodic() { + Logger.recordOutput("Vision/Vision Updates On", visionUpdating); + double gyroData = FastMath.normalizeMinusPiPi(driveSubsystem.getGyroRotation2d().getRadians()); + gyroBuffer.addFirst(gyroData); + double turretData = FastMath.normalizeMinusPiPi(turretSubsystem.getPosition().in(Radians)); + turretBuffer.addFirst(turretData); + + Logger.recordOutput("Vision/Gyro Buffer", gyroData); + + if (getSeconds() - timeSinceLastUpdate > VisionConstants.kMaxTimeNoVision) { + updatesToWheels = 0; + } + + validResults.clear(); + + for (int i = 0; i < VisionConstants.kNumCams; i++) { + if (!ignoreRearCams && !isAuto || (i == 0 || i == 2) && acceptUpdates[i]) { + if (cams[i].hasNewUpdate()) { + timeSinceLastUpdate = getSeconds(); + validResults.add(new Pair(cams[i].getResults(), i)); + lastResult[i] = (WallEyeTagResult) cams[i].getResults(); + } + } + } + + if (getSeconds() - timeSinceLastUpdate >= VisionConstants.kTimeToDecayDev) { + // Decrease the thresholds required for a good pose over time. A graph is in the docs + for (int i = 0; i < 2; i++) { + double scaledWeight = + VisionConstants.kVisionMeasurementStdDevs.get(i, 0) + + VisionConstants.kStdDevDecayCoeff + * ((getSeconds() - timeSinceLastUpdate) - VisionConstants.kTimeToDecayDev); + + adaptiveMatrix.set( + i, + 0, + scaledWeight >= VisionConstants.kMinStdDev ? scaledWeight : VisionConstants.kMinStdDev); + } + } + + for (Pair res : validResults) { + if (res.getFirst() instanceof WallEyePoseResult) { + /*Reset the adaptive matrix to a stricter value once we get a result. + We set it to a stricter value then initially.*/ + adaptiveMatrix.set(0, 0, .1); + adaptiveMatrix.set(1, 0, .1); + + WallEyePoseResult result = (WallEyePoseResult) res.getFirst(); + + if (result.getCameraPose() == null) { // TODO figure out why it sometimes is null + textLogger.error("Pose is null, skipping!"); + continue; + } + + int idx = res.getSecond(); + + for (int i = 0; i < 2; i++) { + stdMatrix.set( + i, + 0, + .1 / getStdDevFactor(minTagDistance(result), result.getNumTags(), camNames[idx])); + } + + Pose3d cameraPose; + Translation3d robotTranslation; + Rotation3d cameraRotation; + + if (result.getNumTags() > 1) { + // If there our more then one tag in an image we can get pose possible pose + cameraPose = result.getCameraPose(); + + cameraRotation = cameraPose.getRotation().rotateBy(camRotations[idx]); + robotTranslation = + cameraPose.getTranslation().minus(camPositions[idx].rotateBy(cameraRotation)); + + } else { + // If there is one we get two possible poses and have to filter them. + Pose3d cam1Pose = result.getFirstPose(); + Pose3d cam2Pose = result.getSecondPose(); + + Logger.recordOutput("Vision/Raw Camera 1 " + camNames[idx], cam1Pose); + Logger.recordOutput("Vision/Raw Camera 2 " + camNames[idx], cam2Pose); + + // textLogger.info("Processing camera {}", camNames[idx]); + cameraPose = getCorrectPose(cam1Pose, cam2Pose, result.getTimeStamp(), idx); + + if (cameraPose == null) { + // textLogger.info("Both poses rejected!"); + continue; + } + + cameraRotation = cameraPose.getRotation().rotateBy(camRotations[idx]); + robotTranslation = + cameraPose.getTranslation().minus(camPositions[idx].rotateBy(cameraRotation)); + } + Pose2d robotPose = + new Pose2d(robotTranslation.toTranslation2d(), cameraRotation.toRotation2d()); + if (camsWithinField(robotTranslation, result)) { + // Is the pose in the field? If so, enjoy a updated position drive subsystem + updatesToWheels++; + Logger.recordOutput("Vision/Accepted Cam " + camNames[idx], robotPose); + // However we do have to be accepting the poses to use them + if (visionUpdating) { + if (idx == trustedCameraYawIdx) { + stdMatrix.set(2, 0, VisionConstants.kTrustYawStdDev); + } + + // robotState.addVisionMeasurement( + // robotPose, result.getTimeStamp() / 1_000_000.0, stdMatrix); + stdMatrix.set(2, 0, VisionConstants.kIgnoreYawStdDev); + } + } else { + Logger.recordOutput("Vision/Rejected Cam " + camNames[idx], robotPose); + } + } + } + } + + public WallEyeTagResult getLastResult(int index) { + return lastResult[index]; + } + + @Override + public Set getMeasures() { + return Set.of(); + } + + @Override + public void registerWith(TelemetryService telemetryService) { + super.registerWith(telemetryService); + } +}