diff --git a/.vscode/settings.json b/.vscode/settings.json index 612cdd0d..9a299ae5 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/src/main/deploy/choreo/2025-project.chor b/src/main/deploy/choreo/2025-project.chor index b24176f8..7d5c338c 100644 --- a/src/main/deploy/choreo/2025-project.chor +++ b/src/main/deploy/choreo/2025-project.chor @@ -9,22 +9,22 @@ "config":{ "frontLeft":{ "x":{ - "exp":"11 in", - "val":0.2794 + "exp":"12.125 in", + "val":0.307975 }, "y":{ - "exp":"11 in", - "val":0.2794 + "exp":"12.125 in", + "val":0.307975 } }, "backLeft":{ "x":{ - "exp":"-11 in", - "val":-0.2794 + "exp":"-12.125 in", + "val":-0.307975 }, "y":{ - "exp":"11 in", - "val":0.2794 + "exp":"12.125 in", + "val":0.307975 } }, "mass":{ @@ -40,8 +40,8 @@ "val":6.5 }, "radius":{ - "exp":"2 in", - "val":0.0508 + "exp":"1.75 in", + "val":0.04445 }, "vmax":{ "exp":"6000 RPM", @@ -52,21 +52,21 @@ "val":1.2 }, "cof":{ - "exp":"1.5", - "val":1.5 + "exp":"1.1", + "val":1.1 }, "bumper":{ "front":{ - "exp":"16 in", - "val":0.4064 + "exp":"35.875 in", + "val":0.911225 }, "side":{ - "exp":"16 in", - "val":0.4064 + "exp":"35.875 in", + "val":0.911225 }, "back":{ - "exp":"16 in", - "val":0.4064 + "exp":"35.875 in", + "val":0.911225 } }, "differentialTrackWidth":{ diff --git a/src/main/java/frc/robot/commands/auton/AutoCommandInterface.java b/src/main/java/frc/robot/commands/auton/AutoCommandInterface.java new file mode 100644 index 00000000..7a81bf6b --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/AutoCommandInterface.java @@ -0,0 +1,5 @@ +package frc.robot.commands.auton; + +public interface AutoCommandInterface { + public void reassignAlliance(); +} diff --git a/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java b/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java new file mode 100644 index 00000000..114e18d8 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java @@ -0,0 +1,106 @@ +package frc.robot.commands.drive; + +import choreo.Choreo; +import choreo.trajectory.SwerveSample; +import choreo.trajectory.Trajectory; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.auton.AutoCommandInterface; +import frc.robot.subsystems.drive.DriveSubsystem; +import java.util.Optional; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +public class DriveAutonCommand extends Command implements AutoCommandInterface { + private final DriveSubsystem driveSubsystem; + private Trajectory trajectory; + private final Timer timer = new Timer(); + private static final Logger logger = LoggerFactory.getLogger(DriveAutonCommand.class); + private boolean isTherePath = false; + private String trajectoryName; + private boolean mirrorTrajectory = false; + + private boolean resetOdometry; + private boolean lastPath; + + public DriveAutonCommand( + DriveSubsystem driveSubsystem, + String trajectoryName, + boolean lastPath, + boolean resetOdometry) { + + addRequirements(driveSubsystem); + this.driveSubsystem = driveSubsystem; + this.resetOdometry = resetOdometry; + this.lastPath = lastPath; + this.trajectoryName = trajectoryName; + Optional> tempTrajectory = Choreo.loadTrajectory(trajectoryName); + if (tempTrajectory.isPresent()) { + trajectory = tempTrajectory.get(); + isTherePath = true; + } else { + logger.error("Trajectory {} not found", trajectoryName); + isTherePath = false; + } + timer.start(); + } + + @Override + public void reassignAlliance() { + mirrorTrajectory = driveSubsystem.shouldFlip(); + } + + @Override + public void initialize() { + if (isTherePath) { + driveSubsystem.setAutoDebugMsg("Initialize " + trajectoryName); + Pose2d initialPose = new Pose2d(); + initialPose = trajectory.getInitialPose(mirrorTrajectory).get(); + if (resetOdometry) { + driveSubsystem.resetOdometry(initialPose); + } + driveSubsystem.setEnableHolo(true); + // driveSubsystem.recordAutoTrajectory(trajectory); + driveSubsystem.resetHolonomicController(); + driveSubsystem.grapherTrajectoryActive(true); + timer.reset(); + logger.info("Begin Trajectory: {}", trajectoryName); + SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get(); + driveSubsystem.calculateController(desiredState); + } + } + + @Override + public void execute() { + if (isTherePath) { + SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get(); + driveSubsystem.calculateController(desiredState); + } + } + + @Override + public boolean isFinished() { + if (!isTherePath) { + return false; + } + return timer.hasElapsed(trajectory.getTotalTime()); + } + + @Override + public void end(boolean interrupted) { + driveSubsystem.setEnableHolo(false); + // driveSubsystem.recordAutoTrajectory(null); + + if (!interrupted && !lastPath) { + driveSubsystem.calculateController( + trajectory.sampleAt(trajectory.getTotalTime(), mirrorTrajectory).get()); + } else { + driveSubsystem.drive(0, 0, 0); + } + + driveSubsystem.grapherTrajectoryActive(false); + logger.info("End Trajectory {}: {}", trajectoryName, timer.get()); + driveSubsystem.setAutoDebugMsg("End " + trajectoryName); + } +} diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index a390c4c3..f917539f 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -1,3 +1,151 @@ package frc.robot.constants; -public class DriveConstants {} +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.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Translation2d; + +public class DriveConstants { + public static final double kDriveGearRatio = 6.5; + public static final double kWheelDiameterInches = 4.0; + public static final double kMaxSpeedMetersPerSecond = 12.0; + public static final double kSpeedStillThreshold = 0.1; // meters per second + public static final double kGyroRateStillThreshold = 10.0; // 25 5 degrees per second + public static final double kGyroDifferentThreshold = 5.0; // 5 degrees + public static final int kGyroDifferentCount = 3; + + public static final double kRobotLength = 22.0; + public static final double kRobotWidth = 22.0; + public static final double kFieldMaxX = 690.0; + + public static final double kPOmega = 4.5; + public static final double kIOmega = 0.0; + public static final double kDOmega = 0.0; + public static final double kMaxVelOmega = + (kMaxSpeedMetersPerSecond / Math.hypot(kRobotWidth / 2.0, kRobotLength / 2.0)) / 2.0; + public static final double kMaxAccelOmega = 5.0; + + public static final double kPHolonomic = 3.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 TalonSRXConfiguration + getAzimuthTalonConfig() { // will be changed to a TalonFXConfiguration + // 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 = 60; + + currentConfig.StatorCurrentLimit = 140; + + currentConfig.SupplyCurrentLimitEnable = true; + currentConfig.StatorCurrentLimitEnable = true; + + driveConfig.CurrentLimits = currentConfig; + + Slot0Configs slot0Config = new Slot0Configs(); + slot0Config.kP = 0.5; // 0.16 using phoenix 6 migrate + slot0Config.kI = 0.5; // 0.0002 using phoenix 6 migrate + slot0Config.kD = 0.0; + slot0Config.kV = 0.12; // 0.047 using phoenix 6 migrate + 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 = 1000; // fix later + + public static Pigeon2Configuration getPigeon2Configuration() { + Pigeon2Configuration config = new Pigeon2Configuration(); + + config.MountPose.MountPoseYaw = -90.0; + config.MountPose.MountPoseRoll = 0.0; + config.MountPose.MountPosePitch = 0.0; + + config.GyroTrim.GyroScalarX = 0.0; + config.GyroTrim.GyroScalarY = 0.0; + config.GyroTrim.GyroScalarZ = -2.12; + + 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/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 823dd676..fe48e79c 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -1,3 +1,5 @@ package frc.robot.constants; -public class RobotConstants {} +public class RobotConstants { + public static final int kTalonConfigTimeout = 10; // ms +} diff --git a/src/main/java/frc/robot/constants/RobotStateConstants.java b/src/main/java/frc/robot/constants/RobotStateConstants.java index 5da79b20..c434fc49 100644 --- a/src/main/java/frc/robot/constants/RobotStateConstants.java +++ b/src/main/java/frc/robot/constants/RobotStateConstants.java @@ -1,3 +1,5 @@ package frc.robot.constants; -public class RobotStateConstants {} +public class RobotStateConstants { + public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0}; +} diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java new file mode 100644 index 00000000..840eea2d --- /dev/null +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -0,0 +1,5 @@ +package frc.robot.constants; + +public class TagServoingConstants { + public static final double kAngleCloseEnough = 3.0; +} 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 00000000..d1a8cfd8 --- /dev/null +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -0,0 +1,100 @@ +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; + +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 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, Units.degreesToRadians(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 = 4.0; + public static final double FOV45MultiTagCoeff = 16.0 / 100.0; + public static final double FOV45powerNumber = 4.5; + public static final double FOV45SinlgeTagCoeff = 21.0 / 100.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 FOV58YUYVMultiTagCoeff = 17.0 / 100.0; + public static final double FOV58YUYVPowerNumber = 4.0; + public static final double FOV58YUYVSingleTagCoeff = 22.0 / 100.0; + + // Constants for cameras + public static final int kNumCams = 4; + + // Names + public static final String kCam1Name = "Shooter"; + public static final String kCam2Name = "Intake"; + public static final String kCam3Name = "AngledShooterLeft"; + public static final String kCam4Name = "AngledShooterRight"; + + public static final String kPi1Name = "Shooter"; + public static final String kPi2Name = "Intake"; + public static final String kPi3Name = "AngledShooters"; + + // Indexs + public static final int kCam1Idx = 0; + public static final int kCam2Idx = 0; + public static final int kCam3Idx = 0; + public static final int kCam4Idx = 1; + + public static final double kLoopTime = 0.02; + public static final int kCircularBufferSize = 1000; + // Poses + public static final Pose3d kCam1Pose = + new Pose3d( + new Translation3d(-0.27, 0.055, 0.20), + new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(180.0))); + public static final Pose3d kCam2Pose = + new Pose3d( + new Translation3d(-0.21, -0.31, 0.44), + new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(0.0))); + public static final Pose3d kCam3Pose = + new Pose3d( + new Translation3d(-0.23, 0.33, 0.56), + new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(-132.0))); + public static final Pose3d kCam4Pose = + new Pose3d( + new Translation3d(-0.22, -0.335, 0.50), + new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0))); + + // 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 Matrix kLocalMeasurementStdDevs = + VecBuilder.fill(Units.degreesToRadians(0.01)); + + // 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 Matrix kVisionMeasurementStdDevs = + VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)); +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 79728e76..79282da4 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -1,3 +1,384 @@ package frc.robot.subsystems.drive; -public class DriveSubsystem {} +import choreo.trajectory.SwerveSample; +import choreo.trajectory.Trajectory; +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.trajectory.TrapezoidProfile; +import frc.robot.constants.DriveConstants; +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.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 SwerveSample holoContInput = + new SwerveSample( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, new double[4], new double[4]); + private Trajectory autoTrajectory; + private double trajectoryActive = 0.0; + + private int gyroDifferentCount = 0; + + public DriveSubsystem(SwerveIO io) { + org.littletonrobotics.junction.Logger.recordOutput("Swerve/YVelSpeed", 0.0); + org.littletonrobotics.junction.Logger.recordOutput("Swerve/UsingDeadEye", false); + org.littletonrobotics.junction.Logger.recordOutput("Swerve/Auto Drive Info", "Nothing"); + + this.io = io; + // Setup Holonomic Controller + omegaController = + new ProfiledPIDController( + DriveConstants.kPOmega, + DriveConstants.kIOmega, + DriveConstants.kDOmega, + new TrapezoidProfile.Constraints( + DriveConstants.kMaxVelOmega, 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); + } + + // Open-Loop Swerve Movements + public void drive(double vXmps, double vYmps, double vOmegaRadps) { + io.drive(vXmps, vYmps, vOmegaRadps, true); + } + + 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 void recordYVel(double val) { + org.littletonrobotics.junction.Logger.recordOutput("Swerve/YVelSpeed", val); + } + + // Choreo Holonomic Controller + public void calculateController(SwerveSample desiredState) { + holoContInput = desiredState; + double xFF = desiredState.vx; + double yFF = desiredState.vy; + double rotationFF = desiredState.heading; + + Pose2d pose = inputs.poseMeters; + double xFeedback = xController.calculate(pose.getX(), desiredState.x); + double yFeedback = yController.calculate(pose.getY(), desiredState.y); + double rotationFeedback = + omegaController.calculate(pose.getRotation().getRadians(), desiredState.heading); + + holoContOutput.vxMetersPerSecond = xFF + xFeedback; + holoContOutput.vyMetersPerSecond = yFF + yFeedback; + holoContOutput.omegaRadiansPerSecond = rotationFF + rotationFeedback; + + io.move( + holoContOutput.vxMetersPerSecond, + holoContOutput.vyMetersPerSecond, + holoContOutput.omegaRadiansPerSecond, + false); + } + + // Choreo Holonomic Controller + public void calculateControllerServo(SwerveSample desiredState, double desiredRobotRelvY) { + holoContInput = desiredState; + double xFF = desiredState.vx; + double rotationFF = desiredState.heading; + + Pose2d pose = inputs.poseMeters; + double xFeedback = xController.calculate(pose.getX(), desiredState.x); + double rotationFeedback = + omegaController.calculate(pose.getRotation().getRadians(), desiredState.heading); + + holoContOutput.vxMetersPerSecond = xFF + xFeedback; + holoContOutput.vyMetersPerSecond = desiredRobotRelvY; + holoContOutput.omegaRadiansPerSecond = rotationFF + rotationFeedback; + + io.move( + holoContOutput.vxMetersPerSecond, + holoContOutput.vyMetersPerSecond, + holoContOutput.omegaRadiansPerSecond, + false); + } + + public void resetOdometry(Pose2d pose) { + io.resetOdometry(pose); + logger.info("reset odometry with: {}", pose); + } + + public void recordAutoTrajectory(Trajectory traj) { + autoTrajectory = traj; + } + + public void setAutoDebugMsg(String msg) { + org.littletonrobotics.junction.Logger.recordOutput("Swerve/Auto Drive Info", msg); + } + + public Trajectory getAutoTrajectory() { + if (autoTrajectory != null) { + return autoTrajectory; + } else { + return null; + } + } + + 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 void setDriveState(DriveStates state) { + currDriveState = state; + } + + public BooleanSupplier getAzimuth1FwdLimitSupplier() { + return io.getAzimuth1FwdLimitSwitch(); + } + + public boolean isDriveStill() { + // logger.info( + // "Timestamp Before FieldRel: {}", + // org.littletonrobotics.junction.Logger.getRealTimestamp() / 1000); + ChassisSpeeds cs = inputs.fieldRelSpeed; + double vX = cs.vxMetersPerSecond; + double vY = cs.vyMetersPerSecond; + // logger.info( + // "Timestamp After FieldRel: {}", + // org.littletonrobotics.junction.Logger.getRealTimestamp() / 1000); + + // Take fieldRel Speed and get the magnitude of the vector + double wheelSpeed = FastMath.hypot(vX, 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)); + } + + public void setEnableHolo(boolean enabled) { + holonomicController.setEnabled(enabled); + logger.info("Holonomic Controller Enabled: {}", enabled); + } + + public void teleResetGyro() { + logger.info("Driver Joystick: Reset Gyro"); + // double gyroResetDegs = robotStateSubsystem.getAllianceColor() == Alliance.Blue ? 0.0 : 180.0; + double gyroResetDegs = 0.0; // TODO change this to the above once we have RobotStateSubsystem + io.setBothGyroOffset(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 robotStateSubsystem.getAllianceColor() == Alliance.Red; + return true; // TODO change this to the above once we have RobotStateSubsystem + } + + 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 void periodic() { + io.updateInputs(inputs); + Logger.processInputs(getName(), inputs); + if (Math.abs(inputs.gyroRotation2d.getDegrees() - inputs.navxRotation2d.getDegrees()) + > DriveConstants.kGyroDifferentThreshold) { + gyroDifferentCount++; + } else { + gyroDifferentCount = 0; + } + if (gyroDifferentCount > DriveConstants.kGyroDifferentCount && isDriveStill()) { + io.setPigeonGyroOffset(inputs.navxRotation2d); + gyroDifferentCount = 0; + } + + switch (currDriveState) { + case IDLE: + break; + case SAFE: + break; + case SAFE_HOLD: + break; + default: + break; + } + } + + public enum DriveStates { + IDLE, + SAFE, + SAFE_HOLD + } + + @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("Trajectory Vel X", () -> holoContInput.vx), + new Measure("Trajectory Vel Y", () -> holoContInput.vy), + new Measure("Trajectory Accel X", () -> holoContInput.ax), + new Measure("Trajectory Accel Y", () -> holoContInput.ay), + new Measure("Trajectory X", () -> holoContInput.x), + new Measure("Trajectory Y", () -> holoContInput.y), + new Measure("Trajectory Rotation2D(deg)", () -> holoContInput.heading), + 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 index bd271064..a68aa4ea 100644 --- a/src/main/java/frc/robot/subsystems/drive/Swerve.java +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -1,3 +1,271 @@ package frc.robot.subsystems.drive; -public class Swerve {} +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +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 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 frc.robot.constants.DriveConstants; +import frc.robot.constants.RobotConstants; +import frc.robot.constants.VisionConstants; +import java.util.function.BooleanSupplier; +import net.jafama.FastMath; +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.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 { + @HealthCheck 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 BooleanSupplier azimuth1FwdLimitSupplier = () -> false; + + 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; + + 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(); + } + + 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"; + } + + 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() { + // 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(); + } + ChassisSpeeds roboRelSpeed = kinematics.toChassisSpeeds(swerveModuleStates); + + 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 void setOdometry(OdometryStrategy Odom) { + swerveDrive.setOdometry(Odom); + } + + public void setPigeonGyroOffset(Rotation2d rotation) { + swerveDrive.setGyroOffset(rotation); + } + + public void setBothGyroOffset(Rotation2d rotation) { + swerveDrive.setGyroOffset(rotation); + navxOffset = rotation; + } + + public void resetGyro() { + swerveDrive.resetGyro(); + navx.reset(); + } + + public void updateSwerve() { + swerveDrive.periodic(); + } + + public void resetOdometry(Pose2d pose) { + swerveDrive.resetOdometry(pose); + navx.reset(); + } + + public void addVisionMeasurement(Pose2d pose, double timestamp) { + odometryStrategy.addVisionMeasurement(pose, timestamp); + } + + public void addVisionMeasurement(Pose2d pose2d, double timestamp, Matrix stdDevs) { + odometryStrategy.addVisionMeasurement(pose2d, timestamp, stdDevs); + } + + public void drive(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + swerveDrive.drive(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + public void move(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + swerveDrive.move(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + public void setAzimuthVel(double vel) { + for (int i = 0; i < 4; i++) { + azimuths[i].set(TalonSRXControlMode.PercentOutput, vel); + } + } + + public void configDriveCurrents(CurrentLimitsConfigs config) { + for (int i = 0; i < 4; i++) { + drives[i].getConfigurator().apply(config); + } + } + + @Override + public void updateInputs(SwerveIOInputs inputs) { + inputs.odometryX = swerveDrive.getPoseMeters().getX(); + inputs.odometryY = swerveDrive.getPoseMeters().getY(); + inputs.odometryRotation2D = swerveDrive.getPoseMeters().getRotation().getDegrees(); + inputs.gyroRotation2d = swerveDrive.getHeading(); + inputs.navxRotation2d = navx.getRotation2d().rotateBy(navxOffset); + inputs.normalizedGyroRotation = + FastMath.toDegrees(FastMath.normalizeZeroTwoPi(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].getSelectedSensorVelocity(); + inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent(); + } + inputs.fieldRelSpeed = getFieldRelSpeed(); + inputs.fieldY = fieldY; + inputs.fieldX = fieldX; + } + + @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 index 66439ffa..e4cef1fe 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java @@ -1,3 +1,98 @@ package frc.robot.subsystems.drive; -public class SwerveIO {} +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 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 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 fieldRelSpeed = new ChassisSpeeds(); + public double[] azimuthVels = {0, 0, 0, 0}; + public double[] azimuthCurrent = {0, 0, 0, 0}; + } + + 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 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; + } +} diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index dda12674..1721e1ac 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -1,3 +1,262 @@ package frc.robot.subsystems.pathHandler; -public class PathHandler {} +import choreo.Choreo; +import choreo.trajectory.SwerveSample; +import choreo.trajectory.Trajectory; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.RobotStateConstants; +import frc.robot.constants.TagServoingConstants; +import frc.robot.subsystems.drive.DriveSubsystem; +import java.util.List; +import java.util.Optional; +import java.util.Set; +import net.jafama.FastMath; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class PathHandler extends MeasurableSubsystem { + DriveSubsystem driveSubsystem; + + private PathStates currState = PathStates.DONE; + private boolean isHandling = false; + private List NodeNames; + private List NodeLevels; + private Timer timer = new Timer(); + private List> fetchPaths; + private List> placePaths; + private String[][] pathNames = new String[12][2]; + private Trajectory currPath; + private String currPathString; + private boolean runningPath = false; + private boolean isServoing = false; + private boolean mirrorTrajectory = false; + private Character startNode = 'a'; + + PathHandler(DriveSubsystem driveSubsystem) { + this.driveSubsystem = driveSubsystem; + reassignAlliance(); + } + + PathHandler( + DriveSubsystem driveSubsystem, + String[][] pathNames, + List NodeNames, + List NodeLevels, + Character startNode) { + this.driveSubsystem = driveSubsystem; + this.pathNames = pathNames; + this.NodeNames = NodeNames; + this.NodeLevels = NodeLevels; + this.startNode = startNode; + } + + public void setPathNames(String[][] pathNames) { + if (!isHandling) { + this.pathNames = pathNames; + } + } + + public void setNodeNames(List NodeNames) { + if (!isHandling) { + this.NodeNames = NodeNames; + } + } + + public void setNodeLevels(List NodeLevels) { + if (!isHandling) { + this.NodeLevels = NodeLevels; + } + } + + public void setStartNode(Character startNode) { + if (!isHandling) { + this.startNode = startNode; + } + } + + public void startPathHandler() { + NodeNames.add(0, startNode); + isHandling = true; + currState = PathStates.DRIVE_FETCH; + } + + public void reassignAlliance() { + mirrorTrajectory = driveSubsystem.shouldFlip(); + for (int i = 0; i < 12; i++) { + Optional> temp = Choreo.loadTrajectory(pathNames[i][0]); + fetchPaths.add(temp.get()); + temp = Choreo.loadTrajectory(pathNames[i][1]); + placePaths.add(temp.get()); + } + } + + private void startPath(Trajectory path) { + if (isHandling && path != null) { + driveSubsystem.setAutoDebugMsg("Start " + currPathString); + currPath = path; + runningPath = true; + timer.stop(); + timer.reset(); + timer.start(); + driveSubsystem.calculateController(currPath.getInitialSample(mirrorTrajectory).get()); + if (currState == PathStates.PLACE) { + currState = PathStates.DRIVE_FETCH; + } else if (currState == PathStates.FETCH) { + currState = PathStates.DRIVE_PLACE; + } + } + } + + private void drivePath() { + if (isHandling && runningPath && currPath != null) { + driveSubsystem.calculateController(currPath.sampleAt(timer.get(), mirrorTrajectory).get()); + if (timer.hasElapsed(currPath.getTotalTime())) { + driveSubsystem.setAutoDebugMsg("End " + currPathString); + runningPath = false; + timer.stop(); + timer.reset(); + driveSubsystem.calculateController(currPath.getFinalSample(mirrorTrajectory).get()); + if (currState == PathStates.DRIVE_FETCH) { + currState = PathStates.FETCH; + } else if (currState == PathStates.DRIVE_PLACE) { + currState = PathStates.PLACE; + } + } else if (shouldTransitionToServoing()) { + currState = PathStates.DRIVE_PLACE_SERVO; + isServoing = true; + } + } + } + + private void drivePathServo() { + if (isHandling && runningPath && currPath != null && isServoing) { + driveSubsystem.calculateControllerServo( + currPath.sampleAt(timer.get(), mirrorTrajectory).get(), + 0.0); // TODO: use tag servoing here when ready + if (timer.hasElapsed(currPath.getTotalTime())) { + driveSubsystem.setAutoDebugMsg("End " + currPathString); + runningPath = false; + timer.stop(); + timer.reset(); + driveSubsystem.calculateControllerServo( + currPath.getFinalSample(mirrorTrajectory).get(), 0.0); + if (currState == PathStates.DRIVE_FETCH) { + currState = PathStates.FETCH; + } else if (currState == PathStates.DRIVE_PLACE_SERVO) { + isServoing = false; + // RobotStateSubsystem.PlacePiece(); + currState = PathStates.PLACE; + } + } + } + } + + private Trajectory nextPath() { + if (NodeNames.size() > 0) { + if (currState == PathStates.DRIVE_FETCH) { + currPathString = pathNames[NodeNames.get(0) - 'a'][0]; + return fetchPaths.get(NodeNames.get(0) - 'a'); + } else if (currState == PathStates.DRIVE_PLACE) { + currPathString = pathNames[NodeNames.get(0) - 'a'][1]; + return placePaths.get(NodeNames.get(0) - 'a'); + } + } else { + killPathHandler(); + } + return null; + } + + private void advanceNodes() { + if (NodeNames.size() > 0) { + NodeNames.remove(0); + } + } + + public void killPathHandler() { + isHandling = false; + currState = PathStates.DONE; + runningPath = false; + timer.stop(); + timer.reset(); + } + + public void killPathHandlerAfterPath() { + NodeNames.clear(); + } + + private boolean shouldTransitionToServoing() { + boolean isCloseEnough = false; + double preNormalizedAngle = + FastMath.toRadians( + RobotStateConstants.kNodeAngles[FastMath.floorToInt((NodeNames.get(0) - 'a') / 2)]); + double goal = + mirrorTrajectory + ? FastMath.normalizeMinusPiPi(preNormalizedAngle + Math.PI) + : preNormalizedAngle; + double pos = driveSubsystem.getGyroRotation2d().getRadians(); + if (goal < -Math.PI / 2 || goal > Math.PI / 2) { + isCloseEnough = + FastMath.abs(FastMath.normalizeZeroTwoPi(pos) - FastMath.normalizeZeroTwoPi(goal)) + < FastMath.toRadians(TagServoingConstants.kAngleCloseEnough); + } else { + isCloseEnough = + FastMath.abs(pos - goal) < FastMath.toRadians(TagServoingConstants.kAngleCloseEnough); + } + return currState == PathStates.DRIVE_PLACE + && timer.hasElapsed(currPath.getTotalTime() - 1.0) + && isCloseEnough; + } + + public void periodic() { + switch (currState) { + case DRIVE_FETCH: + if (!runningPath) { + startPath(nextPath()); + } + drivePath(); + break; + case FETCH: + // if (robotStateSubsystem.hasPiece) { + advanceNodes(); + currState = PathStates.DRIVE_PLACE; + // } + break; + case DRIVE_PLACE: + if (!runningPath) { + startPath(nextPath()); + } + drivePath(); + break; + case DRIVE_PLACE_SERVO: + if (runningPath && isServoing) { + drivePathServo(); + } + break; + case PLACE: + // if (RobotStateSubsystem.State != RobotStateSubsystem.hasPiece) { + currState = PathStates.DRIVE_FETCH; + // } + break; + case DONE: + isHandling = false; + break; + default: + break; + } + } + + @Override + public Set getMeasures() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getMeasures'"); + } + + public enum PathStates { + FETCH, + DRIVE_FETCH, + PLACE, + DRIVE_PLACE, + DRIVE_PLACE_SERVO, + DONE + } +}