diff --git a/src/main/java/frc/lib/math/SeparatingAxis.java b/src/main/java/frc/lib/math/SeparatingAxis.java index 235cce2d..f037c272 100644 --- a/src/main/java/frc/lib/math/SeparatingAxis.java +++ b/src/main/java/frc/lib/math/SeparatingAxis.java @@ -93,6 +93,9 @@ public static boolean solve(ConvexShape shape1, ConvexShape shape2, Penetration Translation2d c1 = shape1.getCenter(); Translation2d c2 = shape2.getCenter(); Translation2d cToc = c1.minus(c2); + if (n == null) { + return false; + } if (n.dot(cToc) < 0) { n = n.unaryMinus(); } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index e5ca3b02..ac7db468 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -1,6 +1,8 @@ package frc.lib.util.swerve; +import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -109,4 +111,20 @@ public SwerveModulePosition getPosition() { Constants.Swerve.wheelCircumference), Rotation2d.fromRotations(inputs.angleMotorSelectedPosition.in(Rotations))); } + + /** Run motor at given voltage with angle motor pointing forward */ + public void runCharacterization(double output) { + io.setDriveMotorPower(output); + io.setAngleMotor(0.0); + } + + /** Returns the module position in radians. */ + public double getWheelRadiusCharacterizationPosition() { + return inputs.driveMotorSelectedPosition.in(Radians); + } + + /** Returns the module velocity in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + return inputs.driveMotorSelectedSensorVelocity.in(RotationsPerSecond); + } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3f608a8d..eb05ab67 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -152,6 +152,16 @@ public static final class Climb { * Swerve Constants */ public static final class Swerve { + + /** Constants for sysid routines */ + public static final class Characterization { + public static final double ffStartDelay = 2.0; // Secs + public static final double ffRampRate = 0.1; // Volts/Sec + public static final double wheelRadiusMaxVelocity = 0.25; // Rad/Sec + public static final double wheelRadiusRampRate = 0.05; // Rad/Sec^2 + } + + public static final double AUTO_ROTATION_KP = 5.0; public static final double AUTO_ROTATION_KI = 0.0; public static final double AUTO_ROTATION_KD = 0.0; @@ -267,7 +277,7 @@ public static class ModuleConstants { public static final double wheelCoeffFriction = 1.2; public static final MomentOfInertia angleMomentOfInertia = KilogramSquareMeters.of(0.02); - public static final Distance wheelRadius = Inches.of(3.87 / 2); + public static final Distance wheelRadius = Inches.of(2.764); public static final Current slipCurrent = Amps.of(120.0); public static final Current driveCurrentLimit = Amps.of(35.0); @@ -284,8 +294,8 @@ public static class ModuleConstants { public static final LinearAcceleration maxDriveRate = MetersPerSecondPerSecond.of(50.0); - public static final double ffkS = 1.0; - public static final double ffkV = 1.51; + public static final double ffkS = .194; + public static final double ffkV = 1.001; public static final double ffkT = 1.0 / driveMotor.KtNMPerAmp; public static final double ffkA = 0.27; public static final double drivekP = 0.12; @@ -434,7 +444,7 @@ public static final class CoralScoringConstants { * MoveToPos constants. */ public static class SwerveTransformPID { - public static final double PID_XKP = 1.0; + public static final double PID_XKP = 4.2; public static final double PID_XKI = 0.0; public static final double PID_XKD = 0.0; public static final double PID_TKP = 2.5; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 114bfb11..cb51f2dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,6 +1,8 @@ package frc.robot; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; import java.util.ArrayList; import java.util.HashSet; @@ -13,6 +15,7 @@ import choreo.auto.AutoFactory; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.DriverStation; @@ -32,6 +35,7 @@ import frc.lib.util.viz.FieldViz; import frc.lib.util.viz.Viz2025; import frc.robot.Robot.RobotRunType; +import frc.robot.commands.MoveToPose; import frc.robot.subsystems.LEDs; import frc.robot.subsystems.algaewrist.AlgaeWrist; import frc.robot.subsystems.algaewrist.AlgaeWristIO; @@ -332,6 +336,15 @@ private void setupPitController() { .onFalse(wrist.runVolts(() -> 0.0)); // driver.a().whileTrue( // elevator.moveTo(() -> Inches.of(SmartDashboard.getNumber("elevatorTargetHeight", 20)))); + + pitController.povDown().whileTrue(swerve.feedforwardCharacterization()) + .onFalse(swerve.stop()); + pitController.povUp().whileTrue(swerve.wheelRadiusCharacterization()) + .onFalse(swerve.stop()); + pitController.povLeft().whileTrue(new MoveToPose(swerve, () -> { + return swerve.getPose() + .plus(new Transform2d(Inches.of(24), Meters.of(0), Rotation2d.kZero)); + }, () -> 200.0, false, 0, 0)).onFalse(swerve.stop()); } private void configureTriggerBindings() { diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 3e992b3d..76564ded 100755 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -96,8 +96,9 @@ public Pose2d getGlobalPoseEstimate() { private final Circle stdDevGlobalCircle = new Circle("State/GlobalEstimateStdDev", new Translation2d(), 0); - private final Circle stdDevLocalCircle = - new Circle("State/LocalEstimateStdDev", new Translation2d(), 0); + private final Circle[] stdDevLocalCircle = + new Circle[] {new Circle("State/LocalEstimateStdDev0", new Translation2d(), 0), + new Circle("State/LocalEstimateStdDev1", new Translation2d(), 0)}; private void addVisionObservation(Pose3d cameraPose, Pose3d robotPose, double timestamp, Vector baseUncertainty, List targets, String prefix, @@ -157,18 +158,18 @@ public void addVisionObservation(PhotonPipelineResult result, Transform3d robotT Constants.StateEstimator.globalVisionTrust.getAsDouble(), Constants.StateEstimator.globalVisionTrustRotation.getAsDouble()), result.getTargets(), "Global", true, stdDevGlobalCircle); - } - if (whichCamera == 1) { + } else { for (var target : result.targets) { double dist = target.getBestCameraToTarget().getTranslation().toTranslation2d().getNorm(); - if (dist > Units.inchesToMeters(36)) { + if (dist > Units.feetToMeters(6)) { continue; } - localCircle.setRadius(dist - Constants.Vision.cameras[whichCamera].offset()); - localCircle.setCenter(Constants.Vision.fieldLayout.getTagPose(target.fiducialId) - .get().getTranslation().toTranslation2d()); - localCircle.draw(); + localCircle[whichCamera] + .setRadius(dist - Constants.Vision.cameras[whichCamera].offset()); + localCircle[whichCamera].setCenter(Constants.Vision.fieldLayout + .getTagPose(target.fiducialId).get().getTranslation().toTranslation2d()); + localCircle[whichCamera].draw(); Optional maybeRobotYaw = sampleRotationAt(result.getTimestampSeconds()); Rotation2d robotYaw; if (maybeRobotYaw.isPresent()) { @@ -176,29 +177,40 @@ public void addVisionObservation(PhotonPipelineResult result, Transform3d robotT } else { continue; } - Rotation2d yaw = Rotation2d.fromDegrees(robotYaw.getDegrees() - target.getYaw() - + 180 + Units.radiansToDegrees(robotToCamera.getRotation().getZ())); - xCircle.setCenter(localCircle.getVertex(yaw)); - xCircle.draw(); + Rotation2d yaw; + if (whichCamera == 0) { + yaw = Rotation2d.fromDegrees(robotYaw.getDegrees() + target.getYaw() + + Units.radiansToDegrees(robotToCamera.getRotation().getZ()) + 180); + } else { + yaw = Rotation2d.fromDegrees(robotYaw.getDegrees() - target.getYaw() + + Units.radiansToDegrees(robotToCamera.getRotation().getZ()) + 180); + } + xCircle[whichCamera].setCenter(localCircle[whichCamera].getVertex(yaw)); + xCircle[whichCamera].draw(); Pose2d robotPose2d = new Pose2d( - xCircle.getCenter() + xCircle[whichCamera].getCenter() .minus(robotToCamera.getTranslation().toTranslation2d().rotateBy(robotYaw)), robotYaw); Pose3d robotPose = new Pose3d(robotPose2d); Pose3d cameraPose = robotPose.plus(robotToCamera); addVisionObservation(cameraPose, robotPose, result.getTimestampSeconds(), - VecBuilder.fill(Constants.StateEstimator.localVisionTrust.getAsDouble(), - Constants.StateEstimator.localVisionTrust.getAsDouble(), + VecBuilder.fill( + Constants.StateEstimator.localVisionTrust.getAsDouble() + * Math.max(dist, 0.5), + Constants.StateEstimator.localVisionTrust.getAsDouble() + * Math.max(dist, 0.5), Double.POSITIVE_INFINITY), - result.getTargets(), "Local", false, stdDevLocalCircle); + result.getTargets(), "Local", false, stdDevLocalCircle[whichCamera]); } } } - private final Circle localCircle = - new Circle("State/LocalEstimationDistance", new Translation2d(), 0); - private final Circle xCircle = - new Circle("State/LocalEstimationPose", new Translation2d(), Units.inchesToMeters(2)); + private final Circle[] localCircle = + new Circle[] {new Circle("State/LocalEstimationDistance0", new Translation2d(), 0), + new Circle("State/LocalEstimationDistance1", new Translation2d(), 0)}; + private final Circle[] xCircle = new Circle[] { + new Circle("State/LocalEstimationPose0", new Translation2d(), Units.inchesToMeters(2)), + new Circle("State/LocalEstimationPose1", new Translation2d(), Units.inchesToMeters(2))}; /** * Add information from swerve drive. @@ -212,8 +224,10 @@ public void addSwerveObservation(SwerveModulePosition[] positions, Rotation2d gy Stream.of(positions).map(x -> x.angle).toArray(this::swerveRotationsArray)); stdDevGlobalCircle.setCenter(getGlobalPoseEstimate().getTranslation()); stdDevGlobalCircle.setRadius(stdDevGlobalCircle.getRadius() + 0.01); - stdDevLocalCircle.setCenter(new Translation2d()); - stdDevLocalCircle.setRadius(0.0); + stdDevLocalCircle[0].setCenter(new Translation2d()); + stdDevLocalCircle[0].setRadius(0.0); + stdDevLocalCircle[1].setCenter(new Translation2d()); + stdDevLocalCircle[1].setRadius(0.0); } private Rotation2d[] swerveRotations = new Rotation2d[4]; diff --git a/src/main/java/frc/robot/commands/MoveAndAvoidReef.java b/src/main/java/frc/robot/commands/MoveAndAvoidReef.java index 8510a92d..a8145f68 100755 --- a/src/main/java/frc/robot/commands/MoveAndAvoidReef.java +++ b/src/main/java/frc/robot/commands/MoveAndAvoidReef.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj2.command.Command; @@ -124,13 +125,32 @@ public void initialize() { if (flipForRed) { pose2d = AllianceFlipUtil.apply(pose2d); } + prev = pose2d; } + private final TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints( + Constants.SwerveTransformPID.MAX_VELOCITY, Constants.SwerveTransformPID.MAX_ACCELERATION)); + + private Pose2d prev; + @Override public void execute() { - // draw(); + double speed = pose2d.minus(prev).getTranslation().getNorm(); + double dist = swerve.state.getGlobalPoseEstimate().getTranslation() + .minus(pose2d.getTranslation()).getNorm(); + double maxSpeed = Math.abs(profile.calculate(0.02, new TrapezoidProfile.State(dist, speed), + new TrapezoidProfile.State(0, 0)).velocity); + double nominalSpeed = maxSpeedSupplier.getAsDouble(); + if (maxSpeed > speed) { + maxSpeed = nominalSpeed; + } + Logger.recordOutput("MoveAndAvoidReef/speed", speed); + Logger.recordOutput("MoveAndAvoidReef/dist", dist); + Logger.recordOutput("MoveAndAvoidReef/maxSpeed", maxSpeed); + Logger.recordOutput("MoveAndAvoidReef/nominalSpeed", nominalSpeed); + swerve.moveToPose(getNextIntermediateTarget(swerve.state.getGlobalPoseEstimate(), pose2d), - maxSpeedSupplier.getAsDouble()); + Math.min(nominalSpeed, maxSpeed)); } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index 2487f81d..e19ee800 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -1,7 +1,12 @@ package frc.robot.subsystems.swerve; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Rotation; +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -10,6 +15,7 @@ import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -18,11 +24,14 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -325,8 +334,6 @@ public Command teleOpDrive(CommandXboxController controller, boolean fieldRelati }); } - - public Command stop() { return this.runOnce(this::setMotorsZero); } @@ -391,4 +398,160 @@ public void moveToPose(Pose2d pose) { Constants.SwerveTransformPID.MAX_ACCELERATION); } + private void runCharacterization(double output) { + for (int i = 0; i < 4; i++) { + swerveMods[i].runCharacterization(output); + } + } + + private double getFFCharacterizationVelocity() { + double output = 0.0; + for (int i = 0; i < 4; i++) { + output += swerveMods[i].getFFCharacterizationVelocity() / 4.0; + } + return output; + } + + private double[] getWheelRadiusCharacterizationPositions() { + double[] values = new double[4]; + for (int i = 0; i < 4; i++) { + values[i] = swerveMods[i].getWheelRadiusCharacterizationPosition(); + } + return values; + } + + /** Sysid routine to determine kS and kV */ + public Command feedforwardCharacterization() { + List velocitySamples = new LinkedList<>(); + List voltageSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce(() -> { + velocitySamples.clear(); + voltageSamples.clear(); + }), + + // Allow modules to orient + Commands.run(() -> this.runCharacterization(0.0), this) + .withTimeout(Constants.Swerve.Characterization.ffStartDelay), + + // Start timer + Commands.runOnce(timer::restart), + + // Accelerate and gather data + Commands.run(() -> { + double voltage = timer.get() * Constants.Swerve.Characterization.ffRampRate; + this.runCharacterization(voltage); + velocitySamples.add(this.getFFCharacterizationVelocity()); + voltageSamples.add(voltage); + }, this) + + // When cancelled, calculate and print results + .finallyDo(() -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); + double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + System.out.println("********** Drive FF Characterization Results **********"); + System.out.println("\tkS: " + formatter.format(kS)); + System.out.println("\tkV: " + formatter.format(kV)); + Logger.recordOutput("Sysid/FF/kS", kS); + Logger.recordOutput("Sysid/FF/kV", kV); + })); + } + + /** Sysid routine to determine wheel radius */ + public Command wheelRadiusCharacterization() { + SlewRateLimiter limiter = + new SlewRateLimiter(Constants.Swerve.Characterization.wheelRadiusRampRate); + WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); + + double driveBaseRadius = Math.hypot(Constants.Swerve.bumperFront.in(Meters), + Constants.Swerve.bumperRight.in(Meters)); + + return Commands.parallel( + // Drive control sequence + Commands.sequence( + // Reset acceleration limiter + Commands.runOnce(() -> limiter.reset(0.0)), + + // Turn in place, accelerating up to full speed + Commands.run(() -> { + double speed = + limiter.calculate(Constants.Swerve.Characterization.wheelRadiusMaxVelocity); + this.setModuleStates(new ChassisSpeeds(0.0, 0.0, speed)); + }, this)), + + // Measurement sequence + Commands.sequence( + // Wait for modules to fully orient before starting measurement + Commands.waitSeconds(1.0), + + // Record starting measurement + Commands.runOnce(() -> { + state.positions = this.getWheelRadiusCharacterizationPositions(); + state.lastAngle = this.getGyroYaw(); + state.gyroDelta = 0.0; + }), + + // Update gyro delta + Commands.run(() -> { + var rotation = this.getGyroYaw(); + state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + + double[] positions = this.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = (state.gyroDelta * driveBaseRadius) / wheelDelta; + + Logger.recordOutput("Drive/WheelDelta", wheelDelta); + Logger.recordOutput("Drive/WheelRadius", wheelRadius); + }) + + // When cancelled, calculate and print results + .finallyDo(() -> { + double[] positions = this.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = (state.gyroDelta * driveBaseRadius) / wheelDelta; + + NumberFormat formatter = + new DecimalFormat("#0.000000000000000000000000000"); + System.out + .println("********** Wheel Radius Characterization Results **********"); + System.out + .println("\tWheel Delta: " + formatter.format(wheelDelta) + " radians"); + System.out.println( + "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians"); + System.out.println( + "\tWheel Radius: " + formatter.format(wheelRadius) + " meters, " + + formatter.format(Units.metersToInches(wheelRadius)) + " inches"); + Logger.recordOutput("Sysid/FF/radius", wheelRadius); + }))); + } + + private static class WheelRadiusCharacterizationState { + double[] positions = new double[4]; + Rotation2d lastAngle = Rotation2d.kZero; + double gyroDelta = 0.0; + } + }