From b1f2167439d430461b85672659f61cabfba2af9d Mon Sep 17 00:00:00 2001 From: moo Date: Sun, 3 May 2026 19:34:07 -0500 Subject: [PATCH] formatting, no actual changes --- .../robot/commands/gpm/Superstructure.java | 514 +++--- .../subsystems/drivetrain/Drivetrain.java | 1523 ++++++++--------- .../robot/subsystems/drivetrain/Module.java | 890 +++++----- .../SwerveStuff/SwerveSetpointGenerator.java | 282 +-- .../java/frc/robot/util/Vision/Vision.java | 582 ++++--- 5 files changed, 1950 insertions(+), 1841 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index 8ca8b16..e1e6088 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -31,289 +31,299 @@ import frc.robot.util.ShooterPhysics; import frc.robot.util.ShooterPhysics.TurretState; public class Superstructure extends Command { - private Turret turret; - private Drivetrain drivetrain; - private Hood hood; - private Shooter shooter; - private Spindexer spindexer; + private Turret turret; + private Drivetrain drivetrain; + private Hood hood; + private Shooter shooter; + private Spindexer spindexer; - private double turretSetpoint; - private double hoodSetpoint; + private double turretSetpoint; + private double hoodSetpoint; - private Pose2d drivepose; + private Pose2d drivepose; - private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); - private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); - - private Rotation2d lastTurretAngle; - private Rotation2d turretAngle; - private double turretVelocity; + private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); + private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); - private double lastHoodAngle; - private double hoodAngle; - private double hoodVelocity; + private Rotation2d lastTurretAngle; + private Rotation2d turretAngle; + private double turretVelocity; - private TurretState goalState; + private double lastHoodAngle; + private double hoodAngle; + private double hoodVelocity; - private LoggedNetworkNumber phaseDelay = new LoggedNetworkNumber("/Tuning/OPERATOR/Phase Delay", 0.03); //Extrapolation delay due to latency + private TurretState goalState; - private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d(); + private LoggedNetworkNumber phaseDelay = new LoggedNetworkNumber("/Tuning/OPERATOR/Phase Delay", 0.03); // Extrapolation + // delay due + // to latency - private PhaseManager phaseManager = new PhaseManager(); + private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d(); - private LoggedNetworkNumber hoodOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Hood Offset", 0.0); + private PhaseManager phaseManager = new PhaseManager(); - private LoggedNetworkNumber turretOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Turret Offet",0.0); + private LoggedNetworkNumber hoodOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Hood Offset", 0.0); - private double distanceFromTarget = 0.0; - private LoggedNetworkNumber shuttlingTOFMultiplier = new LoggedNetworkNumber("/Tuning/OPERATOR/Shuttling TOF Multiplier",0.8); + private LoggedNetworkNumber turretOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Turret Offet", 0.0); - // private double TOFAdjustment = 0.85; - // private double TOFAdjustment = 1.1; - private LoggedNetworkNumber TOFAdjustment = new LoggedNetworkNumber("/Tuning/OPERATOR/TOF Adjustment", 1.1); + private double distanceFromTarget = 0.0; + private LoggedNetworkNumber shuttlingTOFMultiplier = new LoggedNetworkNumber( + "/Tuning/OPERATOR/Shuttling TOF Multiplier", 0.8); - public Superstructure(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) { - this.turret = turret; - this.drivetrain = drivetrain; - this.hood = hood; - this.shooter = shooter; - this.spindexer = spindexer; - drivepose = drivetrain.getPose(); + // private double TOFAdjustment = 0.85; + // private double TOFAdjustment = 1.1; + private LoggedNetworkNumber TOFAdjustment = new LoggedNetworkNumber("/Tuning/OPERATOR/TOF Adjustment", 1.1); - goalState = ShooterPhysics.getShotParams( - Translation2d.kZero, - FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())), - 8.0); // Random initial goalState to prevent it being null - - addRequirements(turret, shooter); - } + public Superstructure(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) { + this.turret = turret; + this.drivetrain = drivetrain; + this.hood = hood; + this.shooter = shooter; + this.spindexer = spindexer; + drivepose = drivetrain.getPose(); - public void updateSetpoints(Pose2d drivepose) { - Pose2d turretPosition = drivepose.transformBy( - new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero)); - - // If the robot is moving, adjust the target position based on velocity - ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); - ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); - - // Rotational adjustment is not being used, since turret is in center of robot - double turretVelocityX = - fieldRelVel.vxMetersPerSecond; - double turretVelocityY = - fieldRelVel.vyMetersPerSecond; - - double timeOfFlight = 0; - Pose2d lookaheadPose = turretPosition; - - /* - * Loop (max 20) until lookaheadPose converges BECAUSE --> - * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate for 6m (t = 0.8s) - * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was derived using t = 1.0s - * So we make a bunch of guesses until it converges - * Early exit when change < 1mm to avoid unnecessary iterations - */ - boolean shuttling = target.equals(FieldConstants.getHubTranslation().toTranslation2d()); - for (int i = 0; i < 20; i++) { - Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ()); - - Translation3d target3d = new Translation3d(target.getX(), target.getY(), - shuttling ? - FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub - - goalState = ShooterPhysics.getShotParams( - Translation2d.kZero, - target3d.minus(lookahead3d), - 2.0); - - if (!shuttling) { - timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get(); - } else { - double distance = target.getDistance(lookaheadPose.getTranslation()); - timeOfFlight = distance * shuttlingTOFMultiplier.get(); - } - - double offsetX = turretVelocityX * timeOfFlight; - double offsetY = turretVelocityY * timeOfFlight; - Pose2d newLookaheadPose = - new Pose2d( - turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), - turretPosition.getRotation()); - - // early exit if converged (change < 1mm) - if (i > 0 && lookaheadPose.getTranslation().getDistance(newLookaheadPose.getTranslation()) < 0.001) { - lookaheadPose = newLookaheadPose; - break; - } - lookaheadPose = newLookaheadPose; - } - - // Get the field angle relative to the target pose - turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); - if (lastTurretAngle == null) { - lastTurretAngle = turretAngle; - } - - // Take the filtered average as the turret's velocity when robot is moving translationally - turretVelocity = - turretAngleFilter.calculate(turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME); - - lastTurretAngle = turretAngle; - - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Turret/Target Pose", target); - Logger.recordOutput("Lookahead Pose", lookaheadPose); - } - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putNumber("Time of flight", timeOfFlight); - SmartDashboard.putNumber("Turret X-Velocity", turretVelocityX); - SmartDashboard.putNumber("Turret Y-Velocity", turretVelocityY); - } - - // Subtract the rotational angle of the robot from the setpoint - double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians()); - - // Shortest path - double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180); - double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset.get(); - - // Stay within physical limits -- if shortest path is past max angle, we go long way around - if (potentialSetpoint > TurretConstants.MAX_ANGLE) { - potentialSetpoint -= 360; - } else if (potentialSetpoint < TurretConstants.MIN_ANGLE) { - potentialSetpoint += 360; - } - - turretSetpoint = potentialSetpoint; - - // Pitch is in radians - hoodAngle = goalState.pitch(); - hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); - hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME); - lastHoodAngle = hoodAngle; - - distanceFromTarget = target.getDistance(lookaheadPose.getTranslation()); - Logger.recordOutput("Shooting/distanceToTarget", distanceFromTarget); - } + goalState = ShooterPhysics.getShotParams( + Translation2d.kZero, + FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())), + 8.0); // Random initial goalState to prevent it being null - public void updateDrivePose(){ - Pose2d currentPose = drivetrain.getPose(); - - drivepose = new Pose2d( - currentPose.getTranslation(), - // Uncomment this if robot is backwards - currentPose.getRotation()//.plus(new Rotation2d(Math.PI)) - ); - ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); - - // Add a phase delay extrapolation component for latency delay - drivepose = drivepose.exp( - new Twist2d( - robotRelVel.vxMetersPerSecond * phaseDelay.get(), - robotRelVel.vyMetersPerSecond * phaseDelay.get(), - robotRelVel.omegaRadiansPerSecond * phaseDelay.get())); - } + addRequirements(turret, shooter); + } + + public void updateSetpoints(Pose2d drivepose) { + Pose2d turretPosition = drivepose.transformBy( + new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero)); + + // If the robot is moving, adjust the target position based on velocity + ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); + ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); + + // Rotational adjustment is not being used, since turret is in center of robot + double turretVelocityX = fieldRelVel.vxMetersPerSecond; + double turretVelocityY = fieldRelVel.vyMetersPerSecond; + + double timeOfFlight = 0; + Pose2d lookaheadPose = turretPosition; - /** - * Stops and stows all subsystems involved in the command + /* + * Loop (max 20) until lookaheadPose converges BECAUSE --> + * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate + * for 6m (t = 0.8s) + * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was + * derived using t = 1.0s + * So we make a bunch of guesses until it converges + * Early exit when change < 1mm to avoid unnecessary iterations */ - public void stowEverything(){ - turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0); - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0); - shooter.setShooter(0.0); - spindexer.noIndexing = true; + boolean shuttling = target.equals(FieldConstants.getHubTranslation().toTranslation2d()); + for (int i = 0; i < 20; i++) { + Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), + TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ()); + + Translation3d target3d = new Translation3d(target.getX(), target.getY(), + shuttling ? FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub + + goalState = ShooterPhysics.getShotParams( + Translation2d.kZero, + target3d.minus(lookahead3d), + 2.0); + + if (!shuttling) { + timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get(); + } else { + double distance = target.getDistance(lookaheadPose.getTranslation()); + timeOfFlight = distance * shuttlingTOFMultiplier.get(); + } + + double offsetX = turretVelocityX * timeOfFlight; + double offsetY = turretVelocityY * timeOfFlight; + Pose2d newLookaheadPose = new Pose2d( + turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), + turretPosition.getRotation()); + + // early exit if converged (change < 1mm) + if (i > 0 && lookaheadPose.getTranslation().getDistance(newLookaheadPose.getTranslation()) < 0.001) { + lookaheadPose = newLookaheadPose; + break; + } + lookaheadPose = newLookaheadPose; } - public void underLadder(){ - spindexer.noIndexing = true; + // Get the field angle relative to the target pose + turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); + if (lastTurretAngle == null) { + lastTurretAngle = turretAngle; } - // shoot higher - public void bumpUpHoodOffset() { - hoodOffset.set(hoodOffset.get() + 1.0); //1 deg - } + // Take the filtered average as the turret's velocity when robot is moving + // translationally + turretVelocity = turretAngleFilter.calculate(turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME); + + lastTurretAngle = turretAngle; - // shoot lower - public void bumpDownHoodOffset() { - hoodOffset.set(hoodOffset.get() - 1.0); //1 deg + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Turret/Target Pose", target); + Logger.recordOutput("Lookahead Pose", lookaheadPose); + } + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("Time of flight", timeOfFlight); + SmartDashboard.putNumber("Turret X-Velocity", turretVelocityX); + SmartDashboard.putNumber("Turret Y-Velocity", turretVelocityY); } - // aim more left - public void bumpUpTurretOffset() { - turretOffset.set(turretOffset.get() + 2.5); //2.5 deg + // Subtract the rotational angle of the robot from the setpoint + double adjustedTurretSetpoint = MathUtil + .angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians()); + + // Shortest path + double error = MathUtil.inputModulus( + Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180); + double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset.get(); + + // Stay within physical limits -- if shortest path is past max angle, we go long + // way around + if (potentialSetpoint > TurretConstants.MAX_ANGLE) { + potentialSetpoint -= 360; + } else if (potentialSetpoint < TurretConstants.MIN_ANGLE) { + potentialSetpoint += 360; } - // aim more right - public void bumpDownTurretOffset() { - turretOffset.set(turretOffset.get() - 2.5); //2.5 deg + turretSetpoint = potentialSetpoint; + + // Pitch is in radians + hoodAngle = goalState.pitch(); + hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); + hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME); + lastHoodAngle = hoodAngle; + + distanceFromTarget = target.getDistance(lookaheadPose.getTranslation()); + Logger.recordOutput("Shooting/distanceToTarget", distanceFromTarget); + } + + public void updateDrivePose() { + Pose2d currentPose = drivetrain.getPose(); + + drivepose = new Pose2d( + currentPose.getTranslation(), + // Uncomment this if robot is backwards + currentPose.getRotation()// .plus(new Rotation2d(Math.PI)) + ); + ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); + + // Add a phase delay extrapolation component for latency delay + drivepose = drivepose.exp( + new Twist2d( + robotRelVel.vxMetersPerSecond * phaseDelay.get(), + robotRelVel.vyMetersPerSecond * phaseDelay.get(), + robotRelVel.omegaRadiansPerSecond * phaseDelay.get())); + } + + /** + * Stops and stows all subsystems involved in the command + */ + public void stowEverything() { + turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0); + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0); + shooter.setShooter(0.0); + spindexer.noIndexing = true; + } + + public void underLadder() { + spindexer.noIndexing = true; + } + + // shoot higher + public void bumpUpHoodOffset() { + hoodOffset.set(hoodOffset.get() + 1.0); // 1 deg + } + + // shoot lower + public void bumpDownHoodOffset() { + hoodOffset.set(hoodOffset.get() - 1.0); // 1 deg + } + + // aim more left + public void bumpUpTurretOffset() { + turretOffset.set(turretOffset.get() + 2.5); // 2.5 deg + } + + // aim more right + public void bumpDownTurretOffset() { + turretOffset.set(turretOffset.get() - 2.5); // 2.5 deg + } + + @Override + public void execute() { + // Phase manager stuff + phaseManager.update(drivepose, shooter, turret); + target = phaseManager.getTarget(drivepose); + + updateDrivePose(); + updateSetpoints(drivepose); + + if (phaseManager.isIdle()) { + underLadder(); + } else { + if (spindexer.noIndexing) { + spindexer.noIndexing = false; + } + turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), + turretVelocity - drivetrain.getAngularRate(2)); + + boolean shuttling = !target.equals(FieldConstants.getHubTranslation().toTranslation2d()); // if we're aiming at + // the hub, we're not + // shuttling + + // shuttling will move the hood so its angles very close to max (less arch) + if (shuttling) { + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)), + hoodVelocity); + } else { + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), + hoodVelocity); + } + + // if (FieldConstants.underTrench(x, y)) { + // System.out.println("Hood forced down"); + // } else { + // hood.forceHoodDown(false); + // } + + // different maps for shuttling vs shooting. Less powerful when shuttling. + if (shuttling) { + shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget)); + } else { + shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget)); + } + + if (!Constants.DISABLE_LOGGING) { + // record when shuttling + Logger.recordOutput("Shuttling", shuttling); + // record distance for tuning if needed + Logger.recordOutput("Distance From Target", distanceFromTarget); + } } - @Override - public void execute() { - // Phase manager stuff - phaseManager.update(drivepose, shooter, turret); - target = phaseManager.getTarget(drivepose); - - updateDrivePose(); - updateSetpoints(drivepose); - - if (phaseManager.isIdle()) { - underLadder(); - } else { - if (spindexer.noIndexing) { - spindexer.noIndexing = false; - } - turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2)); - - boolean shuttling = !target.equals(FieldConstants.getHubTranslation().toTranslation2d()); // if we're aiming at the hub, we're not shuttling - - // shuttling will move the hood so its angles very close to max (less arch) - if (shuttling) { - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity); - } else { - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity); - } - - // if (FieldConstants.underTrench(x, y)) { - // System.out.println("Hood forced down"); - // } else { - // hood.forceHoodDown(false); - // } - - // different maps for shuttling vs shooting. Less powerful when shuttling. - if (shuttling) { - shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget)); - } else { - shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget)); - } - - if (!Constants.DISABLE_LOGGING) { - // record when shuttling - Logger.recordOutput("Shuttling", shuttling); - // record distance for tuning if needed - Logger.recordOutput("Distance From Target", distanceFromTarget); - } - } - - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint); - Logger.recordOutput("Hood Calculate Setpoint", hoodSetpoint); - Logger.recordOutput("Shooter Calculate Velocity", goalState.exitVel()); - - Logger.recordOutput("DistanceToTarget", target.getDistance(drivepose.getTranslation())); - } - - // for operator - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString()); - - } else { - phaseDelay.set(0.03); - } + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint); + Logger.recordOutput("Hood Calculate Setpoint", hoodSetpoint); + Logger.recordOutput("Shooter Calculate Velocity", goalState.exitVel()); + + Logger.recordOutput("DistanceToTarget", target.getDistance(drivepose.getTranslation())); } - @Override - public void end(boolean interrupted) { - stowEverything(); + // for operator + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString()); + + } else { + phaseDelay.set(0.03); } + } + + @Override + public void end(boolean interrupted) { + stowEverything(); + } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 0db0422..c118e51 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -54,796 +54,795 @@ import org.photonvision.EstimatedRobotPose; */ public class Drivetrain extends SubsystemBase { - protected final Module[] modules; + protected final Module[] modules; - private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); - public static Lock odometryLock = new ReentrantLock(); + public static Lock odometryLock = new ReentrantLock(); - private SwerveSetpoint currentSetpoint = new SwerveSetpoint( - new ChassisSpeeds(), - new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }); - // Odometry - private final SwerveDrivePoseEstimator poseEstimator; + private SwerveSetpoint currentSetpoint = new SwerveSetpoint( + new ChassisSpeeds(), + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }); + // Odometry + private final SwerveDrivePoseEstimator poseEstimator; - // Vision - private final Vision vision; + // Vision + private final Vision vision; - // PID Controllers for chassis movement - private final PIDController xController; - private final PIDController yController; - private final PIDController rotationController; + // PID Controllers for chassis movement + private final PIDController xController; + private final PIDController yController; + private final PIDController rotationController; - // If vision is enabled for drivetrain odometry updating - // DO NOT CHANGE THIS HERE TO DISABLE VISION, change VisionConstants.ENABLED - // instead - private boolean visionEnabled = true; + // If vision is enabled for drivetrain odometry updating + // DO NOT CHANGE THIS HERE TO DISABLE VISION, change VisionConstants.ENABLED + // instead + private boolean visionEnabled = true; - // Disables vision for the first few seconds after deploying - private Timer visionEnableTimer = new Timer(); + // Disables vision for the first few seconds after deploying + private Timer visionEnableTimer = new Timer(); - // If the robot should algin to the angle - private boolean isAlign = false; - // Angle to align to, can be null - private Double alignAngle = null; - // used for drift control - private double currentHeading = 0; - // used for drift control - private boolean drive_turning = false; + // If the robot should algin to the angle + private boolean isAlign = false; + // Angle to align to, can be null + private Double alignAngle = null; + // used for drift control + private double currentHeading = 0; + // used for drift control + private boolean drive_turning = false; - private SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(); + private SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(); - // The pose supplier to drive to - private Supplier desiredPoSupplier = () -> null; + // The pose supplier to drive to + private Supplier desiredPoSupplier = () -> null; - private SwerveModulePose modulePoses; + private SwerveModulePose modulePoses; - // The previous pose to reset to if the current pose gets too far off the field - private Pose2d prevPose = new Pose2d(); + // The previous pose to reset to if the current pose gets too far off the field + private Pose2d prevPose = new Pose2d(); - private SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];; + private SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];; - private boolean slipped = false; + private boolean slipped = false; - private double previousAngularVelocity = 0; + private double previousAngularVelocity = 0; - private double centerOfMassHeight = 0; + private double centerOfMassHeight = 0; - private Rotation2d rawGyroRotation = new Rotation2d(); + private Rotation2d rawGyroRotation = new Rotation2d(); - // for vision yaw correction - private GyroBiasEstimator gyroBiasEstimator = new GyroBiasEstimator(); + // for vision yaw correction + private GyroBiasEstimator gyroBiasEstimator = new GyroBiasEstimator(); - private final Field2d field = new Field2d(); + private final Field2d field = new Field2d(); - /** - * Creates a new Swerve Style Drivetrain. - */ - public Drivetrain(Vision vision, GyroIO gyroIO) { - this.vision = vision; - - modules = new Module[4]; - this.gyroIO = gyroIO; - ModuleConstants[] constants = Arrays.copyOfRange(ModuleConstants.values(), 0, 4); - - if (RobotBase.isReal()) { - Arrays.stream(constants).forEach(moduleConstants -> { - modules[moduleConstants.ordinal()] = new Module(moduleConstants); - }); - } else { - Arrays.stream(constants).forEach(moduleConstants -> { - modules[moduleConstants.ordinal()] = new ModuleSim(moduleConstants); - }); - } - - /* - * By pausing init for a second before setting module offsets, we avoid a bug - * with inverting motors. - * See https://github.com/Team364/BaseFalconSwerve/issues/8 for more info. - */ - Timer.delay(1.0); - resetModulesToAbsolute(); - gyroIO.updateInputs(gyroInputs); - poseEstimator = new SwerveDrivePoseEstimator( - DriveConstants.KINEMATICS, - gyroInputs.yawPosition, - updateModulePositions(), - new Pose2d(), - // Defaults, except trust pigeon more - VecBuilder.fill(0.1, 0.1, 0), - VisionConstants.VISION_STD_DEVS); - poseEstimator.setVisionMeasurementStdDevs(VisionConstants.VISION_STD_DEVS); - - // initialize PID controllers - xController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D); - yController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D); - rotationController = new PIDController(DriveConstants.HEADING_P, 0, DriveConstants.HEADING_D); - rotationController.enableContinuousInput(-Math.PI, Math.PI); - rotationController.setTolerance(Units.degreesToRadians(0.25), Units.degreesToRadians(0.25)); - - PhoenixOdometryThread.getInstance().start(); - - modulePoses = new SwerveModulePose(this, DriveConstants.MODULE_LOCATIONS); - - PathPlannerLogging.setLogActivePathCallback( - (activePath) -> { - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - } - }); - PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); - } - }); - - // PPLibTelemetry.enableCompetitionMode(); - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putData("Field", field); - } - } - - public void close() { - // close each of the modules - for (int i = 0; i < modules.length; i++) { - modules[i].close(); - } - } - - @Override - public void periodic() { - odometryLock.lock(); // Prevents odometry updates while reading data - gyroIO.updateInputs(gyroInputs); - Logger.processInputs("Drive/Gyro", gyroInputs); - for (var module : modules) { - module.periodic(); - } - odometryLock.unlock(); - // Update odometry - double[] sampleTimestamps = gyroInputs.odometryYawTimestamps; // All signals are sampled together - int sampleCount = sampleTimestamps.length; - SwerveModulePosition[][] positions = new SwerveModulePosition[4][]; - for (int i = 0; i < modules.length; i++) { - positions[i] = modules[i].getOdometryPositions(); - sampleCount = Math.min(sampleCount, positions[i].length); - } - - // cap samples per cycle, more gives little benefit - final int MAX_SAMPLES_PER_CYCLE = 10; - if (sampleCount > MAX_SAMPLES_PER_CYCLE) { - sampleCount = MAX_SAMPLES_PER_CYCLE; - } - - for (int i = 0; i < sampleCount; i++) { - // Read wheel positions and deltas from each module - SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; - for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { - modulePositions[moduleIndex] = positions[moduleIndex][i]; - } - // Use the real gyro angle - rawGyroRotation = gyroInputs.odometryYawPositions[i]; - // Apply update - poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); - } - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses()); - } - updateOdometryVision(); - - field.setRobotPose(getPose()); - } - - // DRIVE - /** - * Method to drive the robot using joystick info. - * - * @param xSpeed speed of the robot in the x direction (forward) in m/s - * @param ySpeed speed of the robot in the y direction (sideways) in m/s - * @param rot angular rate of the robot in rad/s - * @param fieldRelative whether the provided x and y speeds are relative to the - * field - * @param isOpenLoop whether to use velocity control for the drive motors - */ - public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) { - // rot = headingControl(rot, xSpeed, ySpeed); - ChassisSpeeds speeds = ChassisSpeeds.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME); - if (fieldRelative) { - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw()); - } - setChassisSpeeds(speeds, isOpenLoop); - } + /** + * Creates a new Swerve Style Drivetrain. + */ + public Drivetrain(Vision vision, GyroIO gyroIO) { + this.vision = vision; - /** - * Drives the robot using the provided x speed, y speed, and positional heading. - * - * @param xSpeed speed of the robot in the x direction (forward) - * @param ySpeed speed of the robot in the y direction (sideways) - * @param heading target heading of the robot in radians - * @param fieldRelative whether the provided x and y speeds are relative to the - * field - */ - public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) { - double rot = rotationController.calculate(getYaw().getRadians(), heading); - ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot); - if (fieldRelative) { - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw()); - } - setChassisSpeeds(speeds, false); - } + modules = new Module[4]; + this.gyroIO = gyroIO; + ModuleConstants[] constants = Arrays.copyOfRange(ModuleConstants.values(), 0, 4); - /** - * Runs the PID controllers with the provided x, y, and rot values. Then, calls - * {@link #drive(double, double, double, boolean, boolean)} using the PID - * outputs. - * This is based on the odometry of the chassis. - * - * @param x the position to move to in the x, in meters - * @param y the position to move to in the y, in meters - * @param rot the angle to move to, in radians - */ - public void driveWithPID(double x, double y, double rot) { - Pose2d pose = getPose(); - double xSpeed = xController.calculate(pose.getX(), x); - double ySpeed = yController.calculate(pose.getY(), y); - double rotRadians = rotationController.calculate(pose.getRotation().getRadians(), rot); - drive(xSpeed, ySpeed, rotRadians, true, false); + if (RobotBase.isReal()) { + Arrays.stream(constants).forEach(moduleConstants -> { + modules[moduleConstants.ordinal()] = new Module(moduleConstants); + }); + } else { + Arrays.stream(constants).forEach(moduleConstants -> { + modules[moduleConstants.ordinal()] = new ModuleSim(moduleConstants); + }); } - /** - * Updates odometry using vision + /* + * By pausing init for a second before setting module offsets, we avoid a bug + * with inverting motors. + * See https://github.com/Team364/BaseFalconSwerve/issues/8 for more info. */ - public void updateOdometryVision() { - // Start the timer if it hasn't started yet - visionEnableTimer.start(); - - // Update the swerve module poses - modulePoses.update(); - - if (modulePoses.slipped()) { - slipped = true; - } - - Pose2d pose2 = getPose(); - - // Even if vision is disabled, it should still update inputs - // This prevents it from storing a lot of unread results, and it could be useful - // for replays - if (vision != null) { - vision.updateInputs(); - } - - if (VisionConstants.ENABLED) { - if (vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)) { - vision.updateOdometry(poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped); - - if (vision.canSeeTag()) { - slipped = false; - modulePoses.reset(); - - double currentGyroYaw = gyroInputs.yawPosition.getRadians(); - - // to compare bias - ArrayList visionPoses = vision.getEstimatedPoses(getPose()); - - for (EstimatedRobotPose visionPose : visionPoses) { - if (visionPose.estimatedPose != null && visionPose.timestampSeconds > 0) { - double visionYaw = visionPose.estimatedPose.getRotation().getZ(); - - // gets at vision timestamp, not current gyro yaw - double gyroYawAtTimestamp = getGyroYawAtTimestamp(visionPose.timestampSeconds); - - if (!Double.isNaN(gyroYawAtTimestamp)) { - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("GyroYaw", Math.toDegrees(gyroYawAtTimestamp)); - Logger.recordOutput("VisionYaw", Math.toDegrees(visionYaw)); - } - // use weighted observation - gyroBiasEstimator.addObservation(visionYaw, gyroYawAtTimestamp, 1.0); - } - } - } - - // check if we have enough samples - if (gyroBiasEstimator.getSampleCount() >= GyroBiasConstants.MIN_SAMPLES) { - double fullBias = gyroBiasEstimator.getAndResetBias(); - double bias = gyroBiasEstimator.applyPartialCorrection(fullBias); - - if (Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD) { - gyroIO.setYaw(new Rotation2d(currentGyroYaw + bias)); - } - } + Timer.delay(1.0); + resetModulesToAbsolute(); + gyroIO.updateInputs(gyroInputs); + poseEstimator = new SwerveDrivePoseEstimator( + DriveConstants.KINEMATICS, + gyroInputs.yawPosition, + updateModulePositions(), + new Pose2d(), + // Defaults, except trust pigeon more + VecBuilder.fill(0.1, 0.1, 0), + VisionConstants.VISION_STD_DEVS); + poseEstimator.setVisionMeasurementStdDevs(VisionConstants.VISION_STD_DEVS); + + // initialize PID controllers + xController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D); + yController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D); + rotationController = new PIDController(DriveConstants.HEADING_P, 0, DriveConstants.HEADING_D); + rotationController.enableContinuousInput(-Math.PI, Math.PI); + rotationController.setTolerance(Units.degreesToRadians(0.25), Units.degreesToRadians(0.25)); + + PhoenixOdometryThread.getInstance().start(); + + modulePoses = new SwerveModulePose(this, DriveConstants.MODULE_LOCATIONS); + + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + } + }); + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + } + }); + + // PPLibTelemetry.enableCompetitionMode(); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putData("Field", field); + } + } + + public void close() { + // close each of the modules + for (int i = 0; i < modules.length; i++) { + modules[i].close(); + } + } + + @Override + public void periodic() { + odometryLock.lock(); // Prevents odometry updates while reading data + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + odometryLock.unlock(); + // Update odometry + double[] sampleTimestamps = gyroInputs.odometryYawTimestamps; // All signals are sampled together + int sampleCount = sampleTimestamps.length; + SwerveModulePosition[][] positions = new SwerveModulePosition[4][]; + for (int i = 0; i < modules.length; i++) { + positions[i] = modules[i].getOdometryPositions(); + sampleCount = Math.min(sampleCount, positions[i].length); + } + + // cap samples per cycle, more gives little benefit + final int MAX_SAMPLES_PER_CYCLE = 10; + if (sampleCount > MAX_SAMPLES_PER_CYCLE) { + sampleCount = MAX_SAMPLES_PER_CYCLE; + } + + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = positions[moduleIndex][i]; + } + // Use the real gyro angle + rawGyroRotation = gyroInputs.odometryYawPositions[i]; + // Apply update + poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + } + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses()); + } + updateOdometryVision(); + + field.setRobotPose(getPose()); + } + + // DRIVE + /** + * Method to drive the robot using joystick info. + * + * @param xSpeed speed of the robot in the x direction (forward) in m/s + * @param ySpeed speed of the robot in the y direction (sideways) in m/s + * @param rot angular rate of the robot in rad/s + * @param fieldRelative whether the provided x and y speeds are relative to the + * field + * @param isOpenLoop whether to use velocity control for the drive motors + */ + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) { + // rot = headingControl(rot, xSpeed, ySpeed); + ChassisSpeeds speeds = ChassisSpeeds.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME); + if (fieldRelative) { + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw()); + } + setChassisSpeeds(speeds, isOpenLoop); + } + + /** + * Drives the robot using the provided x speed, y speed, and positional heading. + * + * @param xSpeed speed of the robot in the x direction (forward) + * @param ySpeed speed of the robot in the y direction (sideways) + * @param heading target heading of the robot in radians + * @param fieldRelative whether the provided x and y speeds are relative to the + * field + */ + public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) { + double rot = rotationController.calculate(getYaw().getRadians(), heading); + ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot); + if (fieldRelative) { + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw()); + } + setChassisSpeeds(speeds, false); + } + + /** + * Runs the PID controllers with the provided x, y, and rot values. Then, calls + * {@link #drive(double, double, double, boolean, boolean)} using the PID + * outputs. + * This is based on the odometry of the chassis. + * + * @param x the position to move to in the x, in meters + * @param y the position to move to in the y, in meters + * @param rot the angle to move to, in radians + */ + public void driveWithPID(double x, double y, double rot) { + Pose2d pose = getPose(); + double xSpeed = xController.calculate(pose.getX(), x); + double ySpeed = yController.calculate(pose.getY(), y); + double rotRadians = rotationController.calculate(pose.getRotation().getRadians(), rot); + drive(xSpeed, ySpeed, rotRadians, true, false); + } + + /** + * Updates odometry using vision + */ + public void updateOdometryVision() { + // Start the timer if it hasn't started yet + visionEnableTimer.start(); + + // Update the swerve module poses + modulePoses.update(); + + if (modulePoses.slipped()) { + slipped = true; + } + + Pose2d pose2 = getPose(); + + // Even if vision is disabled, it should still update inputs + // This prevents it from storing a lot of unread results, and it could be useful + // for replays + if (vision != null) { + vision.updateInputs(); + } + + if (VisionConstants.ENABLED) { + if (vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)) { + vision.updateOdometry(poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped); + + if (vision.canSeeTag()) { + slipped = false; + modulePoses.reset(); + + double currentGyroYaw = gyroInputs.yawPosition.getRadians(); + + // to compare bias + ArrayList visionPoses = vision.getEstimatedPoses(getPose()); + + for (EstimatedRobotPose visionPose : visionPoses) { + if (visionPose.estimatedPose != null && visionPose.timestampSeconds > 0) { + double visionYaw = visionPose.estimatedPose.getRotation().getZ(); + + // gets at vision timestamp, not current gyro yaw + double gyroYawAtTimestamp = getGyroYawAtTimestamp(visionPose.timestampSeconds); + + if (!Double.isNaN(gyroYawAtTimestamp)) { + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("GyroYaw", Math.toDegrees(gyroYawAtTimestamp)); + Logger.recordOutput("VisionYaw", Math.toDegrees(visionYaw)); } + // use weighted observation + gyroBiasEstimator.addObservation(visionYaw, gyroYawAtTimestamp, 1.0); + } } - } - - Pose2d pose3 = getPose(); - - // Reset the pose to a position on the field if it is too far off the field - // This uses nearField() instead of onField() so we don't reset the odometry - // when the wheels slip near the edge of the field - // This is meant for poses that are caused by errors - if (!Vision.nearField(prevPose)) { - // If the pose at the beginning of the method is off the field, reset to a - // position in the middle of the field - // Use the rotation of the pose after updating odometry so the yaw is right - prevPose = new Pose2d(FieldConstants.field.getFieldLength() / 2, FieldConstants.field.getFieldWidth() / 2, - pose2.getRotation()); - resetOdometry(prevPose); - } else if (!Vision.nearField(pose2)) { - // if the drivetrain pose is off the field, reset our odometry to the pose - // before(this is the right pose) - // Keep the rotation from pose2 so yaw is correct for driver - prevPose = new Pose2d(prevPose.getTranslation(), pose2.getRotation()); - resetOdometry(prevPose); - } else if (!Vision.nearField(pose3)) { - // if our vision+drivetrain odometry isn't near the field, reset our odometry to - // the pose before(this is the right pose) - resetOdometry(pose2); - prevPose = pose2; - } else { - // Set the previous pose to the current pose if we need to return to that - prevPose = pose3; - } - - // if (Robot.isSimulation()) { - // pigeon.getSimState().addYaw( - // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond - // * Constants.LOOP_TIME)); - // } - } - - /** - * Stops all swerve modules. - */ - public void stop() { - Arrays.stream(modules).forEach(Module::stop); - } - - // GETTERS AND SETTERS - - private boolean trenchAssist = false; - private boolean trenchAlign = false; - - public boolean getTrenchAssist() { - return trenchAssist; - } + } - public boolean getTrenchAlign() { - return trenchAlign; - } - - public void setTrenchAssist(boolean target) { - trenchAssist = target; - } - - public void setTrenchAlign(boolean target) { - trenchAlign = target; - } - - // for current limit setting (brownout protection) - public void applyNewModuleCurrents( - double steerCurrentStator, double steerCurrentSupply, - double driveCurrentStator, double driveCurrentSupply) { - for (Module module : modules) { // iterate over our modules - module.setNewCurrentLimit(steerCurrentStator, steerCurrentSupply, driveCurrentStator, driveCurrentSupply); - } - } - - public double getSubsystemStatorCurrent() { - double sum = 0; - for (Module module : modules) { - sum += module.getModuleStatorCurrent(); - } - return sum; - } - - public double getSubsystemSupplyCurrent() { - double sum = 0; - for (Module module : modules) { - sum += module.getModuleSupplyCurrent(); - } - return sum; - } - - /** - * Sets the desired states for all swerve modules. - * - * @param swerveModuleStates an array of module states to set swerve modules to. - * Order of the array matters here! - */ - public void setModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) { - // makes sure speeds of modules don't exceed maximum allowed - SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.MAX_SPEED); - - for (int i = 0; i < 4; i++) { - modules[i].setDesiredState(swerveModuleStates[i], isOpenLoop); - } - } - - /** - * Sets the chassis speeds of the robot. - * - * @param chassisSpeeds the target chassis speeds - * @param isOpenLoop if open loop control should be used for the drive - * velocity - */ - public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean isOpenLoop) { - - if (DriveConstants.USE_ACTUAL_SPEED) { - SwerveSetpoint currentState = new SwerveSetpoint(getChassisSpeeds(), getModuleStates()); - currentSetpoint = setpointGenerator.generateSetpoint( - DriveConstants.MODULE_LIMITS, - centerOfMassHeight, - currentState, chassisSpeeds, - Constants.LOOP_TIME); - } else { - currentSetpoint = setpointGenerator.generateSetpoint( - DriveConstants.MODULE_LIMITS, - centerOfMassHeight, - currentSetpoint, chassisSpeeds, - Constants.LOOP_TIME); - } - - SwerveModuleState[] swerveModuleStates = currentSetpoint.moduleStates(); - setModuleStates(swerveModuleStates, isOpenLoop); - } + // check if we have enough samples + if (gyroBiasEstimator.getSampleCount() >= GyroBiasConstants.MIN_SAMPLES) { + double fullBias = gyroBiasEstimator.getAndResetBias(); + double bias = gyroBiasEstimator.applyPartialCorrection(fullBias); - public void setDriveVoltages(Voltage voltage) { - for (int i = 0; i < modules.length; i++) { - modules[i].setDriveVoltage(voltage); - } - } - - public void setAngleMotors(Rotation2d[] angles) { - for (int i = 0; i < modules.length; i++) { - modules[i].setAngle(angles[i]); - } - } - - /** - * Returns the angular rate from the pigeon. - * - * @param id 0 for x, 1 for y, 2 for z - * @return the rate in rads/s from the pigeon - */ - public double getAngularRate(int id) { - // double speed = 0; - // switch(id){ - // case 0: - // speed = gyroInputs..getAngularVelocityXWorld().getValueAsDouble(); - // break; - // case 1: - // speed = pigeon.getAngularVelocityYWorld().getValueAsDouble(); - // break; - // case 2: - // speed = pigeon.getAngularVelocityZWorld().getValueAsDouble(); - // break; - // } - // outputs in deg/s, so convert to rad/s - return gyroInputs.yawVelocityRadPerSec; - } - - /** - * Updates and returns the array of SwerveModulePositions, which store the - * distance travleled by the drive and the steer angle. - * - * @return An array of all swerve module positions - */ - private SwerveModulePosition[] updateModulePositions() { - return modulePositions = Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new); - } - - /** - * Gets an array of SwerveModulePositions, which store the distance travleled by - * the drive and the steer angle. - * - * @return An array of all swerve module positions - */ - public SwerveModulePosition[] getModulePositions() { - return modulePositions; - } - - /** - * Enables or disables the state deadband for all swerve modules. - * The state deadband determines if the robot will stop drive and steer motors - * when inputted drive velocity is low. - * It should be enabled for all regular driving, to prevent releasing the - * controls from setting the angles. - */ - public void setStateDeadband(boolean stateDeadBand) { - Arrays.stream(modules).forEach(module -> module.setStateDeadband(stateDeadBand)); - } - - public void setOptimized(boolean optimized) { - Arrays.stream(modules).forEach(module -> module.setOptimize(optimized)); - } - - public void setVisionEnabled(boolean enabled) { - visionEnabled = enabled; - } - - public void setIsAlign(boolean isAlign) { - this.isAlign = isAlign; - } - - public boolean getIsAlign() { - return isAlign; - } - - /** - * Calculates chassis speed of drivetrain using the current SwerveModuleStates - * - * @return ChassisSpeeds object - * This is often used as an input for other methods - */ - public ChassisSpeeds getChassisSpeeds() { - return DriveConstants.KINEMATICS.toChassisSpeeds(getModuleStates()); - } - - /** - * Gets the state of each module - * - * @return An array of 4 SwerveModuleStates - */ - public SwerveModuleState[] getModuleStates() { - return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); - } - - public SwerveSetpoint getCurrSetpoint() { - return currentSetpoint; - } - - /** - * @return the yaw of the robot, aka heading, the direction it is facing - */ - public Rotation2d getYaw() { - return getPose().getRotation(); - } - - /** - * @return an array of modules - */ - public Module[] getModules() { - return modules; - } - - /** - * gets gyro yaw at a specific timestamp with interpolation - * this is used for timestamp-synchronized gyro/vision comparison. - * - * @param timestampSeconds the timestamp to get the gyro yaw at - * @return the gyro yaw in radians, or Double.NaN if no valid data - */ - private double - getGyroYawAtTimestamp(double timestampSeconds) { - return getPose().getRotation().getRadians(); - } - - /** - * Resets the yaw of the robot. - * - * @param rotation the new yaw angle as Rotation2d - */ - public void setYaw(Rotation2d rotation) { - resetOdometry(new Pose2d(getPose().getTranslation(), rotation)); - } - - /** - * Resets the odometry to the given pose. - * - * @param pose the pose to reset to. - */ - public void resetOdometry(Pose2d pose) { - // NOTE: must use pigeon yaw for odometer! - currentHeading = pose.getRotation().getRadians(); - poseEstimator.resetPosition(gyroInputs.yawPosition, getModulePositions(), pose); - modulePoses.reset(); - } - - /** - * @return the pose of the robot as estimated by the odometry - */ - @AutoLogOutput(key = "Odometry/Robot") - public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); - } - - /** - * Sets the angle to align to - * - * @param newAngle The new angle in radians, can be set to null - */ - public void setAlignAngle(Double newAngle) { - alignAngle = newAngle; - } - - /** - * Returns whether or not the robot is at the input align angle - * - * @return true if it within tolerance the align angle, false otherwise - */ - public boolean atAlignAngle() { - if (alignAngle == null) { - return false; - } - double diff = Math.abs(alignAngle - getYaw().getRadians()); - return diff < DriveConstants.HEADING_TOLERANCE || diff > 2 * Math.PI - DriveConstants.HEADING_TOLERANCE; - } - - /** - * Gets the angle to align to - * - * @return The angle in radians - */ - public double getAlignAngle() { - if (alignAngle != null) { - return alignAngle; - } - return 0; - } - - /** - * Sets vision to only use certain April tags - * - * @param ids An array of the tags to only use - */ - public void onlyUseTags(int[] ids) { - if (vision != null) { - vision.onlyUse(ids); - } - } - - /** - * Returns if vision has seen an April tag in the last frame - * - * @return true if vision saw a tag last frame or if vision is disabled - */ - public boolean canSeeTag() { - // if no vision system, then return true - if (vision == null) - return true; - - return vision.canSeeTag() || !visionEnabled || !VisionConstants.ENABLED; - } - - /** - * Gets the pose at a previous time - * - * @param timestamp The timestamp of the pose to get - * @return The pose, null if there are no poses yet, or the current pose if - * timestamp < 0 - */ - public Pose2d getPoseAt(double timestamp) { - if (timestamp < 0) { - return getPose(); - } - Optional pose = poseEstimator.sampleAt(timestamp); - if (pose.isPresent()) { - return pose.get(); - } else { - return null; - } - } - - /** - * Uses pigeon and rotational input to return a rotation that accounts for drift - * - * @return A rotation - */ - public double headingControl(double rot, double xSpeed, double ySpeed) { - if ((!EqualsUtil.epsilonEquals(getAngularRate(0), 0, 0.0004) - && EqualsUtil.epsilonEquals(Math.hypot(xSpeed, ySpeed), 0, 0.1)) - || !EqualsUtil.epsilonEquals(rot, 0, 0.0004)) { - drive_turning = true; - currentHeading = getYaw().getRadians(); - } else { - drive_turning = false; - } - if (!drive_turning) { - rotationController.setSetpoint(currentHeading); - double output = rotationController.calculate(getYaw().getRadians()); - rot = Math.abs(output) > Math.abs(rot) ? output : rot; + if (Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD) { + gyroIO.setYaw(new Rotation2d(currentGyroYaw + bias)); + } + } } - return rot; - } - - /** - * Resets the swerve modules from the absolute encoders - */ - public void resetModulesToAbsolute() { - Arrays.stream(modules).forEach(Module::resetToAbsolute); - } - - // getters for the PID Controllers - public PIDController getXController() { - return xController; - } - - public PIDController getYController() { - return yController; - } - - public PIDController getRotationController() { - return rotationController; - } - - /** - * Set the desired pose to drive to - * This will enable driver assist to go to the pose - * - * @param supplier The supplier for the desired pose, use ()->null to not use a - * desired pose - */ - public void setDesiredPose(Supplier supplier) { - desiredPoSupplier = supplier; - } - - /** - * Set the desired pose to drive to - * This will enable driver assist to go to the pose - * - * @param pose The Pose2d to drive to - */ - public void setDesiredPose(Pose2d pose) { - setDesiredPose(() -> pose); - } - - /** - * Gets the current desired pose, or null if there is no desired pose - * - * @return The Pose2d if it exists, null otherwise - */ - public Pose2d getDesiredPose() { - return desiredPoSupplier.get(); - } - - public boolean atSetpoint() { - Pose2d pose = getDesiredPose(); - return pose != null && getPose().getTranslation().getDistance(pose.getTranslation()) < 0.025; - } - - public SwerveModulePose getSwerveModulePose() { - return modulePoses; - } - - public double getAcceleration() { - double accelX = gyroInputs.accelerationX; - double accelY = gyroInputs.accelerationY; - - double angularVelocity = getAngularRate(3); - double angularAccel = (angularVelocity - previousAngularVelocity) / Constants.LOOP_TIME; - previousAngularVelocity = angularVelocity; - - double pigeonOffsetX = 0.082677; - double pigeonOffsetY = 0.030603444; - - double totalX = accelX + Math.pow(angularVelocity, 2) * pigeonOffsetX + angularAccel * pigeonOffsetY; - double totalY = accelY + Math.pow(angularVelocity, 2) * pigeonOffsetY - angularAccel * pigeonOffsetX; - - return Math.hypot(totalX, totalY); - } - - @AutoLogOutput(key = "Drivetrain/AccelerationFaults") - public boolean accelerationOverMax() { - return getAcceleration() > DriveConstants.MAX_LINEAR_ACCEL; - } - - public void setCenterOfMass(double height) { - centerOfMassHeight = height; - } - - public void alignWheels() { - SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(0)); - setModuleStates(new SwerveModuleState[] { - state, state, state, state - }, false); - } + } + } + + Pose2d pose3 = getPose(); + + // Reset the pose to a position on the field if it is too far off the field + // This uses nearField() instead of onField() so we don't reset the odometry + // when the wheels slip near the edge of the field + // This is meant for poses that are caused by errors + if (!Vision.nearField(prevPose)) { + // If the pose at the beginning of the method is off the field, reset to a + // position in the middle of the field + // Use the rotation of the pose after updating odometry so the yaw is right + prevPose = new Pose2d(FieldConstants.field.getFieldLength() / 2, FieldConstants.field.getFieldWidth() / 2, + pose2.getRotation()); + resetOdometry(prevPose); + } else if (!Vision.nearField(pose2)) { + // if the drivetrain pose is off the field, reset our odometry to the pose + // before(this is the right pose) + // Keep the rotation from pose2 so yaw is correct for driver + prevPose = new Pose2d(prevPose.getTranslation(), pose2.getRotation()); + resetOdometry(prevPose); + } else if (!Vision.nearField(pose3)) { + // if our vision+drivetrain odometry isn't near the field, reset our odometry to + // the pose before(this is the right pose) + resetOdometry(pose2); + prevPose = pose2; + } else { + // Set the previous pose to the current pose if we need to return to that + prevPose = pose3; + } + + // if (Robot.isSimulation()) { + // pigeon.getSimState().addYaw( + // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond + // * Constants.LOOP_TIME)); + // } + } + + /** + * Stops all swerve modules. + */ + public void stop() { + Arrays.stream(modules).forEach(Module::stop); + } + + // GETTERS AND SETTERS + + private boolean trenchAssist = false; + private boolean trenchAlign = false; + + public boolean getTrenchAssist() { + return trenchAssist; + } + + public boolean getTrenchAlign() { + return trenchAlign; + } + + public void setTrenchAssist(boolean target) { + trenchAssist = target; + } + + public void setTrenchAlign(boolean target) { + trenchAlign = target; + } + + // for current limit setting (brownout protection) + public void applyNewModuleCurrents( + double steerCurrentStator, double steerCurrentSupply, + double driveCurrentStator, double driveCurrentSupply) { + for (Module module : modules) { // iterate over our modules + module.setNewCurrentLimit(steerCurrentStator, steerCurrentSupply, driveCurrentStator, driveCurrentSupply); + } + } + + public double getSubsystemStatorCurrent() { + double sum = 0; + for (Module module : modules) { + sum += module.getModuleStatorCurrent(); + } + return sum; + } + + public double getSubsystemSupplyCurrent() { + double sum = 0; + for (Module module : modules) { + sum += module.getModuleSupplyCurrent(); + } + return sum; + } + + /** + * Sets the desired states for all swerve modules. + * + * @param swerveModuleStates an array of module states to set swerve modules to. + * Order of the array matters here! + */ + public void setModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) { + // makes sure speeds of modules don't exceed maximum allowed + SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.MAX_SPEED); + + for (int i = 0; i < 4; i++) { + modules[i].setDesiredState(swerveModuleStates[i], isOpenLoop); + } + } + + /** + * Sets the chassis speeds of the robot. + * + * @param chassisSpeeds the target chassis speeds + * @param isOpenLoop if open loop control should be used for the drive + * velocity + */ + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean isOpenLoop) { + + if (DriveConstants.USE_ACTUAL_SPEED) { + SwerveSetpoint currentState = new SwerveSetpoint(getChassisSpeeds(), getModuleStates()); + currentSetpoint = setpointGenerator.generateSetpoint( + DriveConstants.MODULE_LIMITS, + centerOfMassHeight, + currentState, chassisSpeeds, + Constants.LOOP_TIME); + } else { + currentSetpoint = setpointGenerator.generateSetpoint( + DriveConstants.MODULE_LIMITS, + centerOfMassHeight, + currentSetpoint, chassisSpeeds, + Constants.LOOP_TIME); + } + + SwerveModuleState[] swerveModuleStates = currentSetpoint.moduleStates(); + setModuleStates(swerveModuleStates, isOpenLoop); + } + + public void setDriveVoltages(Voltage voltage) { + for (int i = 0; i < modules.length; i++) { + modules[i].setDriveVoltage(voltage); + } + } + + public void setAngleMotors(Rotation2d[] angles) { + for (int i = 0; i < modules.length; i++) { + modules[i].setAngle(angles[i]); + } + } + + /** + * Returns the angular rate from the pigeon. + * + * @param id 0 for x, 1 for y, 2 for z + * @return the rate in rads/s from the pigeon + */ + public double getAngularRate(int id) { + // double speed = 0; + // switch(id){ + // case 0: + // speed = gyroInputs..getAngularVelocityXWorld().getValueAsDouble(); + // break; + // case 1: + // speed = pigeon.getAngularVelocityYWorld().getValueAsDouble(); + // break; + // case 2: + // speed = pigeon.getAngularVelocityZWorld().getValueAsDouble(); + // break; + // } + // outputs in deg/s, so convert to rad/s + return gyroInputs.yawVelocityRadPerSec; + } + + /** + * Updates and returns the array of SwerveModulePositions, which store the + * distance travleled by the drive and the steer angle. + * + * @return An array of all swerve module positions + */ + private SwerveModulePosition[] updateModulePositions() { + return modulePositions = Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new); + } + + /** + * Gets an array of SwerveModulePositions, which store the distance travleled by + * the drive and the steer angle. + * + * @return An array of all swerve module positions + */ + public SwerveModulePosition[] getModulePositions() { + return modulePositions; + } + + /** + * Enables or disables the state deadband for all swerve modules. + * The state deadband determines if the robot will stop drive and steer motors + * when inputted drive velocity is low. + * It should be enabled for all regular driving, to prevent releasing the + * controls from setting the angles. + */ + public void setStateDeadband(boolean stateDeadBand) { + Arrays.stream(modules).forEach(module -> module.setStateDeadband(stateDeadBand)); + } + + public void setOptimized(boolean optimized) { + Arrays.stream(modules).forEach(module -> module.setOptimize(optimized)); + } + + public void setVisionEnabled(boolean enabled) { + visionEnabled = enabled; + } + + public void setIsAlign(boolean isAlign) { + this.isAlign = isAlign; + } + + public boolean getIsAlign() { + return isAlign; + } + + /** + * Calculates chassis speed of drivetrain using the current SwerveModuleStates + * + * @return ChassisSpeeds object + * This is often used as an input for other methods + */ + public ChassisSpeeds getChassisSpeeds() { + return DriveConstants.KINEMATICS.toChassisSpeeds(getModuleStates()); + } + + /** + * Gets the state of each module + * + * @return An array of 4 SwerveModuleStates + */ + public SwerveModuleState[] getModuleStates() { + return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); + } + + public SwerveSetpoint getCurrSetpoint() { + return currentSetpoint; + } + + /** + * @return the yaw of the robot, aka heading, the direction it is facing + */ + public Rotation2d getYaw() { + return getPose().getRotation(); + } + + /** + * @return an array of modules + */ + public Module[] getModules() { + return modules; + } + + /** + * gets gyro yaw at a specific timestamp with interpolation + * this is used for timestamp-synchronized gyro/vision comparison. + * + * @param timestampSeconds the timestamp to get the gyro yaw at + * @return the gyro yaw in radians, or Double.NaN if no valid data + */ + private double getGyroYawAtTimestamp(double timestampSeconds) { + return getPose().getRotation().getRadians(); + } + + /** + * Resets the yaw of the robot. + * + * @param rotation the new yaw angle as Rotation2d + */ + public void setYaw(Rotation2d rotation) { + resetOdometry(new Pose2d(getPose().getTranslation(), rotation)); + } + + /** + * Resets the odometry to the given pose. + * + * @param pose the pose to reset to. + */ + public void resetOdometry(Pose2d pose) { + // NOTE: must use pigeon yaw for odometer! + currentHeading = pose.getRotation().getRadians(); + poseEstimator.resetPosition(gyroInputs.yawPosition, getModulePositions(), pose); + modulePoses.reset(); + } + + /** + * @return the pose of the robot as estimated by the odometry + */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** + * Sets the angle to align to + * + * @param newAngle The new angle in radians, can be set to null + */ + public void setAlignAngle(Double newAngle) { + alignAngle = newAngle; + } + + /** + * Returns whether or not the robot is at the input align angle + * + * @return true if it within tolerance the align angle, false otherwise + */ + public boolean atAlignAngle() { + if (alignAngle == null) { + return false; + } + double diff = Math.abs(alignAngle - getYaw().getRadians()); + return diff < DriveConstants.HEADING_TOLERANCE || diff > 2 * Math.PI - DriveConstants.HEADING_TOLERANCE; + } + + /** + * Gets the angle to align to + * + * @return The angle in radians + */ + public double getAlignAngle() { + if (alignAngle != null) { + return alignAngle; + } + return 0; + } + + /** + * Sets vision to only use certain April tags + * + * @param ids An array of the tags to only use + */ + public void onlyUseTags(int[] ids) { + if (vision != null) { + vision.onlyUse(ids); + } + } + + /** + * Returns if vision has seen an April tag in the last frame + * + * @return true if vision saw a tag last frame or if vision is disabled + */ + public boolean canSeeTag() { + // if no vision system, then return true + if (vision == null) + return true; + + return vision.canSeeTag() || !visionEnabled || !VisionConstants.ENABLED; + } + + /** + * Gets the pose at a previous time + * + * @param timestamp The timestamp of the pose to get + * @return The pose, null if there are no poses yet, or the current pose if + * timestamp < 0 + */ + public Pose2d getPoseAt(double timestamp) { + if (timestamp < 0) { + return getPose(); + } + Optional pose = poseEstimator.sampleAt(timestamp); + if (pose.isPresent()) { + return pose.get(); + } else { + return null; + } + } + + /** + * Uses pigeon and rotational input to return a rotation that accounts for drift + * + * @return A rotation + */ + public double headingControl(double rot, double xSpeed, double ySpeed) { + if ((!EqualsUtil.epsilonEquals(getAngularRate(0), 0, 0.0004) + && EqualsUtil.epsilonEquals(Math.hypot(xSpeed, ySpeed), 0, 0.1)) + || !EqualsUtil.epsilonEquals(rot, 0, 0.0004)) { + drive_turning = true; + currentHeading = getYaw().getRadians(); + } else { + drive_turning = false; + } + if (!drive_turning) { + rotationController.setSetpoint(currentHeading); + double output = rotationController.calculate(getYaw().getRadians()); + rot = Math.abs(output) > Math.abs(rot) ? output : rot; + } + return rot; + } + + /** + * Resets the swerve modules from the absolute encoders + */ + public void resetModulesToAbsolute() { + Arrays.stream(modules).forEach(Module::resetToAbsolute); + } + + // getters for the PID Controllers + public PIDController getXController() { + return xController; + } + + public PIDController getYController() { + return yController; + } + + public PIDController getRotationController() { + return rotationController; + } + + /** + * Set the desired pose to drive to + * This will enable driver assist to go to the pose + * + * @param supplier The supplier for the desired pose, use ()->null to not use a + * desired pose + */ + public void setDesiredPose(Supplier supplier) { + desiredPoSupplier = supplier; + } + + /** + * Set the desired pose to drive to + * This will enable driver assist to go to the pose + * + * @param pose The Pose2d to drive to + */ + public void setDesiredPose(Pose2d pose) { + setDesiredPose(() -> pose); + } + + /** + * Gets the current desired pose, or null if there is no desired pose + * + * @return The Pose2d if it exists, null otherwise + */ + public Pose2d getDesiredPose() { + return desiredPoSupplier.get(); + } + + public boolean atSetpoint() { + Pose2d pose = getDesiredPose(); + return pose != null && getPose().getTranslation().getDistance(pose.getTranslation()) < 0.025; + } + + public SwerveModulePose getSwerveModulePose() { + return modulePoses; + } + + public double getAcceleration() { + double accelX = gyroInputs.accelerationX; + double accelY = gyroInputs.accelerationY; + + double angularVelocity = getAngularRate(3); + double angularAccel = (angularVelocity - previousAngularVelocity) / Constants.LOOP_TIME; + previousAngularVelocity = angularVelocity; + + double pigeonOffsetX = 0.082677; + double pigeonOffsetY = 0.030603444; + + double totalX = accelX + Math.pow(angularVelocity, 2) * pigeonOffsetX + angularAccel * pigeonOffsetY; + double totalY = accelY + Math.pow(angularVelocity, 2) * pigeonOffsetY - angularAccel * pigeonOffsetX; + + return Math.hypot(totalX, totalY); + } + + @AutoLogOutput(key = "Drivetrain/AccelerationFaults") + public boolean accelerationOverMax() { + return getAcceleration() > DriveConstants.MAX_LINEAR_ACCEL; + } + + public void setCenterOfMass(double height) { + centerOfMassHeight = height; + } + + public void alignWheels() { + SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(0)); + setModuleStates(new SwerveModuleState[] { + state, state, state, state + }, false); + } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Module.java b/src/main/java/frc/robot/subsystems/drivetrain/Module.java index 5511175..c14aa8d 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Module.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Module.java @@ -41,450 +41,454 @@ import frc.robot.constants.swerve.ModuleType; import frc.robot.util.PhoenixOdometryThread; import lib.CTREModuleState; - -public class Module implements ModuleIO{ - private final ModuleType type; - - // Degrees - private final double angleOffset; - - private final TalonFX angleMotor; - private final TalonFX driveMotor; - private final CANcoder CANcoder; - private SwerveModuleState desiredState; - - protected boolean stateDeadband = true; - - private boolean optimizeStates = true; - - // Inputs from drive motor - private final StatusSignal drivePosition; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - // Inputs from turn motor - private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - - // Timestamp inputs from Phoenix thread - protected final Queue timestampQueue; - protected final Queue drivePositionQueue; - protected final Queue turnPositionQueue; - - private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; - - // Connection debouncers - private final Debouncer driveConnectedDebounce = new Debouncer(0.5); - private final Debouncer turnConnectedDebounce = new Debouncer(0.5); - private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5); - - private final Alert driveDisconnectedAlert; - private final Alert turnDisconnectedAlert; - private final Alert turnEncoderDisconnectedAlert; - - protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - - private ModuleConstants moduleConstants; - private final MotionMagicVelocityVoltage velocityRequest = - new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0); - - - public Module(ModuleConstants moduleConstants) { - this.moduleConstants = moduleConstants; - - type = moduleConstants.getType(); - angleOffset = moduleConstants.getSteerOffset(); - - /* Angle Encoder Config */ - CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN); - /* Angle Motor Config */ - angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN); - driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN); - // Create drive status signals - drivePosition = driveMotor.getPosition(); - driveVelocity = driveMotor.getVelocity(); - driveAppliedVolts = driveMotor.getMotorVoltage(); - driveCurrent = driveMotor.getStatorCurrent(); - - // Create turn status signals - turnAbsolutePosition = CANcoder.getAbsolutePosition(); - turnPosition = angleMotor.getPosition(); - turnVelocity = angleMotor.getVelocity(); - turnAppliedVolts = angleMotor.getMotorVoltage(); - turnCurrent = angleMotor.getStatorCurrent(); - - // Create timestamp queue - timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition()); - turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition()); - updateInputs(); - - configCANcoder(); - configAngleMotor(); - configDriveMotor(); - - driveDisconnectedAlert = - new Alert( - "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", - AlertType.kError); - turnDisconnectedAlert = - new Alert( - "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError); - turnEncoderDisconnectedAlert = - new Alert( - "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".", - AlertType.kError); - - - // Configure periodic frames - BaseStatusSignal.setUpdateFrequencyForAll( - 250, drivePosition, turnPosition); - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - setDesiredState(new SwerveModuleState(0, getAngle()), false); - } - - public void close() { - angleMotor.close(); - driveMotor.close(); - CANcoder.close(); - } - - @Override - public void updateInputs() { - // Refresh all signals - var driveStatus = - BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); - var turnStatus = - BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); - var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); - - // Update drive inputs - inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); - inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO); - inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); - - // Update turn inputs - inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); - inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); - inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); - inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio); - inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio); - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); - - // Update encoder inputs - inputs.encoderOffset = Units.rotationsToDegrees(CANcoder.getAbsolutePosition().getValueAsDouble()); - - // Update odometry inputs - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value)) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - - inputs.driveStator = driveMotor.getStatorCurrent().getValueAsDouble(); - inputs.driveSupply = driveMotor.getSupplyCurrent().getValueAsDouble(); - inputs.steerStator = angleMotor.getStatorCurrent().getValueAsDouble(); - inputs.steerSupply = angleMotor.getSupplyCurrent().getValueAsDouble(); - } - - public void periodic() { - updateInputs(); - Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs); - - // Calculate positions for odometry - int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together - odometryPositions = new SwerveModulePosition[sampleCount]; - for (int i = 0; i < sampleCount; i++) { - double positionMeters = inputs.odometryDrivePositionsRad[i]/DriveConstants.DRIVE_GEAR_RATIO * DriveConstants.WHEEL_RADIUS; - Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio); - odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); - } - // Update alerts - driveDisconnectedAlert.set(!inputs.driveConnected); - turnDisconnectedAlert.set(!inputs.turnConnected); - turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Angle "+ moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360)); - } - } - - public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) { - // Separate if here and in setAngle() to avoid warning - if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){ - /* - * This is a custom optimize function, since default WPILib optimize assumes - * continuous controller which CTRE and Rev onboard is not - */ - desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState; - }else{ - desiredState = wantedState; - } - setAngle(); - setSpeed(isOpenLoop); - } - - public void setSpeed(boolean isOpenLoop) { - if(desiredState == null){ - return; - } - if (isOpenLoop) { - double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED; - driveMotor.setControl(new DutyCycleOut(percentOutput)); - } else { - double velocity = desiredState.speedMetersPerSecond/DriveConstants.WHEEL_RADIUS/2/Math.PI*DriveConstants.DRIVE_GEAR_RATIO; - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity); - } - - double feedforward = velocity * moduleConstants.getDriveV(); - driveMotor.setControl( - velocityRequest - .withVelocity(velocity) - .withFeedForward(feedforward)); - } - } - - private void setAngle() { - if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){ - // Prevent rotating module if desired speed < 1%. Prevents jittering and unnecessary movement. - if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) { - stop(); - return; - } - } - if(desiredState == null){ - return; - } - angleMotor.setControl(new PositionDutyCycle(desiredState.angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio)); - } - - public void setDriveVoltage(Voltage voltage){ - driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude())); - } - public void setAngle(Rotation2d angle){ - angleMotor.setControl(new PositionDutyCycle(angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio)); - } - - public void setOptimize(boolean enable) { - optimizeStates = enable; - } - - public byte getModuleIndex() { - return type.id; - } - - public Rotation2d getAngle() { - return inputs.turnPosition; - } - - public Rotation2d getCANcoder() { - return inputs.turnAbsolutePosition; - } - - public void resetToAbsolute() { - // Sensor ticks - double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset); - angleMotor.setPosition(absolutePosition*DriveConstants.MODULE_CONSTANTS.angleGearRatio); - } - - private void configCANcoder() { - CANcoder.getConfigurator().apply(new CANcoderConfiguration()); - CANcoder.getConfigurator().apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1). - withSensorDirection(DriveConstants.MODULE_CONSTANTS.canCoderInvert?SensorDirectionValue.Clockwise_Positive:SensorDirectionValue.CounterClockwise_Positive)); - } - - private void configAngleMotor() { - angleMotor.getConfigurator().apply(new TalonFXConfiguration()); - - CurrentLimitsConfigs config = new CurrentLimitsConfigs(); - config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT; - config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT; - config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT; - config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION; - angleMotor.getConfigurator().apply(config); - angleMotor.getConfigurator().apply(new Slot0Configs() - .withKP(DriveConstants.MODULE_CONSTANTS.angleKP) - .withKI(DriveConstants.MODULE_CONSTANTS.angleKI) - .withKD(DriveConstants.MODULE_CONSTANTS.angleKD)); - angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR)); - angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE); - angleMotor.setPosition(0); - - // optimize bus utilization for angle motor - angleMotor.optimizeBusUtilization(); - - resetToAbsolute(); - } - - /** - * @return Speed in RPM - */ - public double getDriveVelocity() { - return inputs.driveVelocityRadPerSec*60/DriveConstants.MODULE_CONSTANTS.driveGearRatio/2/Math.PI; - } - - public double getDriveVoltage(){ - return inputs.driveAppliedVolts; - } - - public double getDriveStatorCurrent(){ - return inputs.driveCurrentAmps; - } - - public double getModuleStatorCurrent() { - return inputs.steerStator + inputs.driveStator; - } - - public double getModuleSupplyCurrent() { - return inputs.steerSupply + inputs.driveSupply; - } - - // I took the config things straight from this file - public void setNewCurrentLimit(double currentSteerStator, double currentSteerSupply, double currentDriveStator, double currentDriveSupply) { - CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs(); - // steer - steerConfig.SupplyCurrentLimitEnable = true; - steerConfig.StatorCurrentLimitEnable = true; - steerConfig.StatorCurrentLimit = currentSteerSupply; - steerConfig.SupplyCurrentLimit = currentSteerSupply; - steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION; - angleMotor.getConfigurator().apply(steerConfig); // apply - - // drive - CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs(); - driveConfig.SupplyCurrentLimitEnable = true; - driveConfig.StatorCurrentLimitEnable = true; - driveConfig.SupplyCurrentLimit = currentDriveSupply; - driveConfig.StatorCurrentLimit = currentDriveStator; - driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION; - driveMotor.getConfigurator().apply(driveConfig); // apply - } - - private void configDriveMotor() { - var talonFXConfigs = new TalonFXConfiguration(); - // set Motion Magic settings - var motionMagicConfigs = talonFXConfigs.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO; - motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO; - var slot0Configs = talonFXConfigs.Slot0; - slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction - slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output - slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output - slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output - slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error - slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output - driveMotor.getConfigurator().apply(talonFXConfigs); - CurrentLimitsConfigs config = new CurrentLimitsConfigs(); - config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; - config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT; - config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT; - config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION; - config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT; - config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; - driveMotor.getConfigurator().apply(config); - driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR)); - driveMotor.getConfigurator().apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP)); - driveMotor.getConfigurator().apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP)); - driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE); - - // optimize bus utilization for drive motor - driveMotor.optimizeBusUtilization(); - - } - - public SwerveModuleState getState() { - return new SwerveModuleState( - inputs.driveVelocityRadPerSec*DriveConstants.WHEEL_RADIUS, - getAngle()); - } - - public SwerveModulePosition getPosition() { - return new SwerveModulePosition( - inputs.drivePositionRad*DriveConstants.WHEEL_RADIUS, - getAngle()); - } - - public SwerveModuleState getDesiredState() { - return desiredState; - } - - - public double getDriveVelocityError() { - return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond; - } - - public void stop() { - driveMotor.set(0); - angleMotor.set(0); - } - - public ModuleType getModuleType(){ - return type; - } - - public void setStateDeadband(boolean enabled) { - stateDeadband = enabled; - } - - public double getDesiredVelocity() { - return getDesiredState().speedMetersPerSecond; +public class Module implements ModuleIO { + private final ModuleType type; + + // Degrees + private final double angleOffset; + + private final TalonFX angleMotor; + private final TalonFX driveMotor; + private final CANcoder CANcoder; + private SwerveModuleState desiredState; + + protected boolean stateDeadband = true; + + private boolean optimizeStates = true; + + // Inputs from drive motor + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + // Inputs from turn motor + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Timestamp inputs from Phoenix thread + protected final Queue timestampQueue; + protected final Queue drivePositionQueue; + protected final Queue turnPositionQueue; + + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5); + + private final Alert driveDisconnectedAlert; + private final Alert turnDisconnectedAlert; + private final Alert turnEncoderDisconnectedAlert; + + protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + + private ModuleConstants moduleConstants; + private final MotionMagicVelocityVoltage velocityRequest = new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0); + + public Module(ModuleConstants moduleConstants) { + this.moduleConstants = moduleConstants; + + type = moduleConstants.getType(); + angleOffset = moduleConstants.getSteerOffset(); + + /* Angle Encoder Config */ + CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN); + /* Angle Motor Config */ + angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN); + driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN); + // Create drive status signals + drivePosition = driveMotor.getPosition(); + driveVelocity = driveMotor.getVelocity(); + driveAppliedVolts = driveMotor.getMotorVoltage(); + driveCurrent = driveMotor.getStatorCurrent(); + + // Create turn status signals + turnAbsolutePosition = CANcoder.getAbsolutePosition(); + turnPosition = angleMotor.getPosition(); + turnVelocity = angleMotor.getVelocity(); + turnAppliedVolts = angleMotor.getMotorVoltage(); + turnCurrent = angleMotor.getStatorCurrent(); + + // Create timestamp queue + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition()); + turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition()); + updateInputs(); + + configCANcoder(); + configAngleMotor(); + configDriveMotor(); + + driveDisconnectedAlert = new Alert( + "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", + AlertType.kError); + turnDisconnectedAlert = new Alert( + "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError); + turnEncoderDisconnectedAlert = new Alert( + "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".", + AlertType.kError); + + // Configure periodic frames + BaseStatusSignal.setUpdateFrequencyForAll( + 250, drivePosition, turnPosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + setDesiredState(new SwerveModuleState(0, getAngle()), false); + } + + public void close() { + angleMotor.close(); + driveMotor.close(); + CANcoder.close(); + } + + @Override + public void updateInputs() { + // Refresh all signals + var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); + var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + + // Update drive inputs + inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); + inputs.drivePositionRad = Units + .rotationsToRadians(drivePosition.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO); + inputs.driveVelocityRadPerSec = Units + .rotationsToRadians(driveVelocity.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + + // Update turn inputs + inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); + inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); + inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); + inputs.turnPosition = Rotation2d + .fromRotations(turnPosition.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio); + inputs.turnVelocityRadPerSec = Units + .rotationsToRadians(turnVelocity.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + + // Update encoder inputs + inputs.encoderOffset = Units.rotationsToDegrees(CANcoder.getAbsolutePosition().getValueAsDouble()); + + // Update odometry inputs + inputs.odometryTimestamps = timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value)) + .toArray(); + inputs.odometryTurnPositions = turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + + inputs.driveStator = driveMotor.getStatorCurrent().getValueAsDouble(); + inputs.driveSupply = driveMotor.getSupplyCurrent().getValueAsDouble(); + inputs.steerStator = angleMotor.getStatorCurrent().getValueAsDouble(); + inputs.steerSupply = angleMotor.getSupplyCurrent().getValueAsDouble(); + } + + public void periodic() { + updateInputs(); + Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs); + + // Calculate positions for odometry + int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together + odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = inputs.odometryDrivePositionsRad[i] / DriveConstants.DRIVE_GEAR_RATIO + * DriveConstants.WHEEL_RADIUS; + Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio); + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + // Update alerts + driveDisconnectedAlert.set(!inputs.driveConnected); + turnDisconnectedAlert.set(!inputs.turnConnected); + turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Angle " + moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360)); + } + } + + public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) { + // Separate if here and in setAngle() to avoid warning + if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) { + /* + * This is a custom optimize function, since default WPILib optimize assumes + * continuous controller which CTRE and Rev onboard is not + */ + desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState; + } else { + desiredState = wantedState; + } + setAngle(); + setSpeed(isOpenLoop); + } + + public void setSpeed(boolean isOpenLoop) { + if (desiredState == null) { + return; + } + if (isOpenLoop) { + double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED; + driveMotor.setControl(new DutyCycleOut(percentOutput)); + } else { + double velocity = desiredState.speedMetersPerSecond / DriveConstants.WHEEL_RADIUS / 2 / Math.PI + * DriveConstants.DRIVE_GEAR_RATIO; + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity); } - - public Rotation2d getDesiredAngle() { - return getDesiredState().angle; - } - - /** Returns the module positions received this cycle. */ - public SwerveModulePosition[] getOdometryPositions() { - return odometryPositions; - } - /** Returns the timestamps of the samples received this cycle. */ - public double[] getOdometryTimestamps() { - return inputs.odometryTimestamps; - } - - /** returns the drive position status signal for time-synced odometry. */ - public StatusSignal getDrivePositionSignal() { - return drivePosition; - } - - /** returns the turn position status signal for time-synced odometry. */ - public StatusSignal getTurnPositionSignal() { - return turnPosition; - } - - /** returns the turn absolute position status signal for time-synced odometry. */ - public StatusSignal getTurnAbsolutePositionSignal() { - return turnAbsolutePosition; - } - - public TalonFX[] getMotors() { - return new TalonFX[]{angleMotor, driveMotor}; + double feedforward = velocity * moduleConstants.getDriveV(); + driveMotor.setControl( + velocityRequest + .withVelocity(velocity) + .withFeedForward(feedforward)); + } + } + + private void setAngle() { + if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) { + // Prevent rotating module if desired speed < 1%. Prevents jittering and + // unnecessary movement. + if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) { + stop(); + return; + } } + if (desiredState == null) { + return; + } + angleMotor.setControl( + new PositionDutyCycle(desiredState.angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio)); + } + + public void setDriveVoltage(Voltage voltage) { + driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude())); + } + + public void setAngle(Rotation2d angle) { + angleMotor.setControl(new PositionDutyCycle(angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio)); + } + + public void setOptimize(boolean enable) { + optimizeStates = enable; + } + + public byte getModuleIndex() { + return type.id; + } + + public Rotation2d getAngle() { + return inputs.turnPosition; + } + + public Rotation2d getCANcoder() { + return inputs.turnAbsolutePosition; + } + + public void resetToAbsolute() { + // Sensor ticks + double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset); + angleMotor.setPosition(absolutePosition * DriveConstants.MODULE_CONSTANTS.angleGearRatio); + } + + private void configCANcoder() { + CANcoder.getConfigurator().apply(new CANcoderConfiguration()); + CANcoder.getConfigurator() + .apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1).withSensorDirection( + DriveConstants.MODULE_CONSTANTS.canCoderInvert ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive)); + } + + private void configAngleMotor() { + angleMotor.getConfigurator().apply(new TalonFXConfiguration()); + + CurrentLimitsConfigs config = new CurrentLimitsConfigs(); + config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT; + config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT; + config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT; + config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION; + angleMotor.getConfigurator().apply(config); + angleMotor.getConfigurator().apply(new Slot0Configs() + .withKP(DriveConstants.MODULE_CONSTANTS.angleKP) + .withKI(DriveConstants.MODULE_CONSTANTS.angleKI) + .withKD(DriveConstants.MODULE_CONSTANTS.angleKD)); + angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR)); + angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE); + angleMotor.setPosition(0); + + // optimize bus utilization for angle motor + angleMotor.optimizeBusUtilization(); + + resetToAbsolute(); + } + + /** + * @return Speed in RPM + */ + public double getDriveVelocity() { + return inputs.driveVelocityRadPerSec * 60 / DriveConstants.MODULE_CONSTANTS.driveGearRatio / 2 / Math.PI; + } + + public double getDriveVoltage() { + return inputs.driveAppliedVolts; + } + + public double getDriveStatorCurrent() { + return inputs.driveCurrentAmps; + } + + public double getModuleStatorCurrent() { + return inputs.steerStator + inputs.driveStator; + } + + public double getModuleSupplyCurrent() { + return inputs.steerSupply + inputs.driveSupply; + } + + // I took the config things straight from this file + public void setNewCurrentLimit(double currentSteerStator, double currentSteerSupply, double currentDriveStator, + double currentDriveSupply) { + CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs(); + // steer + steerConfig.SupplyCurrentLimitEnable = true; + steerConfig.StatorCurrentLimitEnable = true; + steerConfig.StatorCurrentLimit = currentSteerSupply; + steerConfig.SupplyCurrentLimit = currentSteerSupply; + steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION; + angleMotor.getConfigurator().apply(steerConfig); // apply + + // drive + CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs(); + driveConfig.SupplyCurrentLimitEnable = true; + driveConfig.StatorCurrentLimitEnable = true; + driveConfig.SupplyCurrentLimit = currentDriveSupply; + driveConfig.StatorCurrentLimit = currentDriveStator; + driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION; + driveMotor.getConfigurator().apply(driveConfig); // apply + } + + private void configDriveMotor() { + var talonFXConfigs = new TalonFXConfiguration(); + // set Motion Magic settings + var motionMagicConfigs = talonFXConfigs.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED / DriveConstants.WHEEL_CIRCUMFERENCE + * DriveConstants.DRIVE_GEAR_RATIO; + motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL / DriveConstants.WHEEL_CIRCUMFERENCE + * DriveConstants.DRIVE_GEAR_RATIO; + var slot0Configs = talonFXConfigs.Slot0; + slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction + slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output + slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output + slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output + slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error + slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output + driveMotor.getConfigurator().apply(talonFXConfigs); + CurrentLimitsConfigs config = new CurrentLimitsConfigs(); + config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; + config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT; + config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT; + config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION; + config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT; + config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; + driveMotor.getConfigurator().apply(config); + driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR)); + driveMotor.getConfigurator() + .apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP)); + driveMotor.getConfigurator() + .apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP)); + driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE); + + // optimize bus utilization for drive motor + driveMotor.optimizeBusUtilization(); + + } + + public SwerveModuleState getState() { + return new SwerveModuleState( + inputs.driveVelocityRadPerSec * DriveConstants.WHEEL_RADIUS, + getAngle()); + } + + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + inputs.drivePositionRad * DriveConstants.WHEEL_RADIUS, + getAngle()); + } + + public SwerveModuleState getDesiredState() { + return desiredState; + } + + public double getDriveVelocityError() { + return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond; + } + + public void stop() { + driveMotor.set(0); + angleMotor.set(0); + } + + public ModuleType getModuleType() { + return type; + } + + public void setStateDeadband(boolean enabled) { + stateDeadband = enabled; + } + + public double getDesiredVelocity() { + return getDesiredState().speedMetersPerSecond; + } + + public Rotation2d getDesiredAngle() { + return getDesiredState().angle; + } + + /** Returns the module positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return inputs.odometryTimestamps; + } + + /** returns the drive position status signal for time-synced odometry. */ + public StatusSignal getDrivePositionSignal() { + return drivePosition; + } + + /** returns the turn position status signal for time-synced odometry. */ + public StatusSignal getTurnPositionSignal() { + return turnPosition; + } + + /** + * returns the turn absolute position status signal for time-synced odometry. + */ + public StatusSignal getTurnAbsolutePositionSignal() { + return turnAbsolutePosition; + } + + public TalonFX[] getMotors() { + return new TalonFX[] { angleMotor, driveMotor }; + } } diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java index 2c7dd20..2e96d94 100644 --- a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java +++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java @@ -25,27 +25,37 @@ import frc.robot.util.EqualsUtil; import frc.robot.util.GeomUtil; /** - * "Inspired" by FRC team 254. See the license file in the root directory of this project. + * "Inspired" by FRC team 254. See the license file in the root directory of + * this project. * - *

Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or from a path - * follower), and outputs a new setpoint that respects all of the kinematic constraints on module - * rotation speed and wheel velocity/acceleration. By generating a new setpoint every iteration, the - * robot will converge to the desired setpoint quickly while avoiding any intermediate state that is - * kinematically infeasible (and can result in wheel slip or robot heading drift as a result). + *

+ * Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or + * from a path + * follower), and outputs a new setpoint that respects all of the kinematic + * constraints on module + * rotation speed and wheel velocity/acceleration. By generating a new setpoint + * every iteration, the + * robot will converge to the desired setpoint quickly while avoiding any + * intermediate state that is + * kinematically infeasible (and can result in wheel slip or robot heading drift + * as a result). */ public class SwerveSetpointGenerator { - private final SwerveDriveKinematics kinematics= DriveConstants.KINEMATICS; + private final SwerveDriveKinematics kinematics = DriveConstants.KINEMATICS; private final Translation2d[] moduleLocations = DriveConstants.MODULE_LOCATIONS; /** - * Check if it would be faster to go to the opposite of the goal heading (and reverse drive + * Check if it would be faster to go to the opposite of the goal heading (and + * reverse drive * direction). * - * @param prevToGoal The rotation from the previous state to the goal state (i.e. - * prev.inverse().rotateBy(goal)). - * @return True if the shortest path to achieve this rotation involves flipping the drive - * direction. + * @param prevToGoal The rotation from the previous state to the goal state + * (i.e. + * prev.inverse().rotateBy(goal)). + * @return True if the shortest path to achieve this rotation involves flipping + * the drive + * direction. */ private boolean flipHeading(Rotation2d prevToGoal) { return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0; @@ -68,22 +78,27 @@ public class SwerveSetpointGenerator { } /** - * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. - * This is a pretty naive way to do root finding, but it's usually faster than simple bisection + * Find the root of the generic 2D parametric function 'func' using the regula + * falsi technique. + * This is a pretty naive way to do root finding, but it's usually faster than + * simple bisection * while being robust in ways that e.g. the Newton-Raphson method isn't. * - * @param func The Function2d to take the root of. - * @param x_0 x value of the lower bracket. - * @param y_0 y value of the lower bracket. - * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during - * recursion) - * @param x_1 x value of the upper bracket. - * @param y_1 y value of the upper bracket. - * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during - * recursion) + * @param func The Function2d to take the root of. + * @param x_0 x value of the lower bracket. + * @param y_0 y value of the lower bracket. + * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to + * save a call to 'func' during + * recursion) + * @param x_1 x value of the upper bracket. + * @param y_1 y value of the upper bracket. + * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to + * save a call to 'func' during + * recursion) * @param iterations_left Number of iterations of root finding left. - * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the - * (approximate) root. + * @return The parameter value 's' that interpolating between 0 and 1 that + * corresponds to the + * (approximate) root. */ private double findRoot( Function2d func, @@ -129,10 +144,9 @@ public class SwerveSetpointGenerator { return 1.0; } double offset = f_0 + Math.signum(diff) * max_deviation; - Function2d func = - (x, y) -> { - return unwrapAngle(f_0, Math.atan2(y, x)) - offset; - }; + Function2d func = (x, y) -> { + return unwrapAngle(f_0, Math.atan2(y, x)) - offset; + }; return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); } @@ -151,22 +165,24 @@ public class SwerveSetpointGenerator { return 1.0; } double offset = f_0 + Math.signum(diff) * max_vel_step; - Function2d func = - (x, y) -> { - return Math.hypot(x, y) - offset; - }; + Function2d func = (x, y) -> { + return Math.hypot(x, y) - offset; + }; return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); } /** * Limits the acceleration in all directions. - * This is different from findDriveMaxS because it includes the acceleration perpendicular to the wheel as it rotates. - * Given the same velocity step, this will return a lower S value than findDriveMaxS. - * @param x_0 The initial x velocity - * @param y_0 The initial y velocity - * @param x_1 The final x velocity - * @param y_1 The final y velocity - * @param max_vel_step The maxiumum amount the velocity can change this frame + * This is different from findDriveMaxS because it includes the acceleration + * perpendicular to the wheel as it rotates. + * Given the same velocity step, this will return a lower S value than + * findDriveMaxS. + * + * @param x_0 The initial x velocity + * @param y_0 The initial y velocity + * @param x_1 The final x velocity + * @param y_1 The final y velocity + * @param max_vel_step The maxiumum amount the velocity can change this frame * @param max_iterations The maximum number of iterations to use in findRoot * @return The maximum interpolation value */ @@ -177,8 +193,8 @@ public class SwerveSetpointGenerator { double y_1, double max_vel_step, int max_iterations) { - double dist = Math.hypot(x_1-x_0, y_1-y_0); - if(dist <= max_vel_step){ + double dist = Math.hypot(x_1 - x_0, y_1 - y_0); + if (dist <= max_vel_step) { return 1; } return Math.max(0.0, Math.min(1.0, max_vel_step / dist)); @@ -191,21 +207,21 @@ public class SwerveSetpointGenerator { // length of P(s) is the target T. P(s) is linearly interpolated between the // points, so P(s) = (x_0 + (x_1 - x_0) * s, y_0 + (y_1 - y_0) * s). // Then, - // T = sqrt(P(s).x^2 + P(s).y^2) - // T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2 - // T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2 - // + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2 - // T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2 - // + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2 - // 0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2 - // + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s - // + (x_0^2 + y_0^2 - T^2). + // T = sqrt(P(s).x^2 + P(s).y^2) + // T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2 + // T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2 + // + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2 + // T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2 + // + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2 + // 0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2 + // + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s + // + (x_0^2 + y_0^2 - T^2). // // To simplify, we can factor out some common parts: // Let l_0 = x_0^2 + y_0^2, l_1 = x_1^2 + y_1^2, and // p = x_0 * x_1 + y_0 * y_1. // Then we have - // 0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2), + // 0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2), // with which we can solve for s using the quadratic formula. double l_0 = x_0 * x_0 + y_0 * y_0; @@ -251,17 +267,24 @@ public class SwerveSetpointGenerator { /** * Generate a new setpoint. * - * @param limits The kinematic limits to respect for this setpoint. - * @param centerOfMassHeight The height of the robot's center of mass, in meters, off the ground. - * This assumes that the center of mass is in the center of the robot in the x and y directions. - * If tipping is not a potential problem this year, set this to 0. - * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous - * iteration setpoint instead of the actual measured/estimated kinematic state. - * @param desiredState The desired state of motion, such as from the driver sticks or a path - * following algorithm. - * @param dt The loop time. - * @return A Setpoint object that satisfies all of the KinematicLimits while converging to - * desiredState quickly. + * @param limits The kinematic limits to respect for this setpoint. + * @param centerOfMassHeight The height of the robot's center of mass, in + * meters, off the ground. + * This assumes that the center of mass is in the + * center of the robot in the x and y directions. + * If tipping is not a potential problem this year, + * set this to 0. + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass + * in the previous + * iteration setpoint instead of the actual + * measured/estimated kinematic state. + * @param desiredState The desired state of motion, such as from the + * driver sticks or a path + * following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all of the KinematicLimits while + * converging to + * desiredState quickly. */ public SwerveSetpoint generateSetpoint( final ModuleLimits limits, @@ -278,10 +301,11 @@ public class SwerveSetpointGenerator { desiredState = kinematics.toChassisSpeeds(desiredModuleState); } - // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so + // Special case: desiredState is a complete stop. In this case, module angle is + // arbitrary, so // just use the previous angle. boolean need_to_steer = true; - if (EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState),new Twist2d())) { + if (EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState), new Twist2d())) { need_to_steer = false; for (int i = 0; i < modules.length; ++i) { desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle; @@ -298,61 +322,63 @@ public class SwerveSetpointGenerator { Rotation2d[] desired_heading = new Rotation2d[modules.length]; boolean all_modules_should_flip = true; for (int i = 0; i < modules.length; ++i) { - prev_vx[i] = - prevSetpoint.moduleStates()[i].angle.getCos() - * prevSetpoint.moduleStates()[i].speedMetersPerSecond; - prev_vy[i] = - prevSetpoint.moduleStates()[i].angle.getSin() - * prevSetpoint.moduleStates()[i].speedMetersPerSecond; + prev_vx[i] = prevSetpoint.moduleStates()[i].angle.getCos() + * prevSetpoint.moduleStates()[i].speedMetersPerSecond; + prev_vy[i] = prevSetpoint.moduleStates()[i].angle.getSin() + * prevSetpoint.moduleStates()[i].speedMetersPerSecond; prev_heading[i] = prevSetpoint.moduleStates()[i].angle; if (prevSetpoint.moduleStates()[i].speedMetersPerSecond < 0.0) { prev_heading[i] = prev_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); } - desired_vx[i] = - desiredModuleState[i].angle.getCos() * desiredModuleState[i].speedMetersPerSecond; - desired_vy[i] = - desiredModuleState[i].angle.getSin() * desiredModuleState[i].speedMetersPerSecond; + desired_vx[i] = desiredModuleState[i].angle.getCos() * desiredModuleState[i].speedMetersPerSecond; + desired_vy[i] = desiredModuleState[i].angle.getSin() * desiredModuleState[i].speedMetersPerSecond; desired_heading[i] = desiredModuleState[i].angle; if (desiredModuleState[i].speedMetersPerSecond < 0.0) { desired_heading[i] = desired_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); } if (all_modules_should_flip) { - double required_rotation_rad = - Math.abs(prev_heading[i].unaryMinus().rotateBy(desired_heading[i]).getRadians()); + double required_rotation_rad = Math.abs(prev_heading[i].unaryMinus().rotateBy(desired_heading[i]).getRadians()); if (required_rotation_rad < Math.PI / 2.0) { all_modules_should_flip = false; } } } if (all_modules_should_flip - && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(prevSetpoint.chassisSpeeds()),new Twist2d()) - && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState),new Twist2d())) { - // It will (likely) be faster to stop the robot, rotate the modules in place to the complement + && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(prevSetpoint.chassisSpeeds()), new Twist2d()) + && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState), new Twist2d())) { + // It will (likely) be faster to stop the robot, rotate the modules in place to + // the complement // of the desired // angle, and accelerate again. return generateSetpoint(limits, centerOfMassHeight, prevSetpoint, new ChassisSpeeds(), dt); } - // Compute the deltas between start and goal. We can then interpolate from the start state to + // Compute the deltas between start and goal. We can then interpolate from the + // start state to // the goal state; then - // find the amount we can move from start towards goal in this cycle such that no kinematic + // find the amount we can move from start towards goal in this cycle such that + // no kinematic // limit is exceeded. double dx = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond; double dy = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond; - double dtheta = - desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; + double dtheta = desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; - // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at + // 's' interpolates between start and goal. At 0, we are at prevState and at 1, + // we are at // desiredState. double min_s = 1.0; - // In cases where an individual module is stopped, we want to remember the right steering angle + // In cases where an individual module is stopped, we want to remember the right + // steering angle // to command (since - // inverse kinematics doesn't care about angle, we can be opportunistically lazy). + // inverse kinematics doesn't care about angle, we can be opportunistically + // lazy). List> overrideSteering = new ArrayList<>(modules.length); - // Enforce steering velocity limits. We do this by taking the derivative of steering angle at + // Enforce steering velocity limits. We do this by taking the derivative of + // steering angle at // the current angle, - // and then backing out the maximum interpolant between start and goal states. We remember the + // and then backing out the maximum interpolant between start and goal states. + // We remember the // minimum across all modules, since // that is the active constraint. final double max_theta_step = dt * limits.maxSteeringVelocity(); @@ -363,7 +389,8 @@ public class SwerveSetpointGenerator { } overrideSteering.add(Optional.empty()); if (epsilonEquals(prevSetpoint.moduleStates()[i].speedMetersPerSecond, 0.0)) { - // If module is stopped, we know that we will need to move straight to the final steering + // If module is stopped, we know that we will need to move straight to the final + // steering // angle, so limit based // purely on rotation in place. if (epsilonEquals(desiredModuleState[i].speedMetersPerSecond, 0.0)) { @@ -372,8 +399,7 @@ public class SwerveSetpointGenerator { continue; } - var necessaryRotation = - prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle); + var necessaryRotation = prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle); if (flipHeading(necessaryRotation)) { necessaryRotation = necessaryRotation.rotateBy(Rotation2d.fromRadians(Math.PI)); } @@ -403,16 +429,15 @@ public class SwerveSetpointGenerator { } final int kMaxIterations = 8; - double s = - findSteeringMaxS( - prev_vx[i], - prev_vy[i], - prev_heading[i].getRadians(), - desired_vx[i], - desired_vy[i], - desired_heading[i].getRadians(), - max_theta_step, - kMaxIterations); + double s = findSteeringMaxS( + prev_vx[i], + prev_vy[i], + prev_heading[i].getRadians(), + desired_vx[i], + desired_vy[i], + desired_heading[i].getRadians(), + max_theta_step, + kMaxIterations); min_s = Math.min(min_s, s); } @@ -424,49 +449,53 @@ public class SwerveSetpointGenerator { // No need to carry on. break; } - double vx_min_s = - min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i]; - double vy_min_s = - min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i]; - // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we + double vx_min_s = min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i]; + double vy_min_s = min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i]; + // Find the max s for this drive wheel. Search on the interval between 0 and + // min_s, because we // already know we can't go faster // than that. final int kMaxIterations = 10; - double s = min_s*findDriveMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step); - + double s = min_s * findDriveMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step); + // Limit the overall acceleration of this wheel - double s2 = min_s*findAccelerationMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step_2, kMaxIterations); + double s2 = min_s + * findAccelerationMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step_2, kMaxIterations); min_s = Math.min(Math.min(min_s, s), s2); } - if(centerOfMassHeight > 0.02){ - // Limit the acceleration in the x and y directions separately based on the center of mass. - // To make the torque on the robot 0, we can assume all of the mass is on the back wheel, where the front is the direction the robot is accelerating toward - // Torque is equal to the force times the component of the radius perpendicular to the force - // T = torque, m = mass, a = acceleration, g = gravity acceleration, x = distance from center to wheel + if (centerOfMassHeight > 0.02) { + // Limit the acceleration in the x and y directions separately based on the + // center of mass. + // To make the torque on the robot 0, we can assume all of the mass is on the + // back wheel, where the front is the direction the robot is accelerating toward + // Torque is equal to the force times the component of the radius perpendicular + // to the force + // T = torque, m = mass, a = acceleration, g = gravity acceleration, x = + // distance from center to wheel // T = mgx - mah = 0 // a = gx/h - double maxAccel = Constants.GRAVITY_ACCELERATION*(DriveConstants.TRACK_WIDTH/2)/centerOfMassHeight; + double maxAccel = Constants.GRAVITY_ACCELERATION * (DriveConstants.TRACK_WIDTH / 2) / centerOfMassHeight; // Limit based on this calculated value - // x and y are limited separately because, when tipping in a diagonal direction, the distance is longer + // x and y are limited separately because, when tipping in a diagonal direction, + // the distance is longer double xAccel = Math.abs(desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond) / dt; double yAccel = Math.abs(desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond) / dt; - if(!epsilonEquals(xAccel, 0)){ + if (!epsilonEquals(xAccel, 0)) { double s = maxAccel / xAccel; min_s = Math.min(min_s, s); } - if(!epsilonEquals(yAccel, 0)){ + if (!epsilonEquals(yAccel, 0)) { double s = maxAccel / yAccel; min_s = Math.min(min_s, s); } } - ChassisSpeeds retSpeeds = - new ChassisSpeeds( - prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx, - prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy, - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta); + ChassisSpeeds retSpeeds = new ChassisSpeeds( + prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx, + prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy, + prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta); var retStates = kinematics.toSwerveModuleStates(retSpeeds); for (int i = 0; i < modules.length; ++i) { final var maybeOverride = overrideSteering.get(i); @@ -476,9 +505,8 @@ public class SwerveSetpointGenerator { retStates[i].speedMetersPerSecond *= -1.0; } retStates[i].angle = override; - } - final var deltaRotation = - prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle); + } + final var deltaRotation = prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle); if (flipHeading(deltaRotation)) { retStates[i].angle = retStates[i].angle.rotateBy(Rotation2d.fromRadians(Math.PI)); retStates[i].speedMetersPerSecond *= -1.0; diff --git a/src/main/java/frc/robot/util/Vision/Vision.java b/src/main/java/frc/robot/util/Vision/Vision.java index d17bdd2..7e733ce 100644 --- a/src/main/java/frc/robot/util/Vision/Vision.java +++ b/src/main/java/frc/robot/util/Vision/Vision.java @@ -47,7 +47,7 @@ public class Vision { private NetworkTableEntry objectDistance; private NetworkTableEntry objectClass; private NetworkTableEntry cameraIndex; - + // A list of the cameras on the robot. private ArrayList cameras = new ArrayList<>(); @@ -78,16 +78,16 @@ public class Vision { // Sets the origin to the right side of the blue alliance wall FieldConstants.field.setOrigin(OriginPosition.kBlueAllianceWallRightSide); - if(VisionConstants.ENABLED){ + if (VisionConstants.ENABLED) { // Puts the cameras in an array list for (int i = 0; i < camList.size(); i++) { cameras.add(new VisionCamera(camList.get(i).getFirst(), camList.get(i).getSecond())); } - if(RobotBase.isSimulation()){ + if (RobotBase.isSimulation()) { visionSim = new VisionSystemSim("Vision"); visionSim.addAprilTags(FieldConstants.field); - for(VisionCamera c : cameras){ + for (VisionCamera c : cameras) { PhotonCameraSim cameraSim = new PhotonCameraSim(c.camera); cameraSim.enableDrawWireframe(true); cameraSim.prop.setAvgLatencyMs(30); @@ -99,20 +99,20 @@ public class Vision { Pose3d[] tags = new Pose3d[FieldConstants.field.getTags().size()]; for (int i = 0; i < FieldConstants.field.getTags().size(); i++) { - tags[i] = (FieldConstants.field.getTagPose(i+1).get()); + tags[i] = (FieldConstants.field.getTagPose(i + 1).get()); } if (!Constants.DISABLE_LOGGING) { Logger.recordOutput("AprilTags", tags); } } - /** * Get the horizontal offsets from the crosshair to the targets + * * @return An array of offsets in degrees */ - public double[] getHorizontalOffset(){ - if(!VisionConstants.OBJECT_DETECTION_ENABLED){ + public double[] getHorizontalOffset() { + if (!VisionConstants.OBJECT_DETECTION_ENABLED) { return new double[0]; } return xOffset.getDoubleArray(new double[0]); @@ -120,10 +120,11 @@ public class Vision { /** * Get the vertical offsets from the crosshair to the targets + * * @return An array of offsets in degrees */ - public double[] getVerticalOffset(){ - if(!VisionConstants.OBJECT_DETECTION_ENABLED){ + public double[] getVerticalOffset() { + if (!VisionConstants.OBJECT_DETECTION_ENABLED) { return new double[0]; } return yOffset.getDoubleArray(new double[0]); @@ -131,11 +132,12 @@ public class Vision { /** * Get the target distances + * * @return Distance in meters */ @SuppressWarnings("unused") - public double[] getDistance(){ - if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){ + public double[] getDistance() { + if (!VisionConstants.OBJECT_DETECTION_ENABLED || true) { return new double[0]; } return objectDistance.getDoubleArray(new double[0]); @@ -143,19 +145,21 @@ public class Vision { /** * Returns whether or not a valid object is detected + * * @return true or false */ - public boolean validObjectDetected(){ + public boolean validObjectDetected() { return getHorizontalOffset().length > 0; } /** * Returns what types of object are detected + * * @return The object types as a String array */ @SuppressWarnings("unused") - public String[] getDetectedObjectClass(){ - if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){ + public String[] getDetectedObjectClass() { + if (!VisionConstants.OBJECT_DETECTION_ENABLED || true) { return new String[0]; } return objectClass.getStringArray(new String[0]); @@ -163,11 +167,13 @@ public class Vision { /** * Gets the camera indices (which camera sees the object) - * @return The indices as a long array (method returns long array instead of int array) + * + * @return The indices as a long array (method returns long array instead of int + * array) */ @SuppressWarnings("unused") - public long[] getCameraIndex(){ - if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){ + public long[] getCameraIndex() { + if (!VisionConstants.OBJECT_DETECTION_ENABLED || true) { return new long[0]; } return cameraIndex.getIntegerArray(new long[0]); @@ -175,10 +181,11 @@ public class Vision { /** * Stores all of the detected objects in an array + * * @return The array of DetectedObjects */ - public DetectedObject[] getDetectedObjects(){ - if(!VisionConstants.OBJECT_DETECTION_ENABLED){ + public DetectedObject[] getDetectedObjects() { + if (!VisionConstants.OBJECT_DETECTION_ENABLED) { return new DetectedObject[0]; } double[] xOffset = getHorizontalOffset(); @@ -187,32 +194,37 @@ public class Vision { String[] objectClass = getDetectedObjectClass(); // long[] cameraIndex = getCameraIndex(); DetectedObject[] objects = new DetectedObject[Math.min(xOffset.length, yOffset.length)]; - for(int i = 0; i < objects.length; i++){ + for (int i = 0; i < objects.length; i++) { objects[i] = new DetectedObject( - Units.degreesToRadians(xOffset[i]), - -Units.degreesToRadians(yOffset[i]), - // distance[i], - objectClass[i], - // VisionConstants.OBJECT_DETECTION_CAMERAS.get((int)cameraIndex[i]).getSecond() - VisionConstants.OBJECT_DETECTION_CAMERAS.get(0) - ); + Units.degreesToRadians(xOffset[i]), + -Units.degreesToRadians(yOffset[i]), + // distance[i], + objectClass[i], + // VisionConstants.OBJECT_DETECTION_CAMERAS.get((int)cameraIndex[i]).getSecond() + VisionConstants.OBJECT_DETECTION_CAMERAS.get(0)); } return objects; } /** * Returns the closest game piece in front of the robot - * @param maxAngle The maximum angle between the angle to the object and the robot's heading or rotation to use, in radians - * @param relativeToVelocity Whether to compare the angle to the robot's heading or rotation, true for heading + * + * @param maxAngle The maximum angle between the angle to the object + * and the robot's heading or rotation to use, in + * radians + * @param relativeToVelocity Whether to compare the angle to the robot's heading + * or rotation, true for heading * @return The best DetectedObject */ - public DetectedObject getBestGamePiece(double maxAngle, boolean relativeToVelocity){ + public DetectedObject getBestGamePiece(double maxAngle, boolean relativeToVelocity) { DetectedObject[] objects = getDetectedObjects(); DetectedObject best = null; double closest = Double.POSITIVE_INFINITY; - for(DetectedObject object : objects){ + for (DetectedObject object : objects) { double dist = object.getDistance(); - if(object.isGamePiece() && Math.abs(relativeToVelocity ? object.getVelocityRelativeAngle() : object.getAngle()) < maxAngle && dist < closest){ + if (object.isGamePiece() + && Math.abs(relativeToVelocity ? object.getVelocityRelativeAngle() : object.getAngle()) < maxAngle + && dist < closest) { closest = dist; best = object; } @@ -222,96 +234,112 @@ public class Vision { /** * Gets the pose as a Pose2d using PhotonVision - * @param referencePoses The reference poses in order of preference, null poses will be skipped + * + * @param referencePoses The reference poses in order of preference, null poses + * will be skipped * @return The pose of the robot, or null if it can't see april tags */ - public Pose2d getPose2d(Pose2d... referencePoses){ + public Pose2d getPose2d(Pose2d... referencePoses) { Pose2d referencePose = new Pose2d(); - for (Pose2d checkReferencePose:referencePoses){ + for (Pose2d checkReferencePose : referencePoses) { if (checkReferencePose != null) { referencePose = checkReferencePose; break; } } ArrayList estimatedPoses = getEstimatedPoses(referencePose); - - if (estimatedPoses.size() == 0) return null; - if (estimatedPoses.size() == 1) return estimatedPoses.get(0).estimatedPose.toPose2d(); - + if (estimatedPoses.size() == 0) + return null; + + if (estimatedPoses.size() == 1) + return estimatedPoses.get(0).estimatedPose.toPose2d(); + if (estimatedPoses.size() == 2) { return new Pose2d( - estimatedPoses.get(0).estimatedPose.toPose2d().getTranslation() - .plus(estimatedPoses.get(1).estimatedPose.toPose2d().getTranslation()) - .div(2), - - new Rotation2d(MathUtils.modulusMidpoint( - estimatedPoses.get(0).estimatedPose.toPose2d().getRotation().getRadians(), - estimatedPoses.get(1).estimatedPose.toPose2d().getRotation().getRadians(), - -Math.PI, Math.PI) - ) - ); + estimatedPoses.get(0).estimatedPose.toPose2d().getTranslation() + .plus(estimatedPoses.get(1).estimatedPose.toPose2d().getTranslation()) + .div(2), + + new Rotation2d(MathUtils.modulusMidpoint( + estimatedPoses.get(0).estimatedPose.toPose2d().getRotation().getRadians(), + estimatedPoses.get(1).estimatedPose.toPose2d().getRotation().getRadians(), + -Math.PI, Math.PI))); } - - // The average translation is just the average of all of the translations (sum divided by total) - // Average angle is similar, except every step needs to use a modulus, since -π is the same angle as π - // This calculation is essentially newAverage = (oldAverage * valuesInOldAverage + nextValue) / newNumberOfValues + + // The average translation is just the average of all of the translations (sum + // divided by total) + // Average angle is similar, except every step needs to use a modulus, since -π + // is the same angle as π + // This calculation is essentially newAverage = (oldAverage * valuesInOldAverage + // + nextValue) / newNumberOfValues Translation2d translation = new Translation2d(); double angle = 0; - for(int i = 0; i < estimatedPoses.size(); i ++){ + for (int i = 0; i < estimatedPoses.size(); i++) { translation = translation.plus(estimatedPoses.get(i).estimatedPose.toPose2d().getTranslation()); - angle = MathUtils.modulusInterpolate(angle, estimatedPoses.get(i).estimatedPose.toPose2d().getRotation().getRadians(), 1.0/(i+1), -Math.PI, Math.PI); + angle = MathUtils.modulusInterpolate(angle, + estimatedPoses.get(i).estimatedPose.toPose2d().getRotation().getRadians(), 1.0 / (i + 1), -Math.PI, Math.PI); } return new Pose2d(translation.div(estimatedPoses.size()), new Rotation2d(angle)); } - public AprilTagFieldLayout getAprilTagFieldLayout(){ + public AprilTagFieldLayout getAprilTagFieldLayout() { return FieldConstants.field; } /** * Gets the pose of an april tag + * * @param id AprilTag id (1-8) * @return Pose3d of the AprilTag */ - public Pose3d getTagPose(int id){ - if(id < 1 || id > getAprilTagFieldLayout().getTags().size()){ - System.out.println("Tried to find the pose of april tag "+id); + public Pose3d getTagPose(int id) { + if (id < 1 || id > getAprilTagFieldLayout().getTags().size()) { + System.out.println("Tried to find the pose of april tag " + id); return null; } - return getAprilTagFieldLayout().getTags().get(id-1).pose; + return getAprilTagFieldLayout().getTags().get(id - 1).pose; } /** * Returns where it thinks the robot is - * @param referencePose The pose to use as a reference, usually the previous robot pose - * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time - * @return An array list of estimated poses, one for each camera that can see an april tag + * + * @param referencePose The pose to use as a reference, usually the previous + * robot pose + * @param yawFunction A unary operator that takes a timestamp and returns the + * yaw at that time + * @return An array list of estimated poses, one for each camera that can see an + * april tag */ public ArrayList getEstimatedPoses(Pose2d referencePose) { - return getEstimatedPoses(referencePose, ignoree->referencePose.getRotation().getRadians()); + return getEstimatedPoses(referencePose, ignoree -> referencePose.getRotation().getRadians()); } /** * Returns where it thinks the robot is - * @param referencePose The pose to use as a reference, usually the previous robot pose - * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time - * @return An array list of estimated poses, one for each camera that can see an april tag + * + * @param referencePose The pose to use as a reference, usually the previous + * robot pose + * @param yawFunction A unary operator that takes a timestamp and returns the + * yaw at that time + * @return An array list of estimated poses, one for each camera that can see an + * april tag */ public ArrayList getEstimatedPoses(Pose2d referencePose, DoubleUnaryOperator yawFunction) { ArrayList estimatedPoses = new ArrayList<>(); for (int i = 0; i < cameras.size(); i++) { - if(VisionConstants.USE_MANUAL_CALCULATIONS){ - for(EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(yawFunction)){ - if(pose != null){ + if (VisionConstants.USE_MANUAL_CALCULATIONS) { + for (EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(yawFunction)) { + if (pose != null) { estimatedPoses.add(pose); } } - }else{ - for(EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(referencePose)){ + } else { + for (EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(referencePose)) { // If the camera can see an april tag that exists, add it to the array list - // April tags that don't exist might return a result that is present but doesn't have a pose + // April tags that don't exist might return a result that is present but doesn't + // have a pose if (pose.estimatedPose != null) { estimatedPoses.add(pose); @@ -319,33 +347,37 @@ public class Vision { } } } - if(estimatedPoses.size() > 1){ + if (estimatedPoses.size() > 1) { Translation2d average = new Translation2d(); - for(EstimatedRobotPose pose : estimatedPoses){ + for (EstimatedRobotPose pose : estimatedPoses) { average = average.plus(pose.estimatedPose.getTranslation().toTranslation2d()); } average = average.div(estimatedPoses.size()); - for(int i = estimatedPoses.size()-1; i>=0; i--){ - if(estimatedPoses.get(i).estimatedPose.getTranslation().toTranslation2d().getDistance(average) > VisionConstants.MAX_POSE_DIFFERENCE/2){ + for (int i = estimatedPoses.size() - 1; i >= 0; i--) { + if (estimatedPoses.get(i).estimatedPose.getTranslation().toTranslation2d() + .getDistance(average) > VisionConstants.MAX_POSE_DIFFERENCE / 2) { estimatedPoses.remove(i); } } } - return estimatedPoses; + return estimatedPoses; } /** * Updates the robot's odometry with vision + * * @param poseEstimator The pose estimator to update - * @param yawFunction A function that returns the yaw as a double given the timestamp - * @param slipped True if the wheels have slipped, false otherwise + * @param yawFunction A function that returns the yaw as a double given the + * timestamp + * @param slipped True if the wheels have slipped, false otherwise * @return The list of estimated robot poses from vision */ - public ArrayList updateOdometry(SwerveDrivePoseEstimator poseEstimator, DoubleUnaryOperator yawFunction, boolean slipped){ + public ArrayList updateOdometry(SwerveDrivePoseEstimator poseEstimator, + DoubleUnaryOperator yawFunction, boolean slipped) { // Simulate vision // 2 ifs to avoid warning - if(VisionConstants.ENABLED_SIM){ - if(RobotBase.isSimulation()){ + if (VisionConstants.ENABLED_SIM) { + if (RobotBase.isSimulation()) { visionSim.update(poseEstimator.getEstimatedPosition()); } } @@ -356,15 +388,16 @@ public class Vision { ArrayList estimatedPoses = getEstimatedPoses(poseEstimator.getEstimatedPosition(), yawFunction); for (EstimatedRobotPose estimatedPose : estimatedPoses) { // Continue if this pose doesn't exist - if(estimatedPose.timestampSeconds < 0 || !onField(estimatedPose.estimatedPose.toPose2d()) || Timer.getFPGATimestamp() < estimatedPose.timestampSeconds || Timer.getFPGATimestamp() > estimatedPose.timestampSeconds + 1){ + if (estimatedPose.timestampSeconds < 0 || !onField(estimatedPose.estimatedPose.toPose2d()) + || Timer.getFPGATimestamp() < estimatedPose.timestampSeconds + || Timer.getFPGATimestamp() > estimatedPose.timestampSeconds + 1) { continue; } poseEstimator.addVisionMeasurement( - estimatedPose.estimatedPose.toPose2d(), - estimatedPose.timestampSeconds, - slipped ? VisionConstants.VISION_STD_DEVS_2 : VisionConstants.VISION_STD_DEVS - ); + estimatedPose.estimatedPose.toPose2d(), + estimatedPose.timestampSeconds, + slipped ? VisionConstants.VISION_STD_DEVS_2 : VisionConstants.VISION_STD_DEVS); sawTag = true; } return estimatedPoses; @@ -373,47 +406,52 @@ public class Vision { /** * Updates each camera's inputs for logging */ - public void updateInputs(){ - for(VisionCamera c : cameras){ + public void updateInputs() { + for (VisionCamera c : cameras) { c.updateInputs(); } } /** * If vision saw any April tags last frame + * * @return If vision saw an April tag last frame */ - public boolean canSeeTag(){ + public boolean canSeeTag() { return sawTag; } /** * Enable or disable a single camera - * @param index The camera index + * + * @param index The camera index * @param enabled If it should be enabled or disabled */ - public void enableCamera(int index, boolean enabled){ - try{ + public void enableCamera(int index, boolean enabled) { + try { cameras.get(index).enable(enabled); - }catch(IndexOutOfBoundsException e){ - DriverStation.reportWarning("Camera index "+index+" is out of bounds", false); + } catch (IndexOutOfBoundsException e) { + DriverStation.reportWarning("Camera index " + index + " is out of bounds", false); } } + /** * Sets the cameras to only use April tag in the specified array + * * @param ids The ids of the tags to use, null or empty array to use all */ - public void onlyUse(int[] ids){ + public void onlyUse(int[] ids) { onlyUse = ids; } /** * Checks if one or more cameras are disconnected + * * @return true if at least one camera is disconnected, false otherwise */ - public boolean oneCameraDisconnected(){ - for(VisionCamera camera : cameras){ - if(!camera.inputs.connected){ + public boolean oneCameraDisconnected() { + for (VisionCamera camera : cameras) { + if (!camera.inputs.connected) { return true; } } @@ -422,22 +460,29 @@ public class Vision { /** * Checks if a pose is on the field + * * @param pose The pose to check * @return If the pose is on the field */ - public static boolean onField(Pose2d pose){ - return pose!=null && pose.getX()>0 && pose.getX()0 && pose.getY() 0 && pose.getX() < FieldConstants.field.getFieldLength() && pose.getY() > 0 + && pose.getY() < FieldConstants.field.getFieldWidth(); } /** * Checks if a pose is on or near the field + * * @param pose The pose to check - * @return If the pose is within an area with twice the length and width of the field + * @return If the pose is within an area with twice the length and width of the + * field */ - public static boolean nearField(Pose2d pose){ - return pose!=null && pose.getX()>-FieldConstants.field.getFieldLength()/2 && pose.getX()-FieldConstants.field.getFieldWidth()/2 && pose.getY() -FieldConstants.field.getFieldLength() / 2 + && pose.getX() < FieldConstants.field.getFieldLength() * 1.5 + && pose.getY() > -FieldConstants.field.getFieldWidth() / 2 + && pose.getY() < FieldConstants.field.getFieldWidth() * 1.5; } - + private class VisionCamera implements VisionIO { private PhotonCamera camera; private PhotonPoseEstimator photonPoseEstimator; @@ -445,88 +490,99 @@ public class Vision { private double lastTimestamp = 0; private boolean enabled = true; private final VisionIOInputs inputs = new VisionIOInputs(); - + /** * Stores information about a camera + * * @param cameraName The name of the camera on PhotonVision * @param robotToCam The transformation from the robot to the camera */ public VisionCamera(String cameraName, Transform3d robotToCam) { camera = new PhotonCamera(cameraName); photonPoseEstimator = new PhotonPoseEstimator( - FieldConstants.field, - robotToCam - ); + FieldConstants.field, + robotToCam); lastPose = null; } - + /** * Gets the estimated poses from the camera - * @param referencePose Pose to use for reference, usually the previous estimated robot pose + * + * @param referencePose Pose to use for reference, usually the previous + * estimated robot pose * @return estimated robot poses */ public ArrayList getEstimatedPose(Pose2d referencePose) { ArrayList list = new ArrayList<>(); - if(!enabled){ + if (!enabled) { return list; } - for(PhotonPipelineResult cameraResult : inputs.results){ - if(!cameraResult.hasTargets() || cameraResult.getTimestampSeconds()<0){ - continue; + for (PhotonPipelineResult cameraResult : inputs.results) { + if (!cameraResult.hasTargets() || cameraResult.getTimestampSeconds() < 0) { + continue; } - - // if there is a target detected and the timestamp exists, + + // if there is a target detected and the timestamp exists, // check the ambiguity isn't too high List targetsUsed = cameraResult.targets; - for (int i = targetsUsed.size()-1; i >= 0; i--) { - // Remove it from the list if it should not be used or if it has too high of an ambiguity - if(!useTag(targetsUsed.get(i).getFiducialId()) || targetsUsed.get(i).getPoseAmbiguity() > VisionConstants.HIGHEST_AMBIGUITY || targetsUsed.get(i).bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE){ + for (int i = targetsUsed.size() - 1; i >= 0; i--) { + // Remove it from the list if it should not be used or if it has too high of an + // ambiguity + if (!useTag(targetsUsed.get(i).getFiducialId()) + || targetsUsed.get(i).getPoseAmbiguity() > VisionConstants.HIGHEST_AMBIGUITY + || targetsUsed.get(i).bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE) { targetsUsed.remove(i); } } - // If there are no targets, the timestamp doesn't exist, or there there is only 1 tag and the constant is set to only use 2 tags, continue - if(targetsUsed.size() == 0 || cameraResult.getTimestampSeconds()<0 || targetsUsed.size()==1 && VisionConstants.ONLY_USE_2_TAGS){ + // If there are no targets, the timestamp doesn't exist, or there there is only + // 1 tag and the constant is set to only use 2 tags, continue + if (targetsUsed.size() == 0 || cameraResult.getTimestampSeconds() < 0 + || targetsUsed.size() == 1 && VisionConstants.ONLY_USE_2_TAGS) { continue; } // Set strategy to single tag if there is only 1 good tag and update - PhotonPoseEstimator.PoseStrategy poseStrategy = targetsUsed.size() > 1 ? VisionConstants.POSE_STRATEGY : VisionConstants.MULTITAG_FALLBACK_STRATEGY; - Optional pose; - switch (poseStrategy) { - case AVERAGE_BEST_TARGETS: - pose = photonPoseEstimator.estimateAverageBestTargetsPose(cameraResult); - break; - case CLOSEST_TO_CAMERA_HEIGHT: - pose = photonPoseEstimator.estimateClosestToCameraHeightPose(cameraResult); - break; - case CLOSEST_TO_REFERENCE_POSE: - pose = photonPoseEstimator.estimateClosestToReferencePose(cameraResult, new Pose3d(referencePose)); - break; - case LOWEST_AMBIGUITY: - pose = photonPoseEstimator.estimateLowestAmbiguityPose(cameraResult); - break; - case MULTI_TAG_PNP_ON_COPROCESSOR: - pose = photonPoseEstimator.estimateCoprocMultiTagPose(cameraResult); - break; - case PNP_DISTANCE_TRIG_SOLVE: - pose = photonPoseEstimator.estimatePnpDistanceTrigSolvePose(cameraResult); - break; - case CLOSEST_TO_LAST_POSE: - case CONSTRAINED_SOLVEPNP: - case MULTI_TAG_PNP_ON_RIO: - default: - throw new RuntimeException("Pose estimation method " + poseStrategy.toString() + " is not supported."); - } - - if(pose.isPresent() && pose.get()!=null && onField(pose.get().estimatedPose.toPose2d())){ + PhotonPoseEstimator.PoseStrategy poseStrategy = targetsUsed.size() > 1 ? VisionConstants.POSE_STRATEGY + : VisionConstants.MULTITAG_FALLBACK_STRATEGY; + Optional pose; + switch (poseStrategy) { + case AVERAGE_BEST_TARGETS: + pose = photonPoseEstimator.estimateAverageBestTargetsPose(cameraResult); + break; + case CLOSEST_TO_CAMERA_HEIGHT: + pose = photonPoseEstimator.estimateClosestToCameraHeightPose(cameraResult); + break; + case CLOSEST_TO_REFERENCE_POSE: + pose = photonPoseEstimator.estimateClosestToReferencePose(cameraResult, new Pose3d(referencePose)); + break; + case LOWEST_AMBIGUITY: + pose = photonPoseEstimator.estimateLowestAmbiguityPose(cameraResult); + break; + case MULTI_TAG_PNP_ON_COPROCESSOR: + pose = photonPoseEstimator.estimateCoprocMultiTagPose(cameraResult); + break; + case PNP_DISTANCE_TRIG_SOLVE: + pose = photonPoseEstimator.estimatePnpDistanceTrigSolvePose(cameraResult); + break; + case CLOSEST_TO_LAST_POSE: + case CONSTRAINED_SOLVEPNP: + case MULTI_TAG_PNP_ON_RIO: + default: + throw new RuntimeException("Pose estimation method " + poseStrategy.toString() + " is not supported."); + } + + if (pose.isPresent() && pose.get() != null && onField(pose.get().estimatedPose.toPose2d())) { double timestamp = cameraResult.getTimestampSeconds(); // If the pose moved too much, don't use it - if(lastPose==null || lastPose.getTranslation().getDistance(pose.get().estimatedPose.toPose2d().getTranslation()) > DriveConstants.MAX_SPEED*1.25*(timestamp-lastTimestamp) || timestamp < lastTimestamp){ + if (lastPose == null + || lastPose.getTranslation().getDistance(pose.get().estimatedPose.toPose2d() + .getTranslation()) > DriveConstants.MAX_SPEED * 1.25 * (timestamp - lastTimestamp) + || timestamp < lastTimestamp) { lastPose = pose.get().estimatedPose.toPose2d(); lastTimestamp = timestamp; continue; @@ -541,117 +597,126 @@ public class Vision { return list; } - /** - * Updates the VisionIOInputs object with the results from PhotonVision for logging - */ - @Override + /** + * Updates the VisionIOInputs object with the results from PhotonVision for + * logging + */ + @Override public void updateInputs() { inputs.connected = camera.isConnected(); inputs.results = camera.getAllUnreadResults(); - Logger.processInputs("Vision/"+camera.getName(), inputs); + Logger.processInputs("Vision/" + camera.getName(), inputs); // Mechanical Advantage's vision logging // // Read new camera observations // Set tagIds = new HashSet<>(); // List poseObservations = new LinkedList<>(); // for (var result : camera.getAllUnreadResults()) { - // // Update latest target observation - // if (result.hasTargets()) { - // inputs.latestTargetObservation = - // new TargetObservation( - // Rotation2d.fromDegrees(result.getBestTarget().getYaw()), - // Rotation2d.fromDegrees(result.getBestTarget().getPitch())); - // } else { - // inputs.latestTargetObservation = new TargetObservation(new Rotation2d(), new Rotation2d()); - // } + // // Update latest target observation + // if (result.hasTargets()) { + // inputs.latestTargetObservation = + // new TargetObservation( + // Rotation2d.fromDegrees(result.getBestTarget().getYaw()), + // Rotation2d.fromDegrees(result.getBestTarget().getPitch())); + // } else { + // inputs.latestTargetObservation = new TargetObservation(new Rotation2d(), new + // Rotation2d()); + // } + // } + // // Add pose observation + // if (result.multitagResult.isPresent()) { // Multitag result + // var multitagResult = result.multitagResult.get(); + + // // Calculate robot pose + // Transform3d fieldToCamera = multitagResult.estimatedPose.best; + // Transform3d fieldToRobot = + // fieldToCamera.plus(VisionConstants.APRIL_TAG_CAMERAS.get(0).getSecond().inverse()); + // Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), + // fieldToRobot.getRotation()); + + // // Calculate average tag distance + // double totalTagDistance = 0.0; + // for (var target : result.targets) { + // totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm(); + // } + + // // Add tag IDs + // tagIds.addAll(multitagResult.fiducialIDsUsed); + + // // Add observation + // poseObservations.add( + // new PoseObservation( + // result.getTimestampSeconds(), // Timestamp + // robotPose, // 3D pose estimate + // multitagResult.estimatedPose.ambiguity, // Ambiguity + // multitagResult.fiducialIDsUsed.size(), // Tag count + // totalTagDistance / result.targets.size(), // Average tag distance + // PoseObservationType.PHOTONVISION)); // Observation type + + // } else if (!result.targets.isEmpty()) { // Single tag result + // var target = result.targets.get(0); + + // // Calculate robot pose + // var tagPose = aprilTagLayout.getTagPose(target.fiducialId); + // if (tagPose.isPresent()) { + // Transform3d fieldToTarget = + // new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); + // Transform3d cameraToTarget = target.bestCameraToTarget; + // Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); + // Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + // Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), + // fieldToRobot.getRotation()); + + // // Add tag ID + // tagIds.add((short) target.fiducialId); + + // // Add observation + // poseObservations.add( + // new PoseObservation( + // result.getTimestampSeconds(), // Timestamp + // robotPose, // 3D pose estimate + // target.poseAmbiguity, // Ambiguity + // 1, // Tag count + // cameraToTarget.getTranslation().getNorm(), // Average tag distance + // PoseObservationType.PHOTONVISION)); // Observation type + // } // } - // // Add pose observation - // if (result.multitagResult.isPresent()) { // Multitag result - // var multitagResult = result.multitagResult.get(); - - // // Calculate robot pose - // Transform3d fieldToCamera = multitagResult.estimatedPose.best; - // Transform3d fieldToRobot = fieldToCamera.plus(VisionConstants.APRIL_TAG_CAMERAS.get(0).getSecond().inverse()); - // Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); - - // // Calculate average tag distance - // double totalTagDistance = 0.0; - // for (var target : result.targets) { - // totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm(); - // } - - // // Add tag IDs - // tagIds.addAll(multitagResult.fiducialIDsUsed); - - // // Add observation - // poseObservations.add( - // new PoseObservation( - // result.getTimestampSeconds(), // Timestamp - // robotPose, // 3D pose estimate - // multitagResult.estimatedPose.ambiguity, // Ambiguity - // multitagResult.fiducialIDsUsed.size(), // Tag count - // totalTagDistance / result.targets.size(), // Average tag distance - // PoseObservationType.PHOTONVISION)); // Observation type - - // } else if (!result.targets.isEmpty()) { // Single tag result - // var target = result.targets.get(0); - - // // Calculate robot pose - // var tagPose = aprilTagLayout.getTagPose(target.fiducialId); - // if (tagPose.isPresent()) { - // Transform3d fieldToTarget = - // new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); - // Transform3d cameraToTarget = target.bestCameraToTarget; - // Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); - // Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); - // Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); - - // // Add tag ID - // tagIds.add((short) target.fiducialId); - - // // Add observation - // poseObservations.add( - // new PoseObservation( - // result.getTimestampSeconds(), // Timestamp - // robotPose, // 3D pose estimate - // target.poseAmbiguity, // Ambiguity - // 1, // Tag count - // cameraToTarget.getTranslation().getNorm(), // Average tag distance - // PoseObservationType.PHOTONVISION)); // Observation type - // } - // } // } } - + /** * Gets the pose using manual calculations - * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time + * + * @param yawFunction A unary operator that takes a timestamp and returns the + * yaw at that time * @return A list of estimated poses as EstimatedRobotPoses */ - public ArrayList getEstimatedPose(DoubleUnaryOperator yawFunction){ + public ArrayList getEstimatedPose(DoubleUnaryOperator yawFunction) { ArrayList list = new ArrayList<>(); // Do nothing if this camera is disabled - if(!enabled){ + if (!enabled) { return list; } // The latest camera results - for(PhotonPipelineResult result : inputs.results){ + for (PhotonPipelineResult result : inputs.results) { // TODO: This could be improved by averaging all targets instead of only using 1 // Continue if the target doesn't exist or it should be ignored - if (!result.hasTargets()) continue; + if (!result.hasTargets()) + continue; // Gets the best target to use for the calculations PhotonTrackedTarget target = result.getBestTarget(); // I don't know why this would happen, but keep it in just in case - if(target==null){ + if (target == null) { continue; } // Continue if the id is too high or too low int id = target.getFiducialId(); - if(!useTag(id) || target.bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE || target.poseAmbiguity > VisionConstants.HIGHEST_AMBIGUITY){ + if (!useTag(id) || target.bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE + || target.poseAmbiguity > VisionConstants.HIGHEST_AMBIGUITY) { continue; } // Stores target pose and robot to camera transformation for easy access later @@ -661,59 +726,62 @@ public class Vision { double timestamp = result.getTimestampSeconds(); double yaw = yawFunction.applyAsDouble(timestamp); - // Get the tag position relative to the robot, assuming the robot is on the ground + // Get the tag position relative to the robot, assuming the robot is on the + // ground Translation3d translation = target.getBestCameraToTarget().getTranslation() - .rotateBy(robotToCamera.getRotation()); - translation = translation//.times((targetPose.getZ()-robotToCamera.getZ())/translation.getZ()) - .plus(robotToCamera.getTranslation()) - .rotateBy(new Rotation3d(0, 0, yaw)) - - // Invert it to get the robot position relative to the April tag - // Multiply by a constant. I don't know why this works, but it was consistently 10% off in 2023 Fall Semester - .times(-VisionConstants.DISTANCE_SCALE) - // Get the field relative robot pose - .plus(targetPose.getTranslation()); - try{ + .rotateBy(robotToCamera.getRotation()); + translation = translation// .times((targetPose.getZ()-robotToCamera.getZ())/translation.getZ()) + .plus(robotToCamera.getTranslation()) + .rotateBy(new Rotation3d(0, 0, yaw)) + + // Invert it to get the robot position relative to the April tag + // Multiply by a constant. I don't know why this works, but it was consistently + // 10% off in 2023 Fall Semester + .times(-VisionConstants.DISTANCE_SCALE) + // Get the field relative robot pose + .plus(targetPose.getTranslation()); + try { // Adds an EstimatedRobotPose list.add(new EstimatedRobotPose( - new Pose3d(translation.getX(), translation.getY(), 0, new Rotation3d(0, 0, yaw)), - timestamp, - List.of(target), - VisionConstants.POSE_STRATEGY - )); - }catch(Exception e){ + new Pose3d(translation.getX(), translation.getY(), 0, new Rotation3d(0, 0, yaw)), + timestamp, + List.of(target), + VisionConstants.POSE_STRATEGY)); + } catch (Exception e) { DriverStation.reportError("Error creating EstimatedRobotPose", true); } } return list; } - public boolean useTag(int id){ + public boolean useTag(int id) { // Never use tags that don't exist - if(id <= 0 || id > FieldConstants.field.getTags().size()){ + if (id <= 0 || id > FieldConstants.field.getTags().size()) { return false; } // Return false if it is in the list of tags to ignore - for(int id2 : VisionConstants.TAGS_TO_IGNORE){ - if(id == id2){ + for (int id2 : VisionConstants.TAGS_TO_IGNORE) { + if (id == id2) { return false; } } // If it's in the array to only use and not in the array to ignore, return true - for(int j = 0; onlyUse != null && j < onlyUse.length; j++){ - if(id == onlyUse[j]){ + for (int j = 0; onlyUse != null && j < onlyUse.length; j++) { + if (id == onlyUse[j]) { return true; } } - // If it isn't in the array to only use, only reutrn true if the array is empty/null + // If it isn't in the array to only use, only reutrn true if the array is + // empty/null return onlyUse == null || onlyUse.length == 0; } /** * Enables or disables this camera + * * @param enable If it should be enabled or disabled */ - public void enable(boolean enable){ + public void enable(boolean enable) { enabled = enable; } } -- 2.39.5