From 1ecf7a099ca0a99c24c0335227ab3a57d0256c98 Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Thu, 28 Mar 2024 17:59:12 -0500 Subject: [PATCH 01/16] Add alliance flipping for teleop driving (upstream @jwbonner) --- .../robot/commands/ControlCommands/DriveCommands.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java index ee67e4b..11cccf5 100644 --- a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java @@ -19,6 +19,8 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.drive.Drive; @@ -44,6 +46,10 @@ public static Command joystickDrive( double omega; boolean SlowMode = drive.isSlowMode(); + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + // Apply deadband and slowMode if (!SlowMode) { linearMagnitude = @@ -82,7 +88,9 @@ public static Command joystickDrive( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), 1.25 * omega * drive.getMaxAngularSpeedRadPerSec() / 2.3, - drive.getRotation())); + isFlipped //TODO TEST + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); }, drive); } From ce41b85bda0f674ebf7a3f76a95654e156043b27 Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Fri, 29 Mar 2024 13:16:43 -0500 Subject: [PATCH 02/16] Add pose estimation to advanced swerve drive project (upstream, with tweaks) --- build.gradle | 6 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../ControlCommands/DriveCommands.java | 2 +- .../java/frc/robot/commands/IntakeNote.java | 1 + .../robot/commands/IntakeNoteAndAlign.java | 1 + .../frc/robot/commands/PreRunShooter.java | 1 + .../java/frc/robot/subsystems/arm/Arm.java | 1 + .../frc/robot/subsystems/drive/Drive.java | 96 +++++++++++++------ .../frc/robot/subsystems/drive/GyroIO.java | 1 + .../robot/subsystems/drive/GyroIONavX.java | 7 +- .../frc/robot/subsystems/drive/Module.java | 28 +++--- .../frc/robot/subsystems/drive/ModuleIO.java | 1 + .../robot/subsystems/drive/ModuleIOSim.java | 2 + .../subsystems/drive/ModuleIOSparkMax.java | 5 + .../drive/SparkMaxOdometryThread.java | 28 +++++- .../frc/robot/subsystems/sticks/Sticks.java | 1 + 16 files changed, 135 insertions(+), 48 deletions(-) diff --git a/build.gradle b/build.gradle index 5fee2bc..052d1f1 100644 --- a/build.gradle +++ b/build.gradle @@ -113,7 +113,11 @@ wpi.sim.addDriverstation() // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from { + configurations.runtimeClasspath.collect { + it.isDirectory() ? it : zipTree(it) + } + } from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4f0a838..d24214f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -118,7 +118,7 @@ public RobotContainer() { // Real robot, instantiate hardware IO implementations sDrive = new Drive( - new GyroIONavX(false), + new GyroIONavX(), new ModuleIOSparkMax(0), new ModuleIOSparkMax(1), new ModuleIOSparkMax(2), diff --git a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java index 11cccf5..d7a81f9 100644 --- a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java @@ -88,7 +88,7 @@ public static Command joystickDrive( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), 1.25 * omega * drive.getMaxAngularSpeedRadPerSec() / 2.3, - isFlipped //TODO TEST + isFlipped // TEST TODO ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation())); }, diff --git a/src/main/java/frc/robot/commands/IntakeNote.java b/src/main/java/frc/robot/commands/IntakeNote.java index ce86573..a109b23 100644 --- a/src/main/java/frc/robot/commands/IntakeNote.java +++ b/src/main/java/frc/robot/commands/IntakeNote.java @@ -4,6 +4,7 @@ import frc.robot.Constants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.intake.Intake; + // Moves the note so that it is detected by the conveySensor but not shooterSensor public class IntakeNote extends Command { diff --git a/src/main/java/frc/robot/commands/IntakeNoteAndAlign.java b/src/main/java/frc/robot/commands/IntakeNoteAndAlign.java index 9c54618..18f12be 100644 --- a/src/main/java/frc/robot/commands/IntakeNoteAndAlign.java +++ b/src/main/java/frc/robot/commands/IntakeNoteAndAlign.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.intake.Intake; + // Moves the note so that it is detected by the conveySensor but not shooterSensor public class IntakeNoteAndAlign extends Command { diff --git a/src/main/java/frc/robot/commands/PreRunShooter.java b/src/main/java/frc/robot/commands/PreRunShooter.java index ea1e7d2..dae9cfc 100644 --- a/src/main/java/frc/robot/commands/PreRunShooter.java +++ b/src/main/java/frc/robot/commands/PreRunShooter.java @@ -27,6 +27,7 @@ public void execute() { flywheel.runVelocity(-Constants.FlywheelConstants.shootingVelocity); } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 07f5273..c312c98 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -75,6 +75,7 @@ public void runVelocity(double velocityRPM) { public boolean getIsFinished() { return io.getIsFinished(); } + /** * @param rightPosition */ diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index f719e66..4f9ae1b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -19,6 +19,7 @@ import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -57,14 +58,22 @@ public class Drive extends SubsystemBase { private static double CurrentMaxAngularSpeed = FAST_MAX_ANGULAR_SPEED; private static double CurrentMaxLinearSpeed = FAST_MAX_LINEAR_SPEED; - public static final Lock odometryLock = new ReentrantLock(); + static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); - private Pose2d pose = new Pose2d(); - private Rotation2d lastGyroRotation = new Rotation2d(); + private Rotation2d rawGyroRotation = new Rotation2d(); + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + private SwerveDrivePoseEstimator poseEstimator = + new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); public Drive( GyroIO gyroIO, @@ -78,6 +87,9 @@ public Drive( modules[2] = new Module(blModuleIO, 2); modules[3] = new Module(brModuleIO, 3); + // Start threads (no-op if no signals have been created) + SparkMaxOdometryThread.getInstance().start(); + // Configure AutoBuilder for PathPlanner AutoBuilder.configureHolonomic( this::getPose, @@ -131,31 +143,34 @@ public void periodic() { } // Update odometry - int deltaCount = - gyroInputs.connected ? gyroInputs.odometryYawPositions.length : Integer.MAX_VALUE; - for (int i = 0; i < 4; i++) { - deltaCount = Math.min(deltaCount, modules[i].getPositionDeltas().length); - } - for (int deltaIndex = 0; deltaIndex < deltaCount; deltaIndex++) { - // Read wheel deltas from each module - SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4]; + double[] sampleTimestamps = + modules[0].getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { - wheelDeltas[moduleIndex] = modules[moduleIndex].getPositionDeltas()[deltaIndex]; + modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; + moduleDeltas[moduleIndex] = + new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; } - // The twist represents the motion of the robot since the last - // sample in x, y, and theta based only on the modules, without - // the gyro. The gyro is always disconnected in simulation. - var twist = kinematics.toTwist2d(wheelDeltas); - if (true) { // check to make sure the universe still exists - // If the gyro is connected, replace the theta component of the twist - // with the change in angle since the last sample. - Rotation2d gyroRotation = gyroInputs.odometryYawPositions[deltaIndex]; - twist = new Twist2d(twist.dx, twist.dy, gyroRotation.minus(lastGyroRotation).getRadians()); - lastGyroRotation = gyroRotation; + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.odometryYawPositions[i]; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } - // Apply the twist (change since last sample) to the current pose - pose = pose.exp(twist); + // Apply update + poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); } } @@ -226,25 +241,48 @@ private SwerveModuleState[] getModuleStates() { return states; } + /** Returns the module positions (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getPosition(); + } + return states; + } + /** Returns the current odometry pose. */ @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { - return pose; + return poseEstimator.getEstimatedPosition(); } /** Returns the current odometry rotation. */ public Rotation2d getRotation() { - return pose.getRotation(); + return getPose().getRotation(); } + /** Resets the current odometry/gyro rotation. */ public void resetRotation() { this.gyroIO.resetRotation(); - setPose(new Pose2d(getPose().getTranslation(), Rotation2d.fromRadians(0))); + poseEstimator.resetPosition( + rawGyroRotation, getModulePositions(), poseEstimator.getEstimatedPosition()); + // setPose(new Pose2d(getPose().getTranslation(), Rotation2d.fromRadians(0))); } /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { - this.pose = pose; + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + /** + * Adds a vision measurement to the pose estimator. + * + * @param visionPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. + */ + public void addVisionMeasurement(Pose2d visionPose, double timestamp) { + poseEstimator.addVisionMeasurement(visionPose, timestamp); } /** Returns the maximum linear speed in meters per sec. */ @@ -257,6 +295,7 @@ public double getMaxAngularSpeedRadPerSec() { return CurrentMaxAngularSpeed; } + /** Toggles slowmode. A bit of a janky solution, but it (doesn't) work. */ public void toggleSlowMode() { if (SlowMode) { CurrentMaxLinearSpeed = SLOW_MAX_LINEAR_SPEED; @@ -269,6 +308,7 @@ public void toggleSlowMode() { } } + /** Returns the SlowMode boolean. */ public boolean isSlowMode() { return SlowMode; } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 60f26cf..78cc9d7 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -23,6 +23,7 @@ public static class GyroIOInputs { public boolean calibrating = false; public Rotation2d yawPosition = new Rotation2d(); public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + public double[] odometryYawTimestamps = new double[] {}; public double yawVelocityRadPerSec = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 8b98ce2..2ac7739 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -25,8 +25,9 @@ public class GyroIONavX implements GyroIO { AHRS ahrs; private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; - public GyroIONavX(boolean phoenixDrive) { + public GyroIONavX() { try { ahrs = new AHRS(SPI.Port.kMXP, (byte) Module.ODOMETRY_FREQUENCY); @@ -40,6 +41,7 @@ public GyroIONavX(boolean phoenixDrive) { yawPositionQueue = SparkMaxOdometryThread.getInstance().registerSignal(() -> (double) -ahrs.getAngle()); + yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); } @Override @@ -61,8 +63,11 @@ public void updateInputs(GyroIOInputs inputs) { yawPositionQueue.stream() .map((Double value) -> Rotation2d.fromDegrees(value)) .toArray(Rotation2d[]::new); + inputs.odometryYawTimestamps = + yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); yawPositionQueue.clear(); + yawTimestampQueue.clear(); } public void resetRotation() { diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 0211ab6..a27d898 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -24,7 +24,7 @@ public class Module { private static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); - public static final double ODOMETRY_FREQUENCY = 200.0; + static final double ODOMETRY_FREQUENCY = 200.0; private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); @@ -36,8 +36,7 @@ public class Module { private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute - private double lastPositionMeters = 0.0; // Used for delta calculation - private SwerveModulePosition[] positionDeltas = new SwerveModulePosition[] {}; + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; public Module(ModuleIO io, int index) { this.io = io; @@ -108,17 +107,15 @@ public void periodic() { } } - // Calculate position deltas for odometry - int deltaCount = - Math.min(inputs.odometryDrivePositionsRad.length, inputs.odometryTurnPositions.length); - positionDeltas = new SwerveModulePosition[deltaCount]; - for (int i = 0; i < deltaCount; i++) { + // 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] * WHEEL_RADIUS; Rotation2d angle = inputs.odometryTurnPositions[i].plus( turnRelativeOffset != null ? turnRelativeOffset : new Rotation2d()); - positionDeltas[i] = new SwerveModulePosition(positionMeters - lastPositionMeters, angle); - lastPositionMeters = positionMeters; + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); } } @@ -194,9 +191,14 @@ public SwerveModuleState getState() { return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); } - /** Returns the module position deltas received this cycle. */ - public SwerveModulePosition[] getPositionDeltas() { - return positionDeltas; + /** 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 velocity in radians/sec. */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 8620ae3..200afa3 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -30,6 +30,7 @@ public static class ModuleIOInputs { public double turnAppliedVolts = 0.0; public double[] turnCurrentAmps = new double[] {}; + public double[] odometryTimestamps = new double[] {}; public double[] odometryDrivePositionsRad = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index c309c68..f2ffe48 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; /** @@ -52,6 +53,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnAppliedVolts = turnAppliedVolts; inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index 9d5164a..ee6516b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -49,6 +49,7 @@ public class ModuleIOSparkMax implements ModuleIO { private final RelativeEncoder driveEncoder; private final RelativeEncoder turnRelativeEncoder; private final AnalogInput turnAbsoluteEncoder; + private final Queue timestampQueue; private final Queue drivePositionQueue; private final Queue turnPositionQueue; @@ -126,6 +127,7 @@ public ModuleIOSparkMax(int index) { PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY)); turnSparkMax.setPeriodicFramePeriod( PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY)); + timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); drivePositionQueue = SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition); // turnPositionQueue = @@ -167,6 +169,8 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); inputs.odometryDrivePositionsRad = drivePositionQueue.stream() .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) @@ -177,6 +181,7 @@ public void updateInputs(ModuleIOInputs inputs) { (Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO).minus(absoluteEncoderOffset)) .toArray(Rotation2d[]::new); + timestampQueue.clear(); drivePositionQueue.clear(); turnPositionQueue.clear(); turnRelativeEncoder.setPosition(turnAbsoluteEncoderNew.getPosition()); diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index bbe5267..10c83c3 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -14,11 +14,12 @@ package frc.robot.subsystems.drive; import edu.wpi.first.wpilibj.Notifier; +import java.util.ArrayDeque; import java.util.ArrayList; import java.util.List; import java.util.Queue; -import java.util.concurrent.ArrayBlockingQueue; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; /** * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. @@ -29,6 +30,7 @@ public class SparkMaxOdometryThread { private List signals = new ArrayList<>(); private List> queues = new ArrayList<>(); + private List> timestampQueues = new ArrayList<>(); private final Notifier notifier; private static SparkMaxOdometryThread instance = null; @@ -43,11 +45,16 @@ public static SparkMaxOdometryThread getInstance() { private SparkMaxOdometryThread() { notifier = new Notifier(this::periodic); notifier.setName("SparkMaxOdometryThread"); - notifier.startPeriodic(1.0 / Module.ODOMETRY_FREQUENCY); + } + + public void start() { + if (timestampQueues.size() > 0) { + notifier.startPeriodic(1.0 / Module.ODOMETRY_FREQUENCY); + } } public Queue registerSignal(DoubleSupplier signal) { - Queue queue = new ArrayBlockingQueue<>(100); + Queue queue = new ArrayDeque<>(100); Drive.odometryLock.lock(); try { signals.add(signal); @@ -58,12 +65,27 @@ public Queue registerSignal(DoubleSupplier signal) { return queue; } + public Queue makeTimestampQueue() { + Queue queue = new ArrayDeque<>(100); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + private void periodic() { Drive.odometryLock.lock(); + double timestamp = Logger.getRealTimestamp() / 1e6; try { for (int i = 0; i < signals.size(); i++) { queues.get(i).offer(signals.get(i).getAsDouble()); } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } } finally { Drive.odometryLock.unlock(); } diff --git a/src/main/java/frc/robot/subsystems/sticks/Sticks.java b/src/main/java/frc/robot/subsystems/sticks/Sticks.java index af006c4..b13cee8 100644 --- a/src/main/java/frc/robot/subsystems/sticks/Sticks.java +++ b/src/main/java/frc/robot/subsystems/sticks/Sticks.java @@ -23,6 +23,7 @@ public class Sticks extends SubsystemBase { private final SticksIO io; private final SticksIOInputsAutoLogged inputs = new SticksIOInputsAutoLogged(); + // private final SimpleMotorFeedforward ffModel; /** Creates a new Sticks. */ From 83d2e439634926a6c3802e0cefe77971f613e98b Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Fri, 29 Mar 2024 14:16:37 -0500 Subject: [PATCH 03/16] Add SysId (upstream) --- build.gradle | 2 - src/main/java/frc/robot/RobotContainer.java | 67 +++++- .../commands/FeedForwardCharacterization.java | 106 --------- .../java/frc/robot/subsystems/arm/Arm.java | 28 ++- .../frc/robot/subsystems/arm/ArmIOSim.java | 2 +- .../frc/robot/subsystems/climber/Climber.java | 30 ++- .../subsystems/climber/ClimberIOSim.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 40 +++- .../robot/subsystems/flywheel/Flywheel.java | 29 ++- .../subsystems/flywheel/FlywheelIOSim.java | 2 +- .../frc/robot/subsystems/intake/Intake.java | 26 ++- .../robot/subsystems/intake/IntakeIOSim.java | 2 +- .../frc/robot/util/PolynomialRegression.java | 205 ------------------ 13 files changed, 189 insertions(+), 352 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/FeedForwardCharacterization.java delete mode 100644 src/main/java/frc/robot/util/PolynomialRegression.java diff --git a/build.gradle b/build.gradle index 052d1f1..21bed01 100644 --- a/build.gradle +++ b/build.gradle @@ -94,8 +94,6 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - implementation 'gov.nist.math:jama:1.0.3' - def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d24214f..d235b90 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,10 +24,10 @@ import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.ControlCommands.ArmCommands; import frc.robot.commands.ControlCommands.DriveCommands; import frc.robot.commands.ControlCommands.IntakeShooterControls; -import frc.robot.commands.FeedForwardCharacterization; import frc.robot.commands.IntakeNote; import frc.robot.commands.IntakeNoteAndAlign; import frc.robot.commands.Positions.AmpShot; @@ -299,15 +299,66 @@ public RobotContainer() { options.forEach(auto -> chooser.addOption(auto.getName(), auto)); autoChooser = new LoggedDashboardChooser<>("Auto Choices", chooser); - // Set up feedforward characterization + // Set up SysId routines + // Drive subsystem autoChooser.addOption( - "Drive FF Characterization", - new FeedForwardCharacterization( - sDrive, sDrive::runCharacterizationVolts, sDrive::getCharacterizationVelocity)); + "Drive SysId (Quasistatic Forward)", + sDrive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); autoChooser.addOption( - "Flywheel FF Characterization", - new FeedForwardCharacterization( - sFlywheel, sFlywheel::runVolts, sFlywheel::getCharacterizationVelocity)); + "Drive SysId (Quasistatic Reverse)", + sDrive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Drive SysId (Dynamic Forward)", sDrive.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive SysId (Dynamic Reverse)", sDrive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + // Flywheel subsystem + autoChooser.addOption( + "Flywheel SysId (Quasistatic Forward)", + sFlywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Flywheel SysId (Quasistatic Reverse)", + sFlywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Flywheel SysId (Dynamic Forward)", + sFlywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Flywheel SysId (Dynamic Reverse)", + sFlywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + // Climber subsystem + autoChooser.addOption( + "Climber SysId (Quasistatic Forward)", + sClimber.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Climber SysId (Quasistatic Reverse)", + sClimber.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Climber SysId (Dynamic Forward)", sClimber.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Climber SysId (Dynamic Reverse)", sClimber.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + // Arm subsystem + autoChooser.addOption( + "Arm SysId (Quasistatic Forward)", sArm.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Arm SysId (Quasistatic Reverse)", sArm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Arm SysId (Dynamic Forward)", sArm.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Arm SysId (Dynamic Reverse)", sArm.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + // Intake subsystem + autoChooser.addOption( + "Intake SysId (Quasistatic Forward)", + sIntake.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Intake SysId (Quasistatic Reverse)", + sIntake.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Intake SysId (Dynamic Forward)", sIntake.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Intake SysId (Dynamic Reverse)", sIntake.sysIdDynamic(SysIdRoutine.Direction.kReverse)); // Configure the button bindings configureButtonBindings(); diff --git a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java deleted file mode 100644 index d7ac7a7..0000000 --- a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java +++ /dev/null @@ -1,106 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.util.PolynomialRegression; -import java.util.LinkedList; -import java.util.List; -import java.util.function.Consumer; -import java.util.function.Supplier; - -public class FeedForwardCharacterization extends Command { - private static final double START_DELAY_SECS = 2.0; - private static final double RAMP_VOLTS_PER_SEC = 0.1; - - private FeedForwardCharacterizationData data; - private final Consumer voltageConsumer; - private final Supplier velocitySupplier; - - private final Timer timer = new Timer(); - - /** Creates a new FeedForwardCharacterization command. */ - public FeedForwardCharacterization( - Subsystem subsystem, Consumer voltageConsumer, Supplier velocitySupplier) { - addRequirements(subsystem); - this.voltageConsumer = voltageConsumer; - this.velocitySupplier = velocitySupplier; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - data = new FeedForwardCharacterizationData(); - timer.reset(); - timer.start(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (timer.get() < START_DELAY_SECS) { - voltageConsumer.accept(0.0); - } else { - double voltage = (timer.get() - START_DELAY_SECS) * RAMP_VOLTS_PER_SEC; - voltageConsumer.accept(voltage); - data.add(velocitySupplier.get(), voltage); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - voltageConsumer.accept(0.0); - timer.stop(); - data.print(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } - - public static class FeedForwardCharacterizationData { - private final List velocityData = new LinkedList<>(); - private final List voltageData = new LinkedList<>(); - - public void add(double velocity, double voltage) { - if (Math.abs(velocity) > 1E-4) { - velocityData.add(Math.abs(velocity)); - voltageData.add(Math.abs(voltage)); - } - } - - public void print() { - if (velocityData.size() == 0 || voltageData.size() == 0) { - return; - } - - PolynomialRegression regression = - new PolynomialRegression( - velocityData.stream().mapToDouble(Double::doubleValue).toArray(), - voltageData.stream().mapToDouble(Double::doubleValue).toArray(), - 1); - - System.out.println("FF Characterization Results:"); - System.out.println("\tCount=" + Integer.toString(velocityData.size()) + ""); - System.out.println(String.format("\tR2=%.5f", regression.R2())); - System.out.println(String.format("\tkS=%.5f", regression.beta(0))); - System.out.println(String.format("\tkV=%.5f", regression.beta(1))); - } - } -} diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index c312c98..7a4d3dd 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -13,17 +13,23 @@ package frc.robot.subsystems.arm; +import static edu.wpi.first.units.Units.*; + import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.ArmConstants; import frc.robot.Constants.EnvironmentalConstants; import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; public class Arm extends SubsystemBase { private final ArmIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); private final SimpleMotorFeedforward ffModel; + private final SysIdRoutine sysId; /** Creates a new Arm. */ public Arm(ArmIO io) { @@ -48,6 +54,15 @@ public Arm(ArmIO io) { ffModel = new SimpleMotorFeedforward(ArmConstants.ks, ArmConstants.kv); break; } + // Configure SysId + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Arm/SysIdState", state.toString())), + new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this)); } @Override @@ -107,18 +122,25 @@ public double getVelocityRPM() { return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); } + /** Returns the arm angle in radians?. */ @AutoLogOutput(key = "Arm/PositionRad") public double getPosition() { return inputs.positionRad; } + /** Returns the arm target angle in radians. */ @AutoLogOutput(key = "Arm/TargetPositionRad") public double getTargetPosition() { return inputs.targetPositionRad; } - /** Returns the current velocity in radians per second. */ - public double getCharacterizationVelocity() { - return inputs.velocityRadPerSec; + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysId.quasistatic(direction); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index b03ae56..e35c3d8 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -48,7 +48,7 @@ public void updateInputs(ArmIOInputs inputs) { @Override public void setVoltage(double volts) { closedLoop = false; - appliedVolts = 0.0; + appliedVolts = volts; sim.setInputVoltage(volts); } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index d9c2138..5727380 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -13,10 +13,15 @@ package frc.robot.subsystems.climber; +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.ClimberConstants; import frc.robot.Constants.EnvironmentalConstants; import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; // ! TODO Seperate target positions but they're controlled by one so that they can be controlled // ! seperately from smartdashboard @@ -24,6 +29,7 @@ public class Climber extends SubsystemBase { private final ClimberIO io; private final ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged(); + private final SysIdRoutine sysId; /** Creates a new Climber. */ public Climber(ClimberIO io) { @@ -43,6 +49,15 @@ public Climber(ClimberIO io) { default: break; } + // Configure SysId + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Climber/SysIdState", state.toString())), + new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this)); } @Override @@ -90,23 +105,32 @@ public double getRightVelocity() { return inputs.rightVelocity; } + /** Returns the current left position in m. */ @AutoLogOutput(key = "Climber/LeftPositionRad") public double getLeftPosition() { return inputs.leftPosition; } + /** Returns the current right position in m. */ @AutoLogOutput(key = "Climber/RightPositionRad") public double getRightPosition() { return inputs.rightPosition; } + /** Returns the current target position in m. */ + // TODO separate target positions properly, with smartdashboard reset and movement buttons @AutoLogOutput(key = "Climber/TargetPosition") public double getTargetPosition() { return inputs.targetPosition; } - /** Returns the current velocity in meters per second. */ - public double getCharacterizationVelocity() { - return 0.5 * (inputs.leftVelocity + inputs.rightVelocity); + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysId.quasistatic(direction); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java index 22ef27f..5a49944 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -54,7 +54,7 @@ public void updateInputs(ClimberIOInputs inputs) { @Override public void setVoltage(double volts) { closedLoop = false; - appliedVolts = 0.0; + appliedVolts = volts; sim.setInputVoltage(volts); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 4f9ae1b..429fad4 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -13,6 +13,8 @@ package frc.robot.subsystems.drive; +import static edu.wpi.first.units.Units.*; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; @@ -31,7 +33,9 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.util.LocalADStarAK; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; @@ -63,6 +67,8 @@ public class Drive extends SubsystemBase { private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR + private final SysIdRoutine sysId; + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); private SwerveModulePosition[] lastModulePositions = // For delta tracking @@ -116,6 +122,22 @@ public Drive( (targetPose) -> { Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); }); + // Configure SysId + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(voltage.in(Volts)); + } + }, + null, + this)); } public void periodic() { @@ -215,20 +237,14 @@ public void stopWithX() { stop(); } - /** Runs forwards at the commanded voltage. */ - public void runCharacterizationVolts(double volts) { - for (int i = 0; i < 4; i++) { - modules[i].runCharacterization(volts); - } + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysId.quasistatic(direction); } - /** Returns the average drive velocity in radians/sec. */ - public double getCharacterizationVelocity() { - double driveVelocityAverage = 0.0; - for (var module : modules) { - driveVelocityAverage += module.getCharacterizationVelocity(); - } - return driveVelocityAverage / 4.0; + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); } /** Returns the module states (turn angles and drive velocities) for all of the modules. */ diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java index 56888b9..5951901 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java @@ -13,8 +13,12 @@ package frc.robot.subsystems.flywheel; +import static edu.wpi.first.units.Units.*; + import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.EnvironmentalConstants; import frc.robot.Constants.FlywheelConstants; import org.littletonrobotics.junction.AutoLogOutput; @@ -24,6 +28,7 @@ public class Flywheel extends SubsystemBase { private final FlywheelIO io; private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); private final SimpleMotorFeedforward ffModel; + private final SysIdRoutine sysId; /** Creates a new Flywheel. */ public Flywheel(FlywheelIO io) { @@ -45,6 +50,16 @@ public Flywheel(FlywheelIO io) { ffModel = new SimpleMotorFeedforward(FlywheelConstants.ks, FlywheelConstants.kv); break; } + + // Configure SysId + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Flywheel/SysIdState", state.toString())), + new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this)); } @Override @@ -83,11 +98,13 @@ public double getVelocityRPMBottom() { return inputs.velocityRadPerSecBottom; } - /** - * Returns the current velocity in radians per second. Note: I don't think this will be necessary - * to include in the Auto Manager at all. Is feedforward even appropriate for this? - */ - public double getCharacterizationVelocity() { - return (inputs.velocityRadPerSecTop + inputs.velocityRadPerSecBottom) / 2; + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysId.quasistatic(direction); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); } } diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java index a9e0ab7..4f7a69e 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java @@ -50,7 +50,7 @@ public void updateInputs(FlywheelIOInputs inputs) { @Override public void setVoltage(double volts) { closedLoop = false; - appliedVolts = 0.0; + appliedVolts = volts; sim.setInputVoltage(volts); } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index a873430..62113a2 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -13,9 +13,13 @@ package frc.robot.subsystems.intake; +import static edu.wpi.first.units.Units.*; + import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.EnvironmentalConstants; import java.util.concurrent.locks.Lock; @@ -28,6 +32,7 @@ public class Intake extends SubsystemBase { private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); private final SimpleMotorFeedforward ffModel; public static final Lock odometryLock = new ReentrantLock(); + private final SysIdRoutine sysId; private final ProximitySensorIO proximitySensorIO; private final ProximitySensorIOInputsAutoLogged proximitySensorInputs = @@ -53,6 +58,16 @@ public Intake(IntakeIO io, ProximitySensorIOV3 proximitySensorIO2V3) { ffModel = new SimpleMotorFeedforward(0.0, 0.0); break; } + + // Configure SysId + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Flywheel/SysIdState", state.toString())), + new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this)); } @Override @@ -106,8 +121,13 @@ public double getVelocityRPM() { return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); } - /** Returns the current velocity in radians per second. */ - public double getCharacterizationVelocity() { - return inputs.velocityRadPerSec; + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysId.quasistatic(direction); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 9238277..da340d6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -45,7 +45,7 @@ public void updateInputs(IntakeIOInputs inputs) { @Override public void setVoltage(double volts) { closedLoop = false; - appliedVolts = 0.0; + appliedVolts = volts; sim.setInputVoltage(volts); } diff --git a/src/main/java/frc/robot/util/PolynomialRegression.java b/src/main/java/frc/robot/util/PolynomialRegression.java deleted file mode 100644 index 28c5796..0000000 --- a/src/main/java/frc/robot/util/PolynomialRegression.java +++ /dev/null @@ -1,205 +0,0 @@ -package frc.robot.util; - -import Jama.Matrix; -import Jama.QRDecomposition; - -// NOTE: This file is available at -// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html - -/** - * The {@code PolynomialRegression} class performs a polynomial regression on an set of N - * data points (yi, xi). That is, it fits a polynomial - * y = β0 + β1 x + β2 - * x2 + ... + βd xd (where - * y is the response variable, x is the predictor variable, and the - * βi are the regression coefficients) that minimizes the sum of squared - * residuals of the multiple regression model. It also computes associated the coefficient of - * determination R2. - * - *

This implementation performs a QR-decomposition of the underlying Vandermonde matrix, so it is - * neither the fastest nor the most numerically stable way to perform the polynomial regression. - * - * @author Robert Sedgewick - * @author Kevin Wayne - */ -public class PolynomialRegression implements Comparable { - private final String variableName; // name of the predictor variable - private int degree; // degree of the polynomial regression - private Matrix beta; // the polynomial regression coefficients - private double sse; // sum of squares due to error - private double sst; // total sum of squares - - /** - * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. Uses n as the name - * of the predictor variable. - * - * @param x the values of the predictor variable - * @param y the corresponding values of the response variable - * @param degree the degree of the polynomial to fit - * @throws IllegalArgumentException if the lengths of the two arrays are not equal - */ - public PolynomialRegression(double[] x, double[] y, int degree) { - this(x, y, degree, "n"); - } - - /** - * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. - * - * @param x the values of the predictor variable - * @param y the corresponding values of the response variable - * @param degree the degree of the polynomial to fit - * @param variableName the name of the predictor variable - * @throws IllegalArgumentException if the lengths of the two arrays are not equal - */ - public PolynomialRegression(double[] x, double[] y, int degree, String variableName) { - this.degree = degree; - this.variableName = variableName; - - int n = x.length; - QRDecomposition qr = null; - Matrix matrixX = null; - - // in case Vandermonde matrix does not have full rank, reduce degree until it - // does - while (true) { - - // build Vandermonde matrix - double[][] vandermonde = new double[n][this.degree + 1]; - for (int i = 0; i < n; i++) { - for (int j = 0; j <= this.degree; j++) { - vandermonde[i][j] = Math.pow(x[i], j); - } - } - matrixX = new Matrix(vandermonde); - - // find least squares solution - qr = new QRDecomposition(matrixX); - if (qr.isFullRank()) break; - - // decrease degree and try again - this.degree--; - } - - // create matrix from vector - Matrix matrixY = new Matrix(y, n); - - // linear regression coefficients - beta = qr.solve(matrixY); - - // mean of y[] values - double sum = 0.0; - for (int i = 0; i < n; i++) sum += y[i]; - double mean = sum / n; - - // total variation to be accounted for - for (int i = 0; i < n; i++) { - double dev = y[i] - mean; - sst += dev * dev; - } - - // variation not accounted for - Matrix residuals = matrixX.times(beta).minus(matrixY); - sse = residuals.norm2() * residuals.norm2(); - } - - /** - * Returns the {@code j}th regression coefficient. - * - * @param j the index - * @return the {@code j}th regression coefficient - */ - public double beta(int j) { - // to make -0.0 print as 0.0 - if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0; - return beta.get(j, 0); - } - - /** - * Returns the degree of the polynomial to fit. - * - * @return the degree of the polynomial to fit - */ - public int degree() { - return degree; - } - - /** - * Returns the coefficient of determination R2. - * - * @return the coefficient of determination R2, which is a real number between - * 0 and 1 - */ - public double R2() { - if (sst == 0.0) return 1.0; // constant function - return 1.0 - sse / sst; - } - - /** - * Returns the expected response {@code y} given the value of the predictor variable {@code x}. - * - * @param x the value of the predictor variable - * @return the expected response {@code y} given the value of the predictor variable {@code x} - */ - public double predict(double x) { - // horner's method - double y = 0.0; - for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); - return y; - } - - /** - * Returns a string representation of the polynomial regression model. - * - * @return a string representation of the polynomial regression model, including the best-fit - * polynomial and the coefficient of determination R2 - */ - public String toString() { - StringBuilder s = new StringBuilder(); - int j = degree; - - // ignoring leading zero coefficients - while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; - - // create remaining terms - while (j >= 0) { - if (j == 0) s.append(String.format("%.10f ", beta(j))); - else if (j == 1) s.append(String.format("%.10f %s + ", beta(j), variableName)); - else s.append(String.format("%.10f %s^%d + ", beta(j), variableName, j)); - j--; - } - s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); - - // replace "+ -2n" with "- 2n" - return s.toString().replace("+ -", "- "); - } - - /** Compare lexicographically. */ - public int compareTo(PolynomialRegression that) { - double EPSILON = 1E-5; - int maxDegree = Math.max(this.degree(), that.degree()); - for (int j = maxDegree; j >= 0; j--) { - double term1 = 0.0; - double term2 = 0.0; - if (this.degree() >= j) term1 = this.beta(j); - if (that.degree() >= j) term2 = that.beta(j); - if (Math.abs(term1) < EPSILON) term1 = 0.0; - if (Math.abs(term2) < EPSILON) term2 = 0.0; - if (term1 < term2) return -1; - else if (term1 > term2) return +1; - } - return 0; - } - - /** - * Unit tests the {@code PolynomialRegression} data type. - * - * @param args the command-line arguments - */ - public static void main(String[] args) { - double[] x = {10, 20, 40, 80, 160, 200}; - double[] y = {100, 350, 1500, 6700, 20160, 40000}; - PolynomialRegression regression = new PolynomialRegression(x, y, 3); - - System.out.println(regression); - } -} From b727095c7af6c46241e4abc16269e6281c0d3e49 Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Fri, 29 Mar 2024 14:18:51 -0500 Subject: [PATCH 04/16] Fix measured state logging (upstream) --- src/main/java/frc/robot/subsystems/drive/Drive.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 429fad4..a919b5e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -257,8 +257,7 @@ private SwerveModuleState[] getModuleStates() { return states; } - /** Returns the module positions (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveStates/Measured") + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ private SwerveModulePosition[] getModulePositions() { SwerveModulePosition[] states = new SwerveModulePosition[4]; for (int i = 0; i < 4; i++) { From 54ee7f60711005878137976278ee32bb17b75769 Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Fri, 29 Mar 2024 14:22:56 -0500 Subject: [PATCH 05/16] Fix LocalADStarAK (upstream, mjansen4857) --- src/main/java/frc/robot/util/LocalADStarAK.java | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index 37222f2..191b54f 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -28,7 +28,7 @@ public class LocalADStarAK implements Pathfinder { */ @Override public boolean isNewPathAvailable() { - if (Logger.hasReplaySource()) { + if (!Logger.hasReplaySource()) { io.updateIsNewPathAvailable(); } @@ -46,7 +46,7 @@ public boolean isNewPathAvailable() { */ @Override public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { - if (Logger.hasReplaySource()) { + if (!Logger.hasReplaySource()) { io.updateCurrentPathPoints(constraints, goalEndState); } @@ -67,7 +67,7 @@ public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState */ @Override public void setStartPosition(Translation2d startPosition) { - if (Logger.hasReplaySource()) { + if (!Logger.hasReplaySource()) { io.adStar.setStartPosition(startPosition); } } @@ -80,7 +80,7 @@ public void setStartPosition(Translation2d startPosition) { */ @Override public void setGoalPosition(Translation2d goalPosition) { - if (Logger.hasReplaySource()) { + if (!Logger.hasReplaySource()) { io.adStar.setGoalPosition(goalPosition); } } @@ -96,7 +96,9 @@ public void setGoalPosition(Translation2d goalPosition) { @Override public void setDynamicObstacles( List> obs, Translation2d currentRobotPos) { - io.adStar.setDynamicObstacles(obs, currentRobotPos); + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } } private static class ADStarIO implements LoggableInputs { From b75928f8f433a25cba33c85db8d608c3f77d3a09 Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Fri, 29 Mar 2024 14:25:17 -0500 Subject: [PATCH 06/16] Limit queue capacity (upstream) --- .../frc/robot/subsystems/drive/SparkMaxOdometryThread.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index 10c83c3..8c6360e 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -14,6 +14,7 @@ package frc.robot.subsystems.drive; import edu.wpi.first.wpilibj.Notifier; +import java.util.concurrent.ArrayBlockingQueue; import java.util.ArrayDeque; import java.util.ArrayList; import java.util.List; @@ -54,7 +55,7 @@ public void start() { } public Queue registerSignal(DoubleSupplier signal) { - Queue queue = new ArrayDeque<>(100); + Queue queue = new ArrayBlockingQueue<>(20); Drive.odometryLock.lock(); try { signals.add(signal); @@ -66,7 +67,7 @@ public Queue registerSignal(DoubleSupplier signal) { } public Queue makeTimestampQueue() { - Queue queue = new ArrayDeque<>(100); + Queue queue = new ArrayBlockingQueue<>(20); Drive.odometryLock.lock(); try { timestampQueues.add(queue); From a810034b9049c901b790a5e2a95692a2f39230f9 Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Fri, 29 Mar 2024 14:27:19 -0500 Subject: [PATCH 07/16] Use supply current limit for Talons in examples (upstream) --- .../frc/robot/subsystems/drive/ModuleIOTalonFX.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index b793f09..b2f5c21 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -95,14 +95,14 @@ public ModuleIOTalonFX(int index) { } var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; - driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; driveTalon.getConfigurator().apply(driveConfig); setDriveBrakeMode(true); var turnConfig = new TalonFXConfiguration(); - turnConfig.CurrentLimits.StatorCurrentLimit = 30.0; - turnConfig.CurrentLimits.StatorCurrentLimitEnable = true; + turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; turnTalon.getConfigurator().apply(turnConfig); setTurnBrakeMode(true); @@ -113,7 +113,7 @@ public ModuleIOTalonFX(int index) { PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getStatorCurrent(); + driveCurrent = driveTalon.getSupplyCurrent(); turnAbsolutePosition = cancoder.getAbsolutePosition(); turnPosition = turnTalon.getPosition(); @@ -121,7 +121,7 @@ public ModuleIOTalonFX(int index) { PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); turnVelocity = turnTalon.getVelocity(); turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getStatorCurrent(); + turnCurrent = turnTalon.getSupplyCurrent(); BaseStatusSignal.setUpdateFrequencyForAll( Module.ODOMETRY_FREQUENCY, drivePosition, turnPosition); From 57983d15ddcc815e13b6570681030d9839164254 Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Fri, 29 Mar 2024 14:29:13 -0500 Subject: [PATCH 08/16] Fix import for ArrayBlockingQueue in advanced swerve example (upstream - this doesn't do anything) --- .../frc/robot/subsystems/drive/SparkMaxOdometryThread.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index 8c6360e..b5f27d3 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -14,11 +14,10 @@ package frc.robot.subsystems.drive; import edu.wpi.first.wpilibj.Notifier; -import java.util.concurrent.ArrayBlockingQueue; -import java.util.ArrayDeque; import java.util.ArrayList; import java.util.List; import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; From b90edf7309c8dd6c110977bc85b232c1eb4949e4 Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Fri, 29 Mar 2024 14:40:00 -0500 Subject: [PATCH 09/16] Some upstream changes I missed at some point --- .../subsystems/drive/ModuleIOTalonFX.java | 8 +++++ .../drive/PhoenixOdometryThread.java | 34 +++++++++++++++++-- 2 files changed, 40 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index b2f5c21..599659e 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -44,6 +44,8 @@ public class ModuleIOTalonFX implements ModuleIO { private final TalonFX turnTalon; private final CANcoder cancoder; + private final Queue timestampQueue; + private final StatusSignal drivePosition; private final Queue drivePositionQueue; private final StatusSignal driveVelocity; @@ -108,6 +110,8 @@ public ModuleIOTalonFX(int index) { cancoder.getConfigurator().apply(new CANcoderConfiguration()); + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + drivePosition = driveTalon.getPosition(); drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); @@ -168,6 +172,8 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); inputs.odometryDrivePositionsRad = drivePositionQueue.stream() .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) @@ -176,6 +182,8 @@ public void updateInputs(ModuleIOInputs inputs) { turnPositionQueue.stream() .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) .toArray(Rotation2d[]::new); + + timestampQueue.clear(); drivePositionQueue.clear(); turnPositionQueue.clear(); } diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 33a4448..b7531fa 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -23,6 +23,7 @@ import java.util.concurrent.ArrayBlockingQueue; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.Logger; /** * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. @@ -37,6 +38,7 @@ public class PhoenixOdometryThread extends Thread { new ReentrantLock(); // Prevents conflicts when registering signals private BaseStatusSignal[] signals = new BaseStatusSignal[0]; private final List> queues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); private boolean isCANFD = false; private static PhoenixOdometryThread instance = null; @@ -51,11 +53,17 @@ public static PhoenixOdometryThread getInstance() { private PhoenixOdometryThread() { setName("PhoenixOdometryThread"); setDaemon(true); - start(); + } + + @Override + public void start() { + if (timestampQueues.size() > 0) { + super.start(); + } } public Queue registerSignal(ParentDevice device, StatusSignal signal) { - Queue queue = new ArrayBlockingQueue<>(100); + Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); Drive.odometryLock.lock(); try { @@ -72,6 +80,17 @@ public Queue registerSignal(ParentDevice device, StatusSignal si return queue; } + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(20); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + @Override public void run() { while (true) { @@ -97,9 +116,20 @@ public void run() { // Save new data to queues Drive.odometryLock.lock(); try { + double timestamp = Logger.getRealTimestamp() / 1e6; + double totalLatency = 0.0; + for (BaseStatusSignal signal : signals) { + totalLatency += signal.getTimestamp().getLatency(); + } + if (signals.length > 0) { + timestamp -= totalLatency / signals.length; + } for (int i = 0; i < signals.length; i++) { queues.get(i).offer(signals[i].getValueAsDouble()); } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } } finally { Drive.odometryLock.unlock(); } From 709d540651242b357461de3e4118d9df310f8416 Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Fri, 29 Mar 2024 14:54:26 -0500 Subject: [PATCH 10/16] Add error checking for high frequency odometry Spark Max values (upstream) --- .../frc/robot/subsystems/drive/GyroIO.java | 2 +- .../robot/subsystems/drive/GyroIONavX.java | 14 +++++++-- .../subsystems/drive/ModuleIOSparkMax.java | 30 +++++++++++++++---- .../drive/SparkMaxOdometryThread.java | 24 +++++++++++---- 4 files changed, 56 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 78cc9d7..4f1a1df 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -20,7 +20,7 @@ public interface GyroIO { @AutoLog public static class GyroIOInputs { public boolean connected = false; - public boolean calibrating = false; + public boolean notcalibrating = false; public Rotation2d yawPosition = new Rotation2d(); public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; public double[] odometryYawTimestamps = new double[] {}; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 2ac7739..3c5ec79 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -17,6 +17,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.SPI; +import java.util.OptionalDouble; import java.util.Queue; import org.littletonrobotics.junction.AutoLogOutput; @@ -40,7 +41,16 @@ public GyroIONavX() { ahrs.zeroYaw(); yawPositionQueue = - SparkMaxOdometryThread.getInstance().registerSignal(() -> (double) -ahrs.getAngle()); + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + boolean valid = !ahrs.isCalibrating(); + if (valid) { + return OptionalDouble.of(-ahrs.getAngle()); + } else { + return OptionalDouble.empty(); + } + }); yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); } @@ -56,7 +66,7 @@ public void updateInputs(GyroIOInputs inputs) { // .toArray(Rotation2d[]::new); // yawPositionQueue.clear(); inputs.connected = ahrs.isConnected(); - inputs.calibrating = ahrs.isCalibrating(); + inputs.notcalibrating = !ahrs.isCalibrating(); inputs.yawPosition = Rotation2d.fromDegrees(-ahrs.getAngle()); inputs.yawVelocityRadPerSec = Units.degreesToRadians(-ahrs.getRawGyroZ()); inputs.odometryYawPositions = diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index ee6516b..7c182ed 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -17,12 +17,14 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAbsoluteEncoder; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogInput; import frc.robot.Constants.DriveConstants; +import java.util.OptionalDouble; import java.util.Queue; /** @@ -129,12 +131,30 @@ public ModuleIOSparkMax(int index) { PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY)); timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); drivePositionQueue = - SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition); - // turnPositionQueue = - // SparkMaxOdometryThread.getInstance().registerSignal(turnRelativeEncoder::getPosition); - + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + double value = driveEncoder.getPosition(); + if (driveSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + + // Note, make sure that this is using turnAbsoluteEncoderNew, not turnRelativeEncoder turnPositionQueue = - SparkMaxOdometryThread.getInstance().registerSignal(turnAbsoluteEncoderNew::getPosition); + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + double value = turnAbsoluteEncoderNew.getPosition(); + if (driveSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + driveSparkMax.burnFlash(); turnSparkMax.burnFlash(); } diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index b5f27d3..53b228d 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -16,9 +16,10 @@ import edu.wpi.first.wpilibj.Notifier; import java.util.ArrayList; import java.util.List; +import java.util.OptionalDouble; import java.util.Queue; import java.util.concurrent.ArrayBlockingQueue; -import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; /** @@ -28,7 +29,7 @@ * blocking thread. A Notifier thread is used to gather samples with consistent timing. */ public class SparkMaxOdometryThread { - private List signals = new ArrayList<>(); + private List> signals = new ArrayList<>(); private List> queues = new ArrayList<>(); private List> timestampQueues = new ArrayList<>(); @@ -53,7 +54,7 @@ public void start() { } } - public Queue registerSignal(DoubleSupplier signal) { + public Queue registerSignal(Supplier signal) { Queue queue = new ArrayBlockingQueue<>(20); Drive.odometryLock.lock(); try { @@ -80,11 +81,22 @@ private void periodic() { Drive.odometryLock.lock(); double timestamp = Logger.getRealTimestamp() / 1e6; try { + double[] values = new double[signals.size()]; + boolean isValid = true; for (int i = 0; i < signals.size(); i++) { - queues.get(i).offer(signals.get(i).getAsDouble()); + OptionalDouble value = signals.get(i).get(); + if (value.isPresent()) { + values[i] = value.getAsDouble(); + } else { + isValid = false; + break; + } } - for (int i = 0; i < timestampQueues.size(); i++) { - timestampQueues.get(i).offer(timestamp); + if (isValid) { + for (int i = 0; i < signals.size(); i++) { + queues.get(i).offer(values[i]); + timestampQueues.get(i).offer(timestamp); + } } } finally { Drive.odometryLock.unlock(); From efb5c9a1001ad4e4e66fe51374817294de9d5e4c Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Fri, 29 Mar 2024 14:55:41 -0500 Subject: [PATCH 11/16] Fix indexing on Spark Max odometry thread (upstream) --- .../frc/robot/subsystems/drive/SparkMaxOdometryThread.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index 53b228d..15db266 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -93,8 +93,10 @@ private void periodic() { } } if (isValid) { - for (int i = 0; i < signals.size(); i++) { + for (int i = 0; i < queues.size(); i++) { queues.get(i).offer(values[i]); + } + for (int i = 0; i < timestampQueues.size(); i++) { timestampQueues.get(i).offer(timestamp); } } From bd000c83f452b68fa645569db904a421d2f39322 Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Fri, 29 Mar 2024 14:56:35 -0500 Subject: [PATCH 12/16] Reference correct motor in registerSignal call (upstream) --- src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index 7c182ed..3994906 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -148,7 +148,7 @@ public ModuleIOSparkMax(int index) { .registerSignal( () -> { double value = turnAbsoluteEncoderNew.getPosition(); - if (driveSparkMax.getLastError() == REVLibError.kOk) { + if (turnSparkMax.getLastError() == REVLibError.kOk) { return OptionalDouble.of(value); } else { return OptionalDouble.empty(); From 4f69e6aff4e119bc2e150b9aeb0990dd7119f402 Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Mon, 1 Apr 2024 17:38:55 -0500 Subject: [PATCH 13/16] far right auto --- .../deploy/pathplanner/autos/RFar1S5.auto | 25 +++++++++ .../deploy/pathplanner/paths/Point1-3.path | 4 +- src/main/deploy/pathplanner/paths/RtoF5.path | 52 +++++++++++++++++++ .../frc/robot/subsystems/drive/Drive.java | 2 +- 4 files changed, 80 insertions(+), 3 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/RFar1S5.auto create mode 100644 src/main/deploy/pathplanner/paths/RtoF5.path diff --git a/src/main/deploy/pathplanner/autos/RFar1S5.auto b/src/main/deploy/pathplanner/autos/RFar1S5.auto new file mode 100644 index 0000000..7837c70 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RFar1S5.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7266603914356558, + "y": 4.424143586134395 + }, + "rotation": -58.38161458548115 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StartGroup" + } + } + ] + } + }, + "folder": "RealAutons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Point1-3.path b/src/main/deploy/pathplanner/paths/Point1-3.path index 4805308..0bc5b1e 100644 --- a/src/main/deploy/pathplanner/paths/Point1-3.path +++ b/src/main/deploy/pathplanner/paths/Point1-3.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.2304, + "x": 2.23, "y": 4.4638 }, "prevControl": { - "x": 2.2304, + "x": 2.23, "y": 4.8138 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/RtoF5.path b/src/main/deploy/pathplanner/paths/RtoF5.path new file mode 100644 index 0000000..f0f2ab5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RtoF5.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7433436901379014, + "y": 4.422060029558428 + }, + "prevControl": null, + "nextControl": { + "x": 1.7214164587111003, + "y": 2.0183655192252523 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.51035919317209, + "y": 0.77 + }, + "prevControl": { + "x": 3.819797323984116, + "y": 0.7460162489769423 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "MoveOutPaths", + "previewStartingState": { + "rotation": -61.11046206336895, + "velocity": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index bad756c..0e7bbcb 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -90,7 +90,7 @@ public Drive( this::runVelocity, new HolonomicPathFollowerConfig( new PIDConstants(8.0, 0.0, 0.0), - new PIDConstants(10.0, 0.0, 0.0), + new PIDConstants(8.0, 0.0, 1.0), CurrentMaxLinearSpeed, DRIVE_BASE_RADIUS, new ReplanningConfig()), From 4d54f368ca255282a06a19c9831832d659711521 Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Mon, 1 Apr 2024 18:05:47 -0500 Subject: [PATCH 14/16] Farpath, pid --- .../deploy/pathplanner/autos/RFar1S5.auto | 24 +++++++++ src/main/deploy/pathplanner/paths/F5toR.path | 52 +++++++++++++++++++ src/main/deploy/pathplanner/paths/RtoF5.path | 22 ++++++-- src/main/java/frc/robot/RobotContainer.java | 5 +- .../frc/robot/subsystems/drive/Drive.java | 2 +- 5 files changed, 96 insertions(+), 9 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/F5toR.path diff --git a/src/main/deploy/pathplanner/autos/RFar1S5.auto b/src/main/deploy/pathplanner/autos/RFar1S5.auto index 7837c70..8713ac8 100644 --- a/src/main/deploy/pathplanner/autos/RFar1S5.auto +++ b/src/main/deploy/pathplanner/autos/RFar1S5.auto @@ -16,6 +16,30 @@ "data": { "name": "StartGroup" } + }, + { + "type": "named", + "data": { + "name": "FloorPickupPosition" + } + }, + { + "type": "path", + "data": { + "pathName": "RtoF5" + } + }, + { + "type": "named", + "data": { + "name": "IntakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "F5toR" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/F5toR.path b/src/main/deploy/pathplanner/paths/F5toR.path new file mode 100644 index 0000000..d870dd1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F5toR.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 9.565143110103929, + "y": 0.7532360903118802 + }, + "prevControl": null, + "nextControl": { + "x": 5.912542240632302, + "y": 0.9993287790409061 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7735997987753405, + "y": 4.452689378292487 + }, + "prevControl": { + "x": 2.0431707616188, + "y": 1.708662069599849 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -57.443119948220044, + "rotateFast": false + }, + "reversed": false, + "folder": "MoveOutPaths", + "previewStartingState": { + "rotation": 0.20403802540067167, + "velocity": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RtoF5.path b/src/main/deploy/pathplanner/paths/RtoF5.path index f0f2ab5..3df5ed4 100644 --- a/src/main/deploy/pathplanner/paths/RtoF5.path +++ b/src/main/deploy/pathplanner/paths/RtoF5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.51035919317209, - "y": 0.77 + "x": 9.737110990063995, + "y": 0.7516327121051241 }, "prevControl": { - "x": 3.819797323984116, - "y": 0.7460162489769423 + "x": 6.046549120876021, + "y": 0.7276489610820663 }, "nextControl": null, "isLocked": false, @@ -29,7 +29,19 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.15, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 0.1, + "maxAngularAcceleration": 0.1 + } + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bdd57b8..3c342b3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -252,6 +252,7 @@ public RobotContainer() { // Right Alternate Group "RCloseOut", + "RFar1S5", // Test Autos "SimpleStraight", @@ -310,9 +311,7 @@ private void configureButtonBindings() { .start() .whileTrue(Commands.startEnd(() -> sIntake.runFull(), sIntake::stop, sIntake)); - driverController - .rightBumper() - .whileTrue(Commands.run(() -> sDrive.slowMode())); + driverController.rightBumper().whileTrue(Commands.run(() -> sDrive.slowMode())); // ! TEST < driverController.a().whileTrue(sDrive.getZeroAuton()); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 0e7bbcb..5c6c510 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -89,8 +89,8 @@ public Drive( () -> kinematics.toChassisSpeeds(getModuleStates()), this::runVelocity, new HolonomicPathFollowerConfig( + new PIDConstants(8.0, 0.0, 3.0), new PIDConstants(8.0, 0.0, 0.0), - new PIDConstants(8.0, 0.0, 1.0), CurrentMaxLinearSpeed, DRIVE_BASE_RADIUS, new ReplanningConfig()), From dbd9a4746c069fdd85e40683a5c79674cee74abe Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Wed, 3 Apr 2024 18:44:46 -0500 Subject: [PATCH 15/16] Polynomial readded --- build.gradle | 2 + src/main/java/frc/robot/RobotContainer.java | 1 + .../robot/util/math/PolynomialRegression.java | 205 ++++++++++++++++++ 3 files changed, 208 insertions(+) create mode 100644 src/main/java/frc/robot/util/math/PolynomialRegression.java diff --git a/build.gradle b/build.gradle index 21bed01..052d1f1 100644 --- a/build.gradle +++ b/build.gradle @@ -94,6 +94,8 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + implementation 'gov.nist.math:jama:1.0.3' + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 24eba49..9a16564 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -62,6 +62,7 @@ import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.intake.IntakeIOSparkMax; import frc.robot.subsystems.intake.ProximitySensorIOV3; +import frc.robot.util.math.PolynomialRegression; import java.util.ArrayList; import java.util.List; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; diff --git a/src/main/java/frc/robot/util/math/PolynomialRegression.java b/src/main/java/frc/robot/util/math/PolynomialRegression.java new file mode 100644 index 0000000..f0192d8 --- /dev/null +++ b/src/main/java/frc/robot/util/math/PolynomialRegression.java @@ -0,0 +1,205 @@ +package frc.robot.util.math; + +import Jama.Matrix; +import Jama.QRDecomposition; + +// NOTE: This file is available at +// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html + +/** + * The {@code PolynomialRegression} class performs a polynomial regression on an set of N + * data points (yi, xi). That is, it fits a polynomial + * y = β0 + β1 x + β2 + * x2 + ... + βd xd (where + * y is the response variable, x is the predictor variable, and the + * βi are the regression coefficients) that minimizes the sum of squared + * residuals of the multiple regression model. It also computes associated the coefficient of + * determination R2. + * + *

This implementation performs a QR-decomposition of the underlying Vandermonde matrix, so it is + * neither the fastest nor the most numerically stable way to perform the polynomial regression. + * + * @author Robert Sedgewick + * @author Kevin Wayne + */ +public class PolynomialRegression implements Comparable { + private final String variableName; // name of the predictor variable + private int degree; // degree of the polynomial regression + private Matrix beta; // the polynomial regression coefficients + private double sse; // sum of squares due to error + private double sst; // total sum of squares + + /** + * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. Uses n as the name + * of the predictor variable. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + * @throws IllegalArgumentException if the lengths of the two arrays are not equal + */ + public PolynomialRegression(double[] x, double[] y, int degree) { + this(x, y, degree, "n"); + } + + /** + * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + * @param variableName the name of the predictor variable + * @throws IllegalArgumentException if the lengths of the two arrays are not equal + */ + public PolynomialRegression(double[] x, double[] y, int degree, String variableName) { + this.degree = degree; + this.variableName = variableName; + + int n = x.length; + QRDecomposition qr = null; + Matrix matrixX = null; + + // in case Vandermonde matrix does not have full rank, reduce degree until it + // does + while (true) { + + // build Vandermonde matrix + double[][] vandermonde = new double[n][this.degree + 1]; + for (int i = 0; i < n; i++) { + for (int j = 0; j <= this.degree; j++) { + vandermonde[i][j] = Math.pow(x[i], j); + } + } + matrixX = new Matrix(vandermonde); + + // find least squares solution + qr = new QRDecomposition(matrixX); + if (qr.isFullRank()) break; + + // decrease degree and try again + this.degree--; + } + + // create matrix from vector + Matrix matrixY = new Matrix(y, n); + + // linear regression coefficients + beta = qr.solve(matrixY); + + // mean of y[] values + double sum = 0.0; + for (int i = 0; i < n; i++) sum += y[i]; + double mean = sum / n; + + // total variation to be accounted for + for (int i = 0; i < n; i++) { + double dev = y[i] - mean; + sst += dev * dev; + } + + // variation not accounted for + Matrix residuals = matrixX.times(beta).minus(matrixY); + sse = residuals.norm2() * residuals.norm2(); + } + + /** + * Returns the {@code j}th regression coefficient. + * + * @param j the index + * @return the {@code j}th regression coefficient + */ + public double beta(int j) { + // to make -0.0 print as 0.0 + if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0; + return beta.get(j, 0); + } + + /** + * Returns the degree of the polynomial to fit. + * + * @return the degree of the polynomial to fit + */ + public int degree() { + return degree; + } + + /** + * Returns the coefficient of determination R2. + * + * @return the coefficient of determination R2, which is a real number between + * 0 and 1 + */ + public double R2() { + if (sst == 0.0) return 1.0; // constant function + return 1.0 - sse / sst; + } + + /** + * Returns the expected response {@code y} given the value of the predictor variable {@code x}. + * + * @param x the value of the predictor variable + * @return the expected response {@code y} given the value of the predictor variable {@code x} + */ + public double predict(double x) { + // horner's method + double y = 0.0; + for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); + return y; + } + + /** + * Returns a string representation of the polynomial regression model. + * + * @return a string representation of the polynomial regression model, including the best-fit + * polynomial and the coefficient of determination R2 + */ + public String toString() { + StringBuilder s = new StringBuilder(); + int j = degree; + + // ignoring leading zero coefficients + while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; + + // create remaining terms + while (j >= 0) { + if (j == 0) s.append(String.format("%.10f ", beta(j))); + else if (j == 1) s.append(String.format("%.10f %s + ", beta(j), variableName)); + else s.append(String.format("%.10f %s^%d + ", beta(j), variableName, j)); + j--; + } + s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); + + // replace "+ -2n" with "- 2n" + return s.toString().replace("+ -", "- "); + } + + /** Compare lexicographically. */ + public int compareTo(PolynomialRegression that) { + double EPSILON = 1E-5; + int maxDegree = Math.max(this.degree(), that.degree()); + for (int j = maxDegree; j >= 0; j--) { + double term1 = 0.0; + double term2 = 0.0; + if (this.degree() >= j) term1 = this.beta(j); + if (that.degree() >= j) term2 = that.beta(j); + if (Math.abs(term1) < EPSILON) term1 = 0.0; + if (Math.abs(term2) < EPSILON) term2 = 0.0; + if (term1 < term2) return -1; + else if (term1 > term2) return +1; + } + return 0; + } + + /** + * Unit tests the {@code PolynomialRegression} data type. + * + * @param args the command-line arguments + */ + public static void main(String[] args) { + double[] x = {10, 20, 40, 80, 160, 200}; + double[] y = {100, 350, 1500, 6700, 20160, 40000}; + PolynomialRegression regression = new PolynomialRegression(x, y, 3); + + System.out.println(regression); + } +} From 247ebc503964a832fbf73a3c6e2a4afe699f5ddf Mon Sep 17 00:00:00 2001 From: MZ <63869775+JadedHearth@users.noreply.github.com> Date: Thu, 4 Apr 2024 09:57:20 -0500 Subject: [PATCH 16/16] spot --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9a16564..24eba49 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -62,7 +62,6 @@ import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.intake.IntakeIOSparkMax; import frc.robot.subsystems.intake.ProximitySensorIOV3; -import frc.robot.util.math.PolynomialRegression; import java.util.ArrayList; import java.util.List; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;