From 32e3ea51ada54e8d892b5eba665774be6760430c Mon Sep 17 00:00:00 2001 From: Ishan1522 Date: Mon, 4 Nov 2024 07:22:07 -0500 Subject: [PATCH 01/75] code is cooked --- src/main/java/frc/robot/RobotContainer.java | 17 +++++++++++------ .../robot/subsystems/swerve/SwerveDrive.java | 3 +-- .../swerve/moduleIO/SimulatedModule.java | 2 +- .../frc/robot/subsystems/vision/VisionIO.java | 18 +++++++++--------- 4 files changed, 22 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 67b2169e..1b0cd7f0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,7 @@ import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOReal; import frc.robot.subsystems.swerve.gyroIO.GyroInterface; import frc.robot.subsystems.swerve.gyroIO.PhysicalGyro; @@ -40,13 +41,13 @@ public class RobotContainer { - private final Vision visionSubsystem = new Vision(new VisionIOReal()); + private final Vision visionSubsystem; private final SwerveDrive swerveDrive; private final CommandXboxController operatorController = new CommandXboxController(1); - private final Indexer indexer = new Indexer(new IndexerIOTalonFX()); - private final Intake intake = new Intake(new IntakeIOTalonFX()); - private final Pivot pivot = new Pivot(new PivotIOTalonFX()); - private final Flywheel flywheel = new Flywheel(new FlywheelIOTalonFX()); + // private final Indexer indexer = new Indexer(new IndexerIOTalonFX()); + // private final Intake intake = new Intake(new IntakeIOTalonFX()); + // private final Pivot pivot = new Pivot(new PivotIOTalonFX()); + // private final Flywheel flywheel = new Flywheel(new FlywheelIOTalonFX()); private final CommandXboxController driverController = new CommandXboxController(0); // Simulation, we store them here in the robot container @@ -74,6 +75,7 @@ public RobotContainer() { new PhysicalModule(SwerveConstants.moduleConfigs[1]), new PhysicalModule(SwerveConstants.moduleConfigs[2]), new PhysicalModule(SwerveConstants.moduleConfigs[3])); + visionSubsystem= new Vision(new VisionIOReal()); } case SIM -> { @@ -110,13 +112,16 @@ public RobotContainer() { new SimulatedModule(swerveDriveSimulation.getModules()[2]), new SimulatedModule(swerveDriveSimulation.getModules()[3])); + // TODO: add sim impl + visionSubsystem = new Vision(new VisionIO() {}); + SimulatedField.getInstance().resetFieldForAuto(); resetFieldAndOdometryForAuto(new Pose2d( 1.3980597257614136, 5.493067741394043, Rotation2d.fromRadians(3.1415))); - } default -> { + visionSubsystem = new Vision(new VisionIO() {}); /* Replayed robot, disable IO implementations */ /* physics simulations are also not needed */ diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 4007d602..ca1fbb6e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -188,7 +188,6 @@ private void modulesPeriodic() { * @param ySpeed Speed of the robot in the y direction, positive being left. * @param rotationSpeed Angular rate of the robot in radians per second. * @param fieldRelative Whether the provided x and y speeds are relative to the field. ->>>>>>> 52fbf88798bd786d25dffff6b4769182ad9ea51c */ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative) { SwerveModuleState[] swerveModuleStates = @@ -230,7 +229,7 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { * @param timestampIndex index of the timestamp to sample the pose at */ private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { - final SwerveModulePosition[] modulePositions = getModulesPosition(timestampIndex), + final SwerveModulePosition[] modulePositions = getModulePositions(), moduleDeltas = getModulesDelta(modulePositions); if (gyroInputs.isConnected) { diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index ff474ca9..0e33bf80 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -20,7 +20,7 @@ public class SimulatedModule implements ModuleInterface { private final PIDController drivePID = new PIDController(5, 0, 0); private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(5, 0, 0); - private final Constraints turnConstraints = new Constraints(5, 0); + private final Constraints turnConstraints = new Constraints(5,2); private final ProfiledPIDController turnPID = new ProfiledPIDController(5, 0, 0, turnConstraints); private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(5, 0, 0); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index ad9c1254..b5abe6ef 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -14,21 +14,21 @@ class VisionIOInputs { public int targetsCount = 0; } - void updateInputs(VisionIOInputs inputs); + default void updateInputs(VisionIOInputs inputs) {} - String getLimelightName(int limelightNumber); + default String getLimelightName(int limelightNumber) {return "";} - double getLatencySeconds(int limelightNumber); + default double getLatencySeconds(int limelightNumber) {return 0.0;} - double getTimeStampSeconds(int limelightNumber); + default double getTimeStampSeconds(int limelightNumber) {return 0.0;} - boolean canSeeAprilTags(int limelightNumber); + default boolean canSeeAprilTags(int limelightNumber) {return false;} - double getLimelightAprilTagDistance(int limelightNumber); + default double getLimelightAprilTagDistance(int limelightNumber) {return 0.0;} - int getNumberOfAprilTags(int limelightNumber); + default int getNumberOfAprilTags(int limelightNumber) {return 0;} - Pose2d getPoseFromAprilTags(int limelightNumber); + default Pose2d getPoseFromAprilTags(int limelightNumber) {return null;} - void setHeadingInfo(double headingDegrees, double headingRateDegrees); + default void setHeadingInfo(double headingDegrees, double headingRateDegrees) {} } From 5abf7b6026b6b47f4f82038c23daee4063faddcf Mon Sep 17 00:00:00 2001 From: Ishan1522 Date: Mon, 4 Nov 2024 08:09:31 -0500 Subject: [PATCH 02/75] what did i do :sob: --- .../frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index 0e33bf80..517ea9b5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -38,7 +38,7 @@ public void updateInputs(ModuleInputs inputs) { inputs.driveAppliedVolts = moduleSimulation.getDriveMotorAppliedVolts(); inputs.driveCurrentAmps = Math.abs(moduleSimulation.getDriveMotorSupplyCurrentAmps()); - // inputs.turnAbsolutePosition = moduleSimulation.getTurnAbsolutePosition(); + inputs.turnAbsolutePosition = moduleSimulation.getTurnAbsolutePosition(); inputs.turnPosition = Radians.of(moduleSimulation.getTurnRelativeEncoderPositionRad()).in(Rotations); inputs.turnVelocity = @@ -71,7 +71,7 @@ public void setDesiredState(SwerveModuleState desiredState) { if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) { moduleSimulation.requestDriveVoltageOut(0); - moduleSimulation.requestTurnVoltageOut(0); + // moduleSimulation.requestTurnVoltageOut(0); return; } From 0e65e23d427a2164275f452868bd1f8ad4636516 Mon Sep 17 00:00:00 2001 From: Ishan1522 Date: Mon, 4 Nov 2024 08:10:48 -0500 Subject: [PATCH 03/75] fix typo --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1b0cd7f0..3acdeed0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -104,7 +104,7 @@ public RobotContainer() { swerveDrive = new SwerveDrive( new SimulatedGyro( - gyroSimulation), // SimulateGyro is a wrapper around gyro simulation, that reads + gyroSimulation), // SimulatedGyro is a wrapper around gyro simulation, that reads // the simulation result /* SimulatedModule are edited such that they also wraps around module simulations */ new SimulatedModule(swerveDriveSimulation.getModules()[0]), From ceb11cfa79be91ed1add78a48ddcc5b9a648963d Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 4 Nov 2024 17:12:00 -0500 Subject: [PATCH 04/75] ishan changes before I setal his computer --- .../frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index 517ea9b5..292bc1b5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -84,7 +84,7 @@ public void setDesiredState(SwerveModuleState desiredState) { moduleSimulation.requestDriveVoltageOut( Volts.of( drivePID.calculate( - Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()), + RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()).in(RotationsPerSecond), desiredDriveRPS)) .plus(driveFF.calculate(RotationsPerSecond.of(desiredDriveRPS)))); moduleSimulation.requestTurnVoltageOut( From 2443f0f5aa864af51cc7b17eae4b48ee2869382b Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 4 Nov 2024 20:26:15 -0500 Subject: [PATCH 05/75] format --- src/main/java/frc/robot/Constants.java | 1 - src/main/java/frc/robot/RobotContainer.java | 48 ++++++++----------- .../frc/robot/commands/auto/BlueFourNote.java | 14 ++++-- .../commands/auto/FollowChoreoTrajectory.java | 2 +- .../commands/autodrive/AutoAlignWithAmp.java | 2 - .../robot/commands/drive/DriveCommand.java | 1 - .../intake/ManualIntakeandIndexerRollers.java | 4 +- .../commands/shooter/ManualShooterRoller.java | 4 +- .../commands/shooter/SetFlywheelSpeed.java | 2 - .../robot/commands/shooter/ShootSpeaker.java | 10 ++-- .../commands/shooter/ShootSubwoofer.java | 3 +- .../elevator/ElevatorIOTalonFX.java | 1 - .../frc/robot/subsystems/indexer/Indexer.java | 2 +- .../subsystems/indexer/IndexerConstants.java | 2 +- .../robot/subsystems/indexer/IndexerIO.java | 2 +- .../subsystems/indexer/IndexerIOTalonFX.java | 1 - .../subsystems/intake/IntakeIOTalonFX.java | 1 - .../subsystems/pivot/PivotIOTalonFX.java | 3 -- .../subsystems/shooter/FlywheelIOTalonFX.java | 1 - .../subsystems/swerve/SwerveConstants.java | 6 +-- .../robot/subsystems/swerve/SwerveDrive.java | 3 -- .../swerve/gyroIO/PhysicalGyro.java | 8 ++-- .../swerve/moduleIO/ModuleInterface.java | 4 +- .../swerve/moduleIO/SimulatedModule.java | 10 ++-- .../frc/robot/subsystems/vision/VisionIO.java | 28 ++++++++--- 25 files changed, 80 insertions(+), 83 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3aba09a2..f07409a4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,6 +1,5 @@ package frc.robot; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import frc.robot.subsystems.swerve.SwerveConstants.*; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3acdeed0..241ac374 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,40 +4,30 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.JoystickConstants; import frc.robot.commands.drive.DriveCommand; -import frc.robot.extras.characterization.FeedForwardCharacterization; import frc.robot.extras.simulation.field.SimulatedField; import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveDriveSimulation; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.DRIVE_WHEEL_TYPE; -import frc.robot.subsystems.indexer.Indexer; -import frc.robot.subsystems.indexer.IndexerIOTalonFX; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.intake.IntakeIOTalonFX; -import frc.robot.subsystems.pivot.Pivot; -import frc.robot.subsystems.pivot.PivotIOTalonFX; -import frc.robot.subsystems.shooter.Flywheel; -import frc.robot.subsystems.shooter.FlywheelIOTalonFX; import frc.robot.subsystems.swerve.SwerveConstants; import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; import frc.robot.subsystems.swerve.SwerveDrive; -import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.Logger; -import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionIO; -import frc.robot.subsystems.vision.VisionIOReal; import frc.robot.subsystems.swerve.gyroIO.GyroInterface; import frc.robot.subsystems.swerve.gyroIO.PhysicalGyro; import frc.robot.subsystems.swerve.gyroIO.SimulatedGyro; import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; import frc.robot.subsystems.swerve.moduleIO.PhysicalModule; import frc.robot.subsystems.swerve.moduleIO.SimulatedModule; +import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionIO; +import frc.robot.subsystems.vision.VisionIOReal; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; public class RobotContainer { @@ -49,7 +39,7 @@ public class RobotContainer { // private final Pivot pivot = new Pivot(new PivotIOTalonFX()); // private final Flywheel flywheel = new Flywheel(new FlywheelIOTalonFX()); private final CommandXboxController driverController = new CommandXboxController(0); - + // Simulation, we store them here in the robot container // private final SimulatedField simulatedArena; private final SwerveDriveSimulation swerveDriveSimulation; @@ -75,7 +65,7 @@ public RobotContainer() { new PhysicalModule(SwerveConstants.moduleConfigs[1]), new PhysicalModule(SwerveConstants.moduleConfigs[2]), new PhysicalModule(SwerveConstants.moduleConfigs[3])); - visionSubsystem= new Vision(new VisionIOReal()); + visionSubsystem = new Vision(new VisionIOReal()); } case SIM -> { @@ -104,7 +94,8 @@ public RobotContainer() { swerveDrive = new SwerveDrive( new SimulatedGyro( - gyroSimulation), // SimulatedGyro is a wrapper around gyro simulation, that reads + gyroSimulation), // SimulatedGyro is a wrapper around gyro simulation, that + // reads // the simulation result /* SimulatedModule are edited such that they also wraps around module simulations */ new SimulatedModule(swerveDriveSimulation.getModules()[0]), @@ -116,8 +107,8 @@ public RobotContainer() { visionSubsystem = new Vision(new VisionIO() {}); SimulatedField.getInstance().resetFieldForAuto(); - resetFieldAndOdometryForAuto(new Pose2d( - 1.3980597257614136, 5.493067741394043, Rotation2d.fromRadians(3.1415))); + resetFieldAndOdometryForAuto( + new Pose2d(1.3980597257614136, 5.493067741394043, Rotation2d.fromRadians(3.1415))); } default -> { @@ -148,7 +139,7 @@ private void resetFieldAndOdometryForAuto(Pose2d robotStartingPoseAtBlueAlliance updateFieldSimAndDisplay(); } - // swerveDrive.periodic(); + swerveDrive.periodic(); swerveDrive.setPose(startingPose); } @@ -226,7 +217,7 @@ private void configureButtonBindings() { // // autodrive // Trigger driverAButton = new Trigger(driverController::getAButton); -// lol whatever + // lol whatever // // intake // Trigger operatorLeftTrigger = new Trigger(()->operatorController.getLeftTriggerAxis() > 0.2); // Trigger operatorLeftBumper = new Trigger(operatorController::getLeftBumper); @@ -267,7 +258,6 @@ private void configureButtonBindings() { () -> driverLeftBumper.getAsBoolean()); swerveDrive.setDefaultCommand(driveCommand); - // // shooterSubsystem.setDefaultCommand(new FlywheelSpinUpAuto(shooterSubsystem, // visionSubsystem)); @@ -343,12 +333,16 @@ private void configureButtonBindings() { public Command getAutonomousCommand() { // Resets the pose factoring in the robot side // This is just a failsafe, pose should be reset at the beginning of auto - swerveDrive.setPose(new Pose2d(swerveDrive.getPose().getX(), - swerveDrive.getPose().getY(), - Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset()))); + swerveDrive.setPose( + new Pose2d( + swerveDrive.getPose().getX(), + swerveDrive.getPose().getY(), + Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset()))); // return autoChooser.getSelected(); // return new DriveForwardAndBack(swerveDrive); - return null;} + return null; + } + public void updateFieldSimAndDisplay() { if (swerveDriveSimulation == null) return; Logger.recordOutput( diff --git a/src/main/java/frc/robot/commands/auto/BlueFourNote.java b/src/main/java/frc/robot/commands/auto/BlueFourNote.java index 596c4754..567e03ef 100644 --- a/src/main/java/frc/robot/commands/auto/BlueFourNote.java +++ b/src/main/java/frc/robot/commands/auto/BlueFourNote.java @@ -29,23 +29,27 @@ // // .withTimeout(1.5), // // new ParallelCommandGroup( // new FollowChoreoTrajectory(driveSubsystem, "blue to note 1", false), -// // new IntakeAuto(intakeSubsystem, pivotSubsystem, shooterSubsystem, leds).withTimeout(2)), +// // new IntakeAuto(intakeSubsystem, pivotSubsystem, shooterSubsystem, +// leds).withTimeout(2)), // // new ShootSpeakerAuto( // // driveSubsystem, shooterSubsystem, pivotSubsystem, visionSubsystem, leds) // // .withTimeout(1.5), // // new ParallelCommandGroup( // new FollowChoreoTrajectory(driveSubsystem, "blue note 1 to 2", false), -// // new IntakeAuto(intakeSubsystem, pivotSubsystem, shooterSubsystem, leds).withTimeout(2)), +// // new IntakeAuto(intakeSubsystem, pivotSubsystem, shooterSubsystem, +// leds).withTimeout(2)), // // new ShootSpeakerAuto( // // driveSubsystem, shooterSubsystem, pivotSubsystem, visionSubsystem, leds) // // .withTimeout(1.7), // // new ParallelCommandGroup( // new FollowChoreoTrajectory(driveSubsystem, "blue note 2 to 3", false)); -// // new IntakeAuto(intakeSubsystem, pivotSubsystem, shooterSubsystem, leds).withTimeout(3)), -// // new IntakeAuto(intakeSubsystem, pivotSubsystem, shooterSubsystem, leds).withTimeout(1), +// // new IntakeAuto(intakeSubsystem, pivotSubsystem, shooterSubsystem, +// leds).withTimeout(3)), +// // new IntakeAuto(intakeSubsystem, pivotSubsystem, shooterSubsystem, +// leds).withTimeout(1), // // new ShootSpeakerAuto( // // driveSubsystem, shooterSubsystem, pivotSubsystem, visionSubsystem, leds) // // .withTimeout(2.2), // // new StopShooterAndIntake(intakeSubsystem, pivotSubsystem, shooterSubsystem)); // } -// } \ No newline at end of file +// } diff --git a/src/main/java/frc/robot/commands/auto/FollowChoreoTrajectory.java b/src/main/java/frc/robot/commands/auto/FollowChoreoTrajectory.java index 3e9549e0..11ec6d81 100644 --- a/src/main/java/frc/robot/commands/auto/FollowChoreoTrajectory.java +++ b/src/main/java/frc/robot/commands/auto/FollowChoreoTrajectory.java @@ -79,4 +79,4 @@ // public boolean isFinished() { // return controllerCommand.isFinished(); // } -// } \ No newline at end of file +// } diff --git a/src/main/java/frc/robot/commands/autodrive/AutoAlignWithAmp.java b/src/main/java/frc/robot/commands/autodrive/AutoAlignWithAmp.java index 7f5cc273..4fed7570 100644 --- a/src/main/java/frc/robot/commands/autodrive/AutoAlignWithAmp.java +++ b/src/main/java/frc/robot/commands/autodrive/AutoAlignWithAmp.java @@ -10,8 +10,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.FieldConstants; -import frc.robot.Constants.HardwareConstants; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.swerve.SwerveDrive; import frc.robot.subsystems.vision.Vision; diff --git a/src/main/java/frc/robot/commands/drive/DriveCommand.java b/src/main/java/frc/robot/commands/drive/DriveCommand.java index 24fbcf7b..1b691fc5 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommand.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommand.java @@ -1,6 +1,5 @@ package frc.robot.commands.drive; -import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; import frc.robot.subsystems.swerve.SwerveDrive; import frc.robot.subsystems.vision.Vision; diff --git a/src/main/java/frc/robot/commands/intake/ManualIntakeandIndexerRollers.java b/src/main/java/frc/robot/commands/intake/ManualIntakeandIndexerRollers.java index 4fd63375..3751eddc 100644 --- a/src/main/java/frc/robot/commands/intake/ManualIntakeandIndexerRollers.java +++ b/src/main/java/frc/robot/commands/intake/ManualIntakeandIndexerRollers.java @@ -9,7 +9,6 @@ import frc.robot.subsystems.indexer.IndexerConstants; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeConstants; -import java.util.function.DoubleSupplier; public class ManualIntakeandIndexerRollers extends Command { private final Intake intake; @@ -33,7 +32,8 @@ public void initialize() {} @Override public void execute() { intake.setIntakeSpeed(direction ? IntakeConstants.INTAKE_SPEED : -IntakeConstants.INTAKE_SPEED); - indexer.setIndexerSpeed(direction ? IntakeConstants.INTAKE_SPEED : -IntakeConstants.INTAKE_SPEED); + indexer.setIndexerSpeed( + direction ? IntakeConstants.INTAKE_SPEED : -IntakeConstants.INTAKE_SPEED); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/shooter/ManualShooterRoller.java b/src/main/java/frc/robot/commands/shooter/ManualShooterRoller.java index a6d7e249..1bc1f0f8 100644 --- a/src/main/java/frc/robot/commands/shooter/ManualShooterRoller.java +++ b/src/main/java/frc/robot/commands/shooter/ManualShooterRoller.java @@ -4,14 +4,14 @@ package frc.robot.commands.shooter; -import java.util.function.DoubleSupplier; - import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.shooter.Flywheel; +import java.util.function.DoubleSupplier; public class ManualShooterRoller extends Command { private Flywheel flywheel; private DoubleSupplier speed; + /** Creates a new ManualShooterRoller. */ public ManualShooterRoller(Flywheel flywheel, DoubleSupplier speed) { this.flywheel = flywheel; diff --git a/src/main/java/frc/robot/commands/shooter/SetFlywheelSpeed.java b/src/main/java/frc/robot/commands/shooter/SetFlywheelSpeed.java index fa7df957..15c26290 100644 --- a/src/main/java/frc/robot/commands/shooter/SetFlywheelSpeed.java +++ b/src/main/java/frc/robot/commands/shooter/SetFlywheelSpeed.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.shooter.Flywheel; import frc.robot.subsystems.shooter.ShooterConstants; - import java.util.function.DoubleSupplier; public class SetFlywheelSpeed extends Command { @@ -39,7 +38,6 @@ public void end(boolean interrupted) { flywheel.setFlywheelVelocity(ShooterConstants.SHOOTER_NEUTRAL_SPEED); } - // Returns true when the command should end. @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/shooter/ShootSpeaker.java b/src/main/java/frc/robot/commands/shooter/ShootSpeaker.java index 3cbaf9e2..012ec173 100644 --- a/src/main/java/frc/robot/commands/shooter/ShootSpeaker.java +++ b/src/main/java/frc/robot/commands/shooter/ShootSpeaker.java @@ -9,8 +9,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.FieldConstants; -import frc.robot.Constants.HardwareConstants; import frc.robot.Constants.JoystickConstants; import frc.robot.subsystems.elevator.Elevator; import frc.robot.subsystems.elevator.ElevatorConstants; @@ -79,10 +77,10 @@ public void initialize() { // sets alliance to red isRed = alliance.isPresent() && alliance.get() == Alliance.Red; - speakerPos = new Translation2d(0,0); - // isRed - // ? new Translation2d(FieldConstants.RED_SPEAKER_X, FieldConstants.RED_SPEAKER_Y) - // : new Translation2d(FieldConstants.BLUE_SPEAKER_X, FieldConstants.BLUE_SPEAKER_Y); + speakerPos = new Translation2d(0, 0); + // isRed + // ? new Translation2d(FieldConstants.RED_SPEAKER_X, FieldConstants.RED_SPEAKER_Y) + // : new Translation2d(FieldConstants.BLUE_SPEAKER_X, FieldConstants.BLUE_SPEAKER_Y); turnController.enableContinuousInput(-Math.PI, Math.PI); } diff --git a/src/main/java/frc/robot/commands/shooter/ShootSubwoofer.java b/src/main/java/frc/robot/commands/shooter/ShootSubwoofer.java index e2b5d0a0..a148c5ec 100644 --- a/src/main/java/frc/robot/commands/shooter/ShootSubwoofer.java +++ b/src/main/java/frc/robot/commands/shooter/ShootSubwoofer.java @@ -13,6 +13,7 @@ public class ShootSubwoofer extends Command { private final Pivot pivot; private final Flywheel flywheel; + /** Creates a new ShootSubwoofer. */ public ShootSubwoofer(Pivot pivot, Flywheel flywheel) { this.pivot = pivot; @@ -41,7 +42,7 @@ public void end(boolean interrupted) { flywheel.setFlywheelVelocity(ShooterConstants.SHOOTER_NEUTRAL_SPEED); pivot.setPivotAngle(PivotConstants.PIVOT_INTAKE_ANGLE); flywheel.setRollerSpeed(ShooterConstants.ROLLER_NEUTRAL_SPEED); - } + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java index e7f209d8..e317aa1f 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java @@ -12,7 +12,6 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index c3efcfc8..4431710c 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -20,7 +20,7 @@ public void setIndexerSpeed(double speed) { io.setSpeed(speed); Logger.recordOutput("Indexer", speed); } - + /** Stops the motor */ public void stop() { io.stop(); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 7d96883a..b068ef1b 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -5,4 +5,4 @@ public final class IndexerConstants { public static final double INTAKE_SPEED = 0.8; public static final double OUTTAKE_SPEED = 0 - 9; public static final double INDEXER_PASS_SPEED = 0.1; -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java index 11dd4da8..3b06c92f 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java @@ -14,7 +14,7 @@ public static class IndexerIOInputs { } default void updateInputs(IndexerIOInputs inputs) {} - + default boolean isIndexing() { return false; } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java index a9d93572..bb2c0781 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java index ebbe7f29..ff6aa355 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.Constants.HardwareConstants; diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOTalonFX.java index c4c02b63..ad305b48 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOTalonFX.java @@ -5,8 +5,6 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -14,7 +12,6 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java index c16cec0c..030a5ca0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index 36a426fc..0e3a6266 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -109,9 +109,9 @@ public class ModuleConstants { public static final double TURN_I = 0.0; public static final double TURN_D = 0.0; - public static final double TURN_S = 0; - public static final double TURN_V = 0.0; - public static final double TURN_A = 0.0; + public static final double TURN_S = 0; + public static final double TURN_V = 0.0; + public static final double TURN_A = 0.0; public static final double MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND = 30; public static final double MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED = 24; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index ca1fbb6e..478bc210 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -1,4 +1,3 @@ - package frc.robot.subsystems.swerve; import static edu.wpi.first.units.Units.*; @@ -24,10 +23,8 @@ import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; import frc.robot.subsystems.swerve.odometryThread.OdometryThread; import frc.robot.subsystems.swerve.odometryThread.OdometryThreadInputsAutoLogged; -import frc.robot.subsystems.swerve.SwerveModule; import frc.robot.subsystems.vision.VisionConstants; import java.util.Optional; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java index 82e86312..79c44d11 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java @@ -2,16 +2,14 @@ import static edu.wpi.first.units.Units.*; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; +import com.studica.frc.AHRS.NavXUpdateRate; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj.SPI; import frc.robot.subsystems.swerve.odometryThread.OdometryThread; import java.util.Queue; -import com.studica.frc.AHRS; -import com.studica.frc.AHRS.NavXComType; -import com.studica.frc.AHRS.NavXUpdateRate; - public class PhysicalGyro implements GyroInterface { private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI, NavXUpdateRate.k200Hz); private final Queue yawPositionInput; diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java index 60508406..25b0bf9e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java @@ -46,5 +46,7 @@ default void setTurnVoltage(Voltage voltage) {} default void stopModule() {} - default double getTurnRotations() {return 0.0;} + default double getTurnRotations() { + return 0.0; + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index 292bc1b5..41d68c08 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; import frc.robot.extras.simulation.OdometryTimestampsSim; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation; @@ -20,8 +19,10 @@ public class SimulatedModule implements ModuleInterface { private final PIDController drivePID = new PIDController(5, 0, 0); private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(5, 0, 0); - private final Constraints turnConstraints = new Constraints(5,2); - private final ProfiledPIDController turnPID = new ProfiledPIDController(5, 0, 0, turnConstraints); + + private final Constraints turnConstraints = new Constraints(5, 2); + private final ProfiledPIDController turnPID = + new ProfiledPIDController(50, 0, 0, turnConstraints); private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(5, 0, 0); public SimulatedModule(SwerveModuleSimulation moduleSimulation) { @@ -84,7 +85,8 @@ public void setDesiredState(SwerveModuleState desiredState) { moduleSimulation.requestDriveVoltageOut( Volts.of( drivePID.calculate( - RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()).in(RotationsPerSecond), + RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) + .in(RotationsPerSecond), desiredDriveRPS)) .plus(driveFF.calculate(RotationsPerSecond.of(desiredDriveRPS)))); moduleSimulation.requestTurnVoltageOut( diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index b5abe6ef..668cfeb0 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -16,19 +16,33 @@ class VisionIOInputs { default void updateInputs(VisionIOInputs inputs) {} - default String getLimelightName(int limelightNumber) {return "";} + default String getLimelightName(int limelightNumber) { + return ""; + } - default double getLatencySeconds(int limelightNumber) {return 0.0;} + default double getLatencySeconds(int limelightNumber) { + return 0.0; + } - default double getTimeStampSeconds(int limelightNumber) {return 0.0;} + default double getTimeStampSeconds(int limelightNumber) { + return 0.0; + } - default boolean canSeeAprilTags(int limelightNumber) {return false;} + default boolean canSeeAprilTags(int limelightNumber) { + return false; + } - default double getLimelightAprilTagDistance(int limelightNumber) {return 0.0;} + default double getLimelightAprilTagDistance(int limelightNumber) { + return 0.0; + } - default int getNumberOfAprilTags(int limelightNumber) {return 0;} + default int getNumberOfAprilTags(int limelightNumber) { + return 0; + } - default Pose2d getPoseFromAprilTags(int limelightNumber) {return null;} + default Pose2d getPoseFromAprilTags(int limelightNumber) { + return null; + } default void setHeadingInfo(double headingDegrees, double headingRateDegrees) {} } From eb783a93e5434105e5a6e69af519cec56492d7b9 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Tue, 5 Nov 2024 09:11:47 -0500 Subject: [PATCH 06/75] im actually so dumb --- .../frc/robot/commands/drive/DriveCommand.java | 4 +++- .../frc/robot/subsystems/swerve/SwerveDrive.java | 8 +++++++- .../frc/robot/subsystems/swerve/SwerveModule.java | 14 +++++++++++++- 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive/DriveCommand.java b/src/main/java/frc/robot/commands/drive/DriveCommand.java index 1b691fc5..be6f7bce 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommand.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommand.java @@ -6,6 +6,8 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj2.command.Command; + public class DriveCommand extends DriveCommandBase { private final SwerveDrive driveSubsystem; @@ -61,7 +63,7 @@ public void execute() { rightJoystickX.getAsDouble() * angularSpeed, isFieldRelative.getAsBoolean()); - // Runs all the code from DriveCommand that estimates pose + // Runs all the code from DriveCommandBase that estimates pose super.execute(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 478bc210..1b968bcd 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -226,7 +226,8 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { * @param timestampIndex index of the timestamp to sample the pose at */ private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { - final SwerveModulePosition[] modulePositions = getModulePositions(), + // if (timestampIndex == 0) return; + final SwerveModulePosition[] modulePositions = getModulesPosition(timestampIndex), moduleDeltas = getModulesDelta(modulePositions); if (gyroInputs.isConnected) { @@ -250,6 +251,11 @@ private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { * rotation at the sampled timestamp. */ private SwerveModulePosition[] getModulesPosition(int timestampIndex) { + // Check if swerveModules is null or empty, return early if so + if (swerveModules == null || swerveModules.length == 0) { + // Logger.error("Swerve modules array is empty or not initialized properly!"); + return new SwerveModulePosition[4]; // Return an empty array of the expected size + } SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[swerveModules.length]; for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) swerveModulePositions[moduleIndex] = diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index d10c22e5..e16c18d9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -41,7 +41,19 @@ public void updateOdometryInputs() { } @Override - public void periodic() {} + public void periodic() { + updateOdometryPositions(); + } + + + private void updateOdometryPositions() { + odometryPositions = new SwerveModulePosition[inputs.odometryTimestamps.length]; + for (int i = 0; i < odometryPositions.length; i++) { + double positionMeters = inputs.drivePosition; + Rotation2d angle = inputs.turnAbsolutePosition; + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } +} public void setVoltage(Voltage volts) { io.setDriveVoltage(volts); From b6fb3c66039a492e2f1c3601a115b8e4e296999d Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Tue, 5 Nov 2024 10:49:30 -0500 Subject: [PATCH 07/75] and this proves it --- src/main/java/frc/robot/RobotContainer.java | 35 +- .../robot/commands/drive/DriveCommand.java | 2 - .../simulation/field/SimulatedField.java | 25 + .../swerve/BrushlessMotorSim.java | 775 ++++--- .../swerve/SwerveModuleSimulation.java | 1893 ++++++++++++++--- .../robot/subsystems/swerve/SwerveDrive.java | 9 +- .../robot/subsystems/swerve/SwerveModule.java | 6 +- .../swerve/moduleIO/ModuleInterface.java | 4 +- .../swerve/moduleIO/PhysicalModule.java | 2 - .../swerve/moduleIO/SimulatedModule.java | 35 +- 10 files changed, 2073 insertions(+), 713 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 241ac374..73385411 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.JoystickConstants; @@ -80,10 +81,10 @@ public RobotContainer() { 45, DriveConstants.TRACK_WIDTH, DriveConstants.WHEEL_BASE, - DriveConstants.TRACK_WIDTH, - DriveConstants.WHEEL_BASE, + DriveConstants.TRACK_WIDTH +.2, + DriveConstants.WHEEL_BASE + .2, SwerveModuleSimulation.getModule( - DCMotor.getKrakenX60(1), + DCMotor.getFalcon500(1), DCMotor.getFalcon500(1), 60, DRIVE_WHEEL_TYPE.TIRE, @@ -266,48 +267,48 @@ private void configureButtonBindings() { // driverLeftTrigger.whileFalse(new TowerIntake(intakeSubsystem, pivotSubsystem, // shooterSubsystem, false, ledSubsystem, this::intakeCallback).withTimeout(0.3)); // // Amp Lineup - // driverAButton.whileTrue(new AutoAlignWithAmp(driveSubsystem, visionSubsystem)); + // driverAButton.whileTrue(new AutoAlignWithAmp(swerveDrive, visionSubsystem)); // // Spinup for shoot - // driverRightTrigger.whileTrue(new SpinUpForSpeaker(driveSubsystem, shooterSubsystem, + // driverRightTrigger.whileTrue(new SpinUpForSpeaker(swerveDrive, shooterSubsystem, // pivotSubsystem, visionSubsystem, driverLeftStickX, driverLeftStickY, driverRightBumper, // ledSubsystem)); - // // driverLeftBumper.whileTrue(new ShootSpeaker(driveSubsystem, shooterSubsystem, + // // driverLeftBumper.whileTrue(new ShootSpeaker(swerveDrive, shooterSubsystem, // pivotSubsystem, visionSubsystem, driverLeftStickX, operatorLeftStickY, driverRightBumper, // ledSubsystem)); - // // driverRightTrigger.whileTrue(new ShootWhileMove(driveSubsystem, shooterSubsystem, + // // driverRightTrigger.whileTrue(new ShootWhileMove(swerveDrive, shooterSubsystem, // pivotSubsystem, visionSubsystem, driverLeftStick, driverYButton, ledSubsystem)); // // Resets the robot angle in the odometry, factors in which alliance the robot is on - // driverRightDirectionPad.onTrue(new InstantCommand(() -> driveSubsystem.resetOdometry(new - // Pose2d(driveSubsystem.getPose().getX(), driveSubsystem.getPose().getY(), - // Rotation2d.fromDegrees(driveSubsystem.getAllianceAngleOffset()))))); + driverRightDirectionPad.onTrue(new InstantCommand(() -> swerveDrive.setPose(new + Pose2d(swerveDrive.getPose().getX(), swerveDrive.getPose().getY(), + Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset()))))); // // // Reset robot odometry based on vision pose measurement from april tags // driverLeftDirectionPad.onTrue(new InstantCommand(() -> - // driveSubsystem.resetOdometry(visionSubsystem.getLastSeenPose()))); - // // driverLeftDpad.onTrue(new InstantCommand(() -> driveSubsystem.resetOdometry(new + // swerveDrive.resetOdometry(visionSubsystem.getLastSeenPose()))); + // // driverLeftDpad.onTrue(new InstantCommand(() -> swerveDrive.resetOdometry(new // Pose2d(15.251774787902832, 5.573054313659668, Rotation2d.fromRadians(3.14159265))))); - // // driverBButton.whileTrue(new ShootPass(driveSubsystem, shooterSubsystem, pivotSubsystem, + // // driverBButton.whileTrue(new ShootPass(swerveDrive, shooterSubsystem, pivotSubsystem, // visionSubsystem, driverLeftStickX, driverLeftStickY, driverRightBumper, ledSubsystem)); // // driverXButton. - // driverBButton.whileTrue(new ShootPass(driveSubsystem, shooterSubsystem, pivotSubsystem, + // driverBButton.whileTrue(new ShootPass(swerveDrive, shooterSubsystem, pivotSubsystem, // visionSubsystem, driverLeftStickY, operatorLeftStickY, driverYButton, ledSubsystem)); // // driverDownDirectionPad.whileTrue(new IntakeFromShooter(shooterSubsystem, // intakeSubsystem)); - // // driverYButton.whileTrue(new ShootSpeaker(driveSubsystem, shooterSubsystem, pivotSubsystem, + // // driverYButton.whileTrue(new ShootSpeaker(swerveDrive, shooterSubsystem, pivotSubsystem, // visionSubsystem, driverLeftStickX, operatorLeftStickY, driverRightBumper, ledSubsystem)); // // OPERATOR BUTTONS // // speaker - // operatorRightTrigger.whileTrue(new ShootSpeaker(driveSubsystem, shooterSubsystem, + // operatorRightTrigger.whileTrue(new ShootSpeaker(swerveDrive, shooterSubsystem, // pivotSubsystem, visionSubsystem, driverLeftStickX, driverLeftStickY, driverRightBumper, // ledSubsystem)); // // amp // operatorRightBumper.whileTrue(new ShootAmp(shooterSubsystem, pivotSubsystem, ledSubsystem, // operatorBButton)); // // fender shot - // operatorUpDirectionPad.whileTrue(new SubwooferShot(driveSubsystem, shooterSubsystem, + // operatorUpDirectionPad.whileTrue(new SubwooferShot(swerveDrive, shooterSubsystem, // pivotSubsystem, visionSubsystem, driverLeftStickX, driverLeftStickY, driverRightStickX, // driverRightBumper, ledSubsystem)); // // intake (aka SUCC_BUTTON) diff --git a/src/main/java/frc/robot/commands/drive/DriveCommand.java b/src/main/java/frc/robot/commands/drive/DriveCommand.java index be6f7bce..a777ddda 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommand.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommand.java @@ -6,8 +6,6 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; -import edu.wpi.first.wpilibj2.command.Command; - public class DriveCommand extends DriveCommandBase { private final SwerveDrive driveSubsystem; diff --git a/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java b/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java index d991fa87..34a02bd9 100644 --- a/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java +++ b/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java @@ -3,12 +3,18 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.extras.simulation.gamePiece.CrescendoNoteSimulation; import frc.robot.extras.simulation.gamePiece.GamePieceSimulation; import frc.robot.extras.simulation.mechanismSim.IntakeSimulation; import frc.robot.extras.simulation.mechanismSim.swerve.AbstractDriveTrainSimulation; +import frc.robot.extras.simulation.mechanismSim.swerve.BrushlessMotorSim; import frc.robot.extras.util.GeomUtil; + +import java.lang.ref.WeakReference; import java.util.*; import org.dyn4j.dynamics.Body; import org.dyn4j.dynamics.BodyFixture; @@ -125,6 +131,7 @@ public static void overrideSimulationTimings( protected final Set driveTrainSimulations; protected final Set gamePieces; protected final List simulationSubTickActions; + protected final List> motors; private final List intakeSimulations; /** @@ -149,6 +156,7 @@ protected SimulatedField(FieldMap obstaclesMap) { simulationSubTickActions = new ArrayList<>(); this.gamePieces = new HashSet<>(); this.intakeSimulations = new ArrayList<>(); + motors = new ArrayList<>(); } /** @@ -276,6 +284,11 @@ public void simulationPeriodic() { "MapleArenaSimulation/Dyn4jEngineCPUTimeMS", (System.nanoTime() - t0) / 1000000.0); } + + public void addMotor(BrushlessMotorSim motor) { + motors.add(new WeakReference<>(motor)); + } + /** * * @@ -293,6 +306,18 @@ public void simulationPeriodic() { * */ private void simulationSubTick() { + ArrayList motorCurrents = new ArrayList<>(); + for (var motor : motors) { + BrushlessMotorSim motorRef = motor.get(); + if (motorRef != null) { + motorRef.update(); + } + } + double vin = BatterySim.calculateLoadedBatteryVoltage( + 12.2, + 0.015, + motorCurrents.stream().mapToDouble(Double::doubleValue).toArray()); + RoboRioSim.setVInVoltage(vin); for (AbstractDriveTrainSimulation driveTrainSimulation : driveTrainSimulations) driveTrainSimulation.simulationSubTick(); diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java index 5ba31229..873c4982 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java @@ -1,343 +1,470 @@ -// A drop-in replacement for WPILib's DCMotorSim to simulate modern brushless motors -// Copyright (C) 2024 Team-5516-"Iron Maple" -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -// -// Original source: -// https://github.com/Shenzhen-Robotics-Alliance/maple-sim/blob/main/src/main/java/org/ironmaple/simulation/drivesims/BrushlessMotorSim.java - package frc.robot.extras.simulation.mechanismSim.swerve; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.NewtonMeters; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.AngularAccelerationUnit; +import edu.wpi.first.units.AngularVelocityUnit; +import edu.wpi.first.units.CurrentUnit; +import edu.wpi.first.units.PerUnit; +import edu.wpi.first.units.VoltageUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.units.measure.Per; +import edu.wpi.first.units.measure.Torque; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.extras.simulation.field.SimulatedField; /** * * *

{@link DCMotorSim} with a bit of extra spice.

* - *

By Team 5516 "IRON MAPLE". - * - *

This class extends the functionality of the original {@link DCMotorSim} and models the - * following aspects in addition: + *

This class extends the functionality of the original {@link DCMotorSim} and models the following aspects in + * addition: * *

    *
  • Friction force on the rotor. *
  • Smart current limiting. *
  • Brake and coast modes (only for simulating brushless motors). - *
  • Simulated encoder readings. *
*/ public class BrushlessMotorSim { - private final DCMotor motor; - private final double gearRatio, frictionTorque, loadInertiaJKgMetersSquared; - private double requestedVoltageOutput, currentLimitAmps; - private boolean breakModeEnabled; - - private double angularPositionRad, angularVelocityRadPerSec, appliedVolts, currentDrawAmps; - - /** - * - * - *

Constructs a Brushless Motor Simulation Instance.

- * - * @param motor the {@link DCMotor} model representing the motor(s) in the simulation - * @param gearRatio the gear ratio of the mechanism; values greater than 1 indicate a reduction - * @param loadIntertiaJKgMetersSquared the rotational inertia of the mechanism, in kg·m² - * @param frictionVoltage the voltage required to overcome friction and make the mechanism move - */ - public BrushlessMotorSim( - DCMotor motor, - double gearRatio, - double loadIntertiaJKgMetersSquared, - double frictionVoltage) { - this.motor = motor; - this.gearRatio = gearRatio; - this.frictionTorque = motor.getTorque(motor.getCurrent(0, frictionVoltage)) * gearRatio; - - this.loadInertiaJKgMetersSquared = loadIntertiaJKgMetersSquared; - disableCurrentLimit(); - this.breakModeEnabled = true; - - this.angularPositionRad = angularVelocityRadPerSec = appliedVolts = currentDrawAmps = 0; - } - - /** - * - * - *

Requests an Output Voltage for the Motor.

- * - *

Note: The requested voltage may not always be fully applied due to current - * limits. For details on current limiting, refer to {@link #enableCurrentLimit(double)}. - * - * @param volts the requested output voltage for the motor - */ - public void requestVoltageOutput(double volts) { - this.requestedVoltageOutput = volts; - } - - /** - * - * - *

Configures a Current Limit for the Motor.

- * - *

If the motor's supply current exceeds the specified limit, the motor will reduce its output - * to prevent exceeding the limit. - * - * @param currentLimitAmps the maximum allowed current, in amperes - */ - public void enableCurrentLimit(double currentLimitAmps) { - this.currentLimitAmps = currentLimitAmps; - } - - /** - * - * - *

Disables the Current Limit of the Motor.

- * - *

This method removes the current limit, allowing the motor to operate without any current - * restrictions. - */ - public void disableCurrentLimit() { - this.currentLimitAmps = Double.POSITIVE_INFINITY; - } - - /** - * - * - *

Configures the Zero Power Behavior of the Motor.

- * - *

This method sets the motor's zero power behavior, similar to the - * setZeroPowerBehavior() method for brushless motors. - * - *

Use this feature only when simulating brushless motors. - * - * @param enabled true to enable brake mode, false to enable coast mode - */ - public void setMotorBrakeEnabled(boolean enabled) { - this.breakModeEnabled = enabled; - } - - /** - * - * - *

Sets the Current State of the Motor.

- * - *

This method instantly shifts the motor to a given state, setting its angular position and - * velocity. - * - *

Equivalent to the {@link DCMotorSim#setState(double, double)} method. - * - * @param angularPositionRad the angular position of the motor, in radians - * @param angularVelocityRadPerSec the angular velocity of the motor, in radians per second - */ - public void setState(double angularPositionRad, double angularVelocityRadPerSec) { - this.angularPositionRad = angularPositionRad; - this.angularVelocityRadPerSec = angularVelocityRadPerSec; - } - - /** - * - * - *

Updates the Simulation.

- * - *

This method steps the motor simulation forward by a given time interval (dt), - * recalculating and storing the states of the motor. - * - *

Equivalent to {@link DCMotorSim#update(double)}. - * - * @param dtSeconds the time step for the simulation, in seconds - */ - public void update(double dtSeconds) { - appliedVolts = - constrainOutputVoltage( - motor, angularVelocityRadPerSec * gearRatio, currentLimitAmps, requestedVoltageOutput); - - double totalTorque = getMotorElectricTorque(); - - /* apply friction force */ - final boolean electricTorqueResultingInAcceleration = - totalTorque * angularVelocityRadPerSec > 0; - if (electricTorqueResultingInAcceleration) - totalTorque = MathUtil.applyDeadband(totalTorque, frictionTorque, Double.POSITIVE_INFINITY); - else totalTorque += getCurrentFrictionTorque(); - - this.angularVelocityRadPerSec += totalTorque / loadInertiaJKgMetersSquared * dtSeconds; - this.angularPositionRad += this.angularVelocityRadPerSec * dtSeconds; - } - - /** - * - * - *

Calculates the Electric Torque on the Rotor.

- * - * @return the torque applied to the mechanism by the motor's electrical output - */ - private double getMotorElectricTorque() { - if (!breakModeEnabled && appliedVolts == 0) return currentDrawAmps = 0; - currentDrawAmps = motor.getCurrent(angularVelocityRadPerSec * gearRatio, appliedVolts); - return motor.getTorque(currentDrawAmps) * gearRatio; - } - - /** - * - * - *

Calculates the Dynamic Friction Torque on the Mechanism.

- * - *

This method simulates the amount of dynamic friction acting on the rotor when the mechanism - * is rotating. - * - *

In the real world, dynamic friction torque is typically constant. However, in the - * simulation, it is made proportional to the rotor speed when the speed is small to avoid - * oscillation. - * - * @return the amount of dynamic friction torque on the mechanism, in Newton-meters - */ - private double getCurrentFrictionTorque() { - final double kFriction = 3.0, - percentAngularVelocity = - Math.abs(angularVelocityRadPerSec) * gearRatio / motor.freeSpeedRadPerSec, - currentFrictionTorqueMagnitude = - Math.min(percentAngularVelocity * kFriction * frictionTorque, frictionTorque); - return Math.copySign(currentFrictionTorqueMagnitude, -angularVelocityRadPerSec); - } - - /** - * - * - *

Constrains the Output Voltage of the Motor.

- * - *

This method constrains the output voltage of the motor to ensure it operates within the - * current limit and does not exceed the available voltage from the robot's system. - * - *

The output voltage is constrained such that the motor does not function outside of the - * specified current limit. Additionally, it ensures that the voltage does not exceed the robot's - * input voltage, which can be obtained from {@link RoboRioSim#getVInVoltage()}. - * - * @param motor the {@link DCMotor} that models the motor - * @param motorCurrentVelocityRadPerSec the current velocity of the motor's rotor (geared), in - * radians per second - * @param currentLimitAmps the configured current limit, in amperes. You can configure the current - * limit using {@link #enableCurrentLimit(double)}. - * @param requestedOutputVoltage the requested output voltage - * @return the constrained output voltage based on the current limit and system voltage - */ - public static double constrainOutputVoltage( - DCMotor motor, - double motorCurrentVelocityRadPerSec, - double currentLimitAmps, - double requestedOutputVoltage) { - final double currentAtRequestedVolts = - motor.getCurrent(motorCurrentVelocityRadPerSec, requestedOutputVoltage); - - /* normally, motor controller starts cutting the supply voltage when the current exceed 120% the current limit */ - final boolean currentTooHigh = Math.abs(currentAtRequestedVolts) > 1.2 * currentLimitAmps; - double limitedVoltage = requestedOutputVoltage; - if (currentTooHigh) { - final double limitedCurrent = Math.copySign(currentLimitAmps, currentAtRequestedVolts); - limitedVoltage = - motor.getVoltage(motor.getTorque(limitedCurrent), motorCurrentVelocityRadPerSec); - } - - if (Math.abs(limitedVoltage) > Math.abs(requestedOutputVoltage)) - limitedVoltage = requestedOutputVoltage; - - return MathUtil.clamp(limitedVoltage, -RoboRioSim.getVInVoltage(), RoboRioSim.getVInVoltage()); - } - - /** - * - * - *

Obtains the Voltage Actually Applied to the Motor.

- * - *

This method returns the voltage that is actually applied to the motor after being - * constrained by {@link #constrainOutputVoltage(DCMotor, double, double, double)}. - * - *

The voltage is constrained to ensure that the current limit is not exceeded and that it does - * not surpass the robot's battery voltage. - * - * @return the constrained voltage actually applied to the motor - */ - public double getAppliedVolts() { - return appliedVolts; - } - - /** - * - * - *

Obtains the Angular Position of the Mechanism.

- * - *

This method is equivalent to {@link DCMotorSim#getAngularPositionRad()}. - * - * @return the final angular position of the mechanism, in radians - */ - public double getAngularPositionRad() { - return angularPositionRad; - } - - /** - * - * - *

Obtains the Angular Position of the Motor.

- * - * @return the un-geared angular position of the motor (encoder reading), in radians - */ - public double getEncoderPositionRad() { - return angularPositionRad * gearRatio; - } - - /** - * - * - *

Obtains the Angular Velocity of the Mechanism.

- * - *

This method is equivalent to {@link DCMotorSim#getAngularVelocityRadPerSec()}. - * - * @return the final angular velocity of the mechanism, in radians per second - */ - public double getAngularVelocityRadPerSec() { - return angularVelocityRadPerSec; - } - - /** - * - * - *

Obtains the Angular Velocity of the Encoder.

- * - * @return the un-geared angular velocity of the motor (encoder velocity), in radians per second - */ - public double getEncoderVelocityRadPerSec() { - return angularVelocityRadPerSec * gearRatio; - } - - /** - * - * - *

Obtains the Amount of Current Flowing into the Motor.

- * - *

This method is equivalent to {@link DCMotorSim#getCurrentDrawAmps()}. - * - * @return the amount of current flowing into the motor, in amperes - */ - public double getCurrentDrawAmps() { - return currentDrawAmps; - } -} + public enum OutputType { + VOLTAGE, + CURRENT + } + + public enum OutputMode { + VELOCITY, + POSITION, + OPEN_LOOP + } + + /** The Constants for the motor */ + private final DCMotor motor; + /** The dynamics simulation for the motor */ + private final DCMotorSim sim; + /** The gear ratio, value above 1.0 are a reduction */ + private final double gearing; + /** The voltage required to overcome friction */ + private final Voltage frictionVoltage; + + private final PIDController poseVoltController = new PIDController(0, 0, 0); + private final PIDController veloVoltController = new PIDController(0, 0, 0); + private final PIDController poseCurrentController = new PIDController(0, 0, 0); + private final PIDController veloCurrentController = new PIDController(0, 0, 0); + + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0, 0, 0); + + private Current currentLimit = Amps.of(300.0); + + private OutputType outputType = OutputType.VOLTAGE; + private OutputMode outputMode = OutputMode.OPEN_LOOP; + private double output = 0.0; + + private Angle forwardLimit = Radians.of(Double.POSITIVE_INFINITY); + private Angle reverseLimit = Radians.of(Double.NEGATIVE_INFINITY); + + /** + * + * + *

Constructs a Brushless Motor Simulation Instance.

+ * + * @param motor the {@link DCMotor} model representing the motor(s) in the simulation + * @param gearRatio the gear ratio of the mechanism; values greater than 1 indicate a reduction + * @param loadIntertia the rotational inertia of the mechanism + * @param frictionVoltage the voltage required to keep the motor moving at a constant velocity + */ + public BrushlessMotorSim( + SimulatedField arena, + DCMotor motor, + double gearRatio, + MomentOfInertia loadIntertia, + Voltage frictionVoltage) { + this.sim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(motor, loadIntertia.in(KilogramSquareMeters), gearRatio), motor); + this.motor = motor; + this.gearing = gearRatio; + this.frictionVoltage = frictionVoltage; + + arena.addMotor(this); + } + + /** + * Configures the angle of the motor. + * + * @param angle the angle of the motor + * @return this instance for method chaining + */ + public BrushlessMotorSim withOverrideAngle(Angle angle) { + sim.setAngle(angle.in(Radians)); + return this; + } + + /** + * Configures the angular velocity of the motor. + * + * @param angularVelocity the angular velocity of the motor + * @return this instance for method chaining + */ + public BrushlessMotorSim withOverrideAngularVelocity(AngularVelocity angularVelocity) { + sim.setAngularVelocity(angularVelocity.in(RadiansPerSecond)); + return this; + } + + public BrushlessMotorSim withFeedForward( + Voltage kS, Per kV, Per kA) { + var kVUnit = PerUnit.combine(Volts, RadiansPerSecond); + var kAUnit = PerUnit.combine(Volts, RadiansPerSecondPerSecond); + feedforward = new SimpleMotorFeedforward( + kS.in(Volts), kV.in(kVUnit), kA.in(kAUnit), SimulatedField.getSimulationDt()); + return this; + } + + /** + * Configures the PD controller for Positional Requests using {@link OutputType#VOLTAGE}. + * + *

This is unit safe and can be configure like so: + * + * 


+     * // Volts per Rotation of error is how CTRE handles PID when used with voltage requests
+     * sim.withPositionalVoltageController(
+     *   Volts.per(Rotation).ofNative(100.0),
+     *   Volts.per(RotationsPerSecond).ofNative(5.0)
+     * );
+     *
+ * + * @param kP the proportional gain + * @param kD the derivative gain + * @return this instance for method chaining + */ + public BrushlessMotorSim withPositionalVoltageController( + Per kP, Per kD) { + var kPUnit = PerUnit.combine(Volts, Radians); + var kDUnit = PerUnit.combine(Volts, RadiansPerSecond); + poseVoltController.setP(kP.in(kPUnit)); + poseVoltController.setD(kD.in(kDUnit)); + return this; + } + + /** + * Configures the PD controller for Velocity Requests using {@link OutputType#VOLTAGE}. + * + *

This is unit safe and can be configure like so: + * + * 


+     * // Volts per RPS of error is how CTRE handles PID when used with voltage requests
+     * sim.withVelocityVoltageController(
+     *   Volts.per(RotationsPerSecond).ofNative(0.4)
+     * );
+     *
+ * + * @param kP the proportional gain + * @return this instance for method chaining + */ + public BrushlessMotorSim withVelocityVoltageController(Per kP) { + var kPUnit = PerUnit.combine(Volts, Radians); + veloVoltController.setP(kP.in(kPUnit)); + return this; + } + + /** + * Configures the PD controller for Positional Requests using {@link OutputType#CURRENT}. + * + *

This is unit safe and can be configure like so: + * + * 


+     * // Amps per Rotation of error is how CTRE handles PID when used with current requests
+     * sim.withPositionalCurrentController(
+     *   Amps.per(Rotation).ofNative(100.0),
+     *   Amps.per(RotationsPerSecond).ofNative(5.0)
+     * );
+     *
+ * + * @param kP the proportional gain + * @param kD the derivative gain + * @return this instance for method chaining + */ + public BrushlessMotorSim withPositionalCurrentController( + Per kP, Per kD) { + var kPUnit = PerUnit.combine(Amps, Radians); + var kDUnit = PerUnit.combine(Amps, RadiansPerSecond); + poseCurrentController.setP(kP.in(kPUnit)); + poseCurrentController.setD(kD.in(kDUnit)); + return this; + } + + /** + * Configures the PD controller for Velocity Requests using {@link OutputType#CURRENT}. + * + *

This is unit safe and can be configure like so: + * + * 


+     * // Amps per RPS of error is how CTRE handles PID when used with current requests
+     * sim.withVelocityCurrentController(
+     *   Amps.per(RotationsPerSecond).ofNative(0.4)
+     * );
+     *
+ * + * @param kP the proportional gain + * @return this instance for method chaining + */ + public BrushlessMotorSim withVelocityCurrentController(Per kP) { + var kPUnit = PerUnit.combine(Amps, Radians); + veloCurrentController.setP(kP.in(kPUnit)); + return this; + } + + /** + * Configures the positionaly controllers to use continuous wrap. + * + * @param min the minimum angle + * @param max the maximum angle + * @return this instance for method chaining + * @see PIDController#enableContinuousInput(double, double) + */ + public BrushlessMotorSim withControllerContinousInput(Angle min, Angle max) { + poseVoltController.enableContinuousInput(min.in(Radians), max.in(Radians)); + poseCurrentController.enableContinuousInput(min.in(Radians), max.in(Radians)); + return this; + } + + /** + * Configures the current limit for the motor. + * + *

This is the total current limit for the sim + * + * @param currentLimit the current limit for the motor + * @return + */ + public BrushlessMotorSim withStatorCurrentLimit(Current currentLimit) { + // this is a limit across the sum of all motors output, + // so it should be set to the total current limit of the mechanism + this.currentLimit = currentLimit; + return this; + } + + /** + * Configures the hard limits for the motor. + * + * @param forwardLimit the forward limit + * @param reverseLimit the reverse limit + * @return this instance for method chaining + */ + public BrushlessMotorSim withHardLimits(Angle forwardLimit, Angle reverseLimit) { + this.forwardLimit = forwardLimit; + this.reverseLimit = reverseLimit; + return this; + } + + public MomentOfInertia getMOI() { + return KilogramSquareMeters.of(sim.getJKgMetersSquared()); + } + + public Angle getPosition() { + return Radians.of(sim.getAngularPositionRad()); + } + + public AngularVelocity getVelocity() { + return RadiansPerSecond.of(sim.getAngularVelocityRadPerSec()); + } + + public AngularAcceleration getAcceleration() { + return RadiansPerSecondPerSecond.of(sim.getAngularAccelerationRadPerSecSq()); + } + + public Current getStatorCurrentDraw() { + return Amps.of(sim.getCurrentDrawAmps()); + } + + public Current getSupplyCurrent() { + // https://www.chiefdelphi.com/t/current-limiting-talonfx-values/374780/10 + return getStatorCurrentDraw().times(sim.getInputVoltage() / RobotController.getBatteryVoltage()); + } + + public Voltage getRotorVoltage() { + return Volts.of(sim.getInputVoltage()); + } + + public Voltage getSupplyVoltage() { + return Volts.of(RobotController.getBatteryVoltage()); + } + + public void setControl(OutputType outputType, AngularVelocity velo) { + this.outputType = outputType; + this.outputMode = OutputMode.VELOCITY; + this.output = velo.in(RadiansPerSecond); + } + + public void setControl(OutputType outputType, Angle pos) { + this.outputType = outputType; + this.outputMode = OutputMode.POSITION; + this.output = pos.in(Radians); + } + + public void setControl(Current amps) { + this.outputType = OutputType.CURRENT; + this.outputMode = OutputMode.OPEN_LOOP; + this.output = amps.in(Amps); + } + + public void setControl(Voltage volts) { + this.outputType = OutputType.VOLTAGE; + this.outputMode = OutputMode.OPEN_LOOP; + this.output = volts.in(Volts); + } + + public void setControl() { + this.outputType = OutputType.VOLTAGE; + this.outputMode = OutputMode.OPEN_LOOP; + this.output = 0.0; + } + + public void update() { + double dtSeconds = SimulatedField.getSimulationDt(); + switch (this.outputType) { + case VOLTAGE -> { + switch (this.outputMode) { + case OPEN_LOOP -> { + driveAtVoltage(Volts.of(output)); + } + case POSITION -> { + Voltage voltage = Volts.of( + poseVoltController.calculate(getPosition().in(Radians), output)); + Voltage feedforwardVoltage = feedforward.calculate(getVelocity(), velocityForVolts(voltage)); + driveAtVoltage(feedforwardVoltage.plus(voltage)); + } + case VELOCITY -> { + Voltage voltage = Volts.of( + veloVoltController.calculate(getVelocity().in(RadiansPerSecond), output)); + Voltage feedforwardVoltage = feedforward.calculate(getVelocity(), RadiansPerSecond.of(output)); + driveAtVoltage(voltage.plus(feedforwardVoltage)); + } + } + } + case CURRENT -> { + switch (this.outputMode) { + case OPEN_LOOP -> { + sim.setInputVoltage( + voltsForAmps(Amps.of(output), getVelocity()).in(Volts)); + } + case POSITION -> { + Current current = Amps.of( + poseCurrentController.calculate(getPosition().in(Radians), output)); + Voltage voltage = voltsForAmps(current, getVelocity()); + Voltage feedforwardVoltage = feedforward.calculate(getVelocity(), velocityForVolts(voltage)); + driveAtVoltage(feedforwardVoltage.plus(voltage)); + } + case VELOCITY -> { + Current current = Amps.of( + veloCurrentController.calculate(getPosition().in(Radians), output)); + Voltage feedforwardVoltage = feedforward.calculate(getVelocity(), RadiansPerSecond.of(output)); + Voltage voltage = voltsForAmps(current, getVelocity()).plus(feedforwardVoltage); + driveAtVoltage(voltage); + } + } + } + } + + sim.update(dtSeconds); + + if (getPosition().lte(reverseLimit)) { + sim.setState(reverseLimit.in(Radians), 0.0); + } else if (getPosition().gte(forwardLimit)) { + sim.setState(forwardLimit.in(Radians), 0.0); + } + } + + private void driveAtVoltage(Voltage voltage) { + // The voltage constrained to current limits and battery voltage + Voltage constrained = constrainOutputVoltage(voltage); + Voltage frictionVoltage = applyFriction(constrained); + + sim.setInputVoltage(frictionVoltage.in(Volts)); + } + + private Voltage applyFriction(Voltage voltage) { + // This function is responsible for slowing down acceleration + // and slowing down the velocity of the motor when at lowere output. + // This is not the same as the static friction, which is the force + // required to get the motor moving. + + // to apply friction we convert the motors output to torque then back to voltage + double current = motor.getCurrent(sim.getAngularVelocityRadPerSec() * gearing, voltage.in(Volts)); + double currentVelo = getVelocity().in(RadiansPerSecond) * gearing; + double torque = motor.getTorque(current); + double friction = frictionTorque().in(NewtonMeters); + + boolean movingForward = currentVelo > 0; + + if (movingForward && currentVelo > motor.getSpeed(torque, sim.getInputVoltage())) { + // the motor is moving faster than it should based on the output voltage + // apply the friction to slow it down + torque -= friction; + } else if (!movingForward && currentVelo < motor.getSpeed(torque, sim.getInputVoltage())) { + // the motor is moving slower than it should based on the output voltage + // apply the friction to speed it up + torque += friction; + } + + return Volts.of(motor.getVoltage(torque, currentVelo)); + } + + private Voltage voltsForAmps(Current current, AngularVelocity angularVelocity) { + // find what voltage is needed to get the current + return Volts.of(motor.getVoltage(current.in(Amps), angularVelocity.in(RadiansPerSecond) * gearing)); + } + + private AngularVelocity velocityForVolts(Voltage voltage) { + return RadiansPerSecond.of( + motor.getSpeed(motor.getTorque(getStatorCurrentDraw().in(Amps)), voltage.in(Volts))); + } + + private Torque frictionTorque() { + return NewtonMeters.of(motor.getTorque(motor.getCurrent(0.0, frictionVoltage.in(Volts))) * gearing); + } + + private Voltage constrainOutputVoltage(Voltage requestedOutput) { + final double kCurrentThreshold = 1.2; + + final double motorCurrentVelocityRadPerSec = getVelocity().in(RadiansPerSecond); + final double currentLimitAmps = currentLimit.in(Amps); + final double requestedOutputVoltage = requestedOutput.in(Volts); + final double currentAtRequestedVolts = motor.getCurrent(motorCurrentVelocityRadPerSec, requestedOutputVoltage); + + // Resource for current limiting: + // https://file.tavsys.net/control/controls-engineering-in-frc.pdf (sec 12.1.3) + final boolean currentTooHigh = Math.abs(currentAtRequestedVolts) > (kCurrentThreshold * currentLimitAmps); + double limitedVoltage = requestedOutputVoltage; + if (currentTooHigh) { + final double limitedCurrent = Math.copySign(currentLimitAmps, currentAtRequestedVolts); + limitedVoltage = motor.getVoltage(motor.getTorque(limitedCurrent), motorCurrentVelocityRadPerSec); + } + + // ensure the current limit doesn't cause an increase to output voltage + if (Math.abs(limitedVoltage) > Math.abs(requestedOutputVoltage)) { + limitedVoltage = requestedOutputVoltage; + } + + // constrain the output voltage to the battery voltage + return Volts.of(MathUtil.clamp( + limitedVoltage, -RobotController.getBatteryVoltage(), RobotController.getBatteryVoltage())); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java index cd5fa623..10b79220 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java @@ -1,361 +1,1562 @@ package frc.robot.extras.simulation.mechanismSim.swerve; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Volts; -import static frc.robot.extras.simulation.field.SimulatedField.SIMULATION_DT; -import static frc.robot.extras.simulation.field.SimulatedField.SIMULATION_SUB_TICKS_IN_1_PERIOD; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Voltage; import java.util.Queue; import java.util.concurrent.ConcurrentLinkedQueue; import java.util.function.Supplier; import org.dyn4j.geometry.Vector2; +import frc.robot.extras.simulation.mechanismSim.swerve.BrushlessMotorSim; +import frc.robot.extras.simulation.field.SimulatedField; +/** + * + * + *

Simulation for a Single Swerve Module.

+ * + *

This class provides a simulation for a single swerve module in the {@link SwerveDriveSimulation}. + * + *

1. Purpose

+ * + *

This class serves as the bridge between your code and the physics engine. + * + *

You will apply voltage outputs to the drive/steer motor of the module and obtain their encoder readings in your + * code, just as how you deal with your physical motors. + * + *

2. Perspectives

+ * + *
    + *
  • Simulates the steering mechanism using a custom brushless motor simulator. + *
  • Simulates the propelling force generated by the driving motor, with a current limit. + *
  • Simulates encoder readings, which can be used to simulate a {@link SwerveDriveOdometry}. + *
+ * + *

3. Simulating Odometry

+ * + *
    + *
  • Retrieve the encoder readings from {@link #getDriveEncoderUnGearedPositionRad()}} and + * {@link #getSteerAbsoluteFacing()}. + *
  • Use {@link SwerveDriveOdometry} to estimate the pose of your robot. + *
  • 250Hz + * Odometry is supported. You can retrive cached encoder readings from every sub-tick through + * {@link #getCachedDriveEncoderUnGearedPositionsRad()} and {@link #getCachedSteerAbsolutePositions()}. + *
+ * + *

An example of how to simulate odometry using this class is the ModuleIOSim.java + * from the Advanced Swerve Drive with maple-sim example. + */ public class SwerveModuleSimulation { - private final DCMotor DRIVE_MOTOR; - private final BrushlessMotorSim turnMotorSim; - private final double DRIVE_CURRENT_LIMIT, - DRIVE_GEAR_RATIO, - TURN_GEAR_RATIO, - DRIVE_FRICTION_VOLTAGE, - TURN_FRICTION_VOLTAGE, - WHEELS_COEFFICIENT_OF_FRICTION, - WHEEL_RADIUS_METERS, - DRIVE_WHEEL_INERTIA = 0.01; - private double driveMotorRequestedVolts = 0.0, - turnMotorAppliedVolts = 0.0, - driveMotorAppliedVolts = 0.0, - driveMotorSupplyCurrentAmps = 0.0, - turnMotorSupplyCurrentAmps = 0.0, - turnRelativeEncoderPositionRad = 0.0, - turnRelativeEncoderSpeedRadPerSec = 0.0, - turnAbsoluteEncoderSpeedRadPerSec = 0.0, - driveEncoderUnGearedPositionRad = 0.0, - driveEncoderUnGearedSpeedRadPerSec = 0.0; - private Rotation2d turnAbsoluteRotation = Rotation2d.fromRotations(Math.random()); - - private final double turnRelativeEncoderOffSet = (Math.random() - 0.5) * 30; - - private final Queue cachedTurnRelativeEncoderPositionsRad, - cachedDriveEncoderUnGearedPositionsRad; - private final Queue cachedTurnAbsolutePositions; - - public SwerveModuleSimulation( - DCMotor driveMotor, - DCMotor turnMotor, - double driveCurrentLimit, - double driveGearRatio, - double turnGearRatio, - double driveFrictionVoltage, - double turnFrictionVoltage, - double tireCoefficientOfFriction, - double wheelsRadiusMeters, - double turnRotationalInertia) { - DRIVE_MOTOR = driveMotor; - DRIVE_CURRENT_LIMIT = driveCurrentLimit; - DRIVE_GEAR_RATIO = driveGearRatio; - TURN_GEAR_RATIO = turnGearRatio; - DRIVE_FRICTION_VOLTAGE = driveFrictionVoltage; - TURN_FRICTION_VOLTAGE = turnFrictionVoltage; - WHEELS_COEFFICIENT_OF_FRICTION = tireCoefficientOfFriction; - WHEEL_RADIUS_METERS = wheelsRadiusMeters; - - this.turnMotorSim = - new BrushlessMotorSim( - turnMotor, TURN_GEAR_RATIO, turnRotationalInertia, TURN_FRICTION_VOLTAGE); - - this.cachedDriveEncoderUnGearedPositionsRad = new ConcurrentLinkedQueue<>(); - for (int i = 0; i < SIMULATION_SUB_TICKS_IN_1_PERIOD; i++) - cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); - this.cachedTurnRelativeEncoderPositionsRad = new ConcurrentLinkedQueue<>(); - for (int i = 0; i < SIMULATION_SUB_TICKS_IN_1_PERIOD; i++) - cachedTurnRelativeEncoderPositionsRad.offer(turnRelativeEncoderPositionRad); - this.cachedTurnAbsolutePositions = new ConcurrentLinkedQueue<>(); - for (int i = 0; i < SIMULATION_SUB_TICKS_IN_1_PERIOD; i++) - cachedTurnAbsolutePositions.offer(turnAbsoluteRotation); - - this.turnRelativeEncoderPositionRad = - turnAbsoluteRotation.getRadians() + turnRelativeEncoderOffSet; - } - - public void requestDriveVoltageOut(double volts) { - this.driveMotorRequestedVolts = volts; - } - - public void requestTurnVoltageOut(double volts) { - this.turnMotorAppliedVolts = volts; - // this.turnMotorSim.setInputVoltage(MathUtil.applyDeadband(volts, turn_FRICTION_VOLTAGE, - // 12)); - } - - public void requestDriveVoltageOut(Voltage volts) { - this.driveMotorRequestedVolts = volts.in(Volts); - } - - public void requestTurnVoltageOut(Voltage volts) { - this.turnMotorAppliedVolts = volts.in(Volts); - // this.turnMotorSim.setInputVoltage(MathUtil.applyDeadband(volts, turn_FRICTION_VOLTAGE, - // 12)); - } - - public double getDriveMotorAppliedVolts() { - return driveMotorAppliedVolts; - } - - public double getTurnMotorAppliedVolts() { - return turnMotorAppliedVolts; - } - - public double getDriveMotorSupplyCurrentAmps() { - return driveMotorSupplyCurrentAmps; - } - - public double getTurnMotorSupplyCurrentAmps() { - return turnMotorSupplyCurrentAmps; - } - - public double getDriveEncoderUnGearedPositionRad() { - return driveEncoderUnGearedPositionRad; - } - - public double getDriveEncoderFinalPositionRad() { - return getDriveEncoderUnGearedPositionRad() / DRIVE_GEAR_RATIO; - } - - public double getDriveEncoderUnGearedSpeedRadPerSec() { - return driveEncoderUnGearedSpeedRadPerSec; - } - - public double getDriveWheelFinalSpeedRadPerSec() { - return getDriveEncoderUnGearedSpeedRadPerSec() / DRIVE_GEAR_RATIO; - } - - /** geared */ - public double getTurnRelativeEncoderPositionRad() { - return turnRelativeEncoderPositionRad; - } - - /** geared */ - public double getTurnRelativeEncoderSpeedRadPerSec() { - return turnRelativeEncoderSpeedRadPerSec; - } - - public Rotation2d getTurnAbsolutePosition() { - return turnAbsoluteRotation; - } - - public double getTurnAbsoluteEncoderSpeedRadPerSec() { - return turnAbsoluteEncoderSpeedRadPerSec; - } - - public double[] getCachedDriveEncoderUnGearedPositionsRad() { - return cachedDriveEncoderUnGearedPositionsRad.stream().mapToDouble(value -> value).toArray(); - } - - public double[] getCachedDriveWheelFinalPositionsRad() { - return cachedDriveEncoderUnGearedPositionsRad.stream() - .mapToDouble(value -> value / DRIVE_GEAR_RATIO) - .toArray(); - } - - public double[] getCachedTurnRelativeEncoderPositions() { - return cachedTurnRelativeEncoderPositionsRad.stream().mapToDouble(value -> value).toArray(); - } - - public Rotation2d[] getCachedTurnAbsolutePositions() { - return cachedTurnAbsolutePositions.toArray(Rotation2d[]::new); - } - - protected double getGrippingForceNewtons(double gravityForceOnModuleNewtons) { - return gravityForceOnModuleNewtons * WHEELS_COEFFICIENT_OF_FRICTION; - } - - /** - * updates the simulation sub-tick for this module, updating its inner status (sensor readings) - * and calculating a total force - * - * @param moduleCurrentGroundVelocityWorldRelative - * @return the propelling force that the module generates - */ - public Vector2 updateSimulationSubTickGetModuleForce( - Vector2 moduleCurrentGroundVelocityWorldRelative, - Rotation2d robotRotation, - double gravityForceOnModuleNewtons) { - updateturnSimulation(); - - /* the maximum gripping force that the wheel can generate */ - final double grippingForceNewtons = getGrippingForceNewtons(gravityForceOnModuleNewtons); - final Rotation2d moduleWorldRotation = this.turnAbsoluteRotation.plus(robotRotation); - final Vector2 propellingForce = - getPropellingForce( - grippingForceNewtons, moduleWorldRotation, moduleCurrentGroundVelocityWorldRelative); - updateEncoderTicks(); - - return propellingForce; - } - - /** */ - private void updateturnSimulation() { - turnMotorSim.update(SIMULATION_DT); - - /* update the readings of the sensor */ - this.turnAbsoluteRotation = Rotation2d.fromRadians(turnMotorSim.getAngularPositionRad()); - this.turnRelativeEncoderPositionRad = - turnMotorSim.getAngularPositionRad() + turnRelativeEncoderOffSet; - this.turnAbsoluteEncoderSpeedRadPerSec = turnMotorSim.getAngularVelocityRadPerSec(); - this.turnRelativeEncoderSpeedRadPerSec = turnAbsoluteEncoderSpeedRadPerSec * TURN_GEAR_RATIO; - - /* cache sensor readings to queue for high-frequency odometry */ - this.cachedTurnAbsolutePositions.poll(); - this.cachedTurnAbsolutePositions.offer(turnAbsoluteRotation); - this.cachedTurnRelativeEncoderPositionsRad.poll(); - this.cachedTurnRelativeEncoderPositionsRad.offer(turnRelativeEncoderPositionRad); - } - - private Vector2 getPropellingForce( - double grippingForceNewtons, - Rotation2d moduleWorldRotation, - Vector2 moduleCurrentGroundVelocity) { - final double driveWheelTorque = getDriveWheelTorque(), - theoreticalMaxPropellingForceNewtons = driveWheelTorque / WHEEL_RADIUS_METERS; - final boolean skidding = Math.abs(theoreticalMaxPropellingForceNewtons) > grippingForceNewtons; - final double propellingForceNewtons; - if (skidding) - propellingForceNewtons = - Math.copySign(grippingForceNewtons, theoreticalMaxPropellingForceNewtons); - else propellingForceNewtons = theoreticalMaxPropellingForceNewtons; - - final double floorVelocityProjectionOnWheelDirectionMPS = - moduleCurrentGroundVelocity.getMagnitude() - * Math.cos( - moduleCurrentGroundVelocity.getAngleBetween( - new Vector2(moduleWorldRotation.getRadians()))); - - if (skidding) { - /* if the chassis is skidding, part of the toque will cause the wheels to spin freely */ - final double torqueOnWheel = driveWheelTorque * 0.3; - this.driveEncoderUnGearedSpeedRadPerSec += - torqueOnWheel / DRIVE_WHEEL_INERTIA * SIMULATION_DT * DRIVE_GEAR_RATIO; - } else // if the chassis is tightly gripped on floor, the floor velocity is projected to the - // wheel - this.driveEncoderUnGearedSpeedRadPerSec = - floorVelocityProjectionOnWheelDirectionMPS / WHEEL_RADIUS_METERS * DRIVE_GEAR_RATIO; - - return Vector2.create(propellingForceNewtons, moduleWorldRotation.getRadians()); - } - - private double getDriveWheelTorque() { - final double currentAtRequestedVolts = - DRIVE_MOTOR.getCurrent(this.driveEncoderUnGearedSpeedRadPerSec, driveMotorRequestedVolts); - - driveMotorAppliedVolts = driveMotorRequestedVolts; - /* normally, motor controller starts cutting the supply voltage when the current exceed 150% the current limit */ - final boolean currentTooHigh = Math.abs(currentAtRequestedVolts) > 1.2 * DRIVE_CURRENT_LIMIT, - driveMotorTryingToAccelerate = driveMotorRequestedVolts * driveMotorSupplyCurrentAmps > 0; - - if (currentTooHigh && driveMotorTryingToAccelerate) { - /* activate current limit, cut down the applied voltage to match current limit */ - final double currentWithLimits = Math.copySign(DRIVE_CURRENT_LIMIT, currentAtRequestedVolts); - driveMotorAppliedVolts = - DRIVE_MOTOR.getVoltage( - DRIVE_MOTOR.getTorque(currentWithLimits), this.driveEncoderUnGearedSpeedRadPerSec); - } - - driveMotorAppliedVolts = MathUtil.clamp(driveMotorAppliedVolts, -12, 12); - - /* calculate the actual supply current */ - driveMotorSupplyCurrentAmps = - DRIVE_MOTOR.getCurrent( - this.driveEncoderUnGearedSpeedRadPerSec, - MathUtil.applyDeadband(driveMotorAppliedVolts, DRIVE_FRICTION_VOLTAGE, 12)); - - /* calculate the torque generated, */ - final double torqueOnRotor = DRIVE_MOTOR.getTorque(driveMotorSupplyCurrentAmps); - return torqueOnRotor * DRIVE_GEAR_RATIO; - } - - /** - * @return the current module state of this simulation module - */ - protected SwerveModuleState getCurrentState() { - return new SwerveModuleState( - getDriveWheelFinalSpeedRadPerSec() * WHEEL_RADIUS_METERS, turnAbsoluteRotation); - } - - /** - * gets the state of the module, if it is allowed to spin freely for a long time under the current - * applied drive volts - * - * @return the free spinning module state - */ - protected SwerveModuleState getFreeSpinState() { - return new SwerveModuleState( - DRIVE_MOTOR.getSpeed( - DRIVE_MOTOR.getTorque(DRIVE_MOTOR.getCurrent(0, DRIVE_FRICTION_VOLTAGE)), - driveMotorAppliedVolts) - / DRIVE_GEAR_RATIO - * WHEEL_RADIUS_METERS, - turnAbsoluteRotation); - } - - private void updateEncoderTicks() { - this.driveEncoderUnGearedPositionRad += this.driveEncoderUnGearedSpeedRadPerSec * SIMULATION_DT; - this.cachedDriveEncoderUnGearedPositionsRad.poll(); - this.cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); - } - - public double getModuleTheoreticalSpeedMPS() { - return DRIVE_MOTOR.freeSpeedRadPerSec / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS; - } - - public double getTheoreticalPropellingForcePerModule(double robotMass, int modulesCount) { - final double - maxThrustNewtons = - DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS, - maxGrippingNewtons = 9.8 * robotMass / modulesCount * WHEELS_COEFFICIENT_OF_FRICTION; - - return Math.min(maxThrustNewtons, maxGrippingNewtons); - } - - public double getModuleMaxAccelerationMPSsq(double robotMass, int modulesCount) { - return getTheoreticalPropellingForcePerModule(robotMass, modulesCount) - * modulesCount - / robotMass; - } - - public enum DRIVE_WHEEL_TYPE { - RUBBER, - TIRE - } - - /** - * creates a SDS - * Mark4-n Swerve Module for simulation - */ - public static Supplier getModule( - DCMotor driveMotor, - DCMotor turnMotor, - double driveCurrentLimitAmps, - DRIVE_WHEEL_TYPE driveWheelType, - double driveGearRatio) { - return () -> - new SwerveModuleSimulation( - driveMotor, - turnMotor, - driveCurrentLimitAmps, - driveGearRatio, - 11.3142, - 0.25, - 0.05, - switch (driveWheelType) { - case RUBBER -> 1.55; - case TIRE -> 1.45; - }, - Units.inchesToMeters(2), - 0.05); - } -} + public final DCMotor DRIVE_MOTOR; + private final BrushlessMotorSim steerMotorSim; + public final double DRIVE_CURRENT_LIMIT, + DRIVE_GEAR_RATIO, + STEER_GEAR_RATIO, + DRIVE_FRICTION_VOLTAGE, + WHEELS_COEFFICIENT_OF_FRICTION, + WHEEL_RADIUS_METERS, + DRIVE_WHEEL_INERTIA = 0.01; + private double driveMotorRequestedVolts = 0.0, + driveMotorAppliedVolts = 0.0, + driveMotorSupplyCurrentAmps = 0.0, + steerRelativeEncoderPositionRad = 0.0, + steerRelativeEncoderSpeedRadPerSec = 0.0, + steerAbsoluteEncoderSpeedRadPerSec = 0.0, + driveEncoderUnGearedPositionRad = 0.0, + driveEncoderUnGearedSpeedRadPerSec = 0.0; + private Rotation2d steerAbsoluteFacing = Rotation2d.fromRotations(Math.random()); + + private final double steerRelativeEncoderOffSet = (Math.random() - 0.5) * 30; + + private final Queue cachedSteerRelativeEncoderPositionsRad, cachedDriveEncoderUnGearedPositionsRad; + private final Queue cachedSteerAbsolutePositions; + + /** + * + * + *

Constructs a Swerve Module Simulation.

+ * + *

If you are using {@link SimulatedField#overrideSimulationTimings(double, int)} to use custom timings, you must + * call the method before constructing any swerve module simulations using this constructor. + * + * @param driveMotor the model of the driving motor + * @param steerMotor the model of the steering motor + * @param driveCurrentLimit the current limit for the driving motor, in amperes + * @param driveGearRatio the gear ratio for the driving motor, >1 is reduction + * @param steerGearRatio the gear ratio for the steering motor, >1 is reduction + * @param driveFrictionVoltage the measured minimum amount of voltage that can turn the driving rotter, in volts + * @param steerFrictionVoltage the measured minimum amount of voltage that can turn the steering rotter, in volts + * @param tireCoefficientOfFriction the coefficient + * of friction of the tires, normally around 1.5 + * @param wheelsRadiusMeters the radius of the wheels, in meters. Calculate it using + * {@link Units#inchesToMeters(double)}. + * @param steerRotationalInertia the rotational inertia of the steering mechanism + */ + public SwerveModuleSimulation( + DCMotor driveMotor, + DCMotor steerMotor, + double driveCurrentLimit, + double driveGearRatio, + double steerGearRatio, + double driveFrictionVoltage, + double steerFrictionVoltage, + double tireCoefficientOfFriction, + double wheelsRadiusMeters, + double steerRotationalInertia) { + DRIVE_MOTOR = driveMotor; + DRIVE_CURRENT_LIMIT = driveCurrentLimit; + DRIVE_GEAR_RATIO = driveGearRatio; + STEER_GEAR_RATIO = steerGearRatio; + DRIVE_FRICTION_VOLTAGE = driveFrictionVoltage; + WHEELS_COEFFICIENT_OF_FRICTION = tireCoefficientOfFriction; + WHEEL_RADIUS_METERS = wheelsRadiusMeters; + + this.steerMotorSim = new BrushlessMotorSim( + SimulatedField.getInstance(), + steerMotor, + steerGearRatio, + KilogramSquareMeters.of(steerRotationalInertia), + Volts.of(steerFrictionVoltage)); + + this.cachedDriveEncoderUnGearedPositionsRad = new ConcurrentLinkedQueue<>(); + for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) + cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); + this.cachedSteerRelativeEncoderPositionsRad = new ConcurrentLinkedQueue<>(); + for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) + cachedSteerRelativeEncoderPositionsRad.offer(steerRelativeEncoderPositionRad); + this.cachedSteerAbsolutePositions = new ConcurrentLinkedQueue<>(); + for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) + cachedSteerAbsolutePositions.offer(steerAbsoluteFacing); + + this.steerRelativeEncoderPositionRad = steerAbsoluteFacing.getRadians() + steerRelativeEncoderOffSet; + } + + /** + * + * + *

Requests the Driving Motor to Run at a Specified Voltage Output.

+ * + *

Think of it as the setVoltage() of your physical driving motor.

+ * + *

This method sets the desired voltage output for the driving motor. The change will be applied in the next + * sub-tick of the simulation. + * + *

Note: The requested voltage may not always be fully applied if the current is too high. The + * current limit may reduce the motor's output, similar to real motors. + * + *

To check the actual voltage applied to the drivetrain, use {@link #getDriveMotorAppliedVolts()}. + * + * @param volts the voltage to be applied to the driving motor + */ + public void requestDriveVoltageOut(double volts) { + this.driveMotorRequestedVolts = volts; + } + + /** + * + * + *

Requests the Steering Motor to Run at a Specified Voltage Output.

+ * + *

Think of it as the setVoltage() of your physical steering motor.

+ * + *

This method sets the desired voltage output for the steering motor. The change will be applied in the next + * sub-tick of the simulation. + * + *

Note: Similar to the drive motor, the requested voltage may not always be fully applied if + * the current exceeds the limit. The current limit will reduce the motor's output as needed, mimicking real motor + * behavior. + * + *

To check the actual voltage applied to the steering motor, use {@link #getSteerMotorAppliedVolts()}. + * + * @param volts the voltage to be applied to the steering motor + */ + public void requestSteerVoltageOut(double volts) { + this.steerMotorSim.setControl(Volts.of(volts)); + } + + /** + * + * + *

Obtains the Actual Output Voltage of the Drive Motor.

+ * + *

This method returns the actual voltage being applied to the drive motor. The actual applied voltage may differ + * from the value set by {@link #requestDriveVoltageOut(double)}. + * + *

If the motor's supply current is too high, the motor will automatically reduce its output voltage to protect + * the system. + * + * @return the actual output voltage of the drive motor, in volts + */ + public double getDriveMotorAppliedVolts() { + return driveMotorAppliedVolts; + } + + /** + * + * + *

Obtains the Actual Output Voltage of the Steering Motor.

+ * + *

This method returns the actual voltage being applied to the steering motor. It wraps around the + * {@link BrushlessMotorSim#getAppliedVolts()} method. + * + *

The actual applied voltage may differ from the value set by {@link #requestSteerVoltageOut(double)}. If the + * motor's supply current is too high, the motor will automatically reduce its output voltage to protect the system. + * + * @return the actual output voltage of the steering motor, in volts + */ + public double getSteerMotorAppliedVolts() { + return steerMotorSim.getRotorVoltage().in(Volts); + } + + /** + * + * + *

Obtains the Amount of Current Supplied to the Drive Motor.

+ * + *

Think of it as the getSupplyCurrent() of your physical drive motor.

+ * + * @return the current supplied to the drive motor, in amperes + */ + public double getDriveMotorSupplyCurrentAmps() { + return driveMotorSupplyCurrentAmps; + } + + /** + * + * + *

Obtains the Amount of Current Supplied to the Steer Motor.

+ * + *

Think of it as the getSupplyCurrent() of your physical steer motor.

+ * + *

This method wraps around {@link BrushlessMotorSim#getCurrentDrawAmps()}. + * + * @return the current supplied to the steer motor, in amperes + */ + public double getSteerMotorSupplyCurrentAmps() { + return steerMotorSim.getSupplyCurrent().in(Amps); + } + + /** + * + * + *

Obtains the Position of the Drive Encoder.

+ * + *

Think of it as the getPosition() of your physical driving motor.

+ * + *

This method is used to simulate your {@link SwerveDrivePoseEstimator}. + * + *

This value represents the un-geared position of the encoder, i.e., the amount of radians the drive motor's + * encoder has rotated. + * + *

To get the final wheel rotations, use {@link #getDriveWheelFinalPositionRad()}. + * + * @return the position of the drive motor's encoder, in radians (un-geared) + */ + public double getDriveEncoderUnGearedPositionRad() { + return driveEncoderUnGearedPositionRad; + } + + /** + * + * + *

Obtains the Final Position of the Drive Encoder (Wheel Rotations).

+ * + *

This method is used to simulate the {@link SwerveDrivePoseEstimator} by providing the final position of the + * drive encoder in terms of wheel rotations. + * + *

The value is calculated by dividing the un-geared encoder position (obtained from + * {@link #getDriveEncoderUnGearedPositionRad()}) by the drive gear ratio. + * + * @return the final position of the drive encoder (wheel rotations), in radians + */ + public double getDriveWheelFinalPositionRad() { + return getDriveEncoderUnGearedPositionRad() / DRIVE_GEAR_RATIO; + } + + /** + * + * + *

Obtains the Speed of the Drive Encoder (Un-Geared), in Radians per Second.

+ * + *

Think of it as the getVelocity() of your physical drive motor.

+ * + *

This method returns the current speed of the drive encoder in radians per second, without accounting for the + * drive gear ratio. + * + * @return the un-geared speed of the drive encoder, in radians per second + */ + public double getDriveEncoderUnGearedSpeedRadPerSec() { + return driveEncoderUnGearedSpeedRadPerSec; + } + + /** + * + * + *

Obtains the Final Speed of the Wheel, in Radians per Second.

+ * + * @return the final speed of the drive wheel, in radians per second + */ + public double getDriveWheelFinalSpeedRadPerSec() { + return getDriveEncoderUnGearedSpeedRadPerSec() / DRIVE_GEAR_RATIO; + } + + /** + * + * + *

Obtains the Relative Position of the Steer Encoder, in Radians.

+ * + *

Think of it as the getPosition() of your physical steer motor.

+ * + * @return the relative encoder position of the steer motor, in radians + */ + public double getSteerRelativeEncoderPositionRad() { + return steerRelativeEncoderPositionRad; + } + + /** + * + * + *

Obtains the Speed of the Steer Relative Encoder, in Radians per Second (Geared).

+ * + *

Think of it as the getVelocity() of your physical steer motor.

+ * + * @return the speed of the steer relative encoder, in radians per second (geared) + */ + public double getSteerRelativeEncoderSpeedRadPerSec() { + return steerRelativeEncoderSpeedRadPerSec; + } + + /** + * + * + *

Obtains the Absolute Facing of the Steer Mechanism.

+ * + *

Think of it as the getAbsolutePosition() of your CanCoder.

+ * + * @return the absolute facing of the steer mechanism, as a {@link Rotation2d} + */ + public Rotation2d getSteerAbsoluteFacing() { + return steerAbsoluteFacing; + } + + /** + * + * + *

Obtains the Absolute Rotational Velocity of the Steer Mechanism.

+ * + *

Think of it as the getVelocity() of your CanCoder.

+ * + * @return the absolute angular velocity of the steer mechanism, in radians per second + */ + public double getSteerAbsoluteEncoderSpeedRadPerSec() { + return steerAbsoluteEncoderSpeedRadPerSec; + } + + /** + * + * + *

Obtains the Cached Readings of the Drive Encoder's Un-Geared Position.

+ * + *

The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and can be retrieved + * using this method. + * + * @return an array of cached drive encoder un-geared positions, in radians + */ + public double[] getCachedDriveEncoderUnGearedPositionsRad() { + return cachedDriveEncoderUnGearedPositionsRad.stream() + .mapToDouble(value -> value) + .toArray(); + } + + /** + * + * + *

Obtains the Cached Readings of the Drive Encoder's Final Position (Wheel Rotations).

+ * + *

The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and are divided by the + * gear ratio to obtain the final wheel rotations. + * + * @return an array of cached drive encoder final positions (wheel rotations), in radians + */ + public double[] getCachedDriveWheelFinalPositionsRad() { + return cachedDriveEncoderUnGearedPositionsRad.stream() + .mapToDouble(value -> value / DRIVE_GEAR_RATIO) + .toArray(); + } + + /** + * + * + *

Obtains the Cached Readings of the Steer Relative Encoder's Position.

+ * + *

The values of {@link #getSteerRelativeEncoderPositionRad()} are cached at each sub-tick and can be retrieved + * using this method. + * + * @return an array of cached steer relative encoder positions, in radians + */ + public double[] getCachedSteerRelativeEncoderPositions() { + return cachedSteerRelativeEncoderPositionsRad.stream() + .mapToDouble(value -> value) + .toArray(); + } + + /** + * + * + *

Obtains the Cached Readings of the Steer Absolute Positions.

+ * + *

The values of {@link #getSteerAbsoluteFacing()} are cached at each sub-tick and can be retrieved using this + * method. + * + * @return an array of cached absolute steer positions, as {@link Rotation2d} objects + */ + public Rotation2d[] getCachedSteerAbsolutePositions() { + return cachedSteerAbsolutePositions.toArray(Rotation2d[]::new); + } + + protected double getGrippingForceNewtons(double gravityForceOnModuleNewtons) { + return gravityForceOnModuleNewtons * WHEELS_COEFFICIENT_OF_FRICTION; + } + + /** + * + * + *

Updates the Simulation for This Module.

+ * + *

This method is called once during every sub-tick of the simulation. It performs the following actions: + * + *

    + *
  • Updates the simulation of the steering mechanism. + *
  • Simulates the propelling force generated by the module. + *
  • Updates and caches the encoder readings for odometry simulation. + *
+ * + *

Note: Friction forces are not simulated in this method. + * + * @param moduleCurrentGroundVelocityWorldRelative the current ground velocity of the module, relative to the world + * @param robotFacing the absolute facing of the robot, relative to the world + * @param gravityForceOnModuleNewtons the gravitational force acting on this module, in newtons + * @return the propelling force generated by the module, as a {@link Vector2} object + */ + public Vector2 updateSimulationSubTickGetModuleForce( + Vector2 moduleCurrentGroundVelocityWorldRelative, + Rotation2d robotFacing, + double gravityForceOnModuleNewtons) { + updateSteerSimulation(); + + /* the maximum gripping force that the wheel can generate */ + final double grippingForceNewtons = getGrippingForceNewtons(gravityForceOnModuleNewtons); + final Rotation2d moduleWorldFacing = this.steerAbsoluteFacing.plus(robotFacing); + final Vector2 propellingForce = + getPropellingForce(grippingForceNewtons, moduleWorldFacing, moduleCurrentGroundVelocityWorldRelative); + updateDriveEncoders(); + + return propellingForce; + } + + /** + * + * + *

updates the simulation for the steer mechanism.

+ * + *

Updates the simulation for the steer mechanism and cache the encoder readings. + * + *

The steer mechanism is modeled by a {@link BrushlessMotorSim}. + */ + private void updateSteerSimulation() { + /* update the readings of the sensor */ + this.steerAbsoluteFacing = new Rotation2d(steerMotorSim.getPosition()); + this.steerRelativeEncoderPositionRad = steerMotorSim.getPosition().in(Radians) + steerRelativeEncoderOffSet; + this.steerAbsoluteEncoderSpeedRadPerSec = steerMotorSim.getVelocity().in(RadiansPerSecond); + this.steerRelativeEncoderSpeedRadPerSec = steerAbsoluteEncoderSpeedRadPerSec * STEER_GEAR_RATIO; + + /* cache sensor readings to queue for high-frequency odometry */ + this.cachedSteerAbsolutePositions.poll(); + this.cachedSteerAbsolutePositions.offer(steerAbsoluteFacing); + this.cachedSteerRelativeEncoderPositionsRad.poll(); + this.cachedSteerRelativeEncoderPositionsRad.offer(steerRelativeEncoderPositionRad); + } + + /** + * + * + *

Calculates the amount of propelling force that the module generates.

+ * + *

The swerve module's drive motor will generate a propelling force. + * + *

For most of the time, that propelling force is directly applied to the drivetrain. And the drive wheel runs as + * fast as the ground velocity + * + *

However, if the propelling force exceeds the gripping, only the max gripping force is applied. The rest of the + * propelling force will cause the wheel to start skidding and make the odometry inaccurate. + * + * @param grippingForceNewtons the amount of gripping force that wheel can generate, in newtons + * @param moduleWorldFacing the current world facing of the module + * @param moduleCurrentGroundVelocity the current ground velocity of the module, world-reference + * @return a vector representing the propelling force that the module generates, world-reference + */ + private Vector2 getPropellingForce( + double grippingForceNewtons, Rotation2d moduleWorldFacing, Vector2 moduleCurrentGroundVelocity) { + final double driveWheelTorque = getDriveWheelTorque(), + theoreticalMaxPropellingForceNewtons = driveWheelTorque / WHEEL_RADIUS_METERS; + final boolean skidding = Math.abs(theoreticalMaxPropellingForceNewtons) > grippingForceNewtons; + final double propellingForceNewtons; + if (skidding) + propellingForceNewtons = Math.copySign(grippingForceNewtons, theoreticalMaxPropellingForceNewtons); + else propellingForceNewtons = theoreticalMaxPropellingForceNewtons; + + final double floorVelocityProjectionOnWheelDirectionMPS = moduleCurrentGroundVelocity.getMagnitude() + * Math.cos(moduleCurrentGroundVelocity.getAngleBetween(new Vector2(moduleWorldFacing.getRadians()))); + + // if the chassis is tightly gripped on floor, the floor velocity is projected to the wheel + this.driveEncoderUnGearedSpeedRadPerSec = + floorVelocityProjectionOnWheelDirectionMPS / WHEEL_RADIUS_METERS * DRIVE_GEAR_RATIO; + + // if the module is skidding + if (skidding) + this.driveEncoderUnGearedSpeedRadPerSec = + 0.5 * driveEncoderUnGearedSpeedRadPerSec + 0.5 * DRIVE_MOTOR.getSpeed(0, driveMotorAppliedVolts); + + return Vector2.create(propellingForceNewtons, moduleWorldFacing.getRadians()); + } + + /** + * + * + *

Calculates the amount of torque that the drive motor can generate on the wheel.

+ * + *

Before calculating the torque of the motor, the output voltage of the drive motor is constrained for the + * current limit through {@link BrushlessMotorSim#constrainOutputVoltage(DCMotor, double, double, double)}. + * + * @return the amount of torque on the wheel by the drive motor, in Newton * Meters + */ + private double getDriveWheelTorque() { + driveMotorAppliedVolts = driveMotorRequestedVolts; + + /* calculate the actual supply current */ + driveMotorSupplyCurrentAmps = DRIVE_MOTOR.getCurrent( + this.driveEncoderUnGearedSpeedRadPerSec, + MathUtil.applyDeadband(driveMotorAppliedVolts, DRIVE_FRICTION_VOLTAGE, 12)); + + /* calculate the torque generated, */ + final double torqueOnRotter = DRIVE_MOTOR.getTorque(driveMotorSupplyCurrentAmps); + return torqueOnRotter * DRIVE_GEAR_RATIO; + } + + /** @return the current module state of this simulation module */ + public SwerveModuleState getCurrentState() { + return new SwerveModuleState(getDriveWheelFinalSpeedRadPerSec() * WHEEL_RADIUS_METERS, steerAbsoluteFacing); + } + + /** + * + * + *

Obtains the "free spin" state of the module

+ * + *

The "free spin" state of a simulated module refers to its state after spinning freely for a long time under + * the current input voltage + * + * @return the free spinning module state + */ + protected SwerveModuleState getFreeSpinState() { + return new SwerveModuleState( + DRIVE_MOTOR.getSpeed( + DRIVE_MOTOR.getTorque(DRIVE_MOTOR.getCurrent(0, DRIVE_FRICTION_VOLTAGE)), + driveMotorAppliedVolts) + / DRIVE_GEAR_RATIO + * WHEEL_RADIUS_METERS, + steerAbsoluteFacing); + } + + /** + * + * + *

Cache the encoder values.

+ * + *

An internal method to cache the encoder values to their queues. + */ + private void updateDriveEncoders() { + this.driveEncoderUnGearedPositionRad += + this.driveEncoderUnGearedSpeedRadPerSec * SimulatedField.getSimulationDt(); + this.cachedDriveEncoderUnGearedPositionsRad.poll(); + this.cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); + } + + /** + * + * + *

Obtains the theoretical speed that the module can achieve.

+ * + * @return the theoretical maximum ground speed that the module can achieve, in m/s + */ + public double getModuleTheoreticalSpeedMPS() { + return DRIVE_MOTOR.freeSpeedRadPerSec / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS; + } + + /** + * + * + *

Obtains the theoretical maximum propelling force of ONE module.

+ * + *

Calculates the maximum propelling force with respect to the gripping force and the drive motor's torque under + * its current limit. + * + * @param robotMassKg the mass of the robot, is kilograms + * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force equally + * @return the maximum propelling force of EACH module + */ + public double getTheoreticalPropellingForcePerModule(double robotMassKg, int modulesCount) { + final double + maxThrustNewtons = DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS, + maxGrippingNewtons = 9.8 * robotMassKg / modulesCount * WHEELS_COEFFICIENT_OF_FRICTION; + + return Math.min(maxThrustNewtons, maxGrippingNewtons); + } + + /** + * + * + *

Obtains the theatrical linear acceleration that the robot can achieve.

+ * + *

Calculates the maximum linear acceleration of a robot, with respect to its mass and + * {@link #getTheoreticalPropellingForcePerModule(double, int)}. + * + * @param robotMassKg the mass of the robot, is kilograms + * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force equally + */ + public double getModuleMaxAccelerationMPSsq(double robotMassKg, int modulesCount) { + return getTheoreticalPropellingForcePerModule(robotMassKg, modulesCount) * modulesCount / robotMassKg; + } + + /** + * + * + *

Stores the coefficient of friction of some common used wheels.

+ */ + public enum WHEEL_GRIP { + RUBBER_WHEEL(1.25), + TIRE_WHEEL(1.2); + + public final double cof; + + WHEEL_GRIP(double cof) { + this.cof = cof; + } + } + package org.ironmaple.simulation.drivesims; + + import static edu.wpi.first.units.Units.Amps; + import static edu.wpi.first.units.Units.KilogramSquareMeters; + import static edu.wpi.first.units.Units.Radians; + import static edu.wpi.first.units.Units.RadiansPerSecond; + import static edu.wpi.first.units.Units.Volts; + + import edu.wpi.first.math.MathUtil; + import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; + import edu.wpi.first.math.geometry.Rotation2d; + import edu.wpi.first.math.kinematics.SwerveDriveOdometry; + import edu.wpi.first.math.kinematics.SwerveModuleState; + import edu.wpi.first.math.system.plant.DCMotor; + import edu.wpi.first.math.util.Units; + import java.util.Queue; + import java.util.concurrent.ConcurrentLinkedQueue; + import java.util.function.Supplier; + import org.dyn4j.geometry.Vector2; + import org.ironmaple.simulation.BrushlessMotorSim; + import org.ironmaple.simulation.SimulatedField; + + /** + * + * + *

Simulation for a Single Swerve Module.

+ * + *

This class provides a simulation for a single swerve module in the {@link SwerveDriveSimulation}. + * + *

1. Purpose

+ * + *

This class serves as the bridge between your code and the physics engine. + * + *

You will apply voltage outputs to the drive/steer motor of the module and obtain their encoder readings in your + * code, just as how you deal with your physical motors. + * + *

2. Perspectives

+ * + *
    + *
  • Simulates the steering mechanism using a custom brushless motor simulator. + *
  • Simulates the propelling force generated by the driving motor, with a current limit. + *
  • Simulates encoder readings, which can be used to simulate a {@link SwerveDriveOdometry}. + *
+ * + *

3. Simulating Odometry

+ * + *
    + *
  • Retrieve the encoder readings from {@link #getDriveEncoderUnGearedPositionRad()}} and + * {@link #getSteerAbsoluteFacing()}. + *
  • Use {@link SwerveDriveOdometry} to estimate the pose of your robot. + *
  • 250Hz + * Odometry is supported. You can retrive cached encoder readings from every sub-tick through + * {@link #getCachedDriveEncoderUnGearedPositionsRad()} and {@link #getCachedSteerAbsolutePositions()}. + *
+ * + *

An example of how to simulate odometry using this class is the ModuleIOSim.java + * from the Advanced Swerve Drive with maple-sim example. + */ + public class SwerveModuleSimulation { + public final DCMotor DRIVE_MOTOR; + private final BrushlessMotorSim steerMotorSim; + public final double DRIVE_CURRENT_LIMIT, + DRIVE_GEAR_RATIO, + STEER_GEAR_RATIO, + DRIVE_FRICTION_VOLTAGE, + WHEELS_COEFFICIENT_OF_FRICTION, + WHEEL_RADIUS_METERS, + DRIVE_WHEEL_INERTIA = 0.01; + private double driveMotorRequestedVolts = 0.0, + driveMotorAppliedVolts = 0.0, + driveMotorSupplyCurrentAmps = 0.0, + steerRelativeEncoderPositionRad = 0.0, + steerRelativeEncoderSpeedRadPerSec = 0.0, + steerAbsoluteEncoderSpeedRadPerSec = 0.0, + driveEncoderUnGearedPositionRad = 0.0, + driveEncoderUnGearedSpeedRadPerSec = 0.0; + private Rotation2d steerAbsoluteFacing = Rotation2d.fromRotations(Math.random()); + + private final double steerRelativeEncoderOffSet = (Math.random() - 0.5) * 30; + + private final Queue cachedSteerRelativeEncoderPositionsRad, cachedDriveEncoderUnGearedPositionsRad; + private final Queue cachedSteerAbsolutePositions; + + /** + * + * + *

Constructs a Swerve Module Simulation.

+ * + *

If you are using {@link SimulatedField#overrideSimulationTimings(double, int)} to use custom timings, you must + * call the method before constructing any swerve module simulations using this constructor. + * + * @param driveMotor the model of the driving motor + * @param steerMotor the model of the steering motor + * @param driveCurrentLimit the current limit for the driving motor, in amperes + * @param driveGearRatio the gear ratio for the driving motor, >1 is reduction + * @param steerGearRatio the gear ratio for the steering motor, >1 is reduction + * @param driveFrictionVoltage the measured minimum amount of voltage that can turn the driving rotter, in volts + * @param steerFrictionVoltage the measured minimum amount of voltage that can turn the steering rotter, in volts + * @param tireCoefficientOfFriction the coefficient + * of friction of the tires, normally around 1.5 + * @param wheelsRadiusMeters the radius of the wheels, in meters. Calculate it using + * {@link Units#inchesToMeters(double)}. + * @param steerRotationalInertia the rotational inertia of the steering mechanism + */ + public SwerveModuleSimulation( + DCMotor driveMotor, + DCMotor steerMotor, + double driveCurrentLimit, + double driveGearRatio, + double steerGearRatio, + double driveFrictionVoltage, + double steerFrictionVoltage, + double tireCoefficientOfFriction, + double wheelsRadiusMeters, + double steerRotationalInertia) { + DRIVE_MOTOR = driveMotor; + DRIVE_CURRENT_LIMIT = driveCurrentLimit; + DRIVE_GEAR_RATIO = driveGearRatio; + STEER_GEAR_RATIO = steerGearRatio; + DRIVE_FRICTION_VOLTAGE = driveFrictionVoltage; + WHEELS_COEFFICIENT_OF_FRICTION = tireCoefficientOfFriction; + WHEEL_RADIUS_METERS = wheelsRadiusMeters; + + this.steerMotorSim = new BrushlessMotorSim( + SimulatedField.getInstance(), + steerMotor, + steerGearRatio, + KilogramSquareMeters.of(steerRotationalInertia), + Volts.of(steerFrictionVoltage)); + + this.cachedDriveEncoderUnGearedPositionsRad = new ConcurrentLinkedQueue<>(); + for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) + cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); + this.cachedSteerRelativeEncoderPositionsRad = new ConcurrentLinkedQueue<>(); + for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) + cachedSteerRelativeEncoderPositionsRad.offer(steerRelativeEncoderPositionRad); + this.cachedSteerAbsolutePositions = new ConcurrentLinkedQueue<>(); + for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) + cachedSteerAbsolutePositions.offer(steerAbsoluteFacing); + + this.steerRelativeEncoderPositionRad = steerAbsoluteFacing.getRadians() + steerRelativeEncoderOffSet; + } + + /** + * + * + *

Requests the Driving Motor to Run at a Specified Voltage Output.

+ * + *

Think of it as the setVoltage() of your physical driving motor.

+ * + *

This method sets the desired voltage output for the driving motor. The change will be applied in the next + * sub-tick of the simulation. + * + *

Note: The requested voltage may not always be fully applied if the current is too high. The + * current limit may reduce the motor's output, similar to real motors. + * + *

To check the actual voltage applied to the drivetrain, use {@link #getDriveMotorAppliedVolts()}. + * + * @param volts the voltage to be applied to the driving motor + */ + public void requestDriveVoltageOut(double volts) { + this.driveMotorRequestedVolts = volts; + } + + /** + * + * + *

Requests the Steering Motor to Run at a Specified Voltage Output.

+ * + *

Think of it as the setVoltage() of your physical steering motor.

+ * + *

This method sets the desired voltage output for the steering motor. The change will be applied in the next + * sub-tick of the simulation. + * + *

Note: Similar to the drive motor, the requested voltage may not always be fully applied if + * the current exceeds the limit. The current limit will reduce the motor's output as needed, mimicking real motor + * behavior. + * + *

To check the actual voltage applied to the steering motor, use {@link #getSteerMotorAppliedVolts()}. + * + * @param volts the voltage to be applied to the steering motor + */ + public void requestSteerVoltageOut(double volts) { + this.steerMotorSim.setControl(Volts.of(volts)); + } + + /** + * + * + *

Obtains the Actual Output Voltage of the Drive Motor.

+ * + *

This method returns the actual voltage being applied to the drive motor. The actual applied voltage may differ + * from the value set by {@link #requestDriveVoltageOut(double)}. + * + *

If the motor's supply current is too high, the motor will automatically reduce its output voltage to protect + * the system. + * + * @return the actual output voltage of the drive motor, in volts + */ + public double getDriveMotorAppliedVolts() { + return driveMotorAppliedVolts; + } + + /** + * + * + *

Obtains the Actual Output Voltage of the Steering Motor.

+ * + *

This method returns the actual voltage being applied to the steering motor. It wraps around the + * {@link BrushlessMotorSim#getAppliedVolts()} method. + * + *

The actual applied voltage may differ from the value set by {@link #requestSteerVoltageOut(double)}. If the + * motor's supply current is too high, the motor will automatically reduce its output voltage to protect the system. + * + * @return the actual output voltage of the steering motor, in volts + */ + public double getSteerMotorAppliedVolts() { + return steerMotorSim.getRotorVoltage().in(Volts); + } + + /** + * + * + *

Obtains the Amount of Current Supplied to the Drive Motor.

+ * + *

Think of it as the getSupplyCurrent() of your physical drive motor.

+ * + * @return the current supplied to the drive motor, in amperes + */ + public double getDriveMotorSupplyCurrentAmps() { + return driveMotorSupplyCurrentAmps; + } + + /** + * + * + *

Obtains the Amount of Current Supplied to the Steer Motor.

+ * + *

Think of it as the getSupplyCurrent() of your physical steer motor.

+ * + *

This method wraps around {@link BrushlessMotorSim#getCurrentDrawAmps()}. + * + * @return the current supplied to the steer motor, in amperes + */ + public double getSteerMotorSupplyCurrentAmps() { + return steerMotorSim.getSupplyCurrent().in(Amps); + } + + /** + * + * + *

Obtains the Position of the Drive Encoder.

+ * + *

Think of it as the getPosition() of your physical driving motor.

+ * + *

This method is used to simulate your {@link SwerveDrivePoseEstimator}. + * + *

This value represents the un-geared position of the encoder, i.e., the amount of radians the drive motor's + * encoder has rotated. + * + *

To get the final wheel rotations, use {@link #getDriveWheelFinalPositionRad()}. + * + * @return the position of the drive motor's encoder, in radians (un-geared) + */ + public double getDriveEncoderUnGearedPositionRad() { + return driveEncoderUnGearedPositionRad; + } + + /** + * + * + *

Obtains the Final Position of the Drive Encoder (Wheel Rotations).

+ * + *

This method is used to simulate the {@link SwerveDrivePoseEstimator} by providing the final position of the + * drive encoder in terms of wheel rotations. + * + *

The value is calculated by dividing the un-geared encoder position (obtained from + * {@link #getDriveEncoderUnGearedPositionRad()}) by the drive gear ratio. + * + * @return the final position of the drive encoder (wheel rotations), in radians + */ + public double getDriveWheelFinalPositionRad() { + return getDriveEncoderUnGearedPositionRad() / DRIVE_GEAR_RATIO; + } + + /** + * + * + *

Obtains the Speed of the Drive Encoder (Un-Geared), in Radians per Second.

+ * + *

Think of it as the getVelocity() of your physical drive motor.

+ * + *

This method returns the current speed of the drive encoder in radians per second, without accounting for the + * drive gear ratio. + * + * @return the un-geared speed of the drive encoder, in radians per second + */ + public double getDriveEncoderUnGearedSpeedRadPerSec() { + return driveEncoderUnGearedSpeedRadPerSec; + } + + /** + * + * + *

Obtains the Final Speed of the Wheel, in Radians per Second.

+ * + * @return the final speed of the drive wheel, in radians per second + */ + public double getDriveWheelFinalSpeedRadPerSec() { + return getDriveEncoderUnGearedSpeedRadPerSec() / DRIVE_GEAR_RATIO; + } + + /** + * + * + *

Obtains the Relative Position of the Steer Encoder, in Radians.

+ * + *

Think of it as the getPosition() of your physical steer motor.

+ * + * @return the relative encoder position of the steer motor, in radians + */ + public double getSteerRelativeEncoderPositionRad() { + return steerRelativeEncoderPositionRad; + } + + /** + * + * + *

Obtains the Speed of the Steer Relative Encoder, in Radians per Second (Geared).

+ * + *

Think of it as the getVelocity() of your physical steer motor.

+ * + * @return the speed of the steer relative encoder, in radians per second (geared) + */ + public double getSteerRelativeEncoderSpeedRadPerSec() { + return steerRelativeEncoderSpeedRadPerSec; + } + + /** + * + * + *

Obtains the Absolute Facing of the Steer Mechanism.

+ * + *

Think of it as the getAbsolutePosition() of your CanCoder.

+ * + * @return the absolute facing of the steer mechanism, as a {@link Rotation2d} + */ + public Rotation2d getSteerAbsoluteFacing() { + return steerAbsoluteFacing; + } + + /** + * + * + *

Obtains the Absolute Rotational Velocity of the Steer Mechanism.

+ * + *

Think of it as the getVelocity() of your CanCoder.

+ * + * @return the absolute angular velocity of the steer mechanism, in radians per second + */ + public double getSteerAbsoluteEncoderSpeedRadPerSec() { + return steerAbsoluteEncoderSpeedRadPerSec; + } + + /** + * + * + *

Obtains the Cached Readings of the Drive Encoder's Un-Geared Position.

+ * + *

The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and can be retrieved + * using this method. + * + * @return an array of cached drive encoder un-geared positions, in radians + */ + public double[] getCachedDriveEncoderUnGearedPositionsRad() { + return cachedDriveEncoderUnGearedPositionsRad.stream() + .mapToDouble(value -> value) + .toArray(); + } + + /** + * + * + *

Obtains the Cached Readings of the Drive Encoder's Final Position (Wheel Rotations).

+ * + *

The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and are divided by the + * gear ratio to obtain the final wheel rotations. + * + * @return an array of cached drive encoder final positions (wheel rotations), in radians + */ + public double[] getCachedDriveWheelFinalPositionsRad() { + return cachedDriveEncoderUnGearedPositionsRad.stream() + .mapToDouble(value -> value / DRIVE_GEAR_RATIO) + .toArray(); + } + + /** + * + * + *

Obtains the Cached Readings of the Steer Relative Encoder's Position.

+ * + *

The values of {@link #getSteerRelativeEncoderPositionRad()} are cached at each sub-tick and can be retrieved + * using this method. + * + * @return an array of cached steer relative encoder positions, in radians + */ + public double[] getCachedSteerRelativeEncoderPositions() { + return cachedSteerRelativeEncoderPositionsRad.stream() + .mapToDouble(value -> value) + .toArray(); + } + + /** + * + * + *

Obtains the Cached Readings of the Steer Absolute Positions.

+ * + *

The values of {@link #getSteerAbsoluteFacing()} are cached at each sub-tick and can be retrieved using this + * method. + * + * @return an array of cached absolute steer positions, as {@link Rotation2d} objects + */ + public Rotation2d[] getCachedSteerAbsolutePositions() { + return cachedSteerAbsolutePositions.toArray(Rotation2d[]::new); + } + + protected double getGrippingForceNewtons(double gravityForceOnModuleNewtons) { + return gravityForceOnModuleNewtons * WHEELS_COEFFICIENT_OF_FRICTION; + } + + /** + * + * + *

Updates the Simulation for This Module.

+ * + *

This method is called once during every sub-tick of the simulation. It performs the following actions: + * + *

    + *
  • Updates the simulation of the steering mechanism. + *
  • Simulates the propelling force generated by the module. + *
  • Updates and caches the encoder readings for odometry simulation. + *
+ * + *

Note: Friction forces are not simulated in this method. + * + * @param moduleCurrentGroundVelocityWorldRelative the current ground velocity of the module, relative to the world + * @param robotFacing the absolute facing of the robot, relative to the world + * @param gravityForceOnModuleNewtons the gravitational force acting on this module, in newtons + * @return the propelling force generated by the module, as a {@link Vector2} object + */ + public Vector2 updateSimulationSubTickGetModuleForce( + Vector2 moduleCurrentGroundVelocityWorldRelative, + Rotation2d robotFacing, + double gravityForceOnModuleNewtons) { + updateSteerSimulation(); + + /* the maximum gripping force that the wheel can generate */ + final double grippingForceNewtons = getGrippingForceNewtons(gravityForceOnModuleNewtons); + final Rotation2d moduleWorldFacing = this.steerAbsoluteFacing.plus(robotFacing); + final Vector2 propellingForce = + getPropellingForce(grippingForceNewtons, moduleWorldFacing, moduleCurrentGroundVelocityWorldRelative); + updateDriveEncoders(); + + return propellingForce; + } + + /** + * + * + *

updates the simulation for the steer mechanism.

+ * + *

Updates the simulation for the steer mechanism and cache the encoder readings. + * + *

The steer mechanism is modeled by a {@link BrushlessMotorSim}. + */ + private void updateSteerSimulation() { + /* update the readings of the sensor */ + this.steerAbsoluteFacing = new Rotation2d(steerMotorSim.getPosition()); + this.steerRelativeEncoderPositionRad = steerMotorSim.getPosition().in(Radians) + steerRelativeEncoderOffSet; + this.steerAbsoluteEncoderSpeedRadPerSec = steerMotorSim.getVelocity().in(RadiansPerSecond); + this.steerRelativeEncoderSpeedRadPerSec = steerAbsoluteEncoderSpeedRadPerSec * STEER_GEAR_RATIO; + + /* cache sensor readings to queue for high-frequency odometry */ + this.cachedSteerAbsolutePositions.poll(); + this.cachedSteerAbsolutePositions.offer(steerAbsoluteFacing); + this.cachedSteerRelativeEncoderPositionsRad.poll(); + this.cachedSteerRelativeEncoderPositionsRad.offer(steerRelativeEncoderPositionRad); + } + + /** + * + * + *

Calculates the amount of propelling force that the module generates.

+ * + *

The swerve module's drive motor will generate a propelling force. + * + *

For most of the time, that propelling force is directly applied to the drivetrain. And the drive wheel runs as + * fast as the ground velocity + * + *

However, if the propelling force exceeds the gripping, only the max gripping force is applied. The rest of the + * propelling force will cause the wheel to start skidding and make the odometry inaccurate. + * + * @param grippingForceNewtons the amount of gripping force that wheel can generate, in newtons + * @param moduleWorldFacing the current world facing of the module + * @param moduleCurrentGroundVelocity the current ground velocity of the module, world-reference + * @return a vector representing the propelling force that the module generates, world-reference + */ + private Vector2 getPropellingForce( + double grippingForceNewtons, Rotation2d moduleWorldFacing, Vector2 moduleCurrentGroundVelocity) { + final double driveWheelTorque = getDriveWheelTorque(), + theoreticalMaxPropellingForceNewtons = driveWheelTorque / WHEEL_RADIUS_METERS; + final boolean skidding = Math.abs(theoreticalMaxPropellingForceNewtons) > grippingForceNewtons; + final double propellingForceNewtons; + if (skidding) + propellingForceNewtons = Math.copySign(grippingForceNewtons, theoreticalMaxPropellingForceNewtons); + else propellingForceNewtons = theoreticalMaxPropellingForceNewtons; + + final double floorVelocityProjectionOnWheelDirectionMPS = moduleCurrentGroundVelocity.getMagnitude() + * Math.cos(moduleCurrentGroundVelocity.getAngleBetween(new Vector2(moduleWorldFacing.getRadians()))); + + // if the chassis is tightly gripped on floor, the floor velocity is projected to the wheel + this.driveEncoderUnGearedSpeedRadPerSec = + floorVelocityProjectionOnWheelDirectionMPS / WHEEL_RADIUS_METERS * DRIVE_GEAR_RATIO; + + // if the module is skidding + if (skidding) + this.driveEncoderUnGearedSpeedRadPerSec = + 0.5 * driveEncoderUnGearedSpeedRadPerSec + 0.5 * DRIVE_MOTOR.getSpeed(0, driveMotorAppliedVolts); + + return Vector2.create(propellingForceNewtons, moduleWorldFacing.getRadians()); + } + + /** + * + * + *

Calculates the amount of torque that the drive motor can generate on the wheel.

+ * + *

Before calculating the torque of the motor, the output voltage of the drive motor is constrained for the + * current limit through {@link BrushlessMotorSim#constrainOutputVoltage(DCMotor, double, double, double)}. + * + * @return the amount of torque on the wheel by the drive motor, in Newton * Meters + */ + private double getDriveWheelTorque() { + driveMotorAppliedVolts = driveMotorRequestedVolts; + + /* calculate the actual supply current */ + driveMotorSupplyCurrentAmps = DRIVE_MOTOR.getCurrent( + this.driveEncoderUnGearedSpeedRadPerSec, + MathUtil.applyDeadband(driveMotorAppliedVolts, DRIVE_FRICTION_VOLTAGE, 12)); + + /* calculate the torque generated, */ + final double torqueOnRotter = DRIVE_MOTOR.getTorque(driveMotorSupplyCurrentAmps); + return torqueOnRotter * DRIVE_GEAR_RATIO; + } + + /** @return the current module state of this simulation module */ + public SwerveModuleState getCurrentState() { + return new SwerveModuleState(getDriveWheelFinalSpeedRadPerSec() * WHEEL_RADIUS_METERS, steerAbsoluteFacing); + } + + /** + * + * + *

Obtains the "free spin" state of the module

+ * + *

The "free spin" state of a simulated module refers to its state after spinning freely for a long time under + * the current input voltage + * + * @return the free spinning module state + */ + protected SwerveModuleState getFreeSpinState() { + return new SwerveModuleState( + DRIVE_MOTOR.getSpeed( + DRIVE_MOTOR.getTorque(DRIVE_MOTOR.getCurrent(0, DRIVE_FRICTION_VOLTAGE)), + driveMotorAppliedVolts) + / DRIVE_GEAR_RATIO + * WHEEL_RADIUS_METERS, + steerAbsoluteFacing); + } + + /** + * + * + *

Cache the encoder values.

+ * + *

An internal method to cache the encoder values to their queues. + */ + private void updateDriveEncoders() { + this.driveEncoderUnGearedPositionRad += + this.driveEncoderUnGearedSpeedRadPerSec * SimulatedField.getSimulationDt(); + this.cachedDriveEncoderUnGearedPositionsRad.poll(); + this.cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); + } + + /** + * + * + *

Obtains the theoretical speed that the module can achieve.

+ * + * @return the theoretical maximum ground speed that the module can achieve, in m/s + */ + public double getModuleTheoreticalSpeedMPS() { + return DRIVE_MOTOR.freeSpeedRadPerSec / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS; + } + + /** + * + * + *

Obtains the theoretical maximum propelling force of ONE module.

+ * + *

Calculates the maximum propelling force with respect to the gripping force and the drive motor's torque under + * its current limit. + * + * @param robotMassKg the mass of the robot, is kilograms + * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force equally + * @return the maximum propelling force of EACH module + */ + public double getTheoreticalPropellingForcePerModule(double robotMassKg, int modulesCount) { + final double + maxThrustNewtons = DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS, + maxGrippingNewtons = 9.8 * robotMassKg / modulesCount * WHEELS_COEFFICIENT_OF_FRICTION; + + return Math.min(maxThrustNewtons, maxGrippingNewtons); + } + + /** + * + * + *

Obtains the theatrical linear acceleration that the robot can achieve.

+ * + *

Calculates the maximum linear acceleration of a robot, with respect to its mass and + * {@link #getTheoreticalPropellingForcePerModule(double, int)}. + * + * @param robotMassKg the mass of the robot, is kilograms + * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force equally + */ + public double getModuleMaxAccelerationMPSsq(double robotMassKg, int modulesCount) { + return getTheoreticalPropellingForcePerModule(robotMassKg, modulesCount) * modulesCount / robotMassKg; + } + + /** + * + * + *

Stores the coefficient of friction of some common used wheels.

+ */ + public enum WHEEL_GRIP { + RUBBER_WHEEL(1.25), + TIRE_WHEEL(1.2); + + public final double cof; + + WHEEL_GRIP(double cof) { + this.cof = cof; + } + } + + /** + * creates a SDS Mark4 + * Swerve Module for simulation + */ + public static Supplier getMark4( + DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { + return () -> new SwerveModuleSimulation( + driveMotor, + steerMotor, + driveCurrentLimitAmps, + switch (gearRatioLevel) { + case 1 -> 8.14; + case 2 -> 6.75; + case 3 -> 6.12; + case 4 -> 5.14; + default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); + }, + 12.8, + 0.2, + 0.3, + wheelCOF, + Units.inchesToMeters(2), + 0.03); + } + + /** + * creates a SDS + * Mark4-i Swerve Module for simulation + */ + public static Supplier getMark4i( + DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { + return () -> new SwerveModuleSimulation( + driveMotor, + steerMotor, + driveCurrentLimitAmps, + switch (gearRatioLevel) { + case 1 -> 8.14; + case 2 -> 6.75; + case 3 -> 6.12; + case 4 -> 5.15; + default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); + }, + 150.0 / 7.0, + 0.2, + 1, + wheelCOF, + Units.inchesToMeters(2), + 0.025); + } + + /** + * creates a SDS Mark4-n Swerve + * Module for simulation + */ + public static Supplier getMark4n( + DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { + return () -> new SwerveModuleSimulation( + driveMotor, + steerMotor, + driveCurrentLimitAmps, + switch (gearRatioLevel) { + case 1 -> 7.13; + case 2 -> 5.9; + case 3 -> 5.36; + default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); + }, + 18.75, + 0.25, + 1, + wheelCOF, + Units.inchesToMeters(2), + 0.025); + } + + /** + * creates a WCP SwerveX Swerve Module + * for simulation + * + *

X1 Ratios are gearRatioLevel 1-3
+ * X2 Ratios are gearRatioLevel 4-6
+ * X3 Ratios are gearRatioLevel 7-9 + */ + public static Supplier getSwerveX( + DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { + return () -> new SwerveModuleSimulation( + driveMotor, + steerMotor, + driveCurrentLimitAmps, + switch (gearRatioLevel) { + case 1 -> 7.85; + case 2 -> 7.13; + case 3 -> 6.54; + case 4 -> 6.56; + case 5 -> 5.96; + case 6 -> 5.46; + case 7 -> 5.14; + case 8 -> 4.75; + case 9 -> 4.41; + default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); + }, + 11.3142, + 0.2, + 0.3, + wheelCOF, + Units.inchesToMeters(2), + 0.03); + } + + /** + * creates a WCP SwerveX Flipped + * Swerve Module for simulation + * + *

X1 Ratios are gearRatioLevel 1-3
+ * X2 Ratios are gearRatioLevel 4-6
+ * X3 Ratios are gearRatioLevel 7-9 + */ + public static Supplier getSwerveXFlipped( + DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { + return () -> new SwerveModuleSimulation( + driveMotor, + steerMotor, + driveCurrentLimitAmps, + switch (gearRatioLevel) { + case 1 -> 8.1; + case 2 -> 7.36; + case 3 -> 6.75; + case 4 -> 6.72; + case 5 -> 6.11; + case 6 -> 5.6; + case 7 -> 5.51; + case 8 -> 5.01; + case 9 -> 4.59; + default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); + }, + 11.3714, + 0.2, + 0.3, + wheelCOF, + Units.inchesToMeters(2), + 0.03); + } + + /** + * creates a WCP SwerveXS Swerve + * Module for simulation + * + *

X1 Ratios are gearRatioLevel 1-3
+ * X2 Ratios are gearRatioLevel 4-6 + */ + public static Supplier getSwerveXS( + DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { + return () -> new SwerveModuleSimulation( + driveMotor, + steerMotor, + driveCurrentLimitAmps, + switch (gearRatioLevel) { + case 1 -> 6.0; + case 2 -> 5.54; + case 3 -> 5.14; + case 4 -> 4.71; + case 5 -> 4.4; + case 6 -> 4.13; + default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); + }, + 41.25, + 0.2, + 0.3, + wheelCOF, + Units.inchesToMeters(1.5), + 0.03); + } + + /** + * creates a WCP SwerveX2 Swerve + * Module for simulation + * + *

X1 Ratios are gearRatioLevel 1-3
+ * X2 Ratios are gearRatioLevel 4-6
+ * X3 Ratios are gearRatioLevel 7-9
+ * X4 Ratios are gearRatioLevel 10-12 + */ + public static Supplier getSwerveX2( + DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { + return () -> new SwerveModuleSimulation( + driveMotor, + steerMotor, + driveCurrentLimitAmps, + switch (gearRatioLevel) { + case 1 -> 7.67; + case 2 -> 6.98; + case 3 -> 6.39; + case 4 -> 6.82; + case 5 -> 6.20; + case 6 -> 5.68; + case 7 -> 6.48; + case 8 -> 5.89; + case 9 -> 5.40; + case 10 -> 5.67; + case 11 -> 5.15; + case 12 -> 4.73; + default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); + }, + 12.1, + 0.2, + 0.3, + wheelCOF, + Units.inchesToMeters(2), + 0.03); + } + + /** + * creates a WCP SwerveX2S Swerve + * Module for simulation + * + *

X1 Ratios are gearRatioLevel 1-3
+ * X2 Ratios are gearRatioLevel 4-6
+ * X3 Ratios are gearRatioLevel 7-9 + */ + public static Supplier getSwerveX2S( + DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { + return () -> new SwerveModuleSimulation( + driveMotor, + steerMotor, + driveCurrentLimitAmps, + switch (gearRatioLevel) { + case 1 -> 6.0; + case 2 -> 5.63; + case 3 -> 5.29; + case 4 -> 4.94; + case 5 -> 4.67; + case 6 -> 4.42; + case 7 -> 4.11; + case 8 -> 3.9; + case 9 -> 3.71; + default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); + }, + 25.9, + 0.2, + 0.3, + wheelCOF, + Units.inchesToMeters(1.875), + 0.03); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 1b968bcd..3c90a2e7 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -198,6 +198,7 @@ xSpeed, ySpeed, rotationSpeed, getPose().getRotation()) setModuleStates(swerveModuleStates); Logger.recordOutput("SwerveStates/SwerveModuleStates", swerveModuleStates); + Logger.recordOutput("SwerveStates/MeasuredStates", getModuleStates()); } /** Returns 0 degrees if the robot is on the blue alliance, 180 if on the red alliance. */ @@ -251,11 +252,11 @@ private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { * rotation at the sampled timestamp. */ private SwerveModulePosition[] getModulesPosition(int timestampIndex) { - // Check if swerveModules is null or empty, return early if so - if (swerveModules == null || swerveModules.length == 0) { + // Check if swerveModules is null or empty, return early if so + if (swerveModules == null || swerveModules.length == 0) { // Logger.error("Swerve modules array is empty or not initialized properly!"); - return new SwerveModulePosition[4]; // Return an empty array of the expected size - } + return new SwerveModulePosition[4]; // Return an empty array of the expected size + } SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[swerveModules.length]; for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) swerveModulePositions[moduleIndex] = diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index e16c18d9..1d9c7724 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -47,10 +47,10 @@ public void periodic() { private void updateOdometryPositions() { - odometryPositions = new SwerveModulePosition[inputs.odometryTimestamps.length]; + odometryPositions = new SwerveModulePosition[inputs.odometryDriveWheelRevolutions.length]; for (int i = 0; i < odometryPositions.length; i++) { - double positionMeters = inputs.drivePosition; - Rotation2d angle = inputs.turnAbsolutePosition; + double positionMeters = inputs.odometryDriveWheelRevolutions[i]; + Rotation2d angle = inputs.odometrySteerPositions[i]; odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java index 25b0bf9e..e3915423 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java @@ -22,8 +22,10 @@ class ModuleInputs { public double turnCurrentAmps = 0.0; public double[] odometryTimestamps = new double[] {}; + + public double[] odometryDriveWheelRevolutions = new double[] {}; + public Rotation2d[] odometrySteerPositions = new Rotation2d[] {}; - public double turnPosition = 0.0; } /** diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java index 48da6abe..13463975 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java @@ -153,12 +153,10 @@ public void updateInputs(ModuleInputs inputs) { turnEncoderAbsolutePosition.clear(); } - inputs.turnPosition = turnMotor.getPosition().getValueAsDouble(); inputs.driveAppliedVolts = driveMotorAppliedVoltage.getValueAsDouble(); inputs.driveCurrentAmps = driveMotorCurrent.getValueAsDouble(); - inputs.turnPosition = turnMotor.getPosition().getValueAsDouble(); inputs.turnVelocity = turnEncoderVelocity.getValueAsDouble(); inputs.turnAppliedVolts = turnMotorAppliedVolts.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index 41d68c08..5bb4d4cc 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -2,12 +2,15 @@ import static edu.wpi.first.units.Units.*; +import java.util.Arrays; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; import frc.robot.extras.simulation.OdometryTimestampsSim; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation; @@ -17,12 +20,12 @@ public class SimulatedModule implements ModuleInterface { private final SwerveModuleSimulation moduleSimulation; - private final PIDController drivePID = new PIDController(5, 0, 0); + private final PIDController drivePID = new PIDController(.5, 0, 0); private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(5, 0, 0); private final Constraints turnConstraints = new Constraints(5, 2); private final ProfiledPIDController turnPID = - new ProfiledPIDController(50, 0, 0, turnConstraints); + new ProfiledPIDController(500, 0, 0, turnConstraints); private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(5, 0, 0); public SimulatedModule(SwerveModuleSimulation moduleSimulation) { @@ -31,24 +34,28 @@ public SimulatedModule(SwerveModuleSimulation moduleSimulation) { @Override public void updateInputs(ModuleInputs inputs) { - inputs.drivePosition = - Radians.of(moduleSimulation.getDriveEncoderFinalPositionRad()).in(Rotations); + inputs.drivePosition = Units.radiansToRotations(moduleSimulation.getDriveEncoderFinalPositionRad()); inputs.driveVelocity = - RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) - .in(RotationsPerSecond); + Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()); inputs.driveAppliedVolts = moduleSimulation.getDriveMotorAppliedVolts(); - inputs.driveCurrentAmps = Math.abs(moduleSimulation.getDriveMotorSupplyCurrentAmps()); + inputs.driveCurrentAmps = moduleSimulation.getDriveMotorSupplyCurrentAmps(); inputs.turnAbsolutePosition = moduleSimulation.getTurnAbsolutePosition(); - inputs.turnPosition = - Radians.of(moduleSimulation.getTurnRelativeEncoderPositionRad()).in(Rotations); - inputs.turnVelocity = - RadiansPerSecond.of(moduleSimulation.getTurnRelativeEncoderSpeedRadPerSec()) - .in(RotationsPerSecond); + inputs.turnVelocity = moduleSimulation.getTurnAbsoluteEncoderSpeedRadPerSec(); inputs.turnAppliedVolts = moduleSimulation.getTurnMotorAppliedVolts(); - inputs.turnCurrentAmps = Math.abs(moduleSimulation.getTurnMotorSupplyCurrentAmps()); + inputs.turnCurrentAmps = moduleSimulation.getTurnMotorSupplyCurrentAmps(); + + + inputs.odometryDriveWheelRevolutions = Arrays.stream(moduleSimulation.getCachedDriveWheelFinalPositionsRad()) + .map(Units::radiansToRotations) + .toArray(); + + inputs.odometrySteerPositions = moduleSimulation.getCachedTurnAbsolutePositions(); + inputs.odometryTimestamps = OdometryTimestampsSim.getTimestamps(); + + inputs.isConnected = true; } @Override @@ -72,7 +79,7 @@ public void setDesiredState(SwerveModuleState desiredState) { if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) { moduleSimulation.requestDriveVoltageOut(0); - // moduleSimulation.requestTurnVoltageOut(0); + moduleSimulation.requestTurnVoltageOut(0); return; } From 43a1667c7f583b066600ece4941356b9acfb3256 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Wed, 6 Nov 2024 07:28:01 -0500 Subject: [PATCH 08/75] push whatever this is --- src/main/java/frc/robot/RobotContainer.java | 20 +- .../simulation/field/SimulatedField.java | 31 +- .../swerve/BrushlessMotorSim.java | 866 ++++++------- .../swerve/SwerveModuleSimulation.java | 1070 ++--------------- .../subsystems/swerve/SwerveConstants.java | 6 +- .../robot/subsystems/swerve/SwerveDrive.java | 9 +- .../robot/subsystems/swerve/SwerveModule.java | 19 +- .../swerve/moduleIO/ModuleInterface.java | 3 +- .../swerve/moduleIO/PhysicalModule.java | 2 - .../swerve/moduleIO/SimulatedModule.java | 54 +- 10 files changed, 622 insertions(+), 1458 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 73385411..869a218d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,9 +14,10 @@ import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveDriveSimulation; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation; -import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.DRIVE_WHEEL_TYPE; +import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP; import frc.robot.subsystems.swerve.SwerveConstants; import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; +import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; import frc.robot.subsystems.swerve.SwerveDrive; import frc.robot.subsystems.swerve.gyroIO.GyroInterface; import frc.robot.subsystems.swerve.gyroIO.PhysicalGyro; @@ -81,14 +82,14 @@ public RobotContainer() { 45, DriveConstants.TRACK_WIDTH, DriveConstants.WHEEL_BASE, - DriveConstants.TRACK_WIDTH +.2, + DriveConstants.TRACK_WIDTH + .2, DriveConstants.WHEEL_BASE + .2, SwerveModuleSimulation.getModule( DCMotor.getFalcon500(1), DCMotor.getFalcon500(1), 60, - DRIVE_WHEEL_TYPE.TIRE, - 7.36), + WHEEL_GRIP.TIRE_WHEEL, + ModuleConstants.DRIVE_GEAR_RATIO), gyroSimulation, new Pose2d(3, 3, new Rotation2d())); SimulatedField.getInstance().addDriveTrainSimulation(swerveDriveSimulation); @@ -280,9 +281,14 @@ private void configureButtonBindings() { // pivotSubsystem, visionSubsystem, driverLeftStick, driverYButton, ledSubsystem)); // // Resets the robot angle in the odometry, factors in which alliance the robot is on - driverRightDirectionPad.onTrue(new InstantCommand(() -> swerveDrive.setPose(new - Pose2d(swerveDrive.getPose().getX(), swerveDrive.getPose().getY(), - Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset()))))); + driverRightDirectionPad.onTrue( + new InstantCommand( + () -> + swerveDrive.setPose( + new Pose2d( + swerveDrive.getPose().getX(), + swerveDrive.getPose().getY(), + Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset()))))); // // // Reset robot odometry based on vision pose measurement from april tags // driverLeftDirectionPad.onTrue(new InstantCommand(() -> // swerveDrive.resetOdometry(visionSubsystem.getLastSeenPose()))); diff --git a/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java b/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java index 34a02bd9..317cbc71 100644 --- a/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java +++ b/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java @@ -13,7 +13,6 @@ import frc.robot.extras.simulation.mechanismSim.swerve.AbstractDriveTrainSimulation; import frc.robot.extras.simulation.mechanismSim.swerve.BrushlessMotorSim; import frc.robot.extras.util.GeomUtil; - import java.lang.ref.WeakReference; import java.util.*; import org.dyn4j.dynamics.Body; @@ -284,10 +283,9 @@ public void simulationPeriodic() { "MapleArenaSimulation/Dyn4jEngineCPUTimeMS", (System.nanoTime() - t0) / 1000000.0); } - - public void addMotor(BrushlessMotorSim motor) { - motors.add(new WeakReference<>(motor)); - } + public void addMotor(BrushlessMotorSim motor) { + motors.add(new WeakReference<>(motor)); + } /** * @@ -306,18 +304,17 @@ public void addMotor(BrushlessMotorSim motor) { * */ private void simulationSubTick() { - ArrayList motorCurrents = new ArrayList<>(); - for (var motor : motors) { - BrushlessMotorSim motorRef = motor.get(); - if (motorRef != null) { - motorRef.update(); - } - } - double vin = BatterySim.calculateLoadedBatteryVoltage( - 12.2, - 0.015, - motorCurrents.stream().mapToDouble(Double::doubleValue).toArray()); - RoboRioSim.setVInVoltage(vin); + ArrayList motorCurrents = new ArrayList<>(); + for (var motor : motors) { + BrushlessMotorSim motorRef = motor.get(); + if (motorRef != null) { + motorRef.update(); + } + } + double vin = + BatterySim.calculateLoadedBatteryVoltage( + 12.2, 0.015, motorCurrents.stream().mapToDouble(Double::doubleValue).toArray()); + RoboRioSim.setVInVoltage(vin); for (AbstractDriveTrainSimulation driveTrainSimulation : driveTrainSimulations) driveTrainSimulation.simulationSubTick(); diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java index 873c4982..54348897 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java @@ -36,8 +36,8 @@ * *

{@link DCMotorSim} with a bit of extra spice.

* - *

This class extends the functionality of the original {@link DCMotorSim} and models the following aspects in - * addition: + *

This class extends the functionality of the original {@link DCMotorSim} and models the + * following aspects in addition: * *

    *
  • Friction force on the rotor. @@ -46,425 +46,447 @@ *
*/ public class BrushlessMotorSim { - public enum OutputType { - VOLTAGE, - CURRENT - } - - public enum OutputMode { - VELOCITY, - POSITION, - OPEN_LOOP - } - - /** The Constants for the motor */ - private final DCMotor motor; - /** The dynamics simulation for the motor */ - private final DCMotorSim sim; - /** The gear ratio, value above 1.0 are a reduction */ - private final double gearing; - /** The voltage required to overcome friction */ - private final Voltage frictionVoltage; - - private final PIDController poseVoltController = new PIDController(0, 0, 0); - private final PIDController veloVoltController = new PIDController(0, 0, 0); - private final PIDController poseCurrentController = new PIDController(0, 0, 0); - private final PIDController veloCurrentController = new PIDController(0, 0, 0); - - private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0, 0, 0); - - private Current currentLimit = Amps.of(300.0); - - private OutputType outputType = OutputType.VOLTAGE; - private OutputMode outputMode = OutputMode.OPEN_LOOP; - private double output = 0.0; - - private Angle forwardLimit = Radians.of(Double.POSITIVE_INFINITY); - private Angle reverseLimit = Radians.of(Double.NEGATIVE_INFINITY); - - /** - * - * - *

Constructs a Brushless Motor Simulation Instance.

- * - * @param motor the {@link DCMotor} model representing the motor(s) in the simulation - * @param gearRatio the gear ratio of the mechanism; values greater than 1 indicate a reduction - * @param loadIntertia the rotational inertia of the mechanism - * @param frictionVoltage the voltage required to keep the motor moving at a constant velocity - */ - public BrushlessMotorSim( - SimulatedField arena, - DCMotor motor, - double gearRatio, - MomentOfInertia loadIntertia, - Voltage frictionVoltage) { - this.sim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(motor, loadIntertia.in(KilogramSquareMeters), gearRatio), motor); - this.motor = motor; - this.gearing = gearRatio; - this.frictionVoltage = frictionVoltage; - - arena.addMotor(this); - } - - /** - * Configures the angle of the motor. - * - * @param angle the angle of the motor - * @return this instance for method chaining - */ - public BrushlessMotorSim withOverrideAngle(Angle angle) { - sim.setAngle(angle.in(Radians)); - return this; - } - - /** - * Configures the angular velocity of the motor. - * - * @param angularVelocity the angular velocity of the motor - * @return this instance for method chaining - */ - public BrushlessMotorSim withOverrideAngularVelocity(AngularVelocity angularVelocity) { - sim.setAngularVelocity(angularVelocity.in(RadiansPerSecond)); - return this; - } - - public BrushlessMotorSim withFeedForward( - Voltage kS, Per kV, Per kA) { - var kVUnit = PerUnit.combine(Volts, RadiansPerSecond); - var kAUnit = PerUnit.combine(Volts, RadiansPerSecondPerSecond); - feedforward = new SimpleMotorFeedforward( - kS.in(Volts), kV.in(kVUnit), kA.in(kAUnit), SimulatedField.getSimulationDt()); - return this; - } - - /** - * Configures the PD controller for Positional Requests using {@link OutputType#VOLTAGE}. - * - *

This is unit safe and can be configure like so: - * - * 


-     * // Volts per Rotation of error is how CTRE handles PID when used with voltage requests
-     * sim.withPositionalVoltageController(
-     *   Volts.per(Rotation).ofNative(100.0),
-     *   Volts.per(RotationsPerSecond).ofNative(5.0)
-     * );
-     *
- * - * @param kP the proportional gain - * @param kD the derivative gain - * @return this instance for method chaining - */ - public BrushlessMotorSim withPositionalVoltageController( - Per kP, Per kD) { - var kPUnit = PerUnit.combine(Volts, Radians); - var kDUnit = PerUnit.combine(Volts, RadiansPerSecond); - poseVoltController.setP(kP.in(kPUnit)); - poseVoltController.setD(kD.in(kDUnit)); - return this; - } - - /** - * Configures the PD controller for Velocity Requests using {@link OutputType#VOLTAGE}. - * - *

This is unit safe and can be configure like so: - * - * 


-     * // Volts per RPS of error is how CTRE handles PID when used with voltage requests
-     * sim.withVelocityVoltageController(
-     *   Volts.per(RotationsPerSecond).ofNative(0.4)
-     * );
-     *
- * - * @param kP the proportional gain - * @return this instance for method chaining - */ - public BrushlessMotorSim withVelocityVoltageController(Per kP) { - var kPUnit = PerUnit.combine(Volts, Radians); - veloVoltController.setP(kP.in(kPUnit)); - return this; - } - - /** - * Configures the PD controller for Positional Requests using {@link OutputType#CURRENT}. - * - *

This is unit safe and can be configure like so: - * - * 


-     * // Amps per Rotation of error is how CTRE handles PID when used with current requests
-     * sim.withPositionalCurrentController(
-     *   Amps.per(Rotation).ofNative(100.0),
-     *   Amps.per(RotationsPerSecond).ofNative(5.0)
-     * );
-     *
- * - * @param kP the proportional gain - * @param kD the derivative gain - * @return this instance for method chaining - */ - public BrushlessMotorSim withPositionalCurrentController( - Per kP, Per kD) { - var kPUnit = PerUnit.combine(Amps, Radians); - var kDUnit = PerUnit.combine(Amps, RadiansPerSecond); - poseCurrentController.setP(kP.in(kPUnit)); - poseCurrentController.setD(kD.in(kDUnit)); - return this; - } - - /** - * Configures the PD controller for Velocity Requests using {@link OutputType#CURRENT}. - * - *

This is unit safe and can be configure like so: - * - * 


-     * // Amps per RPS of error is how CTRE handles PID when used with current requests
-     * sim.withVelocityCurrentController(
-     *   Amps.per(RotationsPerSecond).ofNative(0.4)
-     * );
-     *
- * - * @param kP the proportional gain - * @return this instance for method chaining - */ - public BrushlessMotorSim withVelocityCurrentController(Per kP) { - var kPUnit = PerUnit.combine(Amps, Radians); - veloCurrentController.setP(kP.in(kPUnit)); - return this; - } - - /** - * Configures the positionaly controllers to use continuous wrap. - * - * @param min the minimum angle - * @param max the maximum angle - * @return this instance for method chaining - * @see PIDController#enableContinuousInput(double, double) - */ - public BrushlessMotorSim withControllerContinousInput(Angle min, Angle max) { - poseVoltController.enableContinuousInput(min.in(Radians), max.in(Radians)); - poseCurrentController.enableContinuousInput(min.in(Radians), max.in(Radians)); - return this; - } - - /** - * Configures the current limit for the motor. - * - *

This is the total current limit for the sim - * - * @param currentLimit the current limit for the motor - * @return - */ - public BrushlessMotorSim withStatorCurrentLimit(Current currentLimit) { - // this is a limit across the sum of all motors output, - // so it should be set to the total current limit of the mechanism - this.currentLimit = currentLimit; - return this; - } - - /** - * Configures the hard limits for the motor. - * - * @param forwardLimit the forward limit - * @param reverseLimit the reverse limit - * @return this instance for method chaining - */ - public BrushlessMotorSim withHardLimits(Angle forwardLimit, Angle reverseLimit) { - this.forwardLimit = forwardLimit; - this.reverseLimit = reverseLimit; - return this; - } - - public MomentOfInertia getMOI() { - return KilogramSquareMeters.of(sim.getJKgMetersSquared()); - } - - public Angle getPosition() { - return Radians.of(sim.getAngularPositionRad()); - } - - public AngularVelocity getVelocity() { - return RadiansPerSecond.of(sim.getAngularVelocityRadPerSec()); - } - - public AngularAcceleration getAcceleration() { - return RadiansPerSecondPerSecond.of(sim.getAngularAccelerationRadPerSecSq()); - } - - public Current getStatorCurrentDraw() { - return Amps.of(sim.getCurrentDrawAmps()); - } - - public Current getSupplyCurrent() { - // https://www.chiefdelphi.com/t/current-limiting-talonfx-values/374780/10 - return getStatorCurrentDraw().times(sim.getInputVoltage() / RobotController.getBatteryVoltage()); - } - - public Voltage getRotorVoltage() { - return Volts.of(sim.getInputVoltage()); - } - - public Voltage getSupplyVoltage() { - return Volts.of(RobotController.getBatteryVoltage()); - } - - public void setControl(OutputType outputType, AngularVelocity velo) { - this.outputType = outputType; - this.outputMode = OutputMode.VELOCITY; - this.output = velo.in(RadiansPerSecond); - } - - public void setControl(OutputType outputType, Angle pos) { - this.outputType = outputType; - this.outputMode = OutputMode.POSITION; - this.output = pos.in(Radians); - } - - public void setControl(Current amps) { - this.outputType = OutputType.CURRENT; - this.outputMode = OutputMode.OPEN_LOOP; - this.output = amps.in(Amps); - } - - public void setControl(Voltage volts) { - this.outputType = OutputType.VOLTAGE; - this.outputMode = OutputMode.OPEN_LOOP; - this.output = volts.in(Volts); - } - - public void setControl() { - this.outputType = OutputType.VOLTAGE; - this.outputMode = OutputMode.OPEN_LOOP; - this.output = 0.0; - } - - public void update() { - double dtSeconds = SimulatedField.getSimulationDt(); - switch (this.outputType) { - case VOLTAGE -> { - switch (this.outputMode) { - case OPEN_LOOP -> { - driveAtVoltage(Volts.of(output)); - } - case POSITION -> { - Voltage voltage = Volts.of( - poseVoltController.calculate(getPosition().in(Radians), output)); - Voltage feedforwardVoltage = feedforward.calculate(getVelocity(), velocityForVolts(voltage)); - driveAtVoltage(feedforwardVoltage.plus(voltage)); - } - case VELOCITY -> { - Voltage voltage = Volts.of( - veloVoltController.calculate(getVelocity().in(RadiansPerSecond), output)); - Voltage feedforwardVoltage = feedforward.calculate(getVelocity(), RadiansPerSecond.of(output)); - driveAtVoltage(voltage.plus(feedforwardVoltage)); - } - } - } - case CURRENT -> { - switch (this.outputMode) { - case OPEN_LOOP -> { - sim.setInputVoltage( - voltsForAmps(Amps.of(output), getVelocity()).in(Volts)); - } - case POSITION -> { - Current current = Amps.of( - poseCurrentController.calculate(getPosition().in(Radians), output)); - Voltage voltage = voltsForAmps(current, getVelocity()); - Voltage feedforwardVoltage = feedforward.calculate(getVelocity(), velocityForVolts(voltage)); - driveAtVoltage(feedforwardVoltage.plus(voltage)); - } - case VELOCITY -> { - Current current = Amps.of( - veloCurrentController.calculate(getPosition().in(Radians), output)); - Voltage feedforwardVoltage = feedforward.calculate(getVelocity(), RadiansPerSecond.of(output)); - Voltage voltage = voltsForAmps(current, getVelocity()).plus(feedforwardVoltage); - driveAtVoltage(voltage); - } - } - } + public enum OutputType { + VOLTAGE, + CURRENT + } + + public enum OutputMode { + VELOCITY, + POSITION, + OPEN_LOOP + } + + /** The Constants for the motor */ + private final DCMotor motor; + + /** The dynamics simulation for the motor */ + private final DCMotorSim sim; + + /** The gear ratio, value above 1.0 are a reduction */ + private final double gearing; + + /** The voltage required to overcome friction */ + private final Voltage frictionVoltage; + + private final PIDController poseVoltController = new PIDController(0, 0, 0); + private final PIDController veloVoltController = new PIDController(0, 0, 0); + private final PIDController poseCurrentController = new PIDController(0, 0, 0); + private final PIDController veloCurrentController = new PIDController(0, 0, 0); + + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0, 0, 0); + + private Current currentLimit = Amps.of(300.0); + + private OutputType outputType = OutputType.VOLTAGE; + private OutputMode outputMode = OutputMode.OPEN_LOOP; + private double output = 0.0; + + private Angle forwardLimit = Radians.of(Double.POSITIVE_INFINITY); + private Angle reverseLimit = Radians.of(Double.NEGATIVE_INFINITY); + + /** + * + * + *

Constructs a Brushless Motor Simulation Instance.

+ * + * @param motor the {@link DCMotor} model representing the motor(s) in the simulation + * @param gearRatio the gear ratio of the mechanism; values greater than 1 indicate a reduction + * @param loadIntertia the rotational inertia of the mechanism + * @param frictionVoltage the voltage required to keep the motor moving at a constant velocity + */ + public BrushlessMotorSim( + SimulatedField arena, + DCMotor motor, + double gearRatio, + MomentOfInertia loadIntertia, + Voltage frictionVoltage) { + this.sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + motor, loadIntertia.in(KilogramSquareMeters), gearRatio), + motor); + this.motor = motor; + this.gearing = gearRatio; + this.frictionVoltage = frictionVoltage; + + arena.addMotor(this); + } + + /** + * Configures the angle of the motor. + * + * @param angle the angle of the motor + * @return this instance for method chaining + */ + public BrushlessMotorSim withOverrideAngle(Angle angle) { + sim.setAngle(angle.in(Radians)); + return this; + } + + /** + * Configures the angular velocity of the motor. + * + * @param angularVelocity the angular velocity of the motor + * @return this instance for method chaining + */ + public BrushlessMotorSim withOverrideAngularVelocity(AngularVelocity angularVelocity) { + sim.setAngularVelocity(angularVelocity.in(RadiansPerSecond)); + return this; + } + + public BrushlessMotorSim withFeedForward( + Voltage kS, + Per kV, + Per kA) { + var kVUnit = PerUnit.combine(Volts, RadiansPerSecond); + var kAUnit = PerUnit.combine(Volts, RadiansPerSecondPerSecond); + feedforward = + new SimpleMotorFeedforward( + kS.in(Volts), kV.in(kVUnit), kA.in(kAUnit), SimulatedField.getSimulationDt()); + return this; + } + + /** + * Configures the PD controller for Positional Requests using {@link OutputType#VOLTAGE}. + * + *

This is unit safe and can be configure like so: + * + * 


+   * // Volts per Rotation of error is how CTRE handles PID when used with voltage requests
+   * sim.withPositionalVoltageController(
+   *   Volts.per(Rotation).ofNative(100.0),
+   *   Volts.per(RotationsPerSecond).ofNative(5.0)
+   * );
+   *
+ * + * @param kP the proportional gain + * @param kD the derivative gain + * @return this instance for method chaining + */ + public BrushlessMotorSim withPositionalVoltageController( + Per kP, Per kD) { + var kPUnit = PerUnit.combine(Volts, Radians); + var kDUnit = PerUnit.combine(Volts, RadiansPerSecond); + poseVoltController.setP(kP.in(kPUnit)); + poseVoltController.setD(kD.in(kDUnit)); + return this; + } + + /** + * Configures the PD controller for Velocity Requests using {@link OutputType#VOLTAGE}. + * + *

This is unit safe and can be configure like so: + * + * 


+   * // Volts per RPS of error is how CTRE handles PID when used with voltage requests
+   * sim.withVelocityVoltageController(
+   *   Volts.per(RotationsPerSecond).ofNative(0.4)
+   * );
+   *
+ * + * @param kP the proportional gain + * @return this instance for method chaining + */ + public BrushlessMotorSim withVelocityVoltageController(Per kP) { + var kPUnit = PerUnit.combine(Volts, Radians); + veloVoltController.setP(kP.in(kPUnit)); + return this; + } + + /** + * Configures the PD controller for Positional Requests using {@link OutputType#CURRENT}. + * + *

This is unit safe and can be configure like so: + * + * 


+   * // Amps per Rotation of error is how CTRE handles PID when used with current requests
+   * sim.withPositionalCurrentController(
+   *   Amps.per(Rotation).ofNative(100.0),
+   *   Amps.per(RotationsPerSecond).ofNative(5.0)
+   * );
+   *
+ * + * @param kP the proportional gain + * @param kD the derivative gain + * @return this instance for method chaining + */ + public BrushlessMotorSim withPositionalCurrentController( + Per kP, Per kD) { + var kPUnit = PerUnit.combine(Amps, Radians); + var kDUnit = PerUnit.combine(Amps, RadiansPerSecond); + poseCurrentController.setP(kP.in(kPUnit)); + poseCurrentController.setD(kD.in(kDUnit)); + return this; + } + + /** + * Configures the PD controller for Velocity Requests using {@link OutputType#CURRENT}. + * + *

This is unit safe and can be configure like so: + * + * 


+   * // Amps per RPS of error is how CTRE handles PID when used with current requests
+   * sim.withVelocityCurrentController(
+   *   Amps.per(RotationsPerSecond).ofNative(0.4)
+   * );
+   *
+ * + * @param kP the proportional gain + * @return this instance for method chaining + */ + public BrushlessMotorSim withVelocityCurrentController(Per kP) { + var kPUnit = PerUnit.combine(Amps, Radians); + veloCurrentController.setP(kP.in(kPUnit)); + return this; + } + + /** + * Configures the positionaly controllers to use continuous wrap. + * + * @param min the minimum angle + * @param max the maximum angle + * @return this instance for method chaining + * @see PIDController#enableContinuousInput(double, double) + */ + public BrushlessMotorSim withControllerContinousInput(Angle min, Angle max) { + poseVoltController.enableContinuousInput(min.in(Radians), max.in(Radians)); + poseCurrentController.enableContinuousInput(min.in(Radians), max.in(Radians)); + return this; + } + + /** + * Configures the current limit for the motor. + * + *

This is the total current limit for the sim + * + * @param currentLimit the current limit for the motor + * @return + */ + public BrushlessMotorSim withStatorCurrentLimit(Current currentLimit) { + // this is a limit across the sum of all motors output, + // so it should be set to the total current limit of the mechanism + this.currentLimit = currentLimit; + return this; + } + + /** + * Configures the hard limits for the motor. + * + * @param forwardLimit the forward limit + * @param reverseLimit the reverse limit + * @return this instance for method chaining + */ + public BrushlessMotorSim withHardLimits(Angle forwardLimit, Angle reverseLimit) { + this.forwardLimit = forwardLimit; + this.reverseLimit = reverseLimit; + return this; + } + + public MomentOfInertia getMOI() { + return KilogramSquareMeters.of(sim.getJKgMetersSquared()); + } + + public Angle getPosition() { + return Radians.of(sim.getAngularPositionRad()); + } + + public AngularVelocity getVelocity() { + return RadiansPerSecond.of(sim.getAngularVelocityRadPerSec()); + } + + public AngularAcceleration getAcceleration() { + return RadiansPerSecondPerSecond.of(sim.getAngularAccelerationRadPerSecSq()); + } + + public Current getStatorCurrentDraw() { + return Amps.of(sim.getCurrentDrawAmps()); + } + + public Current getSupplyCurrent() { + // https://www.chiefdelphi.com/t/current-limiting-talonfx-values/374780/10 + return getStatorCurrentDraw() + .times(sim.getInputVoltage() / RobotController.getBatteryVoltage()); + } + + public Voltage getRotorVoltage() { + return Volts.of(sim.getInputVoltage()); + } + + public Voltage getSupplyVoltage() { + return Volts.of(RobotController.getBatteryVoltage()); + } + + public void setControl(OutputType outputType, AngularVelocity velo) { + this.outputType = outputType; + this.outputMode = OutputMode.VELOCITY; + this.output = velo.in(RadiansPerSecond); + } + + public void setControl(OutputType outputType, Angle pos) { + this.outputType = outputType; + this.outputMode = OutputMode.POSITION; + this.output = pos.in(Radians); + } + + public void setControl(Current amps) { + this.outputType = OutputType.CURRENT; + this.outputMode = OutputMode.OPEN_LOOP; + this.output = amps.in(Amps); + } + + public void setControl(Voltage volts) { + this.outputType = OutputType.VOLTAGE; + this.outputMode = OutputMode.OPEN_LOOP; + this.output = volts.in(Volts); + } + + public void setControl() { + this.outputType = OutputType.VOLTAGE; + this.outputMode = OutputMode.OPEN_LOOP; + this.output = 0.0; + } + + public void update() { + double dtSeconds = SimulatedField.getSimulationDt(); + switch (this.outputType) { + case VOLTAGE -> { + switch (this.outputMode) { + case OPEN_LOOP -> { + driveAtVoltage(Volts.of(output)); + } + case POSITION -> { + Voltage voltage = + Volts.of(poseVoltController.calculate(getPosition().in(Radians), output)); + Voltage feedforwardVoltage = + feedforward.calculate(getVelocity(), velocityForVolts(voltage)); + driveAtVoltage(feedforwardVoltage.plus(voltage)); + } + case VELOCITY -> { + Voltage voltage = + Volts.of(veloVoltController.calculate(getVelocity().in(RadiansPerSecond), output)); + Voltage feedforwardVoltage = + feedforward.calculate(getVelocity(), RadiansPerSecond.of(output)); + driveAtVoltage(voltage.plus(feedforwardVoltage)); + } } - - sim.update(dtSeconds); - - if (getPosition().lte(reverseLimit)) { - sim.setState(reverseLimit.in(Radians), 0.0); - } else if (getPosition().gte(forwardLimit)) { - sim.setState(forwardLimit.in(Radians), 0.0); + } + case CURRENT -> { + switch (this.outputMode) { + case OPEN_LOOP -> { + sim.setInputVoltage(voltsForAmps(Amps.of(output), getVelocity()).in(Volts)); + } + case POSITION -> { + Current current = + Amps.of(poseCurrentController.calculate(getPosition().in(Radians), output)); + Voltage voltage = voltsForAmps(current, getVelocity()); + Voltage feedforwardVoltage = + feedforward.calculate(getVelocity(), velocityForVolts(voltage)); + driveAtVoltage(feedforwardVoltage.plus(voltage)); + } + case VELOCITY -> { + Current current = + Amps.of(veloCurrentController.calculate(getPosition().in(Radians), output)); + Voltage feedforwardVoltage = + feedforward.calculate(getVelocity(), RadiansPerSecond.of(output)); + Voltage voltage = voltsForAmps(current, getVelocity()).plus(feedforwardVoltage); + driveAtVoltage(voltage); + } } - } - - private void driveAtVoltage(Voltage voltage) { - // The voltage constrained to current limits and battery voltage - Voltage constrained = constrainOutputVoltage(voltage); - Voltage frictionVoltage = applyFriction(constrained); - - sim.setInputVoltage(frictionVoltage.in(Volts)); - } - - private Voltage applyFriction(Voltage voltage) { - // This function is responsible for slowing down acceleration - // and slowing down the velocity of the motor when at lowere output. - // This is not the same as the static friction, which is the force - // required to get the motor moving. - - // to apply friction we convert the motors output to torque then back to voltage - double current = motor.getCurrent(sim.getAngularVelocityRadPerSec() * gearing, voltage.in(Volts)); - double currentVelo = getVelocity().in(RadiansPerSecond) * gearing; - double torque = motor.getTorque(current); - double friction = frictionTorque().in(NewtonMeters); - - boolean movingForward = currentVelo > 0; - - if (movingForward && currentVelo > motor.getSpeed(torque, sim.getInputVoltage())) { - // the motor is moving faster than it should based on the output voltage - // apply the friction to slow it down - torque -= friction; - } else if (!movingForward && currentVelo < motor.getSpeed(torque, sim.getInputVoltage())) { - // the motor is moving slower than it should based on the output voltage - // apply the friction to speed it up - torque += friction; - } - - return Volts.of(motor.getVoltage(torque, currentVelo)); - } - - private Voltage voltsForAmps(Current current, AngularVelocity angularVelocity) { - // find what voltage is needed to get the current - return Volts.of(motor.getVoltage(current.in(Amps), angularVelocity.in(RadiansPerSecond) * gearing)); - } - - private AngularVelocity velocityForVolts(Voltage voltage) { - return RadiansPerSecond.of( - motor.getSpeed(motor.getTorque(getStatorCurrentDraw().in(Amps)), voltage.in(Volts))); - } - - private Torque frictionTorque() { - return NewtonMeters.of(motor.getTorque(motor.getCurrent(0.0, frictionVoltage.in(Volts))) * gearing); - } - - private Voltage constrainOutputVoltage(Voltage requestedOutput) { - final double kCurrentThreshold = 1.2; - - final double motorCurrentVelocityRadPerSec = getVelocity().in(RadiansPerSecond); - final double currentLimitAmps = currentLimit.in(Amps); - final double requestedOutputVoltage = requestedOutput.in(Volts); - final double currentAtRequestedVolts = motor.getCurrent(motorCurrentVelocityRadPerSec, requestedOutputVoltage); - - // Resource for current limiting: - // https://file.tavsys.net/control/controls-engineering-in-frc.pdf (sec 12.1.3) - final boolean currentTooHigh = Math.abs(currentAtRequestedVolts) > (kCurrentThreshold * currentLimitAmps); - double limitedVoltage = requestedOutputVoltage; - if (currentTooHigh) { - final double limitedCurrent = Math.copySign(currentLimitAmps, currentAtRequestedVolts); - limitedVoltage = motor.getVoltage(motor.getTorque(limitedCurrent), motorCurrentVelocityRadPerSec); - } - - // ensure the current limit doesn't cause an increase to output voltage - if (Math.abs(limitedVoltage) > Math.abs(requestedOutputVoltage)) { - limitedVoltage = requestedOutputVoltage; - } - - // constrain the output voltage to the battery voltage - return Volts.of(MathUtil.clamp( - limitedVoltage, -RobotController.getBatteryVoltage(), RobotController.getBatteryVoltage())); - } -} \ No newline at end of file + } + } + + sim.update(dtSeconds); + + if (getPosition().lte(reverseLimit)) { + sim.setState(reverseLimit.in(Radians), 0.0); + } else if (getPosition().gte(forwardLimit)) { + sim.setState(forwardLimit.in(Radians), 0.0); + } + } + + private void driveAtVoltage(Voltage voltage) { + // The voltage constrained to current limits and battery voltage + Voltage constrained = constrainOutputVoltage(voltage); + Voltage frictionVoltage = applyFriction(constrained); + + sim.setInputVoltage(frictionVoltage.in(Volts)); + } + + private Voltage applyFriction(Voltage voltage) { + // This function is responsible for slowing down acceleration + // and slowing down the velocity of the motor when at lowere output. + // This is not the same as the static friction, which is the force + // required to get the motor moving. + + // to apply friction we convert the motors output to torque then back to voltage + double current = + motor.getCurrent(sim.getAngularVelocityRadPerSec() * gearing, voltage.in(Volts)); + double currentVelo = getVelocity().in(RadiansPerSecond) * gearing; + double torque = motor.getTorque(current); + double friction = frictionTorque().in(NewtonMeters); + + boolean movingForward = currentVelo > 0; + + if (movingForward && currentVelo > motor.getSpeed(torque, sim.getInputVoltage())) { + // the motor is moving faster than it should based on the output voltage + // apply the friction to slow it down + torque -= friction; + } else if (!movingForward && currentVelo < motor.getSpeed(torque, sim.getInputVoltage())) { + // the motor is moving slower than it should based on the output voltage + // apply the friction to speed it up + torque += friction; + } + + return Volts.of(motor.getVoltage(torque, currentVelo)); + } + + private Voltage voltsForAmps(Current current, AngularVelocity angularVelocity) { + // find what voltage is needed to get the current + return Volts.of( + motor.getVoltage(current.in(Amps), angularVelocity.in(RadiansPerSecond) * gearing)); + } + + private AngularVelocity velocityForVolts(Voltage voltage) { + return RadiansPerSecond.of( + motor.getSpeed(motor.getTorque(getStatorCurrentDraw().in(Amps)), voltage.in(Volts))); + } + + private Torque frictionTorque() { + return NewtonMeters.of( + motor.getTorque(motor.getCurrent(0.0, frictionVoltage.in(Volts))) * gearing); + } + + private Voltage constrainOutputVoltage(Voltage requestedOutput) { + final double kCurrentThreshold = 1.2; + + final double motorCurrentVelocityRadPerSec = getVelocity().in(RadiansPerSecond); + final double currentLimitAmps = currentLimit.in(Amps); + final double requestedOutputVoltage = requestedOutput.in(Volts); + final double currentAtRequestedVolts = + motor.getCurrent(motorCurrentVelocityRadPerSec, requestedOutputVoltage); + + // Resource for current limiting: + // https://file.tavsys.net/control/controls-engineering-in-frc.pdf (sec 12.1.3) + final boolean currentTooHigh = + Math.abs(currentAtRequestedVolts) > (kCurrentThreshold * currentLimitAmps); + double limitedVoltage = requestedOutputVoltage; + if (currentTooHigh) { + final double limitedCurrent = Math.copySign(currentLimitAmps, currentAtRequestedVolts); + limitedVoltage = + motor.getVoltage(motor.getTorque(limitedCurrent), motorCurrentVelocityRadPerSec); + } + + // ensure the current limit doesn't cause an increase to output voltage + if (Math.abs(limitedVoltage) > Math.abs(requestedOutputVoltage)) { + limitedVoltage = requestedOutputVoltage; + } + + // constrain the output voltage to the battery voltage + return Volts.of( + MathUtil.clamp( + limitedVoltage, + -RobotController.getBatteryVoltage(), + RobotController.getBatteryVoltage())); + } +} diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java index 10b79220..9f6af990 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java @@ -13,11 +13,15 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Voltage; + import java.util.Queue; import java.util.concurrent.ConcurrentLinkedQueue; import java.util.function.Supplier; import org.dyn4j.geometry.Vector2; import frc.robot.extras.simulation.mechanismSim.swerve.BrushlessMotorSim; +import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; +import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; import frc.robot.extras.simulation.field.SimulatedField; /** @@ -46,12 +50,12 @@ * *

    *
  • Retrieve the encoder readings from {@link #getDriveEncoderUnGearedPositionRad()}} and - * {@link #getSteerAbsoluteFacing()}. + * {@link #getTurnAbsolutePosition()}. *
  • Use {@link SwerveDriveOdometry} to estimate the pose of your robot. *
  • 250Hz * Odometry is supported. You can retrive cached encoder readings from every sub-tick through - * {@link #getCachedDriveEncoderUnGearedPositionsRad()} and {@link #getCachedSteerAbsolutePositions()}. + * {@link #getCachedDriveEncoderUnGearedPositionsRad()} and {@link #getCachedTurnAbsolutePositions()}. *
* *

An example of how to simulate odometry using this class is the cachedSteerRelativeEncoderPositionsRad, cachedDriveEncoderUnGearedPositionsRad; - private final Queue cachedSteerAbsolutePositions; + private final Queue cachedTurnRelativeEncoderPositionsRad, cachedDriveEncoderUnGearedPositionsRad; + private final Queue cachedTurnAbsolutePositions; /** * @@ -92,56 +96,56 @@ public class SwerveModuleSimulation { * call the method before constructing any swerve module simulations using this constructor. * * @param driveMotor the model of the driving motor - * @param steerMotor the model of the steering motor + * @param turnMotor the model of the steering motor * @param driveCurrentLimit the current limit for the driving motor, in amperes * @param driveGearRatio the gear ratio for the driving motor, >1 is reduction - * @param steerGearRatio the gear ratio for the steering motor, >1 is reduction + * @param turnGearRatio the gear ratio for the steering motor, >1 is reduction * @param driveFrictionVoltage the measured minimum amount of voltage that can turn the driving rotter, in volts - * @param steerFrictionVoltage the measured minimum amount of voltage that can turn the steering rotter, in volts + * @param turnFrictionVoltage the measured minimum amount of voltage that can turn the steering rotter, in volts * @param tireCoefficientOfFriction the coefficient * of friction of the tires, normally around 1.5 * @param wheelsRadiusMeters the radius of the wheels, in meters. Calculate it using * {@link Units#inchesToMeters(double)}. - * @param steerRotationalInertia the rotational inertia of the steering mechanism + * @param turnRotationalInertia the rotational inertia of the steering mechanism */ public SwerveModuleSimulation( DCMotor driveMotor, - DCMotor steerMotor, + DCMotor turnMotor, double driveCurrentLimit, double driveGearRatio, - double steerGearRatio, + double turnGearRatio, double driveFrictionVoltage, - double steerFrictionVoltage, + double turnFrictionVoltage, double tireCoefficientOfFriction, double wheelsRadiusMeters, - double steerRotationalInertia) { + double turnRotationalInertia) { DRIVE_MOTOR = driveMotor; DRIVE_CURRENT_LIMIT = driveCurrentLimit; DRIVE_GEAR_RATIO = driveGearRatio; - STEER_GEAR_RATIO = steerGearRatio; + TURN_GEAR_RATIO = turnGearRatio; DRIVE_FRICTION_VOLTAGE = driveFrictionVoltage; WHEELS_COEFFICIENT_OF_FRICTION = tireCoefficientOfFriction; WHEEL_RADIUS_METERS = wheelsRadiusMeters; - this.steerMotorSim = new BrushlessMotorSim( + this.turnMotorSim = new BrushlessMotorSim( SimulatedField.getInstance(), - steerMotor, - steerGearRatio, - KilogramSquareMeters.of(steerRotationalInertia), - Volts.of(steerFrictionVoltage)); + turnMotor, + turnGearRatio, + KilogramSquareMeters.of(turnRotationalInertia), + Volts.of(turnFrictionVoltage)); this.cachedDriveEncoderUnGearedPositionsRad = new ConcurrentLinkedQueue<>(); for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); - this.cachedSteerRelativeEncoderPositionsRad = new ConcurrentLinkedQueue<>(); + this.cachedTurnRelativeEncoderPositionsRad = new ConcurrentLinkedQueue<>(); for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) - cachedSteerRelativeEncoderPositionsRad.offer(steerRelativeEncoderPositionRad); - this.cachedSteerAbsolutePositions = new ConcurrentLinkedQueue<>(); + cachedTurnRelativeEncoderPositionsRad.offer(turnRelativeEncoderPositionRad); + this.cachedTurnAbsolutePositions = new ConcurrentLinkedQueue<>(); for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) - cachedSteerAbsolutePositions.offer(steerAbsoluteFacing); + cachedTurnAbsolutePositions.offer(turnAbsolutePosition); - this.steerRelativeEncoderPositionRad = steerAbsoluteFacing.getRadians() + steerRelativeEncoderOffSet; + this.turnRelativeEncoderPositionRad = turnAbsolutePosition.getRadians() + turnRelativeEncoderOffSet; } /** @@ -161,8 +165,8 @@ public SwerveModuleSimulation( * * @param volts the voltage to be applied to the driving motor */ - public void requestDriveVoltageOut(double volts) { - this.driveMotorRequestedVolts = volts; + public void requestDriveVoltageOut(Voltage volts) { + this.driveMotorRequestedVolts = volts.in(Volts); } /** @@ -179,12 +183,12 @@ public void requestDriveVoltageOut(double volts) { * the current exceeds the limit. The current limit will reduce the motor's output as needed, mimicking real motor * behavior. * - *

To check the actual voltage applied to the steering motor, use {@link #getSteerMotorAppliedVolts()}. + *

To check the actual voltage applied to the steering motor, use {@link #getTurnMotorAppliedVolts()}. * * @param volts the voltage to be applied to the steering motor */ - public void requestSteerVoltageOut(double volts) { - this.steerMotorSim.setControl(Volts.of(volts)); + public void requestTurnVoltageOut(Voltage volts) { + this.turnMotorSim.setControl(volts); } /** @@ -212,13 +216,13 @@ public double getDriveMotorAppliedVolts() { *

This method returns the actual voltage being applied to the steering motor. It wraps around the * {@link BrushlessMotorSim#getAppliedVolts()} method. * - *

The actual applied voltage may differ from the value set by {@link #requestSteerVoltageOut(double)}. If the + *

The actual applied voltage may differ from the value set by {@link #requestTurnVoltageOut(double)}. If the * motor's supply current is too high, the motor will automatically reduce its output voltage to protect the system. * * @return the actual output voltage of the steering motor, in volts */ - public double getSteerMotorAppliedVolts() { - return steerMotorSim.getRotorVoltage().in(Volts); + public double getTurnMotorAppliedVolts() { + return turnMotorSim.getRotorVoltage().in(Volts); } /** @@ -245,8 +249,8 @@ public double getDriveMotorSupplyCurrentAmps() { * * @return the current supplied to the steer motor, in amperes */ - public double getSteerMotorSupplyCurrentAmps() { - return steerMotorSim.getSupplyCurrent().in(Amps); + public double getTurnMotorSupplyCurrentAmps() { + return turnMotorSim.getSupplyCurrent().in(Amps); } /** @@ -261,7 +265,7 @@ public double getSteerMotorSupplyCurrentAmps() { *

This value represents the un-geared position of the encoder, i.e., the amount of radians the drive motor's * encoder has rotated. * - *

To get the final wheel rotations, use {@link #getDriveWheelFinalPositionRad()}. + *

To get the final wheel rotations, use {@link #getDriveEncoderFinalPositionRad()}. * * @return the position of the drive motor's encoder, in radians (un-geared) */ @@ -282,8 +286,9 @@ public double getDriveEncoderUnGearedPositionRad() { * * @return the final position of the drive encoder (wheel rotations), in radians */ - public double getDriveWheelFinalPositionRad() { - return getDriveEncoderUnGearedPositionRad() / DRIVE_GEAR_RATIO; + public double getDriveEncoderFinalPositionRad() { + return getDriveEncoderUnGearedPositionRad(); + // / DRIVE_GEAR_RATIO; } /** @@ -310,7 +315,8 @@ public double getDriveEncoderUnGearedSpeedRadPerSec() { * @return the final speed of the drive wheel, in radians per second */ public double getDriveWheelFinalSpeedRadPerSec() { - return getDriveEncoderUnGearedSpeedRadPerSec() / DRIVE_GEAR_RATIO; + return getDriveEncoderUnGearedSpeedRadPerSec(); + // / DRIVE_GEAR_RATIO; } /** @@ -322,8 +328,8 @@ public double getDriveWheelFinalSpeedRadPerSec() { * * @return the relative encoder position of the steer motor, in radians */ - public double getSteerRelativeEncoderPositionRad() { - return steerRelativeEncoderPositionRad; + public double getTurnRelativeEncoderPositionRad() { + return turnRelativeEncoderPositionRad; } /** @@ -335,8 +341,8 @@ public double getSteerRelativeEncoderPositionRad() { * * @return the speed of the steer relative encoder, in radians per second (geared) */ - public double getSteerRelativeEncoderSpeedRadPerSec() { - return steerRelativeEncoderSpeedRadPerSec; + public double getTurnRelativeEncoderSpeedRadPerSec() { + return turnRelativeEncoderSpeedRadPerSec; } /** @@ -348,8 +354,8 @@ public double getSteerRelativeEncoderSpeedRadPerSec() { * * @return the absolute facing of the steer mechanism, as a {@link Rotation2d} */ - public Rotation2d getSteerAbsoluteFacing() { - return steerAbsoluteFacing; + public Rotation2d getTurnAbsolutePosition() { + return turnAbsolutePosition; } /** @@ -361,8 +367,8 @@ public Rotation2d getSteerAbsoluteFacing() { * * @return the absolute angular velocity of the steer mechanism, in radians per second */ - public double getSteerAbsoluteEncoderSpeedRadPerSec() { - return steerAbsoluteEncoderSpeedRadPerSec; + public double getTurnAbsoluteEncoderSpeedRadPerSec() { + return turnAbsoluteEncoderSpeedRadPerSec; } /** @@ -393,7 +399,7 @@ public double[] getCachedDriveEncoderUnGearedPositionsRad() { */ public double[] getCachedDriveWheelFinalPositionsRad() { return cachedDriveEncoderUnGearedPositionsRad.stream() - .mapToDouble(value -> value / DRIVE_GEAR_RATIO) + .mapToDouble(value -> value * ModuleConstants.DRIVE_TO_METERS) .toArray(); } @@ -402,13 +408,13 @@ public double[] getCachedDriveWheelFinalPositionsRad() { * *

Obtains the Cached Readings of the Steer Relative Encoder's Position.

* - *

The values of {@link #getSteerRelativeEncoderPositionRad()} are cached at each sub-tick and can be retrieved + *

The values of {@link #getTurnRelativeEncoderPositionRad()} are cached at each sub-tick and can be retrieved * using this method. * * @return an array of cached steer relative encoder positions, in radians */ - public double[] getCachedSteerRelativeEncoderPositions() { - return cachedSteerRelativeEncoderPositionsRad.stream() + public double[] getCachedTurnRelativeEncoderPositions() { + return cachedTurnRelativeEncoderPositionsRad.stream() .mapToDouble(value -> value) .toArray(); } @@ -418,13 +424,13 @@ public double[] getCachedSteerRelativeEncoderPositions() { * *

Obtains the Cached Readings of the Steer Absolute Positions.

* - *

The values of {@link #getSteerAbsoluteFacing()} are cached at each sub-tick and can be retrieved using this + *

The values of {@link #getTurnAbsolutePosition()} are cached at each sub-tick and can be retrieved using this * method. * * @return an array of cached absolute steer positions, as {@link Rotation2d} objects */ - public Rotation2d[] getCachedSteerAbsolutePositions() { - return cachedSteerAbsolutePositions.toArray(Rotation2d[]::new); + public Rotation2d[] getCachedTurnAbsolutePositions() { + return cachedTurnAbsolutePositions.toArray(Rotation2d[]::new); } protected double getGrippingForceNewtons(double gravityForceOnModuleNewtons) { @@ -455,11 +461,11 @@ public Vector2 updateSimulationSubTickGetModuleForce( Vector2 moduleCurrentGroundVelocityWorldRelative, Rotation2d robotFacing, double gravityForceOnModuleNewtons) { - updateSteerSimulation(); + updateTurnSimulation(); /* the maximum gripping force that the wheel can generate */ final double grippingForceNewtons = getGrippingForceNewtons(gravityForceOnModuleNewtons); - final Rotation2d moduleWorldFacing = this.steerAbsoluteFacing.plus(robotFacing); + final Rotation2d moduleWorldFacing = this.turnAbsolutePosition.plus(robotFacing); final Vector2 propellingForce = getPropellingForce(grippingForceNewtons, moduleWorldFacing, moduleCurrentGroundVelocityWorldRelative); updateDriveEncoders(); @@ -476,18 +482,18 @@ public Vector2 updateSimulationSubTickGetModuleForce( * *

The steer mechanism is modeled by a {@link BrushlessMotorSim}. */ - private void updateSteerSimulation() { + private void updateTurnSimulation() { /* update the readings of the sensor */ - this.steerAbsoluteFacing = new Rotation2d(steerMotorSim.getPosition()); - this.steerRelativeEncoderPositionRad = steerMotorSim.getPosition().in(Radians) + steerRelativeEncoderOffSet; - this.steerAbsoluteEncoderSpeedRadPerSec = steerMotorSim.getVelocity().in(RadiansPerSecond); - this.steerRelativeEncoderSpeedRadPerSec = steerAbsoluteEncoderSpeedRadPerSec * STEER_GEAR_RATIO; + this.turnAbsolutePosition = new Rotation2d(turnMotorSim.getPosition()); + this.turnRelativeEncoderPositionRad = turnMotorSim.getPosition().in(Radians) + turnRelativeEncoderOffSet; + this.turnAbsoluteEncoderSpeedRadPerSec = turnMotorSim.getVelocity().in(RadiansPerSecond); + this.turnRelativeEncoderSpeedRadPerSec = turnAbsoluteEncoderSpeedRadPerSec * TURN_GEAR_RATIO; /* cache sensor readings to queue for high-frequency odometry */ - this.cachedSteerAbsolutePositions.poll(); - this.cachedSteerAbsolutePositions.offer(steerAbsoluteFacing); - this.cachedSteerRelativeEncoderPositionsRad.poll(); - this.cachedSteerRelativeEncoderPositionsRad.offer(steerRelativeEncoderPositionRad); + this.cachedTurnAbsolutePositions.poll(); + this.cachedTurnAbsolutePositions.offer(turnAbsolutePosition); + this.cachedTurnRelativeEncoderPositionsRad.poll(); + this.cachedTurnRelativeEncoderPositionsRad.offer(turnRelativeEncoderPositionRad); } /** @@ -558,7 +564,7 @@ private double getDriveWheelTorque() { /** @return the current module state of this simulation module */ public SwerveModuleState getCurrentState() { - return new SwerveModuleState(getDriveWheelFinalSpeedRadPerSec() * WHEEL_RADIUS_METERS, steerAbsoluteFacing); + return new SwerveModuleState(getDriveWheelFinalSpeedRadPerSec() * WHEEL_RADIUS_METERS, turnAbsolutePosition); } /** @@ -578,7 +584,7 @@ protected SwerveModuleState getFreeSpinState() { driveMotorAppliedVolts) / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS, - steerAbsoluteFacing); + turnAbsolutePosition); } /** @@ -656,907 +662,27 @@ public enum WHEEL_GRIP { this.cof = cof; } } - package org.ironmaple.simulation.drivesims; - - import static edu.wpi.first.units.Units.Amps; - import static edu.wpi.first.units.Units.KilogramSquareMeters; - import static edu.wpi.first.units.Units.Radians; - import static edu.wpi.first.units.Units.RadiansPerSecond; - import static edu.wpi.first.units.Units.Volts; - - import edu.wpi.first.math.MathUtil; - import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; - import edu.wpi.first.math.geometry.Rotation2d; - import edu.wpi.first.math.kinematics.SwerveDriveOdometry; - import edu.wpi.first.math.kinematics.SwerveModuleState; - import edu.wpi.first.math.system.plant.DCMotor; - import edu.wpi.first.math.util.Units; - import java.util.Queue; - import java.util.concurrent.ConcurrentLinkedQueue; - import java.util.function.Supplier; - import org.dyn4j.geometry.Vector2; - import org.ironmaple.simulation.BrushlessMotorSim; - import org.ironmaple.simulation.SimulatedField; - /** - * - * - *

Simulation for a Single Swerve Module.

- * - *

This class provides a simulation for a single swerve module in the {@link SwerveDriveSimulation}. - * - *

1. Purpose

- * - *

This class serves as the bridge between your code and the physics engine. - * - *

You will apply voltage outputs to the drive/steer motor of the module and obtain their encoder readings in your - * code, just as how you deal with your physical motors. - * - *

2. Perspectives

- * - *
    - *
  • Simulates the steering mechanism using a custom brushless motor simulator. - *
  • Simulates the propelling force generated by the driving motor, with a current limit. - *
  • Simulates encoder readings, which can be used to simulate a {@link SwerveDriveOdometry}. - *
- * - *

3. Simulating Odometry

- * - *
    - *
  • Retrieve the encoder readings from {@link #getDriveEncoderUnGearedPositionRad()}} and - * {@link #getSteerAbsoluteFacing()}. - *
  • Use {@link SwerveDriveOdometry} to estimate the pose of your robot. - *
  • 250Hz - * Odometry is supported. You can retrive cached encoder readings from every sub-tick through - * {@link #getCachedDriveEncoderUnGearedPositionsRad()} and {@link #getCachedSteerAbsolutePositions()}. - *
- * - *

An example of how to simulate odometry using this class is the ModuleIOSim.java - * from the Advanced Swerve Drive with maple-sim example. + * creates a SDS + * Mark4-n Swerve Module for simulation */ - public class SwerveModuleSimulation { - public final DCMotor DRIVE_MOTOR; - private final BrushlessMotorSim steerMotorSim; - public final double DRIVE_CURRENT_LIMIT, - DRIVE_GEAR_RATIO, - STEER_GEAR_RATIO, - DRIVE_FRICTION_VOLTAGE, - WHEELS_COEFFICIENT_OF_FRICTION, - WHEEL_RADIUS_METERS, - DRIVE_WHEEL_INERTIA = 0.01; - private double driveMotorRequestedVolts = 0.0, - driveMotorAppliedVolts = 0.0, - driveMotorSupplyCurrentAmps = 0.0, - steerRelativeEncoderPositionRad = 0.0, - steerRelativeEncoderSpeedRadPerSec = 0.0, - steerAbsoluteEncoderSpeedRadPerSec = 0.0, - driveEncoderUnGearedPositionRad = 0.0, - driveEncoderUnGearedSpeedRadPerSec = 0.0; - private Rotation2d steerAbsoluteFacing = Rotation2d.fromRotations(Math.random()); - - private final double steerRelativeEncoderOffSet = (Math.random() - 0.5) * 30; - - private final Queue cachedSteerRelativeEncoderPositionsRad, cachedDriveEncoderUnGearedPositionsRad; - private final Queue cachedSteerAbsolutePositions; - - /** - * - * - *

Constructs a Swerve Module Simulation.

- * - *

If you are using {@link SimulatedField#overrideSimulationTimings(double, int)} to use custom timings, you must - * call the method before constructing any swerve module simulations using this constructor. - * - * @param driveMotor the model of the driving motor - * @param steerMotor the model of the steering motor - * @param driveCurrentLimit the current limit for the driving motor, in amperes - * @param driveGearRatio the gear ratio for the driving motor, >1 is reduction - * @param steerGearRatio the gear ratio for the steering motor, >1 is reduction - * @param driveFrictionVoltage the measured minimum amount of voltage that can turn the driving rotter, in volts - * @param steerFrictionVoltage the measured minimum amount of voltage that can turn the steering rotter, in volts - * @param tireCoefficientOfFriction the coefficient - * of friction of the tires, normally around 1.5 - * @param wheelsRadiusMeters the radius of the wheels, in meters. Calculate it using - * {@link Units#inchesToMeters(double)}. - * @param steerRotationalInertia the rotational inertia of the steering mechanism - */ - public SwerveModuleSimulation( - DCMotor driveMotor, - DCMotor steerMotor, - double driveCurrentLimit, - double driveGearRatio, - double steerGearRatio, - double driveFrictionVoltage, - double steerFrictionVoltage, - double tireCoefficientOfFriction, - double wheelsRadiusMeters, - double steerRotationalInertia) { - DRIVE_MOTOR = driveMotor; - DRIVE_CURRENT_LIMIT = driveCurrentLimit; - DRIVE_GEAR_RATIO = driveGearRatio; - STEER_GEAR_RATIO = steerGearRatio; - DRIVE_FRICTION_VOLTAGE = driveFrictionVoltage; - WHEELS_COEFFICIENT_OF_FRICTION = tireCoefficientOfFriction; - WHEEL_RADIUS_METERS = wheelsRadiusMeters; - - this.steerMotorSim = new BrushlessMotorSim( - SimulatedField.getInstance(), - steerMotor, - steerGearRatio, - KilogramSquareMeters.of(steerRotationalInertia), - Volts.of(steerFrictionVoltage)); - - this.cachedDriveEncoderUnGearedPositionsRad = new ConcurrentLinkedQueue<>(); - for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) - cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); - this.cachedSteerRelativeEncoderPositionsRad = new ConcurrentLinkedQueue<>(); - for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) - cachedSteerRelativeEncoderPositionsRad.offer(steerRelativeEncoderPositionRad); - this.cachedSteerAbsolutePositions = new ConcurrentLinkedQueue<>(); - for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) - cachedSteerAbsolutePositions.offer(steerAbsoluteFacing); - - this.steerRelativeEncoderPositionRad = steerAbsoluteFacing.getRadians() + steerRelativeEncoderOffSet; - } - - /** - * - * - *

Requests the Driving Motor to Run at a Specified Voltage Output.

- * - *

Think of it as the setVoltage() of your physical driving motor.

- * - *

This method sets the desired voltage output for the driving motor. The change will be applied in the next - * sub-tick of the simulation. - * - *

Note: The requested voltage may not always be fully applied if the current is too high. The - * current limit may reduce the motor's output, similar to real motors. - * - *

To check the actual voltage applied to the drivetrain, use {@link #getDriveMotorAppliedVolts()}. - * - * @param volts the voltage to be applied to the driving motor - */ - public void requestDriveVoltageOut(double volts) { - this.driveMotorRequestedVolts = volts; - } - - /** - * - * - *

Requests the Steering Motor to Run at a Specified Voltage Output.

- * - *

Think of it as the setVoltage() of your physical steering motor.

- * - *

This method sets the desired voltage output for the steering motor. The change will be applied in the next - * sub-tick of the simulation. - * - *

Note: Similar to the drive motor, the requested voltage may not always be fully applied if - * the current exceeds the limit. The current limit will reduce the motor's output as needed, mimicking real motor - * behavior. - * - *

To check the actual voltage applied to the steering motor, use {@link #getSteerMotorAppliedVolts()}. - * - * @param volts the voltage to be applied to the steering motor - */ - public void requestSteerVoltageOut(double volts) { - this.steerMotorSim.setControl(Volts.of(volts)); - } - - /** - * - * - *

Obtains the Actual Output Voltage of the Drive Motor.

- * - *

This method returns the actual voltage being applied to the drive motor. The actual applied voltage may differ - * from the value set by {@link #requestDriveVoltageOut(double)}. - * - *

If the motor's supply current is too high, the motor will automatically reduce its output voltage to protect - * the system. - * - * @return the actual output voltage of the drive motor, in volts - */ - public double getDriveMotorAppliedVolts() { - return driveMotorAppliedVolts; - } - - /** - * - * - *

Obtains the Actual Output Voltage of the Steering Motor.

- * - *

This method returns the actual voltage being applied to the steering motor. It wraps around the - * {@link BrushlessMotorSim#getAppliedVolts()} method. - * - *

The actual applied voltage may differ from the value set by {@link #requestSteerVoltageOut(double)}. If the - * motor's supply current is too high, the motor will automatically reduce its output voltage to protect the system. - * - * @return the actual output voltage of the steering motor, in volts - */ - public double getSteerMotorAppliedVolts() { - return steerMotorSim.getRotorVoltage().in(Volts); - } - - /** - * - * - *

Obtains the Amount of Current Supplied to the Drive Motor.

- * - *

Think of it as the getSupplyCurrent() of your physical drive motor.

- * - * @return the current supplied to the drive motor, in amperes - */ - public double getDriveMotorSupplyCurrentAmps() { - return driveMotorSupplyCurrentAmps; - } - - /** - * - * - *

Obtains the Amount of Current Supplied to the Steer Motor.

- * - *

Think of it as the getSupplyCurrent() of your physical steer motor.

- * - *

This method wraps around {@link BrushlessMotorSim#getCurrentDrawAmps()}. - * - * @return the current supplied to the steer motor, in amperes - */ - public double getSteerMotorSupplyCurrentAmps() { - return steerMotorSim.getSupplyCurrent().in(Amps); - } - - /** - * - * - *

Obtains the Position of the Drive Encoder.

- * - *

Think of it as the getPosition() of your physical driving motor.

- * - *

This method is used to simulate your {@link SwerveDrivePoseEstimator}. - * - *

This value represents the un-geared position of the encoder, i.e., the amount of radians the drive motor's - * encoder has rotated. - * - *

To get the final wheel rotations, use {@link #getDriveWheelFinalPositionRad()}. - * - * @return the position of the drive motor's encoder, in radians (un-geared) - */ - public double getDriveEncoderUnGearedPositionRad() { - return driveEncoderUnGearedPositionRad; - } - - /** - * - * - *

Obtains the Final Position of the Drive Encoder (Wheel Rotations).

- * - *

This method is used to simulate the {@link SwerveDrivePoseEstimator} by providing the final position of the - * drive encoder in terms of wheel rotations. - * - *

The value is calculated by dividing the un-geared encoder position (obtained from - * {@link #getDriveEncoderUnGearedPositionRad()}) by the drive gear ratio. - * - * @return the final position of the drive encoder (wheel rotations), in radians - */ - public double getDriveWheelFinalPositionRad() { - return getDriveEncoderUnGearedPositionRad() / DRIVE_GEAR_RATIO; - } - - /** - * - * - *

Obtains the Speed of the Drive Encoder (Un-Geared), in Radians per Second.

- * - *

Think of it as the getVelocity() of your physical drive motor.

- * - *

This method returns the current speed of the drive encoder in radians per second, without accounting for the - * drive gear ratio. - * - * @return the un-geared speed of the drive encoder, in radians per second - */ - public double getDriveEncoderUnGearedSpeedRadPerSec() { - return driveEncoderUnGearedSpeedRadPerSec; - } - - /** - * - * - *

Obtains the Final Speed of the Wheel, in Radians per Second.

- * - * @return the final speed of the drive wheel, in radians per second - */ - public double getDriveWheelFinalSpeedRadPerSec() { - return getDriveEncoderUnGearedSpeedRadPerSec() / DRIVE_GEAR_RATIO; - } - - /** - * - * - *

Obtains the Relative Position of the Steer Encoder, in Radians.

- * - *

Think of it as the getPosition() of your physical steer motor.

- * - * @return the relative encoder position of the steer motor, in radians - */ - public double getSteerRelativeEncoderPositionRad() { - return steerRelativeEncoderPositionRad; - } - - /** - * - * - *

Obtains the Speed of the Steer Relative Encoder, in Radians per Second (Geared).

- * - *

Think of it as the getVelocity() of your physical steer motor.

- * - * @return the speed of the steer relative encoder, in radians per second (geared) - */ - public double getSteerRelativeEncoderSpeedRadPerSec() { - return steerRelativeEncoderSpeedRadPerSec; - } - - /** - * - * - *

Obtains the Absolute Facing of the Steer Mechanism.

- * - *

Think of it as the getAbsolutePosition() of your CanCoder.

- * - * @return the absolute facing of the steer mechanism, as a {@link Rotation2d} - */ - public Rotation2d getSteerAbsoluteFacing() { - return steerAbsoluteFacing; - } - - /** - * - * - *

Obtains the Absolute Rotational Velocity of the Steer Mechanism.

- * - *

Think of it as the getVelocity() of your CanCoder.

- * - * @return the absolute angular velocity of the steer mechanism, in radians per second - */ - public double getSteerAbsoluteEncoderSpeedRadPerSec() { - return steerAbsoluteEncoderSpeedRadPerSec; - } - - /** - * - * - *

Obtains the Cached Readings of the Drive Encoder's Un-Geared Position.

- * - *

The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and can be retrieved - * using this method. - * - * @return an array of cached drive encoder un-geared positions, in radians - */ - public double[] getCachedDriveEncoderUnGearedPositionsRad() { - return cachedDriveEncoderUnGearedPositionsRad.stream() - .mapToDouble(value -> value) - .toArray(); - } - - /** - * - * - *

Obtains the Cached Readings of the Drive Encoder's Final Position (Wheel Rotations).

- * - *

The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and are divided by the - * gear ratio to obtain the final wheel rotations. - * - * @return an array of cached drive encoder final positions (wheel rotations), in radians - */ - public double[] getCachedDriveWheelFinalPositionsRad() { - return cachedDriveEncoderUnGearedPositionsRad.stream() - .mapToDouble(value -> value / DRIVE_GEAR_RATIO) - .toArray(); - } - - /** - * - * - *

Obtains the Cached Readings of the Steer Relative Encoder's Position.

- * - *

The values of {@link #getSteerRelativeEncoderPositionRad()} are cached at each sub-tick and can be retrieved - * using this method. - * - * @return an array of cached steer relative encoder positions, in radians - */ - public double[] getCachedSteerRelativeEncoderPositions() { - return cachedSteerRelativeEncoderPositionsRad.stream() - .mapToDouble(value -> value) - .toArray(); - } - - /** - * - * - *

Obtains the Cached Readings of the Steer Absolute Positions.

- * - *

The values of {@link #getSteerAbsoluteFacing()} are cached at each sub-tick and can be retrieved using this - * method. - * - * @return an array of cached absolute steer positions, as {@link Rotation2d} objects - */ - public Rotation2d[] getCachedSteerAbsolutePositions() { - return cachedSteerAbsolutePositions.toArray(Rotation2d[]::new); - } - - protected double getGrippingForceNewtons(double gravityForceOnModuleNewtons) { - return gravityForceOnModuleNewtons * WHEELS_COEFFICIENT_OF_FRICTION; - } - - /** - * - * - *

Updates the Simulation for This Module.

- * - *

This method is called once during every sub-tick of the simulation. It performs the following actions: - * - *

    - *
  • Updates the simulation of the steering mechanism. - *
  • Simulates the propelling force generated by the module. - *
  • Updates and caches the encoder readings for odometry simulation. - *
- * - *

Note: Friction forces are not simulated in this method. - * - * @param moduleCurrentGroundVelocityWorldRelative the current ground velocity of the module, relative to the world - * @param robotFacing the absolute facing of the robot, relative to the world - * @param gravityForceOnModuleNewtons the gravitational force acting on this module, in newtons - * @return the propelling force generated by the module, as a {@link Vector2} object - */ - public Vector2 updateSimulationSubTickGetModuleForce( - Vector2 moduleCurrentGroundVelocityWorldRelative, - Rotation2d robotFacing, - double gravityForceOnModuleNewtons) { - updateSteerSimulation(); - - /* the maximum gripping force that the wheel can generate */ - final double grippingForceNewtons = getGrippingForceNewtons(gravityForceOnModuleNewtons); - final Rotation2d moduleWorldFacing = this.steerAbsoluteFacing.plus(robotFacing); - final Vector2 propellingForce = - getPropellingForce(grippingForceNewtons, moduleWorldFacing, moduleCurrentGroundVelocityWorldRelative); - updateDriveEncoders(); - - return propellingForce; - } - - /** - * - * - *

updates the simulation for the steer mechanism.

- * - *

Updates the simulation for the steer mechanism and cache the encoder readings. - * - *

The steer mechanism is modeled by a {@link BrushlessMotorSim}. - */ - private void updateSteerSimulation() { - /* update the readings of the sensor */ - this.steerAbsoluteFacing = new Rotation2d(steerMotorSim.getPosition()); - this.steerRelativeEncoderPositionRad = steerMotorSim.getPosition().in(Radians) + steerRelativeEncoderOffSet; - this.steerAbsoluteEncoderSpeedRadPerSec = steerMotorSim.getVelocity().in(RadiansPerSecond); - this.steerRelativeEncoderSpeedRadPerSec = steerAbsoluteEncoderSpeedRadPerSec * STEER_GEAR_RATIO; - - /* cache sensor readings to queue for high-frequency odometry */ - this.cachedSteerAbsolutePositions.poll(); - this.cachedSteerAbsolutePositions.offer(steerAbsoluteFacing); - this.cachedSteerRelativeEncoderPositionsRad.poll(); - this.cachedSteerRelativeEncoderPositionsRad.offer(steerRelativeEncoderPositionRad); - } - - /** - * - * - *

Calculates the amount of propelling force that the module generates.

- * - *

The swerve module's drive motor will generate a propelling force. - * - *

For most of the time, that propelling force is directly applied to the drivetrain. And the drive wheel runs as - * fast as the ground velocity - * - *

However, if the propelling force exceeds the gripping, only the max gripping force is applied. The rest of the - * propelling force will cause the wheel to start skidding and make the odometry inaccurate. - * - * @param grippingForceNewtons the amount of gripping force that wheel can generate, in newtons - * @param moduleWorldFacing the current world facing of the module - * @param moduleCurrentGroundVelocity the current ground velocity of the module, world-reference - * @return a vector representing the propelling force that the module generates, world-reference - */ - private Vector2 getPropellingForce( - double grippingForceNewtons, Rotation2d moduleWorldFacing, Vector2 moduleCurrentGroundVelocity) { - final double driveWheelTorque = getDriveWheelTorque(), - theoreticalMaxPropellingForceNewtons = driveWheelTorque / WHEEL_RADIUS_METERS; - final boolean skidding = Math.abs(theoreticalMaxPropellingForceNewtons) > grippingForceNewtons; - final double propellingForceNewtons; - if (skidding) - propellingForceNewtons = Math.copySign(grippingForceNewtons, theoreticalMaxPropellingForceNewtons); - else propellingForceNewtons = theoreticalMaxPropellingForceNewtons; - - final double floorVelocityProjectionOnWheelDirectionMPS = moduleCurrentGroundVelocity.getMagnitude() - * Math.cos(moduleCurrentGroundVelocity.getAngleBetween(new Vector2(moduleWorldFacing.getRadians()))); - - // if the chassis is tightly gripped on floor, the floor velocity is projected to the wheel - this.driveEncoderUnGearedSpeedRadPerSec = - floorVelocityProjectionOnWheelDirectionMPS / WHEEL_RADIUS_METERS * DRIVE_GEAR_RATIO; - - // if the module is skidding - if (skidding) - this.driveEncoderUnGearedSpeedRadPerSec = - 0.5 * driveEncoderUnGearedSpeedRadPerSec + 0.5 * DRIVE_MOTOR.getSpeed(0, driveMotorAppliedVolts); - - return Vector2.create(propellingForceNewtons, moduleWorldFacing.getRadians()); - } - - /** - * - * - *

Calculates the amount of torque that the drive motor can generate on the wheel.

- * - *

Before calculating the torque of the motor, the output voltage of the drive motor is constrained for the - * current limit through {@link BrushlessMotorSim#constrainOutputVoltage(DCMotor, double, double, double)}. - * - * @return the amount of torque on the wheel by the drive motor, in Newton * Meters - */ - private double getDriveWheelTorque() { - driveMotorAppliedVolts = driveMotorRequestedVolts; - - /* calculate the actual supply current */ - driveMotorSupplyCurrentAmps = DRIVE_MOTOR.getCurrent( - this.driveEncoderUnGearedSpeedRadPerSec, - MathUtil.applyDeadband(driveMotorAppliedVolts, DRIVE_FRICTION_VOLTAGE, 12)); - - /* calculate the torque generated, */ - final double torqueOnRotter = DRIVE_MOTOR.getTorque(driveMotorSupplyCurrentAmps); - return torqueOnRotter * DRIVE_GEAR_RATIO; - } - - /** @return the current module state of this simulation module */ - public SwerveModuleState getCurrentState() { - return new SwerveModuleState(getDriveWheelFinalSpeedRadPerSec() * WHEEL_RADIUS_METERS, steerAbsoluteFacing); - } - - /** - * - * - *

Obtains the "free spin" state of the module

- * - *

The "free spin" state of a simulated module refers to its state after spinning freely for a long time under - * the current input voltage - * - * @return the free spinning module state - */ - protected SwerveModuleState getFreeSpinState() { - return new SwerveModuleState( - DRIVE_MOTOR.getSpeed( - DRIVE_MOTOR.getTorque(DRIVE_MOTOR.getCurrent(0, DRIVE_FRICTION_VOLTAGE)), - driveMotorAppliedVolts) - / DRIVE_GEAR_RATIO - * WHEEL_RADIUS_METERS, - steerAbsoluteFacing); - } - - /** - * - * - *

Cache the encoder values.

- * - *

An internal method to cache the encoder values to their queues. - */ - private void updateDriveEncoders() { - this.driveEncoderUnGearedPositionRad += - this.driveEncoderUnGearedSpeedRadPerSec * SimulatedField.getSimulationDt(); - this.cachedDriveEncoderUnGearedPositionsRad.poll(); - this.cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); - } - - /** - * - * - *

Obtains the theoretical speed that the module can achieve.

- * - * @return the theoretical maximum ground speed that the module can achieve, in m/s - */ - public double getModuleTheoreticalSpeedMPS() { - return DRIVE_MOTOR.freeSpeedRadPerSec / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS; - } - - /** - * - * - *

Obtains the theoretical maximum propelling force of ONE module.

- * - *

Calculates the maximum propelling force with respect to the gripping force and the drive motor's torque under - * its current limit. - * - * @param robotMassKg the mass of the robot, is kilograms - * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force equally - * @return the maximum propelling force of EACH module - */ - public double getTheoreticalPropellingForcePerModule(double robotMassKg, int modulesCount) { - final double - maxThrustNewtons = DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS, - maxGrippingNewtons = 9.8 * robotMassKg / modulesCount * WHEELS_COEFFICIENT_OF_FRICTION; - - return Math.min(maxThrustNewtons, maxGrippingNewtons); - } - - /** - * - * - *

Obtains the theatrical linear acceleration that the robot can achieve.

- * - *

Calculates the maximum linear acceleration of a robot, with respect to its mass and - * {@link #getTheoreticalPropellingForcePerModule(double, int)}. - * - * @param robotMassKg the mass of the robot, is kilograms - * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force equally - */ - public double getModuleMaxAccelerationMPSsq(double robotMassKg, int modulesCount) { - return getTheoreticalPropellingForcePerModule(robotMassKg, modulesCount) * modulesCount / robotMassKg; - } - - /** - * - * - *

Stores the coefficient of friction of some common used wheels.

- */ - public enum WHEEL_GRIP { - RUBBER_WHEEL(1.25), - TIRE_WHEEL(1.2); - - public final double cof; - - WHEEL_GRIP(double cof) { - this.cof = cof; - } - } - - /** - * creates a SDS Mark4 - * Swerve Module for simulation - */ - public static Supplier getMark4( - DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { - return () -> new SwerveModuleSimulation( - driveMotor, - steerMotor, - driveCurrentLimitAmps, - switch (gearRatioLevel) { - case 1 -> 8.14; - case 2 -> 6.75; - case 3 -> 6.12; - case 4 -> 5.14; - default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); - }, - 12.8, - 0.2, - 0.3, - wheelCOF, - Units.inchesToMeters(2), - 0.03); - } - - /** - * creates a SDS - * Mark4-i Swerve Module for simulation - */ - public static Supplier getMark4i( - DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { - return () -> new SwerveModuleSimulation( - driveMotor, - steerMotor, - driveCurrentLimitAmps, - switch (gearRatioLevel) { - case 1 -> 8.14; - case 2 -> 6.75; - case 3 -> 6.12; - case 4 -> 5.15; - default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); - }, - 150.0 / 7.0, - 0.2, - 1, - wheelCOF, - Units.inchesToMeters(2), - 0.025); - } - - /** - * creates a SDS Mark4-n Swerve - * Module for simulation - */ - public static Supplier getMark4n( - DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { - return () -> new SwerveModuleSimulation( - driveMotor, - steerMotor, - driveCurrentLimitAmps, - switch (gearRatioLevel) { - case 1 -> 7.13; - case 2 -> 5.9; - case 3 -> 5.36; - default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); - }, - 18.75, - 0.25, - 1, - wheelCOF, - Units.inchesToMeters(2), - 0.025); - } - - /** - * creates a WCP SwerveX Swerve Module - * for simulation - * - *

X1 Ratios are gearRatioLevel 1-3
- * X2 Ratios are gearRatioLevel 4-6
- * X3 Ratios are gearRatioLevel 7-9 - */ - public static Supplier getSwerveX( - DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { - return () -> new SwerveModuleSimulation( - driveMotor, - steerMotor, - driveCurrentLimitAmps, - switch (gearRatioLevel) { - case 1 -> 7.85; - case 2 -> 7.13; - case 3 -> 6.54; - case 4 -> 6.56; - case 5 -> 5.96; - case 6 -> 5.46; - case 7 -> 5.14; - case 8 -> 4.75; - case 9 -> 4.41; - default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); - }, - 11.3142, - 0.2, - 0.3, - wheelCOF, - Units.inchesToMeters(2), - 0.03); - } - - /** - * creates a WCP SwerveX Flipped - * Swerve Module for simulation - * - *

X1 Ratios are gearRatioLevel 1-3
- * X2 Ratios are gearRatioLevel 4-6
- * X3 Ratios are gearRatioLevel 7-9 - */ - public static Supplier getSwerveXFlipped( - DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { - return () -> new SwerveModuleSimulation( - driveMotor, - steerMotor, - driveCurrentLimitAmps, - switch (gearRatioLevel) { - case 1 -> 8.1; - case 2 -> 7.36; - case 3 -> 6.75; - case 4 -> 6.72; - case 5 -> 6.11; - case 6 -> 5.6; - case 7 -> 5.51; - case 8 -> 5.01; - case 9 -> 4.59; - default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); - }, - 11.3714, - 0.2, - 0.3, - wheelCOF, - Units.inchesToMeters(2), - 0.03); - } - - /** - * creates a WCP SwerveXS Swerve - * Module for simulation - * - *

X1 Ratios are gearRatioLevel 1-3
- * X2 Ratios are gearRatioLevel 4-6 - */ - public static Supplier getSwerveXS( - DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { - return () -> new SwerveModuleSimulation( - driveMotor, - steerMotor, - driveCurrentLimitAmps, - switch (gearRatioLevel) { - case 1 -> 6.0; - case 2 -> 5.54; - case 3 -> 5.14; - case 4 -> 4.71; - case 5 -> 4.4; - case 6 -> 4.13; - default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); - }, - 41.25, - 0.2, - 0.3, - wheelCOF, - Units.inchesToMeters(1.5), - 0.03); - } - - /** - * creates a WCP SwerveX2 Swerve - * Module for simulation - * - *

X1 Ratios are gearRatioLevel 1-3
- * X2 Ratios are gearRatioLevel 4-6
- * X3 Ratios are gearRatioLevel 7-9
- * X4 Ratios are gearRatioLevel 10-12 - */ - public static Supplier getSwerveX2( - DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { - return () -> new SwerveModuleSimulation( - driveMotor, - steerMotor, - driveCurrentLimitAmps, - switch (gearRatioLevel) { - case 1 -> 7.67; - case 2 -> 6.98; - case 3 -> 6.39; - case 4 -> 6.82; - case 5 -> 6.20; - case 6 -> 5.68; - case 7 -> 6.48; - case 8 -> 5.89; - case 9 -> 5.40; - case 10 -> 5.67; - case 11 -> 5.15; - case 12 -> 4.73; - default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); - }, - 12.1, - 0.2, - 0.3, - wheelCOF, - Units.inchesToMeters(2), - 0.03); - } - - /** - * creates a WCP SwerveX2S Swerve - * Module for simulation - * - *

X1 Ratios are gearRatioLevel 1-3
- * X2 Ratios are gearRatioLevel 4-6
- * X3 Ratios are gearRatioLevel 7-9 - */ - public static Supplier getSwerveX2S( - DCMotor driveMotor, DCMotor steerMotor, double driveCurrentLimitAmps, double wheelCOF, int gearRatioLevel) { - return () -> new SwerveModuleSimulation( - driveMotor, - steerMotor, - driveCurrentLimitAmps, - switch (gearRatioLevel) { - case 1 -> 6.0; - case 2 -> 5.63; - case 3 -> 5.29; - case 4 -> 4.94; - case 5 -> 4.67; - case 6 -> 4.42; - case 7 -> 4.11; - case 8 -> 3.9; - case 9 -> 3.71; - default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel); - }, - 25.9, - 0.2, - 0.3, - wheelCOF, - Units.inchesToMeters(1.875), - 0.03); - } + public static Supplier getModule( + DCMotor driveMotor, + DCMotor turnMotor, + double driveCurrentLimitAmps, + WHEEL_GRIP driveWheelType, + double driveGearRatio) { + return () -> + new SwerveModuleSimulation( + driveMotor, + turnMotor, + driveCurrentLimitAmps, + driveGearRatio, + 11.3142, + 0.25, + 0.05, + driveWheelType.cof, + Units.inchesToMeters(2), + 0.05); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index 0e3a6266..a9409a1e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -82,8 +82,8 @@ public static final class DriveConstants { public static final InvertedValue REAR_RIGHT_DRIVE_ENCODER_REVERSED = InvertedValue.CounterClockwise_Positive; - public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 3 * Math.PI; - public static final double LOW_ANGULAR_SPEED_RADIANS_PER_SECOND = 2 * Math.PI; + public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 6 * Math.PI; + public static final double LOW_ANGULAR_SPEED_RADIANS_PER_SECOND = 3 * Math.PI; public static final double MAX_SPEED_METERS_PER_SECOND = 4.5; public static final double MAX_SHOOT_SPEED_METERS_PER_SECOND = 3; @@ -186,7 +186,7 @@ public class SimulationConstants { public static final DCMotor DRIVE_MOTOR = DCMotor.getKrakenX60(1), STEER_MOTOR = DCMotor.getFalcon500(1); - public static final double WHEEL_RADIUS_METERS = ModuleConstants.WHEEL_DIAMETER_METERS / 2, + public static final double WHEEL_RADIUS_METERS = ModuleConstants.WHEEL_DIAMETER_METERS / 2.0, DRIVE_GEAR_RATIO = ModuleConstants.DRIVE_GEAR_RATIO, STEER_GEAR_RATIO = 16.0, TIME_ROBOT_STOP_ROTATING_SECONDS = 0.06, diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 3c90a2e7..ff38a99c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -197,8 +197,7 @@ xSpeed, ySpeed, rotationSpeed, getPose().getRotation()) swerveModuleStates, DriveConstants.MAX_SPEED_METERS_PER_SECOND); setModuleStates(swerveModuleStates); - Logger.recordOutput("SwerveStates/SwerveModuleStates", swerveModuleStates); - Logger.recordOutput("SwerveStates/MeasuredStates", getModuleStates()); + Logger.recordOutput("SwerveStates/DesiredStates", swerveModuleStates); } /** Returns 0 degrees if the robot is on the blue alliance, 180 if on the red alliance. */ @@ -227,7 +226,6 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { * @param timestampIndex index of the timestamp to sample the pose at */ private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { - // if (timestampIndex == 0) return; final SwerveModulePosition[] modulePositions = getModulesPosition(timestampIndex), moduleDeltas = getModulesDelta(modulePositions); @@ -252,11 +250,6 @@ private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { * rotation at the sampled timestamp. */ private SwerveModulePosition[] getModulesPosition(int timestampIndex) { - // Check if swerveModules is null or empty, return early if so - if (swerveModules == null || swerveModules.length == 0) { - // Logger.error("Swerve modules array is empty or not initialized properly!"); - return new SwerveModulePosition[4]; // Return an empty array of the expected size - } SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[swerveModules.length]; for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) swerveModulePositions[moduleIndex] = diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 1d9c7724..cdd101e7 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -1,15 +1,17 @@ package frc.robot.subsystems.swerve; -import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.*; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; +import frc.robot.subsystems.swerve.SwerveConstants.SimulationConstants; import frc.robot.subsystems.swerve.moduleIO.ModuleInputsAutoLogged; import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; import org.littletonrobotics.junction.Logger; @@ -45,14 +47,21 @@ public void periodic() { updateOdometryPositions(); } - private void updateOdometryPositions() { odometryPositions = new SwerveModulePosition[inputs.odometryDriveWheelRevolutions.length]; for (int i = 0; i < odometryPositions.length; i++) { - double positionMeters = inputs.odometryDriveWheelRevolutions[i]; - Rotation2d angle = inputs.odometrySteerPositions[i]; - odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + double positionMeters = inputs.odometryDriveWheelRevolutions[i]; + Rotation2d angle = inputs.odometrySteerPositions[i]; + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + + SmartDashboard.putNumber("updated drive position", positionMeters); + SmartDashboard.putNumber("updated angle", angle.getDegrees()); } + } + + + private double driveRevolutionsToMeters(double driveWheelRevolutions) { + return Rotations.of(driveWheelRevolutions).in(Radians) * SimulationConstants.WHEEL_RADIUS_METERS; } public void setVoltage(Voltage volts) { diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java index e3915423..54d60aef 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java @@ -22,10 +22,9 @@ class ModuleInputs { public double turnCurrentAmps = 0.0; public double[] odometryTimestamps = new double[] {}; - + public double[] odometryDriveWheelRevolutions = new double[] {}; public Rotation2d[] odometrySteerPositions = new Rotation2d[] {}; - } /** diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java index 13463975..41dcdcef 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java @@ -153,11 +153,9 @@ public void updateInputs(ModuleInputs inputs) { turnEncoderAbsolutePosition.clear(); } - inputs.driveAppliedVolts = driveMotorAppliedVoltage.getValueAsDouble(); inputs.driveCurrentAmps = driveMotorCurrent.getValueAsDouble(); - inputs.turnVelocity = turnEncoderVelocity.getValueAsDouble(); inputs.turnAppliedVolts = turnMotorAppliedVolts.getValueAsDouble(); inputs.turnCurrentAmps = turnMotorCurrent.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index 5bb4d4cc..0b17fe91 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -2,8 +2,6 @@ import static edu.wpi.first.units.Units.*; -import java.util.Arrays; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -12,31 +10,41 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.extras.simulation.OdometryTimestampsSim; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; +import frc.robot.subsystems.swerve.SwerveConstants.SimulationConstants; + +import java.util.Arrays; /** Wrapper class around {@link SwerveModuleSimulation} */ public class SimulatedModule implements ModuleInterface { private final SwerveModuleSimulation moduleSimulation; - private final PIDController drivePID = new PIDController(.5, 0, 0); - private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(5, 0, 0); + private final PIDController drivePID = new PIDController(.05, 0, 0); + private final SimpleMotorFeedforward driveFF = + new SimpleMotorFeedforward(0.0, .0, 0.0); - private final Constraints turnConstraints = new Constraints(5, 2); + private final Constraints turnConstraints = + new Constraints( + RadiansPerSecond.of(2 * Math.PI).in(RotationsPerSecond), + RadiansPerSecondPerSecond.of(4 * Math.PI).in(RotationsPerSecondPerSecond)); private final ProfiledPIDController turnPID = - new ProfiledPIDController(500, 0, 0, turnConstraints); - private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(5, 0, 0); + new ProfiledPIDController(Radians.of(10).in(Rotations), 0, 0, turnConstraints); + private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(0.77, 0.75, 0); public SimulatedModule(SwerveModuleSimulation moduleSimulation) { this.moduleSimulation = moduleSimulation; + turnPID.enableContinuousInput(-Math.PI, Math.PI); } @Override public void updateInputs(ModuleInputs inputs) { - inputs.drivePosition = Units.radiansToRotations(moduleSimulation.getDriveEncoderFinalPositionRad()); + inputs.drivePosition = + Units.radiansToRotations(moduleSimulation.getDriveEncoderFinalPositionRad()); inputs.driveVelocity = - Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()); + Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()); inputs.driveAppliedVolts = moduleSimulation.getDriveMotorAppliedVolts(); inputs.driveCurrentAmps = moduleSimulation.getDriveMotorSupplyCurrentAmps(); @@ -45,17 +53,19 @@ public void updateInputs(ModuleInputs inputs) { inputs.turnAppliedVolts = moduleSimulation.getTurnMotorAppliedVolts(); inputs.turnCurrentAmps = moduleSimulation.getTurnMotorSupplyCurrentAmps(); - - inputs.odometryDriveWheelRevolutions = Arrays.stream(moduleSimulation.getCachedDriveWheelFinalPositionsRad()) - .map(Units::radiansToRotations) - .toArray(); - - inputs.odometrySteerPositions = moduleSimulation.getCachedTurnAbsolutePositions(); + inputs.odometryDriveWheelRevolutions = + Arrays.stream(moduleSimulation.getCachedDriveWheelFinalPositionsRad()) + .map(Units::radiansToRotations) + .toArray(); + inputs.odometrySteerPositions = moduleSimulation.getCachedTurnAbsolutePositions(); inputs.odometryTimestamps = OdometryTimestampsSim.getTimestamps(); inputs.isConnected = true; + + SmartDashboard.putNumber("turn absolute position", moduleSimulation.getTurnAbsolutePosition().getDegrees()); + SmartDashboard.putNumber("drive pos", Radians.of(moduleSimulation.getDriveEncoderFinalPositionRad()).in(Rotations) * ModuleConstants.DRIVE_TO_METERS); } @Override @@ -68,18 +78,18 @@ public void setTurnVoltage(Voltage volts) { moduleSimulation.requestTurnVoltageOut(volts); } + @Override public void setDesiredState(SwerveModuleState desiredState) { double turnRotations = getTurnRotations(); // Optimize the reference state to avoid spinning further than 90 degrees SwerveModuleState setpoint = new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); - setpoint.optimize(Rotation2d.fromRotations(turnRotations)); setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); + setpoint.optimize(Rotation2d.fromRotations(turnRotations)); if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) { - moduleSimulation.requestDriveVoltageOut(0); - moduleSimulation.requestTurnVoltageOut(0); + stopModule(); return; } @@ -88,6 +98,9 @@ public void setDesiredState(SwerveModuleState desiredState) { setpoint.speedMetersPerSecond * ModuleConstants.DRIVE_GEAR_RATIO / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; + SmartDashboard.putNumber("desired drive RPS", desiredDriveRPS); + SmartDashboard.putNumber("current drive RPS", RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()).in(RotationsPerSecond)); + SmartDashboard.putNumber("desired angle", desiredState.angle.getDegrees()); moduleSimulation.requestDriveVoltageOut( Volts.of( @@ -104,13 +117,14 @@ public void setDesiredState(SwerveModuleState desiredState) { .plus(turnFF.calculate(RotationsPerSecond.of(turnPID.getSetpoint().velocity)))); } + @Override public double getTurnRotations() { return moduleSimulation.getTurnAbsolutePosition().getRotations(); } @Override public void stopModule() { - moduleSimulation.requestDriveVoltageOut(Volts.zero()); - moduleSimulation.requestTurnVoltageOut(Volts.zero()); + moduleSimulation.requestDriveVoltageOut(Volts.of(0)); + moduleSimulation.requestTurnVoltageOut(Volts.of(0)); } } From 82c2f253956b10b124f644d14c744d52ba7fafce Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Wed, 6 Nov 2024 08:02:50 -0500 Subject: [PATCH 09/75] im actually such a bad programmer --- .../swerve/SwerveModuleSimulation.java | 1293 +++++++++-------- .../robot/subsystems/swerve/SwerveModule.java | 12 +- .../swerve/moduleIO/SimulatedModule.java | 34 +- 3 files changed, 683 insertions(+), 656 deletions(-) diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java index 9f6af990..87037b71 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java @@ -14,29 +14,27 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; - +import frc.robot.extras.simulation.field.SimulatedField; +import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; import java.util.Queue; import java.util.concurrent.ConcurrentLinkedQueue; import java.util.function.Supplier; import org.dyn4j.geometry.Vector2; -import frc.robot.extras.simulation.mechanismSim.swerve.BrushlessMotorSim; -import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; -import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; -import frc.robot.extras.simulation.field.SimulatedField; /** * * *

Simulation for a Single Swerve Module.

* - *

This class provides a simulation for a single swerve module in the {@link SwerveDriveSimulation}. + *

This class provides a simulation for a single swerve module in the {@link + * SwerveDriveSimulation}. * *

1. Purpose

* *

This class serves as the bridge between your code and the physics engine. * - *

You will apply voltage outputs to the drive/steer motor of the module and obtain their encoder readings in your - * code, just as how you deal with your physical motors. + *

You will apply voltage outputs to the drive/steer motor of the module and obtain their encoder + * readings in your code, just as how you deal with your physical motors. * *

2. Perspectives

* @@ -54,8 +52,9 @@ *
  • Use {@link SwerveDriveOdometry} to estimate the pose of your robot. *
  • 250Hz - * Odometry is supported. You can retrive cached encoder readings from every sub-tick through - * {@link #getCachedDriveEncoderUnGearedPositionsRad()} and {@link #getCachedTurnAbsolutePositions()}. + * Odometry is supported. You can retrive cached encoder readings from every sub-tick + * through {@link #getCachedDriveEncoderUnGearedPositionsRad()} and {@link + * #getCachedTurnAbsolutePositions()}. * * *

    An example of how to simulate odometry using this class is the Advanced Swerve Drive with maple-sim example. */ public class SwerveModuleSimulation { - public final DCMotor DRIVE_MOTOR; - private final BrushlessMotorSim turnMotorSim; - public final double DRIVE_CURRENT_LIMIT, - DRIVE_GEAR_RATIO, - TURN_GEAR_RATIO, - DRIVE_FRICTION_VOLTAGE, - WHEELS_COEFFICIENT_OF_FRICTION, - WHEEL_RADIUS_METERS, - DRIVE_WHEEL_INERTIA = 0.01; - private double driveMotorRequestedVolts = 0.0, - driveMotorAppliedVolts = 0.0, - driveMotorSupplyCurrentAmps = 0.0, - turnRelativeEncoderPositionRad = 0.0, - turnRelativeEncoderSpeedRadPerSec = 0.0, - turnAbsoluteEncoderSpeedRadPerSec = 0.0, - driveEncoderUnGearedPositionRad = 0.0, - driveEncoderUnGearedSpeedRadPerSec = 0.0; - private Rotation2d turnAbsolutePosition = Rotation2d.fromRotations(Math.random()); - - private final double turnRelativeEncoderOffSet = (Math.random() - 0.5) * 30; - - private final Queue cachedTurnRelativeEncoderPositionsRad, cachedDriveEncoderUnGearedPositionsRad; - private final Queue cachedTurnAbsolutePositions; - - /** - * - * - *

    Constructs a Swerve Module Simulation.

    - * - *

    If you are using {@link SimulatedField#overrideSimulationTimings(double, int)} to use custom timings, you must - * call the method before constructing any swerve module simulations using this constructor. - * - * @param driveMotor the model of the driving motor - * @param turnMotor the model of the steering motor - * @param driveCurrentLimit the current limit for the driving motor, in amperes - * @param driveGearRatio the gear ratio for the driving motor, >1 is reduction - * @param turnGearRatio the gear ratio for the steering motor, >1 is reduction - * @param driveFrictionVoltage the measured minimum amount of voltage that can turn the driving rotter, in volts - * @param turnFrictionVoltage the measured minimum amount of voltage that can turn the steering rotter, in volts - * @param tireCoefficientOfFriction the coefficient - * of friction of the tires, normally around 1.5 - * @param wheelsRadiusMeters the radius of the wheels, in meters. Calculate it using - * {@link Units#inchesToMeters(double)}. - * @param turnRotationalInertia the rotational inertia of the steering mechanism - */ - public SwerveModuleSimulation( - DCMotor driveMotor, - DCMotor turnMotor, - double driveCurrentLimit, - double driveGearRatio, - double turnGearRatio, - double driveFrictionVoltage, - double turnFrictionVoltage, - double tireCoefficientOfFriction, - double wheelsRadiusMeters, - double turnRotationalInertia) { - DRIVE_MOTOR = driveMotor; - DRIVE_CURRENT_LIMIT = driveCurrentLimit; - DRIVE_GEAR_RATIO = driveGearRatio; - TURN_GEAR_RATIO = turnGearRatio; - DRIVE_FRICTION_VOLTAGE = driveFrictionVoltage; - WHEELS_COEFFICIENT_OF_FRICTION = tireCoefficientOfFriction; - WHEEL_RADIUS_METERS = wheelsRadiusMeters; - - this.turnMotorSim = new BrushlessMotorSim( - SimulatedField.getInstance(), - turnMotor, - turnGearRatio, - KilogramSquareMeters.of(turnRotationalInertia), - Volts.of(turnFrictionVoltage)); - - this.cachedDriveEncoderUnGearedPositionsRad = new ConcurrentLinkedQueue<>(); - for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) - cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); - this.cachedTurnRelativeEncoderPositionsRad = new ConcurrentLinkedQueue<>(); - for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) - cachedTurnRelativeEncoderPositionsRad.offer(turnRelativeEncoderPositionRad); - this.cachedTurnAbsolutePositions = new ConcurrentLinkedQueue<>(); - for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) - cachedTurnAbsolutePositions.offer(turnAbsolutePosition); - - this.turnRelativeEncoderPositionRad = turnAbsolutePosition.getRadians() + turnRelativeEncoderOffSet; - } - - /** - * - * - *

    Requests the Driving Motor to Run at a Specified Voltage Output.

    - * - *

    Think of it as the setVoltage() of your physical driving motor.

    - * - *

    This method sets the desired voltage output for the driving motor. The change will be applied in the next - * sub-tick of the simulation. - * - *

    Note: The requested voltage may not always be fully applied if the current is too high. The - * current limit may reduce the motor's output, similar to real motors. - * - *

    To check the actual voltage applied to the drivetrain, use {@link #getDriveMotorAppliedVolts()}. - * - * @param volts the voltage to be applied to the driving motor - */ - public void requestDriveVoltageOut(Voltage volts) { - this.driveMotorRequestedVolts = volts.in(Volts); - } - - /** - * - * - *

    Requests the Steering Motor to Run at a Specified Voltage Output.

    - * - *

    Think of it as the setVoltage() of your physical steering motor.

    - * - *

    This method sets the desired voltage output for the steering motor. The change will be applied in the next - * sub-tick of the simulation. - * - *

    Note: Similar to the drive motor, the requested voltage may not always be fully applied if - * the current exceeds the limit. The current limit will reduce the motor's output as needed, mimicking real motor - * behavior. - * - *

    To check the actual voltage applied to the steering motor, use {@link #getTurnMotorAppliedVolts()}. - * - * @param volts the voltage to be applied to the steering motor - */ - public void requestTurnVoltageOut(Voltage volts) { - this.turnMotorSim.setControl(volts); - } - - /** - * - * - *

    Obtains the Actual Output Voltage of the Drive Motor.

    - * - *

    This method returns the actual voltage being applied to the drive motor. The actual applied voltage may differ - * from the value set by {@link #requestDriveVoltageOut(double)}. - * - *

    If the motor's supply current is too high, the motor will automatically reduce its output voltage to protect - * the system. - * - * @return the actual output voltage of the drive motor, in volts - */ - public double getDriveMotorAppliedVolts() { - return driveMotorAppliedVolts; - } - - /** - * - * - *

    Obtains the Actual Output Voltage of the Steering Motor.

    - * - *

    This method returns the actual voltage being applied to the steering motor. It wraps around the - * {@link BrushlessMotorSim#getAppliedVolts()} method. - * - *

    The actual applied voltage may differ from the value set by {@link #requestTurnVoltageOut(double)}. If the - * motor's supply current is too high, the motor will automatically reduce its output voltage to protect the system. - * - * @return the actual output voltage of the steering motor, in volts - */ - public double getTurnMotorAppliedVolts() { - return turnMotorSim.getRotorVoltage().in(Volts); - } - - /** - * - * - *

    Obtains the Amount of Current Supplied to the Drive Motor.

    - * - *

    Think of it as the getSupplyCurrent() of your physical drive motor.

    - * - * @return the current supplied to the drive motor, in amperes - */ - public double getDriveMotorSupplyCurrentAmps() { - return driveMotorSupplyCurrentAmps; - } - - /** - * - * - *

    Obtains the Amount of Current Supplied to the Steer Motor.

    - * - *

    Think of it as the getSupplyCurrent() of your physical steer motor.

    - * - *

    This method wraps around {@link BrushlessMotorSim#getCurrentDrawAmps()}. - * - * @return the current supplied to the steer motor, in amperes - */ - public double getTurnMotorSupplyCurrentAmps() { - return turnMotorSim.getSupplyCurrent().in(Amps); - } - - /** - * - * - *

    Obtains the Position of the Drive Encoder.

    - * - *

    Think of it as the getPosition() of your physical driving motor.

    - * - *

    This method is used to simulate your {@link SwerveDrivePoseEstimator}. - * - *

    This value represents the un-geared position of the encoder, i.e., the amount of radians the drive motor's - * encoder has rotated. - * - *

    To get the final wheel rotations, use {@link #getDriveEncoderFinalPositionRad()}. - * - * @return the position of the drive motor's encoder, in radians (un-geared) - */ - public double getDriveEncoderUnGearedPositionRad() { - return driveEncoderUnGearedPositionRad; - } - - /** - * - * - *

    Obtains the Final Position of the Drive Encoder (Wheel Rotations).

    - * - *

    This method is used to simulate the {@link SwerveDrivePoseEstimator} by providing the final position of the - * drive encoder in terms of wheel rotations. - * - *

    The value is calculated by dividing the un-geared encoder position (obtained from - * {@link #getDriveEncoderUnGearedPositionRad()}) by the drive gear ratio. - * - * @return the final position of the drive encoder (wheel rotations), in radians - */ - public double getDriveEncoderFinalPositionRad() { - return getDriveEncoderUnGearedPositionRad(); - // / DRIVE_GEAR_RATIO; - } - - /** - * - * - *

    Obtains the Speed of the Drive Encoder (Un-Geared), in Radians per Second.

    - * - *

    Think of it as the getVelocity() of your physical drive motor.

    - * - *

    This method returns the current speed of the drive encoder in radians per second, without accounting for the - * drive gear ratio. - * - * @return the un-geared speed of the drive encoder, in radians per second - */ - public double getDriveEncoderUnGearedSpeedRadPerSec() { - return driveEncoderUnGearedSpeedRadPerSec; - } - - /** - * - * - *

    Obtains the Final Speed of the Wheel, in Radians per Second.

    - * - * @return the final speed of the drive wheel, in radians per second - */ - public double getDriveWheelFinalSpeedRadPerSec() { - return getDriveEncoderUnGearedSpeedRadPerSec(); - // / DRIVE_GEAR_RATIO; - } - - /** - * - * - *

    Obtains the Relative Position of the Steer Encoder, in Radians.

    - * - *

    Think of it as the getPosition() of your physical steer motor.

    - * - * @return the relative encoder position of the steer motor, in radians - */ - public double getTurnRelativeEncoderPositionRad() { - return turnRelativeEncoderPositionRad; - } - - /** - * - * - *

    Obtains the Speed of the Steer Relative Encoder, in Radians per Second (Geared).

    - * - *

    Think of it as the getVelocity() of your physical steer motor.

    - * - * @return the speed of the steer relative encoder, in radians per second (geared) - */ - public double getTurnRelativeEncoderSpeedRadPerSec() { - return turnRelativeEncoderSpeedRadPerSec; - } - - /** - * - * - *

    Obtains the Absolute Facing of the Steer Mechanism.

    - * - *

    Think of it as the getAbsolutePosition() of your CanCoder.

    - * - * @return the absolute facing of the steer mechanism, as a {@link Rotation2d} - */ - public Rotation2d getTurnAbsolutePosition() { - return turnAbsolutePosition; - } - - /** - * - * - *

    Obtains the Absolute Rotational Velocity of the Steer Mechanism.

    - * - *

    Think of it as the getVelocity() of your CanCoder.

    - * - * @return the absolute angular velocity of the steer mechanism, in radians per second - */ - public double getTurnAbsoluteEncoderSpeedRadPerSec() { - return turnAbsoluteEncoderSpeedRadPerSec; - } - - /** - * - * - *

    Obtains the Cached Readings of the Drive Encoder's Un-Geared Position.

    - * - *

    The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and can be retrieved - * using this method. - * - * @return an array of cached drive encoder un-geared positions, in radians - */ - public double[] getCachedDriveEncoderUnGearedPositionsRad() { - return cachedDriveEncoderUnGearedPositionsRad.stream() - .mapToDouble(value -> value) - .toArray(); - } - - /** - * - * - *

    Obtains the Cached Readings of the Drive Encoder's Final Position (Wheel Rotations).

    - * - *

    The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and are divided by the - * gear ratio to obtain the final wheel rotations. - * - * @return an array of cached drive encoder final positions (wheel rotations), in radians - */ - public double[] getCachedDriveWheelFinalPositionsRad() { - return cachedDriveEncoderUnGearedPositionsRad.stream() - .mapToDouble(value -> value * ModuleConstants.DRIVE_TO_METERS) - .toArray(); - } - - /** - * - * - *

    Obtains the Cached Readings of the Steer Relative Encoder's Position.

    - * - *

    The values of {@link #getTurnRelativeEncoderPositionRad()} are cached at each sub-tick and can be retrieved - * using this method. - * - * @return an array of cached steer relative encoder positions, in radians - */ - public double[] getCachedTurnRelativeEncoderPositions() { - return cachedTurnRelativeEncoderPositionsRad.stream() - .mapToDouble(value -> value) - .toArray(); - } - - /** - * - * - *

    Obtains the Cached Readings of the Steer Absolute Positions.

    - * - *

    The values of {@link #getTurnAbsolutePosition()} are cached at each sub-tick and can be retrieved using this - * method. - * - * @return an array of cached absolute steer positions, as {@link Rotation2d} objects - */ - public Rotation2d[] getCachedTurnAbsolutePositions() { - return cachedTurnAbsolutePositions.toArray(Rotation2d[]::new); - } - - protected double getGrippingForceNewtons(double gravityForceOnModuleNewtons) { - return gravityForceOnModuleNewtons * WHEELS_COEFFICIENT_OF_FRICTION; - } - - /** - * - * - *

    Updates the Simulation for This Module.

    - * - *

    This method is called once during every sub-tick of the simulation. It performs the following actions: - * - *

      - *
    • Updates the simulation of the steering mechanism. - *
    • Simulates the propelling force generated by the module. - *
    • Updates and caches the encoder readings for odometry simulation. - *
    - * - *

    Note: Friction forces are not simulated in this method. - * - * @param moduleCurrentGroundVelocityWorldRelative the current ground velocity of the module, relative to the world - * @param robotFacing the absolute facing of the robot, relative to the world - * @param gravityForceOnModuleNewtons the gravitational force acting on this module, in newtons - * @return the propelling force generated by the module, as a {@link Vector2} object - */ - public Vector2 updateSimulationSubTickGetModuleForce( - Vector2 moduleCurrentGroundVelocityWorldRelative, - Rotation2d robotFacing, - double gravityForceOnModuleNewtons) { - updateTurnSimulation(); - - /* the maximum gripping force that the wheel can generate */ - final double grippingForceNewtons = getGrippingForceNewtons(gravityForceOnModuleNewtons); - final Rotation2d moduleWorldFacing = this.turnAbsolutePosition.plus(robotFacing); - final Vector2 propellingForce = - getPropellingForce(grippingForceNewtons, moduleWorldFacing, moduleCurrentGroundVelocityWorldRelative); - updateDriveEncoders(); - - return propellingForce; - } - - /** - * - * - *

    updates the simulation for the steer mechanism.

    - * - *

    Updates the simulation for the steer mechanism and cache the encoder readings. - * - *

    The steer mechanism is modeled by a {@link BrushlessMotorSim}. - */ - private void updateTurnSimulation() { - /* update the readings of the sensor */ - this.turnAbsolutePosition = new Rotation2d(turnMotorSim.getPosition()); - this.turnRelativeEncoderPositionRad = turnMotorSim.getPosition().in(Radians) + turnRelativeEncoderOffSet; - this.turnAbsoluteEncoderSpeedRadPerSec = turnMotorSim.getVelocity().in(RadiansPerSecond); - this.turnRelativeEncoderSpeedRadPerSec = turnAbsoluteEncoderSpeedRadPerSec * TURN_GEAR_RATIO; - - /* cache sensor readings to queue for high-frequency odometry */ - this.cachedTurnAbsolutePositions.poll(); - this.cachedTurnAbsolutePositions.offer(turnAbsolutePosition); - this.cachedTurnRelativeEncoderPositionsRad.poll(); - this.cachedTurnRelativeEncoderPositionsRad.offer(turnRelativeEncoderPositionRad); - } - - /** - * - * - *

    Calculates the amount of propelling force that the module generates.

    - * - *

    The swerve module's drive motor will generate a propelling force. - * - *

    For most of the time, that propelling force is directly applied to the drivetrain. And the drive wheel runs as - * fast as the ground velocity - * - *

    However, if the propelling force exceeds the gripping, only the max gripping force is applied. The rest of the - * propelling force will cause the wheel to start skidding and make the odometry inaccurate. - * - * @param grippingForceNewtons the amount of gripping force that wheel can generate, in newtons - * @param moduleWorldFacing the current world facing of the module - * @param moduleCurrentGroundVelocity the current ground velocity of the module, world-reference - * @return a vector representing the propelling force that the module generates, world-reference - */ - private Vector2 getPropellingForce( - double grippingForceNewtons, Rotation2d moduleWorldFacing, Vector2 moduleCurrentGroundVelocity) { - final double driveWheelTorque = getDriveWheelTorque(), - theoreticalMaxPropellingForceNewtons = driveWheelTorque / WHEEL_RADIUS_METERS; - final boolean skidding = Math.abs(theoreticalMaxPropellingForceNewtons) > grippingForceNewtons; - final double propellingForceNewtons; - if (skidding) - propellingForceNewtons = Math.copySign(grippingForceNewtons, theoreticalMaxPropellingForceNewtons); - else propellingForceNewtons = theoreticalMaxPropellingForceNewtons; - - final double floorVelocityProjectionOnWheelDirectionMPS = moduleCurrentGroundVelocity.getMagnitude() - * Math.cos(moduleCurrentGroundVelocity.getAngleBetween(new Vector2(moduleWorldFacing.getRadians()))); - - // if the chassis is tightly gripped on floor, the floor velocity is projected to the wheel - this.driveEncoderUnGearedSpeedRadPerSec = - floorVelocityProjectionOnWheelDirectionMPS / WHEEL_RADIUS_METERS * DRIVE_GEAR_RATIO; - - // if the module is skidding - if (skidding) - this.driveEncoderUnGearedSpeedRadPerSec = - 0.5 * driveEncoderUnGearedSpeedRadPerSec + 0.5 * DRIVE_MOTOR.getSpeed(0, driveMotorAppliedVolts); - - return Vector2.create(propellingForceNewtons, moduleWorldFacing.getRadians()); - } - - /** - * - * - *

    Calculates the amount of torque that the drive motor can generate on the wheel.

    - * - *

    Before calculating the torque of the motor, the output voltage of the drive motor is constrained for the - * current limit through {@link BrushlessMotorSim#constrainOutputVoltage(DCMotor, double, double, double)}. - * - * @return the amount of torque on the wheel by the drive motor, in Newton * Meters - */ - private double getDriveWheelTorque() { - driveMotorAppliedVolts = driveMotorRequestedVolts; - - /* calculate the actual supply current */ - driveMotorSupplyCurrentAmps = DRIVE_MOTOR.getCurrent( - this.driveEncoderUnGearedSpeedRadPerSec, - MathUtil.applyDeadband(driveMotorAppliedVolts, DRIVE_FRICTION_VOLTAGE, 12)); - - /* calculate the torque generated, */ - final double torqueOnRotter = DRIVE_MOTOR.getTorque(driveMotorSupplyCurrentAmps); - return torqueOnRotter * DRIVE_GEAR_RATIO; - } - - /** @return the current module state of this simulation module */ - public SwerveModuleState getCurrentState() { - return new SwerveModuleState(getDriveWheelFinalSpeedRadPerSec() * WHEEL_RADIUS_METERS, turnAbsolutePosition); - } - - /** - * - * - *

    Obtains the "free spin" state of the module

    - * - *

    The "free spin" state of a simulated module refers to its state after spinning freely for a long time under - * the current input voltage - * - * @return the free spinning module state - */ - protected SwerveModuleState getFreeSpinState() { - return new SwerveModuleState( - DRIVE_MOTOR.getSpeed( - DRIVE_MOTOR.getTorque(DRIVE_MOTOR.getCurrent(0, DRIVE_FRICTION_VOLTAGE)), - driveMotorAppliedVolts) - / DRIVE_GEAR_RATIO - * WHEEL_RADIUS_METERS, - turnAbsolutePosition); - } - - /** - * - * - *

    Cache the encoder values.

    - * - *

    An internal method to cache the encoder values to their queues. - */ - private void updateDriveEncoders() { - this.driveEncoderUnGearedPositionRad += - this.driveEncoderUnGearedSpeedRadPerSec * SimulatedField.getSimulationDt(); - this.cachedDriveEncoderUnGearedPositionsRad.poll(); - this.cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); - } - - /** - * - * - *

    Obtains the theoretical speed that the module can achieve.

    - * - * @return the theoretical maximum ground speed that the module can achieve, in m/s - */ - public double getModuleTheoreticalSpeedMPS() { - return DRIVE_MOTOR.freeSpeedRadPerSec / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS; - } - - /** - * - * - *

    Obtains the theoretical maximum propelling force of ONE module.

    - * - *

    Calculates the maximum propelling force with respect to the gripping force and the drive motor's torque under - * its current limit. - * - * @param robotMassKg the mass of the robot, is kilograms - * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force equally - * @return the maximum propelling force of EACH module - */ - public double getTheoreticalPropellingForcePerModule(double robotMassKg, int modulesCount) { - final double - maxThrustNewtons = DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS, - maxGrippingNewtons = 9.8 * robotMassKg / modulesCount * WHEELS_COEFFICIENT_OF_FRICTION; - - return Math.min(maxThrustNewtons, maxGrippingNewtons); - } - - /** - * - * - *

    Obtains the theatrical linear acceleration that the robot can achieve.

    - * - *

    Calculates the maximum linear acceleration of a robot, with respect to its mass and - * {@link #getTheoreticalPropellingForcePerModule(double, int)}. - * - * @param robotMassKg the mass of the robot, is kilograms - * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force equally - */ - public double getModuleMaxAccelerationMPSsq(double robotMassKg, int modulesCount) { - return getTheoreticalPropellingForcePerModule(robotMassKg, modulesCount) * modulesCount / robotMassKg; - } - - /** - * - * - *

    Stores the coefficient of friction of some common used wheels.

    - */ - public enum WHEEL_GRIP { - RUBBER_WHEEL(1.25), - TIRE_WHEEL(1.2); - - public final double cof; - - WHEEL_GRIP(double cof) { - this.cof = cof; - } - } - /** - * creates a SDS - * Mark4-n Swerve Module for simulation - */ - public static Supplier getModule( - DCMotor driveMotor, - DCMotor turnMotor, - double driveCurrentLimitAmps, - WHEEL_GRIP driveWheelType, - double driveGearRatio) { - return () -> - new SwerveModuleSimulation( - driveMotor, - turnMotor, - driveCurrentLimitAmps, - driveGearRatio, - 11.3142, - 0.25, - 0.05, - driveWheelType.cof, - Units.inchesToMeters(2), - 0.05); - } -} \ No newline at end of file + public final DCMotor DRIVE_MOTOR; + private final BrushlessMotorSim turnMotorSim; + public final double DRIVE_CURRENT_LIMIT, + DRIVE_GEAR_RATIO, + TURN_GEAR_RATIO, + DRIVE_FRICTION_VOLTAGE, + WHEELS_COEFFICIENT_OF_FRICTION, + WHEEL_RADIUS_METERS, + DRIVE_WHEEL_INERTIA = 0.01; + private double driveMotorRequestedVolts = 0.0, + driveMotorAppliedVolts = 0.0, + driveMotorSupplyCurrentAmps = 0.0, + turnRelativeEncoderPositionRad = 0.0, + turnRelativeEncoderSpeedRadPerSec = 0.0, + turnAbsoluteEncoderSpeedRadPerSec = 0.0, + driveEncoderUnGearedPositionRad = 0.0, + driveEncoderUnGearedSpeedRadPerSec = 0.0; + private Rotation2d turnAbsolutePosition = Rotation2d.fromRotations(Math.random()); + + private final double turnRelativeEncoderOffSet = (Math.random() - 0.5) * 30; + + private final Queue cachedTurnRelativeEncoderPositionsRad, + cachedDriveEncoderUnGearedPositionsRad; + private final Queue cachedTurnAbsolutePositions; + + /** + * + * + *

    Constructs a Swerve Module Simulation.

    + * + *

    If you are using {@link SimulatedField#overrideSimulationTimings(double, int)} to use custom + * timings, you must call the method before constructing any swerve module simulations using this + * constructor. + * + * @param driveMotor the model of the driving motor + * @param turnMotor the model of the steering motor + * @param driveCurrentLimit the current limit for the driving motor, in amperes + * @param driveGearRatio the gear ratio for the driving motor, >1 is reduction + * @param turnGearRatio the gear ratio for the steering motor, >1 is reduction + * @param driveFrictionVoltage the measured minimum amount of voltage that can turn the driving + * rotter, in volts + * @param turnFrictionVoltage the measured minimum amount of voltage that can turn the steering + * rotter, in volts + * @param tireCoefficientOfFriction the coefficient + * of friction of the tires, normally around 1.5 + * @param wheelsRadiusMeters the radius of the wheels, in meters. Calculate it using {@link + * Units#inchesToMeters(double)}. + * @param turnRotationalInertia the rotational inertia of the steering mechanism + */ + public SwerveModuleSimulation( + DCMotor driveMotor, + DCMotor turnMotor, + double driveCurrentLimit, + double driveGearRatio, + double turnGearRatio, + double driveFrictionVoltage, + double turnFrictionVoltage, + double tireCoefficientOfFriction, + double wheelsRadiusMeters, + double turnRotationalInertia) { + DRIVE_MOTOR = driveMotor; + DRIVE_CURRENT_LIMIT = driveCurrentLimit; + DRIVE_GEAR_RATIO = driveGearRatio; + TURN_GEAR_RATIO = turnGearRatio; + DRIVE_FRICTION_VOLTAGE = driveFrictionVoltage; + WHEELS_COEFFICIENT_OF_FRICTION = tireCoefficientOfFriction; + WHEEL_RADIUS_METERS = wheelsRadiusMeters; + + this.turnMotorSim = + new BrushlessMotorSim( + SimulatedField.getInstance(), + turnMotor, + turnGearRatio, + KilogramSquareMeters.of(turnRotationalInertia), + Volts.of(turnFrictionVoltage)); + + this.cachedDriveEncoderUnGearedPositionsRad = new ConcurrentLinkedQueue<>(); + for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) + cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); + this.cachedTurnRelativeEncoderPositionsRad = new ConcurrentLinkedQueue<>(); + for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) + cachedTurnRelativeEncoderPositionsRad.offer(turnRelativeEncoderPositionRad); + this.cachedTurnAbsolutePositions = new ConcurrentLinkedQueue<>(); + for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) + cachedTurnAbsolutePositions.offer(turnAbsolutePosition); + + this.turnRelativeEncoderPositionRad = + turnAbsolutePosition.getRadians() + turnRelativeEncoderOffSet; + } + + /** + * + * + *

    Requests the Driving Motor to Run at a Specified Voltage Output.

    + * + *

    Think of it as the setVoltage() of your physical driving motor.

    + * + *

    This method sets the desired voltage output for the driving motor. The change will be + * applied in the next sub-tick of the simulation. + * + *

    Note: The requested voltage may not always be fully applied if the current + * is too high. The current limit may reduce the motor's output, similar to real motors. + * + *

    To check the actual voltage applied to the drivetrain, use {@link + * #getDriveMotorAppliedVolts()}. + * + * @param volts the voltage to be applied to the driving motor + */ + public void requestDriveVoltageOut(Voltage volts) { + this.driveMotorRequestedVolts = volts.in(Volts); + } + + /** + * + * + *

    Requests the Steering Motor to Run at a Specified Voltage Output.

    + * + *

    Think of it as the setVoltage() of your physical steering motor.

    + * + *

    This method sets the desired voltage output for the steering motor. The change will be + * applied in the next sub-tick of the simulation. + * + *

    Note: Similar to the drive motor, the requested voltage may not always be + * fully applied if the current exceeds the limit. The current limit will reduce the motor's + * output as needed, mimicking real motor behavior. + * + *

    To check the actual voltage applied to the steering motor, use {@link + * #getTurnMotorAppliedVolts()}. + * + * @param volts the voltage to be applied to the steering motor + */ + public void requestTurnVoltageOut(Voltage volts) { + this.turnMotorSim.setControl(volts); + } + + /** + * + * + *

    Obtains the Actual Output Voltage of the Drive Motor.

    + * + *

    This method returns the actual voltage being applied to the drive motor. The actual applied + * voltage may differ from the value set by {@link #requestDriveVoltageOut(double)}. + * + *

    If the motor's supply current is too high, the motor will automatically reduce its output + * voltage to protect the system. + * + * @return the actual output voltage of the drive motor, in volts + */ + public double getDriveMotorAppliedVolts() { + return driveMotorAppliedVolts; + } + + /** + * + * + *

    Obtains the Actual Output Voltage of the Steering Motor.

    + * + *

    This method returns the actual voltage being applied to the steering motor. It wraps around + * the {@link BrushlessMotorSim#getAppliedVolts()} method. + * + *

    The actual applied voltage may differ from the value set by {@link + * #requestTurnVoltageOut(double)}. If the motor's supply current is too high, the motor will + * automatically reduce its output voltage to protect the system. + * + * @return the actual output voltage of the steering motor, in volts + */ + public double getTurnMotorAppliedVolts() { + return turnMotorSim.getRotorVoltage().in(Volts); + } + + /** + * + * + *

    Obtains the Amount of Current Supplied to the Drive Motor.

    + * + *

    Think of it as the getSupplyCurrent() of your physical drive motor.

    + * + * @return the current supplied to the drive motor, in amperes + */ + public double getDriveMotorSupplyCurrentAmps() { + return driveMotorSupplyCurrentAmps; + } + + /** + * + * + *

    Obtains the Amount of Current Supplied to the Steer Motor.

    + * + *

    Think of it as the getSupplyCurrent() of your physical steer motor.

    + * + *

    This method wraps around {@link BrushlessMotorSim#getCurrentDrawAmps()}. + * + * @return the current supplied to the steer motor, in amperes + */ + public double getTurnMotorSupplyCurrentAmps() { + return turnMotorSim.getSupplyCurrent().in(Amps); + } + + /** + * + * + *

    Obtains the Position of the Drive Encoder.

    + * + *

    Think of it as the getPosition() of your physical driving motor.

    + * + *

    This method is used to simulate your {@link SwerveDrivePoseEstimator}. + * + *

    This value represents the un-geared position of the encoder, i.e., the amount of radians the + * drive motor's encoder has rotated. + * + *

    To get the final wheel rotations, use {@link #getDriveEncoderFinalPositionRad()}. + * + * @return the position of the drive motor's encoder, in radians (un-geared) + */ + public double getDriveEncoderUnGearedPositionRad() { + return driveEncoderUnGearedPositionRad; + } + + /** + * + * + *

    Obtains the Final Position of the Drive Encoder (Wheel Rotations).

    + * + *

    This method is used to simulate the {@link SwerveDrivePoseEstimator} by providing the final + * position of the drive encoder in terms of wheel rotations. + * + *

    The value is calculated by dividing the un-geared encoder position (obtained from {@link + * #getDriveEncoderUnGearedPositionRad()}) by the drive gear ratio. + * + * @return the final position of the drive encoder (wheel rotations), in radians + */ + public double getDriveEncoderFinalPositionRad() { + return getDriveEncoderUnGearedPositionRad(); + // / DRIVE_GEAR_RATIO; + } + + /** + * + * + *

    Obtains the Speed of the Drive Encoder (Un-Geared), in Radians per Second.

    + * + *

    Think of it as the getVelocity() of your physical drive motor.

    + * + *

    This method returns the current speed of the drive encoder in radians per second, without + * accounting for the drive gear ratio. + * + * @return the un-geared speed of the drive encoder, in radians per second + */ + public double getDriveEncoderUnGearedSpeedRadPerSec() { + return driveEncoderUnGearedSpeedRadPerSec; + } + + /** + * + * + *

    Obtains the Final Speed of the Wheel, in Radians per Second.

    + * + * @return the final speed of the drive wheel, in radians per second + */ + public double getDriveWheelFinalSpeedRadPerSec() { + return getDriveEncoderUnGearedSpeedRadPerSec(); + // / DRIVE_GEAR_RATIO; + } + + /** + * + * + *

    Obtains the Relative Position of the Steer Encoder, in Radians.

    + * + *

    Think of it as the getPosition() of your physical steer motor.

    + * + * @return the relative encoder position of the steer motor, in radians + */ + public double getTurnRelativeEncoderPositionRad() { + return turnRelativeEncoderPositionRad; + } + + /** + * + * + *

    Obtains the Speed of the Steer Relative Encoder, in Radians per Second (Geared).

    + * + *

    Think of it as the getVelocity() of your physical steer motor.

    + * + * @return the speed of the steer relative encoder, in radians per second (geared) + */ + public double getTurnRelativeEncoderSpeedRadPerSec() { + return turnRelativeEncoderSpeedRadPerSec; + } + + /** + * + * + *

    Obtains the Absolute Facing of the Steer Mechanism.

    + * + *

    Think of it as the getAbsolutePosition() of your CanCoder.

    + * + * @return the absolute facing of the steer mechanism, as a {@link Rotation2d} + */ + public Rotation2d getTurnAbsolutePosition() { + return turnAbsolutePosition; + } + + /** + * + * + *

    Obtains the Absolute Rotational Velocity of the Steer Mechanism.

    + * + *

    Think of it as the getVelocity() of your CanCoder.

    + * + * @return the absolute angular velocity of the steer mechanism, in radians per second + */ + public double getTurnAbsoluteEncoderSpeedRadPerSec() { + return turnAbsoluteEncoderSpeedRadPerSec; + } + + /** + * + * + *

    Obtains the Cached Readings of the Drive Encoder's Un-Geared Position.

    + * + *

    The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and + * can be retrieved using this method. + * + * @return an array of cached drive encoder un-geared positions, in radians + */ + public double[] getCachedDriveEncoderUnGearedPositionsRad() { + return cachedDriveEncoderUnGearedPositionsRad.stream().mapToDouble(value -> value).toArray(); + } + + /** + * + * + *

    Obtains the Cached Readings of the Drive Encoder's Final Position (Wheel Rotations).

    + * + *

    The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and + * are divided by the gear ratio to obtain the final wheel rotations. + * + * @return an array of cached drive encoder final positions (wheel rotations), in radians + */ + public double[] getCachedDriveWheelFinalPositionsRad() { + return cachedDriveEncoderUnGearedPositionsRad.stream() + .mapToDouble(value -> value * ModuleConstants.DRIVE_TO_METERS) + .toArray(); + } + + /** + * + * + *

    Obtains the Cached Readings of the Steer Relative Encoder's Position.

    + * + *

    The values of {@link #getTurnRelativeEncoderPositionRad()} are cached at each sub-tick and + * can be retrieved using this method. + * + * @return an array of cached steer relative encoder positions, in radians + */ + public double[] getCachedTurnRelativeEncoderPositions() { + return cachedTurnRelativeEncoderPositionsRad.stream().mapToDouble(value -> value).toArray(); + } + + /** + * + * + *

    Obtains the Cached Readings of the Steer Absolute Positions.

    + * + *

    The values of {@link #getTurnAbsolutePosition()} are cached at each sub-tick and can be + * retrieved using this method. + * + * @return an array of cached absolute steer positions, as {@link Rotation2d} objects + */ + public Rotation2d[] getCachedTurnAbsolutePositions() { + return cachedTurnAbsolutePositions.toArray(Rotation2d[]::new); + } + + protected double getGrippingForceNewtons(double gravityForceOnModuleNewtons) { + return gravityForceOnModuleNewtons * WHEELS_COEFFICIENT_OF_FRICTION; + } + + /** + * + * + *

    Updates the Simulation for This Module.

    + * + *

    This method is called once during every sub-tick of the simulation. It performs the + * following actions: + * + *

      + *
    • Updates the simulation of the steering mechanism. + *
    • Simulates the propelling force generated by the module. + *
    • Updates and caches the encoder readings for odometry simulation. + *
    + * + *

    Note: Friction forces are not simulated in this method. + * + * @param moduleCurrentGroundVelocityWorldRelative the current ground velocity of the module, + * relative to the world + * @param robotFacing the absolute facing of the robot, relative to the world + * @param gravityForceOnModuleNewtons the gravitational force acting on this module, in newtons + * @return the propelling force generated by the module, as a {@link Vector2} object + */ + public Vector2 updateSimulationSubTickGetModuleForce( + Vector2 moduleCurrentGroundVelocityWorldRelative, + Rotation2d robotFacing, + double gravityForceOnModuleNewtons) { + updateTurnSimulation(); + + /* the maximum gripping force that the wheel can generate */ + final double grippingForceNewtons = getGrippingForceNewtons(gravityForceOnModuleNewtons); + final Rotation2d moduleWorldFacing = this.turnAbsolutePosition.plus(robotFacing); + final Vector2 propellingForce = + getPropellingForce( + grippingForceNewtons, moduleWorldFacing, moduleCurrentGroundVelocityWorldRelative); + updateDriveEncoders(); + + return propellingForce; + } + + /** + * + * + *

    updates the simulation for the steer mechanism.

    + * + *

    Updates the simulation for the steer mechanism and cache the encoder readings. + * + *

    The steer mechanism is modeled by a {@link BrushlessMotorSim}. + */ + private void updateTurnSimulation() { + /* update the readings of the sensor */ + this.turnAbsolutePosition = new Rotation2d(turnMotorSim.getPosition()); + this.turnRelativeEncoderPositionRad = + turnMotorSim.getPosition().in(Radians) + turnRelativeEncoderOffSet; + this.turnAbsoluteEncoderSpeedRadPerSec = turnMotorSim.getVelocity().in(RadiansPerSecond); + this.turnRelativeEncoderSpeedRadPerSec = turnAbsoluteEncoderSpeedRadPerSec * TURN_GEAR_RATIO; + + /* cache sensor readings to queue for high-frequency odometry */ + this.cachedTurnAbsolutePositions.poll(); + this.cachedTurnAbsolutePositions.offer(turnAbsolutePosition); + this.cachedTurnRelativeEncoderPositionsRad.poll(); + this.cachedTurnRelativeEncoderPositionsRad.offer(turnRelativeEncoderPositionRad); + } + + /** + * + * + *

    Calculates the amount of propelling force that the module generates.

    + * + *

    The swerve module's drive motor will generate a propelling force. + * + *

    For most of the time, that propelling force is directly applied to the drivetrain. And the + * drive wheel runs as fast as the ground velocity + * + *

    However, if the propelling force exceeds the gripping, only the max gripping force is + * applied. The rest of the propelling force will cause the wheel to start skidding and make the + * odometry inaccurate. + * + * @param grippingForceNewtons the amount of gripping force that wheel can generate, in newtons + * @param moduleWorldFacing the current world facing of the module + * @param moduleCurrentGroundVelocity the current ground velocity of the module, world-reference + * @return a vector representing the propelling force that the module generates, world-reference + */ + private Vector2 getPropellingForce( + double grippingForceNewtons, + Rotation2d moduleWorldFacing, + Vector2 moduleCurrentGroundVelocity) { + final double driveWheelTorque = getDriveWheelTorque(), + theoreticalMaxPropellingForceNewtons = driveWheelTorque / WHEEL_RADIUS_METERS; + final boolean skidding = Math.abs(theoreticalMaxPropellingForceNewtons) > grippingForceNewtons; + final double propellingForceNewtons; + if (skidding) + propellingForceNewtons = + Math.copySign(grippingForceNewtons, theoreticalMaxPropellingForceNewtons); + else propellingForceNewtons = theoreticalMaxPropellingForceNewtons; + + final double floorVelocityProjectionOnWheelDirectionMPS = + moduleCurrentGroundVelocity.getMagnitude() + * Math.cos( + moduleCurrentGroundVelocity.getAngleBetween( + new Vector2(moduleWorldFacing.getRadians()))); + + // if the chassis is tightly gripped on floor, the floor velocity is projected to the wheel + this.driveEncoderUnGearedSpeedRadPerSec = + floorVelocityProjectionOnWheelDirectionMPS / WHEEL_RADIUS_METERS * DRIVE_GEAR_RATIO; + + // if the module is skidding + if (skidding) + this.driveEncoderUnGearedSpeedRadPerSec = + 0.5 * driveEncoderUnGearedSpeedRadPerSec + + 0.5 * DRIVE_MOTOR.getSpeed(0, driveMotorAppliedVolts); + + return Vector2.create(propellingForceNewtons, moduleWorldFacing.getRadians()); + } + + /** + * + * + *

    Calculates the amount of torque that the drive motor can generate on the wheel.

    + * + *

    Before calculating the torque of the motor, the output voltage of the drive motor is + * constrained for the current limit through {@link + * BrushlessMotorSim#constrainOutputVoltage(DCMotor, double, double, double)}. + * + * @return the amount of torque on the wheel by the drive motor, in Newton * Meters + */ + private double getDriveWheelTorque() { + driveMotorAppliedVolts = driveMotorRequestedVolts; + + /* calculate the actual supply current */ + driveMotorSupplyCurrentAmps = + DRIVE_MOTOR.getCurrent( + this.driveEncoderUnGearedSpeedRadPerSec, + MathUtil.applyDeadband(driveMotorAppliedVolts, DRIVE_FRICTION_VOLTAGE, 12)); + + /* calculate the torque generated, */ + final double torqueOnRotter = DRIVE_MOTOR.getTorque(driveMotorSupplyCurrentAmps); + return torqueOnRotter * DRIVE_GEAR_RATIO; + } + + /** + * @return the current module state of this simulation module + */ + public SwerveModuleState getCurrentState() { + return new SwerveModuleState( + getDriveWheelFinalSpeedRadPerSec() * WHEEL_RADIUS_METERS, turnAbsolutePosition); + } + + /** + * + * + *

    Obtains the "free spin" state of the module

    + * + *

    The "free spin" state of a simulated module refers to its state after spinning freely for a + * long time under the current input voltage + * + * @return the free spinning module state + */ + protected SwerveModuleState getFreeSpinState() { + return new SwerveModuleState( + DRIVE_MOTOR.getSpeed( + DRIVE_MOTOR.getTorque(DRIVE_MOTOR.getCurrent(0, DRIVE_FRICTION_VOLTAGE)), + driveMotorAppliedVolts) + / DRIVE_GEAR_RATIO + * WHEEL_RADIUS_METERS, + turnAbsolutePosition); + } + + /** + * + * + *

    Cache the encoder values.

    + * + *

    An internal method to cache the encoder values to their queues. + */ + private void updateDriveEncoders() { + this.driveEncoderUnGearedPositionRad += + this.driveEncoderUnGearedSpeedRadPerSec * SimulatedField.getSimulationDt(); + this.cachedDriveEncoderUnGearedPositionsRad.poll(); + this.cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); + } + + /** + * + * + *

    Obtains the theoretical speed that the module can achieve.

    + * + * @return the theoretical maximum ground speed that the module can achieve, in m/s + */ + public double getModuleTheoreticalSpeedMPS() { + return DRIVE_MOTOR.freeSpeedRadPerSec / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS; + } + + /** + * + * + *

    Obtains the theoretical maximum propelling force of ONE module.

    + * + *

    Calculates the maximum propelling force with respect to the gripping force and the drive + * motor's torque under its current limit. + * + * @param robotMassKg the mass of the robot, is kilograms + * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force + * equally + * @return the maximum propelling force of EACH module + */ + public double getTheoreticalPropellingForcePerModule(double robotMassKg, int modulesCount) { + final double + maxThrustNewtons = + DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS, + maxGrippingNewtons = 9.8 * robotMassKg / modulesCount * WHEELS_COEFFICIENT_OF_FRICTION; + + return Math.min(maxThrustNewtons, maxGrippingNewtons); + } + + /** + * + * + *

    Obtains the theatrical linear acceleration that the robot can achieve.

    + * + *

    Calculates the maximum linear acceleration of a robot, with respect to its mass and {@link + * #getTheoreticalPropellingForcePerModule(double, int)}. + * + * @param robotMassKg the mass of the robot, is kilograms + * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force + * equally + */ + public double getModuleMaxAccelerationMPSsq(double robotMassKg, int modulesCount) { + return getTheoreticalPropellingForcePerModule(robotMassKg, modulesCount) + * modulesCount + / robotMassKg; + } + + /** + * + * + *

    Stores the coefficient of friction of some common used wheels.

    + */ + public enum WHEEL_GRIP { + RUBBER_WHEEL(1.25), + TIRE_WHEEL(1.2); + + public final double cof; + + WHEEL_GRIP(double cof) { + this.cof = cof; + } + } + + /** + * creates a SDS + * Mark4-n Swerve Module for simulation + */ + public static Supplier getModule( + DCMotor driveMotor, + DCMotor turnMotor, + double driveCurrentLimitAmps, + WHEEL_GRIP driveWheelType, + double driveGearRatio) { + return () -> + new SwerveModuleSimulation( + driveMotor, + turnMotor, + driveCurrentLimitAmps, + driveGearRatio, + 11.3142, + 0.25, + 0.05, + driveWheelType.cof, + Units.inchesToMeters(2), + 0.05); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index cdd101e7..beb5b554 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -53,16 +53,16 @@ private void updateOdometryPositions() { double positionMeters = inputs.odometryDriveWheelRevolutions[i]; Rotation2d angle = inputs.odometrySteerPositions[i]; odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); - - SmartDashboard.putNumber("updated drive position", positionMeters); - SmartDashboard.putNumber("updated angle", angle.getDegrees()); + + SmartDashboard.putNumber("updated drive position", positionMeters); + SmartDashboard.putNumber("updated angle", angle.getDegrees()); } } - private double driveRevolutionsToMeters(double driveWheelRevolutions) { - return Rotations.of(driveWheelRevolutions).in(Radians) * SimulationConstants.WHEEL_RADIUS_METERS; -} + return Rotations.of(driveWheelRevolutions).in(Radians) + * SimulationConstants.WHEEL_RADIUS_METERS; + } public void setVoltage(Voltage volts) { io.setDriveVoltage(volts); diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index 0b17fe91..312853b9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -14,24 +14,21 @@ import frc.robot.extras.simulation.OdometryTimestampsSim; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; -import frc.robot.subsystems.swerve.SwerveConstants.SimulationConstants; - import java.util.Arrays; /** Wrapper class around {@link SwerveModuleSimulation} */ public class SimulatedModule implements ModuleInterface { private final SwerveModuleSimulation moduleSimulation; - private final PIDController drivePID = new PIDController(.05, 0, 0); - private final SimpleMotorFeedforward driveFF = - new SimpleMotorFeedforward(0.0, .0, 0.0); + private final PIDController drivePID = new PIDController(.07, 0, 0); + private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.0, .0, 0.0); private final Constraints turnConstraints = new Constraints( RadiansPerSecond.of(2 * Math.PI).in(RotationsPerSecond), RadiansPerSecondPerSecond.of(4 * Math.PI).in(RotationsPerSecondPerSecond)); private final ProfiledPIDController turnPID = - new ProfiledPIDController(Radians.of(10).in(Rotations), 0, 0, turnConstraints); + new ProfiledPIDController(Radians.of(3.5).in(Rotations), 0, 0, turnConstraints); private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(0.77, 0.75, 0); public SimulatedModule(SwerveModuleSimulation moduleSimulation) { @@ -64,8 +61,12 @@ public void updateInputs(ModuleInputs inputs) { inputs.isConnected = true; - SmartDashboard.putNumber("turn absolute position", moduleSimulation.getTurnAbsolutePosition().getDegrees()); - SmartDashboard.putNumber("drive pos", Radians.of(moduleSimulation.getDriveEncoderFinalPositionRad()).in(Rotations) * ModuleConstants.DRIVE_TO_METERS); + SmartDashboard.putNumber( + "turn absolute position", moduleSimulation.getTurnAbsolutePosition().getDegrees()); + SmartDashboard.putNumber( + "drive pos", + Radians.of(moduleSimulation.getDriveEncoderFinalPositionRad()).in(Rotations) + * ModuleConstants.DRIVE_TO_METERS); } @Override @@ -81,25 +82,24 @@ public void setTurnVoltage(Voltage volts) { @Override public void setDesiredState(SwerveModuleState desiredState) { double turnRotations = getTurnRotations(); - // Optimize the reference state to avoid spinning further than 90 degrees - SwerveModuleState setpoint = - new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); - - setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); - setpoint.optimize(Rotation2d.fromRotations(turnRotations)); + // setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); + desiredState.optimize(Rotation2d.fromRotations(turnRotations)); - if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) { + if (Math.abs(desiredState.speedMetersPerSecond) < 0.01) { stopModule(); return; } // Converts meters per second to rotations per second double desiredDriveRPS = - setpoint.speedMetersPerSecond + desiredState.speedMetersPerSecond * ModuleConstants.DRIVE_GEAR_RATIO / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; SmartDashboard.putNumber("desired drive RPS", desiredDriveRPS); - SmartDashboard.putNumber("current drive RPS", RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()).in(RotationsPerSecond)); + SmartDashboard.putNumber( + "current drive RPS", + RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) + .in(RotationsPerSecond)); SmartDashboard.putNumber("desired angle", desiredState.angle.getDegrees()); moduleSimulation.requestDriveVoltageOut( From ef0232b4f0d1527570300f27e579b522286f0242 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Wed, 6 Nov 2024 11:39:55 -0500 Subject: [PATCH 10/75] huh --- .../swerve/moduleIO/SimulatedModule.java | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index 312853b9..0f98ba57 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -20,7 +20,7 @@ public class SimulatedModule implements ModuleInterface { private final SwerveModuleSimulation moduleSimulation; - private final PIDController drivePID = new PIDController(.07, 0, 0); + private final PIDController drivePID = new PIDController(.04, 0, 0); private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.0, .0, 0.0); private final Constraints turnConstraints = @@ -28,7 +28,7 @@ public class SimulatedModule implements ModuleInterface { RadiansPerSecond.of(2 * Math.PI).in(RotationsPerSecond), RadiansPerSecondPerSecond.of(4 * Math.PI).in(RotationsPerSecondPerSecond)); private final ProfiledPIDController turnPID = - new ProfiledPIDController(Radians.of(3.5).in(Rotations), 0, 0, turnConstraints); + new ProfiledPIDController(Radians.of(35).in(Rotations), 0, 0, turnConstraints); private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(0.77, 0.75, 0); public SimulatedModule(SwerveModuleSimulation moduleSimulation) { @@ -85,16 +85,18 @@ public void setDesiredState(SwerveModuleState desiredState) { // setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); desiredState.optimize(Rotation2d.fromRotations(turnRotations)); - if (Math.abs(desiredState.speedMetersPerSecond) < 0.01) { - stopModule(); - return; - } - + // Converts meters per second to rotations per second double desiredDriveRPS = desiredState.speedMetersPerSecond * ModuleConstants.DRIVE_GEAR_RATIO / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; + + if (Math.abs(desiredDriveRPS) < 0.01) { + stopModule(); + return; + } + SmartDashboard.putNumber("desired drive RPS", desiredDriveRPS); SmartDashboard.putNumber( "current drive RPS", From be16c07585e064be03bd26417c0e42a8ece69644 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 7 Nov 2024 15:59:37 -0500 Subject: [PATCH 11/75] push changes --- .../mechanismSim/swerve/SwerveModuleSimulation.java | 2 +- src/main/java/frc/robot/subsystems/swerve/SwerveModule.java | 2 +- .../frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java index 87037b71..9b659532 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java @@ -405,7 +405,7 @@ public double[] getCachedDriveEncoderUnGearedPositionsRad() { */ public double[] getCachedDriveWheelFinalPositionsRad() { return cachedDriveEncoderUnGearedPositionsRad.stream() - .mapToDouble(value -> value * ModuleConstants.DRIVE_TO_METERS) + .mapToDouble(value -> value / ModuleConstants.DRIVE_GEAR_RATIO) .toArray(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index beb5b554..7ca3e3f6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -52,7 +52,7 @@ private void updateOdometryPositions() { for (int i = 0; i < odometryPositions.length; i++) { double positionMeters = inputs.odometryDriveWheelRevolutions[i]; Rotation2d angle = inputs.odometrySteerPositions[i]; - odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + odometryPositions[i] = new SwerveModulePosition(driveRevolutionsToMeters(positionMeters), angle); SmartDashboard.putNumber("updated drive position", positionMeters); SmartDashboard.putNumber("updated angle", angle.getDegrees()); diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index 0f98ba57..4189ff52 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -20,8 +20,8 @@ public class SimulatedModule implements ModuleInterface { private final SwerveModuleSimulation moduleSimulation; - private final PIDController drivePID = new PIDController(.04, 0, 0); - private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.0, .0, 0.0); + private final PIDController drivePID = new PIDController(.06, 0, 0); + private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.12, .012, 0.0012); private final Constraints turnConstraints = new Constraints( From 71d40407894b9e0cede6afd4e8a86da89c38b83a Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Fri, 8 Nov 2024 10:57:46 -0500 Subject: [PATCH 12/75] idk what this is --- .../mechanismSim/swerve/SwerveModuleSimulation.java | 2 +- .../frc/robot/subsystems/swerve/SwerveDrive.java | 1 + .../frc/robot/subsystems/swerve/SwerveModule.java | 5 ++--- .../subsystems/swerve/moduleIO/SimulatedModule.java | 12 ++++++------ 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java index 9b659532..a418955f 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java @@ -405,7 +405,7 @@ public double[] getCachedDriveEncoderUnGearedPositionsRad() { */ public double[] getCachedDriveWheelFinalPositionsRad() { return cachedDriveEncoderUnGearedPositionsRad.stream() - .mapToDouble(value -> value / ModuleConstants.DRIVE_GEAR_RATIO) + .mapToDouble(value -> value) .toArray(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index ff38a99c..61bcddec 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -217,6 +217,7 @@ public double getAllianceAngleOffset() { public void setModuleStates(SwerveModuleState[] desiredStates) { for (int i = 0; i < 4; i++) { swerveModules[i].runSetPoint(desiredStates[i]); + Logger.recordOutput("SwerveStates/desiredStatesagain", desiredStates[i]); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 7ca3e3f6..49c91ae2 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -54,14 +54,13 @@ private void updateOdometryPositions() { Rotation2d angle = inputs.odometrySteerPositions[i]; odometryPositions[i] = new SwerveModulePosition(driveRevolutionsToMeters(positionMeters), angle); - SmartDashboard.putNumber("updated drive position", positionMeters); + SmartDashboard.putNumber("updated drive position", driveRevolutionsToMeters(positionMeters)); SmartDashboard.putNumber("updated angle", angle.getDegrees()); } } private double driveRevolutionsToMeters(double driveWheelRevolutions) { - return Rotations.of(driveWheelRevolutions).in(Radians) - * SimulationConstants.WHEEL_RADIUS_METERS; + return driveWheelRevolutions * ModuleConstants.DRIVE_TO_METERS; } public void setVoltage(Voltage volts) { diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index 4189ff52..0b5ca4c5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -20,8 +20,8 @@ public class SimulatedModule implements ModuleInterface { private final SwerveModuleSimulation moduleSimulation; - private final PIDController drivePID = new PIDController(.06, 0, 0); - private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.12, .012, 0.0012); + private final PIDController drivePID = new PIDController(.005, 0, 0); + private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.03155, 0.010831, 0.005); private final Constraints turnConstraints = new Constraints( @@ -82,7 +82,7 @@ public void setTurnVoltage(Voltage volts) { @Override public void setDesiredState(SwerveModuleState desiredState) { double turnRotations = getTurnRotations(); - // setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); + // desiredState.cosineScale(Rotation2d.fromRotations(turnRotations)); desiredState.optimize(Rotation2d.fromRotations(turnRotations)); @@ -92,7 +92,7 @@ public void setDesiredState(SwerveModuleState desiredState) { * ModuleConstants.DRIVE_GEAR_RATIO / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; - if (Math.abs(desiredDriveRPS) < 0.01) { + if (Math.abs(desiredState.speedMetersPerSecond) < 0.01) { stopModule(); return; } @@ -126,7 +126,7 @@ public double getTurnRotations() { @Override public void stopModule() { - moduleSimulation.requestDriveVoltageOut(Volts.of(0)); - moduleSimulation.requestTurnVoltageOut(Volts.of(0)); + moduleSimulation.requestDriveVoltageOut(Volts.zero()); + moduleSimulation.requestTurnVoltageOut(Volts.zero()); } } From bb468600f39a5b6155cc2cf42d316d786b8b580c Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Tue, 12 Nov 2024 10:04:54 -0500 Subject: [PATCH 13/75] yall are cooked --- src/main/java/frc/robot/RobotContainer.java | 12 +++--- .../swerve/SwerveModuleSimulation.java | 9 ++-- .../subsystems/swerve/SwerveConstants.java | 4 +- .../robot/subsystems/swerve/SwerveDrive.java | 33 +++++++------- .../robot/subsystems/swerve/SwerveModule.java | 22 +++------- .../swerve/moduleIO/SimulatedModule.java | 43 ++++++------------- 6 files changed, 47 insertions(+), 76 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 869a218d..6d929f68 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -45,7 +45,7 @@ public class RobotContainer { // Simulation, we store them here in the robot container // private final SimulatedField simulatedArena; private final SwerveDriveSimulation swerveDriveSimulation; - private final frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation gyroSimulation; + private final GyroSimulation gyroSimulation; // Subsystems // private final XboxController driverController = new XboxController(0); @@ -79,7 +79,7 @@ public RobotContainer() { /* create a swerve drive simulation */ this.swerveDriveSimulation = new SwerveDriveSimulation( - 45, + 60, DriveConstants.TRACK_WIDTH, DriveConstants.WHEEL_BASE, DriveConstants.TRACK_WIDTH + .2, @@ -141,8 +141,8 @@ private void resetFieldAndOdometryForAuto(Pose2d robotStartingPoseAtBlueAlliance updateFieldSimAndDisplay(); } - swerveDrive.periodic(); - swerveDrive.setPose(startingPose); + // swerveDrive.periodic(); + swerveDrive.resetPosition(startingPose); } private static double deadband(double value, double deadband) { @@ -284,7 +284,7 @@ private void configureButtonBindings() { driverRightDirectionPad.onTrue( new InstantCommand( () -> - swerveDrive.setPose( + swerveDrive.resetPosition( new Pose2d( swerveDrive.getPose().getX(), swerveDrive.getPose().getY(), @@ -340,7 +340,7 @@ private void configureButtonBindings() { public Command getAutonomousCommand() { // Resets the pose factoring in the robot side // This is just a failsafe, pose should be reset at the beginning of auto - swerveDrive.setPose( + swerveDrive.resetPosition( new Pose2d( swerveDrive.getPose().getX(), swerveDrive.getPose().getY(), diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java index a418955f..dfe9d8d5 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java @@ -15,7 +15,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; import frc.robot.extras.simulation.field.SimulatedField; -import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; import java.util.Queue; import java.util.concurrent.ConcurrentLinkedQueue; import java.util.function.Supplier; @@ -295,8 +294,7 @@ public double getDriveEncoderUnGearedPositionRad() { * @return the final position of the drive encoder (wheel rotations), in radians */ public double getDriveEncoderFinalPositionRad() { - return getDriveEncoderUnGearedPositionRad(); - // / DRIVE_GEAR_RATIO; + return getDriveEncoderUnGearedPositionRad() / DRIVE_GEAR_RATIO; } /** @@ -323,8 +321,7 @@ public double getDriveEncoderUnGearedSpeedRadPerSec() { * @return the final speed of the drive wheel, in radians per second */ public double getDriveWheelFinalSpeedRadPerSec() { - return getDriveEncoderUnGearedSpeedRadPerSec(); - // / DRIVE_GEAR_RATIO; + return getDriveEncoderUnGearedSpeedRadPerSec() / DRIVE_GEAR_RATIO; } /** @@ -405,7 +402,7 @@ public double[] getCachedDriveEncoderUnGearedPositionsRad() { */ public double[] getCachedDriveWheelFinalPositionsRad() { return cachedDriveEncoderUnGearedPositionsRad.stream() - .mapToDouble(value -> value) + .mapToDouble(value -> value / DRIVE_GEAR_RATIO) .toArray(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index a9409a1e..aa2195ed 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -82,8 +82,8 @@ public static final class DriveConstants { public static final InvertedValue REAR_RIGHT_DRIVE_ENCODER_REVERSED = InvertedValue.CounterClockwise_Positive; - public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 6 * Math.PI; - public static final double LOW_ANGULAR_SPEED_RADIANS_PER_SECOND = 3 * Math.PI; + public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 3 * Math.PI; + public static final double LOW_ANGULAR_SPEED_RADIANS_PER_SECOND = 2 * Math.PI; public static final double MAX_SPEED_METERS_PER_SECOND = 4.5; public static final double MAX_SHOOT_SPEED_METERS_PER_SECOND = 3; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 61bcddec..e430fe9b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.extras.util.DeviceCANBus; import frc.robot.extras.util.TimeUtil; @@ -71,7 +72,7 @@ public SwerveDrive( this.poseEstimator = new SwerveDrivePoseEstimator( DriveConstants.DRIVE_KINEMATICS, - rawGyroRotation, + getRawGyroYaw(), lastModulePositions, new Pose2d(), VecBuilder.fill( @@ -97,12 +98,12 @@ public double getGyroRate() { return gyroInputs.yawVelocity; } - /** Updates the pose estimator with the pose calculated from the swerve modules. */ - public void addPoseEstimatorSwerveMeasurement() { - for (int timestampIndex = 0; - timestampIndex < odometryThreadInputs.measurementTimeStamps.length; - timestampIndex++) addPoseEstimatorSwerveMeasurement(timestampIndex); - } + // /** Updates the pose estimator with the pose calculated from the swerve modules. */ + // public void addPoseEstimatorSwerveMeasurement() { + // for (int timestampIndex = 0; + // timestampIndex < odometryThreadInputs.measurementTimeStamps.length; + // timestampIndex++) addPoseEstimatorSwerveMeasurement(timestampIndex); + // } /* * Updates the pose estimator with the pose calculated from the april tags. How much it @@ -217,7 +218,6 @@ public double getAllianceAngleOffset() { public void setModuleStates(SwerveModuleState[] desiredStates) { for (int i = 0; i < 4; i++) { swerveModules[i].runSetPoint(desiredStates[i]); - Logger.recordOutput("SwerveStates/desiredStatesagain", desiredStates[i]); } } @@ -226,21 +226,21 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { * * @param timestampIndex index of the timestamp to sample the pose at */ - private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { - final SwerveModulePosition[] modulePositions = getModulesPosition(timestampIndex), + public void addPoseEstimatorSwerveMeasurement() { + final SwerveModulePosition[] modulePositions = getModulePositions(), moduleDeltas = getModulesDelta(modulePositions); if (gyroInputs.isConnected) { - rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; + // rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; + rawGyroRotation = getRawGyroYaw(); } else { Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas); rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } poseEstimator.updateWithTime( - odometryThreadInputs.measurementTimeStamps[timestampIndex], - rawGyroRotation, - modulePositions); + // odometryThreadInputs.measurementTimeStamps[timestampIndex], + Timer.getFPGATimestamp(), rawGyroRotation, modulePositions); } /** @@ -302,7 +302,10 @@ public Rotation2d getRawGyroYaw() { * * @param pose pose to set */ - public void setPose(Pose2d pose) { + public void resetPosition(Pose2d pose) { + // for (int timestampIndex = 0; + // timestampIndex < odometryThreadInputs.measurementTimeStamps.length; + // timestampIndex++) poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 49c91ae2..0fcee643 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -7,11 +7,9 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; -import frc.robot.subsystems.swerve.SwerveConstants.SimulationConstants; import frc.robot.subsystems.swerve.moduleIO.ModuleInputsAutoLogged; import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; import org.littletonrobotics.junction.Logger; @@ -50,19 +48,13 @@ public void periodic() { private void updateOdometryPositions() { odometryPositions = new SwerveModulePosition[inputs.odometryDriveWheelRevolutions.length]; for (int i = 0; i < odometryPositions.length; i++) { - double positionMeters = inputs.odometryDriveWheelRevolutions[i]; + double positionMeters = + inputs.odometryDriveWheelRevolutions[i] * ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; Rotation2d angle = inputs.odometrySteerPositions[i]; - odometryPositions[i] = new SwerveModulePosition(driveRevolutionsToMeters(positionMeters), angle); - - SmartDashboard.putNumber("updated drive position", driveRevolutionsToMeters(positionMeters)); - SmartDashboard.putNumber("updated angle", angle.getDegrees()); + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); } } - private double driveRevolutionsToMeters(double driveWheelRevolutions) { - return driveWheelRevolutions * ModuleConstants.DRIVE_TO_METERS; - } - public void setVoltage(Voltage volts) { io.setDriveVoltage(volts); io.setTurnVoltage(Volts.zero()); @@ -92,12 +84,12 @@ public double getTurnVelocity() { /** Returns the current drive position of the module in meters. */ public double getDrivePositionMeters() { - return ModuleConstants.DRIVE_TO_METERS * inputs.drivePosition; + return ModuleConstants.WHEEL_CIRCUMFERENCE_METERS * inputs.drivePosition; } /** Returns the current drive velocity of the module in meters per second. */ public double getDriveVelocityMetersPerSec() { - return ModuleConstants.DRIVE_TO_METERS_PER_SECOND * inputs.driveVelocity; + return ModuleConstants.WHEEL_CIRCUMFERENCE_METERS * inputs.driveVelocity; } /** Returns the module state (turn angle and drive velocity). */ @@ -117,8 +109,6 @@ public SwerveModulePosition[] getOdometryPositions() { * @return a SwerveModulePosition object containing position and rotation */ public SwerveModulePosition getPosition() { - double position = ModuleConstants.DRIVE_TO_METERS * getDrivePositionMeters(); - Rotation2d rotation = getTurnRotation(); - return new SwerveModulePosition(position, rotation); + return new SwerveModulePosition(getDrivePositionMeters(), getTurnRotation()); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index 0b5ca4c5..717e1f28 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -8,40 +8,39 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.extras.simulation.OdometryTimestampsSim; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; -import java.util.Arrays; /** Wrapper class around {@link SwerveModuleSimulation} */ public class SimulatedModule implements ModuleInterface { private final SwerveModuleSimulation moduleSimulation; - private final PIDController drivePID = new PIDController(.005, 0, 0); - private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.03155, 0.010831, 0.005); + private final PIDController drivePID = new PIDController(.05, 0, 0); + private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.1, 0.13); private final Constraints turnConstraints = new Constraints( - RadiansPerSecond.of(2 * Math.PI).in(RotationsPerSecond), - RadiansPerSecondPerSecond.of(4 * Math.PI).in(RotationsPerSecondPerSecond)); + RadiansPerSecond.of(2 * Math.PI).in(RotationsPerSecond) * 0, + RadiansPerSecondPerSecond.of(4 * Math.PI).in(RotationsPerSecondPerSecond) * 0); private final ProfiledPIDController turnPID = new ProfiledPIDController(Radians.of(35).in(Rotations), 0, 0, turnConstraints); private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(0.77, 0.75, 0); public SimulatedModule(SwerveModuleSimulation moduleSimulation) { this.moduleSimulation = moduleSimulation; - turnPID.enableContinuousInput(-Math.PI, Math.PI); + turnPID.enableContinuousInput( + Radians.of(-Math.PI).in(Rotations), Radians.of(Math.PI).in(Rotations)); } @Override public void updateInputs(ModuleInputs inputs) { inputs.drivePosition = - Units.radiansToRotations(moduleSimulation.getDriveEncoderFinalPositionRad()); + Radians.of(moduleSimulation.getDriveEncoderFinalPositionRad()).in(Rotations); inputs.driveVelocity = - Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()); + RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) + .in(RotationsPerSecond); inputs.driveAppliedVolts = moduleSimulation.getDriveMotorAppliedVolts(); inputs.driveCurrentAmps = moduleSimulation.getDriveMotorSupplyCurrentAmps(); @@ -50,23 +49,13 @@ public void updateInputs(ModuleInputs inputs) { inputs.turnAppliedVolts = moduleSimulation.getTurnMotorAppliedVolts(); inputs.turnCurrentAmps = moduleSimulation.getTurnMotorSupplyCurrentAmps(); - inputs.odometryDriveWheelRevolutions = - Arrays.stream(moduleSimulation.getCachedDriveWheelFinalPositionsRad()) - .map(Units::radiansToRotations) - .toArray(); + inputs.odometryDriveWheelRevolutions = moduleSimulation.getCachedDriveWheelFinalPositionsRad(); inputs.odometrySteerPositions = moduleSimulation.getCachedTurnAbsolutePositions(); inputs.odometryTimestamps = OdometryTimestampsSim.getTimestamps(); inputs.isConnected = true; - - SmartDashboard.putNumber( - "turn absolute position", moduleSimulation.getTurnAbsolutePosition().getDegrees()); - SmartDashboard.putNumber( - "drive pos", - Radians.of(moduleSimulation.getDriveEncoderFinalPositionRad()).in(Rotations) - * ModuleConstants.DRIVE_TO_METERS); } @Override @@ -82,10 +71,8 @@ public void setTurnVoltage(Voltage volts) { @Override public void setDesiredState(SwerveModuleState desiredState) { double turnRotations = getTurnRotations(); - // desiredState.cosineScale(Rotation2d.fromRotations(turnRotations)); desiredState.optimize(Rotation2d.fromRotations(turnRotations)); - // Converts meters per second to rotations per second double desiredDriveRPS = desiredState.speedMetersPerSecond @@ -97,18 +84,12 @@ public void setDesiredState(SwerveModuleState desiredState) { return; } - SmartDashboard.putNumber("desired drive RPS", desiredDriveRPS); - SmartDashboard.putNumber( - "current drive RPS", - RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) - .in(RotationsPerSecond)); - SmartDashboard.putNumber("desired angle", desiredState.angle.getDegrees()); - moduleSimulation.requestDriveVoltageOut( Volts.of( drivePID.calculate( RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) - .in(RotationsPerSecond), + .in(RotationsPerSecond) + * ModuleConstants.WHEEL_CIRCUMFERENCE_METERS, desiredDriveRPS)) .plus(driveFF.calculate(RotationsPerSecond.of(desiredDriveRPS)))); moduleSimulation.requestTurnVoltageOut( From ffc77ad0d3d7b3179d5cbf5dbc710f7763ad9cb7 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Wed, 13 Nov 2024 15:57:08 -0500 Subject: [PATCH 14/75] changes --- ascopeAssets/Robot_Offseason/model.glb | Bin 31696352 -> 31677996 bytes src/main/java/frc/robot/RobotContainer.java | 1 + .../mechanismSim/swerve/GyroSimulation.java | 2 +- .../robot/subsystems/swerve/SwerveDrive.java | 18 +++++++----- .../robot/subsystems/swerve/SwerveModule.java | 27 +++++++++++------- .../swerve/gyroIO/SimulatedGyro.java | 1 + .../swerve/moduleIO/PhysicalModule.java | 20 ++++++------- .../swerve/moduleIO/SimulatedModule.java | 16 +++-------- 8 files changed, 43 insertions(+), 42 deletions(-) diff --git a/ascopeAssets/Robot_Offseason/model.glb b/ascopeAssets/Robot_Offseason/model.glb index ab693a6451dd7cf754c5e44cc670b353553eb567..8da9ea593394e0794d12c9161ad01ce73365ceeb 100644 GIT binary patch delta 57986 zcmdSC2Y3|a+Bg2r?9MEhN)LS#Km-EWUML|Hfl!0(*nj|Hp$0{y6YPjUU?4Z000K%& zLW>|JfS`av5W7^c;!=y#?bM1}+O@l?9isDzQi>L=UMzcqACu^z}%DX0J|$&}Yq zvTB3s?2&^WeF;Y=DM8zkkfe=CQnl=)0x75+IMzgKTKAyjpFbysI^GTfsSwUhNX$zGQ>wpOyXqQO4dA7E;ewdoBHNM1@l4M7wHW3-`-4q%2y zJJqkT)~)eD8LU@Jz_3>KLz0Ka_Ql7<&5s!?9Ra^EYLmPb)qb z;=HQX^|tNWkrquLrA@NOEjGE_z0UlWg_4iPJ|-p8oL-WuebjQVCFpPqK1*7?u6P`7 zmxr7$LL2wCYo_f=T`v1PVBM}czK_wKXu6bZlC+_o9Wci1Z=eH*w+y0j3Pzjwj#&KA z#n@UYTDd<~tLNVe!=kaBVTxr>g1$j@Abo$4C4dQ6{+_J87uYEU=|B}MTM#^C@wmb~ zuXL%}(N@*8SKH2(yn^??wL2sSvC$k|DBau7vUpuSIWx#HHhRna~6t5+A+Aaq`)|+<9xC^vi=(G=OELSMI>?g`qOuInsLSAY(rLz8{F#b{66 zzRlut>Bils+W{Fu9eW=rbar1R`=Ha6cI|TagC<`^U1GGeJ$CS6SwDw%ICC2-nZ9@) zGnVz-N~eB<>a%`|(Z=?A6M`#E2P28uw`fCp?~*+pp@Zy^@mjAwC2}ArthH(119A|S zHyrfX`n?4sJoOf|mh)gOtu#}apS4}~ICVwlt%}h;&N^c8I{ZvN+$K9-E9<|DeUM#c z;~fVrE=NEs&P~%+-nmou>cNQ&7iYuSO7j|lqWkVXq%~fYqNU&S28a~w=G~d7wfY;} zPPZwtFOcFi<=#Uqk1w9ZX4xYXw5#`SXLn+U=e^I}x6|T*p|FgYt0VVsgMQ~dgV9SL zfcwD6PxWi5J@;S<9F00X4GwwO$N|tJLKB;=&ZbrCKX3;OQ+Kx(gIILJ<_sxljkCAG zMyf5x%KylI4UXy417qpn!*b9~Uu;A`-Z11KM692mGqjjF=^uv)Lmt{@L0lI`u9f#o z(7F%Xfyk;yQyjK_c!%tR&A}`wZwUM`<`IY?g5L5+wub1KEc+18&#|0t(MF8i$s~*T$pW}z6qLi^9zk&QL95FF zk7NZ_8}pXzbW*j)u^8(kCPz?~+zrRP&QCeJBo{f3!o=7Xj*Z17~O*IxD9<#NFI z%$hu(yB)rZt&kF!>BBR&Ycn2C(Uv}anDYncYaTZ58BmV^YpCOiXJ3~*+JUvYpdNS* z)`*M9z0T*~wgeF2PW4OETD<_J>!D!&i$^R$GuGxjSXcY?#qF|RPk|?9?&sKjXa zc8k##=hx6aT{s8JXoJ6Ptx^6a)`0Lto`Pdbz@fAFX>q*PZ_ygWXv*n_-GgW1v?hyR zH6oJu;@yXir%l2CQek1;K@$|&< zt?V9OB(ghz>3dcb@bTDTIA-xSUAX~%=@Dvw^rLv~&y{On9Y&-`ToA9FE_g+Tv_)mW z#IIZRip7Pxi_M1@oVm@a;&ONe)Ycjul7a|J8&4$ojITU>RQDm-6;cSAIT*M}<2Lmo( zIGI-|MiG;5my@)z;y2}>OH4&I5u?4nbvG5)!Uf02N!sddi)FuIvw7>2v=Q5v$$k*Y z36)jo+3^aFMHRq8tGaW&Mrql6sHFd zAVE75P;wwa#%e*<_iOLH-BT2a2vcmTy^pRzp(tWC&uVmRjl~yq z==JHVL*lickFB@*oThya+#ahfeW#Xo_V{cIdYurxOW)aqV?B(pL*N*+eDNHdm#ys1 z6R*nNfY4;alN)58VJ|@4tc6HTaoiZT|rEcqaPJ= zaMqK8<6}fpw^J`I2X2qkYJIX4Cs=OD+WVjELW30$Wl7PeJ7hmdMS-g=?U|tEfA%WM zKcAl2a(cySqd#AdLRydifiv)Lo!^AB`?zrvbzS*cBt!l5sdESjqV3SK-;2|>oiAi% zvDva5=6~@DTQRHu-Y;M0!n&Fb+_CgKezh8wcVG?or!lBgk3D zu5I{cJJP5hKAfWEyc(zFeGAFLwhNQByT99I379I*SQD>x{C+FSYY%K78KE{_Yx2XZ z94qEx3nu2;53e9DAj{FTg(zy?`f(=*YTX#Qidl7`kjw4|5bSd%p_+c`BAYfsO)>&R zy!J=;s`FdMY14ix;7rQNJLA%BE{n$A59?@j`4HzD5nF_smGVV$z{jyiD=lAc@u4vm z^LeqQj5>2cRrN&suW9Ui$PS_Sz8K8|C@sDGi2*1IWD zqu=rCL5t7qFj%Cw{{|aFHXHnHidOe3ihiF_^5{)+6K&DBM9q6`mgv?Gyq~OneQh67 zj9afI&Cc)V-zT!V-C zCdDuOLwZeii4G~hD$ac9ExoFi2LrNPOfT<0t zNC#vW%onLEG_wxXttxGn;hkLQHN4!Alv|_?GK?3Zrd$oeFZ?E!zPd%)D(mHXGQA!n zZAJy_{6I=FyOLYz;}oeDZHkp%fm9d`pvU8+-LeaQ^kXvhi#WOqtm2N~R3TiRoF1d&nT-V~@KmCEQM*~Y0} zlC*`v4^gINte0%`bCR@720T%mYp0^R(gBMT{RW5h1@)xWEbMukLC$^kr9v#}(>G0R zAT5#+!C*J^T?6SY_;PuXltQN(VkM-q2dSz;b$z*FO#J=EiMBXqGKsip~VfCU=WK6HUdpr-9!SC;}_sgL;Uz0gbwqRUXvmbDv4+&84l_00|CBQs6)3-Qy_bHt7|sE9mpF zB2wt$ZLmeALWG}_E!eyeSzx;~a0^)6)lx#(K&8ss$ZsX>vv?si&dc;vs#MHH7z!Th z)f%+ItFFA6tc`ZZQLv4)29il0n2eUaIwht_+u$ZBi5P?VE=?+u99rVOCX}^7QmE7+ zEw#9iwpgFjoJ@i!V|hOhEp$n<+5G?`P-!vr-_s3}kYV)vgZ7JV^GGF%GYF&6D~9qK zjkQmIk0+;B%7ribIV~{qQ&W3+ga}b8#rmX|Kw!0fDYVNc9cJ1)J(NPL{h%AGaAkP@ zmKb_8Agz%R-x$833PE!z`xgwU1nWMl(APaBhZ4dmX?91+Z330_RY$O4Ae9Pr%#d1F zfGcTUhU7x!9l|TW#j%N1QeJNbgtYOlkz!XLUw?g)G`O>ubxFd%_&!lv4Bvkpv>e&{N>L!hMF03>O_ z?UE+~kyNSc?NTen?eKdAGD)4gVf8RHsnGInQX4pziBHm{Zj#dkD5+C-j5ZKTqVAH1 zb0i0)OS7alireKfU`i@_R%$Jd1x!g=50;)`X+0#T0aQ}kOvx3ARN_EKDe3D>a03k? z>;k4#sbf#6O$1iyWQ>wU<>msDLuvxd*XnjVQ5PF1rQ1X(>FZvSD*~dVcD+H4j!#m~ zM4ksd2#l`0hENP71n?T|3Ou82ALuO%NO}w~lD_5hVKg$47LH)a`Va*}%4v(^NF0(j z97SSDUoJJZG7N75k_nWbCAH>=$uIt*NrzU>FgHGoOV=*pt@`6GZy2aXL$ zlD^A=;6j+BNtNG#CJlD|03c2!CsW(rlGEz<=%}Q)rnq4lIw(os^n&XBNC*u60F$)Q zr8}Sn9hju_J0-8F{)KnK%1mgIF5M|PLf9nb-vwcZfk{^cDA^9+P6&)t=<;1sDq7Sq zBuR-yQkvp%nTRB%-vfo~Xe57jnQ$a&_pmYq9w{tA@wlA^)>_m>U)kz*30SfjWeky0 zVJZeNNweol4ldowev|5owAk@p$q5lMI!Sq>m;$#~M<*$71VRc9b##*2--m4tc#`Jh z05u}xljImBwY7RY5!fW9-_MR@fRo0$9xvRGt@pe8pVa_x zl#)6OgjRK!vZ*dKzt2Ga%!PagQYjc#JbnYDr1^tFR%pn=59o(+5o(_epO1hj70SjwYeR zr}jg!n2A)<{GrgE4pq|kLqWa3Dk=RTSey=4va}plrvX*+c1|4WNF}8Y!!;ACr1`@@ zw1HLX191z@K@3%jRb*EfsHFK1!`zVHfmDjI09o`xR_x%4gekoyM9Cz&tnQ$WQ0kPK z@FdND6jo{Ale{8C7Ku($`eUF@hbQmUi(uH?2*eu>3mo#q9KViAiUSjpWL>ticpycQ zeEG=gF|kN-5r+mGN#BeB83rCnc_Z0#u{=r^{>(rmeZwbpB$7`e`s+9(^W)M%Nd3G= zhaf398WyQzkg&*+h?yY}QlTG4g9Q_bWMbM|f+*DhPtlw)93Ggt6bjCj+NfTiiADBR z@Y$$s5KBChEOB;}4p?u~#S9T=%&j+8qN0T4$5aiokM zQX4UiK}Y)JaSjDYRd}M~(S^Ng9 zs23?{WVkTVVZ}7C7Z8YIe+xbUPo#ZQK!^?|Qmv^lOA}30Y0Ol_tuUnMLijM@#J22m z3@6g`e3nZvV8DsYm{Sg#Kw><#dJm=yWuJj3)|%!`z^OC$h=4wc;=YCmIU# zA&Uw~QHaXwb2tU0NbxhcZqV^WdI$$PpeRhImEv;*bWoAvo`wmA5k)J_d;+BmuIs2G z?ZEXAq{yS4CZb85snS)(BK@^xc~~;DV(Us3<#0FY>POHL;a#>9*af5h}>836&*!n!cnWc zbP$o!=3|_JA~LCI7B{0X01@dUTryBZv9$`s%>WT;hQ@-{QAEm_EVZ`!+&YLzEf;Wl zF;GNdf{G7?mjDqth{B#xu?Y}S(1NNggdw^s;D_|VLeamN03wou7cdXwhs?aldOB*T z(x>?diw1N^tq4p-Vuv(?H~^X8p+cV%OVT_M2t}I^l7IpR=t^fqO!WiI!bxfpz0sgoSHXMNgdL!UJg_f=7NMT@r7uQSP5C-^7hU8G4 zjt~mC1Q@=F0{;9e=rT~i%-3)}i~{-$0Pt5n9RdJ#Yr=BDNDQzYpD^*i>l?s%2>-i# zBWx!U|4TQZKfP0SIvq$|CiGX025yqlLiitK3cZG*zqWLK6Kb6>@W;KaGXnJ6z8N_r z67*9jZVMKSK>oZT(C@KgE`dU*UtA%lb{+K_iu)D@>PH)oN-+X$P@GO5nqNSF+#R#Y z8?fK7B7~_B?B`URegpHvjRQSis7|LpXw=8p)*Au)Ay!5reo0$lMimf0Q{s%l>3|== z6l{V0~Z5+`r=3U!$0@{K;0KdF|GF=tl@Q%ldr7XgDXg4 zV{6r>?B;R@tJCF16zG5=G^duFPG9T=)CWSn@bUgvFhSkN6nN=Mb!<{}j6D#R`^8gu zdh3YP0Z@Ke ztFhoe#3PS*WXe!O>o?ijL?=|6a_n*}J&!YxS3wOqg|heI(<6tarb6AYXvjmTHMI-| z4s-W2h*5{iR80K(FqT4!1854p9}#=HbRj+b4qOQc2uNlN(betupp#>+S#@v*ULgj=0$Nc$6%uU5ik*SLWF|A zk?%@=q+K9nhLVrG%UpY@S}Ty%_ykLvJ>0lD;RHy7-%_<*xW#z_m#NbX^q<0nPU0f! zcD`Q@HM2OTorJ}pq8&Ti6cC?5vE|iwhlag6(3VV_f0x{f%jtFESzz|NEx!Zm#WA-` zF`&hBO7bB!;+Y)Y`sGuYiIGR8CcKM_gPDC`66KwiI$9h7(VN&Q`&QYlx?D~_nh`PW z!h4toF)wF!PfNHJK&P5u@tm)qdb9;jI>lb~^JxeH2f~Ajs>3~czb|xxn}zH_yosMt zJq7mr$NOL@;Bc|blk?AmVgD@Q>QUTY~VpV#27_wJycBpl;`1DNDhR?03dq8Fxz~_mgmv0^}H%U zwjRHaaSK+-S-uJ=R#=Yzb1dWciKv-z6(*ebxv+ghrjTH|)$0Hu5_1EezzrhY1GvoonQ8YIG`JrBfb6w_=8`YVnWe^ zV8=;8pXNjzEwsJ=OK1&9lUN-@a-zc1%R>0DXWfaj}qBRq>;|w3wJ%R%dj1QzqqGcs8gaK2=NUiHU%N#AlC*g zVx+xTTJwV_^6<>3L3q#=^y*Yiva^?>mIGlC)7`*oc)HkHz95vX$8yBP3>mj0MFh*f z2%eBs^{E_1P>b<>F!lnzi0ABd&Sq6X0l?Q_{s{2-zoW|04>{n34uiV8^^23as^ds* zhnFW_f@7nh6pk?L5-Sx#h zHHj!GVoMh9gmc*GYb-JMU(g*e4o+jqboHv3fQm*~P(iHh##m@a>|KP6f~RU+7<@kq zmh*r5uh`Iqsz(^A{b%-P;lRl>fe&=ZQp6!F<-vV!5ni(5Wl+Y4a7ht<-Lk_&oqj?f zJ1IkQ4iDpPf(vlT5IgSfhnaL*5DRpZ<)wJI$u#m1?}P#d0aIVv>hR(YqbSl4H{5(5 zlj}0Be0Epo(S8U~hzKShLofdV%k#4}r$A3mivwyAK|Df$ew2f-h&wPBpG3|8^IX~* zx?!@=L{Q#lWGqpB4Xh@+kS*}6$W^fX2c{)UL8gWu>h+&^+)DB-FAdxXA3{PL$u|Czj%` z!#JT7QQkcEr_iCW8^qm?eBVacRP1%hyRat~c=S)U79%X|$8d`iwULNJr8SY=6MjOM z?{+!ag1DfD$8g5x`yM7mbeyA2Do_z!)nbhi`L6kmKjz7eNk=b#mAfv80bdO+0@C+Qm^eTPR%6 zWyLi->%z4zTd0bgj&AH1_F_Jc@?y9hvcO@!{qrx;6Qax#?6TY9B20P^6opE*BcAjy zl<%^Eby$ic9=3~sW0Z&I_%s_br=@EM5i-1d|F>8haST`VL?paHG@HDz2$jLAG6#7< zDsItKg*Um;c|!LBbMo2$MoRB38!{qZL@4^LUpSmb+0W z-G6~y7wt`05OI|M9%Fj|okSMJq&T|t9$N}di5u$41iypb>J3d3x(eaQ=rn})LlGXn zhsNPdINiE9jOANrLh=xVs3=7k1~Vu)kq0(!{hMa?0C5mH%uFv7Pcg{O*z0vz|a59P^_yr1)N zeswtmH-q#LH>A4kwE|@@m_X!UkVALxUZ|t29yq_w#$z>b4Y^H7TnJ!fBe)grq9Ry` zJ?qwF@9^qdK3)^F2Xz}h#-H&dFp722?YIF{is~qZR@Mb&URWbQ4sp0n?=J$gvsHuc*L4+SqD(P{&Up)-;>18Se z8$*BAW0?d4rmtVG$5xKV<@7#S5AF@ni{TtRMZ_!wxft3~A1eXHhgT)@Djo?9p3_VR z4cMMQ95=Mv8?c!5g2P-7(G!l5Bh@j9KvXw^uduki4Y9hJ#|L0F+_yB+n5jxHEKL;X zusXMb7TFUn1!AdTV;LCrS4mtmwZ~NtPzY3I$#fhSxUUm6zaXcHT^5)bXt&E9`T1@@ z)J6z6ZI}J1Gojrk)Fv6)a3UOt;Qnkfu6T8AoJnTO#`gDOULJ>8e+o;-I3uh-F9l;z z1|XuQ(2pq`Sx`QS_}ZyS#VC!E48dLWezoK{`ezfi7(GjirFu8j?G}@Psx;*!ALN_9 zJVQw5I?R&sagOl_k#TexPvj|HSI{Ba#bhdM2I4W}QP3+o&Z@U!27GT1g zx#bS^aL3S7@w&@{0t@#=$>ot-tGJzs@4c1`^T^M8DQBVFh87K%yHMdUxdu&tOdhG9 zTQEXSrOkU~fIU29O-t0e>@_$mT2NA3=~sx z*I|W*Bdw3q_3;=udnpE98ZF;Lo5tdB+6C)h4#l*lIr8#|jX$4=RYtvlxsxZ#Ln!kJ zxfT7<&|;8^?!hpYkSQ zqjx6AqawD9VzBWfc`OzDYHdX;-UWRXrh)y61f^M(J88=lEPDA>JS9~0wWTg){AO)U z{eFYa_T51jtfI7{)Ft@jm;n(qdcl%J zdvfJ*)c9$P&AtX+ghaxc{p4vvn{AybccuU3yi_2*8!dSPOh%bjls!c0YQjvp zeg0a^5|$NhEEV-yVY4W$Cy{-UoD*RpQTtRl975L*n+Cb^usn#S&BfvDLvnYzydHXq za@6qLpAO37Lxb^j$1=HB#8U9CG*bU)rrK)>@LTriEY(k!jGMoFT+ zZ^<1hxlo4&{bISD=FGRWq*0Hfu&^ITVR3Fgru}r36TXH|>BsPC??QYU?LZp%@6K>g z?of-B;vd2SFSND}q6b@}1nQWNEmwRcH=sS+)Ed-an56}k5zd|c*jktD@8H6bRv1?} z)G~-x6d>mM;3I zEr&2<+9Q@mxIJuf(gUY4P84pAC$LT87g#-~jkN_$$icYQFG9#zz)$C9Svo^T$eouS z|Ldo|y{tX*r(n6PrIrcQc(tVmB`&kL=$*w_JSY(yF70J?P`iHCR)RyZX~rign+s-Gg4BASrAz(E1ViR67(Xq zeA3cY48Us*CQw=@>+Rxb(;2WA-ebxc1cQu`7O{sIkkKAQq_5G5uu?*MtCP~s$vP25 zEiEeLwzGDk!gJ6@{0E%dT3T#m|3HZ+`*P-RWFc>qOe~lqHo_l0=EOz@%CiS=!R1uR-|n30U{i z6sT^%4$CAO{VYD=urtLtt&?f=w^$|P0+ds57l;_J5KOF`WyxR-&E%DPP!_sB^5yEf zSIC+Rv0wTgD{ppN`ynb~(Jim(_7GwqlS(hh3QcAhi>80Muh7a^EN<*>8tf56T*kFs>73Vf&0zFhbsxb4PDS|LP~<+pYx`+97jIvLv! zEC%UgE-QG(p&Sic>3*COrYF{HlVzPmEhkzgm)L)nOC%O%8gq7Uw{tTPFAn@RSMWGhWs33k)BVu39`%6;gQ znV{{&W=k()x4GC&s2|G3v&jajx6#NKpt{jtq5v^jnn+V!Ry34Mnpn9nZsE|DYY;lj zq!DGWgCN3`o-2Yl*+baP6e=u&h6de(1t8NH8uUBj*5%(Z7cm7z?RAu6%j-ZVKUIOX zG=7IO*a^68PFsYSpZ>r&n6el*fI4_!c)w?W@Hy8J>B=sF(lK8mZD(}C9+Urs-pt;r zKE3-V(|6g@oCf{_GB_h!I3Bg5vY#-^j6znj-)pNzSv3(J`qT!E=k|hbvD`{NOza1e z(W=#_cp1|~R1sIR`S4tfr5Qc;cYNA+904jO1~afvPwE@6jH8-X5ZdMgtar|0@loIE zU~>7pI?qlj?8#*OX7XGfqgJ7dR;YaTX;3TtTNrIZHH1$`HPAQxM_rYTsJw^8O9K)s zte0rRdP0VCvtSTcGaVeH_ug^P-soevB+Ryc9OBINYD_SP!m{_lNmg~H>$=>GVq!6P z`BzAQd08Oy{KpVcyBZjrdJ@D98i4i2)Wo3~hP_mHk44`n?kjxgVC_lSUtz%V+Az?4 z_hVf&|F=@cHwff>as74k;&0c@izn|wws8DuNvAV)a4zjFAOo$Fz|h&JK~ni;d}5Iz zF~!i-Bt(>!bup}Axz24nnxzU>QTiEqu`Bh?!ZxAemo+;TFxPj6Vroi$pVJ zAh_sI9|{MUlu2{zLz4R%LO1966V_`+0~+fydeFTc@aa;(5=&XVEgfiM2W<4+B5>O{ zorin{V@XT~xrhusC_5ROW2swdPH&8Z0cFy8{$yHuUCOEf%EQ^*AP$+IM1YlrkE?1U*U@w?4-*+3_j|^D&^ne!sw%>xfXnD$)fXr z;3^83ru63zSOCVCNmoSDpX;I(1M) zsJK~h$)xQ5pjiY1Q0k(%i(o5kAbs5^X2LFBK^mFVy9(x-mJ^PVp6D1SR&K#2@c)Hv zpunjMg(7YApcSh3fiePqf^)o6R8Nvz2jj`@SaTS+Z2Suyl))W zqmq}9(#`xZlP3L%vc!~JrHQzb5EahN;UxP<28gRH)!!`pih5S13Of0>=I#%&g+M$bEYdS;RYcORnid*iE<9%-$K3@yMcup`ZB5?XGCnx zsEUn{!y8k^)^-&$|9~4MeUu3UkOhCVLe-+C5cQT&-+dfL z&8Qk%c`oIA*C!4kX-+K^3}<^oK&DEg!l;wx)Q6$M`YVs&?=;3$5v%&NMxiJuFhf!% zUFEt(7+_QeLh_5fr&vv3JlE&Yt415Ucu#5r!SOi|FwmBN*Agps_;N z)Q2jx(saJRwRfni=}R3yktb5PwMl#wtwgA+MpNgfy^kTCL^WMzhlU{C)9MYu1;Q3Y z^JuisW4<(7f5siSLvxbZ$M>b2=L{sE%^BI~Sw#mbir^ceBoLjArXPaJ1t5Zsr+28+ z?M?42hxSMB0mc2^0J`v})7mwneINBL`ueAp?D=>^<~rH^K-6k^w+u67@R(`WpQ#Pv(Ys5_vb`KlIO_FS2iqY_Ua6 z6~;KG^@eT>Ct%vhXF;L~K1`&IXF!12X+`$^VMN13V>*Xg?;a75gIH^h3zMa=1z$B4Jd+?8^rU%hJ|wa3yg3|i^E zO$Dqf3Oo?Tm!<;+8PCBCqmY;oE@od1#}o1~O*V}&hRGpd?%%ckCZ^Ii0+`R+gLL4IJn}W_1cqa7LowCXRyL5O88Z=SK ziS+JrNUc##YcfqfgR;cL6!r4pMo}h&SB7n&GWZGcvpsqUw8H|)Li0Nq7*9oLGGt4F z%n;71n`{{N6S^+jY{|^;WF1H4zajEYbD>Uo_n&~GvR;A33KUjUcm>-nov6`zJup=c za)|}v7@iW4RRt64L;L$8J;(0=)h#AkrV!9N;pX9`BIAzPR!76lA`yncM`7b(uMe@U z<8q~W*hiRhWl*4qIA+Mr_W?2$wvG_jC>p#p<6JbPtm`uhk^K*lx{wv=bi&mm!hJX- zZ5bud>`9?qF_A_+3j|ffFcC{C!x0&f4dJ$ZXv}k1ykLhVSI>kdI#~ zNh`p6xaSgyOFC)g+bHtGn4xG+`UD^^(M(1mlp?Z-Az_^J=1u3#TlEdx>!Cc)q-AyB zjQG@(`c%O=v+D@A;G*ZMPFonJtPG*70KYY)C?rqNe6yCW2gS96Ab<`u5cXlBgnDHw z+6YmwR}O^U8Nz9!0Ld_Fkx7gJa-YzXL?>QdAi)AS?k^islV?W*VJW zVX$Ur(S;G$9(p^K7l#VZXb23n{(sb5Wrlj#OuBi)E1Ew6UI`IKH50SXLj8^FLpEp! z>ga(g4EaX)hXbLXC+E8PGi>`9ken!TjiB16oKAPQLWd@GnJP&=T7A%HTBDn`Ks5R2>I zAfZvvn7~deLq~=8gzsIT4T_%-4hWXE5I-RGf3!89flNv=H_=~gxVY1O%S3o zAoSnfwCO=@rU7`)76$_aT6RO|*g!s_VpfG4TTezL1W}XS zARZex?Zfv{5ITDhlJ%1zIHgWRps(!imBSiG5O|S(#n$^7$6f`LsEk!I7Pwh+yb-R6aJkab>dsg=NOvJM~CNdS- z-{f4ylQiZdA`p)Ol0`nh6YdDnx;G{@7f|ITev$`moblLCxPzPi9T3SdrGP(s(fNM> z=0&H6#>iNHg6JGY3kRsQiw?hu5!_(y{7JS-c!6H{xvTCptsVL*@hFt&q4C&O^b=E2 z{dsGCbZ6gZfFng)%W*CWIEJ1pf1C>B>zJ<~(1drR8+UP0biAVS33Y~M*B@#p!_QGo zq?gwq8i-61)z}LtbMI3qC4sc|p*{z~UHb$b1ithT>gbz2B-cIS*)~B^NFrf8so*^X zPZMYEL&q1R#ua#q*q_nu3g}dC>bn%pPbCk@h1S+lhK7?E2nfNcHwCMRq`>m3>d#>rB7f9EWd&PDnq*ZPzQdp3fA-&fRaHgW(&_s z8_zEOrx2+DA5U@d&7wR2y1+H(>Nh9C&LW;-gq4KBsYvV;4Em@s!|0#QgvacC0T1ov zJb?8coQ9jfWh1TV6KCOY`2*I0H0Ei9aXc{7oG#78C-J}xgs4)%Gq{WK`v6YKOE4|! ziQCNsfa0{RY5`aa8%#z zvT;>>HQ|kH9QOSUPu}2h*$&id2*zD_1Y1@{MrBJ)8DcIqYbchA8)1!n99htEX$OYj zpsAF{}JYmzg;EjsFkyJ9~KW&FPI&R+Mz2u8Kw5EVBL?GJZU8INq9AGGnpTE`@AuH(vQ| zT5vcFRA4r#1STL?uIS2y9z|DJ0J`_Du{J0fG|Fnj4?BcH7F_+B?(`yfIo87i_qnRX z(8}%BQ6)IET5hbUwB2T8!8dAJ(}<1Mw`t!5FjZOP3-2j*$_-P4T@vVgw<;;L_*HD0 zvCUd2>oo11fpWUCC>_OVSdSHSfmg}hQU%H9b?f#@SBY}=za|SKE(!77erFYDiS~xo z5msRmRa~JYcdvDd6#51dzLtB+N|ZFMN@tq+F*xh^i}fx%1$`1;7sWuu!yOK#hg4Gb zru7{;v>3iE^MiG9#5{a|C6BP-GgMGrNh?V|W*uP-jjmgA@q~4!C3IW|OLmb61niDj zLXYW~O?!`9yOw~kJyP^#roIH!FNqb4m0;U3VXAZ%OWOQk^+<&Omqbq&5)d4Vy~U^h z-CvxF=3ws6*5AT%6e83m#p)a^|L^_QV9B4qTi;M3NEi9!{{ao?;uRKKay~}UbaN|N z9w<6jW>IwJqj7n5zZXzqF?&W&a9!{Ht;z8xRHdrks) z->8Qt;OEEven-%Wug&8{UhW|O_V}w?W@xGM|r7>0UD|vVYMc*`~0sYITRHK3{ z#Y$slC^7VbUwN6X1{C~2Q5&Trm9|rCbS$7aXh~4Hh3eg{;7hz)FxVbcCgG(|OM}W8 zYTHhUr{~)$+v!3(r52TTQex?WcFJ6QAM8{crB;4>94E9_rrzfBrTX#LpXv{!`h%$f zM`{3)ais?F*PR;lq~e#+Aw3Tjw^xqfy(IIND7`7erzFq|U6ezt&s9L^) zCj@U-R?q|8vGJmA$|1ZEXJB89Sk)aHcv9)daroXt`Tf|S{5k}+;*Kiul-om@P1iD& zUKP*PpsAV4G%6m33t(w1mGo4ckScb(n-U&S?#IiT!Oz-WN}3$N?VXuG2g)9RzRpin zQYgK*GFie4iSEYd&wDG&@FJd6>Nr6T()K>eYakY6{UE!-EN_?GB^gQPH$T?8yNpC->;HPSZdL8SPL`wX{R+B~A zk**9-I)XHey2$T>1J^^C&+4@K1W5QiP^AV2@?bFyLI zcro-qESC0y5>wCTPxS>-eZf?}0}h?)hg-W-;pBLS%&5T%-Xu%K)0OtjOf2ON5&l(e zsB(#$3Tw%K7~b{I zhZX!R25-^UJQ+tPAHkdX{dDSf7_g%kggAPrlE?3g7bcxB4D)V(Oqs2-(UYK>#UN8$ z{CK#MM~V4}1z}ofgffrc(Q?w(D@@I^BjLxo2YE&*Yp@;E?5Bj$ptR9wrM4v%ug?5w zno^hgj8?XS?kmgShJqUB7-bf%%;B(c(gwuMWbi_cQkyzX$CrX@j#aADojFP`{sEJU zykXFMm=I3(qN8KsJ>|=kW+Zr;`M9FdE#t86k|jzPmUuip^#snnGY)a8?Kow=E_4K1 zD+h%6lw1H+JNBUrWtW6N<7n%Xn2@|3MIvZ@l0F}=v<$geR#!~?{B8{I_moo4im2q) zS$z5_rC8U3Q1nuKMu5YQv++)jiOO7>Jc-%O=O}3Gj$Z_vq)cEB;5Z`$vtbhUzB(Br zuP#&S>*vNyhOn;OU!_-sLhha-6e1)lIIlYu6A>*Qq$<;t7pQ1D_}xDZ+>}jI8iG#5 zcc|hJy_Ac!cTZQ!sMid07lYL=VCueru0G9lS5VW4r$q!ets1uU z3{DvKW3E<%lAcvYM_ba4XAvy%rnitOOnMI9y=^YIGeuR69(rDxr8~0lusF(iL1|vy zB$$A(`;B7kl*kZ5zz7CE)GdOW+F!5-ICwWRQAJEUVTX^ zftW?k*fdL7BDqVtzpTg-5?#;P%2AFcA~ZYZAc*M}{mmR@5gP%gs2Ex`S7`zv;*~jX z&Q*@n=y?!{u(0V1A-W5B$}`wmXfbb|vYg-dBN%Kzu)o3cl{xf|riRXke{jUSo#Mk zDWDWDB&xkgd7DZWEA8^%MbvD#SXn}OOUx}qB>iQvvX~|=1%D&5t8}HaOO$0Cnprh@ zE5X;6rOI=H zZHnOnp?o89xd?adxxVU0_D2y?u5D7B$!2`JznQd7hP4&Mh($n|LR zE@d2OcGk!Dd+=C1?kYSE-SVDn_~UHNX-bKjSaM;v@;Au}%H0$^ zhr#xP%34}=2r9y-hiLV?F#Tr_DGlj~Hx(a!bVylBJr6^7yU!zx=X?rxs(wWIXZ|nB za(d-0Y++wzt4FrCpa2i`{}vhSjklET5KGS6%6|I&D8fQUeOPw)S8cTgVf@xYv2n9w zXmTKpdFSvrP@jL`>pJE8KzFZqlq&|8$l1x`)!RaZb*48?uz#Ibj^ovjaQ+LRkeMxc zpiH?(qOwn6a$lY@iOM>~bs$Natu>R&uK^oe4xxSe9^$@`d<}j|wF>P!jE8rZo>rFO zN83=Go;xcyr$1Y$D2?aBke_@XQQAp~A1J5ky$|6itv`fomA4K9QF zt9-7kYQ?$F$!;FH6-;6Zj~r@ zD4_Vfg4cqFg!B-le5s^Uy|3V)Id@rYRK5u!pZO(xEkG|nZM#)pbK}>>nlP&wg|Na7 zA8=3e4H{1T67x68d7Ouu-=S~6Q(mL#zo0fN{2ogRGf4a%3Y_`_bYA+svR9V?B-EUK z`UP88{ZUDWK1^A$*Of<#h~G{Mm3~gUegc~lzd^Zcif;w`grV(K`~!x9+dY?1>fvpv z@2DLG@g-HSD78>bv21b+Z7s?EPY4@EB&na%WB~9g@9y_E8(_TfL zf7fkyHH)qVfxgxJ!a$QiD>XWy;FomqZgi8G)eG-a6LsP3`4d*6`_RJC>Q?Lyy3f#Y z8HQM}Pfem`vbqlPTlK11pX%+m#gzOat8T;wH`dE@sof~CrrICVW+Wg?olj67Vk6j% zazuRRezc;FD|t*+>q^wFhuTUECR-I)e%T4x%1=b$HQSg(1y$4-YFl5ug`cEAyi?R6 zCC#g<1rp6lu;I0m7=zyz0h$WqD2h{u(#aT%-5sw^rIE4xX@J_0eu+`X)9okE52*l~A%ycgl6htL}Fq9y(WH7UH~Fm{tu@8V|W zeRso69JSSt~*&*b5qkO;l_J~hTsdJSAV?$cSmgFfR! zvDy7@wHZ(UiryHG7$yc!uEivuv&50DmO74NYh&<)9yas}wLxwD=IUf>T?a${xI=A6 zxk=c1e;o{&bO#u6)WdGh9;%lwJV>?UtnoB339GECqCQXmtcysNTNh%{>cQ%>>wpg- z@3i_R_pJwjAhBpEi@()wSS-HRWuxBpp}X!4)Rl(r%2L(-G@=o7C!~w-gWKqjc37~W znL327HPRJ`mkOpsVdC84#yA(aSM5wX!tRT{x_T*=jzOfN(> zh2e?3RCPXGQF$1!CJy@e4uunYmp6dT9czQ-U=`Kq#cpb|{5173JU^q3fpZtdt99tH zG&L@ckt#3#`BL#)1)Rp4(7H5rGc|Q0lp>d}hi`BiFB#y3ThR10wNCy{rRqY54NuYe z)ExTUjZI9=KU0$9QGse-Q(+ioUiDF3Ut7FtE* zs-+6eehA9OxG{Q&!UO0n#m3xYU+JfES3RbOt+g&!Ute{qMGrr$N&GCEpuL`Mz31=3 zDwpoimDGTy*M%45-2w6A+xvH@>q}bPr6yQ)Udo251Mu@*lyG;#iZO))Amq{-;Li9WV{CM#U`2?3R*t$0 zIeFeP^<$ZT1AuERu(R5%1elbPS>y0~O*DNXRuO4GbG(Y5G~}ZEOL}<%mc{o$s&L_5 zO$;izXQH~-7>^pU1ufCw-`M13TU*MVs1{J~sZ1Z2?j=*yg;Z}E4$UH3kXyi6o(r7v zrRqwqFhn`AcZT{f&6utR=(SuVMjSpu!*f;q(mjN@KA43sDe zt<6tEF7=)y0w}eO^$rEMf}(W|gR+3SNRR zGB(+|(6h7PFMqs@g}*zC2f+}3^jUOdwy`=F6a5#fNBDQEQ1^a7X?d)QNf1Q-_W<&* z)Rbu�X7Ro%dH^dOe^qTl_2#S7sr)CjL!Zu9iP|o}#tWaeHD)0f z=oPq-%f|~4=&ml*+3!pbEmZUAoqQZ3DJJOM$lI@~!)RtcQsyRthi!b`=A*(zunqhG z&0?r+siv;rN(0963C&}wCM^c97nkrVa}YTDEKyfdqXUYcWwVB>;TCjxDFicoIfg8K z+2)|4MWD*N0{j+CgDEdqj;mj^v(-WYm_&nC z=b%orKpF6uMEY?G()rYlDEdC$gu3?fa5aw>ZBjEhcXO=`Mpn}BE$a7F@VvT-O&fOJ zK$N=)UN_&U^5CC0)rTHHlmK%J)gr21td{_BG`>h(MlH93!QlZ!kzU0d7WpO5=;(b^ zP^`X07q>#9h*zkV_;AZM3`SL81TsW8{Q`{M?CfKxV7s~?G)Q#wsL6Tuxg9LaT`02o z5XFGm@>>}LFjk7{^2OAdbaWT2N^G3b(rTrtC2F;j`6VKB`pnQ-_MN(nUt!;AzO5zw za~F7f;0;Jo#4hyNcnRnxOTYsxt- zb`(uHq_GNfg&=Z128mNXgw(~S+dl$BA|=)N8MeLSLp2WufhMj!-cp9p{QXC2I@m-L z-AgCt{ZCN77VSdLS@Nl_nPnp6;RnJY>hBe?^D_P^LUZe($9gPWUT7hq0e7szorZT|`$S9(-kN)x|^886)r#hm#ntQaw<_P4rG zEZ1XS$8Vv>_1_tG!y&E&!^T@E_eba-cVXW~o7C+B47Jxq@DMWtVH-QIFqQr6MTmQ> zhBaa7ufVo|Bz!MoeTCvBl#x$Q{Tt z>)k}@=quTOX%|lOxVZpQfr=ykE=GLfffz^7uQ9D z?`K5Fh0@8c*k;lc$ri)4@GAVStZ0hs(PGKAgwDyf5-w*^ek-<@sJ+!D`iBPk;dMSl z=h2X6iOO5CE#Ugy7(7zNRkLwNUBwF&4@dXUpV(=+GsPQgmXthO#TGAzwwX!kF`x#G zK~3(=@scr6m>-LYsCV%Twj2(d5&Bxxl(+)u1MjShv)1OmSRYJ{<837iKeGX`Nx(|n z^Wr<1HhTFEV>L=m#Cdcgcp^0sM5D_;M1|Er@6z3{JQTS6^g5~ z{J6R}mE)n$78lEpq~PYY6L+tJbZHyP(vN($?UWs`;mzg*$)T%%4e~c5n^owjSXI%JUZf>Gu8(tb&U#m2~c48zRx?Nyb&`(-HSU zfPV@2q<|V`fG8jxZsGp-2GDX$M~o1tT&JwPgpOZs(X&cU15^930{ zKmx6`WddHH1KmLnAO!syHOWNJHNA&zEq#`WB~7cS#)I)I=Op#EZPc4tXbh+Fdt;Po zS^6m2)LR#&Sn@@x*B3)fqpc}su|sd9@%?Ox{H^J#bhfW;F{Nf8G6>Vnk=_PzIH)d0Ja$*Mys!xKTyZ&hHM>>dS?7 zVjcCs_Lhfjb0XW6DG(H9iS758eos_EX6JU0n&y4tUKJkF^h19q~ zXdg!g)1}a97gF6RXU4cA|M#KHgG}u!osQqK$ z3YaG6W^@;4(prjEHV<@exEqi~B}7;`6AwgKybTbDFIeF=z}N0kyf!|IVa2N@FVRUV&6`!#dVwmbwS4$>XvFhbSxGJW)*n2Pyb#Zc%^-TXpz3YIBVZn3& z-p6ISUM;;?XsyCGv8!=B*17alMg;3;B*Q+>T*&>~nIFLhRe3Am*MtJmb zT2-FPdgdeoStYoaK96TvYxwysb3Hy`#A2kZYBdw&JKc&zo5bWPOnr|tQIt0)sF}T#$c}By?t=5{v zG4m~K$_?9Lg|TPC^1`K37UGO-3#}t+`6A0p6}^_(NBy$UT8pNkHCr_wBW+kLO5b{c zHIBb6tIEV;-GqDP_%FM$Rx+~29g;F`#p0C3Nu7R522Fdbs3lgQ+PV~(oLOSwE28dY zLh4dnwfQonV$EDhA(MDja5+rkeyEiCf~UFI_`6T z`M%|13CoWPDlXpQ!U%JDJk7<)Ds?qW{si7MbS~6uvf7GK!%k-|AG-zzCnO*XUH|&r z`fH&P?m!e$>k{ar=@U4tpSGjG*VnoIca=(5k5l#HJ;Qo-NEli85EM6#3TkEe2_^ji z&%O&YjpPm1Xp~A1)p#PBh_QATpC}>M1Y;4~W=rvLt(lSLP)9dgOW+>vr@S$Ept$p} zwTg=qr}cT_ciXJC8Cj~;?g?$^EMi zrZ4f!KC~7r%5-Z9R|of@64O>@TJc$l@N8|PjtB5-G^7G*Pol-G4p?H8SK80lAFw8& zczR)>e|7$l`BiKZ%!M^l1tpU#ZfoGu3{}ueHQr-+s(Od5>YQ%g!{g{3hf&3Ws_s_n zzIv66iL1{cYa4!HZU}fLR&-hTo?taR+xO#U96x-V&i|-Y#DhB@{J8bO4^y1ZN3B{a z{-|{o?y~bI+#J!HD7|*z9h~FGtP!>VY|Ja6Iv%%{sFNq4{OjY^DOKSl{2}EP=28z% z;O73Z-kGr{s2=V}K6!=7!Ss_>6bjH7v24L^vaEHvJZk)FILqTkYE3U%NSkqCc7%y2MHpSYl#-DX(>Z|UgSSwV}IYh@l z!hJ8jA@O-Cy!Pl>YdklOl9QRIU z6_A}{E6c3v)@7>_s#|Zsd-W|e?R~}KQn^(TiT|n1y^}RQu_D#PtH=iXh4RHV!G^}+ zt)++h@e^w%$~^RUTyE$!++~ma0SnTzSFMqz^tx_vXogBkwL(>`PjIz{sa6Eae)N@l z!j@CP5;NHsUq}0aZ}yU5tfd@k-YqL@V#OO)hrBGTrwaJqnuQ-Ge{Y@Vv;RLRxK+)5 zVyr#)Df|~*IcrLn-;`|})I_$GW2?egHGhOHaEIGh_EqJdVd%ie@t#?ap%1*u2?bD( z`PHcidxa%TJ-FRUw zGp6qT67`8^#M}h!`^t((wkU?vz41<$Fy=XW$Oo@6Z_(z&SPw7WiSy=q$%1gEJlg2K zHaU}Dx}W!@)l~g#lk50}X1z5Gn@ksBWcQb{hMJL4u2oxXGMhU16P%6ptWBx*XF`Q8 z&wvAZe;Miqvxm%*bq$Ybq&#d(KjzH;frrt^8t+rTl z8q9<|Z}KB``Vzf7;067%N=F8K%2(@aWs~zV;4^yI%ae~UvK&+%S#j}BMPx~}IJ?}W zzRHSi(%J~aJmfq&B6lq6T}^(Tl3V|Do5Zz*2bI7<1_%@p{Ta=y)9?*yKaM{ZDCbK$Vwtxn@&-&1ZiCa)ar zHZT^H`Q#eaFAp?-kWa?DjagGA<(C`P@JHxTQB`xGhTfRlr=U#Wiq3ekw+bsH7xI>l zHtVxOI2AAUcuD+E?81^avC&@fu{^h-KU|tgs<5JR53<(h=Po8U(!-7Sx2lPq5*doi z+3vk{+lx!})_&EO-=g`A_K^$Jp<>vN%F5CX*vJq_b#@* z3=NU+pL@uPscKLuIk{PNyJIzWpShoGs_z@ffH%>J%CiCngL(I?I=sq6b(hhqcky&^$bAd1-ZE`XMfr_-{kfG> zThY6!jOH3#k2A-s%A8gJww&Vi33cZk8LYPdY_)N$FWDrb%zQ?Y*UbM2R$9^(O zU8(|o(!2Y0{xVk%?56X>&2$j$hY{N;s=weeYR(*SgjwFgP*otRw>xrS1HQB96k zqx_*JI<~6mo%@mMGEyxGkOlct8@5Z)*2`+3w{ZJ=tD16|>QY_0H|+IlkMerglH9(N z-j{d>%29kl!iQUU^g9Xf;9oP#cIshG`UUqZ>&uR;O~2RUc8NN2eFl6d7H(fe{jHAl z%1(`H&@*t`6rK_6c8c;I^oi>93GCTb?*+MiA_brBPBZMD5cIZTc8QN>VVa%h7>V#v zEjJHEKY~vdr?~-M#jwj*)i^}vR738-hjp}csM{eDRYtqp4fd;4J5GTIoKow zvl*OlLlgW|PZ16_lLyexprY{q8=A|88Ss5q-N{yI0qY02Uv0otAcxA*QqJb!rhB2J zmXh1vbWc946)xePNf?(|jDlh+fR7unvUe7SVS?MBN5`IzpD+@q7wOXe%vreqbl4w# zV+Curo%~*%YXcM1XfKbeac$B2U>qu?M{GUksMkS$sOq#s`Uk(mBe7rJ%Yz2RD?Olq zN{L@}kS#K(`kmx6RiQmji-V(#)s**#vYTo=9Fxr&-Q@?c{Jpg@ zLgkn&d#NcsWL0&iE7Zs~O@?r~$S*Y^_pCkTTv)z!Ux{b3XYp3&yopFX-%~DBTe_pL zUE*aM)twW|OTFZH4x%VbOBEL`<1^wVEA{kZra*OZGm5tAFoya=@5z4New9iL4Ik2{ zM4b|?!$*$xQ6oBHL<~#Db=$s=>oyBVF>Vcz#gu1nxm?wG57EwjWl?phw_FA_v|h#r zG3vqlFa+Kgz`It_ICE-Wxd8QIOfpJ-pP{lwFJ=m>mVRddgCauxK zFy0N0Lhs#VIOYfVgzu#|+0R|6okzegFy882%QZ*J1srNqM#|6O@w&RSS6>??*Kjvg zdOZwT059+vgb~kw49bxp+bmrAJC z4m39?K9^s8W_i&n@&D>3ptx{$Q(SDHNOL;w#@ChVGakLzj7f4jiVlx{9y?7ocCT;X zhVxaWNydVD`+fdMxl(z3h_Yq7i9)0OY*ASp_{Haw<+_Z4SUja?HO(o@sOKNaacXa* zY~i-@*(q|Yikpnz>sb=ak*Ts=yg=j(gE) z2`^^gs2+rj(|M?EvoKrrtG(C{lYm~cp=%Nw!A)P(L;6YHlVvyy?4`b(Ba6e3D0D~F zbuL;8K0DJ_R#!cf(X^{Z<04IBSAP20f+4;cL;?(3oT(#H`R9fId)HXiYu07WL>^(Jgk*uJ~EI^&4TNh2F zLPmVsm?drG*5vgvI1EhRQ#Fj$Z7lr|njhYroQ$sTd;(5F5sGE-4{-lh=e$j!7sHJ? zz9*TU@h?lzPo}j)V{bfoTMB(^P~S4_EWK5|IJEVkB{1uaIQbi_w|X9`R9hEiTB@?g z!8=lR$l7i#kyp5yv>!(4vV!Hnf`_)O!b-lnLe78Fm#)W+a_nJiCa)e?aZbd$gcWq$1FdL9ki~=$~(~WVO5D&o$i~S@6=ALG~X_~(aTGn z+9hN8u4lSI)BT~q9{80WU+TZn9|sR|aFT@T1(YTN#|%;&B8 z9{i&Q>4Sc5HTDqZMqLizd_5}a5x#s9stU-wVs6BKqPgMpYf{^)aGdKEcsmXJn|VbP7F2*H3UC1W~&? z=hCq^P%*DkaQO0RS>{a)O$+66A6aN^^PMU_%vf6qFw~ngalch8H z_?(W6>T*v4S%GXocEAJh1iS!mAP0~W$OYsE@&I{(d_aDn08kJp1QZ5}07ZdfKykna z@C8Z$C4o{v>BKJg%0%yV{9O5$;F-%xgb4iIIXc;SbZ48;VYxD{HR*{Rc88jD%5SF| znZLEQt&_B@+LmCR^C)okA)jv+Yk6^j`E|ToOwn@A6GMUy9P6j$1yipC4J_H(p(9V; zEficnJWR_WomvFj9tLVT!JGK0BN`{c!k*Br1Z-^sv$kb)`b)-dbmXOo! zf77zh!%QK?CT0tvHvJ=QAw#~&r)8rhH8xsOHd<0PT2fBaGMKValCn`!I1n*f@_^Bj zveA;V(UP*!lCsf~veA;V(US7MA#Nop8zm`INe8uL#AwO7H(FAABlDbZ<4-!<4i61w zx{;Z_{^_Hn6`NzVmSeVFa0Yx9L?0?;(s{l~r)<*kQ-^5SX>C%+J|E~GfA=zXQqQm^ zTK=lcm^x8aeYKo1aY4}X`WdyHG~Eg=vg~dUGycuDN$`|iXSM9PZ))(wmKTDUas1h} z!6`lerqfrAJ|4VnWmf&$=QlnJewtcd%bjcd8ocMZqpgnI8fb;wdoo$enR4X_IT61? z%ZJw$uI0GG*PIQ*nrQjupsUVuKh@{?rP|iL;(UF^uG5X@Gu_Cv zkXG1{W(uP*4;YOp8;vO&jVT+ADI1L`8;vO&jVT+ADI0|;JB-GR7>y|#jVT+ADI1L` z8;vO&jVYVH;cb~!`Sv)nVZ}7(yh-gH+7ZIvk9H=0^RAY^dlcjB@p&gLSIHIY4CvIw zea@=o&a3vWT8`?I;0#&dmY-MJ?EJQQntXJp^Oq6s^V>b$@AQZs+= z_m4W8CAuAOPJ72mXO6yky;Gd$mNe6Hv%M+Kz?x08yuN&jGxACU zEgv0q+9_t#)w0oy9-gL|u6v^xwKs}UHj3%~KRit{9x$3wHk$FG&(k!cY&4^6G^1=Z z<8SA*cPmEOD8_S)VoaxE4r<1T(TuXuOsA*y?X1hUz1MWxO#c@;F2_IJb1~z8z2kCt z>2dw<^<1V?VrG9y6LqBZO3dKzwb8$#iJGaK?*EPVvJhsxr)*|8l+6r>vYFvfHZvT` zW`;xA%y1~12@Yj5!J&)^j^p1nkNe+Euy(<~{QW=WaN(!u2wBc@$a{!c7C($*c0KP)^f4T$yf~ zD`nGMDVyd>*)&(mrnypfH&;hjJ-_>_hSz^Ix(peo=8 z_yYkzHK00B1E>kq0s?{BKph|m2nIrcP{0m^0d;|TKz*PA&=6<@GzOXg?*L7KW`1+)g*0BwPGKzpDA@Gj61=mc~Ix&U2)Za{aS2hbDf1%w0d0q+C7fj&TApdZj5 z7yt|e1_6VCApq?&3>XfK07e3%fYHDhAOiRR7z>O8#sd=)A1sAfi*w^uohSctOqs#8-Y#0W?&1j71#!B2X+8EfnC6EU=Oet*az$f5`hE2 zLEsRO1RMsOKr-M0jsQo2W599X1aK1g7&rx-2F?H}z**oNa2~h-Tm&uwmw_w5C%{$U z8juQH2R;RE05^eKz-{0Ta2NOtxCh(^K2P*1_QlqB9TPY8aK){Anarm6FUodYN6%p! zTx(>%Z}+wHnoAdzxBu|5UCY@2Xiv@6S<5k@Cqf^L9PH4M$r1fRul5|Lhu9iqLP2U6`|#R8LlU{?$BS$A2!b6YCN``mKzuIcg;=>)Uw^Dg)4uB zf?B>j^L^&y9Vv$)_aE}9z7;)1hi z8P4K@vuGL4k_>0jGR&9^U(qsr#R*^0GOQQ^U(qsrB^17*W%x=cd_~Lf6+3)I%kUNZ z>{Lgfj=)*$a274YS?q8YcYP<1vcp-loVUd!JA6gU@D)3JMUU9<6+5gLN(JC6cKC{x z|BSU7IQ~zy4|F+L>i@9zZ(0y6Uow76a-Lcvw9F2V)x-Oq z0vWlOAu~fpE@ns>8M&AtWn|=HhLn+!iy2ZzMlNPZ85y}IBSXr_$i)mPBcs39I_z|RYXnM*pc`{8;875Dr=_$kH zNi;oWm^_rGrwo&a()5%$7}4~U=}XLzGJT2pQGR~N(ap{b8KE;VL&|g}W=NUN#0)9Z znV2DEIukRb{B|9uX{N9F4;6FZ$A67xe@?L?|C45kIbNnFW=y=;=h}*lQRhZ~0elHO z0R9Gi1$+&B13U!21-=6w0gr+2fhWLI;0NGG;2H1}@H6ln_yzbCcmezdyaZkWuYuoZ zof~b70Z1808A+K)nMqkl7DDGwu5+UUZ4-OM`@9ac6^J2&YLn`af=I!n5K<_~P6{K{CDkLD36p1qMXQ!upCfclofdpD+`npWkhbo$^d0W zX^~Up0_Q?nX`qZKg_P2wq{xA=Bv49}5Z;KDaDYk*U*UzQFHl1G2oK>2_C%Tw;46wF z#YYqa7YAn(*}>TnD+;-oC?cfD3eJjH5um6jjFcjxkgyOI0t$4Nb4xV5OxrD(O$FzcR*-IS_ct|u$>42+JZwJ5P~7L0f!(Ag47xujIfSqC0c@8 zBdmk8)}l7TmLd=#xrJyBZi%oK!senTLUMD_Ow>R~ZYG+F>cBhT8VGxeZlXK52f`6z zq;QNvFiK1l!-1*b5eSDtngSkDW+2SIXx2O}IPJ`|I{ z4uk`d<`4rAP7?iriQoYU`$3uj?k^^05q-sYF)sf1h&WqbM_(~c^cH=@Sn)nW@&_UU zJQm@52qVA|VvGn!I7akBNFF0bi!EZhhypJ~xLGK%1iS>X&A=A12`QV!MzL7Lf@2Ze z2y7A?kg`#%7mEUh2VuqTnDTdYmu@}B#3!pK6pN23BX#h1}O<*wTMBu z8dxJ%iMfca0#=K7F$b}DV3k-YW{KJ0*+^Rn#ETV3St*u_XoSmw6=Io~AsjOi%tYcc zV7Z7x(lU`Mwu>F$9pbw96uc9>Q|tm?N9>x|isRRSRB;2TyTP9#yoqqPxCQJ1-$ZyD z(q8Z_gm)0`1@9I6#9d%N_>R~YzieT&Eo8u9@r6hdhsBo&o#4aZ2MC?wZwQmYPT>M4 zgTF#}M0|qq2;dT5i)|>;*T6OLE99@im&FzE6~ulKmk|2}_*Fa?m*VR#inbMYoEI0w zPvV(45B^b{L--@`Oq@md1Na=m6i83OXAz!3_!Rt9JQ1f6lD`*^!A}sLLiku*MVXF> zj}dzeek>k|lL*O=#CPH-jvo;x5c^J?aDcuQ$B=Xs{0+k6IP@*}8^n)`hX{`&{!o;# z{U%<3Uy9$rB@vdgy%OZt;uZL%C~5Pxm9V`KrER~9*WlMkEd#l&R-8wyL(QfS)ZJLb9LDAM9@nuw_RWVDmso4zN|Tc>>kJ9teGq z?F&%@VR2gx$Ue5lkcxq8AZ!GwD7Z1gh6sz=8URJW4H4FdRM=KO2cku6g%B-ls|PL= z9~2vHE0{mO&29^`iXw3f7vw4Stqw2`!lw3)Ppw3W1tw4Jnrw3D=pw41bt zw3oDxw4annIzT!|Iz&n$9k#hXE#|Z}$rsbVsnZ5ZCb>vQNJmM>NXJPhNGC}jlTMLN zlg^M*NM}jsNaslxNEb<$NS8@hNS~0dlCF_bN!LlAl5UW0l5UZ1lkSl2l0GBdBi$!` OZgcf-`h{(5*Z&9m&A7?{ delta 70800 zcmd442Y40b);9dj>^-xQRB8w{2}OZGb}!Tb(ggx`5D1`%QbG}>NV5V81O~F`Nu-Jh zp>CyyqM%ZO6_FA=qA1di5{5N z$F7xJbUsP0rp5P<*1nA0BD*i?Yk7IVL4j zfkUdS#oNnk`Bhe=ouiT1XZ6m3M+z4Ti~K^17%Z+%(mu&f$y22i zs@)0A?n>T`^SKwK1T8*2N}F;%O8Yr^t85R@^--9%)xv1)^OS88So0q#f#N$$vD&7( zdt`@0Yx8lkHm@E=b}~=2XY22`IK6^DTAv07EKXlQ$fwQS@5y$DR^g!}D(r#@o8GfW zc6zn*+n5piEq1Sm={MO`ZeF8Y>f9C7+0&$o+PzmQY8M*4fetix0myl?@d4TI)!O_g zMaxbNDy~4l<#5rf4}q%dnQG zzD>c1``z!zcBif#u?^$27M`t=o6e7gyeoO%0|%U}3EEBX`xb9NEK|t--@eUIB%Qyk z3!nNoTYNr;<`|Z&tqW|2UeJJbf`m%icgiaDtjwR0_BksQK4M%*viwArm*kvx?D9fVozm7+C& z>%x%y_hC809)3zcXmNlfX6@d#vD(EpTVX~*&U@M(ko=Ubz`zPqqO>dPVzkZeHd`Fv zY~J|foadnR1)Wt*yLTI8>*G}>Ys1>(#1GEG3~A&;k}_|Vs`cow#p3V_l{T)G*W4X9 zTbzEsF6_HHEtOq%wnJSRzU)+BF4vf5r?B4{jkSec-eK9&^*qd$c{)~mq3cS%$66LR z6QezN-#fAwYEHOq?{+}4Yel)qT6UjUt!{TX0lU!DKf3ReJwDb{?OcyTtaGD&l&Zpw zBx+w|Y=f<{Xp^;#JrBulFFF1VlPMZkU3;_FJ1~zn^Pp=Z`ar>F+`r4>@CNkG++f@T z+hBg$sCv5F>HFa3oWl~krzpyL4D9kfv|DyM1+7&d#*&QX#}AIvc3U?24G#unv{8v^-%aL9{$(Ntsq2V^)D_Tr5zox7IsMc zUIITB4t!7cxP_)q83bE*vq2gIdOmb-}DSZRQxhZty!+%X(&;#pQMAOz${$zijsipO^JG zR6_1iQET_?Tdd8jk@3&HXLSWIAA9rPpW6!`t_y|7{eIkb80htEOt)`5yekjGN*kkX znXuX7_1d-k8BMjEiQ9RG^ZhXzI!$_hi{#OIzM<=M=A?HlK3IcLU)I2`lMTnD-%p!T zVDZC^a%^BO#ZP@lb~$uYy)<>H>=ws7a=iBLG`3PNSd*qLe_@Mg#{qQAi|@$}yVmBJ z#@Z7vy^G0)u)m6=c_@B&#-jb zy8>o#X8JxEK0+uvEBNsl+a#Cf=$ovqp1GGDJNw5OU&Uw(UfXW<`k>QNZWHbK*H>D+ ze%%R{xR&m-w#q)gR^dvrmXr%RX0O0pZe7ubZH>{?IjdwJrh*F1+nb<$B~G=uo57x( z7_A_CCEp$J0}MRF8l`2u9;3ZH_iet`rZEg4eq92egT=7a`H+)K1NU{2m7BkZ-3hDW zviw->!v*cN<_j{xo&n90MKJwf!78Y`VS_nvXnvtWNx3mv7Oj_jInlzgR@J&JTu=Fr zNp-cvMVmx;5VAfStKD4mCagrnuGoe#+9!)Q!y1ghq!YVs$s6pUIAR)(vb;8ZDH0)Q z93o!dWqZKb{2E9-kOg&Jz8Si&r#nfx8)Umr%ZyJhVdWpW>)4m%KZ$jwpN!SM$Xg*h zkY}*vFUx1YFJik`+vGP^;#?0CNx5;_11s`yZkT;W({gnQ$?vS-k8}`7@<+sJ^ z?%B9gJO4?FwqtdyR;d6^N+>p6n+#pOTEHxfhhZ`c&urRk@q>luVFJ|`#b||_*TcS$ zmwgL`+5>6J|eb>l)eRrsM()Ui@W@)^6KU3{s(*88oA`3fpsWrRQ3c{pGdu+tEjN zZR>(-(H-GASsVLKq3m>*D9+>WE;T~|?TXdxI~H?d%5iL2evJ0}jx`*+ITP{hiPKK( zTmj+?M@75VAm`LmkLCH%+KAmNajx^)vu6u3X(4!K`v--+wNZN?-rx!_%3VmfcRwCShSY>-fGBM zk2$Pr|9KCRg^6)GR{UkGw(L*^?acdgEKaBioBq;6Z()wPn^Uyb3!}6pht_k}&I-=t zW_+-ceFl4aru5ejS0H){&%{dd%ZIoN%|q%xWl)^<;YVw%9>37OWUXjUls4vQ6>a;G zSr(7qSwh~Jqw6^~3%5|1QdR4IY(4JlQ?N!gKE4)tK=KuM4Ni=HJ^m)BV-?v~w2obr z5D3$<^u!xD*M&OZ(FV_Ma@VdIN5@gF4!HED7pvenn7>Id-HmX8TyP|bVUxaG3jf86 z)7kZ-IM>Sjss`5qFyrBz76qQth=}%QTz4m zMp0#PCU*GTUXvxrcC>ZHn^{C$ElfVYi#1pmeT??>*N}%0eX{2G7o3FC2*5dU+7I94 zvEH*ffA!5q)<01Y#h;DSc3xOvb^0K;QGJrg8YNZGv~O3)UN3EW6SFg~pZsox#fwCi zwZF;5cacsAp|dlp^!;`u$}lKID<16T_iH)BFg50rA3%~G%NAYQg#^PXGPrR+LLP2i z9$haZ*>Feq6?cJ>TO+UaYH zU_5OoMO%1%8FDluF?;0SJCKCw4&7cSit;u~3e8TEs?p?t6sHaR=>>~RXlk93vXdTd zCM9Yuf1Y9SKx}PxCsXZ2lqbLaoDX9*vTmacdd1>G!N?BYs9CFMv;TZlQ~wjNLOpc% zJLo@qke+zm)Mhcb*x|Q*C@h8IH2j^*I=w2$c(A;dcw>v;3oG2!8wVxNg6rs@m)6y_ zTQ`?LM|8*2cUi3V!5^EDNw``3W>Gesb2X@BI5G3!`bDv6)>%>MP3%qWh$D8V-yH}z z{Vu1=69}Z@qSNEXe;iI<-_(@hPmM@v)`y$A-GQ*CUR1?tH6*X%^E$vCj-u`U;!;(o zBzC!^E@&$4mwkGzLUGH%?_(C}E=rc9pk|+xLL017ER~a`trj0#3Rkk{Wa&Mt&ko^3 z7n4i+?6x?eshsbi%X_j35;4A4wxg4JfI+v`_L;&ki8O5+xm!eR}Ap z@1vv*j301rKn2m#$1Qbl7=Qodu6tw*&wc4 zHR)~H?_p;I>>^Qm1B>QBO7PleL zFk_0M;mOi8#bLKY!=P=H_7<3wm;&*;yngtQWcnjT+GugRMOHw^>PiPKez)#vblZue z>32&t=yE-27N+IOgO=8ZVD(^A!u5T~NVQS>N)ROOwOBZZ>nD@BWR9f=?vYknU3Qm< zy;QRy80A9QkbEqeq(&@wm(bSi4Pe@(Mv$x@RkC2w;l|P)+3TVFL72VuM%cuLRB64{ z=S8-F*hG_>;J)8z3^ZgD=k7+aA4lnF(l*!!@-of_DA6tzSR6)01W&}^Sq#NGq$!Nm z)q*_1Kv1KnafG?ubjT^qk^{O0J?oMR`8cmpGCk^+-cg)({7}2}XC#?W?b)(uM#nE!VlxuN95QxR>px$Md z02f$S`zx(R5H5DGLY31cR~YiB(#UkFDZID=Kf1!8N2=CFa+=s94QnI0L|D|pM}_LN zmr|{8Y65?xs% z_!=D~H`WXAkw)XtKp*KaABVw@xFMiNdZr_o5W*heSvyKiFuw^vx^ZlRj}$*oYNEKD zP6K?T%x9#gR-8LZz{eJx>i~G9tj?0%KptsdXUSpgcCWGz#AzsLvmo*BHGgM3}};qIErR)*gD>5oataAjp)g<>1ZPz#BdBV ziq!y0a+?^V!s|@6`rIL?QK)ZY1&vC8MwJ=VG=wyI!cj&#bid>ZNjjbmwuYv<9ey2d zq|pz6U;}TYS`T6=ejRP3nh#3OFu0MHJujtM-662iuDG2h%t&+Ih9=`Y3~6+RLyVB8 zj;&Gkhgn~B=Q8qP=7NqO(PW z5W)yf8R(+8XZ1Ksz(rjy7Dy@_VT4W#bde4|#$*^QU=povucriFjHj`EF|UC!QhYzD zg~g>KjK+F=2Eb^Zx($$#4)&AWj51PwmefOW`@9CqXg2g3D5E&H;=TYGDKS&mKSmkN zTL>v8%18$@C5H(!Qndlx*u)uCdT9pB#_uxlMmjhEYGVM7RBa%X*+d+Tq5Y6CP{*ED zw_nE_DPa)!QJPdgQcQgZX8$0}rz4J3?QzKy4mr}uAz+@v03GSQ$I%p-EV~x4)nNr- zCNM|iETBVpG<0W}(D*s&3L0u3LA*$k+W@3E;e&M2m9f>4fj zh?X>LI8((?qkf0+5P*$(7d?p+Y=^)@V56IXMpnn3rBFt@)q_B%XT*Y34>BM?j8tVL zxT0f>w0Dk_rh2>#F;emiQg;Q%VF)9}@ao_q6^?`pHh@LiJqp@tpo%$~6yRn715~t9 z(k!X5xKe^E=8TilP@5W{Vqr=Q#gCDil%R?m-iYD9q zJ{?vxX7U-RA_*#dI;O}02#Si7MAr-;X(gbdU?FFN-~uh41czb5igbOFh<~D7=`a}> zAAm(#f`c$r5y_*0D2j!pS_1~ANE^q3^MMeeXr=nE!}*~I){#Y4J-Zcqp+?y>=OyG# zxNCxocB_|5ImQ*!r(sSVP^9}NB1n1dT(SaGq)U8DM-?gk14RtV)VvxFVLmf-xhMXH4V4`4ns?{Dautc8DAqNDKNcQPitQ`=T zUOLQp840I}BpP{#*WuLBL~_3(%vQjOG?PzsJQ2Ez>5)m6z==Z58uK*+QY3H*c`w=s zS@u8$DZoUEg%xtU5-?Ga3x}k`iB$Xw^F~jHL98PLB#NNaqy$N%Y?cEuYk?s$Nmeh; zOaxJwBU2Ot5rqad*0PQ$lyU`IkH8VB{%ev$2NG%AYhbs5Br+Lr(5P=XiA;W-Patp( zL2>hVVJ3>m$^n@HCeu+wv2I4qu+yu9h)B>Kq69xU3*)m(;Cft)AA%1{_-CMqCBv40 zh=NT`xugUJOHa*V)qn%!vG&Xn;@`&=cGe>ZkY1O8Byt+%2!j%J4&c6{1X7HnD1v8l zaXjSxT^ceM9D&dnY2{7ST{t&zMP56O1+b{=0Llj)SEL}{3IU6T5&}#T*dq1Qgqn-$ z7kHpfi0VZKWZ;YC=A(guEwaYik(`59YzLd>L-R_YMN>=NA#_n6P4T)tI>M;;N1i6i zeo}If{~8D*^~n)xtgjV*3&I2RC1A!396EG_k@^t<7HT@=FdmaHK(jXk8T5<661$zDILsF=3USGYT(%AbEEPWok2`CyI#T6Y-0nDlAiv*ZAbST^fivKMtQMyRp!dz&n9v0Dnr&dImjHU65SF)O z6Qt)21M^%V9B;u!^fGZgHW7^P4H#G+SJY7P5*&{TFp!Dk!R%mzz7T+CGRGG%fjms? zF)_SYG@(_XD(VDHm1B+Wq6|8pXmkHl_Ldf0fEl7Yv$X&}4 z&<^8xOwca4ZmU#Lp-MX>FA@(tERsw^cSt^HG!ovblo131|K1L?=gr6<%8Z&jB_Aqh z2c2-pNi@B_>=c4S5_tZYREI`Zlbs5r2Aw_e7n;}Gg@Iv<7clViyD+ey(S)j$_YDUA z95*@CIQF4s)7_E}x#0EYfXNFFNv%-XAgi~@$#ibF#~=?5vAj_+jeYR5*dm?UQ}W_0_Qwq&}>Cx9s`JXBR5 zpi?0(KvEUrv9vs#NMLod1j_ykBi$$j7m&r`X^|xAxleK^!>UN`nL(qxz7@Oe;murLG*19nypL_P?X`-v4yJd72<7Q9s9 z577AOVazIaKaKn8M=J!J<$X!>6Efo^Te=xVwSjY#D&{ccZy&dU%R)`%R#$E}lT z=7;EuZ7ykF9R%!ncvYHW^al?GLj|3`9L4a+HHD-EBf1~M6-HiRcR~Pbk6}Dm zM-@3G=Qs}8DJ0O;4pb3G+{0f;m9RRxWhnXE>e-#}(9)w-i^Ia!kaj<4hx6vr7`Z2hTSS!RCHiy&h zvg>o$i@{VU3d){uB+$rcxuq4Q1t%;pt4eYV{b+|x;1a?PU<^TgD>MOmJNE!)C+Pgb5uX{iiJ1z5)8xpxsaCq z8;m6Sx*R^k6|wNB@N;Z40;o2uq2(9Ck)e4C<=~hdqOc6HKo=kx_Ct_cG<7-&wxWR9 z_o1->3qWq(@)Kfcg#-aRmNzj@9yb)ZKO}w#VqNnnpOY;%GtB)?EEI0F`8E34as zXNP$W`kB{`YLRVSEa|SVAPwZ{hJ(dSULPJ)gpOR2Vra)#i0)8dz6;j3RGe;~j^YXZ zjG`f@!4U`NQDPx6wEr}Ez!wR@6h_I>lz4_iH#WqX#Xn-F4XcAh-}AU1Xj#puNllwfT3c^*QzcA>LvzGgY_6J|PJEdtZv&~xo; zro+#{Bbhq?i%*Qjfg+mp`euwZDncv|$%5tV3 zCKg2pzQf$e^Z7#jMJZj2zardaYYUzo(<9-B7qJ*OHhN9+to{HLPcU4}3cp4@*I?F> z-(z&_Sqm+I-%HnecCE%&TcbAN9a9l$(jgO|qoR3h3$}3e_Rzd^F2D78{&fkHa;hN9 zJ*~*zplm#Y5Q6S&0zQ^M#WjQ`_6Y@6S#JY3&WVJ1 z?t`S*ZUjfZjFIg;BQ}#qByix>Pbypy(u$(1R}6OWh#jv&Kad7og3xoXa=;bVi*87w zP@Cm9s+yz9EiCN$rNd~Hsme}%+-TpY)wmIFHzAcuq3+jN1(7+idDQ%!Jt6#@pc=u( zskrRC1y>Rvuv*l6$T9`pf-7*@FlhvQSh2Vu{RG_+o9rUUU^~OuoB4sU_*N=rhW9ps|?)Ei)E zFYlr2iNup|6ueOw05T`Md<+#LbYLqBk3kuP&$)%g+0ma>nb~K>iQ)835rG8Mx4OkX zrpQkHRD_-pzd~8;+hxBoNI+%K%7YB8B;sSIuuP`-Nc&k zJSK02gtQZz7YI?P$iy=F1n{=Og>!$fNPK#102AHV9^v5)2OmXX6M_fi#E~41B#OSf z#cs!lhgdqox}Epoc!7)m;?_p$booyfT1cN3bdufRIGi3UH8-U;OYm5X7*#Ze|6sFX zbM7++h$}N$EoOrmJo`k$1$sXPT7v@^RG2WRr$;*&s zFLL_qAZ%orzj-Q_h-&OuzgxW0Y45S?GXADMKECAxpkYAv;r47 z5HhJ2Y_RHiIg^XCfG%;U5aUfccQL03aoF+U1o(Qs5Kk-d=p^&HLn z_=I?1!-$bFHVlb`oDD`{w;jNM^TWZnPb68!xUi6Mmh<`q+>@>m(ur0;jJzh zgzU{HHQ%jy@=n&X|?O?<%_QYZ( zBgNVP_S_=`%- zg{>w%mYZF81$c`lnVw9*@Tg!bv)4?J-JB?tY^*NER=H2>VAQjHYST zF_?)nq}70@0n53}#j(o@OiOIpC)4bjXoqp|W?nL#<#t{YSwFW^+;)JQdRmBv9vPuS zMC%JR;jUoT7tu+KfUqornUJ@=2C7cCJ%Fc9gde_K3&Y}Jnv+6_;l*o0Qyq3Fgt!fv zdVx8WXeFjaW#-av-dBfZX~d9_8bUpQAqcMCtOE&nwd~EJ=#QcYl5h^dvo2Wr7LF}` zSh>h-Ey=Q<>oH+FeUovEpAr&j(Y9pB9Gge1RKnNArQo&)TR$Q?KA9qOKsU|`Q_$E2 z&&gF^VqM(gOjn$Zt}DjT;hm3gi)k0DtXq%o=mmyQ+ex?p9txp+UJt+&kh>gph&1X;tQV{wRO6Km+R5t z89|ZTW2W_jBp^zagQ`I1}R)jRgy^=Js^oXt^DIKThsVS7zec@bU79$k`%RdGd966djw0$+H?@(WUxU zi{$XNb@Khxc@`SS6@&W1nwCTw`J=T7FQx%yOu~&ORs|1%|7LAWllkaF!nF~TzBS!jTn%s#FkCYn;agKXXsYFd?$_?q`X=n@fKmA`v zdnpCX*@u^Uga&B~Q~Y{9x@WDnM6_>D&IPzJEJq&tU#Ad7TV)ru&A9{f zLhKQYk2w9izU^twGOYQronT(^atLVoa&YN5{G{Mnoot*5MvI&71t6ei9-N2iS`w(# zWF07b8G7(=W%Vx^oc@lcgnzhOYQ_ixZtGiWQKQ46La9;iM0b}v^SqR*wWruzuu&-X zf2A{gVddsS8(_h`~#DDs49%DSiu< z_gJAkNN0+0bAN6J%WhwCd$R9DL=cojtTtkf2rnu`m9`HP!C96)QN(zq1{hOfJ?&}i zK~vo`$Qna`9FiTh;gH-$SBEBtadgKU1!)$fU57yiyj2wac-dN=UjGPUwjGi0rZar> z=n;hSrblqJ?g;q(zwObVrf}c5VU}7sgDeTOVS%MFoqgQWmNKu%cy93tOLMX}MMAji z3u_{!e~Cyi@))wmnoV$R_fa;GCoHvSPE({SxyKMZryjFJQE_8y8jTkKr~pk)`z&>-wQRkIGM8DJ(6I(cX11Sz z0RLQStwl*+!FGxSZlywrr?`~|ZtrPj*}1v$}=WoN^=ee^*DSf5X z=)A0!ajiRDD~3=6>oTWWI#co2SoFfLaSh~lpd_#LUj1&;XsivwYeiYRFeuVx4B8H> zjNOFOKaYb$qHnDWEaaD%v6Jvm<^^!0*0=KR(u^=hF1{$+XwI)#^yKXr1v*rVE@O9< zTCcE-qgo!M%l%eby3w5Pk+4TB0V)M4th0D1%?TBllm`uK=CG#fv}Ak?>&v|a%_-b~ zCZ`X8O;>J0@3Yrfe3aP+?OMN#Qw(bGK-@Up%sP{0ge8<_SknSaU#f8hWQcYdH+YrY z>n$Cqe+KBqTD#NUpP@go z!*ux=bj0_vPN7-9LJWc@x;efM_W(8S#ER;njQjNkccY9vn9rHs)_FAbCouZm-@?f1 zLPZat-JZ7qafnmTS@8Sr#YlS*`OmiuP`@)S!?4(2X{MGH+o4wQjMs+>-lI>qjxB@FL6|UZqv1JwKt_vI{VV_@8l9D}bX9EYSY%``ThCv!$gWwYK8M+Z7@C9}e@v zZpm(XUBR_$mvIV}t!;Recu<#{i5~ALxB(rlO+~2|yc^ybPVsOiuC+b}Ln`!xmmhZ4 zhf1S#hef5_E@*P}V+HL-=2@GLkQ@i)!LNOEMQ`D1(QPtAlw5-`DhH9%vM;N-sa(1aw8qM7BiS#HJ`S81VoPb z6)F-pz!IRWSPZ}HRbKfy=vY!G8&vc@u<2?n`wZ?eJPrdia$!6;Cxw>xQW2Ggg3?<~ z$nETWYdsnok3nxOg)>In@zCCQ4BD~^Xr2=fai6UK1KV>SuCXrD)Cyp6x*Mmn`&gnW zGsaqjvif3bp~<=C zkXG(rU=xU}PE92&W@CG>eX69hy%+8M0t8*YX<-wMqWFIyZe=855D^?iJV$J-M#s;i zS^B^5PiT?p#R&8_&!e#rR(1`rWnwW-g*kSu!A8LfcH=azoo%kOsyf-*V}1Sm>o!t@ zX4J%8VI!w%GATt^X$D)SX-0{ZSBs^?N>cDFWZbGYT+SL5>;q!dky=GzRdX(bEl0k= zl*ej8lans!gLa~k7tj}zccu}%ge6~N_>o`ZZj&U0%8XVx0=b>(uWxY|%GrR%xS0PI zaAU+OKuLx5u&no8xLdnEMqY9WNBwP>_tGV_1B`Mvr6q!v5cni|oO{R|xk)p$10y=p z#CkZYRUbVjdNGPfHALg1UNj!=!;L>{VyLsp;8ot;mU#NgXUPCJaP4RV+->YfHzYR5 zP#S`&Nq%&L4vHw=i9YIp)7_1+n1n_Uui$vzJusB?j(npd2H1HIj*#|rp_qm^5-We5 z8;h0qegO0VEUHIuJpfL^6Lh3od?Zv21XZLzhuGq%=Do15_{REdU8w!Nph`FkXhb~y zc#Us-k88qv!Abcj;W|#&vu8$h+?T0>*@txCTx798TGEj-CGh z)D1lZSS-(!5v)4V)I^Azr3Hg89|~?ru$xjrKbt}|sv$Kl6=e;lE2L&>Gve|U`HsH7 zTS;8&n3Dun+`;TRQ7(t-vcqziZ_+D-(g7OU)H&xU)JjB@vO@k@4$yZD%fg=^`!(!F zgpk|y*Kj$14Y9>Ws(n@s>oZhX19SYnZ!HzP3dq`R*3z_ z_>QcaqtsU%bM6>vg!O;W8M69E|JaH0{<8SXjIJ1^)1704&bm{W!;JJT>HEX_l$|Kc z0Uqcn4_DN8jKD&)wT#r*6fNh+$S~T4rEY91lynQj|9?y*;>x2e%HNgix~5 z+KoZ~J`GChhbA9BYH62q92^Pdj&dT~?zPmj zzb01^@~&lI7Q?MI*D-@VjjcU(MB$D=L{TGa2OU;u#iea2T%tdnWtiiCg>{q^R|38$ zRfE?lUH^2WOD9o>#LPo#X;wla`?S*DA&4NNLg&&w=Vzp_g+GIm$vu&uMpSJ>os33d3rCDrmc>V6hARm__ z%mlji2|y=Q7)J4ooL>|OMu9HM_uY1+9{vBVB5@rJZxexm2oNO%7DUv|LP8PO1Q_&s z0pkg!i)=J5g=26@58Pn_jabSRc?tM1Yn-J=&Nf^MVJ)KWMvXl|FS*mZSto?o;Zzl0B{6vYF+@28-Z^Wqyw4wayaxh0#o1~IbDI2gg_>&%&6yqarkKOuR5Y) z)Ze$STX@PidiP66 zTm2@FC*l9Ry^*BDm((ha)CxX_+ZpWsHYiM}`5%lq0?UF24~OM?P~I!>IVI2?!L5`H z_L-G-nAwF?Rm8iHiJDw`j&-mL*`G#6Gy4(i935MGY$70v2(Q3y5?)1U?u1B}6LL+$ zW1FK3G+)G#2!HuccwLFD=|JMer@)#f9jv2u2)Su#^iw#ajjsWiJ=Ywmc=31O&#kxN zmTFv(pQk0GEnN`yt!23v(cLIwJ zdD9TZ=Hkb3uo$n$MYv=i{du2t933AIc9#X-3ulk~G{WnK!{oEyLIf(Bh$N-^MHWyQ zuzS*JEFM1j-}G7me2WMkViQ6H6a=pEIt(2Vg(GBLR^$_4Hl&MM$BWD|0%-t-6wS$E zMLL}dS%noSW#AP8Q47I?5kRI{-Xd8njX8>%bWaKdb*2K+f{_Yn3Dp|n?hhZp&y`ht z{(YI=vN?DvO6?lAkWh)Eh@$%pZw3H|!0QzZq1_S96eWe3u6Ck`!XA}F-+w6emNC>@ zf>)N6&5E*u)6~B$pA%sFQB+VG@)c>W2}+cWmqt_-#h(E`Z1o8E0|fInIQH)>aDKoMZ?xJqldC1I~#} zzrVqNJ5i5TMs-w@<@YEPRutgX9V+lnG$j#X5(Z{KemLq(CLRQI(||L?oYv|2h<5~DFJFSemI<=*K8LwwayyoV?YI(X2Zqyyj{E`+{4>y076K$(-tGH7ooQAh zNaPO4X(#%7_zA9}6Se*o8zf@)`+tFL2^pzPfSX|#*Edl^c@ z23Z$cQdy0mIs>>C=Z!QGW5f;{{5o9v-)xnYwHecyM2HvbHC?I!OO=iZ|C2p5( zEN3ioQY-_Q1Mi%NvQ9C_KYHmqZ!(nOxJ8MPmut1sts%Df(mMv7%LK^H5O90)ZSD@x zoW{Hv_qUsrWrZ!VMOckP)Zgs{-nMrb-4Gl_qLfD02B|7tZiL64%m=S-do1f8A=XDC zp9Cw|j6sW!;Ng(6fz|$D&xOIy8Q`Hno6`M2;+i|KAW^866vUxI8J5|dYH|?>v}yw7 zUHTh%3WFP^eMFfEY@izhP+j!rhk&=u;0I>L30N+IBs}C3j<1Er6xC^{Fg-Tb(v?Q| zP~ieRS*BOQq9W|y2C={d^Y~TGeG2SuQJO-84P^2^wV4d?VMF$nWjJR z1I$$7$#D>D83D@36<|a47<>|&e%x^J?Y1Ve9jm0IEKXm zyoMuN_tD6~fRuKh#PeeUlrD?G0x6BG^8Xps5}b=bjF}X=%ufIU%9=;^LTGCUNNV*G zu8F6xLb!h^T&X(+egUX#E;j-s>qb*%LVL?XyKEn$M8-3S|6{x~48?`M_Mm?6BBjW_ z33U+7;qGg9ExXT9$$| zH2{7vxEO6OzJ%c*K$Utthvzs$^kn8({Url;puqz$QtSe`9(^(p!Mfi9Is6gd`h#$- z6e+hYXzC_w9AzK3B~Z*NOnzgmzLtS>bSsX!uaWVn^bD-vb}rpE)G@N6svT@nFPKq& zA?Op&)F(Y<_0pLkxVxb)-pCSnZ#~C%7g>9SEihuyx3ckgZ(cFF^;?d`h?hTnLvduX z@5!)7nrHBh+h0v-Pp_{S7fU~Qzn4DA*MzwQ|Ma6$fcVE(PXt0rpq8Y zHrQdZb*aS6w9{)tjWq?oo??AXDpO*m8|p&HoTq#8+f*$=^&vn{@bHV)@|G|nBHG+R zRZ1DuowNy(IB_SUO{3+P1ck7s2Z8mLrEo`IhPM~W#p?D51fO4GmCMqCQo;y-g{(dO@tkQtrmfUcsOT;1>zh6tTvp37JOtwo;^u+EEyMEhPxLx>>^LyrV2jDKwrWo`=2r2nm`B_->i> zM)POZWY4~;P?L2)<&NY^zA4;c6(}i8uo|@c1fl0Bm}Xf)6tBmMbuar9?9Wen|Zr8vmiO6ibuzfytPw^kNl1t7LA6^#S+QR&J{ zbgH$IL>tqUCHUZg!&MHycGE`5r!H+UN!7N>IjmsR8pTcx+9~s7f5Z$6DcDZwOy}-Z z=HlZbquRsz9n`dgas;1Sxz$a<^A{a4){PDr3-2RVqB*^lI4W8VTI~5sWonYCbf>AE zlvJ>==Mzc~ipc~mH9ISlK?^Q);Z=;Hhq_=Di#sb(bYlciEi4ZZTW@6ujd&J|xVNi< z4=bcn_Dp^06=>>sC56s(RVL!Qaae3JO}I~4hHqS_()n?6fRcJ*e)Nx}(cP4ppc|Ba z54vO8D!uTOjuKCudw@~S;JF@(DzSvjZtGdpuPOIY-Ujr(yO)wfM|xsyhk7ZeWp64y zG!1Xr|8~C;1k=RCCmv9ifrzXJmBaj?y^J zKFU%W@VL^Io_tJM1;T`kV>{u42Vit-%I>SQ28n{()%z)L@Yf@;@PU-nUs*J6m&?68+qpEDH(gimTk4^TdY67-y+v|!?aa}E^d zQ!r4u0xm+O>QVXx@b2g!Wj>_MY=uR|(yqsq`8vJnVad#)$fCENc9I&@pZFV zla)mJak#P>vlq=(>Vc3r`gnvg1B~4?O?ix78>v*GbCdB|gW@cuFZIrXF87`Strly* zT{a)^sw)i{C1@6-Fm3mYRx}#*6z*Jm8Jbyj3{H2B2H(p2mFjH8(e&jQWgfjb1~xi- zJf`0^jla&}I>FLQz9VY1jIE))#f)r*E zlM+Y$rYf&f!>QouTYuTA3DMV_1}*w=DyD#=Y@@R|<^^Ro6->i5u-?U8W9i!$piN6& zz%`SfF+5x)diO(GnBpHYUi2o zAr3Z*ZzX5&;%ka5ftX9LD<9AguY=f29U+0@S<1_%s=hK?Sx8UJhWHUxqO1-lyiuz; z@bgd1QShB!rt>V>v(a_J8VL9JJTR-(TqT>oUyL;sV$Qkql+&0KXHLq~5Y~kCHlv~i zFthRVm6s`XzRssanw*Ulr!7!kpg;NAg!Y&i?eaj*s2t@R8kvKu;Bo^NU^IRO3~EN8 zCP>O*dXS*1Q>Vb8?OF)mG-V-dx#&^FN}ZNNn^!DSX7NYagrA+f7(VFuTm^c&1OlzG zL?-~hVY(RIElXj?_(20!1Pm2R2bT&T0|f!uB_rl81E1Ww-^*SGeR*Ixni;Zl=0X>f z$10WM1`T;?*yE$0;9vj5m!;tJb3t&qJj60#)G4|fESHb=pXT8f*dI?HFAj~?V@p(mjDq}75Y2I46R(w0`P323P`X;7$bc6B`6>n3k z(D8N3Bzkn6X(2(x-k2WcV#(eaMCa!A+fUK&w)QD z4Jc5KVGZ_83chJ>1fAG_JEJWCC?D|ge? zC)9dW{EpHKe9qXSrjh*}7o;JAZk4CZKLixu(F9C z!<42Sg7@lp2pU-Du!67c(}}}Sd@*lBI`jd2pO5lt+p5u(pU@@qJl323zS0VFwE0!R zi(IF``jn4QD(>B<45oHRk>_OWQ<3o;RhDxmBi+E~0y54cB`rLzVB6pr6lG_gElKnY zx{fOkNwntzlAtjs;B5{S!L*7GEBKLvv$Bnfo>Jqe-~_n!@(J7+ITSBnf@+N*fXe)+ zdpYX=o?4Mwe4;Gn-$kRZK2biSQ=g!1;it+$iobva;Miv{#Fsw9(eBTcHB{kE6_3MQ zRr2|V+srlO(!D34KD&NYmRs!bpdqS@Pb)2D{0hjXlMvyTry!(_r?7-azmcoZsU@}w z^yXJeE<13?Y!xqdVqCaVA#dEs!*Td^A#-j+JI^ZjFw0&hN3oK`=Dtt$Gt-vxm&QW` zY~t*4>aTKDYIzC#Z}hb?m-V~YZLLbDe*|~LXgj{fihKO3)L^bIrx*W)!OwoGJV{@D zqwv=XD_l^DL5h(7tvA6$ZpOyUIw(fi{jIW&>=z-2SHEKs%u(^?9}}8!5xH2#uQ1Ma z-$Quu$8ZGf5n^40s#O$Ts zm*Jn9Pqx*ezE_kLpi8={oTp8Bwnk+CNy!a&K zc*y5{NnH(t94@Q)@?L6ClU0|*ju3B0Th(4POorom>}gwd%B!Rf(yk=9cOZ;{@b__8T z^(~KAEr!-mC$gsO0ORqd7chIil@*0sr{>@vJZFgS@nH&zjs~ysGCZzzR*@@O)tm1c2@ugE)CW>fFE6CH;y#52lhV1{c{y@e_~sch>4Zdc{IPWI-O=# z1{3hocP+hL%_?Zu`!4l)T3-cpF6yPGi)rzrBXpvwI){p?qCubjm?*v)=FLpN>0NG1 zEd7-LW=nO|m+|RYd@>W$V}bZ6KsQ?@z3Gi=5agaj^;x1s936iQ@rPF@vd_u08|DIv_!PB{-mlnkxZ`gTf>w;@jDY@VRzvbqdAQ1_56_thUWb zf=HL~*`l_PxM?K!OQldwtMrxi!%~I7FWKYGkHQZ`r5P_eArQF)!PoW*u zB61F*PEFKkU8D^=sP(xGec1%#M%4wMhc;EKQTa4A<}Suw-1zfQ6DPixB0_w}H1%y5 zfe7)9?doFsIZYKIKFW$83P}y#z&EIcC9H&!gk+xSQAg0UD&Rw&OC1@U*cvh z(gLIwVIpSgSEcKwB)%(AHK(mQisq>pd}>?e79B6D~pY8rBtq!do<^yzXj01fBR!wkFKa5n!OMYd8+x+fAKhG1Qco z+PNpIXIldb7-LJ-3S?BZU_`~`tUX|S_p?lgI z8@*%Fke{d3$*qz zBzJhVIgv6SRMy`O)pVoW_ft1MDa zae=N3&}D@hp0iV)iDnGcWz|$atv?7J=dMwp7eg$dw5@8zoWWqyvybCCa^iY|162D7 z#B(IZ__0RLiR)2wuPw^zb|d8E4_5VbwgX)pr4FEb=Blx@X$Y(j!E2SekS0B)@-Gw# zPi7`=n`rA(piP9cy`L#@R5)6FgMYIa8SMv@ITVwNO6}%4IEVqm;7J+{LtdZW93@cU z5O_Rfc6ulJei#N4$=~_mkl#DQ(GD4k!Z8`|U51Hoc^DHjl zc_JQPn*v@GKCR}``Kw5j(ucs8cOH$rMnrs&xq@bnW(69m?hOtcqwbODhp}o7bY$AI z>euj`=WfDf@NzhByMeqAB;*I1j8k_@)Lg^3Gbca-cioWj`6F{K8K1Uqb5u>GKF`CP zL;-;ZMr?F-4Ll|HmjcY0hIC;Pma}sb<`)AXbq3BcO&v@rQ;;Ewj25R)Qq)u^Q|~D% zzAOYvsPj}zd2=CB3*21|!bH|}Z92kGuNTxs(40-5DoBX9?GG=YEw7Gtz6g)H@g;Q? z1H3)IkqyOW<}e%xf({4ky`pAVSXK&ETc@G;=n zLgbA?J5Yy>2RGV}RCDoLYGn)ojx{9rGKeHRgr4%?gL1yx{8q+MTE3S5Cs~1Tb_sY56Mu>JlvUbNsr4TS$}g-T*~{-`T71^sc-T zr$bj@wWn68N2$?Dj@Co5?xw4t-uS69y|Yk-L$qc!=v}-(sz1u=fg`1h1t1;n(o0jes59ZokQOvx zuU0`^ppQ1`KF>IvwHXV4bc zE~x#h>LZpa{y_!%%O=4Rf0_A#ieH-4ZD9Vpa9%^-H3VH!h;Ps}>GF>t?Y^DrB>H0q zrm68JfDP`=-{D8`c`&$Yms(k-^g>w1ioM7We%r$%m9CXhWi8;63y!Ldt}(5?Qt@Kc zK4=1h*y@F%jUC~Xp3mo9W zB6OSi5jb1^Dy%2z06101wgXaP=-H!inbV#|-HzdxQMqF#wO=1ouW{dHSe#I`aVON} z&`p?NfHFQ-HC|&=dioUDh|m0sQe4=P;L7*=pe%zvRqL>~d|&+pKJxlE>U;FiXQsKm zdmVx9q0iN9PB1swn$w>@!Sl}eLS_5|4z?>z{S1@iC#+7QU%^RrJ^3yIdVl6i$Vb%B zv7bP*o1enMn$3pon3MBvoB(pVUs9J@yl`?MUVMF8T}f}A#>{=ckt=Yf5l36kz~)2^ zC~Pi@F8zqnpe?QGhqG!+>T?z<6Y^m~ZvZ+2-ng6o`U$~fUa{JOJ+dHLObS#kY*Oxl zMLH(i8VaqW*785B>wD3-{dSCv`qpZ30-^^tjkn&<(Lc+>L1aFVMZ$f5D{Rd;(?p@k6K( zLf2A&SYmlSKjtC9pWvVWQ76Cx#{!6GdlQ`!ZeqDtZh{68ZoATzKfpyJ(n5!q(ezvJ zr(J#pvc>KUze?;(4Nm^kC|g`qT+ucIs*61RE&M05k*x*=B-;|IC7~ujE{zZWq6K{_ z*@9e`*VIo_Ex2ps;<rEy!Ta>W}bF2SZzz++E> zXR90oLXit6>!-tFF?niZgEaO_$Kz~AxQ0i)&Sej><2+gtkJHy=TR!!ufNQ)kF5au~ z7&GaQ3SrBeNqZ{Vw(Ct$w_#I_oAiuDA;L$&&nw#oN@3LmDsBYysG@lEXH2oSX0R1lXMdZz>zwl#q+?>}F%t)~jzbbv1h7e78--@42z~0F~FsvLP?S3CvndW*qmt zh_b}2H+31(wKOpP&DwbB0EPP!T`hV$47EUY>V}x2+?W9LcpcU|ztMjuMrzCX?z#s26POf}R$($3w+y9yMO&CUR#(4sPAmOTQdm(dgB z!SwJ;G<-L{7fvCqeEx|bU5HqrU32hbVN5pWzH zYuwrxe6#VcIngI_<)tjw<7^i}#Pd{SYDVN|QsI@Q(yfE~-p5FA8$1oxs+A2ER zm#6Bh+VvZ=`_ez5 ze&Yv>;ooO+q(V$$DVK-f9uy;=Rz0ORtw4!GZA*L0iQ*&wZF3) zcn1hA^xi>wNF)T35KwxrDj*O*q#IBOosCohArxWgp@ga+BE1EWjx>J8u>}XW>Yg9!{-ihcI(vq%W0jYj)Ej z$(I|`>z>X@KL3LKGgBf7MUbWF2$M0<*T~dK$BZ^_B06>AShS3!rFcGi9RBtte3a^@ z{&?SFj2Ys_<0CQ~d~2L~{HD@a9~O8MeBYYxlYIEFxp$8@)h}P@Ta9mx%ljUzdUtSU z&wO!#+}2>0^xkGIde&#m&}Y%$@i%sJa1~d;j5*-zWp+(Q;c84qfw35=jWZ6uR8I9+ zHdi(CW}^@|cM)8Nd39-QmQBU&p&4-8vDU3(j!p9=^9_UNGGIXx#s}B(fRrr#{W^e- zJqN+l!f!Lsv+($a;A#_3<~0*n`udytvrwkRGtte4FY&#LZ<3lu=d(k5NR>VUUN@tZ z#`6*C&hf1@Ip;u+a3ms3zPZpNzHt`AZWY|nNL}OWV|s7G?qt^#Um5CfJ(l&uS0RxO zcaU(S`52Vi&hvFvCUGt{BvTgn!eok&t(Cmy^##y>lz&OTbfGWJ3|Z*QW8zn!FES}R z+%wk8=F}o=Toqn~#s;sfv_G4h40iYdXZ5`bHX58g#Kapox8B|Y;+`-MEb-x`qVct& z?bo7^UoSz?MlVH+X}c8qMmzCmgiBcBRyBo}qc8t$87|3-u^r~XasJKqc!%NJnZ{Lh4P$bINompqRfROzLKF>FXQ7vjKRLr4@Z3U@uNs=d`4e=jxTJetS$o;X|C<+N|SDSjHG|Mk@GF#~7-yh`<1n*#MsR2;o!eww_F*po??<1< zFQR5^9Gyf#@|zQH_$Hws-eJJnw_%~lZ~C6&z;|RcYErr#Y8ie7PGHU}?7kZ@`&OTz1QUqdtFZA|mnGCGa7`?cTk89N%wC?1o_ z7n!{}F3j(MSuFgN4vDA{UMsTxgLR_nMd4d?kr9y*I0IV?FMlE;@LBZeI=B+mzh<4N zo;Au3>;KHa@(qwspI70wMx68&+f(+cuPYV@bF$?(8Jn@#Zk8A4SuW+uUbNbSQT6L1 z>ALuOb3J@A5+CD>u8%{Bb!y>5<`MYLRRk{7ty>pK8}%hDhLc}#5(+_AlmCXP_PsAr zyCPruj@uDbUO;TsE`9xb6`P3zM75!2x# zpEe(V?W=_r`1KY&z>NC_lL5Zd{H+h4(?wf8#JBif+qv?cZ$Ffi^1bh4OiE>P>q`8M zMP9S%2XrtT4XP2)TvXlqI85_LiVLrYuK?rY@pbA)*Nv?AJH_G4dOQ?q5*oq$Cx7sz zm})4a~A1eJ`;t$;l!0&BO>~Iq&bV`-waBL@%N1|AC5|;G{Iak{S3{H=L&Y zy}l!AMASmxjf%o|6YJp2Po4i+BKTuGY~1fBjDq;m*`wLYnFsDb#rQT~30>37`vNnW z`q?)b{n0yIb?IkJ;OL5(?K2~n;+78~T|qYSCQ1PCW=a;FL@u-3`YOd^yYkrE8!>UZyz4RNg#d>&YmV zj4#-yck~n#3r(S>*&c&(dR^mtM?Yh}$gbhGT*8I2(P)|5L-aKI7@zs-njzWrdY%o) zOb17^3YekUwQC;AfeL)Z0i}pfPs8`;yjm1D`+ORo70T?qdpVTxH1^0GdY)+=ipyjMH?5UM?#yo%IJEgasj;tiu4-7XOyG;@ka~ljeN%@ zgCE*tAw7q8@;LIK!a5C~9?ZOq)z&r~*JxTqFXYFJy|(mo5j~rl^EwwQzMEs4jDdOa z7mW1=P6fDJ*fcDz_nTIwpv;3qF%%Rnp*NY((g>a{q0gX9UfbDKQZIs{FmjjXR%Sjk zwv=ArV2ZjY)r7V>j6Hv*Nh}A&<(#E^n%FXW9Y30h3cn2|!|)I|8zVyUo4nU?IO0%MJ;8)k!NWHy>MiEuD!POz8;c{-l~B%258#1>%6bP+ zH{5Eh3;G|t7>Cg!xf*V0Pz@tOudVo*|IeH}k3@omDlUpxcd6O=J~jsyEj7 z1_GbhQqzB-zCSpw7J@O2pqQpPv2wv&liz%C0-dIO4U}s|ZCv#x;L|m9ejhf*P$TS! z)zD{AWsHW{*{q4C{&*c6K*W4k!fdOh^W>wWz>~G%cwGvJHm}#x7fgJFE-AlG%WE#z z)@k^}D88uaHEOT5e;lrp%;%99>L19hv5(wVmo~@hpo?&o5*PqqHB;-NH+GEF!Cyq= zeo%AMF|+4l1Snz()YT)*xq7HTgGdB%q&~md)?UY$HW%>H;FpV9n~Y-6Pthoi1I+bt zAI`~Q)_xiHB}M7k{$Z!#7<#m zcq^QhD%KQ@VRj2PhHxEkR5QKEl+LXybFwUz-P2h#TTd*`RzS@)n(Iks?)O-`W!yo# zyHO4eJZ}p<;a(T2gh_9qccN>KZh zJR04`%-y_`9*@tQ2BypFopjiqpF8PRHd;UjjM17wk4xscs}p8sTi|{_J;yzKdJ}%2d?(K8DFeEsQZ~KPoS0GdIi%*dNMq* z$ZnYVuunf8es{}Z7)|PR*Q3p?HGm1@b%L4JL)SKKd!g{l7Qm2s`rw&dkL$T6wm0JL zOhCt+Hx_XhAJ_b000#bcCf{&GBuqd#-sq{v+4wFl+}ze?X)nFjZ0Q4&-5IGHo4Ipw z*Yx(>D5fY2;Z?E;*ilwr1$j%UC~^k9A(N4g2{hZ$ExPHzPdB!8{UQ! zhuKZ-bx37;KfM5Y$66v#@vRqO;+ao)GwA#Uy2bBP8Hco`+_}v|PwIL6q_mgwuoS2@ z#{fNx-*<-U@r9BUl={j5J;&@Cti#QUxzJnkd`#%y4um%}Vu)^Lep`a}H)@ccP48qd zx_JKc*iiOj=RFOTHXEX=qs&sBJm&UjRQQV_dXV|+Q=-&~LouzNON7noE;5!oj>M+rM#-m<*W*AR$+2DA3R3$9S_RV!h2@r zSRMFLPg{RWnl%oI!uKApe>5v6p`Es$px2mLuD>#e%rnWlO|bFw#HUYnZIldVZDu~6 z1lJ7{BopTjQHYtnA5HovSC7Tl`cS`5uR=NKE5^pQw^vHfEW=krr&nj8yuUuJOY;NQ zjrVK(wd1iUZ;#1(zKNZNnOhn|wCO%YcVT0vV;r6a_st!Osp{qwbc^fL{U$qR4$a_f zALyx}M%rL&^TkkIo!|RL9<9vTWw^iVbUlrJl-SPu*;}TLE1OO;^dL+=s2Tf1^I>R} zL#6|sTmyHZ#&fz8F5=_I&*{Suu%XX_l|2ZHJUvt6CscdXEFGfI0;F1aSvgxTFazeo zD!()51E$UMXovhA8mzO<jrvdKDPfk=9T%-ePW7UhIN;>e({=T z%sf5E#%HYYJBM~=-dsJ9{UqGRC!NvqyBJUBO^3i`jadX0V05{L0>X^=!tg%$V`o=m zUd)*SbJVHJ=oW1Bi6-A7bTbsMEt|k#Ch0RjFTzS7(4jJGGk&pNXqGMYm%oHxX-y1@ zJAN7B(2Wwz2cw~<9!vCi&Wlp1{3iEO{k)A2IeWkOL9WZOYvKQC7n>9;=1n7+<8Ef2 zN0)nz*;|u%?SJbE6ym2;RNfmocdvxje_btQ8n#NW<>)mMYnnl+dLC5k^)P9abNmgv z1xMz*L5q*G;Y)h=c|+)B`bmEW@Y+{=8q!9e+XB;iov_7v zz1Tk>uoLDt(HqdnAKr`x3s)<|v>S=$e0PIhf{_u0YvrFRGB)ZIlosoniumCWdeh6B z^b6+Bi)cfrD^{%&@aVkF-Y83>FJ6jNp4bBa{=x7)$XhEe^hC}1OvXaoANdkWk9mzh zx%9?^LR*mw%E~5oW-AsY-ni8wSg<_i%r^K$863-`31m0Lw_{d2wL>@Y(pa`#^F3N- z2gkVE!@T^8jx@`5LiyDOp-!1Y_mUl$;~w9I3kT-YBMFXg@iQ>6cRrxIo4lKKBzwG! zbUAlo;S{f){ zdp&3MAIqHVpVjbt#NZHw`G2k{Ge|e!F9>s)utWM)`WaY@1L$nP4kr%jx6Lm{bXD`y zAxzh2j>5Wg(oq9*4AYP&^?MD2PNk!WYkUQ98AtU-qfQ_${g_TOZw93`Cy~mpmoa0c zAJ>g-eDD0$HKg{-W%LHdnAp<@cE5y1iEXBMlj{sp%M4yS0~hk{DY*X|&*H+}GwAMK zaMU?}aFcoXb&-%iHh<3{|98(L?)+J-fi_=&H1sO+zw|m%NxF#KGK2Lm!Fac?;SXqB zynvfSF5_lTxUZEx-@FD*deXM*h)cYRI!$;N zNulT)P22Y%WxtLBmwq2>nT#`NM+~AxK)IEeK|V9(J@^jLzNa)VxYUWj+0$FBrUks4DEqOs7;*5l?0YSN0V3WM+K!U@=c-fx;#h_hc6I$~Ue* zz#5D%Uc$S;nqB$EAOGBCCr%!)W@d|%gdVHpi<1m;F4J+6@zHm4#7Tkn+{_s#1--jE zSDaMfNa4^psn`IwVQ8Gxm={D7d?28r7$zjDV+Nv9W-O{^fy4wXsF;Qo$TZpHVEl_! zpY`r{Y45MvCI=Imc6U-RgDbNp1~V?zazd~`m79zY7WBrHals0d$}=ulv!qYP25ZcC z(FE@o4Iv#js5)jKDrLr^Y8FULz=DctSc6QHO%BF?{^jK0{p-(838r&-$<$!_)jCfL zsj!(I!nC|G00i_Q3e*o+BDYi9;xGG;6$V}ZnEEU1`_6%doLVu*LWE|=%~ z*Mbk6y>vX7&=0Fl1T$#g?PM_HYIjZs3$%X1sbE3Rymu;Cfh|Q&2P-zQ`srYe880R~ z^PYPus3ImLHe&{2GiEF{V}Ycbv7llyRv`Bm^$u|qJzq!DrScd(h>OHdi z2Se|@58Rum#B&&yc#+fdl#HeM7{9whg;<)8@)A`omgb{;=jrfRnvZgN)2LXQ&t+g) zo0wSIkMc7K@v*cY%qfC==H1zIA%{WR@ zW{oADnvnacA*AE_AAss41FBRqrmCetG=UV9rjZ)ZWKuK6OO1KIXoBg8hM2x+F8v)> z1y#xtiK5Sxd5ugd^ zjqn0(MrVYw-)6QaGoEuf<9R>lcu9zJKV@b>pMWwmrmsMm1)>i@nFXb9L766V>2!Dx zpk@Q$j8JBcC7zm)2dE)Qh^<{Rph_iUs#*#}6G%a68mWNaWM0Kwulq@j`G9DG33)1F zfslcyjv0$eSs+n03o0gH1;jKQdS#U45KLKy;Z~up1UjG*{od1+Fok0Ju6-{<56SJ51=ewHAK`u;8yd3 zxYdbaPN=xmj2E|>vbfch#jU0+ZZ%~#WO~&S&-tFRc-55sUbUOkyC7~gFNj-BS=?&M z;#N}>x0*5sANm25#jU0+UNtKtUNz&zt7d#DH{e$Dg1FU`#jU2y!G{Y5%Hmc_JWa-g z#I0sL)1iyz-4pPtDT`N4S-fhm{fS%63*uH&X3glkQ5LtFvbfch#jU0+ZZ&1`sws_S6+{tK|W@)s)4prYvqXWpS%1du}znYRckOQx>n9vUt^A`xCdC z7sRcmEN(SrajPkdTTNNqYRckPvjXB(GhV!E%Hma1_PlC0(9OiH<^^%9nUJ{Ej2E|> zvbfch#jU0saI3vGAzn4(#jB<)UNvP`+-e5It)?t)H47nbHRHvtrYvr?#7j5h{bDll zs(Fui)szEXwd<8s+-hDBx0*6*AR7@3i_GCu#_-H||2WTh@rEgjH%ytcAJtBovtsln z*B{_?NwxC=r%|e%GUsWkoie9gs-3cU!<5AvmUt;N?`LHg&vaNx%1ocNr=0F0W2%J# zmWb-2%#u=#l*JqN9>6qBwNvISM{`i->?ikgR+Mza8}{zUEGijuc4dLQ{U1)dQc}*- zQUmdZ`7q)jvlhAGoz5>HK}xq%0$B1wpTGYu*7o;misw1Q)W7xvMDnI zPUV!DF{gOSED+~@%1e~%cZV5Z4d_I84+Okn%B-=(QxkL|7*7pJLe!jOK#fYqRJ9a{ zCXj;CG+qT86R-ken*X^G;c_UY zw<;GzX8eC)6Ux8;Bhw{aX_+o5%XCTEn=S);y)s)eUS>6EHL=2EHEg``hv2oFDRp8Zg6v078txB3k=G#z@RJ(49c>=pezdv%Cf+q z%&AshwaEHH;{AKT*#GAPq9R#f@FrPcP?iM-Wm#ZQmIVf7Szu76$)x*<`Dj)#A7#&c zF1nw$;t9NV;RUfDC`|$y>A7!y0Ww9S+ zu^(mD>^|3rt~bbt{g|lOkFwa0ve?hl)}OXMbp5Ygn1R@jve?gW%Rl8!=O4f5jxDJ8Eul>rCuNP0)Uxcgv*{b}1?Un+pu@+5dfg;rh4RnZbW%f$>k)7xe%C zqYDhA|NmQGxPSHnL)L^`N8Brm`4}(jL&$D$f#^LDSRhgkED(SHx&-mEKJ*g0@3$rH zzxKLB-iokhf6ChuH?TltZDfH+Sr&+t#eNdcQB3Zq`6$c!kg}`~Da-nhvaAm&%lgoJ z|0@eb?}2-|9BWC3Lb{)<`~So(8P|J5nc4mP+b~@B%Z>)^Ejt~5{+0~(e^~q9?aBN% zw`APF?$o_|GWWYBLz9UqMP~P-`4}(eqwLC-n)d*@AN@hf?0)nIDO2tA2Pw;z8fDp1 zqbyr$5>NBV{WKqCF&|~753f4#{ude3-R1?^Qlre0(jTPE8qiFXy@BQb_JY}ee_%(A z&r-W|=P4H&=l-W(F#nVH%Y6UK1}L_C%2&bb1+}`^g?K?Jax5wBH}@Vk6^!a}(S3YJb1G*eo`hAG_GU+r*Y*IAT6&yfhzWF(2hSZoqyFi2W$DdvF_uGS$w)QKmV#Z$p{g zkF}vJ=975VUhbz>D2w?hGkxxcc;i1aru}$<4T`%Vlvz^lhEQhrqnRkPW`EL=9QO~- z1lkJP26`E^9rOxl2k2GMPS7sUZqOdkUeG?!e$WBXLC_)4VNg2g2!9tML6K^&+WqsDG|hpvB9+Sy zcBjJMD_*%&=kVhtC&b@8@kPZZrCvU~BV~AeXw-Gylm5=DJ>x$NIa=|+oQmlaOEih^ z`oo%v;OPj*(<{dHblex;x<`$QcrG2!H~i6gxdPHlN4gE~{FpN*J*kK~b1EW!9pYVH zIE)9<0uLPK><~yO9T}trGDt_pX@QK>Q6Lz^D^NNLnieQ%Ix3JBs6aY9Y~>A?H!#CM zd_xu_aDPMQ7aHYy3B@zRKnC&5FpzOPGYk|6RH3Iz=i4pDrLr^Y8FULz>dulX>rW@YzFceA$pUFX1RWnK51<7RF5o9N!(@Oi)8 zFWtSr;q!jG-vl>nyWfv))^@)uZkEn|*VEjr?S2*AES>!(mT>ROd%5rKchkKu@8!O` zU%WT||J&Znea?Qz4tGi&a6DQSN*xXiO(6p66B-a=2(g4XLPJ74p%I}m;UU7qgeHWh zgl2^1gcgLBgjR&sgf@h>gm#4Xgbsv`gieIcgf4`xghvRE5*{OTBXlS9AUsa!N$5rB zO-La0A@n8mBRoOCuKV!gPZ94X`C=LjZ&lBbl=Bo7J$5Yfx zWmATJk)i5Ec;@6AWPqVJTr5VL4$1VI^S|A(gP2u!gXfu#S*MSWnnM z*htt!*i6_$c#-fDVJl%9;bp>h!YhOwgjWeW3A+fp33~{83Hu292?tdA&@T?E7KPou zmTmR3o&ecaA5dSbKSH*Z;Dw(=$hLZW;Q`i@;Jv)?K%ip{v<83=0(Y#z;Epu}+_8p& zJJv98$9f9fu@b=@s|~ngwFP&qhE_K#-fHMt-61rBNPNtC6xaiZ+K`Rni z7rZ3m>RQDRUmREuyqFaYUJ6$tfss~(Rn&@h5naTpk4r^BMXl1f6k(MC)d7|UFAFIg zSO&Ztq}sr;;N>CJ0+s`>0I4RhJa|P&HGmbsD}mPlR=27FYk*eaj$_Lgz&C)0Q_Chb?a^1kPb|@j#zJj6OUL&fk&-X)_b_~UEqH3D~MZV zy$McSWu;ngfK~%nTWf%CB6Jyijdcl}xW-xwTx+efE`s2-T|{u5bpbrhIuA}vv({U$ zgEj!qgO5gb?*rerJ^+4T-2mROZUS#ww}7{-4}l+A9|1qIJ_del{RQ|J>lko9o*oT_ z?*r~bXf$G9vy#AHv)m-WaRiQACx9oclfaYKDc~vVH1M=_26)Cg3p{I`1D->nlB|tr zB^#mOF`&u7BouZd@OAJ{tWT}at-pdkwLU}WbL+Mj{?huJ^@a7N^%Zz0)meRQb#Vcm zRY%ncyelJA2ZX-19`VBM5&GJC)C;#$?ZF>Y-&jP(c7yzw>JIFtdH}nt$ALXmPvGOK z7qF-54eX_2fxT56Fjm!4wN*=COVtYKwoCcvbCzCnj=&P6s~F@+y+<^ zJOc4`fX%?GBd!gwjcThR5fcGy3jQE464(U1F7QF%!{AZCy1<9P>j9&Hja>-Qsxic9 z6|Wir8zEF5ax}0ZcmrU46@#+J0pk#g0X0xLRb^F0I0uh@>a zezkrB{$||;-nA@Tu#^HSWdm*H038(q3{lyD*;ID*t@WMtz4a~V2jCCZkH8=KgJ77^1!_BbTp)qAxZB;uYT@ALZ2Aj5pToqD= zZ24YP^~N>Z8Vj#;J)2^-*KI@Fax#sL@{7Rg=JzyznU1 z4}6pt9tnArdd3ACsh$QtqecLqR>OfKR3dP=dJ34Rh5?^aLxICos+y{%09S#pR+H7U zz$xIX5jRDxRMQZ@QcVY632BvDgDcOf6^L1>)~aOX0-pt6i!0Bn<;tCbOUu<0Y6fsR zZd`|3`T*C0FGKutwG_Av*dKfea4GOfaH9r*8_*K9SPevIG03PzY7jz;K#SE2YA`}C zfEKBRY6wCLK`*EUsLl}J5El{0)JF9ha1;13wHf><@R&M|*p0xAYJ)lfS`R!9egs#K z0=Iyt1CIb-1W#AbfuB@qDE>*@od!GsehPe@I*hC7>X4cVeh73}9aOWFI}>asA`Yn8 z2p#|(RQuKQ2<=yM!1qHsp!PxDkDc~8koKy%;Cq03fhpj76x?OC8@NX$sME+WL7f3l zP-npt)H(13^*VThIuD+pE`TSfi|T?4_7a2yl&}wQD?$lKV4do#wjtD4y$s%0Z3pkG zUIFi`c7XR)uY&i*9SOjl2=!IF!26;YyMX=FeEjbR6)yk|Ma+B@uPM`X25o8h%rS*f-QomCX*Z zZvk(rTPnMq4YJS9Zhr{8r9J|F2-LREPOX@-LRBpKsmf)4qCQojb}suf;HT;{mD>)r zQxkBxN$Q<1m#eS_h3tZMhAM0q0v5EtQeQ*PP~WH`c47M)$X}~(A%6obY8SD;1AeQ% zQ^o9}c51V)maFj8vhz@p0xj(3c6qy*UCypxx3pUTTcq~Jm72M#*lq1L_5;8wsqZEo zQpHlYBR)JwExU`|*{*GONqyz~Yj6w0=C4rQb0^rn?FM!)yS^P`C!~%=T$R)|4{lb) zQ};tI+;osV#~x@8vgd*iwo||dg9h0{?0E?Hb_Q&=SV7h%2 zcm((f_)+^)@MFNEsfoC=SdruQZToZk1n{_h5_kgmSNnEq;tMN~+Fsn#x6t?YCHu1d zgZ;hz2JkZQNBf7=ySSrg?#fOj$8st z@d{O`esgDt)68k^3 zb9-l$Gt%h*Z0~dgb^s=UcLH_;xPG-q{%o-UZm%=?d%u90%Uj84vyluxqL_ z2#u(~B&VCx-Eo~sz+~_qPI77-uEfMob$UB}oaxRq;4~+}>Fdlus4u9G)6aPhp?;vg z&J)f|gq{HPbNWMm0yxW=nYst5R4O{ind1y}203%V2LlHIQ@~Sp+J_Y2Fd?0Agm9E_ zjPM%aIN=20B;gd{G~o>4Ea4pCb;5bV1;RzbCBkLG8-zCrR|szr-X^?5xJtN2xK4PN b@E+lP!Uu#Kgqwt0gb!8vPWvM@rpNyS@QP`> diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6d929f68..9c810012 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -289,6 +289,7 @@ private void configureButtonBindings() { swerveDrive.getPose().getX(), swerveDrive.getPose().getY(), Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset()))))); + driverController.x().onTrue(new InstantCommand(()-> swerveDrive.resetPosition(swerveDriveSimulation.getSimulatedDriveTrainPose()))); // // // Reset robot odometry based on vision pose measurement from april tags // driverLeftDirectionPad.onTrue(new InstantCommand(() -> // swerveDrive.resetOdometry(visionSubsystem.getLastSeenPose()))); diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java index 74235ba3..0702d5e8 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java @@ -170,7 +170,7 @@ public static GyroSimulation createPigeon2() { * IMU */ public static GyroSimulation createNavX2() { - return new GyroSimulation(2, 0.04); + return new GyroSimulation(0.00208333333, 0.04); } /** diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index e430fe9b..a4a6255c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -152,6 +152,7 @@ public void runCharacterization(double volts) { } /** Returns the average drive velocity in rotations/sec. */ + // TODO: fix method public double getCharacterizationVelocity() { double velocity = 0.0; for (SwerveModule module : swerveModules) { @@ -188,12 +189,15 @@ private void modulesPeriodic() { * @param fieldRelative Whether the provided x and y speeds are relative to the field. */ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative) { + ChassisSpeeds discreteSpeeds = + // ChassisSpeeds.discretize( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rotationSpeed, getPose().getRotation().plus(Rotation2d.fromDegrees(getAllianceAngleOffset()))) + : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); + // , 0.02); SwerveModuleState[] swerveModuleStates = - DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rotationSpeed, getPose().getRotation()) - : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed)); + DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DriveConstants.MAX_SPEED_METERS_PER_SECOND); @@ -294,7 +298,7 @@ public Pose2d getPose() { /** Gets the current gyro yaw */ public Rotation2d getRawGyroYaw() { - return gyroInputs.yawDegreesRotation2d; + return Rotation2d.fromDegrees(gyroInputs.yawDegrees); } /** @@ -306,6 +310,6 @@ public void resetPosition(Pose2d pose) { // for (int timestampIndex = 0; // timestampIndex < odometryThreadInputs.measurementTimeStamps.length; // timestampIndex++) - poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + poseEstimator.resetPosition(getRawGyroYaw(), getModulePositions(), pose); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 0fcee643..7cfc40ad 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -42,18 +42,17 @@ public void updateOdometryInputs() { @Override public void periodic() { - updateOdometryPositions(); + // updateOdometryPositions(); } - private void updateOdometryPositions() { - odometryPositions = new SwerveModulePosition[inputs.odometryDriveWheelRevolutions.length]; - for (int i = 0; i < odometryPositions.length; i++) { - double positionMeters = - inputs.odometryDriveWheelRevolutions[i] * ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; - Rotation2d angle = inputs.odometrySteerPositions[i]; - odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); - } - } + // private void updateOdometryPositions() { + // odometryPositions = new SwerveModulePosition[inputs.odometryDriveWheelRevolutions.length]; + // for (int i = 0; i < odometryPositions.length; i++) { + // double positionMeters = getDrivePositionMeters(); + // Rotation2d angle = getTurnRotation(); + // odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + // } + // } public void setVoltage(Voltage volts) { io.setDriveVoltage(volts); @@ -68,8 +67,14 @@ public double getCharacterizationVelocity() { return inputs.driveVelocity; } - /** Runs the module with the specified setpoint state. Returns the optimized state. */ + /** Runs the module with the specified setpoint state. Returns optimized setpoint*/ public void runSetPoint(SwerveModuleState state) { + state.optimize(getTurnRotation()); + + if (Math.abs(state.speedMetersPerSecond) < 0.01) { + io.stopModule(); + } + io.setDesiredState(state); } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java index cea6b996..f7405590 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java @@ -19,6 +19,7 @@ public void updateInputs(GyroInputs inputs) { inputs.odometryYawPositions = gyroSimulation.getCachedGyroReadings(); inputs.odometryYawTimestamps = OdometryTimestampsSim.getTimestamps(); inputs.yawDegreesRotation2d = gyroSimulation.getGyroReading(); + inputs.yawDegrees = -gyroSimulation.getGyroReading().getDegrees(); inputs.yawVelocity = RadiansPerSecond.of(gyroSimulation.getMeasuredAngularVelocityRadPerSec()) .in(DegreesPerSecond); diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java index 41dcdcef..40e1eeea 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java @@ -175,27 +175,25 @@ public void setTurnVoltage(Voltage volts) { public void setDesiredState(SwerveModuleState desiredState) { double turnRotations = getTurnRotations(); // Optimize the reference state to avoid spinning further than 90 degrees - SwerveModuleState setpoint = - new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); - setpoint.optimize(Rotation2d.fromRotations(turnRotations)); - setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); + // desiredState.optimize(Rotation2d.fromRotations(turnRotations)); + // setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); - if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) { - driveMotor.set(0); - turnMotor.set(0); - return; - } + // if (Math.abs(desiredState.speedMetersPerSecond) < 0.01) { + // driveMotor.set(0); + // turnMotor.set(0); + // return; + // } // Converts meters per second to rotations per second double desiredDriveRPS = - setpoint.speedMetersPerSecond + desiredState.speedMetersPerSecond * ModuleConstants.DRIVE_GEAR_RATIO / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; driveMotor.setControl(velocityRequest.withVelocity(RotationsPerSecond.of(desiredDriveRPS))); turnMotor.setControl( - mmPositionRequest.withPosition(Rotations.of(setpoint.angle.getRotations()))); + mmPositionRequest.withPosition(Rotations.of(desiredState.angle.getRotations()))); } public double getTurnRotations() { diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index 717e1f28..a7816edd 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -22,16 +22,16 @@ public class SimulatedModule implements ModuleInterface { private final Constraints turnConstraints = new Constraints( - RadiansPerSecond.of(2 * Math.PI).in(RotationsPerSecond) * 0, - RadiansPerSecondPerSecond.of(4 * Math.PI).in(RotationsPerSecondPerSecond) * 0); + RadiansPerSecond.of(2 * Math.PI).in(RotationsPerSecond), + RadiansPerSecondPerSecond.of(4 * Math.PI).in(RotationsPerSecondPerSecond)); private final ProfiledPIDController turnPID = - new ProfiledPIDController(Radians.of(35).in(Rotations), 0, 0, turnConstraints); + new ProfiledPIDController(Radians.of(30).in(Rotations), 0, 0, turnConstraints); private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(0.77, 0.75, 0); public SimulatedModule(SwerveModuleSimulation moduleSimulation) { this.moduleSimulation = moduleSimulation; turnPID.enableContinuousInput( - Radians.of(-Math.PI).in(Rotations), Radians.of(Math.PI).in(Rotations)); + -Math.PI, Math.PI); } @Override @@ -70,20 +70,12 @@ public void setTurnVoltage(Voltage volts) { @Override public void setDesiredState(SwerveModuleState desiredState) { - double turnRotations = getTurnRotations(); - desiredState.optimize(Rotation2d.fromRotations(turnRotations)); - // Converts meters per second to rotations per second double desiredDriveRPS = desiredState.speedMetersPerSecond * ModuleConstants.DRIVE_GEAR_RATIO / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; - if (Math.abs(desiredState.speedMetersPerSecond) < 0.01) { - stopModule(); - return; - } - moduleSimulation.requestDriveVoltageOut( Volts.of( drivePID.calculate( From 1d5fad8719bc55eff5fdf00ba3ac3968bb14072c Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Wed, 13 Nov 2024 15:57:34 -0500 Subject: [PATCH 15/75] sim stuffs --- .../subsystems/vision/VisionConstants.java | 2 + .../robot/subsystems/vision/VisionIOSim.java | 44 ----- .../subsystems/vision/VisionIOSimPhoton.java | 164 ++++++++++++++++++ vendordeps/photonlib-v2025.0.0-beta-2.json | 97 +++++++++++ 4 files changed, 263 insertions(+), 44 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java create mode 100644 vendordeps/photonlib-v2025.0.0-beta-2.json diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index ac54ce9c..8f63ee12 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -1,8 +1,10 @@ package frc.robot.subsystems.vision; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.util.Units; public final class VisionConstants { + public static final AprilTagFields APRIL_TAG_FIELD = AprilTagFields.k2024Crescendo; public static final double VISION_X_POS_TRUST = 0.5; // meters public static final double VISION_Y_POS_TRUST = 0.5; // meters diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java deleted file mode 100644 index 343d4b4e..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java +++ /dev/null @@ -1,44 +0,0 @@ -// package frc.robot.subsystems.vision; - -// import edu.wpi.first.apriltag.AprilTagFieldLayout; -// import edu.wpi.first.math.geometry.Pose2d; -// import org.photonvision.simulation.PhotonCameraSim; -// import org.photonvision.simulation.VisionSystemSim; - -// import java.util.List; -// import java.util.function.Supplier; - -// public class VisionIOSim extends VisionIOPhoton { -// private final VisionSystemSim visionSystemSim; -// private final PhotonCameraSim[] camerasSim; -// private final Supplier robotActualPoseInSimulationSupplier; -// public VisionIOSim(List cameraProperties, AprilTagFieldLayout -// aprilTagFieldLayout, Supplier robotActualPoseInSimulationSupplier) { -// super(cameraProperties); - -// this.robotActualPoseInSimulationSupplier = robotActualPoseInSimulationSupplier; -// this.visionSystemSim = new VisionSystemSim("main"); -// visionSystemSim.addAprilTags(aprilTagFieldLayout); -// camerasSim = new PhotonCameraSim[cameraProperties.size()]; - -// for (int i = 0; i < cameraProperties.size(); i++) { -// final PhotonCameraSim cameraSim = new PhotonCameraSim( -// super.cameras[i], -// cameraProperties.get(i).getSimulationProperties() -// ); -// cameraSim.enableRawStream(true); -// cameraSim.enableProcessedStream(true); -// cameraSim.enableDrawWireframe(true); -// visionSystemSim.addCamera( -// camerasSim[i] = cameraSim, -// cameraProperties.get(i).robotToCamera -// ); -// } -// } - -// @Override -// public void updateInputs(VisionIOInputs inputs) { -// visionSystemSim.update(robotActualPoseInSimulationSupplier.get()); -// super.updateInputs(inputs); -// } -// } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java b/src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java new file mode 100644 index 00000000..fc37baff --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java @@ -0,0 +1,164 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import frc.robot.extras.simulation.field.SimulatedField; +import frc.robot.extras.simulation.mechanismSim.swerve.SwerveDriveSimulation; + +import org.littletonrobotics.junction.Logger; +import org.photonvision.PhotonCamera; +import org.photonvision.estimation.TargetModel; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.simulation.VisionTargetSim; +import org.photonvision.targeting.PhotonPipelineResult; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.function.Supplier; + +// Simulate the vision system. +// Please see the following link for example code +// https://github.com/PhotonVision/photonvision/blob/2a6fa1b6ac81f239c59d724da5339f608897c510/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java +public class VisionIOSimPhoton extends VisionIOReal { + private final PhotonCamera shooterCamera = new PhotonCamera("shooterCamera"); + private final PhotonCamera frontLeftCamera = new PhotonCamera("frontLeftCamera"); + private final PhotonCamera frontRightCamera = new PhotonCamera("frontRightCamera"); + PhotonCameraSim shooterCameraSim; + PhotonCameraSim frontLeftCameraSim; + PhotonCameraSim frontRightCameraSim; + private final VisionSystemSim visionSim; + private final Supplier robotActualPoseInSimulationSupplier; + + private final int kResWidth = 1280; + private final int kResHeight = 800; + + public VisionIOSimPhoton(Supplier robotActualPoseInSimulationSupplier) { + this.robotActualPoseInSimulationSupplier = robotActualPoseInSimulationSupplier; + // Create the vision system simulation which handles cameras and targets on the + // field. + visionSim = new VisionSystemSim("main"); + + // Add all the AprilTags inside the tag layout as visible targets to this + // simulated field. + visionSim.addAprilTags(AprilTagFieldLayout.loadFromResource(VisionConstants.APRIL_TAG_FIELD.m_resourceFile)); + + // Create simulated camera properties. These can be set to mimic your actual + // camera. + var turretProp = new SimCameraProperties(); + turretProp.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7)); + turretProp.setCalibError(0.35, 0.10); + turretProp.setFPS(15); + turretProp.setAvgLatencyMs(20); + turretProp.setLatencyStdDevMs(5); + + var elevatorProp = new SimCameraProperties(); + elevatorProp = turretProp.copy(); + + // Create a PhotonCameraSim which will update the linked PhotonCamera's values + // with visible + // targets. + // Instance variables + shooterCameraSim = new PhotonCameraSim(shooterCamera, turretProp); + frontLeftCameraSim = new PhotonCameraSim(frontLeftCamera, elevatorProp); + frontRightCameraSim = new PhotonCameraSim(frontRightCamera, elevatorProp); + + // Add the simulated camera to view the targets on this simulated field. + visionSim.addCamera(shooterCameraSim, new Transform3d()); + + Transform3d robotToElevatorCamera = new Transform3d( + new Translation3d( + Constants.kTurretToCameraBX, + Constants.kTurretToCameraBY, + Constants.kCameraBHeightOffGroundMeters), + new Rotation3d(0.0, -Constants.kCameraBPitchRads, 0.0)); + visionSim.addCamera(frontLeftCameraSim, robotToElevatorCamera); + + + // Enable the raw and processed streams. (http://localhost:1181 / 1182) + shooterCameraSim.enableRawStream(true); + shooterCameraSim.enableProcessedStream(true); + frontLeftCameraSim.enableRawStream(true); + frontLeftCameraSim.enableProcessedStream(true); + frontRightCameraSim.enableRawStream(true); + frontRightCameraSim.enableProcessedStream(true); + + // Enable drawing a wireframe visualization of the field to the camera streams. + // This is extremely resource-intensive and is disabled by default. + shooterCameraSim.enableDrawWireframe(true); + frontLeftCameraSim.enableDrawWireframe(true); + frontRightCameraSim.enableDrawWireframe(true); + + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + // Abuse the updateInputs periodic call to update the sim + + // Move the vision sim robot on the field + if (robotActualPoseInSimulationSupplier.get() != null) { + visionSim.update(robotActualPoseInSimulationSupplier.get()); + Logger.recordOutput("Vision/SimIO/updateSimPose", robotActualPoseInSimulationSupplier.get()); + } + + NetworkTable shooterTable = NetworkTableInstance.getDefault().getTable(super.getLimelightName(0)); + NetworkTable frontLeftTable = NetworkTableInstance.getDefault().getTable(super.getLimelightName(1)); + NetworkTable frontRightTable = NetworkTableInstance.getDefault().getTable(super.getLimelightName(2)); + // Write to limelight table + writeToTable(shooterCamera.getAllUnreadResults(), shooterTable); + writeToTable(frontLeftCamera.getAllUnreadResults(), frontLeftTable); + writeToTable(frontRightCamera.getAllUnreadResults(), frontRightTable); + + super.updateInputs(inputs); + } + + private void writeToTable(List results, NetworkTable table) { + //write to ll table + for (PhotonPipelineResult result : results) { + if (result.getMultiTagResult().isPresent()) { + Transform3d best = result.getMultiTagResult().estimatedPose.isPresent; + Pose2d fieldToCamera = new Pose2d(best.getTranslation().toTranslation2d(), + best.getRotation().toRotation2d()); + List pose_data = new ArrayList<>(Arrays.asList( + best.getX(), // 0: X + best.getY(), // 1: Y + best.getZ(), // 2: Z, + 0.0, // 3: roll + 0.0, // 4: pitch + fieldToCamera.getRotation().getDegrees(), // 5: yaw + result.getLatencyMillis(), // 6: latency ms, + (double) result.getMultiTagResult().fiducialIDsUsed.size(), // 7: tag count + 0.0, // 8: tag span + 0.0, // 9: tag dist + result.getBestTarget().getArea() // 10: tag area + )); + // Add RawFiducials + // This is super inefficient but it's sim only, who cares. + for (var target : result.targets) { + pose_data.add((double) target.getFiducialId()); // 0: id + pose_data.add(target.getYaw()); // 1: txnc + pose_data.add(target.getPitch()); // 2: tync + pose_data.add(0.0); // 3: ta + pose_data.add(0.0); // 4: distToCamera + pose_data.add(0.0); // 5: distToRobot + pose_data.add(0.5); // 6: ambiguity + } + + + table.getEntry("botpose_wpiblue") + .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); + table.getEntry("botpose_orb_wpiblue") + .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); + } + + table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); + // table.getEntry("cl").setDouble(result.getTimestampSeconds()); + } +} +} \ No newline at end of file diff --git a/vendordeps/photonlib-v2025.0.0-beta-2.json b/vendordeps/photonlib-v2025.0.0-beta-2.json new file mode 100644 index 00000000..73fcefd1 --- /dev/null +++ b/vendordeps/photonlib-v2025.0.0-beta-2.json @@ -0,0 +1,97 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.0.0-beta-2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "edu.wpi.first.wpilibc", + "artifactId": "wpilibc-cpp", + "version": "2025.1.1-beta-1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-2", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-jni", + "version": "v2025.0.0-beta-2", + "skipInvalidPlatforms": true, + "isJar": true, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.0.0-beta-2", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-2", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.0.0-beta-2" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.0.0-beta-2" + } + ] +} From afc0139388a534261d4d35341c27171a6a1a6c00 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 14 Nov 2024 10:58:20 -0500 Subject: [PATCH 16/75] no errors yay --- .../{model.glb => MAIN_ASSEMBLY.glb} | Bin .../subsystems/vision/VisionIOSimPhoton.java | 17 +++++++++-------- 2 files changed, 9 insertions(+), 8 deletions(-) rename ascopeAssets/Robot_Offseason/{model.glb => MAIN_ASSEMBLY.glb} (100%) diff --git a/ascopeAssets/Robot_Offseason/model.glb b/ascopeAssets/Robot_Offseason/MAIN_ASSEMBLY.glb similarity index 100% rename from ascopeAssets/Robot_Offseason/model.glb rename to ascopeAssets/Robot_Offseason/MAIN_ASSEMBLY.glb diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java b/src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java index fc37baff..df0cc659 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java @@ -18,6 +18,7 @@ import org.photonvision.simulation.VisionTargetSim; import org.photonvision.targeting.PhotonPipelineResult; +import java.io.IOException; import java.util.ArrayList; import java.util.Arrays; import java.util.List; @@ -39,7 +40,7 @@ public class VisionIOSimPhoton extends VisionIOReal { private final int kResWidth = 1280; private final int kResHeight = 800; - public VisionIOSimPhoton(Supplier robotActualPoseInSimulationSupplier) { + public VisionIOSimPhoton(Supplier robotActualPoseInSimulationSupplier) throws IOException { this.robotActualPoseInSimulationSupplier = robotActualPoseInSimulationSupplier; // Create the vision system simulation which handles cameras and targets on the // field. @@ -74,10 +75,10 @@ public VisionIOSimPhoton(Supplier robotActualPoseInSimulationSupplier) { Transform3d robotToElevatorCamera = new Transform3d( new Translation3d( - Constants.kTurretToCameraBX, - Constants.kTurretToCameraBY, - Constants.kCameraBHeightOffGroundMeters), - new Rotation3d(0.0, -Constants.kCameraBPitchRads, 0.0)); + 0.0, + 0.0, + 0.0), + new Rotation3d(0.0, 0.0, 0.0)); visionSim.addCamera(frontLeftCameraSim, robotToElevatorCamera); @@ -122,7 +123,7 @@ private void writeToTable(List results, NetworkTable table //write to ll table for (PhotonPipelineResult result : results) { if (result.getMultiTagResult().isPresent()) { - Transform3d best = result.getMultiTagResult().estimatedPose.isPresent; + Transform3d best = result.getMultiTagResult().get().estimatedPose.best; Pose2d fieldToCamera = new Pose2d(best.getTranslation().toTranslation2d(), best.getRotation().toRotation2d()); List pose_data = new ArrayList<>(Arrays.asList( @@ -132,8 +133,8 @@ private void writeToTable(List results, NetworkTable table 0.0, // 3: roll 0.0, // 4: pitch fieldToCamera.getRotation().getDegrees(), // 5: yaw - result.getLatencyMillis(), // 6: latency ms, - (double) result.getMultiTagResult().fiducialIDsUsed.size(), // 7: tag count + result.getTimestampSeconds(), // 6: latency ms, + (double) result.getMultiTagResult().get().fiducialIDsUsed.size(), // 7: tag count 0.0, // 8: tag span 0.0, // 9: tag dist result.getBestTarget().getArea() // 10: tag area From d94714e5d6232bde5def78668f536d006c5aabfc Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 14 Nov 2024 11:19:12 -0500 Subject: [PATCH 17/75] format --- src/main/java/frc/robot/RobotContainer.java | 7 +- .../robot/subsystems/swerve/SwerveDrive.java | 17 +- .../robot/subsystems/swerve/SwerveModule.java | 2 +- .../swerve/moduleIO/SimulatedModule.java | 4 +- .../subsystems/vision/VisionIOSimPhoton.java | 259 +++++++++--------- 5 files changed, 145 insertions(+), 144 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9c810012..2fc027fe 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -289,7 +289,12 @@ private void configureButtonBindings() { swerveDrive.getPose().getX(), swerveDrive.getPose().getY(), Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset()))))); - driverController.x().onTrue(new InstantCommand(()-> swerveDrive.resetPosition(swerveDriveSimulation.getSimulatedDriveTrainPose()))); + driverController + .x() + .onTrue( + new InstantCommand( + () -> + swerveDrive.resetPosition(swerveDriveSimulation.getSimulatedDriveTrainPose()))); // // // Reset robot odometry based on vision pose measurement from april tags // driverLeftDirectionPad.onTrue(new InstantCommand(() -> // swerveDrive.resetOdometry(visionSubsystem.getLastSeenPose()))); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index a4a6255c..ada715de 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -189,13 +189,16 @@ private void modulesPeriodic() { * @param fieldRelative Whether the provided x and y speeds are relative to the field. */ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative) { - ChassisSpeeds discreteSpeeds = - // ChassisSpeeds.discretize( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rotationSpeed, getPose().getRotation().plus(Rotation2d.fromDegrees(getAllianceAngleOffset()))) - : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); - // , 0.02); + ChassisSpeeds discreteSpeeds = + // ChassisSpeeds.discretize( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, + ySpeed, + rotationSpeed, + getPose().getRotation().plus(Rotation2d.fromDegrees(getAllianceAngleOffset()))) + : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); + // , 0.02); SwerveModuleState[] swerveModuleStates = DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds( diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 7cfc40ad..c71ec301 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -67,7 +67,7 @@ public double getCharacterizationVelocity() { return inputs.driveVelocity; } - /** Runs the module with the specified setpoint state. Returns optimized setpoint*/ + /** Runs the module with the specified setpoint state. Returns optimized setpoint */ public void runSetPoint(SwerveModuleState state) { state.optimize(getTurnRotation()); diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index a7816edd..8418d7a2 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -5,7 +5,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.units.measure.Voltage; @@ -30,8 +29,7 @@ public class SimulatedModule implements ModuleInterface { public SimulatedModule(SwerveModuleSimulation moduleSimulation) { this.moduleSimulation = moduleSimulation; - turnPID.enableContinuousInput( - -Math.PI, Math.PI); + turnPID.enableContinuousInput(-Math.PI, Math.PI); } @Override diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java b/src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java index df0cc659..0c2636b9 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java @@ -1,132 +1,125 @@ package frc.robot.subsystems.vision; import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.*; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; -import frc.robot.extras.simulation.field.SimulatedField; -import frc.robot.extras.simulation.mechanismSim.swerve.SwerveDriveSimulation; - +import java.io.IOException; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; import org.photonvision.PhotonCamera; -import org.photonvision.estimation.TargetModel; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.simulation.VisionTargetSim; import org.photonvision.targeting.PhotonPipelineResult; -import java.io.IOException; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; -import java.util.function.Supplier; - // Simulate the vision system. // Please see the following link for example code // https://github.com/PhotonVision/photonvision/blob/2a6fa1b6ac81f239c59d724da5339f608897c510/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java public class VisionIOSimPhoton extends VisionIOReal { - private final PhotonCamera shooterCamera = new PhotonCamera("shooterCamera"); - private final PhotonCamera frontLeftCamera = new PhotonCamera("frontLeftCamera"); - private final PhotonCamera frontRightCamera = new PhotonCamera("frontRightCamera"); - PhotonCameraSim shooterCameraSim; - PhotonCameraSim frontLeftCameraSim; - PhotonCameraSim frontRightCameraSim; - private final VisionSystemSim visionSim; - private final Supplier robotActualPoseInSimulationSupplier; - - private final int kResWidth = 1280; - private final int kResHeight = 800; - - public VisionIOSimPhoton(Supplier robotActualPoseInSimulationSupplier) throws IOException { - this.robotActualPoseInSimulationSupplier = robotActualPoseInSimulationSupplier; - // Create the vision system simulation which handles cameras and targets on the - // field. - visionSim = new VisionSystemSim("main"); - - // Add all the AprilTags inside the tag layout as visible targets to this - // simulated field. - visionSim.addAprilTags(AprilTagFieldLayout.loadFromResource(VisionConstants.APRIL_TAG_FIELD.m_resourceFile)); - - // Create simulated camera properties. These can be set to mimic your actual - // camera. - var turretProp = new SimCameraProperties(); - turretProp.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7)); - turretProp.setCalibError(0.35, 0.10); - turretProp.setFPS(15); - turretProp.setAvgLatencyMs(20); - turretProp.setLatencyStdDevMs(5); - - var elevatorProp = new SimCameraProperties(); - elevatorProp = turretProp.copy(); - - // Create a PhotonCameraSim which will update the linked PhotonCamera's values - // with visible - // targets. - // Instance variables - shooterCameraSim = new PhotonCameraSim(shooterCamera, turretProp); - frontLeftCameraSim = new PhotonCameraSim(frontLeftCamera, elevatorProp); - frontRightCameraSim = new PhotonCameraSim(frontRightCamera, elevatorProp); - - // Add the simulated camera to view the targets on this simulated field. - visionSim.addCamera(shooterCameraSim, new Transform3d()); - - Transform3d robotToElevatorCamera = new Transform3d( - new Translation3d( - 0.0, - 0.0, - 0.0), - new Rotation3d(0.0, 0.0, 0.0)); - visionSim.addCamera(frontLeftCameraSim, robotToElevatorCamera); - - - // Enable the raw and processed streams. (http://localhost:1181 / 1182) - shooterCameraSim.enableRawStream(true); - shooterCameraSim.enableProcessedStream(true); - frontLeftCameraSim.enableRawStream(true); - frontLeftCameraSim.enableProcessedStream(true); - frontRightCameraSim.enableRawStream(true); - frontRightCameraSim.enableProcessedStream(true); - - // Enable drawing a wireframe visualization of the field to the camera streams. - // This is extremely resource-intensive and is disabled by default. - shooterCameraSim.enableDrawWireframe(true); - frontLeftCameraSim.enableDrawWireframe(true); - frontRightCameraSim.enableDrawWireframe(true); - - } - - @Override - public void updateInputs(VisionIOInputs inputs) { - // Abuse the updateInputs periodic call to update the sim - - // Move the vision sim robot on the field - if (robotActualPoseInSimulationSupplier.get() != null) { - visionSim.update(robotActualPoseInSimulationSupplier.get()); - Logger.recordOutput("Vision/SimIO/updateSimPose", robotActualPoseInSimulationSupplier.get()); - } - - NetworkTable shooterTable = NetworkTableInstance.getDefault().getTable(super.getLimelightName(0)); - NetworkTable frontLeftTable = NetworkTableInstance.getDefault().getTable(super.getLimelightName(1)); - NetworkTable frontRightTable = NetworkTableInstance.getDefault().getTable(super.getLimelightName(2)); - // Write to limelight table - writeToTable(shooterCamera.getAllUnreadResults(), shooterTable); - writeToTable(frontLeftCamera.getAllUnreadResults(), frontLeftTable); - writeToTable(frontRightCamera.getAllUnreadResults(), frontRightTable); - - super.updateInputs(inputs); + private final PhotonCamera shooterCamera = new PhotonCamera("shooterCamera"); + private final PhotonCamera frontLeftCamera = new PhotonCamera("frontLeftCamera"); + private final PhotonCamera frontRightCamera = new PhotonCamera("frontRightCamera"); + PhotonCameraSim shooterCameraSim; + PhotonCameraSim frontLeftCameraSim; + PhotonCameraSim frontRightCameraSim; + private final VisionSystemSim visionSim; + private final Supplier robotActualPoseInSimulationSupplier; + + private final int kResWidth = 1280; + private final int kResHeight = 800; + + public VisionIOSimPhoton(Supplier robotActualPoseInSimulationSupplier) + throws IOException { + this.robotActualPoseInSimulationSupplier = robotActualPoseInSimulationSupplier; + // Create the vision system simulation which handles cameras and targets on the + // field. + visionSim = new VisionSystemSim("main"); + + // Add all the AprilTags inside the tag layout as visible targets to this + // simulated field. + visionSim.addAprilTags( + AprilTagFieldLayout.loadFromResource(VisionConstants.APRIL_TAG_FIELD.m_resourceFile)); + + // Create simulated camera properties. These can be set to mimic your actual + // camera. + var turretProp = new SimCameraProperties(); + turretProp.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7)); + turretProp.setCalibError(0.35, 0.10); + turretProp.setFPS(15); + turretProp.setAvgLatencyMs(20); + turretProp.setLatencyStdDevMs(5); + + var elevatorProp = new SimCameraProperties(); + elevatorProp = turretProp.copy(); + + // Create a PhotonCameraSim which will update the linked PhotonCamera's values + // with visible + // targets. + // Instance variables + shooterCameraSim = new PhotonCameraSim(shooterCamera, turretProp); + frontLeftCameraSim = new PhotonCameraSim(frontLeftCamera, elevatorProp); + frontRightCameraSim = new PhotonCameraSim(frontRightCamera, elevatorProp); + + // Add the simulated camera to view the targets on this simulated field. + visionSim.addCamera(shooterCameraSim, new Transform3d()); + + Transform3d robotToElevatorCamera = + new Transform3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, 0.0, 0.0)); + visionSim.addCamera(frontLeftCameraSim, robotToElevatorCamera); + + // Enable the raw and processed streams. (http://localhost:1181 / 1182) + shooterCameraSim.enableRawStream(true); + shooterCameraSim.enableProcessedStream(true); + frontLeftCameraSim.enableRawStream(true); + frontLeftCameraSim.enableProcessedStream(true); + frontRightCameraSim.enableRawStream(true); + frontRightCameraSim.enableProcessedStream(true); + + // Enable drawing a wireframe visualization of the field to the camera streams. + // This is extremely resource-intensive and is disabled by default. + shooterCameraSim.enableDrawWireframe(true); + frontLeftCameraSim.enableDrawWireframe(true); + frontRightCameraSim.enableDrawWireframe(true); + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + // Abuse the updateInputs periodic call to update the sim + + // Move the vision sim robot on the field + if (robotActualPoseInSimulationSupplier.get() != null) { + visionSim.update(robotActualPoseInSimulationSupplier.get()); + Logger.recordOutput("Vision/SimIO/updateSimPose", robotActualPoseInSimulationSupplier.get()); } - private void writeToTable(List results, NetworkTable table) { - //write to ll table - for (PhotonPipelineResult result : results) { - if (result.getMultiTagResult().isPresent()) { - Transform3d best = result.getMultiTagResult().get().estimatedPose.best; - Pose2d fieldToCamera = new Pose2d(best.getTranslation().toTranslation2d(), - best.getRotation().toRotation2d()); - List pose_data = new ArrayList<>(Arrays.asList( + NetworkTable shooterTable = + NetworkTableInstance.getDefault().getTable(super.getLimelightName(0)); + NetworkTable frontLeftTable = + NetworkTableInstance.getDefault().getTable(super.getLimelightName(1)); + NetworkTable frontRightTable = + NetworkTableInstance.getDefault().getTable(super.getLimelightName(2)); + // Write to limelight table + writeToTable(shooterCamera.getAllUnreadResults(), shooterTable); + writeToTable(frontLeftCamera.getAllUnreadResults(), frontLeftTable); + writeToTable(frontRightCamera.getAllUnreadResults(), frontRightTable); + + super.updateInputs(inputs); + } + + private void writeToTable(List results, NetworkTable table) { + // write to ll table + for (PhotonPipelineResult result : results) { + if (result.getMultiTagResult().isPresent()) { + Transform3d best = result.getMultiTagResult().get().estimatedPose.best; + Pose2d fieldToCamera = + new Pose2d(best.getTranslation().toTranslation2d(), best.getRotation().toRotation2d()); + List pose_data = + new ArrayList<>( + Arrays.asList( best.getX(), // 0: X best.getY(), // 1: Y best.getZ(), // 2: Z, @@ -134,32 +127,34 @@ private void writeToTable(List results, NetworkTable table 0.0, // 4: pitch fieldToCamera.getRotation().getDegrees(), // 5: yaw result.getTimestampSeconds(), // 6: latency ms, - (double) result.getMultiTagResult().get().fiducialIDsUsed.size(), // 7: tag count + (double) + result.getMultiTagResult().get().fiducialIDsUsed.size(), // 7: tag count 0.0, // 8: tag span 0.0, // 9: tag dist result.getBestTarget().getArea() // 10: tag area - )); - // Add RawFiducials - // This is super inefficient but it's sim only, who cares. - for (var target : result.targets) { - pose_data.add((double) target.getFiducialId()); // 0: id - pose_data.add(target.getYaw()); // 1: txnc - pose_data.add(target.getPitch()); // 2: tync - pose_data.add(0.0); // 3: ta - pose_data.add(0.0); // 4: distToCamera - pose_data.add(0.0); // 5: distToRobot - pose_data.add(0.5); // 6: ambiguity - } - - - table.getEntry("botpose_wpiblue") - .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); - table.getEntry("botpose_orb_wpiblue") - .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); + )); + // Add RawFiducials + // This is super inefficient but it's sim only, who cares. + for (var target : result.targets) { + pose_data.add((double) target.getFiducialId()); // 0: id + pose_data.add(target.getYaw()); // 1: txnc + pose_data.add(target.getPitch()); // 2: tync + pose_data.add(0.0); // 3: ta + pose_data.add(0.0); // 4: distToCamera + pose_data.add(0.0); // 5: distToRobot + pose_data.add(0.5); // 6: ambiguity } - table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); - // table.getEntry("cl").setDouble(result.getTimestampSeconds()); + table + .getEntry("botpose_wpiblue") + .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); + table + .getEntry("botpose_orb_wpiblue") + .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); + } + + table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); + table.getEntry("cl").setDouble(result.getTimestampSeconds()); } + } } -} \ No newline at end of file From 38ce7065952679c9e35cae15ace17c4d69e02228 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 14 Nov 2024 12:40:42 -0500 Subject: [PATCH 18/75] naming is wip --- src/main/java/frc/robot/Robot.java | 10 ++++++- src/main/java/frc/robot/RobotContainer.java | 15 ++++++----- ...{VisionIOReal.java => PhysicalVision.java} | 6 ++--- ...nIOSimPhoton.java => SimulatedVision.java} | 6 ++--- .../frc/robot/subsystems/vision/Vision.java | 26 +++++++++---------- .../{VisionIO.java => VisionInterface.java} | 6 ++--- 6 files changed, 40 insertions(+), 29 deletions(-) rename src/main/java/frc/robot/subsystems/vision/{VisionIOReal.java => PhysicalVision.java} (99%) rename src/main/java/frc/robot/subsystems/vision/{VisionIOSimPhoton.java => SimulatedVision.java} (97%) rename src/main/java/frc/robot/subsystems/vision/{VisionIO.java => VisionInterface.java} (89%) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 49b3d6dd..437095f6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,9 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Constants.Mode; import frc.robot.extras.simulation.field.SimulatedField; + +import java.io.IOException; + import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -77,7 +80,12 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); + try { + m_robotContainer = new RobotContainer(); + } catch (IOException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2fc027fe..1a000002 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,8 +26,11 @@ import frc.robot.subsystems.swerve.moduleIO.PhysicalModule; import frc.robot.subsystems.swerve.moduleIO.SimulatedModule; import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionIO; -import frc.robot.subsystems.vision.VisionIOReal; +import frc.robot.subsystems.vision.VisionInterface; +import frc.robot.subsystems.vision.PhysicalVision; +import frc.robot.subsystems.vision.SimulatedVision; + +import java.io.IOException; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -50,7 +53,7 @@ public class RobotContainer { // Subsystems // private final XboxController driverController = new XboxController(0); - public RobotContainer() { + public RobotContainer() throws IOException { switch (Constants.currentMode) { case REAL -> { /* Real robot, instantiate hardware IO implementations */ @@ -67,7 +70,7 @@ public RobotContainer() { new PhysicalModule(SwerveConstants.moduleConfigs[1]), new PhysicalModule(SwerveConstants.moduleConfigs[2]), new PhysicalModule(SwerveConstants.moduleConfigs[3])); - visionSubsystem = new Vision(new VisionIOReal()); + visionSubsystem = new Vision(new PhysicalVision()); } case SIM -> { @@ -106,7 +109,7 @@ public RobotContainer() { new SimulatedModule(swerveDriveSimulation.getModules()[3])); // TODO: add sim impl - visionSubsystem = new Vision(new VisionIO() {}); + visionSubsystem = new Vision(new SimulatedVision(swerveDrive::getPose)); SimulatedField.getInstance().resetFieldForAuto(); resetFieldAndOdometryForAuto( @@ -114,7 +117,7 @@ public RobotContainer() { } default -> { - visionSubsystem = new Vision(new VisionIO() {}); + visionSubsystem = new Vision(new VisionInterface() {}); /* Replayed robot, disable IO implementations */ /* physics simulations are also not needed */ diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java similarity index 99% rename from src/main/java/frc/robot/subsystems/vision/VisionIOReal.java rename to src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index aa4405d8..12f46414 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -14,7 +14,7 @@ import java.util.concurrent.TimeUnit; import java.util.concurrent.atomic.AtomicBoolean; -public class VisionIOReal implements VisionIO { +public class PhysicalVision implements VisionInterface { private Pose2d lastSeenPose = new Pose2d(); private double headingDegrees = 0; @@ -28,7 +28,7 @@ public class VisionIOReal implements VisionIO { */ private PoseEstimate[] limelightEstimates; - public VisionIOReal() { + public PhysicalVision() { limelightEstimates = new PoseEstimate[3]; for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { limelightThreads.put(limelightNumber, new AtomicBoolean(true)); @@ -37,7 +37,7 @@ public VisionIOReal() { } @Override - public void updateInputs(VisionIOInputs inputs) { + public void updateInputs(VisionInputs inputs) { inputs.camerasAmount = limelightEstimates.length; inputs.cameraConnected = true; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java similarity index 97% rename from src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java rename to src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 0c2636b9..fc10ca7b 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOSimPhoton.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -19,7 +19,7 @@ // Simulate the vision system. // Please see the following link for example code // https://github.com/PhotonVision/photonvision/blob/2a6fa1b6ac81f239c59d724da5339f608897c510/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java -public class VisionIOSimPhoton extends VisionIOReal { +public class SimulatedVision extends PhysicalVision { private final PhotonCamera shooterCamera = new PhotonCamera("shooterCamera"); private final PhotonCamera frontLeftCamera = new PhotonCamera("frontLeftCamera"); private final PhotonCamera frontRightCamera = new PhotonCamera("frontRightCamera"); @@ -32,7 +32,7 @@ public class VisionIOSimPhoton extends VisionIOReal { private final int kResWidth = 1280; private final int kResHeight = 800; - public VisionIOSimPhoton(Supplier robotActualPoseInSimulationSupplier) + public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) throws IOException { this.robotActualPoseInSimulationSupplier = robotActualPoseInSimulationSupplier; // Create the vision system simulation which handles cameras and targets on the @@ -87,7 +87,7 @@ public VisionIOSimPhoton(Supplier robotActualPoseInSimulationSupplier) } @Override - public void updateInputs(VisionIOInputs inputs) { + public void updateInputs(VisionInputs inputs) { // Abuse the updateInputs periodic call to update the sim // Move the vision sim robot on the field diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 72a4df2d..493763df 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -6,49 +6,49 @@ import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { - private final VisionIO visionIO; - private final VisionIOInputsAutoLogged inputs = new VisionIOInputsAutoLogged(); + private final VisionInterface visionInterface; + private final VisionInputsAutoLogged inputs = new VisionInputsAutoLogged(); - public Vision(VisionIO visionIO) { + public Vision(VisionInterface visionInterface) { // Initializing Fields - this.visionIO = visionIO; + this.visionInterface = visionInterface; } @Override public void periodic() { // Updates limelight inputs - visionIO.updateInputs(inputs); - Logger.processInputs(visionIO.getLimelightName(0), inputs); + visionInterface.updateInputs(inputs); + Logger.processInputs(visionInterface.getLimelightName(0), inputs); } // Add methods to support DriveCommand public int getNumberOfAprilTags(int limelightNumber) { - return visionIO.getNumberOfAprilTags(limelightNumber); + return visionInterface.getNumberOfAprilTags(limelightNumber); } public double getLimelightAprilTagDistance(int limelightNumber) { - return visionIO.getLimelightAprilTagDistance(limelightNumber); + return visionInterface.getLimelightAprilTagDistance(limelightNumber); } public double getTimeStampSeconds(int limelightNumber) { - return visionIO.getTimeStampSeconds(limelightNumber); + return visionInterface.getTimeStampSeconds(limelightNumber); } public double getLatencySeconds(int limelightNumber) { - return visionIO.getLatencySeconds(limelightNumber); + return visionInterface.getLatencySeconds(limelightNumber); } public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { - visionIO.setHeadingInfo(headingDegrees, headingRateDegrees); + visionInterface.setHeadingInfo(headingDegrees, headingRateDegrees); } @AutoLogOutput(key = "Vision/Has Targets") public boolean canSeeAprilTags(int limelightNumber) { - return visionIO.canSeeAprilTags( + return visionInterface.canSeeAprilTags( limelightNumber); // Assuming we're checking the shooter limelight } public Pose2d getPoseFromAprilTags(int limelightNumber) { - return visionIO.getPoseFromAprilTags(limelightNumber); + return visionInterface.getPoseFromAprilTags(limelightNumber); } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java similarity index 89% rename from src/main/java/frc/robot/subsystems/vision/VisionIO.java rename to src/main/java/frc/robot/subsystems/vision/VisionInterface.java index 668cfeb0..f7f7f849 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -3,9 +3,9 @@ import edu.wpi.first.math.geometry.Pose2d; import org.littletonrobotics.junction.AutoLog; -public interface VisionIO { +public interface VisionInterface { @AutoLog - class VisionIOInputs { + class VisionInputs { public boolean cameraConnected = false; public double latency = 0.0; public double fiducialMarksID = 0.0; @@ -14,7 +14,7 @@ class VisionIOInputs { public int targetsCount = 0; } - default void updateInputs(VisionIOInputs inputs) {} + default void updateInputs(VisionInputs inputs) {} default String getLimelightName(int limelightNumber) { return ""; From 3ee584c1a9358450b5474ff7be7b95387739959b Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 14 Nov 2024 12:45:29 -0500 Subject: [PATCH 19/75] fix things and stuff --- src/main/java/frc/robot/Robot.java | 10 +--------- src/main/java/frc/robot/RobotContainer.java | 10 ++++------ .../frc/robot/subsystems/vision/SimulatedVision.java | 8 ++------ .../frc/robot/subsystems/vision/VisionConstants.java | 4 +++- 4 files changed, 10 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 437095f6..49b3d6dd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,9 +4,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Constants.Mode; import frc.robot.extras.simulation.field.SimulatedField; - -import java.io.IOException; - import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -80,12 +77,7 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. - try { - m_robotContainer = new RobotContainer(); - } catch (IOException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } + m_robotContainer = new RobotContainer(); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1a000002..a37dda15 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,12 +25,10 @@ import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; import frc.robot.subsystems.swerve.moduleIO.PhysicalModule; import frc.robot.subsystems.swerve.moduleIO.SimulatedModule; -import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionInterface; import frc.robot.subsystems.vision.PhysicalVision; import frc.robot.subsystems.vision.SimulatedVision; - -import java.io.IOException; +import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionInterface; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -53,7 +51,7 @@ public class RobotContainer { // Subsystems // private final XboxController driverController = new XboxController(0); - public RobotContainer() throws IOException { + public RobotContainer() { switch (Constants.currentMode) { case REAL -> { /* Real robot, instantiate hardware IO implementations */ @@ -109,7 +107,7 @@ public RobotContainer() throws IOException { new SimulatedModule(swerveDriveSimulation.getModules()[3])); // TODO: add sim impl - visionSubsystem = new Vision(new SimulatedVision(swerveDrive::getPose)); + visionSubsystem = new Vision(new SimulatedVision(swerveDrive::getPose)); SimulatedField.getInstance().resetFieldForAuto(); resetFieldAndOdometryForAuto( diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index fc10ca7b..54b5e784 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -1,10 +1,8 @@ package frc.robot.subsystems.vision; -import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.*; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; -import java.io.IOException; import java.util.ArrayList; import java.util.Arrays; import java.util.List; @@ -32,8 +30,7 @@ public class SimulatedVision extends PhysicalVision { private final int kResWidth = 1280; private final int kResHeight = 800; - public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) - throws IOException { + public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { this.robotActualPoseInSimulationSupplier = robotActualPoseInSimulationSupplier; // Create the vision system simulation which handles cameras and targets on the // field. @@ -41,8 +38,7 @@ public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) // Add all the AprilTags inside the tag layout as visible targets to this // simulated field. - visionSim.addAprilTags( - AprilTagFieldLayout.loadFromResource(VisionConstants.APRIL_TAG_FIELD.m_resourceFile)); + visionSim.addAprilTags(VisionConstants.FIELD_LAYOUT); // Create simulated camera properties. These can be set to mimic your actual // camera. diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 8f63ee12..5274a131 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -1,10 +1,12 @@ package frc.robot.subsystems.vision; +import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.util.Units; public final class VisionConstants { - public static final AprilTagFields APRIL_TAG_FIELD = AprilTagFields.k2024Crescendo; + public static final AprilTagFieldLayout FIELD_LAYOUT = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); public static final double VISION_X_POS_TRUST = 0.5; // meters public static final double VISION_Y_POS_TRUST = 0.5; // meters From e27a6447af8e26e1797da623b36cd4de41412fd5 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 14 Nov 2024 13:22:41 -0500 Subject: [PATCH 20/75] huh --- src/main/java/frc/robot/subsystems/vision/PhysicalVision.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 12f46414..cb409014 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -39,7 +39,6 @@ public PhysicalVision() { @Override public void updateInputs(VisionInputs inputs) { inputs.camerasAmount = limelightEstimates.length; - inputs.cameraConnected = true; for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { // Update camera connection status From cf813657675442445c4533e247b99e1d95a9ddc7 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 14 Nov 2024 15:13:08 -0500 Subject: [PATCH 21/75] epic --- .../subsystems/vision/PhysicalVision.java | 36 ++++++++++++++++++- .../subsystems/vision/VisionInterface.java | 3 ++ 2 files changed, 38 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index cb409014..306f5813 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.FieldConstants; @@ -13,6 +14,7 @@ import java.util.concurrent.Executors; import java.util.concurrent.TimeUnit; import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicReference; public class PhysicalVision implements VisionInterface { @@ -21,6 +23,7 @@ public class PhysicalVision implements VisionInterface { private double headingRateDegreesPerSecond = 0; private final Map limelightThreads = new ConcurrentHashMap<>(); private final ExecutorService executorService = Executors.newFixedThreadPool(3); + private final AtomicReference latestInputs = new AtomicReference<>(new VisionInputs()); /** * The pose estimates from the limelights in the following order {shooterLimelight, @@ -36,9 +39,33 @@ public PhysicalVision() { } } +// private void setLLSettings() { +// double[] camerapose = { 0.0, 0.0, Constants.kCameraHeightOffGroundMeters, 0.0, Constants.kCameraPitchDegrees, +// 0.0 }; +// table.getEntry("camerapose_robotspace_set").setDoubleArray(camerapose); + +// double[] cameraBpose = { 0.0, 0.0, Constants.kCameraBHeightOffGroundMeters, Constants.kCameraBRollDegrees, +// Constants.kCameraBPitchDegrees, 0.0 }; +// tableB.getEntry("camerapose_robotspace_set").setDoubleArray(cameraBpose); + +// var fieldToTurretRotation = robotState.getLatestFieldToRobot().getValue().getRotation() +// .rotateBy(robotState.getLatestRobotToTurret().getValue()); +// var fieldToTurretVelocity = Units.radiansToDegrees(robotState.getLatestTurretAngularVelocity() +// + robotState.getLatestRobotRelativeChassisSpeed().omegaRadiansPerSecond); +// LimelightHelpers.SetRobotOrientation(Constants.kLimelightTableName, fieldToTurretRotation.getDegrees(), +// fieldToTurretVelocity, 0, 0, 0, 0); + +// var gyroAngle = robotState.getLatestFieldToRobot().getValue().getRotation(); +// var gyroAngularVelocity = Units +// .radiansToDegrees(robotState.getLatestRobotRelativeChassisSpeed().omegaRadiansPerSecond); +// LimelightHelpers.SetRobotOrientation(Constants.kLimelightBTableName, gyroAngle.getDegrees(), +// gyroAngularVelocity, 0, 0, 0, 0); +// } + @Override public void updateInputs(VisionInputs inputs) { inputs.camerasAmount = limelightEstimates.length; + inputs.isShooterLimelightConnected = isLimelightConnected(0); for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { // Update camera connection status @@ -56,7 +83,7 @@ public void updateInputs(VisionInputs inputs) { // Calculate average latency inputs.latency /= limelightEstimates.length; - + latestInputs.set(inputs); periodic(); } @@ -294,6 +321,13 @@ public String getLimelightName(int limelightNumber) { }; } + public boolean isLimelightConnected(int limelightNumber) { + NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(getLimelightName(limelightNumber)); + if (limelightTable.containsKey("tx")) { + return true; + } return false; + } + /** * This checks is there is new pose detected by a limelight, and if so, updates the pose estimate * diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index f7f7f849..74c80dc7 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -6,6 +6,9 @@ public interface VisionInterface { @AutoLog class VisionInputs { + public boolean isShooterLimelightConnected = false; + public boolean isFrontLeftLimelightConnected = false; + public boolean isFrontRightLimelightConnect = false; public boolean cameraConnected = false; public double latency = 0.0; public double fiducialMarksID = 0.0; From 5a19eeda33c7fdfd8899f3f1d1a35a6480895f32 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Fri, 15 Nov 2024 11:51:45 -0500 Subject: [PATCH 22/75] add Limelight enum --- .../subsystems/vision/PhysicalVision.java | 242 +++++++++--------- .../subsystems/vision/SimulatedVision.java | 4 +- .../subsystems/vision/VisionConstants.java | 13 +- .../subsystems/vision/VisionInterface.java | 22 +- 4 files changed, 156 insertions(+), 125 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 306f5813..5e8868e8 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -4,10 +4,11 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.FieldConstants; import frc.robot.extras.vision.LimelightHelpers; import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; +import frc.robot.subsystems.vision.VisionConstants.Limelight; + import java.util.Map; import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.ExecutorService; @@ -23,7 +24,8 @@ public class PhysicalVision implements VisionInterface { private double headingRateDegreesPerSecond = 0; private final Map limelightThreads = new ConcurrentHashMap<>(); private final ExecutorService executorService = Executors.newFixedThreadPool(3); - private final AtomicReference latestInputs = new AtomicReference<>(new VisionInputs()); + private final AtomicReference latestInputs = + new AtomicReference<>(new VisionInputs()); /** * The pose estimates from the limelights in the following order {shooterLimelight, @@ -39,38 +41,49 @@ public PhysicalVision() { } } -// private void setLLSettings() { -// double[] camerapose = { 0.0, 0.0, Constants.kCameraHeightOffGroundMeters, 0.0, Constants.kCameraPitchDegrees, -// 0.0 }; -// table.getEntry("camerapose_robotspace_set").setDoubleArray(camerapose); - -// double[] cameraBpose = { 0.0, 0.0, Constants.kCameraBHeightOffGroundMeters, Constants.kCameraBRollDegrees, -// Constants.kCameraBPitchDegrees, 0.0 }; -// tableB.getEntry("camerapose_robotspace_set").setDoubleArray(cameraBpose); - -// var fieldToTurretRotation = robotState.getLatestFieldToRobot().getValue().getRotation() -// .rotateBy(robotState.getLatestRobotToTurret().getValue()); -// var fieldToTurretVelocity = Units.radiansToDegrees(robotState.getLatestTurretAngularVelocity() -// + robotState.getLatestRobotRelativeChassisSpeed().omegaRadiansPerSecond); -// LimelightHelpers.SetRobotOrientation(Constants.kLimelightTableName, fieldToTurretRotation.getDegrees(), -// fieldToTurretVelocity, 0, 0, 0, 0); - -// var gyroAngle = robotState.getLatestFieldToRobot().getValue().getRotation(); -// var gyroAngularVelocity = Units -// .radiansToDegrees(robotState.getLatestRobotRelativeChassisSpeed().omegaRadiansPerSecond); -// LimelightHelpers.SetRobotOrientation(Constants.kLimelightBTableName, gyroAngle.getDegrees(), -// gyroAngularVelocity, 0, 0, 0, 0); -// } + // private void setLLSettings() { + // double[] camerapose = { 0.0, 0.0, Constants.kCameraHeightOffGroundMeters, 0.0, + // Constants.kCameraPitchDegrees, + // 0.0 }; + // table.getEntry("camerapose_robotspace_set").setDoubleArray(camerapose); + + // double[] cameraBpose = { 0.0, 0.0, Constants.kCameraBHeightOffGroundMeters, + // Constants.kCameraBRollDegrees, + // Constants.kCameraBPitchDegrees, 0.0 }; + // tableB.getEntry("camerapose_robotspace_set").setDoubleArray(cameraBpose); + + // var fieldToTurretRotation = robotState.getLatestFieldToRobot().getValue().getRotation() + // .rotateBy(robotState.getLatestRobotToTurret().getValue()); + // var fieldToTurretVelocity = + // Units.radiansToDegrees(robotState.getLatestTurretAngularVelocity() + // + robotState.getLatestRobotRelativeChassisSpeed().omegaRadiansPerSecond); + // LimelightHelpers.SetRobotOrientation(Constants.kLimelightTableName, + // fieldToTurretRotation.getDegrees(), + // fieldToTurretVelocity, 0, 0, 0, 0); + + // var gyroAngle = robotState.getLatestFieldToRobot().getValue().getRotation(); + // var gyroAngularVelocity = Units + // + // .radiansToDegrees(robotState.getLatestRobotRelativeChassisSpeed().omegaRadiansPerSecond); + // LimelightHelpers.SetRobotOrientation(Constants.kLimelightBTableName, + // gyroAngle.getDegrees(), + // gyroAngularVelocity, 0, 0, 0, 0); + // } @Override public void updateInputs(VisionInputs inputs) { inputs.camerasAmount = limelightEstimates.length; - inputs.isShooterLimelightConnected = isLimelightConnected(0); - + inputs.isShooterLimelightConnected = isLimelightConnected(Limelight.SHOOTER); + inputs.isFrontLeftLimelightConnected = isLimelightConnected(Limelight.FRONT_LEFT); + inputs.isFrontRightLimelightConnected = isLimelightConnected(Limelight.FRONT_RIGHT); + + if (inputs.isShooterLimelightConnected) { + inputs.shooterMegaTag1Pose = getMegaTag1PoseEstimate(Limelight.SHOOTER).pose; + inputs.shooterMegaTag2Pose = getMegaTag2PoseEstimate(Limelight.SHOOTER).pose; + // inputs. + visionThread(Limelight.SHOOTER); + } for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { - // Update camera connection status - inputs.cameraConnected = true; - // Add number of April tags seen by this limelight inputs.targetsCount += getNumberOfAprilTags(limelightNumber); @@ -84,26 +97,25 @@ public void updateInputs(VisionInputs inputs) { // Calculate average latency inputs.latency /= limelightEstimates.length; latestInputs.set(inputs); - periodic(); } /** * Checks if the specified limelight can fully see one or more April Tag. * - * @param limelightNumber the number of the limelight + * @param limelight the number of the limelight * @return true if the limelight can fully see one or more April Tag */ - @Override - public boolean canSeeAprilTags(int limelightNumber) { + // @Override + public boolean canSeeAprilTags(Limelight limelight) { // First checks if it can see an april tag, then checks if it is fully in frame // Different Limelights have different FOVs - if (getNumberOfAprilTags(limelightNumber) > 0 - && getNumberOfAprilTags(limelightNumber) <= VisionConstants.APRIL_TAG_POSITIONS.length) { - if (getLimelightName(limelightNumber).equals(VisionConstants.SHOOTER_LIMELIGHT_NAME)) { - return Math.abs(LimelightHelpers.getTX(getLimelightName(limelightNumber))) + if (getNumberOfAprilTags(limelight) > 0 + && getNumberOfAprilTags(limelight) <= VisionConstants.APRIL_TAG_POSITIONS.length) { + if (getLimelightName(limelight).equals(VisionConstants.SHOOTER_LIMELIGHT_NAME)) { + return Math.abs(LimelightHelpers.getTX(getLimelightName(limelight))) <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR; } else { - return Math.abs(LimelightHelpers.getTX(getLimelightName(limelightNumber))) + return Math.abs(LimelightHelpers.getTX(getLimelightName(limelight))) <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR; } } @@ -114,47 +126,47 @@ && getNumberOfAprilTags(limelightNumber) <= VisionConstants.APRIL_TAG_POSITIONS. * Gets the JSON dump from the specified limelight and puts it into a PoseEstimate object, which * is then placed into its corresponding spot in the limelightEstimates array. * - * @param limelightNumber the number of the limelight + * @param limelight the number of the limelight */ - public PoseEstimate enabledPoseUpdate(int limelightNumber) { - if (canSeeAprilTags(limelightNumber) && isValidPoseEstimate(limelightNumber)) { - if (isLargeDiscrepancyBetweenMegaTag1And2(limelightNumber) - && getLimelightAprilTagDistance(limelightNumber) + public PoseEstimate enabledPoseUpdate(Limelight limelight) { + if (canSeeAprilTags(limelight) && isValidPoseEstimate(limelight)) { + if (isLargeDiscrepancyBetweenMegaTag1And2(limelight) + && getLimelightAprilTagDistance(limelight) < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { - return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber); + return limelightEstimates[limelight.id] = getMegaTag1PoseEstimate(limelight); } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) { LimelightHelpers.SetRobotOrientation( - getLimelightName(limelightNumber), headingDegrees, 0, 0, 0, 0, 0); - return limelightEstimates[limelightNumber] = getMegaTag2PoseEstimate(limelightNumber); + getLimelightName(limelight), headingDegrees, 0, 0, 0, 0, 0); + return limelightEstimates[limelight.id] = getMegaTag2PoseEstimate(limelight); } else { - return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber); + return limelightEstimates[limelight.id] = getMegaTag1PoseEstimate(limelight); } } - return limelightEstimates[limelightNumber] = new PoseEstimate(); + return limelightEstimates[limelight.id] = new PoseEstimate(); } /** * If the robot is not enabled, update the pose using MegaTag1 and after it is enabled, run {@link * #enabledPoseUpdate(int)} * - * @param limelightNumber the number of the limelight + * @param limelight the number of the limelight */ - public void updatePoseEstimate(int limelightNumber) { - limelightEstimates[limelightNumber] = + public void updatePoseEstimate(Limelight limelight) { + limelightEstimates[limelight.id] = DriverStation.isEnabled() - ? enabledPoseUpdate(limelightNumber) - : getMegaTag1PoseEstimate(limelightNumber); + ? enabledPoseUpdate(limelight) + : getMegaTag1PoseEstimate(limelight); } /** * Checks if there is a large discrepancy between the MegaTag1 and MegaTag2 estimates. * - * @param limelightNumber the number of the limelight + * @param limelight the number of the limelight * @return true if the discrepancy is larger than the defined threshold, false otherwise */ - public boolean isLargeDiscrepancyBetweenMegaTag1And2(int limelightNumber) { - PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelightNumber); - PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber); + public boolean isLargeDiscrepancyBetweenMegaTag1And2(Limelight limelight) { + PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelight); + PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelight); // Extract the positions of the two poses Translation2d megaTag1TranslationMeters = megaTag1Estimate.pose.getTranslation(); @@ -183,35 +195,35 @@ public boolean isLargeDiscrepancyBetweenMegaTag1And2(int limelightNumber) { * Gets the MegaTag1 pose of the robot calculated by specified limelight via any April Tags it * sees * - * @param limelightNumber the number of the limelight + * @param limelight the number of the limelight * @return the MegaTag1 pose of the robot, if the limelight can't see any April Tags, it will * return 0 for x, y, and theta */ - public PoseEstimate getMegaTag1PoseEstimate(int limelightNumber) { - return LimelightHelpers.getBotPoseEstimate_wpiBlue(getLimelightName(limelightNumber)); + public PoseEstimate getMegaTag1PoseEstimate(Limelight limelight) { + return LimelightHelpers.getBotPoseEstimate_wpiBlue(getLimelightName(limelight)); } /** * Gets the MegaTag2 pose of the robot calculated by specified limelight via any April Tags it * sees * - * @param limelightNumber the number of the limelight + * @param limelight the number of the limelight * @return the MegaTag2 pose of the robot, if the limelight can't see any April Tags, it will * return 0 for x, y, and theta */ - public PoseEstimate getMegaTag2PoseEstimate(int limelightNumber) { - return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(getLimelightName(limelightNumber)); + public PoseEstimate getMegaTag2PoseEstimate(Limelight limelight) { + return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(getLimelightName(limelight)); } /** * Checks if the MT1 and MT2 pose estimate exists and whether it is within the field * - * @param limelightNumber the number of the limelight + * @param limelight the number of the limelight * @return true if the pose estimate exists within the field and the pose estimate is not null */ - public boolean isValidPoseEstimate(int limelightNumber) { - PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelightNumber); - PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber); + public boolean isValidPoseEstimate(Limelight limelight) { + PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelight); + PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelight); return LimelightHelpers.isValidPoseEstimate(megaTag1Estimate) && LimelightHelpers.isValidPoseEstimate(megaTag2Estimate) @@ -249,18 +261,18 @@ public Pose2d getPoseFromAprilTags(int limelightNumber) { } /** Returns how many april tags the limelight that is being used for pose estimation can see. */ - @Override - public int getNumberOfAprilTags(int limelightNumber) { - return limelightEstimates[limelightNumber].tagCount; + // @Override + public int getNumberOfAprilTags(Limelight limelightNumber) { + return limelightEstimates[limelightNumber.id].tagCount; } /** * Returns the timestamp for the vision measurement from the limelight that is being used for pose * estimation. */ - @Override - public double getTimeStampSeconds(int limelightNumber) { - return limelightEstimates[limelightNumber].timestampSeconds / 1000.0; + // @Override + public double getTimeStampSeconds(Limelight limelight) { + return limelightEstimates[limelight.id].timestampSeconds / 1000.0; } /** @@ -268,9 +280,9 @@ public double getTimeStampSeconds(int limelightNumber) { * calculated the robot's pose. It adds the pipeline latency, capture latency, and json parsing * latency. */ - @Override - public double getLatencySeconds(int limelightNumber) { - return (limelightEstimates[limelightNumber].latency) / 1000.0; + // @Override + public double getLatencySeconds(Limelight limelight) { + return (limelightEstimates[limelight.id].latency) / 1000.0; } /** Gets the pose calculated the last time a limelight saw an April Tag */ @@ -284,9 +296,9 @@ public Pose2d getLastSeenPose() { * @param limelightNumber the number of the limelight * @return the average distance between the robot and the April Tag(s) in meters */ - public double getLimelightAprilTagDistance(int limelightNumber) { - if (canSeeAprilTags(limelightNumber)) { - return limelightEstimates[limelightNumber].avgTagDist; + public double getLimelightAprilTagDistance(Limelight limelight) { + if (canSeeAprilTags(limelight)) { + return limelightEstimates[limelight.id].avgTagDist; } // To be safe returns a big distance from the april tags if it can't see any return Double.MAX_VALUE; @@ -308,32 +320,34 @@ public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { /** * Gets the limelight name associated with the specified limelight number/index * - * @param limelightNumber the limelight number + * @param limelight the limelight number * @return 0 = limelight-shooter, 1 = limelight-left, 2 = limelight-right */ - public String getLimelightName(int limelightNumber) { - return switch (limelightNumber) { - case 0 -> VisionConstants.SHOOTER_LIMELIGHT_NAME; - case 1 -> VisionConstants.FRONT_LEFT_LIMELIGHT_NAME; - case 2 -> VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME; + public String getLimelightName(Limelight limelight) { + return switch (limelight) { + case SHOOTER -> VisionConstants.SHOOTER_LIMELIGHT_NAME; + case FRONT_LEFT -> VisionConstants.FRONT_LEFT_LIMELIGHT_NAME; + case FRONT_RIGHT -> VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME; default -> throw new IllegalArgumentException("You entered a number for a non-existent limelight"); }; } - public boolean isLimelightConnected(int limelightNumber) { - NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(getLimelightName(limelightNumber)); + public boolean isLimelightConnected(Limelight limelight) { + NetworkTable limelightTable = + LimelightHelpers.getLimelightNTTable(getLimelightName(limelight)); if (limelightTable.containsKey("tx")) { return true; - } return false; } + return false; + } /** * This checks is there is new pose detected by a limelight, and if so, updates the pose estimate * - * @param limelightNumber the limelight number + * @param limelight the limelight number */ - public void checkAndUpdatePose(int limelightNumber) { + public void checkAndUpdatePose(Limelight limelight) { double last_TX = 0; double last_TY = 0; @@ -346,31 +360,31 @@ public void checkAndUpdatePose(int limelightNumber) { // object, as its reference was modified earlier. synchronized (this) { try { - double current_TX = LimelightHelpers.getTX(getLimelightName(limelightNumber)); - double current_TY = LimelightHelpers.getTY(getLimelightName(limelightNumber)); + double current_TX = LimelightHelpers.getTX(getLimelightName(limelight)); + double current_TY = LimelightHelpers.getTY(getLimelightName(limelight)); // This checks if the limelight reading is new. The reasoning being that if the TX and TY // are EXACTLY the same, it hasn't updated yet with a new reading. We are doing it this way, // because to get the timestamp of the reading, you need to parse the JSON dump which can be // very demanding whereas this only has to get the Network Table entries for TX and TY. if (current_TX != last_TX || current_TY != last_TY) { - updatePoseEstimate(limelightNumber); + updatePoseEstimate(limelight); limelightThreads.computeIfPresent( - limelightNumber, (key, value) -> new AtomicBoolean(true)); + limelight.id, (key, value) -> new AtomicBoolean(true)); // This is to keep track of the last valid pose calculated by the limelights // it is used when the driver resets the robot odometry to the limelight calculated // position - if (canSeeAprilTags(limelightNumber)) { - lastSeenPose = getMegaTag1PoseEstimate(limelightNumber).pose; + if (canSeeAprilTags(limelight)) { + lastSeenPose = getMegaTag1PoseEstimate(limelight).pose; } } else { // Retrieve the AtomicBoolean for the given limelight number AtomicBoolean isThreadRunning = - limelightThreads.getOrDefault(limelightNumber, new AtomicBoolean()); + limelightThreads.getOrDefault(limelight, new AtomicBoolean()); // Only stop the thread if it's currently running if (isThreadRunning.get()) { // stop the thread for the specified limelight - stopThread(limelightNumber); + stopThread(limelight); } } last_TX = current_TX; @@ -378,7 +392,7 @@ public void checkAndUpdatePose(int limelightNumber) { } catch (Exception e) { System.err.println( "Error communicating with the: " - + getLimelightName(limelightNumber) + + getLimelightName(limelight) + ": " + e.getMessage()); } @@ -397,20 +411,20 @@ public void checkAndUpdatePose(int limelightNumber) { * corresponding limelight's thread is marked as "running". This ensures that pose estimates are * updated in real-time, leveraging the parallel processing capabilities of the executor service. * - * @param limelightNumber the limelight number + * @param limelight the limelight number */ - public void visionThread(int limelightNumber) { + public void visionThread(Limelight limelight) { executorService.submit( () -> { try { // while (limelightThreads.get(limelightNumber).get()) { - checkAndUpdatePose(limelightNumber); + checkAndUpdatePose(limelight); // } } catch (Exception e) { System.err.println( "Error executing task for the: " - + getLimelightName(limelightNumber) + + getLimelightName(limelight) + ": " + e.getMessage()); } @@ -421,18 +435,18 @@ public void visionThread(int limelightNumber) { * Sets the AtomicBoolean 'runningThreads' to false for the specified limelight. Stops the thread * for the specified limelight. * - * @param limelightNumber the limelight number + * @param limelight the limelight number */ - public void stopThread(int limelightNumber) { + public void stopThread(Limelight limelight) { try { // Since we can't see an April Tag, set the estimate for the specified limelight to an empty // PoseEstimate() - limelightEstimates[limelightNumber] = new PoseEstimate(); - limelightThreads.get(limelightNumber).set(false); + limelightEstimates[limelight.id] = new PoseEstimate(); + limelightThreads.get(limelight.id).set(false); } catch (Exception e) { System.err.println( "Error stopping thread for the: " - + getLimelightName(limelightNumber) + + getLimelightName(limelight) + ": " + e.getMessage()); } @@ -457,16 +471,4 @@ public void endAllThreads() { Thread.currentThread().interrupt(); } } - - // Override periodic method to start the vision threads at the beginning of each subsystem tick - public void periodic() { - visionThread(VisionConstants.SHOOTER_LIMELIGHT_NUMBER); - visionThread(VisionConstants.FRONT_LEFT_LIMELIGHT_NUMBER); - visionThread(VisionConstants.FRONT_RIGHT_LIMELIGHT_NUMBER); - SmartDashboard.putNumber("april tag dist", getLimelightAprilTagDistance(0)); - SmartDashboard.putString("shooter ll odom", getPoseFromAprilTags(0).toString()); - SmartDashboard.putString("left ll odom", getPoseFromAprilTags(1).toString()); - - SmartDashboard.putString("right ll odom", getPoseFromAprilTags(2).toString()); - } } diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 54b5e784..953a64ef 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -122,7 +122,7 @@ private void writeToTable(List results, NetworkTable table 0.0, // 3: roll 0.0, // 4: pitch fieldToCamera.getRotation().getDegrees(), // 5: yaw - result.getTimestampSeconds(), // 6: latency ms, + super.getLatencySeconds(0), // 6: latency ms, (double) result.getMultiTagResult().get().fiducialIDsUsed.size(), // 7: tag count 0.0, // 8: tag span @@ -150,7 +150,7 @@ private void writeToTable(List results, NetworkTable table } table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); - table.getEntry("cl").setDouble(result.getTimestampSeconds()); + table.getEntry("cl").setDouble(super.getLatencySeconds(0)); } } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 5274a131..eac08c9e 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -5,9 +5,20 @@ import edu.wpi.first.math.util.Units; public final class VisionConstants { + public enum Limelight { + SHOOTER(0), + FRONT_LEFT(1), + FRONT_RIGHT(2); + + public final int id; + + Limelight(int id) { + this.id = id; + } + } + public static final AprilTagFieldLayout FIELD_LAYOUT = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - public static final double VISION_X_POS_TRUST = 0.5; // meters public static final double VISION_Y_POS_TRUST = 0.5; // meters public static final double VISION_ANGLE_TRUST = Units.degreesToRadians(50); // radians diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index 74c80dc7..3cefc898 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -8,8 +8,26 @@ public interface VisionInterface { class VisionInputs { public boolean isShooterLimelightConnected = false; public boolean isFrontLeftLimelightConnected = false; - public boolean isFrontRightLimelightConnect = false; - public boolean cameraConnected = false; + public boolean isFrontRightLimelightConnected = false; + + public Pose2d shooterMegaTag1Pose = new Pose2d(); + public double shooterTagCount = 0.0; + public Pose2d shooterMegaTag2Pose = new Pose2d(); + public double shooterLatency = 0.0; + public double shooterTargets = 0.0; + + public Pose2d frontLeftMegaTag1Pose = new Pose2d(); + public double frontLeftTagCount = 0.0; + public Pose2d frontLeftMegaTag2Pose = new Pose2d(); + public double frontLeftLatency = 0.0; + public double frontLeftTargets = 0.0; + + public Pose2d frontRightMegaTag1Pose = new Pose2d(); + public double frontRightTagCount = 0.0; + public Pose2d frontRightMegaTag2Pose = new Pose2d(); + public double frontRightLatency = 0.0; + public double frontRightTargets = 0.0; + public double latency = 0.0; public double fiducialMarksID = 0.0; From ff2f5aa18b2bed04a798edc4bbbf93b77d2143f0 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Fri, 15 Nov 2024 11:56:32 -0500 Subject: [PATCH 23/75] format --- .../robot/subsystems/vision/PhysicalVision.java | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 5e8868e8..e632c61f 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -8,7 +8,6 @@ import frc.robot.extras.vision.LimelightHelpers; import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; - import java.util.Map; import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.ExecutorService; @@ -334,8 +333,7 @@ public String getLimelightName(Limelight limelight) { } public boolean isLimelightConnected(Limelight limelight) { - NetworkTable limelightTable = - LimelightHelpers.getLimelightNTTable(getLimelightName(limelight)); + NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(getLimelightName(limelight)); if (limelightTable.containsKey("tx")) { return true; } @@ -369,8 +367,7 @@ public void checkAndUpdatePose(Limelight limelight) { // very demanding whereas this only has to get the Network Table entries for TX and TY. if (current_TX != last_TX || current_TY != last_TY) { updatePoseEstimate(limelight); - limelightThreads.computeIfPresent( - limelight.id, (key, value) -> new AtomicBoolean(true)); + limelightThreads.computeIfPresent(limelight.id, (key, value) -> new AtomicBoolean(true)); // This is to keep track of the last valid pose calculated by the limelights // it is used when the driver resets the robot odometry to the limelight calculated // position @@ -391,10 +388,7 @@ public void checkAndUpdatePose(Limelight limelight) { last_TY = current_TY; } catch (Exception e) { System.err.println( - "Error communicating with the: " - + getLimelightName(limelight) - + ": " - + e.getMessage()); + "Error communicating with the: " + getLimelightName(limelight) + ": " + e.getMessage()); } } } @@ -445,10 +439,7 @@ public void stopThread(Limelight limelight) { limelightThreads.get(limelight.id).set(false); } catch (Exception e) { System.err.println( - "Error stopping thread for the: " - + getLimelightName(limelight) - + ": " - + e.getMessage()); + "Error stopping thread for the: " + getLimelightName(limelight) + ": " + e.getMessage()); } } From c4135f043980eb941109940dd6bc04170e3706ce Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Fri, 15 Nov 2024 12:26:19 -0500 Subject: [PATCH 24/75] idk i did smthn probably --- .../subsystems/vision/PhysicalVision.java | 37 +++++++++++-------- .../subsystems/vision/VisionInterface.java | 6 +-- 2 files changed, 22 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index e632c61f..013e1077 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -79,22 +79,27 @@ public void updateInputs(VisionInputs inputs) { if (inputs.isShooterLimelightConnected) { inputs.shooterMegaTag1Pose = getMegaTag1PoseEstimate(Limelight.SHOOTER).pose; inputs.shooterMegaTag2Pose = getMegaTag2PoseEstimate(Limelight.SHOOTER).pose; - // inputs. + inputs.shooterLatency = getLatencySeconds(Limelight.SHOOTER); + inputs.shooterTargets = getNumberOfAprilTags(Limelight.SHOOTER); visionThread(Limelight.SHOOTER); } - for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { - // Add number of April tags seen by this limelight - inputs.targetsCount += getNumberOfAprilTags(limelightNumber); - // Add fiducial mark ID - inputs.fiducialMarksID = LimelightHelpers.getFiducialID(getLimelightName(limelightNumber)); + if (inputs.isFrontLeftLimelightConnected) { + inputs.frontLeftMegaTag1Pose = getMegaTag1PoseEstimate(Limelight.FRONT_LEFT).pose; + inputs.frontLeftMegaTag2Pose = getMegaTag2PoseEstimate(Limelight.FRONT_LEFT).pose; + inputs.frontLeftLatency = getLatencySeconds(Limelight.FRONT_LEFT); + inputs.frontLeftTargets = getNumberOfAprilTags(Limelight.FRONT_LEFT); + visionThread(Limelight.FRONT_LEFT); + } - // Add latency for this limelight - inputs.latency += getLatencySeconds(limelightNumber) / 1000.0; + if (inputs.isFrontRightLimelightConnected) { + inputs.frontRightMegaTag1Pose = getMegaTag1PoseEstimate(Limelight.FRONT_RIGHT).pose; + inputs.frontRightMegaTag2Pose = getMegaTag2PoseEstimate(Limelight.FRONT_RIGHT).pose; + inputs.frontRightLatency = getLatencySeconds(Limelight.FRONT_RIGHT); + inputs.frontRightTargets = getNumberOfAprilTags(Limelight.FRONT_RIGHT); + visionThread(Limelight.FRONT_RIGHT); } - // Calculate average latency - inputs.latency /= limelightEstimates.length; latestInputs.set(inputs); } @@ -250,19 +255,19 @@ private boolean isWithinFieldBounds( /** * Gets the pose of the robot calculated by specified limelight via any April Tags it sees * - * @param limelightNumber the number of the limelight + * @param limelight the number of the limelight * @return the pose of the robot, if the limelight can't see any April Tags, it will return 0 for * x, y, and theta */ - @Override - public Pose2d getPoseFromAprilTags(int limelightNumber) { - return limelightEstimates[limelightNumber].pose; + // @Override + public Pose2d getPoseFromAprilTags(Limelight limelight) { + return limelightEstimates[limelight.id].pose; } /** Returns how many april tags the limelight that is being used for pose estimation can see. */ // @Override - public int getNumberOfAprilTags(Limelight limelightNumber) { - return limelightEstimates[limelightNumber.id].tagCount; + public int getNumberOfAprilTags(Limelight limelight) { + return limelightEstimates[limelight.id].tagCount; } /** diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index 3cefc898..8401d47a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -28,11 +28,7 @@ class VisionInputs { public double frontRightLatency = 0.0; public double frontRightTargets = 0.0; - public double latency = 0.0; - public double fiducialMarksID = 0.0; - - public int camerasAmount = 0; - public int targetsCount = 0; + public double camerasAmount = 0.0; } default void updateInputs(VisionInputs inputs) {} From 92c19f0b85c857039933f4a3cf3cd486cc6e7f1e Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Sat, 16 Nov 2024 09:53:09 -0500 Subject: [PATCH 25/75] fix dumbass ll --- .../robot/extras/vision/LimelightHelpers.java | 10 +++++---- .../subsystems/vision/PhysicalVision.java | 21 ++++++++++++++++--- .../subsystems/vision/SimulatedVision.java | 18 +++++++++------- .../frc/robot/subsystems/vision/Vision.java | 2 +- .../subsystems/vision/VisionInterface.java | 6 ++++++ 5 files changed, 41 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/extras/vision/LimelightHelpers.java b/src/main/java/frc/robot/extras/vision/LimelightHelpers.java index a859a061..e588d637 100644 --- a/src/main/java/frc/robot/extras/vision/LimelightHelpers.java +++ b/src/main/java/frc/robot/extras/vision/LimelightHelpers.java @@ -679,12 +679,14 @@ public static double[] getCameraPose_TargetSpace(String limelightName) { return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); } - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + public static Pose2d getTargetPose_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose2D(poseArray); } - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + public static Pose2d getTargetPose_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose2D(poseArray); } public static double[] getTargetColor(String limelightName) { diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 013e1077..695abbec 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants.FieldConstants; import frc.robot.extras.vision.LimelightHelpers; +import frc.robot.extras.vision.LimelightHelpers.LimelightTarget_Fiducial; import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; import java.util.Map; @@ -80,7 +81,9 @@ public void updateInputs(VisionInputs inputs) { inputs.shooterMegaTag1Pose = getMegaTag1PoseEstimate(Limelight.SHOOTER).pose; inputs.shooterMegaTag2Pose = getMegaTag2PoseEstimate(Limelight.SHOOTER).pose; inputs.shooterLatency = getLatencySeconds(Limelight.SHOOTER); - inputs.shooterTargets = getNumberOfAprilTags(Limelight.SHOOTER); + inputs.shooterTargets = getNumberOfAprilTags(Limelight.SHOOTER); + inputs.shooterCameraToTargets = getAprilTagPositionToLimelight(Limelight.SHOOTER); + inputs.shooterRobotToTargets = getAprilTagPositionToRobot(Limelight.SHOOTER); visionThread(Limelight.SHOOTER); } @@ -89,6 +92,8 @@ public void updateInputs(VisionInputs inputs) { inputs.frontLeftMegaTag2Pose = getMegaTag2PoseEstimate(Limelight.FRONT_LEFT).pose; inputs.frontLeftLatency = getLatencySeconds(Limelight.FRONT_LEFT); inputs.frontLeftTargets = getNumberOfAprilTags(Limelight.FRONT_LEFT); + inputs.frontLeftCameraToTargets = getAprilTagPositionToLimelight(Limelight.FRONT_LEFT); + inputs.frontLeftRobotToTargets = getAprilTagPositionToRobot(Limelight.FRONT_LEFT); visionThread(Limelight.FRONT_LEFT); } @@ -97,6 +102,8 @@ public void updateInputs(VisionInputs inputs) { inputs.frontRightMegaTag2Pose = getMegaTag2PoseEstimate(Limelight.FRONT_RIGHT).pose; inputs.frontRightLatency = getLatencySeconds(Limelight.FRONT_RIGHT); inputs.frontRightTargets = getNumberOfAprilTags(Limelight.FRONT_RIGHT); + inputs.frontRightCameraToTargets = getAprilTagPositionToLimelight(Limelight.FRONT_RIGHT); + inputs.frontRightRobotToTargets = getAprilTagPositionToRobot(Limelight.FRONT_RIGHT); visionThread(Limelight.FRONT_RIGHT); } @@ -264,6 +271,14 @@ public Pose2d getPoseFromAprilTags(Limelight limelight) { return limelightEstimates[limelight.id].pose; } + public Pose2d getAprilTagPositionToLimelight(Limelight limelight) { + return LimelightHelpers.getTargetPose_CameraSpace(getLimelightName(limelight)); + } + + public Pose2d getAprilTagPositionToRobot(Limelight limelight) { + return LimelightHelpers.getTargetPose_RobotSpace(getLimelightName(limelight)); + } + /** Returns how many april tags the limelight that is being used for pose estimation can see. */ // @Override public int getNumberOfAprilTags(Limelight limelight) { @@ -324,8 +339,8 @@ public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { /** * Gets the limelight name associated with the specified limelight number/index * - * @param limelight the limelight number - * @return 0 = limelight-shooter, 1 = limelight-left, 2 = limelight-right + * @param limelight the limelight enum + * @return SHOOTER = limelight-shooter, FRONT_LEFT = limelight-left, FRONT_RIGHT = limelight-right */ public String getLimelightName(Limelight limelight) { return switch (limelight) { diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 953a64ef..5286f70d 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -3,6 +3,8 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import frc.robot.subsystems.vision.VisionConstants.Limelight; + import java.util.ArrayList; import java.util.Arrays; import java.util.List; @@ -25,13 +27,13 @@ public class SimulatedVision extends PhysicalVision { PhotonCameraSim frontLeftCameraSim; PhotonCameraSim frontRightCameraSim; private final VisionSystemSim visionSim; - private final Supplier robotActualPoseInSimulationSupplier; + private final Supplier robotSimulationPose; private final int kResWidth = 1280; private final int kResHeight = 800; public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { - this.robotActualPoseInSimulationSupplier = robotActualPoseInSimulationSupplier; + this.robotSimulationPose = robotActualPoseInSimulationSupplier; // Create the vision system simulation which handles cameras and targets on the // field. visionSim = new VisionSystemSim("main"); @@ -87,17 +89,17 @@ public void updateInputs(VisionInputs inputs) { // Abuse the updateInputs periodic call to update the sim // Move the vision sim robot on the field - if (robotActualPoseInSimulationSupplier.get() != null) { - visionSim.update(robotActualPoseInSimulationSupplier.get()); - Logger.recordOutput("Vision/SimIO/updateSimPose", robotActualPoseInSimulationSupplier.get()); + if (robotSimulationPose.get() != null) { + visionSim.update(robotSimulationPose.get()); + Logger.recordOutput("Vision/SimIO/updateSimPose", robotSimulationPose.get()); } NetworkTable shooterTable = - NetworkTableInstance.getDefault().getTable(super.getLimelightName(0)); + NetworkTableInstance.getDefault().getTable(super.getLimelightName(Limelight.SHOOTER)); NetworkTable frontLeftTable = - NetworkTableInstance.getDefault().getTable(super.getLimelightName(1)); + NetworkTableInstance.getDefault().getTable(super.getLimelightName(Limelight.FRONT_LEFT)); NetworkTable frontRightTable = - NetworkTableInstance.getDefault().getTable(super.getLimelightName(2)); + NetworkTableInstance.getDefault().getTable(super.getLimelightName(Limelight.FRONT_RIGHT)); // Write to limelight table writeToTable(shooterCamera.getAllUnreadResults(), shooterTable); writeToTable(frontLeftCamera.getAllUnreadResults(), frontLeftTable); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 493763df..f1d9ef36 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -18,7 +18,7 @@ public Vision(VisionInterface visionInterface) { public void periodic() { // Updates limelight inputs visionInterface.updateInputs(inputs); - Logger.processInputs(visionInterface.getLimelightName(0), inputs); + Logger.processInputs("Vision/", inputs); } // Add methods to support DriveCommand diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index 8401d47a..a69128eb 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -15,18 +15,24 @@ class VisionInputs { public Pose2d shooterMegaTag2Pose = new Pose2d(); public double shooterLatency = 0.0; public double shooterTargets = 0.0; + public Pose2d shooterCameraToTargets = new Pose2d(); + public Pose2d shooterRobotToTargets = new Pose2d(); public Pose2d frontLeftMegaTag1Pose = new Pose2d(); public double frontLeftTagCount = 0.0; public Pose2d frontLeftMegaTag2Pose = new Pose2d(); public double frontLeftLatency = 0.0; public double frontLeftTargets = 0.0; + public Pose2d frontLeftCameraToTargets = new Pose2d(); + public Pose2d frontLeftRobotToTargets = new Pose2d(); public Pose2d frontRightMegaTag1Pose = new Pose2d(); public double frontRightTagCount = 0.0; public Pose2d frontRightMegaTag2Pose = new Pose2d(); public double frontRightLatency = 0.0; public double frontRightTargets = 0.0; + public Pose2d frontRightCameraToTargets = new Pose2d(); + public Pose2d frontRightRobotToTargets = new Pose2d(); public double camerasAmount = 0.0; } From dfc9643c887d3fc070584d7090f56b4b6b320ef7 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Sat, 16 Nov 2024 10:05:03 -0500 Subject: [PATCH 26/75] format --- src/main/java/frc/robot/Constants.java | 6 +++--- .../java/frc/robot/subsystems/vision/PhysicalVision.java | 3 +-- .../java/frc/robot/subsystems/vision/SimulatedVision.java | 1 - 3 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f07409a4..fc3c3499 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,12 +3,12 @@ import edu.wpi.first.math.util.Units; import frc.robot.subsystems.swerve.SwerveConstants.*; +/** Values are statically stored here used globally throughout the code. */ public final class Constants { public static final class LogPaths { public static final String SYSTEM_PERFORMANCE_PATH = "SystemPerformance/"; public static final String PHYSICS_SIMULATION_PATH = "MaplePhysicsSimulation/"; - public static final String APRIL_TAGS_VISION_PATH = "Vision/AprilTags/"; } public static final Mode currentMode = Mode.SIM; @@ -51,8 +51,8 @@ public static final class HardwareConstants { public static final class FieldConstants { // TODO: Now that I think about it, I'm pretty sure these measurements stay the same every year, // so consider setting them in the base code - public static final double FIELD_LENGTH_METERS = Units.inchesToMeters(0 - 9); - public static final double FIELD_WIDTH_METERS = Units.inchesToMeters(0 - 9); + public static final double FIELD_LENGTH_METERS = Units.inchesToMeters(653); + public static final double FIELD_WIDTH_METERS = Units.inchesToMeters(325); } public static final class JoystickConstants { diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 695abbec..23c785ac 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants.FieldConstants; import frc.robot.extras.vision.LimelightHelpers; -import frc.robot.extras.vision.LimelightHelpers.LimelightTarget_Fiducial; import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; import java.util.Map; @@ -81,7 +80,7 @@ public void updateInputs(VisionInputs inputs) { inputs.shooterMegaTag1Pose = getMegaTag1PoseEstimate(Limelight.SHOOTER).pose; inputs.shooterMegaTag2Pose = getMegaTag2PoseEstimate(Limelight.SHOOTER).pose; inputs.shooterLatency = getLatencySeconds(Limelight.SHOOTER); - inputs.shooterTargets = getNumberOfAprilTags(Limelight.SHOOTER); + inputs.shooterTargets = getNumberOfAprilTags(Limelight.SHOOTER); inputs.shooterCameraToTargets = getAprilTagPositionToLimelight(Limelight.SHOOTER); inputs.shooterRobotToTargets = getAprilTagPositionToRobot(Limelight.SHOOTER); visionThread(Limelight.SHOOTER); diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 5286f70d..17cbf216 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -4,7 +4,6 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import frc.robot.subsystems.vision.VisionConstants.Limelight; - import java.util.ArrayList; import java.util.Arrays; import java.util.List; From ce8decbabe70d2cf15b6cb0c695dfbb9d109fc6f Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 18 Nov 2024 12:12:19 -0500 Subject: [PATCH 27/75] idk im gonna switch branches now --- src/main/java/frc/robot/RobotContainer.java | 5 +- .../commands/drive/DriveCommandBase.java | 23 +- .../extras/vision/FiducialObservation.java | 85 +++++ .../extras/vision/MegatagPoseEstimate.java | 90 +++++ .../vision/VisionFieldPoseEstimate.java | 34 ++ .../swerve/gyroIO/SimulatedGyro.java | 2 +- .../subsystems/vision/PhysicalVision.java | 36 +- .../subsystems/vision/SimulatedVision.java | 49 ++- .../frc/robot/subsystems/vision/Vision.java | 361 +++++++++++++++++- .../subsystems/vision/VisionInterface.java | 81 ++-- 10 files changed, 682 insertions(+), 84 deletions(-) create mode 100644 src/main/java/frc/robot/extras/vision/FiducialObservation.java create mode 100644 src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java create mode 100644 src/main/java/frc/robot/extras/vision/VisionFieldPoseEstimate.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a37dda15..bf75d60e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -106,7 +106,6 @@ public RobotContainer() { new SimulatedModule(swerveDriveSimulation.getModules()[2]), new SimulatedModule(swerveDriveSimulation.getModules()[3])); - // TODO: add sim impl visionSubsystem = new Vision(new SimulatedVision(swerveDrive::getPose)); SimulatedField.getInstance().resetFieldForAuto(); @@ -297,8 +296,8 @@ private void configureButtonBindings() { () -> swerveDrive.resetPosition(swerveDriveSimulation.getSimulatedDriveTrainPose()))); // // // Reset robot odometry based on vision pose measurement from april tags - // driverLeftDirectionPad.onTrue(new InstantCommand(() -> - // swerveDrive.resetOdometry(visionSubsystem.getLastSeenPose()))); + driverLeftDirectionPad.onTrue( + new InstantCommand(() -> swerveDrive.resetPosition(visionSubsystem.getLastSeenPose()))); // // driverLeftDpad.onTrue(new InstantCommand(() -> swerveDrive.resetOdometry(new // Pose2d(15.251774787902832, 5.573054313659668, Rotation2d.fromRadians(3.14159265))))); // // driverBButton.whileTrue(new ShootPass(swerveDrive, shooterSubsystem, pivotSubsystem, diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java index a15af59d..84445d2b 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java @@ -8,6 +8,7 @@ import frc.robot.subsystems.swerve.SwerveDrive; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionConstants; +import frc.robot.subsystems.vision.VisionConstants.Limelight; public abstract class DriveCommandBase extends Command { private final MultiLinearInterpolator oneAprilTagLookupTable = @@ -39,19 +40,19 @@ public void execute() { swerveDrive.addPoseEstimatorSwerveMeasurement(); vision.setHeadingInfo( swerveDrive.getPose().getRotation().getDegrees(), swerveDrive.getGyroRate()); - calculatePoseFromLimelight(VisionConstants.SHOOTER_LIMELIGHT_NUMBER); - calculatePoseFromLimelight(VisionConstants.FRONT_LEFT_LIMELIGHT_NUMBER); - calculatePoseFromLimelight(VisionConstants.FRONT_RIGHT_LIMELIGHT_NUMBER); + calculatePoseFromLimelight(Limelight.SHOOTER); + calculatePoseFromLimelight(Limelight.FRONT_LEFT); + calculatePoseFromLimelight(Limelight.FRONT_RIGHT); } - public void calculatePoseFromLimelight(int limelightNumber) { + public void calculatePoseFromLimelight(Limelight limelight) { double currentTimeStampSeconds = lastTimeStampSeconds; // Updates the robot's odometry with april tags - if (vision.canSeeAprilTags(limelightNumber)) { - currentTimeStampSeconds = vision.getTimeStampSeconds(limelightNumber); + if (vision.canSeeAprilTags(limelight)) { + currentTimeStampSeconds = vision.getTimeStampSeconds(limelight); - double distanceFromClosestAprilTag = vision.getLimelightAprilTagDistance(limelightNumber); + double distanceFromClosestAprilTag = vision.getLimelightAprilTagDistance(limelight); // Depending on how many april tags we see, we change our confidence as more april tags // results in a much more accurate pose estimate @@ -59,12 +60,12 @@ public void calculatePoseFromLimelight(int limelightNumber) { // so it only uses 1 april tag, if they set up the field wrong (they can set april tags +-1 // inch I believe) // using multiple *could* really mess things up. - if (vision.getNumberOfAprilTags(limelightNumber) == 1) { + if (vision.getNumberOfAprilTags(limelight) == 1) { double[] standardDeviations = oneAprilTagLookupTable.getLookupValue(distanceFromClosestAprilTag); swerveDrive.setPoseEstimatorVisionConfidence( standardDeviations[0], standardDeviations[1], standardDeviations[2]); - } else if (vision.getNumberOfAprilTags(limelightNumber) > 1) { + } else if (vision.getNumberOfAprilTags(limelight) > 1) { double[] standardDeviations = twoAprilTagLookupTable.getLookupValue(distanceFromClosestAprilTag); swerveDrive.setPoseEstimatorVisionConfidence( @@ -72,8 +73,8 @@ public void calculatePoseFromLimelight(int limelightNumber) { } swerveDrive.addPoseEstimatorVisionMeasurement( - vision.getPoseFromAprilTags(limelightNumber), - Timer.getFPGATimestamp() - vision.getLatencySeconds(limelightNumber)); + vision.getPoseFromAprilTags(limelight), + Timer.getFPGATimestamp() - vision.getLatencySeconds(limelight)); } lastTimeStampSeconds = currentTimeStampSeconds; diff --git a/src/main/java/frc/robot/extras/vision/FiducialObservation.java b/src/main/java/frc/robot/extras/vision/FiducialObservation.java new file mode 100644 index 00000000..6c5b8ad4 --- /dev/null +++ b/src/main/java/frc/robot/extras/vision/FiducialObservation.java @@ -0,0 +1,85 @@ +package frc.robot.extras.vision; + +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import java.nio.ByteBuffer; + +public class FiducialObservation implements StructSerializable { + public static class FiducialObservationStruct implements Struct { + public int id; + public double txnc; + public double tync; + public double ambiguity; + + @Override + public Class getTypeClass() { + return FiducialObservation.class; + } + + @Override + public String getTypeString() { + return "struct:FiducialObservation"; + } + + @Override + public int getSize() { + return kSizeInt32 + 3 * kSizeDouble; + } + + @Override + public String getSchema() { + return "int id;double txnc;double tync;double ambiguity"; + } + + @Override + public FiducialObservation unpack(ByteBuffer bb) { + FiducialObservation rv = new FiducialObservation(); + rv.id = bb.getInt(); + rv.txnc = bb.getDouble(); + rv.tync = bb.getDouble(); + rv.ambiguity = bb.getDouble(); + return rv; + } + + @Override + public void pack(ByteBuffer bb, FiducialObservation value) { + bb.putInt(value.id); + bb.putDouble(value.txnc); + bb.putDouble(value.tync); + bb.putDouble(value.ambiguity); + } + + @Override + public String getTypeName() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getTypeName'"); + } + } + + public int id; + public double txnc; + public double tync; + public double ambiguity; + + public FiducialObservation() {} + + public static FiducialObservation fromLimelight(LimelightHelpers.RawFiducial fiducial) { + FiducialObservation rv = new FiducialObservation(); + rv.id = fiducial.id; + rv.txnc = fiducial.txnc; + rv.tync = fiducial.tync; + rv.ambiguity = fiducial.ambiguity; + + return rv; + } + + public static FiducialObservation[] fromLimelight(LimelightHelpers.RawFiducial[] fiducials) { + FiducialObservation[] rv = new FiducialObservation[fiducials.length]; + for (int i = 0; i < fiducials.length; ++i) { + rv[i] = fromLimelight(fiducials[i]); + } + return rv; + } + + public static final FiducialObservationStruct struct = new FiducialObservationStruct(); +} diff --git a/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java b/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java new file mode 100644 index 00000000..bf84f7ca --- /dev/null +++ b/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java @@ -0,0 +1,90 @@ +package frc.robot.extras.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import java.nio.ByteBuffer; + +public class MegatagPoseEstimate implements StructSerializable { + public static class MegatagPoseEstimateStruct implements Struct { + public Pose2d fieldToCamera = Pose2d.kZero; + public double timestampSeconds; + public double latency; + public double avgTagArea; + + @Override + public Class getTypeClass() { + return MegatagPoseEstimate.class; + } + + @Override + public String getTypeString() { + return "struct:MegatagPoseEstimate"; + } + + @Override + public int getSize() { + return Pose2d.struct.getSize() + kSizeDouble * 3; + } + + @Override + public String getSchema() { + return "Pose2d fieldToCamera;double timestampSeconds;double latency;double avgTagArea"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Pose2d.struct}; + } + + @Override + public MegatagPoseEstimate unpack(ByteBuffer bb) { + MegatagPoseEstimate rv = new MegatagPoseEstimate(); + rv.fieldToCamera = Pose2d.struct.unpack(bb); + rv.timestampSeconds = bb.getDouble(); + rv.latency = bb.getDouble(); + rv.avgTagArea = bb.getDouble(); + rv.fiducialIds = new int[0]; + return rv; + } + + @Override + public void pack(ByteBuffer bb, MegatagPoseEstimate value) { + Pose2d.struct.pack(bb, value.fieldToCamera); + bb.putDouble(value.timestampSeconds); + bb.putDouble(value.latency); + bb.putDouble(value.avgTagArea); + } + + @Override + public String getTypeName() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getTypeName'"); + } + } + + public Pose2d fieldToCamera = Pose2d.kZero; + public double timestampSeconds; + public double latency; + public double avgTagArea; + public int[] fiducialIds; + + public MegatagPoseEstimate() {} + + public static MegatagPoseEstimate fromLimelight(LimelightHelpers.PoseEstimate poseEstimate) { + MegatagPoseEstimate rv = new MegatagPoseEstimate(); + rv.fieldToCamera = poseEstimate.pose; + if (rv.fieldToCamera == null) rv.fieldToCamera = Pose2d.kZero; + rv.timestampSeconds = poseEstimate.timestampSeconds; + rv.latency = poseEstimate.latency; + rv.avgTagArea = poseEstimate.avgTagArea; + rv.fiducialIds = new int[poseEstimate.rawFiducials.length]; + for (int i = 0; i < rv.fiducialIds.length; ++i) { + rv.fiducialIds[i] = poseEstimate.rawFiducials[i].id; + } + + return rv; + } + + public static final MegatagPoseEstimateStruct struct = new MegatagPoseEstimateStruct(); +} diff --git a/src/main/java/frc/robot/extras/vision/VisionFieldPoseEstimate.java b/src/main/java/frc/robot/extras/vision/VisionFieldPoseEstimate.java new file mode 100644 index 00000000..577ed3c5 --- /dev/null +++ b/src/main/java/frc/robot/extras/vision/VisionFieldPoseEstimate.java @@ -0,0 +1,34 @@ +package frc.robot.extras.vision; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +public class VisionFieldPoseEstimate { + + private final Pose2d visionRobotPoseMeters; + private final double timestampSeconds; + private final Matrix visionMeasurementStdDevs; + + public VisionFieldPoseEstimate( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + this.visionRobotPoseMeters = visionRobotPoseMeters; + this.timestampSeconds = timestampSeconds; + this.visionMeasurementStdDevs = visionMeasurementStdDevs; + } + + public Pose2d getVisionRobotPoseMeters() { + return visionRobotPoseMeters; + } + + public double getTimestampSeconds() { + return timestampSeconds; + } + + public Matrix getVisionMeasurementStdDevs() { + return visionMeasurementStdDevs; + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java index f7405590..62621520 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java @@ -19,7 +19,7 @@ public void updateInputs(GyroInputs inputs) { inputs.odometryYawPositions = gyroSimulation.getCachedGyroReadings(); inputs.odometryYawTimestamps = OdometryTimestampsSim.getTimestamps(); inputs.yawDegreesRotation2d = gyroSimulation.getGyroReading(); - inputs.yawDegrees = -gyroSimulation.getGyroReading().getDegrees(); + inputs.yawDegrees = gyroSimulation.getGyroReading().getDegrees(); inputs.yawVelocity = RadiansPerSecond.of(gyroSimulation.getMeasuredAngularVelocityRadPerSec()) .in(DegreesPerSecond); diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 23c785ac..af53922b 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -7,6 +7,7 @@ import frc.robot.Constants.FieldConstants; import frc.robot.extras.vision.LimelightHelpers; import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; +import frc.robot.extras.vision.MegatagPoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; import java.util.Map; import java.util.concurrent.ConcurrentHashMap; @@ -77,33 +78,39 @@ public void updateInputs(VisionInputs inputs) { inputs.isFrontRightLimelightConnected = isLimelightConnected(Limelight.FRONT_RIGHT); if (inputs.isShooterLimelightConnected) { - inputs.shooterMegaTag1Pose = getMegaTag1PoseEstimate(Limelight.SHOOTER).pose; - inputs.shooterMegaTag2Pose = getMegaTag2PoseEstimate(Limelight.SHOOTER).pose; + inputs.shooterMegaTag1Pose = + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.SHOOTER)); + inputs.shooterMegaTag2Pose = + MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.SHOOTER)); inputs.shooterLatency = getLatencySeconds(Limelight.SHOOTER); inputs.shooterTargets = getNumberOfAprilTags(Limelight.SHOOTER); inputs.shooterCameraToTargets = getAprilTagPositionToLimelight(Limelight.SHOOTER); inputs.shooterRobotToTargets = getAprilTagPositionToRobot(Limelight.SHOOTER); - visionThread(Limelight.SHOOTER); + // visionThread(Limelight.SHOOTER); } if (inputs.isFrontLeftLimelightConnected) { - inputs.frontLeftMegaTag1Pose = getMegaTag1PoseEstimate(Limelight.FRONT_LEFT).pose; - inputs.frontLeftMegaTag2Pose = getMegaTag2PoseEstimate(Limelight.FRONT_LEFT).pose; + inputs.frontLeftMegaTag1Pose = + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.FRONT_LEFT)); + inputs.frontLeftMegaTag2Pose = + MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.FRONT_LEFT)); inputs.frontLeftLatency = getLatencySeconds(Limelight.FRONT_LEFT); inputs.frontLeftTargets = getNumberOfAprilTags(Limelight.FRONT_LEFT); inputs.frontLeftCameraToTargets = getAprilTagPositionToLimelight(Limelight.FRONT_LEFT); inputs.frontLeftRobotToTargets = getAprilTagPositionToRobot(Limelight.FRONT_LEFT); - visionThread(Limelight.FRONT_LEFT); + // visionThread(Limelight.FRONT_LEFT); } if (inputs.isFrontRightLimelightConnected) { - inputs.frontRightMegaTag1Pose = getMegaTag1PoseEstimate(Limelight.FRONT_RIGHT).pose; - inputs.frontRightMegaTag2Pose = getMegaTag2PoseEstimate(Limelight.FRONT_RIGHT).pose; + inputs.frontRightMegaTag1Pose = + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.FRONT_RIGHT)); + inputs.frontRightMegaTag2Pose = + MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.FRONT_RIGHT)); inputs.frontRightLatency = getLatencySeconds(Limelight.FRONT_RIGHT); inputs.frontRightTargets = getNumberOfAprilTags(Limelight.FRONT_RIGHT); inputs.frontRightCameraToTargets = getAprilTagPositionToLimelight(Limelight.FRONT_RIGHT); inputs.frontRightRobotToTargets = getAprilTagPositionToRobot(Limelight.FRONT_RIGHT); - visionThread(Limelight.FRONT_RIGHT); + // visionThread(Limelight.FRONT_RIGHT); } latestInputs.set(inputs); @@ -115,7 +122,7 @@ public void updateInputs(VisionInputs inputs) { * @param limelight the number of the limelight * @return true if the limelight can fully see one or more April Tag */ - // @Override + @Override public boolean canSeeAprilTags(Limelight limelight) { // First checks if it can see an april tag, then checks if it is fully in frame // Different Limelights have different FOVs @@ -265,7 +272,7 @@ private boolean isWithinFieldBounds( * @return the pose of the robot, if the limelight can't see any April Tags, it will return 0 for * x, y, and theta */ - // @Override + @Override public Pose2d getPoseFromAprilTags(Limelight limelight) { return limelightEstimates[limelight.id].pose; } @@ -279,7 +286,7 @@ public Pose2d getAprilTagPositionToRobot(Limelight limelight) { } /** Returns how many april tags the limelight that is being used for pose estimation can see. */ - // @Override + @Override public int getNumberOfAprilTags(Limelight limelight) { return limelightEstimates[limelight.id].tagCount; } @@ -288,7 +295,7 @@ public int getNumberOfAprilTags(Limelight limelight) { * Returns the timestamp for the vision measurement from the limelight that is being used for pose * estimation. */ - // @Override + @Override public double getTimeStampSeconds(Limelight limelight) { return limelightEstimates[limelight.id].timestampSeconds / 1000.0; } @@ -298,12 +305,13 @@ public double getTimeStampSeconds(Limelight limelight) { * calculated the robot's pose. It adds the pipeline latency, capture latency, and json parsing * latency. */ - // @Override + @Override public double getLatencySeconds(Limelight limelight) { return (limelightEstimates[limelight.id].latency) / 1000.0; } /** Gets the pose calculated the last time a limelight saw an April Tag */ + @Override public Pose2d getLastSeenPose() { return lastSeenPose; } diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 17cbf216..311187dd 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -100,14 +100,15 @@ public void updateInputs(VisionInputs inputs) { NetworkTable frontRightTable = NetworkTableInstance.getDefault().getTable(super.getLimelightName(Limelight.FRONT_RIGHT)); // Write to limelight table - writeToTable(shooterCamera.getAllUnreadResults(), shooterTable); - writeToTable(frontLeftCamera.getAllUnreadResults(), frontLeftTable); - writeToTable(frontRightCamera.getAllUnreadResults(), frontRightTable); + writeToTable(shooterCamera.getAllUnreadResults(), shooterTable, Limelight.SHOOTER); + writeToTable(frontLeftCamera.getAllUnreadResults(), frontLeftTable, Limelight.FRONT_LEFT); + writeToTable(frontRightCamera.getAllUnreadResults(), frontRightTable, Limelight.FRONT_RIGHT); super.updateInputs(inputs); } - private void writeToTable(List results, NetworkTable table) { + private void writeToTable( + List results, NetworkTable table, Limelight limelight) { // write to ll table for (PhotonPipelineResult result : results) { if (result.getMultiTagResult().isPresent()) { @@ -123,7 +124,7 @@ private void writeToTable(List results, NetworkTable table 0.0, // 3: roll 0.0, // 4: pitch fieldToCamera.getRotation().getDegrees(), // 5: yaw - super.getLatencySeconds(0), // 6: latency ms, + super.getLatencySeconds(limelight), // 6: latency ms, (double) result.getMultiTagResult().get().fiducialIDsUsed.size(), // 7: tag count 0.0, // 8: tag span @@ -151,7 +152,43 @@ private void writeToTable(List results, NetworkTable table } table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); - table.getEntry("cl").setDouble(super.getLatencySeconds(0)); + table.getEntry("cl").setDouble(super.getLatencySeconds(limelight)); } } + + public String getLimelightName(Limelight limelight) { + return super.getLimelightName(limelight); + } + + public double getLatencySeconds(Limelight limelight) { + return super.getLatencySeconds(limelight); + } + + public double getTimeStampSeconds(Limelight limelight) { + return super.getTimeStampSeconds(limelight); + } + + public boolean canSeeAprilTags(Limelight limelight) { + return super.canSeeAprilTags(limelight); + } + + public double getLimelightAprilTagDistance(Limelight limelight) { + return super.getLimelightAprilTagDistance(limelight); + } + + public int getNumberOfAprilTags(Limelight limelight) { + return super.getNumberOfAprilTags(limelight); + } + + public Pose2d getPoseFromAprilTags(Limelight limelight) { + return getPoseFromAprilTags(limelight); + } + + public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { + super.setHeadingInfo(headingDegrees, headingRateDegrees); + } + + public Pose2d getLastSeenPose() { + return super.getLastSeenPose(); + } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index f1d9ef36..1b7b10f9 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,7 +1,27 @@ +// All praise 254 lib + package frc.robot.subsystems.vision; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.extras.vision.FiducialObservation; +import frc.robot.extras.vision.MegatagPoseEstimate; +import frc.robot.extras.vision.VisionFieldPoseEstimate; +import frc.robot.subsystems.vision.VisionConstants.Limelight; +import java.util.Arrays; +import java.util.HashSet; +import java.util.Optional; +import java.util.Set; +import java.util.stream.Collectors; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -19,23 +39,335 @@ public void periodic() { // Updates limelight inputs visionInterface.updateInputs(inputs); Logger.processInputs("Vision/", inputs); + + updateVision(inputs.shooterMegaTag1Pose, inputs.shooterMegaTag2Pose, inputs.shooter); + + updateVision(inputs.frontLeftMegaTag1Pose, inputs.frontLeftMegaTag2Pose, inputs.frontLeft); + + updateVision(inputs.frontRightMegaTag1Pose, inputs.frontRightMegaTag2Pose, inputs.frontRight); + } + + private void updateVision( + MegatagPoseEstimate cameraMegatagPoseEstimate, + MegatagPoseEstimate cameraMegatag2PoseEstimate, + Limelight limelightName) { + if (cameraMegatagPoseEstimate != null) { + + String logPreface = "Vision/" + visionInterface.getLatencySeconds(limelightName); + var updateTimestamp = cameraMegatagPoseEstimate.timestampSeconds; + boolean alreadyProcessedTimestamp = + visionInterface.getTimeStampSeconds(limelightName) == updateTimestamp; + if (!alreadyProcessedTimestamp && canSeeAprilTags(limelightName)) { + // if (!isTurretCamera && !Util.epsilonEquals(state.getElevatorHeight(), 0.0, 0.05)) return; + Optional pinholeEstimate = + Optional.empty(); // processPinholeVisionEstimate(pinholeObservations, + // updateTimestamp, + // isTurretCamera); + + Optional megatagEstimate = + processMegatagPoseEstimate(cameraMegatagPoseEstimate); + Optional megatag2Estimate = + processMegatag2PoseEstimate(cameraMegatag2PoseEstimate, logPreface); + + boolean used_megatag = false; + if (megatagEstimate.isPresent()) { + if (shouldUseMegatag( + cameraMegatagPoseEstimate, cameraFiducialObservations, isTurretCamera, logPreface)) { + Logger.recordOutput( + logPreface + "MegatagEstimate", megatagEstimate.get().getVisionRobotPoseMeters()); + state.updateMegatagEstimate(megatagEstimate.get()); + used_megatag = true; + } else { + if (megatagEstimate.isPresent()) { + Logger.recordOutput( + logPreface + "MegatagEstimateRejected", + megatagEstimate.get().getVisionRobotPoseMeters()); + } + } + } + + if (megatag2Estimate.isPresent() && !used_megatag) { + if (shouldUseMegatag2(cameraMegatag2PoseEstimate, logPreface)) { + Logger.recordOutput( + logPreface + "Megatag2Estimate", megatag2Estimate.get().getVisionRobotPoseMeters()); + state.updateMegatagEstimate(megatag2Estimate.get()); + } else { + if (megatagEstimate.isPresent()) { + Logger.recordOutput( + logPreface + "Megatag2EstimateRejected", + megatag2Estimate.get().getVisionRobotPoseMeters()); + } + } + } + } + } + } + + private boolean shouldUseMegatag( + MegatagPoseEstimate poseEstimate, + FiducialObservation[] fiducials, + boolean isTurretCamera, + String logPreface) { + final double kMinAreaForTurretMegatagEnabled = 0.4; + final double kMinAreaForTurretMegatagDisabled = 0.05; + + final double kMinAreaForElevatorMegatagEnabled = 0.4; + final double kMinAreaForElevatorMegatagDisabled = 0.05; + + double kMinAreaForMegatag = 0.0; + if (DriverStation.isDisabled()) { + kMinAreaForMegatag = + isTurretCamera ? kMinAreaForTurretMegatagDisabled : kMinAreaForElevatorMegatagDisabled; + } else { + kMinAreaForMegatag = + isTurretCamera ? kMinAreaForTurretMegatagEnabled : kMinAreaForElevatorMegatagEnabled; + } + + final int kExpectedTagCount = 2; + + final double kLargeYawThreshold = Units.degreesToRadians(200.0); + final double kLargeYawEventTimeWindowS = 0.05; + + if (!isTurretCamera) { + var maxYawVel = + state.getMaxAbsDriveYawAngularVelocityInRange( + poseEstimate.timestampSeconds - kLargeYawEventTimeWindowS, + poseEstimate.timestampSeconds); + if (maxYawVel.isPresent() && Math.abs(maxYawVel.get()) > kLargeYawThreshold) { + Logger.recordOutput("Vision/Elevator/MegatagYawAngular", false); + return false; + } + Logger.recordOutput("Vision/Elevator/MegatagYawAngular", true); + } + + if (poseEstimate.avgTagArea < kMinAreaForMegatag) { + Logger.recordOutput(logPreface + "megaTagAvgTagArea", false); + return false; + } + Logger.recordOutput(logPreface + "megaTagAvgTagArea", true); + + if (poseEstimate.fiducialIds.length != kExpectedTagCount) { + Logger.recordOutput(logPreface + "fiducialLength", false); + return false; + } + Logger.recordOutput(logPreface + "fiducialLength", true); + + if (poseEstimate.fiducialIds.length < 1) { + Logger.recordOutput(logPreface + "fiducialLengthLess1", false); + return false; + } + Logger.recordOutput(logPreface + "fiducialLengthLess1", true); + + if (poseEstimate.fieldToCamera.getTranslation().getNorm() < 1.0) { + Logger.recordOutput(logPreface + "NormCheck", false); + return false; + } + Logger.recordOutput(logPreface + "NormCheck", true); + + for (var fiducial : fiducials) { + if (fiducial.ambiguity > .9) { + Logger.recordOutput(logPreface + "Ambiguity", false); + return false; + } + } + Logger.recordOutput(logPreface + "Ambiguity", true); + + Set seenTags = + Arrays.stream(poseEstimate.fiducialIds) + .boxed() + .collect(Collectors.toCollection(HashSet::new)); + Set expectedTags = state.isRedAlliance() ? kTagsRedSpeaker : kTagsBlueSpeaker; + var result = expectedTags.equals(seenTags); + Logger.recordOutput(logPreface + "SeenTags", result); + return result; + } + + private boolean shouldUseMegatag2( + MegatagPoseEstimate poseEstimate, boolean isTurretCamera, String logPreface) { + return shouldUsePinhole(poseEstimate.timestampSeconds, isTurretCamera, logPreface); + } + + private Optional getFieldToRobotEstimate( + MegatagPoseEstimate poseEstimate, boolean isTurretCamera) { + var fieldToCamera = poseEstimate.fieldToCamera; + if (fieldToCamera.getX() == 0.0) { + return Optional.empty(); + } + var turretToCameraTransform = state.getTurretToCamera(isTurretCamera); + var cameraToTurretTransform = turretToCameraTransform.inverse(); + var fieldToTurretPose = fieldToCamera.plus(cameraToTurretTransform); + var fieldToRobotEstimate = MathHelpers.kPose2dZero; + if (isTurretCamera) { + var robotToTurretObservation = state.getRobotToTurret(poseEstimate.timestampSeconds); + if (robotToTurretObservation.isEmpty()) { + return Optional.empty(); + } + var turretToRobot = + MathHelpers.transform2dFromRotation(robotToTurretObservation.get().unaryMinus()); + fieldToRobotEstimate = fieldToTurretPose.plus(turretToRobot); + } else { + fieldToRobotEstimate = fieldToCamera.plus(turretToCameraTransform.inverse()); + } + + return Optional.of(fieldToRobotEstimate); + } + + private Optional processMegatag2PoseEstimate( + MegatagPoseEstimate poseEstimate, String logPreface) { + var loggedFieldToRobot = state.getFieldToRobot(poseEstimate.timestampSeconds); + if (loggedFieldToRobot.isEmpty()) { + return Optional.empty(); + } + + var maybeFieldToRobotEstimate = getFieldToRobotEstimate(poseEstimate, isTurretCamera); + if (maybeFieldToRobotEstimate.isEmpty()) return Optional.empty(); + var fieldToRobotEstimate = maybeFieldToRobotEstimate.get(); + + // distance from current pose to vision estimated pose + double poseDifference = + fieldToRobotEstimate + .getTranslation() + .getDistance(loggedFieldToRobot.get().getTranslation()); + + var defaultSet = state.isRedAlliance() ? kTagsRedSpeaker : kTagsBlueSpeaker; + Set speakerTags = new HashSet<>(defaultSet); + speakerTags.removeAll( + Arrays.stream(poseEstimate.fiducialIds) + .boxed() + .collect(Collectors.toCollection(HashSet::new))); + boolean seesSpeakerTags = speakerTags.size() < 2; + + double xyStds; + if (poseEstimate.fiducialIds.length > 0) { + // multiple targets detected + if (poseEstimate.fiducialIds.length >= 2 && poseEstimate.avgTagArea > 0.1) { + xyStds = 0.2; + } + // we detect at least one of our speaker tags and we're close to it. + else if (seesSpeakerTags && poseEstimate.avgTagArea > 0.2) { + xyStds = 0.5; + } + // 1 target with large area and close to estimated pose + else if (poseEstimate.avgTagArea > 0.8 && poseDifference < 0.5) { + xyStds = 0.5; + } + // 1 target farther away and estimated pose is close + else if (poseEstimate.avgTagArea > 0.1 && poseDifference < 0.3) { + xyStds = 1.0; + } else if (poseEstimate.fiducialIds.length > 1) { + xyStds = 1.2; + } else { + xyStds = 2.0; + } + + Logger.recordOutput(logPreface + "Megatag2StdDev", xyStds); + Logger.recordOutput(logPreface + "Megatag2AvgTagArea", poseEstimate.avgTagArea); + Logger.recordOutput(logPreface + "Megatag2PoseDifference", poseDifference); + + Matrix visionMeasurementStdDevs = + VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(50.0)); + fieldToRobotEstimate = + new Pose2d(fieldToRobotEstimate.getTranslation(), loggedFieldToRobot.get().getRotation()); + return Optional.of( + new VisionFieldPoseEstimate( + fieldToRobotEstimate, poseEstimate.timestampSeconds, visionMeasurementStdDevs)); + } + return Optional.empty(); + } + + private Optional processMegatagPoseEstimate( + MegatagPoseEstimate poseEstimate, boolean isTurretCamera) { + var loggedFieldToRobot = state.getFieldToRobot(poseEstimate.timestampSeconds); + if (loggedFieldToRobot.isEmpty()) { + return Optional.empty(); + } + + var maybeFieldToRobotEstimate = getFieldToRobotEstimate(poseEstimate, isTurretCamera); + if (maybeFieldToRobotEstimate.isEmpty()) return Optional.empty(); + var fieldToRobotEstimate = maybeFieldToRobotEstimate.get(); + + // distance from current pose to vision estimated pose + double poseDifference = + fieldToRobotEstimate + .getTranslation() + .getDistance(loggedFieldToRobot.get().getTranslation()); + + if (poseEstimate.fiducialIds.length > 0) { + double xyStds = 1.0; + double degStds = 12; + // multiple targets detected + if (poseEstimate.fiducialIds.length >= 2) { + xyStds = 0.5; + degStds = 6; + } + // 1 target with large area and close to estimated pose + else if (poseEstimate.avgTagArea > 0.8 && poseDifference < 0.5) { + xyStds = 1.0; + degStds = 12; + } + // 1 target farther away and estimated pose is close + else if (poseEstimate.avgTagArea > 0.1 && poseDifference < 0.3) { + xyStds = 2.0; + degStds = 30; + } + + Matrix visionMeasurementStdDevs = + VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds)); + return Optional.of( + new VisionFieldPoseEstimate( + fieldToRobotEstimate, poseEstimate.timestampSeconds, visionMeasurementStdDevs)); + } + return Optional.empty(); + } + + private Pose2d estimateFieldToRobot( + Translation2d cameraToTarget, + Pose3d fieldToTarget, + Rotation2d robotToTurret, + Rotation2d gyroAngle, + Rotation2d cameraYawOffset, + boolean isTurretCamera) { + Transform2d cameraToTargetFixed = + MathHelpers.transform2dFromTranslation(cameraToTarget.rotateBy(cameraYawOffset)); + Transform2d turretToTarget = state.getTurretToCamera(isTurretCamera).plus(cameraToTargetFixed); + // In robot frame + Transform2d robotToTarget = turretToTarget; + if (isTurretCamera) { + robotToTarget = MathHelpers.transform2dFromRotation(robotToTurret).plus(turretToTarget); + } + + // In field frame + Transform2d robotToTargetField = + MathHelpers.transform2dFromTranslation(robotToTarget.getTranslation().rotateBy(gyroAngle)); + + // In field frame + Pose2d fieldToTarget2d = + MathHelpers.pose2dFromTranslation(fieldToTarget.toPose2d().getTranslation()); + + Pose2d fieldToRobot = + fieldToTarget2d.transformBy( + MathHelpers.transform2dFromTranslation( + robotToTargetField.getTranslation().unaryMinus())); + + Pose2d fieldToRobotYawAdjusted = new Pose2d(fieldToRobot.getTranslation(), gyroAngle); + return fieldToRobotYawAdjusted; } // Add methods to support DriveCommand - public int getNumberOfAprilTags(int limelightNumber) { - return visionInterface.getNumberOfAprilTags(limelightNumber); + public int getNumberOfAprilTags(Limelight limelight) { + return visionInterface.getNumberOfAprilTags(limelight); } - public double getLimelightAprilTagDistance(int limelightNumber) { - return visionInterface.getLimelightAprilTagDistance(limelightNumber); + public double getLimelightAprilTagDistance(Limelight limelight) { + return visionInterface.getLimelightAprilTagDistance(limelight); } - public double getTimeStampSeconds(int limelightNumber) { - return visionInterface.getTimeStampSeconds(limelightNumber); + public double getTimeStampSeconds(Limelight limelight) { + return visionInterface.getTimeStampSeconds(limelight); } - public double getLatencySeconds(int limelightNumber) { - return visionInterface.getLatencySeconds(limelightNumber); + public double getLatencySeconds(Limelight limelight) { + return visionInterface.getLatencySeconds(limelight); } public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { @@ -43,12 +375,15 @@ public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { } @AutoLogOutput(key = "Vision/Has Targets") - public boolean canSeeAprilTags(int limelightNumber) { - return visionInterface.canSeeAprilTags( - limelightNumber); // Assuming we're checking the shooter limelight + public boolean canSeeAprilTags(Limelight limelight) { + return visionInterface.canSeeAprilTags(limelight); + } + + public Pose2d getPoseFromAprilTags(Limelight limelight) { + return visionInterface.getPoseFromAprilTags(limelight); } - public Pose2d getPoseFromAprilTags(int limelightNumber) { - return visionInterface.getPoseFromAprilTags(limelightNumber); + public Pose2d getLastSeenPose() { + return visionInterface.getLastSeenPose(); } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index a69128eb..1841a9b5 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -1,71 +1,80 @@ package frc.robot.subsystems.vision; import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.extras.vision.MegatagPoseEstimate; +import frc.robot.subsystems.vision.VisionConstants.Limelight; import org.littletonrobotics.junction.AutoLog; public interface VisionInterface { @AutoLog class VisionInputs { - public boolean isShooterLimelightConnected = false; - public boolean isFrontLeftLimelightConnected = false; - public boolean isFrontRightLimelightConnected = false; - - public Pose2d shooterMegaTag1Pose = new Pose2d(); - public double shooterTagCount = 0.0; - public Pose2d shooterMegaTag2Pose = new Pose2d(); - public double shooterLatency = 0.0; - public double shooterTargets = 0.0; - public Pose2d shooterCameraToTargets = new Pose2d(); - public Pose2d shooterRobotToTargets = new Pose2d(); - - public Pose2d frontLeftMegaTag1Pose = new Pose2d(); - public double frontLeftTagCount = 0.0; - public Pose2d frontLeftMegaTag2Pose = new Pose2d(); - public double frontLeftLatency = 0.0; - public double frontLeftTargets = 0.0; - public Pose2d frontLeftCameraToTargets = new Pose2d(); - public Pose2d frontLeftRobotToTargets = new Pose2d(); - - public Pose2d frontRightMegaTag1Pose = new Pose2d(); - public double frontRightTagCount = 0.0; - public Pose2d frontRightMegaTag2Pose = new Pose2d(); - public double frontRightLatency = 0.0; - public double frontRightTargets = 0.0; - public Pose2d frontRightCameraToTargets = new Pose2d(); - public Pose2d frontRightRobotToTargets = new Pose2d(); - - public double camerasAmount = 0.0; + public boolean isShooterLimelightConnected; + public boolean isFrontLeftLimelightConnected; + public boolean isFrontRightLimelightConnected; + + public MegatagPoseEstimate shooterMegaTag1Pose; + public double shooterTagCount; + public MegatagPoseEstimate shooterMegaTag2Pose; + public double shooterLatency; + public double shooterTargets; + public Pose2d shooterCameraToTargets; + public Pose2d shooterRobotToTargets; + public Limelight shooter; + + public MegatagPoseEstimate frontLeftMegaTag1Pose; + public double frontLeftTagCount; + public MegatagPoseEstimate frontLeftMegaTag2Pose; + public double frontLeftLatency; + public double frontLeftTargets; + public Pose2d frontLeftCameraToTargets; + public Pose2d frontLeftRobotToTargets; + public Limelight frontLeft; + + public MegatagPoseEstimate frontRightMegaTag1Pose; + public double frontRightTagCount; + public MegatagPoseEstimate frontRightMegaTag2Pose; + public double frontRightLatency; + public double frontRightTargets; + public Pose2d frontRightCameraToTargets; + public Pose2d frontRightRobotToTargets; + public Limelight frontRight; + + public double camerasAmount; } default void updateInputs(VisionInputs inputs) {} - default String getLimelightName(int limelightNumber) { + default String getLimelightName(Limelight limelight) { return ""; } - default double getLatencySeconds(int limelightNumber) { + default double getLatencySeconds(Limelight limelight) { return 0.0; } - default double getTimeStampSeconds(int limelightNumber) { + default double getTimeStampSeconds(Limelight limelight) { return 0.0; } - default boolean canSeeAprilTags(int limelightNumber) { + default boolean canSeeAprilTags(Limelight limelight) { return false; } - default double getLimelightAprilTagDistance(int limelightNumber) { + default double getLimelightAprilTagDistance(Limelight limelight) { return 0.0; } - default int getNumberOfAprilTags(int limelightNumber) { + default int getNumberOfAprilTags(Limelight limelight) { return 0; } - default Pose2d getPoseFromAprilTags(int limelightNumber) { + default Pose2d getPoseFromAprilTags(Limelight limelight) { return null; } default void setHeadingInfo(double headingDegrees, double headingRateDegrees) {} + + default Pose2d getLastSeenPose() { + return null; + } } From 9e6415db2c35b146eade21b0bfb68c65dfa2b1f0 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Tue, 19 Nov 2024 14:42:00 -0500 Subject: [PATCH 28/75] progresso --- .../subsystems/vision/PhysicalVision.java | 176 +++--- .../frc/robot/subsystems/vision/Vision.java | 526 +++++++----------- 2 files changed, 308 insertions(+), 394 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index af53922b..2c171fd7 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -9,6 +9,8 @@ import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; import frc.robot.extras.vision.MegatagPoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; +import frc.robot.subsystems.vision.VisionInterface.VisionInputs; + import java.util.Map; import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.ExecutorService; @@ -17,12 +19,15 @@ import java.util.concurrent.atomic.AtomicBoolean; import java.util.concurrent.atomic.AtomicReference; +import org.littletonrobotics.junction.Logger; + public class PhysicalVision implements VisionInterface { private Pose2d lastSeenPose = new Pose2d(); private double headingDegrees = 0; private double headingRateDegreesPerSecond = 0; - private final Map limelightThreads = new ConcurrentHashMap<>(); + // private final Map limelightThreads = new ConcurrentHashMap<>(); + private final ConcurrentHashMap> limelightThreads = new ConcurrentHashMap<>(); private final ExecutorService executorService = Executors.newFixedThreadPool(3); private final AtomicReference latestInputs = new AtomicReference<>(new VisionInputs()); @@ -36,8 +41,10 @@ public class PhysicalVision implements VisionInterface { public PhysicalVision() { limelightEstimates = new PoseEstimate[3]; for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { - limelightThreads.put(limelightNumber, new AtomicBoolean(true)); + Limelight limelight = Limelight.values()[limelightNumber]; + limelightThreads.put(limelightNumber, new AtomicReference<>(new VisionInputs())); limelightEstimates[limelightNumber] = new PoseEstimate(); + visionThread(limelightNumber); } } @@ -72,48 +79,23 @@ public PhysicalVision() { @Override public void updateInputs(VisionInputs inputs) { - inputs.camerasAmount = limelightEstimates.length; - inputs.isShooterLimelightConnected = isLimelightConnected(Limelight.SHOOTER); - inputs.isFrontLeftLimelightConnected = isLimelightConnected(Limelight.FRONT_LEFT); - inputs.isFrontRightLimelightConnected = isLimelightConnected(Limelight.FRONT_RIGHT); - - if (inputs.isShooterLimelightConnected) { - inputs.shooterMegaTag1Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.SHOOTER)); - inputs.shooterMegaTag2Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.SHOOTER)); - inputs.shooterLatency = getLatencySeconds(Limelight.SHOOTER); - inputs.shooterTargets = getNumberOfAprilTags(Limelight.SHOOTER); - inputs.shooterCameraToTargets = getAprilTagPositionToLimelight(Limelight.SHOOTER); - inputs.shooterRobotToTargets = getAprilTagPositionToRobot(Limelight.SHOOTER); - // visionThread(Limelight.SHOOTER); - } - - if (inputs.isFrontLeftLimelightConnected) { - inputs.frontLeftMegaTag1Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.FRONT_LEFT)); - inputs.frontLeftMegaTag2Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.FRONT_LEFT)); - inputs.frontLeftLatency = getLatencySeconds(Limelight.FRONT_LEFT); - inputs.frontLeftTargets = getNumberOfAprilTags(Limelight.FRONT_LEFT); - inputs.frontLeftCameraToTargets = getAprilTagPositionToLimelight(Limelight.FRONT_LEFT); - inputs.frontLeftRobotToTargets = getAprilTagPositionToRobot(Limelight.FRONT_LEFT); - // visionThread(Limelight.FRONT_LEFT); - } - - if (inputs.isFrontRightLimelightConnected) { - inputs.frontRightMegaTag1Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.FRONT_RIGHT)); - inputs.frontRightMegaTag2Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.FRONT_RIGHT)); - inputs.frontRightLatency = getLatencySeconds(Limelight.FRONT_RIGHT); - inputs.frontRightTargets = getNumberOfAprilTags(Limelight.FRONT_RIGHT); - inputs.frontRightCameraToTargets = getAprilTagPositionToLimelight(Limelight.FRONT_RIGHT); - inputs.frontRightRobotToTargets = getAprilTagPositionToRobot(Limelight.FRONT_RIGHT); - // visionThread(Limelight.FRONT_RIGHT); - } - - latestInputs.set(inputs); + limelightThreads.forEach((limelightId, inputRef) -> { + VisionInputs limelightInputs = inputRef.get(); + // Combine inputs into the main inputs object + if (limelightId == 0) { + inputs.isShooterLimelightConnected = limelightInputs.isShooterLimelightConnected; + inputs.shooterMegaTag1Pose = limelightInputs.shooterMegaTag1Pose; + inputs.shooterLatency = limelightInputs.shooterLatency; + } else if (limelightId == 1) { + inputs.isFrontLeftLimelightConnected = limelightInputs.isFrontLeftLimelightConnected; + inputs.frontLeftMegaTag1Pose = limelightInputs.frontLeftMegaTag1Pose; + inputs.frontLeftLatency = limelightInputs.frontLeftLatency; + } else if (limelightId == 2) { + inputs.isFrontRightLimelightConnected = limelightInputs.isFrontRightLimelightConnected; + inputs.frontRightMegaTag1Pose = limelightInputs.frontRightMegaTag1Pose; + inputs.frontRightLatency = limelightInputs.frontRightLatency; + } + }); } /** @@ -354,11 +336,18 @@ public String getLimelightName(Limelight limelight) { case SHOOTER -> VisionConstants.SHOOTER_LIMELIGHT_NAME; case FRONT_LEFT -> VisionConstants.FRONT_LEFT_LIMELIGHT_NAME; case FRONT_RIGHT -> VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME; - default -> - throw new IllegalArgumentException("You entered a number for a non-existent limelight"); }; } + public Limelight getLimelight(int id) { + return switch (id) { + case 0 -> Limelight.SHOOTER; + case 1 -> Limelight.FRONT_LEFT; + case 2 -> Limelight.FRONT_RIGHT; + default -> throw new IllegalArgumentException("You entered a number for a non-existent limelight"); + }; + } + public boolean isLimelightConnected(Limelight limelight) { NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(getLimelightName(limelight)); if (limelightTable.containsKey("tx")) { @@ -394,7 +383,7 @@ public void checkAndUpdatePose(Limelight limelight) { // very demanding whereas this only has to get the Network Table entries for TX and TY. if (current_TX != last_TX || current_TY != last_TY) { updatePoseEstimate(limelight); - limelightThreads.computeIfPresent(limelight.id, (key, value) -> new AtomicBoolean(true)); + limelightThreads.computeIfPresent(limelight.id, (key, value) -> new AtomicReference<>(new VisionInputs())); // This is to keep track of the last valid pose calculated by the limelights // it is used when the driver resets the robot odometry to the limelight calculated // position @@ -403,13 +392,13 @@ public void checkAndUpdatePose(Limelight limelight) { } } else { // Retrieve the AtomicBoolean for the given limelight number - AtomicBoolean isThreadRunning = - limelightThreads.getOrDefault(limelight, new AtomicBoolean()); - // Only stop the thread if it's currently running - if (isThreadRunning.get()) { + // AtomicReference isThreadRunning = + // limelightThreads.getOrDefault(limelight, new AtomicReference<>(new VisionInputs())); + // // Only stop the thread if it's currently running + // if (isThreadRunning.get()) { // stop the thread for the specified limelight stopThread(limelight); - } + // } } last_TX = current_TX; last_TY = current_TY; @@ -420,38 +409,59 @@ public void checkAndUpdatePose(Limelight limelight) { } } - /** - * Starts a separate thread dedicated to updating the pose estimate for a specified limelight. - * This approach is adopted to prevent loop overruns that would occur if we attempted to parse the - * JSON dump for each limelight sequentially within a single scheduler loop. - * - *

    To achieve efficient and safe parallel execution, an ExecutorService is utilized to manage - * the lifecycle of these threads. - * - *

    Each thread continuously runs the {@link #checkAndUpdatePose(int)} method as long as the - * corresponding limelight's thread is marked as "running". This ensures that pose estimates are - * updated in real-time, leveraging the parallel processing capabilities of the executor service. - * - * @param limelight the limelight number - */ - public void visionThread(Limelight limelight) { - - executorService.submit( - () -> { - try { - // while (limelightThreads.get(limelightNumber).get()) { - checkAndUpdatePose(limelight); - // } - } catch (Exception e) { - System.err.println( - "Error executing task for the: " - + getLimelightName(limelight) - + ": " - + e.getMessage()); - } - }); + // /** + // * Starts a separate thread dedicated to updating the pose estimate for a specified limelight. + // * This approach is adopted to prevent loop overruns that would occur if we attempted to parse the + // * JSON dump for each limelight sequentially within a single scheduler loop. + // * + // *

    To achieve efficient and safe parallel execution, an ExecutorService is utilized to manage + // * the lifecycle of these threads. + // * + // *

    Each thread continuously runs the {@link #checkAndUpdatePose(int)} method as long as the + // * corresponding limelight's thread is marked as "running". This ensures that pose estimates are + // * updated in real-time, leveraging the parallel processing capabilities of the executor service. + // * + // * @param limelight the limelight number + // */ + // public void visionThread(Limelight limelight) { + + // executorService.submit( + // () -> { + // try { + // // while (limelightThreads.get(limelightNumber).get()) { + // checkAndUpdatePose(limelight); + // // } + // } catch (Exception e) { + // System.err.println( + // "Error executing task for the: " + // + getLimelightName(limelight) + // + ": " + // + e.getMessage()); + // } + // }); + // } + public void visionThread(int limelightId) { + executorService.submit(() -> { + try { + while (!Thread.currentThread().isInterrupted()) { + // Collect data into the vision inputs + VisionInputs inputs = new VisionInputs(); + inputs.isShooterLimelightConnected = isLimelightConnected(getLimelight(limelightId)); + inputs.shooterMegaTag1Pose = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(getLimelight(limelightId))); + inputs.shooterLatency = getLatencySeconds(getLimelight(limelightId)); + + limelightThreads.get(limelightId).set(inputs); + + // Sleep to avoid overloading + Thread.sleep(20); // Example delay + } + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); // Restore interrupted status + } + }); } + /** * Sets the AtomicBoolean 'runningThreads' to false for the specified limelight. Stops the thread * for the specified limelight. @@ -463,7 +473,7 @@ public void stopThread(Limelight limelight) { // Since we can't see an April Tag, set the estimate for the specified limelight to an empty // PoseEstimate() limelightEstimates[limelight.id] = new PoseEstimate(); - limelightThreads.get(limelight.id).set(false); + // limelightThreads.get(limelight.id).set();(false); } catch (Exception e) { System.err.println( "Error stopping thread for the: " + getLimelightName(limelight) + ": " + e.getMessage()); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 1b7b10f9..bbc7adf5 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -39,319 +39,223 @@ public void periodic() { // Updates limelight inputs visionInterface.updateInputs(inputs); Logger.processInputs("Vision/", inputs); - - updateVision(inputs.shooterMegaTag1Pose, inputs.shooterMegaTag2Pose, inputs.shooter); - - updateVision(inputs.frontLeftMegaTag1Pose, inputs.frontLeftMegaTag2Pose, inputs.frontLeft); - - updateVision(inputs.frontRightMegaTag1Pose, inputs.frontRightMegaTag2Pose, inputs.frontRight); - } - - private void updateVision( - MegatagPoseEstimate cameraMegatagPoseEstimate, - MegatagPoseEstimate cameraMegatag2PoseEstimate, - Limelight limelightName) { - if (cameraMegatagPoseEstimate != null) { - - String logPreface = "Vision/" + visionInterface.getLatencySeconds(limelightName); - var updateTimestamp = cameraMegatagPoseEstimate.timestampSeconds; - boolean alreadyProcessedTimestamp = - visionInterface.getTimeStampSeconds(limelightName) == updateTimestamp; - if (!alreadyProcessedTimestamp && canSeeAprilTags(limelightName)) { - // if (!isTurretCamera && !Util.epsilonEquals(state.getElevatorHeight(), 0.0, 0.05)) return; - Optional pinholeEstimate = - Optional.empty(); // processPinholeVisionEstimate(pinholeObservations, - // updateTimestamp, - // isTurretCamera); - - Optional megatagEstimate = - processMegatagPoseEstimate(cameraMegatagPoseEstimate); - Optional megatag2Estimate = - processMegatag2PoseEstimate(cameraMegatag2PoseEstimate, logPreface); - - boolean used_megatag = false; - if (megatagEstimate.isPresent()) { - if (shouldUseMegatag( - cameraMegatagPoseEstimate, cameraFiducialObservations, isTurretCamera, logPreface)) { - Logger.recordOutput( - logPreface + "MegatagEstimate", megatagEstimate.get().getVisionRobotPoseMeters()); - state.updateMegatagEstimate(megatagEstimate.get()); - used_megatag = true; - } else { - if (megatagEstimate.isPresent()) { - Logger.recordOutput( - logPreface + "MegatagEstimateRejected", - megatagEstimate.get().getVisionRobotPoseMeters()); - } - } - } - - if (megatag2Estimate.isPresent() && !used_megatag) { - if (shouldUseMegatag2(cameraMegatag2PoseEstimate, logPreface)) { - Logger.recordOutput( - logPreface + "Megatag2Estimate", megatag2Estimate.get().getVisionRobotPoseMeters()); - state.updateMegatagEstimate(megatag2Estimate.get()); - } else { - if (megatagEstimate.isPresent()) { - Logger.recordOutput( - logPreface + "Megatag2EstimateRejected", - megatag2Estimate.get().getVisionRobotPoseMeters()); - } - } - } - } - } - } - - private boolean shouldUseMegatag( - MegatagPoseEstimate poseEstimate, - FiducialObservation[] fiducials, - boolean isTurretCamera, - String logPreface) { - final double kMinAreaForTurretMegatagEnabled = 0.4; - final double kMinAreaForTurretMegatagDisabled = 0.05; - - final double kMinAreaForElevatorMegatagEnabled = 0.4; - final double kMinAreaForElevatorMegatagDisabled = 0.05; - - double kMinAreaForMegatag = 0.0; - if (DriverStation.isDisabled()) { - kMinAreaForMegatag = - isTurretCamera ? kMinAreaForTurretMegatagDisabled : kMinAreaForElevatorMegatagDisabled; - } else { - kMinAreaForMegatag = - isTurretCamera ? kMinAreaForTurretMegatagEnabled : kMinAreaForElevatorMegatagEnabled; - } - - final int kExpectedTagCount = 2; - - final double kLargeYawThreshold = Units.degreesToRadians(200.0); - final double kLargeYawEventTimeWindowS = 0.05; - - if (!isTurretCamera) { - var maxYawVel = - state.getMaxAbsDriveYawAngularVelocityInRange( - poseEstimate.timestampSeconds - kLargeYawEventTimeWindowS, - poseEstimate.timestampSeconds); - if (maxYawVel.isPresent() && Math.abs(maxYawVel.get()) > kLargeYawThreshold) { - Logger.recordOutput("Vision/Elevator/MegatagYawAngular", false); - return false; - } - Logger.recordOutput("Vision/Elevator/MegatagYawAngular", true); - } - - if (poseEstimate.avgTagArea < kMinAreaForMegatag) { - Logger.recordOutput(logPreface + "megaTagAvgTagArea", false); - return false; - } - Logger.recordOutput(logPreface + "megaTagAvgTagArea", true); - - if (poseEstimate.fiducialIds.length != kExpectedTagCount) { - Logger.recordOutput(logPreface + "fiducialLength", false); - return false; - } - Logger.recordOutput(logPreface + "fiducialLength", true); - - if (poseEstimate.fiducialIds.length < 1) { - Logger.recordOutput(logPreface + "fiducialLengthLess1", false); - return false; - } - Logger.recordOutput(logPreface + "fiducialLengthLess1", true); - - if (poseEstimate.fieldToCamera.getTranslation().getNorm() < 1.0) { - Logger.recordOutput(logPreface + "NormCheck", false); - return false; - } - Logger.recordOutput(logPreface + "NormCheck", true); - - for (var fiducial : fiducials) { - if (fiducial.ambiguity > .9) { - Logger.recordOutput(logPreface + "Ambiguity", false); - return false; - } - } - Logger.recordOutput(logPreface + "Ambiguity", true); - - Set seenTags = - Arrays.stream(poseEstimate.fiducialIds) - .boxed() - .collect(Collectors.toCollection(HashSet::new)); - Set expectedTags = state.isRedAlliance() ? kTagsRedSpeaker : kTagsBlueSpeaker; - var result = expectedTags.equals(seenTags); - Logger.recordOutput(logPreface + "SeenTags", result); - return result; - } - - private boolean shouldUseMegatag2( - MegatagPoseEstimate poseEstimate, boolean isTurretCamera, String logPreface) { - return shouldUsePinhole(poseEstimate.timestampSeconds, isTurretCamera, logPreface); - } - - private Optional getFieldToRobotEstimate( - MegatagPoseEstimate poseEstimate, boolean isTurretCamera) { - var fieldToCamera = poseEstimate.fieldToCamera; - if (fieldToCamera.getX() == 0.0) { - return Optional.empty(); - } - var turretToCameraTransform = state.getTurretToCamera(isTurretCamera); - var cameraToTurretTransform = turretToCameraTransform.inverse(); - var fieldToTurretPose = fieldToCamera.plus(cameraToTurretTransform); - var fieldToRobotEstimate = MathHelpers.kPose2dZero; - if (isTurretCamera) { - var robotToTurretObservation = state.getRobotToTurret(poseEstimate.timestampSeconds); - if (robotToTurretObservation.isEmpty()) { - return Optional.empty(); - } - var turretToRobot = - MathHelpers.transform2dFromRotation(robotToTurretObservation.get().unaryMinus()); - fieldToRobotEstimate = fieldToTurretPose.plus(turretToRobot); - } else { - fieldToRobotEstimate = fieldToCamera.plus(turretToCameraTransform.inverse()); - } - - return Optional.of(fieldToRobotEstimate); - } - - private Optional processMegatag2PoseEstimate( - MegatagPoseEstimate poseEstimate, String logPreface) { - var loggedFieldToRobot = state.getFieldToRobot(poseEstimate.timestampSeconds); - if (loggedFieldToRobot.isEmpty()) { - return Optional.empty(); - } - - var maybeFieldToRobotEstimate = getFieldToRobotEstimate(poseEstimate, isTurretCamera); - if (maybeFieldToRobotEstimate.isEmpty()) return Optional.empty(); - var fieldToRobotEstimate = maybeFieldToRobotEstimate.get(); - - // distance from current pose to vision estimated pose - double poseDifference = - fieldToRobotEstimate - .getTranslation() - .getDistance(loggedFieldToRobot.get().getTranslation()); - - var defaultSet = state.isRedAlliance() ? kTagsRedSpeaker : kTagsBlueSpeaker; - Set speakerTags = new HashSet<>(defaultSet); - speakerTags.removeAll( - Arrays.stream(poseEstimate.fiducialIds) - .boxed() - .collect(Collectors.toCollection(HashSet::new))); - boolean seesSpeakerTags = speakerTags.size() < 2; - - double xyStds; - if (poseEstimate.fiducialIds.length > 0) { - // multiple targets detected - if (poseEstimate.fiducialIds.length >= 2 && poseEstimate.avgTagArea > 0.1) { - xyStds = 0.2; - } - // we detect at least one of our speaker tags and we're close to it. - else if (seesSpeakerTags && poseEstimate.avgTagArea > 0.2) { - xyStds = 0.5; - } - // 1 target with large area and close to estimated pose - else if (poseEstimate.avgTagArea > 0.8 && poseDifference < 0.5) { - xyStds = 0.5; - } - // 1 target farther away and estimated pose is close - else if (poseEstimate.avgTagArea > 0.1 && poseDifference < 0.3) { - xyStds = 1.0; - } else if (poseEstimate.fiducialIds.length > 1) { - xyStds = 1.2; - } else { - xyStds = 2.0; - } - - Logger.recordOutput(logPreface + "Megatag2StdDev", xyStds); - Logger.recordOutput(logPreface + "Megatag2AvgTagArea", poseEstimate.avgTagArea); - Logger.recordOutput(logPreface + "Megatag2PoseDifference", poseDifference); - - Matrix visionMeasurementStdDevs = - VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(50.0)); - fieldToRobotEstimate = - new Pose2d(fieldToRobotEstimate.getTranslation(), loggedFieldToRobot.get().getRotation()); - return Optional.of( - new VisionFieldPoseEstimate( - fieldToRobotEstimate, poseEstimate.timestampSeconds, visionMeasurementStdDevs)); - } - return Optional.empty(); - } - - private Optional processMegatagPoseEstimate( - MegatagPoseEstimate poseEstimate, boolean isTurretCamera) { - var loggedFieldToRobot = state.getFieldToRobot(poseEstimate.timestampSeconds); - if (loggedFieldToRobot.isEmpty()) { - return Optional.empty(); - } - - var maybeFieldToRobotEstimate = getFieldToRobotEstimate(poseEstimate, isTurretCamera); - if (maybeFieldToRobotEstimate.isEmpty()) return Optional.empty(); - var fieldToRobotEstimate = maybeFieldToRobotEstimate.get(); - - // distance from current pose to vision estimated pose - double poseDifference = - fieldToRobotEstimate - .getTranslation() - .getDistance(loggedFieldToRobot.get().getTranslation()); - - if (poseEstimate.fiducialIds.length > 0) { - double xyStds = 1.0; - double degStds = 12; - // multiple targets detected - if (poseEstimate.fiducialIds.length >= 2) { - xyStds = 0.5; - degStds = 6; - } - // 1 target with large area and close to estimated pose - else if (poseEstimate.avgTagArea > 0.8 && poseDifference < 0.5) { - xyStds = 1.0; - degStds = 12; - } - // 1 target farther away and estimated pose is close - else if (poseEstimate.avgTagArea > 0.1 && poseDifference < 0.3) { - xyStds = 2.0; - degStds = 30; - } - - Matrix visionMeasurementStdDevs = - VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds)); - return Optional.of( - new VisionFieldPoseEstimate( - fieldToRobotEstimate, poseEstimate.timestampSeconds, visionMeasurementStdDevs)); - } - return Optional.empty(); } - private Pose2d estimateFieldToRobot( - Translation2d cameraToTarget, - Pose3d fieldToTarget, - Rotation2d robotToTurret, - Rotation2d gyroAngle, - Rotation2d cameraYawOffset, - boolean isTurretCamera) { - Transform2d cameraToTargetFixed = - MathHelpers.transform2dFromTranslation(cameraToTarget.rotateBy(cameraYawOffset)); - Transform2d turretToTarget = state.getTurretToCamera(isTurretCamera).plus(cameraToTargetFixed); - // In robot frame - Transform2d robotToTarget = turretToTarget; - if (isTurretCamera) { - robotToTarget = MathHelpers.transform2dFromRotation(robotToTurret).plus(turretToTarget); - } - - // In field frame - Transform2d robotToTargetField = - MathHelpers.transform2dFromTranslation(robotToTarget.getTranslation().rotateBy(gyroAngle)); - - // In field frame - Pose2d fieldToTarget2d = - MathHelpers.pose2dFromTranslation(fieldToTarget.toPose2d().getTranslation()); - - Pose2d fieldToRobot = - fieldToTarget2d.transformBy( - MathHelpers.transform2dFromTranslation( - robotToTargetField.getTranslation().unaryMinus())); - - Pose2d fieldToRobotYawAdjusted = new Pose2d(fieldToRobot.getTranslation(), gyroAngle); - return fieldToRobotYawAdjusted; - } + // private void updateVision( + // MegatagPoseEstimate cameraMegatagPoseEstimate, + // MegatagPoseEstimate cameraMegatag2PoseEstimate, + // Limelight limelightName) { + // if (cameraMegatagPoseEstimate != null) { + + // String logPreface = "Vision/" + visionInterface.getLimelightName(limelightName); + // var updateTimestamp = cameraMegatagPoseEstimate.timestampSeconds; + // boolean alreadyProcessedTimestamp = + // visionInterface.getTimeStampSeconds(limelightName) == updateTimestamp; + // if (!alreadyProcessedTimestamp && canSeeAprilTags(limelightName)) { + + // Optional megatagEstimate = + // processMegatagPoseEstimate(cameraMegatagPoseEstimate); + // Optional megatag2Estimate = + // processMegatag2PoseEstimate(cameraMegatag2PoseEstimate, logPreface); + + // boolean used_megatag = false; + // if (megatagEstimate.isPresent()) { + // if (shouldUseMegatag( + // cameraMegatagPoseEstimate, cameraFiducialObservations, isTurretCamera, logPreface)) { + // Logger.recordOutput( + // logPreface + "MegatagEstimate", megatagEstimate.get().getVisionRobotPoseMeters()); + // state.updateMegatagEstimate(megatagEstimate.get()); + // used_megatag = true; + // } else { + // if (megatagEstimate.isPresent()) { + // Logger.recordOutput( + // logPreface + "MegatagEstimateRejected", + // megatagEstimate.get().getVisionRobotPoseMeters()); + // } + // } + // } + + // if (megatag2Estimate.isPresent() && !used_megatag) { + // if (shouldUseMegatag2(cameraMegatag2PoseEstimate, logPreface)) { + // Logger.recordOutput( + // logPreface + "Megatag2Estimate", megatag2Estimate.get().getVisionRobotPoseMeters()); + // state.updateMegatagEstimate(megatag2Estimate.get()); + // } else { + // if (megatagEstimate.isPresent()) { + // Logger.recordOutput( + // logPreface + "Megatag2EstimateRejected", + // megatag2Estimate.get().getVisionRobotPoseMeters()); + // } + // } + // } + // } + // } + // } + + // private Optional getFieldToRobotEstimate( + // MegatagPoseEstimate poseEstimate, boolean isTurretCamera) { + // var fieldToCamera = poseEstimate.fieldToCamera; + // if (fieldToCamera.getX() == 0.0) { + // return Optional.empty(); + // } + // var cameraToTurretTransform = turretToCameraTransform.inverse(); + // var fieldToTurretPose = fieldToCamera.plus(cameraToTurretTransform); + // var fieldToRobotEstimate = Pose2d.kZero; + // if (isTurretCamera) { + // var robotToTurretObservation = state.getRobotToTurret(poseEstimate.timestampSeconds); + // if (robotToTurretObservation.isEmpty()) { + // return Optional.empty(); + // } + // var turretToRobot = + // MathHelpers.transform2dFromRotation(robotToTurretObservation.get().unaryMinus()); + // fieldToRobotEstimate = fieldToTurretPose.plus(turretToRobot); + // } else { + // fieldToRobotEstimate = fieldToCamera.plus(turretToCameraTransform.inverse()); + // } + + // return Optional.of(fieldToRobotEstimate); + // } + + // private Optional processMegatag2PoseEstimate( + // MegatagPoseEstimate poseEstimate, String logPreface) { + // var loggedFieldToRobot = state.getFieldToRobot(poseEstimate.timestampSeconds); + // if (loggedFieldToRobot.isEmpty()) { + // return Optional.empty(); + // } + + // var maybeFieldToRobotEstimate = getFieldToRobotEstimate(poseEstimate, isTurretCamera); + // if (maybeFieldToRobotEstimate.isEmpty()) return Optional.empty(); + // var fieldToRobotEstimate = maybeFieldToRobotEstimate.get(); + + // // distance from current pose to vision estimated pose + // double poseDifference = + // fieldToRobotEstimate + // .getTranslation() + // .getDistance(loggedFieldToRobot.get().getTranslation()); + + // var defaultSet = state.isRedAlliance() ? kTagsRedSpeaker : kTagsBlueSpeaker; + // Set speakerTags = new HashSet<>(defaultSet); + // speakerTags.removeAll( + // Arrays.stream(poseEstimate.fiducialIds) + // .boxed() + // .collect(Collectors.toCollection(HashSet::new))); + // boolean seesSpeakerTags = speakerTags.size() < 2; + + // double xyStds; + // if (poseEstimate.fiducialIds.length > 0) { + // // multiple targets detected + // if (poseEstimate.fiducialIds.length >= 2 && poseEstimate.avgTagArea > 0.1) { + // xyStds = 0.2; + // } + // // we detect at least one of our speaker tags and we're close to it. + // else if (seesSpeakerTags && poseEstimate.avgTagArea > 0.2) { + // xyStds = 0.5; + // } + // // 1 target with large area and close to estimated pose + // else if (poseEstimate.avgTagArea > 0.8 && poseDifference < 0.5) { + // xyStds = 0.5; + // } + // // 1 target farther away and estimated pose is close + // else if (poseEstimate.avgTagArea > 0.1 && poseDifference < 0.3) { + // xyStds = 1.0; + // } else if (poseEstimate.fiducialIds.length > 1) { + // xyStds = 1.2; + // } else { + // xyStds = 2.0; + // } + + // Logger.recordOutput(logPreface + "Megatag2StdDev", xyStds); + // Logger.recordOutput(logPreface + "Megatag2AvgTagArea", poseEstimate.avgTagArea); + // Logger.recordOutput(logPreface + "Megatag2PoseDifference", poseDifference); + + // Matrix visionMeasurementStdDevs = + // VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(50.0)); + // fieldToRobotEstimate = + // new Pose2d(fieldToRobotEstimate.getTranslation(), loggedFieldToRobot.get().getRotation()); + // return Optional.of( + // new VisionFieldPoseEstimate( + // fieldToRobotEstimate, poseEstimate.timestampSeconds, visionMeasurementStdDevs)); + // } + // return Optional.empty(); + // } + + // private Optional processMegatagPoseEstimate( + // MegatagPoseEstimate poseEstimate, boolean isTurretCamera) { + // var loggedFieldToRobot = state.getFieldToRobot(poseEstimate.timestampSeconds); + // if (loggedFieldToRobot.isEmpty()) { + // return Optional.empty(); + // } + + // var maybeFieldToRobotEstimate = getFieldToRobotEstimate(poseEstimate, isTurretCamera); + // if (maybeFieldToRobotEstimate.isEmpty()) return Optional.empty(); + // var fieldToRobotEstimate = maybeFieldToRobotEstimate.get(); + + // // distance from current pose to vision estimated pose + // double poseDifference = + // fieldToRobotEstimate + // .getTranslation() + // .getDistance(loggedFieldToRobot.get().getTranslation()); + + // if (poseEstimate.fiducialIds.length > 0) { + // double xyStds = 1.0; + // double degStds = 12; + // // multiple targets detected + // if (poseEstimate.fiducialIds.length >= 2) { + // xyStds = 0.5; + // degStds = 6; + // } + // // 1 target with large area and close to estimated pose + // else if (poseEstimate.avgTagArea > 0.8 && poseDifference < 0.5) { + // xyStds = 1.0; + // degStds = 12; + // } + // // 1 target farther away and estimated pose is close + // else if (poseEstimate.avgTagArea > 0.1 && poseDifference < 0.3) { + // xyStds = 2.0; + // degStds = 30; + // } + + // Matrix visionMeasurementStdDevs = + // VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds)); + // return Optional.of( + // new VisionFieldPoseEstimate( + // fieldToRobotEstimate, poseEstimate.timestampSeconds, visionMeasurementStdDevs)); + // } + // return Optional.empty(); + // } + + // private Pose2d estimateFieldToRobot( + // Translation2d cameraToTarget, + // Pose3d fieldToTarget, + // Rotation2d robotToTurret, + // Rotation2d gyroAngle, + // Rotation2d cameraYawOffset, + // boolean isTurretCamera) { + // Transform2d cameraToTargetFixed = + // MathHelpers.transform2dFromTranslation(cameraToTarget.rotateBy(cameraYawOffset)); + // Transform2d turretToTarget = state.getTurretToCamera(isTurretCamera).plus(cameraToTargetFixed); + // // In robot frame + // Transform2d robotToTarget = turretToTarget; + // if (isTurretCamera) { + // robotToTarget = MathHelpers.transform2dFromRotation(robotToTurret).plus(turretToTarget); + // } + + // // In field frame + // Transform2d robotToTargetField = + // MathHelpers.transform2dFromTranslation(robotToTarget.getTranslation().rotateBy(gyroAngle)); + + // // In field frame + // Pose2d fieldToTarget2d = + // MathHelpers.pose2dFromTranslation(fieldToTarget.toPose2d().getTranslation()); + + // Pose2d fieldToRobot = + // fieldToTarget2d.transformBy( + // MathHelpers.transform2dFromTranslation( + // robotToTargetField.getTranslation().unaryMinus())); + + // Pose2d fieldToRobotYawAdjusted = new Pose2d(fieldToRobot.getTranslation(), gyroAngle); + // return fieldToRobotYawAdjusted; + // } // Add methods to support DriveCommand public int getNumberOfAprilTags(Limelight limelight) { From 666951468729a699c825906a6eb4526b56911c42 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Tue, 19 Nov 2024 17:14:52 -0500 Subject: [PATCH 29/75] more progressisssismo --- .../subsystems/vision/PhysicalVision.java | 31 +--------------- .../subsystems/vision/SimulatedVision.java | 37 ++++++++++--------- 2 files changed, 20 insertions(+), 48 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 2c171fd7..23cbfce0 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -47,36 +47,7 @@ public PhysicalVision() { visionThread(limelightNumber); } } - - // private void setLLSettings() { - // double[] camerapose = { 0.0, 0.0, Constants.kCameraHeightOffGroundMeters, 0.0, - // Constants.kCameraPitchDegrees, - // 0.0 }; - // table.getEntry("camerapose_robotspace_set").setDoubleArray(camerapose); - - // double[] cameraBpose = { 0.0, 0.0, Constants.kCameraBHeightOffGroundMeters, - // Constants.kCameraBRollDegrees, - // Constants.kCameraBPitchDegrees, 0.0 }; - // tableB.getEntry("camerapose_robotspace_set").setDoubleArray(cameraBpose); - - // var fieldToTurretRotation = robotState.getLatestFieldToRobot().getValue().getRotation() - // .rotateBy(robotState.getLatestRobotToTurret().getValue()); - // var fieldToTurretVelocity = - // Units.radiansToDegrees(robotState.getLatestTurretAngularVelocity() - // + robotState.getLatestRobotRelativeChassisSpeed().omegaRadiansPerSecond); - // LimelightHelpers.SetRobotOrientation(Constants.kLimelightTableName, - // fieldToTurretRotation.getDegrees(), - // fieldToTurretVelocity, 0, 0, 0, 0); - - // var gyroAngle = robotState.getLatestFieldToRobot().getValue().getRotation(); - // var gyroAngularVelocity = Units - // - // .radiansToDegrees(robotState.getLatestRobotRelativeChassisSpeed().omegaRadiansPerSecond); - // LimelightHelpers.SetRobotOrientation(Constants.kLimelightBTableName, - // gyroAngle.getDegrees(), - // gyroAngularVelocity, 0, 0, 0, 0); - // } - + @Override public void updateInputs(VisionInputs inputs) { limelightThreads.forEach((limelightId, inputRef) -> { diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 311187dd..6adb3a5e 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -43,30 +43,31 @@ public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { // Create simulated camera properties. These can be set to mimic your actual // camera. - var turretProp = new SimCameraProperties(); - turretProp.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7)); - turretProp.setCalibError(0.35, 0.10); - turretProp.setFPS(15); - turretProp.setAvgLatencyMs(20); - turretProp.setLatencyStdDevMs(5); - - var elevatorProp = new SimCameraProperties(); - elevatorProp = turretProp.copy(); + var cameraProperties = new SimCameraProperties(); + cameraProperties.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7)); + cameraProperties.setCalibError(0.35, 0.10); + cameraProperties.setFPS(15); + cameraProperties.setAvgLatencyMs(20); + cameraProperties.setLatencyStdDevMs(5); // Create a PhotonCameraSim which will update the linked PhotonCamera's values // with visible // targets. // Instance variables - shooterCameraSim = new PhotonCameraSim(shooterCamera, turretProp); - frontLeftCameraSim = new PhotonCameraSim(frontLeftCamera, elevatorProp); - frontRightCameraSim = new PhotonCameraSim(frontRightCamera, elevatorProp); + shooterCameraSim = new PhotonCameraSim(shooterCamera, cameraProperties); + frontLeftCameraSim = new PhotonCameraSim(frontLeftCamera, cameraProperties); + frontRightCameraSim = new PhotonCameraSim(frontRightCamera, cameraProperties); - // Add the simulated camera to view the targets on this simulated field. - visionSim.addCamera(shooterCameraSim, new Transform3d()); + Transform3d robotToShooterCamera = + new Transform3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, 0.0, 0.0)); + visionSim.addCamera(shooterCameraSim, robotToShooterCamera); - Transform3d robotToElevatorCamera = + Transform3d robotToFrontLeftCamera = + new Transform3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, 0.0, 0.0)); + visionSim.addCamera(frontLeftCameraSim, robotToFrontLeftCamera); + Transform3d robotToFrontRightCamera = new Transform3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, 0.0, 0.0)); - visionSim.addCamera(frontLeftCameraSim, robotToElevatorCamera); + visionSim.addCamera(frontRightCameraSim, robotToFrontRightCamera); // Enable the raw and processed streams. (http://localhost:1181 / 1182) shooterCameraSim.enableRawStream(true); @@ -124,7 +125,7 @@ private void writeToTable( 0.0, // 3: roll 0.0, // 4: pitch fieldToCamera.getRotation().getDegrees(), // 5: yaw - super.getLatencySeconds(limelight), // 6: latency ms, + result.metadata.getLatencyMillis(), // 6: latency ms, (double) result.getMultiTagResult().get().fiducialIDsUsed.size(), // 7: tag count 0.0, // 8: tag span @@ -152,7 +153,7 @@ private void writeToTable( } table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); - table.getEntry("cl").setDouble(super.getLatencySeconds(limelight)); + table.getEntry("cl").setDouble(result.metadata.getLatencyMillis()); } } From d8df64a3b0bb6c7ef8be7e3cdb074ae9271e614d Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Tue, 19 Nov 2024 17:21:49 -0500 Subject: [PATCH 30/75] huh --- .../subsystems/vision/PhysicalVision.java | 112 +++++++++--------- .../frc/robot/subsystems/vision/Vision.java | 35 ++---- 2 files changed, 71 insertions(+), 76 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 23cbfce0..e9af7fd0 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -10,24 +10,20 @@ import frc.robot.extras.vision.MegatagPoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; import frc.robot.subsystems.vision.VisionInterface.VisionInputs; - -import java.util.Map; import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.ExecutorService; import java.util.concurrent.Executors; import java.util.concurrent.TimeUnit; -import java.util.concurrent.atomic.AtomicBoolean; import java.util.concurrent.atomic.AtomicReference; -import org.littletonrobotics.junction.Logger; - public class PhysicalVision implements VisionInterface { private Pose2d lastSeenPose = new Pose2d(); private double headingDegrees = 0; private double headingRateDegreesPerSecond = 0; // private final Map limelightThreads = new ConcurrentHashMap<>(); - private final ConcurrentHashMap> limelightThreads = new ConcurrentHashMap<>(); + private final ConcurrentHashMap> limelightThreads = + new ConcurrentHashMap<>(); private final ExecutorService executorService = Executors.newFixedThreadPool(3); private final AtomicReference latestInputs = new AtomicReference<>(new VisionInputs()); @@ -47,26 +43,27 @@ public PhysicalVision() { visionThread(limelightNumber); } } - + @Override public void updateInputs(VisionInputs inputs) { - limelightThreads.forEach((limelightId, inputRef) -> { - VisionInputs limelightInputs = inputRef.get(); - // Combine inputs into the main inputs object - if (limelightId == 0) { - inputs.isShooterLimelightConnected = limelightInputs.isShooterLimelightConnected; - inputs.shooterMegaTag1Pose = limelightInputs.shooterMegaTag1Pose; - inputs.shooterLatency = limelightInputs.shooterLatency; - } else if (limelightId == 1) { - inputs.isFrontLeftLimelightConnected = limelightInputs.isFrontLeftLimelightConnected; - inputs.frontLeftMegaTag1Pose = limelightInputs.frontLeftMegaTag1Pose; - inputs.frontLeftLatency = limelightInputs.frontLeftLatency; - } else if (limelightId == 2) { - inputs.isFrontRightLimelightConnected = limelightInputs.isFrontRightLimelightConnected; - inputs.frontRightMegaTag1Pose = limelightInputs.frontRightMegaTag1Pose; - inputs.frontRightLatency = limelightInputs.frontRightLatency; - } - }); + limelightThreads.forEach( + (limelightId, inputRef) -> { + VisionInputs limelightInputs = inputRef.get(); + // Combine inputs into the main inputs object + if (limelightId == 0) { + inputs.isShooterLimelightConnected = limelightInputs.isShooterLimelightConnected; + inputs.shooterMegaTag1Pose = limelightInputs.shooterMegaTag1Pose; + inputs.shooterLatency = limelightInputs.shooterLatency; + } else if (limelightId == 1) { + inputs.isFrontLeftLimelightConnected = limelightInputs.isFrontLeftLimelightConnected; + inputs.frontLeftMegaTag1Pose = limelightInputs.frontLeftMegaTag1Pose; + inputs.frontLeftLatency = limelightInputs.frontLeftLatency; + } else if (limelightId == 2) { + inputs.isFrontRightLimelightConnected = limelightInputs.isFrontRightLimelightConnected; + inputs.frontRightMegaTag1Pose = limelightInputs.frontRightMegaTag1Pose; + inputs.frontRightLatency = limelightInputs.frontRightLatency; + } + }); } /** @@ -315,9 +312,10 @@ public Limelight getLimelight(int id) { case 0 -> Limelight.SHOOTER; case 1 -> Limelight.FRONT_LEFT; case 2 -> Limelight.FRONT_RIGHT; - default -> throw new IllegalArgumentException("You entered a number for a non-existent limelight"); + default -> + throw new IllegalArgumentException("You entered a number for a non-existent limelight"); }; - } + } public boolean isLimelightConnected(Limelight limelight) { NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(getLimelightName(limelight)); @@ -354,7 +352,8 @@ public void checkAndUpdatePose(Limelight limelight) { // very demanding whereas this only has to get the Network Table entries for TX and TY. if (current_TX != last_TX || current_TY != last_TY) { updatePoseEstimate(limelight); - limelightThreads.computeIfPresent(limelight.id, (key, value) -> new AtomicReference<>(new VisionInputs())); + limelightThreads.computeIfPresent( + limelight.id, (key, value) -> new AtomicReference<>(new VisionInputs())); // This is to keep track of the last valid pose calculated by the limelights // it is used when the driver resets the robot odometry to the limelight calculated // position @@ -364,11 +363,12 @@ public void checkAndUpdatePose(Limelight limelight) { } else { // Retrieve the AtomicBoolean for the given limelight number // AtomicReference isThreadRunning = - // limelightThreads.getOrDefault(limelight, new AtomicReference<>(new VisionInputs())); + // limelightThreads.getOrDefault(limelight, new AtomicReference<>(new + // VisionInputs())); // // Only stop the thread if it's currently running // if (isThreadRunning.get()) { - // stop the thread for the specified limelight - stopThread(limelight); + // stop the thread for the specified limelight + stopThread(limelight); // } } last_TX = current_TX; @@ -382,15 +382,19 @@ public void checkAndUpdatePose(Limelight limelight) { // /** // * Starts a separate thread dedicated to updating the pose estimate for a specified limelight. - // * This approach is adopted to prevent loop overruns that would occur if we attempted to parse the + // * This approach is adopted to prevent loop overruns that would occur if we attempted to parse + // the // * JSON dump for each limelight sequentially within a single scheduler loop. // * - // *

    To achieve efficient and safe parallel execution, an ExecutorService is utilized to manage + // *

    To achieve efficient and safe parallel execution, an ExecutorService is utilized to + // manage // * the lifecycle of these threads. // * // *

    Each thread continuously runs the {@link #checkAndUpdatePose(int)} method as long as the - // * corresponding limelight's thread is marked as "running". This ensures that pose estimates are - // * updated in real-time, leveraging the parallel processing capabilities of the executor service. + // * corresponding limelight's thread is marked as "running". This ensures that pose estimates + // are + // * updated in real-time, leveraging the parallel processing capabilities of the executor + // service. // * // * @param limelight the limelight number // */ @@ -412,27 +416,29 @@ public void checkAndUpdatePose(Limelight limelight) { // }); // } public void visionThread(int limelightId) { - executorService.submit(() -> { - try { - while (!Thread.currentThread().isInterrupted()) { - // Collect data into the vision inputs - VisionInputs inputs = new VisionInputs(); - inputs.isShooterLimelightConnected = isLimelightConnected(getLimelight(limelightId)); - inputs.shooterMegaTag1Pose = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(getLimelight(limelightId))); - inputs.shooterLatency = getLatencySeconds(getLimelight(limelightId)); - - limelightThreads.get(limelightId).set(inputs); - - // Sleep to avoid overloading - Thread.sleep(20); // Example delay - } - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); // Restore interrupted status - } - }); + executorService.submit( + () -> { + try { + while (!Thread.currentThread().isInterrupted()) { + // Collect data into the vision inputs + VisionInputs inputs = new VisionInputs(); + inputs.isShooterLimelightConnected = isLimelightConnected(getLimelight(limelightId)); + inputs.shooterMegaTag1Pose = + MegatagPoseEstimate.fromLimelight( + getMegaTag1PoseEstimate(getLimelight(limelightId))); + inputs.shooterLatency = getLatencySeconds(getLimelight(limelightId)); + + limelightThreads.get(limelightId).set(inputs); + + // Sleep to avoid overloading + Thread.sleep(20); // Example delay + } + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); // Restore interrupted status + } + }); } - /** * Sets the AtomicBoolean 'runningThreads' to false for the specified limelight. Stops the thread * for the specified limelight. diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index bbc7adf5..53a0a4e9 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -2,26 +2,9 @@ package frc.robot.subsystems.vision; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.extras.vision.FiducialObservation; -import frc.robot.extras.vision.MegatagPoseEstimate; -import frc.robot.extras.vision.VisionFieldPoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; -import java.util.Arrays; -import java.util.HashSet; -import java.util.Optional; -import java.util.Set; -import java.util.stream.Collectors; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -61,9 +44,11 @@ public void periodic() { // boolean used_megatag = false; // if (megatagEstimate.isPresent()) { // if (shouldUseMegatag( - // cameraMegatagPoseEstimate, cameraFiducialObservations, isTurretCamera, logPreface)) { + // cameraMegatagPoseEstimate, cameraFiducialObservations, isTurretCamera, logPreface)) + // { // Logger.recordOutput( - // logPreface + "MegatagEstimate", megatagEstimate.get().getVisionRobotPoseMeters()); + // logPreface + "MegatagEstimate", + // megatagEstimate.get().getVisionRobotPoseMeters()); // state.updateMegatagEstimate(megatagEstimate.get()); // used_megatag = true; // } else { @@ -78,7 +63,8 @@ public void periodic() { // if (megatag2Estimate.isPresent() && !used_megatag) { // if (shouldUseMegatag2(cameraMegatag2PoseEstimate, logPreface)) { // Logger.recordOutput( - // logPreface + "Megatag2Estimate", megatag2Estimate.get().getVisionRobotPoseMeters()); + // logPreface + "Megatag2Estimate", + // megatag2Estimate.get().getVisionRobotPoseMeters()); // state.updateMegatagEstimate(megatag2Estimate.get()); // } else { // if (megatagEstimate.isPresent()) { @@ -171,7 +157,8 @@ public void periodic() { // Matrix visionMeasurementStdDevs = // VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(50.0)); // fieldToRobotEstimate = - // new Pose2d(fieldToRobotEstimate.getTranslation(), loggedFieldToRobot.get().getRotation()); + // new Pose2d(fieldToRobotEstimate.getTranslation(), + // loggedFieldToRobot.get().getRotation()); // return Optional.of( // new VisionFieldPoseEstimate( // fieldToRobotEstimate, poseEstimate.timestampSeconds, visionMeasurementStdDevs)); @@ -233,7 +220,8 @@ public void periodic() { // boolean isTurretCamera) { // Transform2d cameraToTargetFixed = // MathHelpers.transform2dFromTranslation(cameraToTarget.rotateBy(cameraYawOffset)); - // Transform2d turretToTarget = state.getTurretToCamera(isTurretCamera).plus(cameraToTargetFixed); + // Transform2d turretToTarget = + // state.getTurretToCamera(isTurretCamera).plus(cameraToTargetFixed); // // In robot frame // Transform2d robotToTarget = turretToTarget; // if (isTurretCamera) { @@ -242,7 +230,8 @@ public void periodic() { // // In field frame // Transform2d robotToTargetField = - // MathHelpers.transform2dFromTranslation(robotToTarget.getTranslation().rotateBy(gyroAngle)); + // + // MathHelpers.transform2dFromTranslation(robotToTarget.getTranslation().rotateBy(gyroAngle)); // // In field frame // Pose2d fieldToTarget2d = From 034d51309853172806fa9bfdf837a2010a6f6df6 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Wed, 20 Nov 2024 11:39:22 -0500 Subject: [PATCH 31/75] i am mucho sleepy --- src/main/java/frc/robot/RobotContainer.java | 12 ++++++------ .../frc/robot/subsystems/swerve/SwerveDrive.java | 6 +++--- .../frc/robot/subsystems/swerve/SwerveModule.java | 5 +++-- .../swerve/{gyroIO => gyro}/GyroInterface.java | 2 +- .../swerve/{gyroIO => gyro}/PhysicalGyro.java | 2 +- .../swerve/{gyroIO => gyro}/SimulatedGyro.java | 2 +- .../swerve/{moduleIO => module}/ModuleInterface.java | 2 +- .../swerve/{moduleIO => module}/PhysicalModule.java | 2 +- .../swerve/{moduleIO => module}/SimulatedModule.java | 2 +- 9 files changed, 18 insertions(+), 17 deletions(-) rename src/main/java/frc/robot/subsystems/swerve/{gyroIO => gyro}/GyroInterface.java (93%) rename src/main/java/frc/robot/subsystems/swerve/{gyroIO => gyro}/PhysicalGyro.java (96%) rename src/main/java/frc/robot/subsystems/swerve/{gyroIO => gyro}/SimulatedGyro.java (95%) rename src/main/java/frc/robot/subsystems/swerve/{moduleIO => module}/ModuleInterface.java (96%) rename src/main/java/frc/robot/subsystems/swerve/{moduleIO => module}/PhysicalModule.java (99%) rename src/main/java/frc/robot/subsystems/swerve/{moduleIO => module}/SimulatedModule.java (98%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bf75d60e..6bd8c021 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,13 +18,13 @@ import frc.robot.subsystems.swerve.SwerveConstants; import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; +import frc.robot.subsystems.swerve.gyro.GyroInterface; +import frc.robot.subsystems.swerve.gyro.PhysicalGyro; +import frc.robot.subsystems.swerve.gyro.SimulatedGyro; import frc.robot.subsystems.swerve.SwerveDrive; -import frc.robot.subsystems.swerve.gyroIO.GyroInterface; -import frc.robot.subsystems.swerve.gyroIO.PhysicalGyro; -import frc.robot.subsystems.swerve.gyroIO.SimulatedGyro; -import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; -import frc.robot.subsystems.swerve.moduleIO.PhysicalModule; -import frc.robot.subsystems.swerve.moduleIO.SimulatedModule; +import frc.robot.subsystems.swerve.module.ModuleInterface; +import frc.robot.subsystems.swerve.module.PhysicalModule; +import frc.robot.subsystems.swerve.module.SimulatedModule; import frc.robot.subsystems.vision.PhysicalVision; import frc.robot.subsystems.vision.SimulatedVision; import frc.robot.subsystems.vision.Vision; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index ada715de..9f50b57e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -19,9 +19,9 @@ import frc.robot.extras.util.DeviceCANBus; import frc.robot.extras.util.TimeUtil; import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; -import frc.robot.subsystems.swerve.gyroIO.GyroInputsAutoLogged; -import frc.robot.subsystems.swerve.gyroIO.GyroInterface; -import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; +import frc.robot.subsystems.swerve.gyro.GyroInterface; +import frc.robot.subsystems.swerve.gyro.GyroInputsAutoLogged; +import frc.robot.subsystems.swerve.module.ModuleInterface; import frc.robot.subsystems.swerve.odometryThread.OdometryThread; import frc.robot.subsystems.swerve.odometryThread.OdometryThreadInputsAutoLogged; import frc.robot.subsystems.vision.VisionConstants; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index c71ec301..ebd28af0 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -10,8 +10,9 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; -import frc.robot.subsystems.swerve.moduleIO.ModuleInputsAutoLogged; -import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; +import frc.robot.subsystems.swerve.module.ModuleInterface; +import frc.robot.subsystems.swerve.module.ModuleInputsAutoLogged; + import org.littletonrobotics.junction.Logger; public class SwerveModule extends SubsystemBase { diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java similarity index 93% rename from src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java rename to src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java index b2fa5669..0b2a8eb6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.swerve.gyroIO; +package frc.robot.subsystems.swerve.gyro; import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java similarity index 96% rename from src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java rename to src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java index 79c44d11..3de37cd7 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.swerve.gyroIO; +package frc.robot.subsystems.swerve.gyro; import static edu.wpi.first.units.Units.*; diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java similarity index 95% rename from src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java rename to src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java index 62621520..9b762c0b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.swerve.gyroIO; +package frc.robot.subsystems.swerve.gyro; import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java similarity index 96% rename from src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java rename to src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java index 54d60aef..7fdbbcd4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.swerve.moduleIO; +package frc.robot.subsystems.swerve.module; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java similarity index 99% rename from src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java rename to src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java index 40e1eeea..a987556e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.swerve.moduleIO; +package frc.robot.subsystems.swerve.module; import static edu.wpi.first.units.Units.*; diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java similarity index 98% rename from src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java rename to src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java index 8418d7a2..faae575e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.swerve.moduleIO; +package frc.robot.subsystems.swerve.module; import static edu.wpi.first.units.Units.*; From 85ee27c48a3ba86884495be0acf8c195b5a432c5 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Wed, 20 Nov 2024 11:42:55 -0500 Subject: [PATCH 32/75] enum update --- .../java/frc/robot/subsystems/vision/VisionConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index eac08c9e..56de4bea 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -6,9 +6,9 @@ public final class VisionConstants { public enum Limelight { - SHOOTER(0), - FRONT_LEFT(1), - FRONT_RIGHT(2); + SHOOTER(SHOOTER_LIMELIGHT_NUMBER), + FRONT_LEFT(FRONT_LEFT_LIMELIGHT_NUMBER), + FRONT_RIGHT(FRONT_RIGHT_LIMELIGHT_NUMBER); public final int id; From ccf3ddc7d5906d89ac1725f7f95223641a176863 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Wed, 20 Nov 2024 14:03:04 -0500 Subject: [PATCH 33/75] more enum stuff --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/subsystems/swerve/SwerveDrive.java | 2 +- .../robot/subsystems/swerve/SwerveModule.java | 3 +- .../subsystems/vision/PhysicalVision.java | 34 ++++++++----------- 4 files changed, 18 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6bd8c021..890b952a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,10 +18,10 @@ import frc.robot.subsystems.swerve.SwerveConstants; import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; +import frc.robot.subsystems.swerve.SwerveDrive; import frc.robot.subsystems.swerve.gyro.GyroInterface; import frc.robot.subsystems.swerve.gyro.PhysicalGyro; import frc.robot.subsystems.swerve.gyro.SimulatedGyro; -import frc.robot.subsystems.swerve.SwerveDrive; import frc.robot.subsystems.swerve.module.ModuleInterface; import frc.robot.subsystems.swerve.module.PhysicalModule; import frc.robot.subsystems.swerve.module.SimulatedModule; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 9f50b57e..b7907010 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -19,8 +19,8 @@ import frc.robot.extras.util.DeviceCANBus; import frc.robot.extras.util.TimeUtil; import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; -import frc.robot.subsystems.swerve.gyro.GyroInterface; import frc.robot.subsystems.swerve.gyro.GyroInputsAutoLogged; +import frc.robot.subsystems.swerve.gyro.GyroInterface; import frc.robot.subsystems.swerve.module.ModuleInterface; import frc.robot.subsystems.swerve.odometryThread.OdometryThread; import frc.robot.subsystems.swerve.odometryThread.OdometryThreadInputsAutoLogged; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index ebd28af0..4dc90e98 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -10,9 +10,8 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; -import frc.robot.subsystems.swerve.module.ModuleInterface; import frc.robot.subsystems.swerve.module.ModuleInputsAutoLogged; - +import frc.robot.subsystems.swerve.module.ModuleInterface; import org.littletonrobotics.junction.Logger; public class SwerveModule extends SubsystemBase { diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index e9af7fd0..bc09b636 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -21,44 +21,41 @@ public class PhysicalVision implements VisionInterface { private Pose2d lastSeenPose = new Pose2d(); private double headingDegrees = 0; private double headingRateDegreesPerSecond = 0; - // private final Map limelightThreads = new ConcurrentHashMap<>(); - private final ConcurrentHashMap> limelightThreads = + private final ConcurrentHashMap> limelightThreads = new ConcurrentHashMap<>(); private final ExecutorService executorService = Executors.newFixedThreadPool(3); - private final AtomicReference latestInputs = - new AtomicReference<>(new VisionInputs()); /** * The pose estimates from the limelights in the following order {shooterLimelight, * frontLeftLimelight, frontRightLimelight} */ - private PoseEstimate[] limelightEstimates; + private PoseEstimate[] limelightEstimates = + new PoseEstimate[] {new PoseEstimate(), new PoseEstimate(), new PoseEstimate()}; public PhysicalVision() { limelightEstimates = new PoseEstimate[3]; for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { Limelight limelight = Limelight.values()[limelightNumber]; - limelightThreads.put(limelightNumber, new AtomicReference<>(new VisionInputs())); - limelightEstimates[limelightNumber] = new PoseEstimate(); - visionThread(limelightNumber); + limelightThreads.put(limelight, new AtomicReference<>(new VisionInputs())); + visionThread(limelight); } } @Override public void updateInputs(VisionInputs inputs) { limelightThreads.forEach( - (limelightId, inputRef) -> { + (limelight, inputRef) -> { VisionInputs limelightInputs = inputRef.get(); // Combine inputs into the main inputs object - if (limelightId == 0) { + if (limelight == Limelight.SHOOTER) { inputs.isShooterLimelightConnected = limelightInputs.isShooterLimelightConnected; inputs.shooterMegaTag1Pose = limelightInputs.shooterMegaTag1Pose; inputs.shooterLatency = limelightInputs.shooterLatency; - } else if (limelightId == 1) { + } else if (limelight == Limelight.FRONT_LEFT) { inputs.isFrontLeftLimelightConnected = limelightInputs.isFrontLeftLimelightConnected; inputs.frontLeftMegaTag1Pose = limelightInputs.frontLeftMegaTag1Pose; inputs.frontLeftLatency = limelightInputs.frontLeftLatency; - } else if (limelightId == 2) { + } else if (limelight == Limelight.FRONT_RIGHT) { inputs.isFrontRightLimelightConnected = limelightInputs.isFrontRightLimelightConnected; inputs.frontRightMegaTag1Pose = limelightInputs.frontRightMegaTag1Pose; inputs.frontRightLatency = limelightInputs.frontRightLatency; @@ -353,7 +350,7 @@ public void checkAndUpdatePose(Limelight limelight) { if (current_TX != last_TX || current_TY != last_TY) { updatePoseEstimate(limelight); limelightThreads.computeIfPresent( - limelight.id, (key, value) -> new AtomicReference<>(new VisionInputs())); + limelight, (key, value) -> new AtomicReference<>(new VisionInputs())); // This is to keep track of the last valid pose calculated by the limelights // it is used when the driver resets the robot odometry to the limelight calculated // position @@ -415,20 +412,19 @@ public void checkAndUpdatePose(Limelight limelight) { // } // }); // } - public void visionThread(int limelightId) { + public void visionThread(Limelight limelight) { executorService.submit( () -> { try { while (!Thread.currentThread().isInterrupted()) { // Collect data into the vision inputs VisionInputs inputs = new VisionInputs(); - inputs.isShooterLimelightConnected = isLimelightConnected(getLimelight(limelightId)); + inputs.isShooterLimelightConnected = isLimelightConnected(limelight); inputs.shooterMegaTag1Pose = - MegatagPoseEstimate.fromLimelight( - getMegaTag1PoseEstimate(getLimelight(limelightId))); - inputs.shooterLatency = getLatencySeconds(getLimelight(limelightId)); + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); + inputs.shooterLatency = getLatencySeconds(limelight); - limelightThreads.get(limelightId).set(inputs); + limelightThreads.get(limelight).set(inputs); // Sleep to avoid overloading Thread.sleep(20); // Example delay From d3f0f7bc64b6d3486bd08ee93ac4a5a499a01c2a Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Wed, 20 Nov 2024 14:04:33 -0500 Subject: [PATCH 34/75] huh? --- src/main/java/frc/robot/subsystems/vision/PhysicalVision.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index bc09b636..1392f050 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -33,7 +33,6 @@ public class PhysicalVision implements VisionInterface { new PoseEstimate[] {new PoseEstimate(), new PoseEstimate(), new PoseEstimate()}; public PhysicalVision() { - limelightEstimates = new PoseEstimate[3]; for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { Limelight limelight = Limelight.values()[limelightNumber]; limelightThreads.put(limelight, new AtomicReference<>(new VisionInputs())); From 70f27e29bc8d693d5fda0a586345dd6835327730 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 21 Nov 2024 18:13:49 -0500 Subject: [PATCH 35/75] constants and methods and such --- .../swerve/odometryThread/OdometryThread.java | 3 +- .../subsystems/vision/PhysicalVision.java | 167 ++++++++---------- .../subsystems/vision/VisionConstants.java | 34 +++- 3 files changed, 103 insertions(+), 101 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java index b65ad202..a4a98ea3 100644 --- a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java +++ b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import edu.wpi.first.units.measure.Angle; +import frc.robot.Constants; import frc.robot.Constants.HardwareConstants; import frc.robot.Robot; import frc.robot.extras.util.DeviceCANBus; @@ -47,7 +48,7 @@ static Queue registerInput(Supplier supplier) { } static OdometryThread createInstance(DeviceCANBus canBus) { - return switch (Robot.CURRENT_ROBOT_MODE) { + return switch (Constants.currentMode) { case REAL -> new OdometryThreadReal( canBus, diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 1392f050..ec694d44 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -24,6 +24,7 @@ public class PhysicalVision implements VisionInterface { private final ConcurrentHashMap> limelightThreads = new ConcurrentHashMap<>(); private final ExecutorService executorService = Executors.newFixedThreadPool(3); + private final AtomicReference latestInputs = new AtomicReference<>(); /** * The pose estimates from the limelights in the following order {shooterLimelight, @@ -46,20 +47,27 @@ public void updateInputs(VisionInputs inputs) { (limelight, inputRef) -> { VisionInputs limelightInputs = inputRef.get(); // Combine inputs into the main inputs object - if (limelight == Limelight.SHOOTER) { - inputs.isShooterLimelightConnected = limelightInputs.isShooterLimelightConnected; - inputs.shooterMegaTag1Pose = limelightInputs.shooterMegaTag1Pose; - inputs.shooterLatency = limelightInputs.shooterLatency; - } else if (limelight == Limelight.FRONT_LEFT) { - inputs.isFrontLeftLimelightConnected = limelightInputs.isFrontLeftLimelightConnected; - inputs.frontLeftMegaTag1Pose = limelightInputs.frontLeftMegaTag1Pose; - inputs.frontLeftLatency = limelightInputs.frontLeftLatency; - } else if (limelight == Limelight.FRONT_RIGHT) { - inputs.isFrontRightLimelightConnected = limelightInputs.isFrontRightLimelightConnected; - inputs.frontRightMegaTag1Pose = limelightInputs.frontRightMegaTag1Pose; - inputs.frontRightLatency = limelightInputs.frontRightLatency; + + switch (limelight) { + case SHOOTER -> { + inputs.isShooterLimelightConnected = limelightInputs.isShooterLimelightConnected; + inputs.shooterMegaTag1Pose = limelightInputs.shooterMegaTag1Pose; + inputs.shooterLatency = limelightInputs.shooterLatency; + } + case FRONT_LEFT -> { + inputs.isFrontLeftLimelightConnected = limelightInputs.isFrontLeftLimelightConnected; + inputs.frontLeftMegaTag1Pose = limelightInputs.frontLeftMegaTag1Pose; + inputs.frontLeftLatency = limelightInputs.frontLeftLatency; + } + case FRONT_RIGHT -> { + inputs.isFrontRightLimelightConnected = + limelightInputs.isFrontRightLimelightConnected; + inputs.frontRightMegaTag1Pose = limelightInputs.frontRightMegaTag1Pose; + inputs.frontRightLatency = limelightInputs.frontRightLatency; + } } }); + latestInputs.set(inputs); } /** @@ -74,11 +82,11 @@ public boolean canSeeAprilTags(Limelight limelight) { // Different Limelights have different FOVs if (getNumberOfAprilTags(limelight) > 0 && getNumberOfAprilTags(limelight) <= VisionConstants.APRIL_TAG_POSITIONS.length) { - if (getLimelightName(limelight).equals(VisionConstants.SHOOTER_LIMELIGHT_NAME)) { - return Math.abs(LimelightHelpers.getTX(getLimelightName(limelight))) + if (limelight.getName().equals(VisionConstants.SHOOTER_LIMELIGHT_NAME)) { + return Math.abs(LimelightHelpers.getTX(limelight.getName())) <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR; } else { - return Math.abs(LimelightHelpers.getTX(getLimelightName(limelight))) + return Math.abs(LimelightHelpers.getTX(limelight.getName())) <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR; } } @@ -96,16 +104,15 @@ public PoseEstimate enabledPoseUpdate(Limelight limelight) { if (isLargeDiscrepancyBetweenMegaTag1And2(limelight) && getLimelightAprilTagDistance(limelight) < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { - return limelightEstimates[limelight.id] = getMegaTag1PoseEstimate(limelight); + return limelightEstimates[limelight.getId()] = getMegaTag1PoseEstimate(limelight); } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) { - LimelightHelpers.SetRobotOrientation( - getLimelightName(limelight), headingDegrees, 0, 0, 0, 0, 0); - return limelightEstimates[limelight.id] = getMegaTag2PoseEstimate(limelight); + LimelightHelpers.SetRobotOrientation(limelight.getName(), headingDegrees, 0, 0, 0, 0, 0); + return limelightEstimates[limelight.getId()] = getMegaTag2PoseEstimate(limelight); } else { - return limelightEstimates[limelight.id] = getMegaTag1PoseEstimate(limelight); + return limelightEstimates[limelight.getId()] = getMegaTag1PoseEstimate(limelight); } } - return limelightEstimates[limelight.id] = new PoseEstimate(); + return limelightEstimates[limelight.getId()] = new PoseEstimate(); } /** @@ -115,7 +122,7 @@ && getLimelightAprilTagDistance(limelight) * @param limelight the number of the limelight */ public void updatePoseEstimate(Limelight limelight) { - limelightEstimates[limelight.id] = + limelightEstimates[limelight.getId()] = DriverStation.isEnabled() ? enabledPoseUpdate(limelight) : getMegaTag1PoseEstimate(limelight); @@ -128,30 +135,26 @@ public void updatePoseEstimate(Limelight limelight) { * @return true if the discrepancy is larger than the defined threshold, false otherwise */ public boolean isLargeDiscrepancyBetweenMegaTag1And2(Limelight limelight) { - PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelight); - PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelight); - - // Extract the positions of the two poses - Translation2d megaTag1TranslationMeters = megaTag1Estimate.pose.getTranslation(); - Translation2d megaTag2TranslationMeters = megaTag2Estimate.pose.getTranslation(); - - double megaTag1RotationDegrees = megaTag1Estimate.pose.getRotation().getDegrees(); - double megaTag2RotationDegrees = megaTag2Estimate.pose.getRotation().getDegrees(); - - // Calculate the discrepancy between the two MegaTag translations in meters - double megaTagTranslationDiscrepancyMeters = - megaTag1TranslationMeters.getDistance(megaTag2TranslationMeters); - double megaTagRotationDiscrepancyDegrees = - Math.abs(megaTag1RotationDegrees - megaTag2RotationDegrees); + PoseEstimate megatag1Estimate = getMegaTag1PoseEstimate(limelight); + PoseEstimate megatag2Estimate = getMegaTag2PoseEstimate(limelight); + + return !isPoseWithinThreshold( + megatag1Estimate.pose.getTranslation(), + megatag2Estimate.pose.getTranslation(), + VisionConstants.MEGA_TAG_TRANSLATION_DISCREPANCY_THRESHOLD) + || !isRotationWithinThreshold( + megatag1Estimate.pose.getRotation().getDegrees(), + megatag2Estimate.pose.getRotation().getDegrees(), + VisionConstants.MEGA_TAG_ROTATION_DISCREPANCY_THREASHOLD); + } - // Define a threshold (meters) for what constitutes a "large" discrepancy - // This value should be determined based on your testing - double thresholdMeters = 0.5; - double thresholdDegrees = 45; + private boolean isPoseWithinThreshold( + Translation2d t1, Translation2d t2, double thresholdMeters) { + return t1.getDistance(t2) <= thresholdMeters; + } - // Check if the discrepancy is larger than the threshold (meters) - return megaTagTranslationDiscrepancyMeters > thresholdMeters - || megaTagRotationDiscrepancyDegrees > thresholdDegrees; + private boolean isRotationWithinThreshold(double r1, double r2, double thresholdDegrees) { + return Math.abs(r1 - r2) <= thresholdDegrees; } /** @@ -163,7 +166,7 @@ public boolean isLargeDiscrepancyBetweenMegaTag1And2(Limelight limelight) { * return 0 for x, y, and theta */ public PoseEstimate getMegaTag1PoseEstimate(Limelight limelight) { - return LimelightHelpers.getBotPoseEstimate_wpiBlue(getLimelightName(limelight)); + return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()); } /** @@ -175,7 +178,7 @@ public PoseEstimate getMegaTag1PoseEstimate(Limelight limelight) { * return 0 for x, y, and theta */ public PoseEstimate getMegaTag2PoseEstimate(Limelight limelight) { - return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(getLimelightName(limelight)); + return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName()); } /** @@ -220,21 +223,21 @@ private boolean isWithinFieldBounds( */ @Override public Pose2d getPoseFromAprilTags(Limelight limelight) { - return limelightEstimates[limelight.id].pose; + return limelightEstimates[limelight.getId()].pose; } public Pose2d getAprilTagPositionToLimelight(Limelight limelight) { - return LimelightHelpers.getTargetPose_CameraSpace(getLimelightName(limelight)); + return LimelightHelpers.getTargetPose_CameraSpace(limelight.getName()); } public Pose2d getAprilTagPositionToRobot(Limelight limelight) { - return LimelightHelpers.getTargetPose_RobotSpace(getLimelightName(limelight)); + return LimelightHelpers.getTargetPose_RobotSpace(limelight.getName()); } /** Returns how many april tags the limelight that is being used for pose estimation can see. */ @Override public int getNumberOfAprilTags(Limelight limelight) { - return limelightEstimates[limelight.id].tagCount; + return limelightEstimates[limelight.getId()].tagCount; } /** @@ -243,7 +246,7 @@ public int getNumberOfAprilTags(Limelight limelight) { */ @Override public double getTimeStampSeconds(Limelight limelight) { - return limelightEstimates[limelight.id].timestampSeconds / 1000.0; + return limelightEstimates[limelight.getId()].timestampSeconds / 1000.0; } /** @@ -253,7 +256,7 @@ public double getTimeStampSeconds(Limelight limelight) { */ @Override public double getLatencySeconds(Limelight limelight) { - return (limelightEstimates[limelight.id].latency) / 1000.0; + return (limelightEstimates[limelight.getId()].latency) / 1000.0; } /** Gets the pose calculated the last time a limelight saw an April Tag */ @@ -270,7 +273,7 @@ public Pose2d getLastSeenPose() { */ public double getLimelightAprilTagDistance(Limelight limelight) { if (canSeeAprilTags(limelight)) { - return limelightEstimates[limelight.id].avgTagDist; + return limelightEstimates[limelight.getId()].avgTagDist; } // To be safe returns a big distance from the april tags if it can't see any return Double.MAX_VALUE; @@ -289,32 +292,8 @@ public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { this.headingRateDegreesPerSecond = headingRateDegrees; } - /** - * Gets the limelight name associated with the specified limelight number/index - * - * @param limelight the limelight enum - * @return SHOOTER = limelight-shooter, FRONT_LEFT = limelight-left, FRONT_RIGHT = limelight-right - */ - public String getLimelightName(Limelight limelight) { - return switch (limelight) { - case SHOOTER -> VisionConstants.SHOOTER_LIMELIGHT_NAME; - case FRONT_LEFT -> VisionConstants.FRONT_LEFT_LIMELIGHT_NAME; - case FRONT_RIGHT -> VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME; - }; - } - - public Limelight getLimelight(int id) { - return switch (id) { - case 0 -> Limelight.SHOOTER; - case 1 -> Limelight.FRONT_LEFT; - case 2 -> Limelight.FRONT_RIGHT; - default -> - throw new IllegalArgumentException("You entered a number for a non-existent limelight"); - }; - } - public boolean isLimelightConnected(Limelight limelight) { - NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(getLimelightName(limelight)); + NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(limelight.getName()); if (limelightTable.containsKey("tx")) { return true; } @@ -339,8 +318,8 @@ public void checkAndUpdatePose(Limelight limelight) { // object, as its reference was modified earlier. synchronized (this) { try { - double current_TX = LimelightHelpers.getTX(getLimelightName(limelight)); - double current_TY = LimelightHelpers.getTY(getLimelightName(limelight)); + double current_TX = LimelightHelpers.getTX(limelight.getName()); + double current_TY = LimelightHelpers.getTY(limelight.getName()); // This checks if the limelight reading is new. The reasoning being that if the TX and TY // are EXACTLY the same, it hasn't updated yet with a new reading. We are doing it this way, @@ -371,7 +350,7 @@ public void checkAndUpdatePose(Limelight limelight) { last_TY = current_TY; } catch (Exception e) { System.err.println( - "Error communicating with the: " + getLimelightName(limelight) + ": " + e.getMessage()); + "Error communicating with the: " + limelight.getName() + ": " + e.getMessage()); } } } @@ -405,7 +384,7 @@ public void checkAndUpdatePose(Limelight limelight) { // } catch (Exception e) { // System.err.println( // "Error executing task for the: " - // + getLimelightName(limelight) + // + limelight.getName() // + ": " // + e.getMessage()); // } @@ -416,17 +395,15 @@ public void visionThread(Limelight limelight) { () -> { try { while (!Thread.currentThread().isInterrupted()) { - // Collect data into the vision inputs - VisionInputs inputs = new VisionInputs(); - inputs.isShooterLimelightConnected = isLimelightConnected(limelight); - inputs.shooterMegaTag1Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); - inputs.shooterLatency = getLatencySeconds(limelight); - - limelightThreads.get(limelight).set(inputs); - - // Sleep to avoid overloading - Thread.sleep(20); // Example delay + synchronized (latestInputs) { + VisionInputs currentInputs = latestInputs.get(); + currentInputs.isShooterLimelightConnected = isLimelightConnected(limelight); + currentInputs.shooterMegaTag1Pose = + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); + currentInputs.shooterLatency = getLatencySeconds(limelight); + latestInputs.set(currentInputs); + } + Thread.sleep(VisionConstants.THREAD_SLEEP_MS); } } catch (InterruptedException e) { Thread.currentThread().interrupt(); // Restore interrupted status @@ -444,11 +421,11 @@ public void stopThread(Limelight limelight) { try { // Since we can't see an April Tag, set the estimate for the specified limelight to an empty // PoseEstimate() - limelightEstimates[limelight.id] = new PoseEstimate(); + limelightEstimates[limelight.getId()] = new PoseEstimate(); // limelightThreads.get(limelight.id).set();(false); } catch (Exception e) { System.err.println( - "Error stopping thread for the: " + getLimelightName(limelight) + ": " + e.getMessage()); + "Error stopping thread for the: " + limelight.getName() + ": " + e.getMessage()); } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 56de4bea..51679e36 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -6,17 +6,38 @@ public final class VisionConstants { public enum Limelight { - SHOOTER(SHOOTER_LIMELIGHT_NUMBER), - FRONT_LEFT(FRONT_LEFT_LIMELIGHT_NUMBER), - FRONT_RIGHT(FRONT_RIGHT_LIMELIGHT_NUMBER); + SHOOTER(0, VisionConstants.SHOOTER_LIMELIGHT_NAME), + FRONT_LEFT(1, VisionConstants.FRONT_LEFT_LIMELIGHT_NAME), + FRONT_RIGHT(2, VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME); - public final int id; + private final int id; + private final String name; - Limelight(int id) { + Limelight(int id, String name) { this.id = id; + this.name = name; + } + + public int getId() { + return id; + } + + public String getName() { + return name; + } + + public static Limelight fromId(int id) { + return switch (id) { + case 0 -> SHOOTER; + case 1 -> FRONT_LEFT; + case 2 -> FRONT_RIGHT; + default -> throw new IllegalArgumentException("Invalid Limelight ID: " + id); + }; } } + public static final int THREAD_SLEEP_MS = 20; + public static final AprilTagFieldLayout FIELD_LAYOUT = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); public static final double VISION_X_POS_TRUST = 0.5; // meters @@ -32,6 +53,9 @@ public enum Limelight { public static final double MEGA_TAG_2_DISTANCE_THRESHOLD = 5; // TODO: Tune + public static final double MEGA_TAG_TRANSLATION_DISCREPANCY_THRESHOLD = 0.5; // TODO: tune + public static final double MEGA_TAG_ROTATION_DISCREPANCY_THREASHOLD = 45; + public static final String SHOOTER_LIMELIGHT_NAME = "limelight-shooter"; public static final int SHOOTER_LIMELIGHT_NUMBER = 0; public static final String FRONT_LEFT_LIMELIGHT_NAME = "limelight-left"; From d56de839eff847f0bf816eabeb69e42a51b00b42 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Fri, 22 Nov 2024 09:10:26 -0500 Subject: [PATCH 36/75] geom util --- .../java/frc/robot/extras/util/GeomUtil.java | 28 ++++++++++++++++ .../subsystems/vision/PhysicalVision.java | 32 +++++++------------ 2 files changed, 40 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/extras/util/GeomUtil.java b/src/main/java/frc/robot/extras/util/GeomUtil.java index 3b52c448..f52008a5 100644 --- a/src/main/java/frc/robot/extras/util/GeomUtil.java +++ b/src/main/java/frc/robot/extras/util/GeomUtil.java @@ -50,7 +50,35 @@ public static ChassisSpeeds toWpilibChassisSpeeds( dyn4jLinearVelocity.x, dyn4jLinearVelocity.y, angularVelocityRadPerSec); } + /** + * Gets the x and y velocities of a ChassisSpeeds + * @param chassisSpeeds the ChassisSpeeds to retrieve velocities from + * @return a Translation2d containing the velocities in the x and y direction in meters per second + */ public static Translation2d getChassisSpeedsTranslationalComponent(ChassisSpeeds chassisSpeeds) { return new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); } + + /** + * Checks if two translations are within a certain threshold in meters + * @param t1 Translation 1 as a Translation2d + * @param t2 Translation 2 as a Translation2d + * @param thresholdMeters the threshold between the two translations in meters + * @return true if the two translations are within thresholdMeters + */ + public static boolean isTranslationWithinThreshold( + Translation2d t1, Translation2d t2, double thresholdMeters) { + return t1.getDistance(t2) <= thresholdMeters; + } + + /** + * Checks if two rotations are within a certain threshold in degrees + * @param r1 Rotations 1 as a Rotation2d + * @param r2 Rotation 2 as a Rotation2d + * @param thresholdMeters the threshold between the two rotations in degrees + * @return true if the two rotations are within thresholdDegrees + */ + public static boolean isRotationWithinThreshold(double r1, double r2, double thresholdDegrees) { + return Math.abs(r1 - r2) <= thresholdDegrees; + } } diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index ec694d44..bdf44da7 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -5,6 +5,7 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants.FieldConstants; +import frc.robot.extras.util.GeomUtil; import frc.robot.extras.vision.LimelightHelpers; import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; import frc.robot.extras.vision.MegatagPoseEstimate; @@ -73,7 +74,7 @@ public void updateInputs(VisionInputs inputs) { /** * Checks if the specified limelight can fully see one or more April Tag. * - * @param limelight the number of the limelight + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return true if the limelight can fully see one or more April Tag */ @Override @@ -97,7 +98,7 @@ && getNumberOfAprilTags(limelight) <= VisionConstants.APRIL_TAG_POSITIONS.length * Gets the JSON dump from the specified limelight and puts it into a PoseEstimate object, which * is then placed into its corresponding spot in the limelightEstimates array. * - * @param limelight the number of the limelight + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). */ public PoseEstimate enabledPoseUpdate(Limelight limelight) { if (canSeeAprilTags(limelight) && isValidPoseEstimate(limelight)) { @@ -119,7 +120,7 @@ && getLimelightAprilTagDistance(limelight) * If the robot is not enabled, update the pose using MegaTag1 and after it is enabled, run {@link * #enabledPoseUpdate(int)} * - * @param limelight the number of the limelight + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). */ public void updatePoseEstimate(Limelight limelight) { limelightEstimates[limelight.getId()] = @@ -131,37 +132,28 @@ public void updatePoseEstimate(Limelight limelight) { /** * Checks if there is a large discrepancy between the MegaTag1 and MegaTag2 estimates. * - * @param limelight the number of the limelight + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return true if the discrepancy is larger than the defined threshold, false otherwise */ public boolean isLargeDiscrepancyBetweenMegaTag1And2(Limelight limelight) { PoseEstimate megatag1Estimate = getMegaTag1PoseEstimate(limelight); PoseEstimate megatag2Estimate = getMegaTag2PoseEstimate(limelight); - return !isPoseWithinThreshold( + return !GeomUtil.isTranslationWithinThreshold( megatag1Estimate.pose.getTranslation(), megatag2Estimate.pose.getTranslation(), VisionConstants.MEGA_TAG_TRANSLATION_DISCREPANCY_THRESHOLD) - || !isRotationWithinThreshold( + || !GeomUtil.isRotationWithinThreshold( megatag1Estimate.pose.getRotation().getDegrees(), megatag2Estimate.pose.getRotation().getDegrees(), VisionConstants.MEGA_TAG_ROTATION_DISCREPANCY_THREASHOLD); } - private boolean isPoseWithinThreshold( - Translation2d t1, Translation2d t2, double thresholdMeters) { - return t1.getDistance(t2) <= thresholdMeters; - } - - private boolean isRotationWithinThreshold(double r1, double r2, double thresholdDegrees) { - return Math.abs(r1 - r2) <= thresholdDegrees; - } - /** * Gets the MegaTag1 pose of the robot calculated by specified limelight via any April Tags it * sees * - * @param limelight the number of the limelight + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return the MegaTag1 pose of the robot, if the limelight can't see any April Tags, it will * return 0 for x, y, and theta */ @@ -173,7 +165,7 @@ public PoseEstimate getMegaTag1PoseEstimate(Limelight limelight) { * Gets the MegaTag2 pose of the robot calculated by specified limelight via any April Tags it * sees * - * @param limelight the number of the limelight + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return the MegaTag2 pose of the robot, if the limelight can't see any April Tags, it will * return 0 for x, y, and theta */ @@ -184,7 +176,7 @@ public PoseEstimate getMegaTag2PoseEstimate(Limelight limelight) { /** * Checks if the MT1 and MT2 pose estimate exists and whether it is within the field * - * @param limelight the number of the limelight + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return true if the pose estimate exists within the field and the pose estimate is not null */ public boolean isValidPoseEstimate(Limelight limelight) { @@ -217,7 +209,7 @@ private boolean isWithinFieldBounds( /** * Gets the pose of the robot calculated by specified limelight via any April Tags it sees * - * @param limelight the number of the limelight + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return the pose of the robot, if the limelight can't see any April Tags, it will return 0 for * x, y, and theta */ @@ -268,7 +260,7 @@ public Pose2d getLastSeenPose() { /** * Gets the average distance between the specified limelight and the April Tags it sees * - * @param limelightNumber the number of the limelight + * @param limelightNumber a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return the average distance between the robot and the April Tag(s) in meters */ public double getLimelightAprilTagDistance(Limelight limelight) { From 63f1a54902af3b83c99755cb338b151d4ca3eb73 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Fri, 22 Nov 2024 14:52:20 -0500 Subject: [PATCH 37/75] sim like moderatly works --- .../commands/drive/DriveCommandBase.java | 4 - .../robot/subsystems/swerve/SwerveDrive.java | 99 +++++++++++++------ .../subsystems/swerve/gyro/GyroInterface.java | 5 + .../subsystems/swerve/gyro/PhysicalGyro.java | 11 ++- .../subsystems/swerve/gyro/SimulatedGyro.java | 7 ++ 5 files changed, 89 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java index 84445d2b..65a4980d 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java @@ -56,10 +56,6 @@ public void calculatePoseFromLimelight(Limelight limelight) { // Depending on how many april tags we see, we change our confidence as more april tags // results in a much more accurate pose estimate - // TODO: check if this is necessary anymore with MT2, also we might want to set the limelight - // so it only uses 1 april tag, if they set up the field wrong (they can set april tags +-1 - // inch I believe) - // using multiple *could* really mess things up. if (vision.getNumberOfAprilTags(limelight) == 1) { double[] standardDeviations = oneAprilTagLookupTable.getLookupValue(distanceFromClosestAprilTag); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index b7907010..032fb6f2 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -37,10 +37,12 @@ public class SwerveDrive extends SubsystemBase { private Rotation2d rawGyroRotation; private final SwerveModulePosition[] lastModulePositions; - private final SwerveDrivePoseEstimator poseEstimator; + private final SwerveDrivePoseEstimator odometry; private final OdometryThread odometryThread; + private Optional alliance; + private final Alert gyroDisconnectedAlert = new Alert("Gyro Hardware Fault", Alert.AlertType.kError); @@ -69,10 +71,10 @@ public SwerveDrive( new SwerveModulePosition(), new SwerveModulePosition() }; - this.poseEstimator = + this.odometry = new SwerveDrivePoseEstimator( DriveConstants.DRIVE_KINEMATICS, - getRawGyroYaw(), + getGyroRotation2d(), lastModulePositions, new Pose2d(), VecBuilder.fill( @@ -89,15 +91,6 @@ public SwerveDrive( gyroDisconnectedAlert.set(false); } - /** - * Gets the current velocity of the gyro's yaw - * - * @return the yaw velocity - */ - public double getGyroRate() { - return gyroInputs.yawVelocity; - } - // /** Updates the pose estimator with the pose calculated from the swerve modules. */ // public void addPoseEstimatorSwerveMeasurement() { // for (int timestampIndex = 0; @@ -115,7 +108,7 @@ public double getGyroRate() { */ public void addPoseEstimatorVisionMeasurement( Pose2d visionMeasurement, double currentTimeStampSeconds) { - poseEstimator.addVisionMeasurement(visionMeasurement, currentTimeStampSeconds); + odometry.addVisionMeasurement(visionMeasurement, currentTimeStampSeconds); } /** @@ -128,7 +121,7 @@ public void addPoseEstimatorVisionMeasurement( */ public void setPoseEstimatorVisionConfidence( double xStandardDeviation, double yStandardDeviation, double thetaStandardDeviation) { - poseEstimator.setVisionMeasurementStdDevs( + odometry.setVisionMeasurementStdDevs( VecBuilder.fill(xStandardDeviation, yStandardDeviation, thetaStandardDeviation)); } @@ -196,7 +189,7 @@ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fi xSpeed, ySpeed, rotationSpeed, - getPose().getRotation().plus(Rotation2d.fromDegrees(getAllianceAngleOffset()))) + getOdometryAllianceRelativeRotation2d()) : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); // , 0.02); SwerveModuleState[] swerveModuleStates = @@ -208,14 +201,71 @@ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fi Logger.recordOutput("SwerveStates/DesiredStates", swerveModuleStates); } + /** + * Returns the heading of the robot in degrees from 0 to 360. + * + * @return Value is Counter-clockwise positive. + */ + public double getHeading() { + return -gyroInputs.yawDegrees; + } + + /** + * Gets the rate of rotation of the NavX. + * + * @return The current rate in degrees per second. + */ + public double getGyroRate() { + return -gyroInputs.yawVelocity; + } + + /** Returns a Rotation2d for the heading of the robot. */ + public Rotation2d getGyroRotation2d() { + return Rotation2d.fromDegrees(getHeading()); + } + + /** Returns a Rotation2d for the heading of the robot. */ + public Rotation2d getGyroFieldRelativeRotation2d() { + return Rotation2d.fromDegrees(getHeading() + getAllianceAngleOffset()); + } + /** Returns 0 degrees if the robot is on the blue alliance, 180 if on the red alliance. */ public double getAllianceAngleOffset() { - Optional alliance = DriverStation.getAlliance(); + alliance = DriverStation.getAlliance(); double offset = alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red ? 180.0 : 0.0; return offset; } + /** Zeroes the heading of the robot. */ + public void zeroHeading() { + gyroIO.reset(); + } + + /** + * Returns the estimated field-relative pose of the robot. Positive x being forward, positive y + * being left. + */ + @AutoLogOutput(key = "Odometry/RobotPosition") + public Pose2d getPose() { + return odometry.getEstimatedPosition(); + } + + /** Returns a Rotation2d for the heading of the robot */ + public Rotation2d getOdometryRotation2d() { + return getPose().getRotation(); + } + + /** + * Returns a Rotation2d for the heading of the robot relative to the field from the driver's + * perspective. This method is needed so that the drive command and poseEstimator don't fight each + * other. It uses odometry rotation. + */ + public Rotation2d getOdometryAllianceRelativeRotation2d() { + return getPose().getRotation().plus(Rotation2d.fromDegrees(getAllianceAngleOffset())); + } + + /** * Sets the modules to the specified states. * @@ -239,13 +289,13 @@ public void addPoseEstimatorSwerveMeasurement() { if (gyroInputs.isConnected) { // rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; - rawGyroRotation = getRawGyroYaw(); + rawGyroRotation = getGyroRotation2d(); } else { Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas); rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } - poseEstimator.updateWithTime( + odometry.updateWithTime( // odometryThreadInputs.measurementTimeStamps[timestampIndex], Timer.getFPGATimestamp(), rawGyroRotation, modulePositions); } @@ -293,17 +343,6 @@ private SwerveModulePosition[] getModulePositions() { return positions; } - /** Gets the fused pose from the pose estimator. */ - @AutoLogOutput(key = "Odometry/RobotPosition") - public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); - } - - /** Gets the current gyro yaw */ - public Rotation2d getRawGyroYaw() { - return Rotation2d.fromDegrees(gyroInputs.yawDegrees); - } - /** * Sets the pose * @@ -313,6 +352,6 @@ public void resetPosition(Pose2d pose) { // for (int timestampIndex = 0; // timestampIndex < odometryThreadInputs.measurementTimeStamps.length; // timestampIndex++) - poseEstimator.resetPosition(getRawGyroYaw(), getModulePositions(), pose); + odometry.resetPosition(getGyroRotation2d(), getModulePositions(), pose); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java index 0b2a8eb6..b3ac4b82 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java @@ -22,4 +22,9 @@ public static class GyroInputs { * @param inputs inputs to update */ default void updateInputs(GyroInputs inputs) {} + + /** + * Resets the gyro yaw + */ + default void reset() {} } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java index 3de37cd7..0dfc028a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java @@ -15,15 +15,15 @@ public class PhysicalGyro implements GyroInterface { private final Queue yawPositionInput; public PhysicalGyro() { - yawPositionInput = OdometryThread.registerInput(() -> Degrees.of(-gyro.getAngle())); + yawPositionInput = OdometryThread.registerInput(() -> Degrees.of(gyro.getAngle())); } @Override public void updateInputs(GyroInputs inputs) { inputs.isConnected = gyro.isConnected(); inputs.yawDegreesRotation2d = gyro.getRotation2d(); - inputs.yawVelocity = -gyro.getRate(); - inputs.yawDegrees = -gyro.getAngle(); + inputs.yawVelocity = gyro.getRate(); + inputs.yawDegrees = gyro.getAngle(); // Handle odometry yaw positions if (!yawPositionInput.isEmpty()) { @@ -36,4 +36,9 @@ public void updateInputs(GyroInputs inputs) { yawPositionInput.clear(); } } + + @Override + public void reset() { + gyro.reset(); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java index 9b762c0b..dbd99c9b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java @@ -2,7 +2,9 @@ import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Rotation; +import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.extras.simulation.OdometryTimestampsSim; import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation; @@ -24,4 +26,9 @@ public void updateInputs(GyroInputs inputs) { RadiansPerSecond.of(gyroSimulation.getMeasuredAngularVelocityRadPerSec()) .in(DegreesPerSecond); } + + @Override + public void reset() { + gyroSimulation.setRotation(Rotation2d.kZero); + } } From 60acab84934a45dcecfb3f8abbb54fda9669aee8 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Fri, 22 Nov 2024 15:49:36 -0500 Subject: [PATCH 38/75] mucho progresso --- .../subsystems/vision/PhysicalVision.java | 133 ++++++------------ .../subsystems/vision/ThreadManager.java | 77 ++++++++++ .../subsystems/vision/VisionInterface.java | 6 + 3 files changed, 128 insertions(+), 88 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/vision/ThreadManager.java diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index bdf44da7..1224784b 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -26,6 +26,8 @@ public class PhysicalVision implements VisionInterface { new ConcurrentHashMap<>(); private final ExecutorService executorService = Executors.newFixedThreadPool(3); private final AtomicReference latestInputs = new AtomicReference<>(); + private final ThreadManager threadManager = new ThreadManager(3); + /** * The pose estimates from the limelights in the following order {shooterLimelight, @@ -35,11 +37,16 @@ public class PhysicalVision implements VisionInterface { new PoseEstimate[] {new PoseEstimate(), new PoseEstimate(), new PoseEstimate()}; public PhysicalVision() { - for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { - Limelight limelight = Limelight.values()[limelightNumber]; - limelightThreads.put(limelight, new AtomicReference<>(new VisionInputs())); - visionThread(limelight); - } + for (Limelight limelight : Limelight.values()) { + VisionInputs inputs = new VisionInputs(); // Create vision inputs for each Limelight + limelightThreads.put(limelight, new AtomicReference<>(inputs)); + + // Start a vision input task for each Limelight + threadManager.startVisionInputTask( + limelight.getName(), + inputs, + () -> visionThreadTask(limelight, inputs)); + } } @Override @@ -48,7 +55,6 @@ public void updateInputs(VisionInputs inputs) { (limelight, inputRef) -> { VisionInputs limelightInputs = inputRef.get(); // Combine inputs into the main inputs object - switch (limelight) { case SHOOTER -> { inputs.isShooterLimelightConnected = limelightInputs.isShooterLimelightConnected; @@ -335,7 +341,7 @@ public void checkAndUpdatePose(Limelight limelight) { // // Only stop the thread if it's currently running // if (isThreadRunning.get()) { // stop the thread for the specified limelight - stopThread(limelight); + stopLimelightThread(limelight); // } } last_TX = current_TX; @@ -347,97 +353,48 @@ public void checkAndUpdatePose(Limelight limelight) { } } - // /** - // * Starts a separate thread dedicated to updating the pose estimate for a specified limelight. - // * This approach is adopted to prevent loop overruns that would occur if we attempted to parse - // the - // * JSON dump for each limelight sequentially within a single scheduler loop. - // * - // *

    To achieve efficient and safe parallel execution, an ExecutorService is utilized to - // manage - // * the lifecycle of these threads. - // * - // *

    Each thread continuously runs the {@link #checkAndUpdatePose(int)} method as long as the - // * corresponding limelight's thread is marked as "running". This ensures that pose estimates - // are - // * updated in real-time, leveraging the parallel processing capabilities of the executor - // service. - // * - // * @param limelight the limelight number - // */ - // public void visionThread(Limelight limelight) { - - // executorService.submit( - // () -> { - // try { - // // while (limelightThreads.get(limelightNumber).get()) { - // checkAndUpdatePose(limelight); - // // } - // } catch (Exception e) { - // System.err.println( - // "Error executing task for the: " - // + limelight.getName() - // + ": " - // + e.getMessage()); - // } - // }); - // } - public void visionThread(Limelight limelight) { - executorService.submit( - () -> { - try { - while (!Thread.currentThread().isInterrupted()) { - synchronized (latestInputs) { - VisionInputs currentInputs = latestInputs.get(); - currentInputs.isShooterLimelightConnected = isLimelightConnected(limelight); - currentInputs.shooterMegaTag1Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); - currentInputs.shooterLatency = getLatencySeconds(limelight); - latestInputs.set(currentInputs); - } - Thread.sleep(VisionConstants.THREAD_SLEEP_MS); - } - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); // Restore interrupted status - } - }); - } - /** - * Sets the AtomicBoolean 'runningThreads' to false for the specified limelight. Stops the thread - * for the specified limelight. + * Starts a separate thread dedicated to updating the pose estimate for a specified limelight. + * This approach is adopted to prevent loop overruns that would occur if we attempted to parse the + * JSON dump for each limelight sequentially within a single scheduler loop. + * + *

    To achieve efficient and safe parallel execution, an ExecutorService is utilized to manage + * the lifecycle of these threads. + * + *

    Each thread continuously runs the {@link #checkAndUpdatePose(int)} method as long as the + * corresponding limelight's thread is marked as "running". This ensures that pose estimates are + * updated in real-time, leveraging the parallel processing capabilities of the executor service. * * @param limelight the limelight number */ - public void stopThread(Limelight limelight) { +private void visionThreadTask(Limelight limelight, VisionInputs inputs) { try { - // Since we can't see an April Tag, set the estimate for the specified limelight to an empty - // PoseEstimate() - limelightEstimates[limelight.getId()] = new PoseEstimate(); - // limelightThreads.get(limelight.id).set();(false); + synchronized (inputs) { + inputs.isShooterLimelightConnected = isLimelightConnected(limelight); + inputs.shooterMegaTag1Pose = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); + inputs.shooterLatency = getLatencySeconds(limelight); + + limelightThreads.get(limelight).set(inputs); + } } catch (Exception e) { - System.err.println( - "Error stopping thread for the: " + limelight.getName() + ": " + e.getMessage()); + System.err.println("Error updating inputs for " + limelight.getName() + ": " + e.getMessage()); } +} + + /** + * Sets the AtomicBoolean 'runningThreads' to false for the specified limelight. Stops the thread + * for the specified limelight. + * + * @param limelight the limelight number + */ + public void stopLimelightThread(Limelight limelight) { + threadManager.stopThread(limelight.getName()); + } /** Shuts down all the threads. */ + // @Override public void endAllThreads() { - // Properly shut down the executor service when the subsystem ends - executorService.shutdown(); // Prevents new tasks from being submitted - try { - // Wait for existing tasks to finish - if (!executorService.awaitTermination(5, TimeUnit.SECONDS)) { - executorService.shutdownNow(); - // Wait a bit longer for tasks to respond to being cancelled - if (!executorService.awaitTermination(5, TimeUnit.SECONDS)) - System.err.println("ExecutorService did not terminate"); - } - } catch (InterruptedException e) { - // (Re-)Cancel if current thread also interrupted - executorService.shutdownNow(); - // Preserve interrupt status - Thread.currentThread().interrupt(); - } + threadManager.shutdownAllThreads(); } } diff --git a/src/main/java/frc/robot/subsystems/vision/ThreadManager.java b/src/main/java/frc/robot/subsystems/vision/ThreadManager.java new file mode 100644 index 00000000..01d4da5c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/ThreadManager.java @@ -0,0 +1,77 @@ +package frc.robot.subsystems.vision; + +import java.util.Map; +import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; +import java.util.concurrent.Future; +import java.util.concurrent.TimeUnit; + +import frc.robot.subsystems.vision.VisionInterface.VisionInputs; + +public class ThreadManager { + private final ExecutorService executorService; + private final Map> threadFutures = new ConcurrentHashMap<>(); + + public ThreadManager(int maxThreads) { + executorService = Executors.newFixedThreadPool(maxThreads); + } + + // Start a thread for a specific task + public void startThread(String threadName, Runnable task) { + if (threadFutures.containsKey(threadName)) { + System.err.println("Thread " + threadName + " is already running."); + return; + } + + Future future = executorService.submit(() -> { + try { + task.run(); + } catch (Exception e) { + System.err.println("Error in thread " + threadName + ": " + e.getMessage()); + } + }); + + threadFutures.put(threadName, future); + } + + // Stop a specific thread + public void stopThread(String threadName) { + Future future = threadFutures.get(threadName); + if (future != null) { + future.cancel(true); + threadFutures.remove(threadName); + } else { + System.err.println("Thread " + threadName + " is not running."); + } + } + + // New method: Submit a vision input task + public void startVisionInputTask(String threadName, VisionInputs inputs, Runnable visionTask) { + startThread(threadName, () -> { + try { + while (!Thread.currentThread().isInterrupted()) { + visionTask.run(); + Thread.sleep(20); // Sleep to avoid excessive updates + } + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + }); + } + + // Shut down all threads + public void shutdownAllThreads() { + threadFutures.values().forEach(future -> future.cancel(true)); + threadFutures.clear(); + executorService.shutdown(); + try { + if (!executorService.awaitTermination(5, TimeUnit.SECONDS)) { + executorService.shutdownNow(); + } + } catch (InterruptedException e) { + executorService.shutdownNow(); + Thread.currentThread().interrupt(); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index 1841a9b5..43dbc7af 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -1,8 +1,14 @@ package frc.robot.subsystems.vision; import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; import frc.robot.extras.vision.MegatagPoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; + +import java.util.EnumMap; +import java.util.Map; +import java.util.concurrent.ConcurrentHashMap; + import org.littletonrobotics.junction.AutoLog; public interface VisionInterface { From 04fbd0fa3c593aaaf4753de3f30cec30c58514be Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Sat, 23 Nov 2024 08:13:34 -0500 Subject: [PATCH 39/75] fix gyro --- src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 032fb6f2..57b82bfc 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -207,7 +207,7 @@ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fi * @return Value is Counter-clockwise positive. */ public double getHeading() { - return -gyroInputs.yawDegrees; + return gyroInputs.yawDegrees; } /** @@ -216,7 +216,7 @@ public double getHeading() { * @return The current rate in degrees per second. */ public double getGyroRate() { - return -gyroInputs.yawVelocity; + return gyroInputs.yawVelocity; } /** Returns a Rotation2d for the heading of the robot. */ From afaa62cf69f13a11480b9eca821a7fe735517888 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Sun, 24 Nov 2024 16:44:16 -0500 Subject: [PATCH 40/75] i cooked --- .../subsystems/vision/PhysicalVision.java | 30 ++++++++++++++++--- .../subsystems/vision/SimulatedVision.java | 10 ++----- .../subsystems/vision/VisionInterface.java | 4 --- 3 files changed, 29 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 1224784b..63e94df9 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -60,17 +60,20 @@ public void updateInputs(VisionInputs inputs) { inputs.isShooterLimelightConnected = limelightInputs.isShooterLimelightConnected; inputs.shooterMegaTag1Pose = limelightInputs.shooterMegaTag1Pose; inputs.shooterLatency = limelightInputs.shooterLatency; + inputs.shooterMegaTag2Pose = limelightInputs.shooterMegaTag2Pose; } case FRONT_LEFT -> { inputs.isFrontLeftLimelightConnected = limelightInputs.isFrontLeftLimelightConnected; inputs.frontLeftMegaTag1Pose = limelightInputs.frontLeftMegaTag1Pose; inputs.frontLeftLatency = limelightInputs.frontLeftLatency; + inputs.frontLeftMegaTag2Pose = limelightInputs.frontLeftMegaTag2Pose; } case FRONT_RIGHT -> { inputs.isFrontRightLimelightConnected = limelightInputs.isFrontRightLimelightConnected; inputs.frontRightMegaTag1Pose = limelightInputs.frontRightMegaTag1Pose; inputs.frontRightLatency = limelightInputs.frontRightLatency; + inputs.frontRightMegaTag2Pose = limelightInputs.frontRightMegaTag2Pose; } } }); @@ -292,7 +295,7 @@ public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { public boolean isLimelightConnected(Limelight limelight) { NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(limelight.getName()); - if (limelightTable.containsKey("tx")) { + if (limelightTable.containsKey(limelight.getName())) { return true; } return false; @@ -370,10 +373,29 @@ public void checkAndUpdatePose(Limelight limelight) { private void visionThreadTask(Limelight limelight, VisionInputs inputs) { try { synchronized (inputs) { - inputs.isShooterLimelightConnected = isLimelightConnected(limelight); - inputs.shooterMegaTag1Pose = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); - inputs.shooterLatency = getLatencySeconds(limelight); + switch (limelight) { + case SHOOTER -> { + + inputs.isShooterLimelightConnected = isLimelightConnected(Limelight.SHOOTER); + inputs.shooterMegaTag1Pose = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.SHOOTER)); + inputs.shooterLatency = getLatencySeconds(Limelight.SHOOTER); + inputs.shooterMegaTag2Pose = MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.SHOOTER)); + } + case FRONT_LEFT -> { + + inputs.isFrontLeftLimelightConnected = isLimelightConnected(Limelight.FRONT_LEFT); + inputs.frontLeftMegaTag1Pose = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.FRONT_LEFT)); + inputs.frontLeftLatency = getLatencySeconds(Limelight.FRONT_LEFT); + inputs.frontLeftMegaTag2Pose = MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.FRONT_LEFT)); + } + case FRONT_RIGHT -> { + inputs.isFrontRightLimelightConnected = isLimelightConnected(Limelight.FRONT_RIGHT); + inputs.frontRightMegaTag1Pose = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.FRONT_RIGHT)); + inputs.frontRightLatency = getLatencySeconds(Limelight.FRONT_RIGHT); + inputs.frontRightMegaTag2Pose = MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.FRONT_RIGHT)); + } + } limelightThreads.get(limelight).set(inputs); } } catch (Exception e) { diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 6adb3a5e..d38fb37b 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -95,11 +95,11 @@ public void updateInputs(VisionInputs inputs) { } NetworkTable shooterTable = - NetworkTableInstance.getDefault().getTable(super.getLimelightName(Limelight.SHOOTER)); + NetworkTableInstance.getDefault().getTable(Limelight.SHOOTER.getName()); NetworkTable frontLeftTable = - NetworkTableInstance.getDefault().getTable(super.getLimelightName(Limelight.FRONT_LEFT)); + NetworkTableInstance.getDefault().getTable(Limelight.FRONT_LEFT.getName()); NetworkTable frontRightTable = - NetworkTableInstance.getDefault().getTable(super.getLimelightName(Limelight.FRONT_RIGHT)); + NetworkTableInstance.getDefault().getTable(Limelight.FRONT_RIGHT.getName()); // Write to limelight table writeToTable(shooterCamera.getAllUnreadResults(), shooterTable, Limelight.SHOOTER); writeToTable(frontLeftCamera.getAllUnreadResults(), frontLeftTable, Limelight.FRONT_LEFT); @@ -157,10 +157,6 @@ private void writeToTable( } } - public String getLimelightName(Limelight limelight) { - return super.getLimelightName(limelight); - } - public double getLatencySeconds(Limelight limelight) { return super.getLatencySeconds(limelight); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index 43dbc7af..3a69bd4a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -50,10 +50,6 @@ class VisionInputs { default void updateInputs(VisionInputs inputs) {} - default String getLimelightName(Limelight limelight) { - return ""; - } - default double getLatencySeconds(Limelight limelight) { return 0.0; } From f9d913430865ce9fd22188faa644f43cf8fb6367 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Sun, 24 Nov 2024 16:55:45 -0500 Subject: [PATCH 41/75] format --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../java/frc/robot/extras/util/GeomUtil.java | 3 + .../robot/subsystems/swerve/SwerveDrive.java | 13 +- .../subsystems/swerve/gyro/GyroInterface.java | 4 +- .../subsystems/swerve/gyro/SimulatedGyro.java | 1 - .../subsystems/vision/PhysicalVision.java | 64 +++-- .../subsystems/vision/SimulatedVision.java | 2 +- .../subsystems/vision/ThreadManager.java | 109 ++++----- .../frc/robot/subsystems/vision/Vision.java | 222 ------------------ .../subsystems/vision/VisionInterface.java | 6 - 10 files changed, 97 insertions(+), 331 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 890b952a..348f7131 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -106,7 +106,9 @@ public RobotContainer() { new SimulatedModule(swerveDriveSimulation.getModules()[2]), new SimulatedModule(swerveDriveSimulation.getModules()[3])); - visionSubsystem = new Vision(new SimulatedVision(swerveDrive::getPose)); + visionSubsystem = + new Vision( + new SimulatedVision(() -> swerveDriveSimulation.getSimulatedDriveTrainPose())); SimulatedField.getInstance().resetFieldForAuto(); resetFieldAndOdometryForAuto( diff --git a/src/main/java/frc/robot/extras/util/GeomUtil.java b/src/main/java/frc/robot/extras/util/GeomUtil.java index f52008a5..017c98ed 100644 --- a/src/main/java/frc/robot/extras/util/GeomUtil.java +++ b/src/main/java/frc/robot/extras/util/GeomUtil.java @@ -52,6 +52,7 @@ public static ChassisSpeeds toWpilibChassisSpeeds( /** * Gets the x and y velocities of a ChassisSpeeds + * * @param chassisSpeeds the ChassisSpeeds to retrieve velocities from * @return a Translation2d containing the velocities in the x and y direction in meters per second */ @@ -61,6 +62,7 @@ public static Translation2d getChassisSpeedsTranslationalComponent(ChassisSpeeds /** * Checks if two translations are within a certain threshold in meters + * * @param t1 Translation 1 as a Translation2d * @param t2 Translation 2 as a Translation2d * @param thresholdMeters the threshold between the two translations in meters @@ -73,6 +75,7 @@ public static boolean isTranslationWithinThreshold( /** * Checks if two rotations are within a certain threshold in degrees + * * @param r1 Rotations 1 as a Rotation2d * @param r2 Rotation 2 as a Rotation2d * @param thresholdMeters the threshold between the two rotations in degrees diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 57b82bfc..9c38bfd9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -13,8 +13,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.extras.util.DeviceCANBus; import frc.robot.extras.util.TimeUtil; @@ -186,10 +184,7 @@ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fi // ChassisSpeeds.discretize( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, - ySpeed, - rotationSpeed, - getOdometryAllianceRelativeRotation2d()) + xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d()) : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); // , 0.02); SwerveModuleState[] swerveModuleStates = @@ -265,7 +260,6 @@ public Rotation2d getOdometryAllianceRelativeRotation2d() { return getPose().getRotation().plus(Rotation2d.fromDegrees(getAllianceAngleOffset())); } - /** * Sets the modules to the specified states. * @@ -297,7 +291,7 @@ public void addPoseEstimatorSwerveMeasurement() { odometry.updateWithTime( // odometryThreadInputs.measurementTimeStamps[timestampIndex], - Timer.getFPGATimestamp(), rawGyroRotation, modulePositions); + Logger.getTimestamp(), rawGyroRotation, modulePositions); } /** @@ -349,9 +343,6 @@ private SwerveModulePosition[] getModulePositions() { * @param pose pose to set */ public void resetPosition(Pose2d pose) { - // for (int timestampIndex = 0; - // timestampIndex < odometryThreadInputs.measurementTimeStamps.length; - // timestampIndex++) odometry.resetPosition(getGyroRotation2d(), getModulePositions(), pose); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java index b3ac4b82..2d45260e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java @@ -23,8 +23,6 @@ public static class GyroInputs { */ default void updateInputs(GyroInputs inputs) {} - /** - * Resets the gyro yaw - */ + /** Resets the gyro yaw */ default void reset() {} } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java index dbd99c9b..4197e452 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java @@ -2,7 +2,6 @@ import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Rotation; import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.extras.simulation.OdometryTimestampsSim; diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 63e94df9..44b44108 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.vision; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants.FieldConstants; @@ -14,7 +13,6 @@ import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.ExecutorService; import java.util.concurrent.Executors; -import java.util.concurrent.TimeUnit; import java.util.concurrent.atomic.AtomicReference; public class PhysicalVision implements VisionInterface { @@ -28,7 +26,6 @@ public class PhysicalVision implements VisionInterface { private final AtomicReference latestInputs = new AtomicReference<>(); private final ThreadManager threadManager = new ThreadManager(3); - /** * The pose estimates from the limelights in the following order {shooterLimelight, * frontLeftLimelight, frontRightLimelight} @@ -43,10 +40,8 @@ public PhysicalVision() { // Start a vision input task for each Limelight threadManager.startVisionInputTask( - limelight.getName(), - inputs, - () -> visionThreadTask(limelight, inputs)); - } + limelight.getName(), inputs, () -> visionThreadTask(limelight, inputs)); + } } @Override @@ -370,38 +365,42 @@ public void checkAndUpdatePose(Limelight limelight) { * * @param limelight the limelight number */ -private void visionThreadTask(Limelight limelight, VisionInputs inputs) { + private void visionThreadTask(Limelight limelight, VisionInputs inputs) { try { synchronized (inputs) { switch (limelight) { case SHOOTER -> { - - inputs.isShooterLimelightConnected = isLimelightConnected(Limelight.SHOOTER); - inputs.shooterMegaTag1Pose = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.SHOOTER)); - inputs.shooterLatency = getLatencySeconds(Limelight.SHOOTER); - inputs.shooterMegaTag2Pose = MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.SHOOTER)); + inputs.isShooterLimelightConnected = isLimelightConnected(Limelight.SHOOTER); + inputs.shooterMegaTag1Pose = + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.SHOOTER)); + inputs.shooterLatency = getLatencySeconds(Limelight.SHOOTER); + inputs.shooterMegaTag2Pose = + MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.SHOOTER)); + } + case FRONT_LEFT -> { + inputs.isFrontLeftLimelightConnected = isLimelightConnected(Limelight.FRONT_LEFT); + inputs.frontLeftMegaTag1Pose = + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.FRONT_LEFT)); + inputs.frontLeftLatency = getLatencySeconds(Limelight.FRONT_LEFT); + inputs.frontLeftMegaTag2Pose = + MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.FRONT_LEFT)); + } + case FRONT_RIGHT -> { + inputs.isFrontRightLimelightConnected = isLimelightConnected(Limelight.FRONT_RIGHT); + inputs.frontRightMegaTag1Pose = + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.FRONT_RIGHT)); + inputs.frontRightLatency = getLatencySeconds(Limelight.FRONT_RIGHT); + inputs.frontRightMegaTag2Pose = + MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.FRONT_RIGHT)); } - case FRONT_LEFT -> { - - inputs.isFrontLeftLimelightConnected = isLimelightConnected(Limelight.FRONT_LEFT); - inputs.frontLeftMegaTag1Pose = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.FRONT_LEFT)); - inputs.frontLeftLatency = getLatencySeconds(Limelight.FRONT_LEFT); - inputs.frontLeftMegaTag2Pose = MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.FRONT_LEFT)); - } - case FRONT_RIGHT -> { - inputs.isFrontRightLimelightConnected = isLimelightConnected(Limelight.FRONT_RIGHT); - inputs.frontRightMegaTag1Pose = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.FRONT_RIGHT)); - inputs.frontRightLatency = getLatencySeconds(Limelight.FRONT_RIGHT); - inputs.frontRightMegaTag2Pose = MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.FRONT_RIGHT)); - - } } limelightThreads.get(limelight).set(inputs); } } catch (Exception e) { - System.err.println("Error updating inputs for " + limelight.getName() + ": " + e.getMessage()); + System.err.println( + "Error updating inputs for " + limelight.getName() + ": " + e.getMessage()); } -} + } /** * Sets the AtomicBoolean 'runningThreads' to false for the specified limelight. Stops the thread @@ -409,14 +408,13 @@ private void visionThreadTask(Limelight limelight, VisionInputs inputs) { * * @param limelight the limelight number */ - public void stopLimelightThread(Limelight limelight) { - threadManager.stopThread(limelight.getName()); - + public void stopLimelightThread(Limelight limelight) { + threadManager.stopThread(limelight.getName()); } /** Shuts down all the threads. */ // @Override public void endAllThreads() { - threadManager.shutdownAllThreads(); + threadManager.shutdownAllThreads(); } } diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index d38fb37b..7bce96ca 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -178,7 +178,7 @@ public int getNumberOfAprilTags(Limelight limelight) { } public Pose2d getPoseFromAprilTags(Limelight limelight) { - return getPoseFromAprilTags(limelight); + return super.getPoseFromAprilTags(limelight); } public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { diff --git a/src/main/java/frc/robot/subsystems/vision/ThreadManager.java b/src/main/java/frc/robot/subsystems/vision/ThreadManager.java index 01d4da5c..83da90cd 100644 --- a/src/main/java/frc/robot/subsystems/vision/ThreadManager.java +++ b/src/main/java/frc/robot/subsystems/vision/ThreadManager.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.vision; +import frc.robot.subsystems.vision.VisionInterface.VisionInputs; import java.util.Map; import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.ExecutorService; @@ -7,71 +8,73 @@ import java.util.concurrent.Future; import java.util.concurrent.TimeUnit; -import frc.robot.subsystems.vision.VisionInterface.VisionInputs; - public class ThreadManager { - private final ExecutorService executorService; - private final Map> threadFutures = new ConcurrentHashMap<>(); + private final ExecutorService executorService; + private final Map> threadFutures = new ConcurrentHashMap<>(); - public ThreadManager(int maxThreads) { - executorService = Executors.newFixedThreadPool(maxThreads); - } + public ThreadManager(int maxThreads) { + executorService = Executors.newFixedThreadPool(maxThreads); + } - // Start a thread for a specific task - public void startThread(String threadName, Runnable task) { - if (threadFutures.containsKey(threadName)) { - System.err.println("Thread " + threadName + " is already running."); - return; - } + // Start a thread for a specific task + public void startThread(String threadName, Runnable task) { + if (threadFutures.containsKey(threadName)) { + System.err.println("Thread " + threadName + " is already running."); + return; + } - Future future = executorService.submit(() -> { - try { + Future future = + executorService.submit( + () -> { + try { task.run(); - } catch (Exception e) { + } catch (Exception e) { System.err.println("Error in thread " + threadName + ": " + e.getMessage()); - } - }); + } + }); - threadFutures.put(threadName, future); - } + threadFutures.put(threadName, future); + } - // Stop a specific thread - public void stopThread(String threadName) { - Future future = threadFutures.get(threadName); - if (future != null) { - future.cancel(true); - threadFutures.remove(threadName); - } else { - System.err.println("Thread " + threadName + " is not running."); - } + // Stop a specific thread + public void stopThread(String threadName) { + Future future = threadFutures.get(threadName); + if (future != null) { + future.cancel(true); + threadFutures.remove(threadName); + } else { + System.err.println("Thread " + threadName + " is not running."); } + } - // New method: Submit a vision input task - public void startVisionInputTask(String threadName, VisionInputs inputs, Runnable visionTask) { - startThread(threadName, () -> { - try { - while (!Thread.currentThread().isInterrupted()) { - visionTask.run(); - Thread.sleep(20); // Sleep to avoid excessive updates - } - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); + // New method: Submit a vision input task + public void startVisionInputTask(String threadName, VisionInputs inputs, Runnable visionTask) { + startThread( + threadName, + () -> { + try { + while (!Thread.currentThread().isInterrupted()) { + visionTask.run(); + Thread.sleep(20); // Sleep to avoid excessive updates } + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } }); - } + } - // Shut down all threads - public void shutdownAllThreads() { - threadFutures.values().forEach(future -> future.cancel(true)); - threadFutures.clear(); - executorService.shutdown(); - try { - if (!executorService.awaitTermination(5, TimeUnit.SECONDS)) { - executorService.shutdownNow(); - } - } catch (InterruptedException e) { - executorService.shutdownNow(); - Thread.currentThread().interrupt(); - } + // Shut down all threads + public void shutdownAllThreads() { + threadFutures.values().forEach(future -> future.cancel(true)); + threadFutures.clear(); + executorService.shutdown(); + try { + if (!executorService.awaitTermination(5, TimeUnit.SECONDS)) { + executorService.shutdownNow(); + } + } catch (InterruptedException e) { + executorService.shutdownNow(); + Thread.currentThread().interrupt(); } + } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 53a0a4e9..788837ab 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -24,228 +24,6 @@ public void periodic() { Logger.processInputs("Vision/", inputs); } - // private void updateVision( - // MegatagPoseEstimate cameraMegatagPoseEstimate, - // MegatagPoseEstimate cameraMegatag2PoseEstimate, - // Limelight limelightName) { - // if (cameraMegatagPoseEstimate != null) { - - // String logPreface = "Vision/" + visionInterface.getLimelightName(limelightName); - // var updateTimestamp = cameraMegatagPoseEstimate.timestampSeconds; - // boolean alreadyProcessedTimestamp = - // visionInterface.getTimeStampSeconds(limelightName) == updateTimestamp; - // if (!alreadyProcessedTimestamp && canSeeAprilTags(limelightName)) { - - // Optional megatagEstimate = - // processMegatagPoseEstimate(cameraMegatagPoseEstimate); - // Optional megatag2Estimate = - // processMegatag2PoseEstimate(cameraMegatag2PoseEstimate, logPreface); - - // boolean used_megatag = false; - // if (megatagEstimate.isPresent()) { - // if (shouldUseMegatag( - // cameraMegatagPoseEstimate, cameraFiducialObservations, isTurretCamera, logPreface)) - // { - // Logger.recordOutput( - // logPreface + "MegatagEstimate", - // megatagEstimate.get().getVisionRobotPoseMeters()); - // state.updateMegatagEstimate(megatagEstimate.get()); - // used_megatag = true; - // } else { - // if (megatagEstimate.isPresent()) { - // Logger.recordOutput( - // logPreface + "MegatagEstimateRejected", - // megatagEstimate.get().getVisionRobotPoseMeters()); - // } - // } - // } - - // if (megatag2Estimate.isPresent() && !used_megatag) { - // if (shouldUseMegatag2(cameraMegatag2PoseEstimate, logPreface)) { - // Logger.recordOutput( - // logPreface + "Megatag2Estimate", - // megatag2Estimate.get().getVisionRobotPoseMeters()); - // state.updateMegatagEstimate(megatag2Estimate.get()); - // } else { - // if (megatagEstimate.isPresent()) { - // Logger.recordOutput( - // logPreface + "Megatag2EstimateRejected", - // megatag2Estimate.get().getVisionRobotPoseMeters()); - // } - // } - // } - // } - // } - // } - - // private Optional getFieldToRobotEstimate( - // MegatagPoseEstimate poseEstimate, boolean isTurretCamera) { - // var fieldToCamera = poseEstimate.fieldToCamera; - // if (fieldToCamera.getX() == 0.0) { - // return Optional.empty(); - // } - // var cameraToTurretTransform = turretToCameraTransform.inverse(); - // var fieldToTurretPose = fieldToCamera.plus(cameraToTurretTransform); - // var fieldToRobotEstimate = Pose2d.kZero; - // if (isTurretCamera) { - // var robotToTurretObservation = state.getRobotToTurret(poseEstimate.timestampSeconds); - // if (robotToTurretObservation.isEmpty()) { - // return Optional.empty(); - // } - // var turretToRobot = - // MathHelpers.transform2dFromRotation(robotToTurretObservation.get().unaryMinus()); - // fieldToRobotEstimate = fieldToTurretPose.plus(turretToRobot); - // } else { - // fieldToRobotEstimate = fieldToCamera.plus(turretToCameraTransform.inverse()); - // } - - // return Optional.of(fieldToRobotEstimate); - // } - - // private Optional processMegatag2PoseEstimate( - // MegatagPoseEstimate poseEstimate, String logPreface) { - // var loggedFieldToRobot = state.getFieldToRobot(poseEstimate.timestampSeconds); - // if (loggedFieldToRobot.isEmpty()) { - // return Optional.empty(); - // } - - // var maybeFieldToRobotEstimate = getFieldToRobotEstimate(poseEstimate, isTurretCamera); - // if (maybeFieldToRobotEstimate.isEmpty()) return Optional.empty(); - // var fieldToRobotEstimate = maybeFieldToRobotEstimate.get(); - - // // distance from current pose to vision estimated pose - // double poseDifference = - // fieldToRobotEstimate - // .getTranslation() - // .getDistance(loggedFieldToRobot.get().getTranslation()); - - // var defaultSet = state.isRedAlliance() ? kTagsRedSpeaker : kTagsBlueSpeaker; - // Set speakerTags = new HashSet<>(defaultSet); - // speakerTags.removeAll( - // Arrays.stream(poseEstimate.fiducialIds) - // .boxed() - // .collect(Collectors.toCollection(HashSet::new))); - // boolean seesSpeakerTags = speakerTags.size() < 2; - - // double xyStds; - // if (poseEstimate.fiducialIds.length > 0) { - // // multiple targets detected - // if (poseEstimate.fiducialIds.length >= 2 && poseEstimate.avgTagArea > 0.1) { - // xyStds = 0.2; - // } - // // we detect at least one of our speaker tags and we're close to it. - // else if (seesSpeakerTags && poseEstimate.avgTagArea > 0.2) { - // xyStds = 0.5; - // } - // // 1 target with large area and close to estimated pose - // else if (poseEstimate.avgTagArea > 0.8 && poseDifference < 0.5) { - // xyStds = 0.5; - // } - // // 1 target farther away and estimated pose is close - // else if (poseEstimate.avgTagArea > 0.1 && poseDifference < 0.3) { - // xyStds = 1.0; - // } else if (poseEstimate.fiducialIds.length > 1) { - // xyStds = 1.2; - // } else { - // xyStds = 2.0; - // } - - // Logger.recordOutput(logPreface + "Megatag2StdDev", xyStds); - // Logger.recordOutput(logPreface + "Megatag2AvgTagArea", poseEstimate.avgTagArea); - // Logger.recordOutput(logPreface + "Megatag2PoseDifference", poseDifference); - - // Matrix visionMeasurementStdDevs = - // VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(50.0)); - // fieldToRobotEstimate = - // new Pose2d(fieldToRobotEstimate.getTranslation(), - // loggedFieldToRobot.get().getRotation()); - // return Optional.of( - // new VisionFieldPoseEstimate( - // fieldToRobotEstimate, poseEstimate.timestampSeconds, visionMeasurementStdDevs)); - // } - // return Optional.empty(); - // } - - // private Optional processMegatagPoseEstimate( - // MegatagPoseEstimate poseEstimate, boolean isTurretCamera) { - // var loggedFieldToRobot = state.getFieldToRobot(poseEstimate.timestampSeconds); - // if (loggedFieldToRobot.isEmpty()) { - // return Optional.empty(); - // } - - // var maybeFieldToRobotEstimate = getFieldToRobotEstimate(poseEstimate, isTurretCamera); - // if (maybeFieldToRobotEstimate.isEmpty()) return Optional.empty(); - // var fieldToRobotEstimate = maybeFieldToRobotEstimate.get(); - - // // distance from current pose to vision estimated pose - // double poseDifference = - // fieldToRobotEstimate - // .getTranslation() - // .getDistance(loggedFieldToRobot.get().getTranslation()); - - // if (poseEstimate.fiducialIds.length > 0) { - // double xyStds = 1.0; - // double degStds = 12; - // // multiple targets detected - // if (poseEstimate.fiducialIds.length >= 2) { - // xyStds = 0.5; - // degStds = 6; - // } - // // 1 target with large area and close to estimated pose - // else if (poseEstimate.avgTagArea > 0.8 && poseDifference < 0.5) { - // xyStds = 1.0; - // degStds = 12; - // } - // // 1 target farther away and estimated pose is close - // else if (poseEstimate.avgTagArea > 0.1 && poseDifference < 0.3) { - // xyStds = 2.0; - // degStds = 30; - // } - - // Matrix visionMeasurementStdDevs = - // VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds)); - // return Optional.of( - // new VisionFieldPoseEstimate( - // fieldToRobotEstimate, poseEstimate.timestampSeconds, visionMeasurementStdDevs)); - // } - // return Optional.empty(); - // } - - // private Pose2d estimateFieldToRobot( - // Translation2d cameraToTarget, - // Pose3d fieldToTarget, - // Rotation2d robotToTurret, - // Rotation2d gyroAngle, - // Rotation2d cameraYawOffset, - // boolean isTurretCamera) { - // Transform2d cameraToTargetFixed = - // MathHelpers.transform2dFromTranslation(cameraToTarget.rotateBy(cameraYawOffset)); - // Transform2d turretToTarget = - // state.getTurretToCamera(isTurretCamera).plus(cameraToTargetFixed); - // // In robot frame - // Transform2d robotToTarget = turretToTarget; - // if (isTurretCamera) { - // robotToTarget = MathHelpers.transform2dFromRotation(robotToTurret).plus(turretToTarget); - // } - - // // In field frame - // Transform2d robotToTargetField = - // - // MathHelpers.transform2dFromTranslation(robotToTarget.getTranslation().rotateBy(gyroAngle)); - - // // In field frame - // Pose2d fieldToTarget2d = - // MathHelpers.pose2dFromTranslation(fieldToTarget.toPose2d().getTranslation()); - - // Pose2d fieldToRobot = - // fieldToTarget2d.transformBy( - // MathHelpers.transform2dFromTranslation( - // robotToTargetField.getTranslation().unaryMinus())); - - // Pose2d fieldToRobotYawAdjusted = new Pose2d(fieldToRobot.getTranslation(), gyroAngle); - // return fieldToRobotYawAdjusted; - // } - // Add methods to support DriveCommand public int getNumberOfAprilTags(Limelight limelight) { return visionInterface.getNumberOfAprilTags(limelight); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index 3a69bd4a..f9c43eac 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -1,14 +1,8 @@ package frc.robot.subsystems.vision; import edu.wpi.first.math.geometry.Pose2d; -import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; import frc.robot.extras.vision.MegatagPoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; - -import java.util.EnumMap; -import java.util.Map; -import java.util.concurrent.ConcurrentHashMap; - import org.littletonrobotics.junction.AutoLog; public interface VisionInterface { From 8733a45f32e48f5d445dc4c7e06b79040222ee07 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 25 Nov 2024 08:17:23 -0500 Subject: [PATCH 42/75] i is hyped --- .../robot/subsystems/swerve/SwerveDrive.java | 21 ++++++++++--------- .../subsystems/vision/PhysicalVision.java | 10 ++++----- .../subsystems/vision/SimulatedVision.java | 6 ++++-- 3 files changed, 20 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 9c38bfd9..084525ca 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -90,11 +90,11 @@ public SwerveDrive( } // /** Updates the pose estimator with the pose calculated from the swerve modules. */ - // public void addPoseEstimatorSwerveMeasurement() { - // for (int timestampIndex = 0; - // timestampIndex < odometryThreadInputs.measurementTimeStamps.length; - // timestampIndex++) addPoseEstimatorSwerveMeasurement(timestampIndex); - // } + public void addPoseEstimatorSwerveMeasurement() { + for (int timestampIndex = 0; + timestampIndex < odometryThreadInputs.measurementTimeStamps.length; + timestampIndex++) addPoseEstimatorSwerveMeasurement(timestampIndex); + } /* * Updates the pose estimator with the pose calculated from the april tags. How much it @@ -277,21 +277,22 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { * * @param timestampIndex index of the timestamp to sample the pose at */ - public void addPoseEstimatorSwerveMeasurement() { + public void addPoseEstimatorSwerveMeasurement(int timestampIndex) { final SwerveModulePosition[] modulePositions = getModulePositions(), moduleDeltas = getModulesDelta(modulePositions); if (gyroInputs.isConnected) { - // rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; - rawGyroRotation = getGyroRotation2d(); + rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; + // rawGyroRotation = getGyroRotation2d(); } else { Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas); rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } odometry.updateWithTime( - // odometryThreadInputs.measurementTimeStamps[timestampIndex], - Logger.getTimestamp(), rawGyroRotation, modulePositions); + odometryThreadInputs.measurementTimeStamps[timestampIndex], + // Logger.getTimestamp(), + rawGyroRotation, modulePositions); } /** diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 44b44108..775ac1b1 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -324,7 +324,7 @@ public void checkAndUpdatePose(Limelight limelight) { if (current_TX != last_TX || current_TY != last_TY) { updatePoseEstimate(limelight); limelightThreads.computeIfPresent( - limelight, (key, value) -> new AtomicReference<>(new VisionInputs())); + limelight, (key, value) -> latestInputs.get()); // This is to keep track of the last valid pose calculated by the limelights // it is used when the driver resets the robot odometry to the limelight calculated // position @@ -333,14 +333,14 @@ public void checkAndUpdatePose(Limelight limelight) { } } else { // Retrieve the AtomicBoolean for the given limelight number - // AtomicReference isThreadRunning = - // limelightThreads.getOrDefault(limelight, new AtomicReference<>(new - // VisionInputs())); + AtomicReference isThreadRunning = + limelightThreads.getOrDefault(limelight, new AtomicReference<>(new + VisionInputs())); // // Only stop the thread if it's currently running // if (isThreadRunning.get()) { // stop the thread for the specified limelight stopLimelightThread(limelight); - // } + } } last_TX = current_TX; last_TY = current_TY; diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 7bce96ca..22b63f63 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -10,6 +10,7 @@ import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; import org.photonvision.PhotonCamera; +import org.photonvision.estimation.TargetModel; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; @@ -40,6 +41,7 @@ public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { // Add all the AprilTags inside the tag layout as visible targets to this // simulated field. visionSim.addAprilTags(VisionConstants.FIELD_LAYOUT); + // visionSim.addVisionTargets(TargetModel.kAprilTag36h11); // Create simulated camera properties. These can be set to mimic your actual // camera. @@ -122,8 +124,8 @@ private void writeToTable( best.getX(), // 0: X best.getY(), // 1: Y best.getZ(), // 2: Z, - 0.0, // 3: roll - 0.0, // 4: pitch + best.getRotation().getX(), // 3: roll + best.getRotation().getY(), // 4: pitch fieldToCamera.getRotation().getDegrees(), // 5: yaw result.metadata.getLatencyMillis(), // 6: latency ms, (double) From ee3f22ee942cd01988b53efa218d4264962deb61 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 25 Nov 2024 13:35:19 -0500 Subject: [PATCH 43/75] wtf --- .../commands/drive/DriveCommandBase.java | 8 +- .../robot/extras/vision/LimelightHelpers.java | 2 +- .../robot/subsystems/swerve/SwerveDrive.java | 2 +- .../subsystems/vision/PhysicalVision.java | 139 +++++++++--------- .../subsystems/vision/SimulatedVision.java | 1 + .../subsystems/vision/ThreadManager.java | 4 +- .../frc/robot/subsystems/vision/Vision.java | 14 +- .../subsystems/vision/VisionInterface.java | 41 ++---- 8 files changed, 103 insertions(+), 108 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java index 65a4980d..b47bdfdf 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java @@ -2,6 +2,8 @@ package frc.robot.commands.drive; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.extras.interpolators.MultiLinearInterpolator; @@ -70,9 +72,13 @@ public void calculatePoseFromLimelight(Limelight limelight) { swerveDrive.addPoseEstimatorVisionMeasurement( vision.getPoseFromAprilTags(limelight), - Timer.getFPGATimestamp() - vision.getLatencySeconds(limelight)); + Logger.getTimestamp() - vision.getLatencySeconds(limelight)); } + + Logger.recordOutput("Odometry/CurrentVisionPose" + limelight.getName(), vision.getPoseFromAprilTags(limelight)); + Logger.recordOutput("Odometry/CurrentCalculatePose", swerveDrive.getPose()); + lastTimeStampSeconds = currentTimeStampSeconds; } } diff --git a/src/main/java/frc/robot/extras/vision/LimelightHelpers.java b/src/main/java/frc/robot/extras/vision/LimelightHelpers.java index e588d637..a7ddf38b 100644 --- a/src/main/java/frc/robot/extras/vision/LimelightHelpers.java +++ b/src/main/java/frc/robot/extras/vision/LimelightHelpers.java @@ -516,7 +516,7 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); } - private static void printPoseEstimate(PoseEstimate pose) { + public static void printPoseEstimate(PoseEstimate pose) { if (pose == null) { System.out.println("No PoseEstimate available."); return; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 084525ca..0b578c37 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -241,7 +241,7 @@ public void zeroHeading() { * Returns the estimated field-relative pose of the robot. Positive x being forward, positive y * being left. */ - @AutoLogOutput(key = "Odometry/RobotPosition") + @AutoLogOutput(key = "Odometry/Odometry") public Pose2d getPose() { return odometry.getEstimatedPosition(); } diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 775ac1b1..f93abf76 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -9,10 +9,10 @@ import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; import frc.robot.extras.vision.MegatagPoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; -import frc.robot.subsystems.vision.VisionInterface.VisionInputs; +// import frc.robot.subsystems.vision.VisionInterface.VisionInputs; import java.util.concurrent.ConcurrentHashMap; -import java.util.concurrent.ExecutorService; -import java.util.concurrent.Executors; +// import java.util.concurrent.ExecutorService; +// import java.util.concurrent.Executors; import java.util.concurrent.atomic.AtomicReference; public class PhysicalVision implements VisionInterface { @@ -22,22 +22,28 @@ public class PhysicalVision implements VisionInterface { private double headingRateDegreesPerSecond = 0; private final ConcurrentHashMap> limelightThreads = new ConcurrentHashMap<>(); - private final ExecutorService executorService = Executors.newFixedThreadPool(3); + // private final ExecutorService executorService = Executors.newFixedThreadPool(3); private final AtomicReference latestInputs = new AtomicReference<>(); - private final ThreadManager threadManager = new ThreadManager(3); + private final ThreadManager threadManager = new ThreadManager(Limelight.values().length); /** * The pose estimates from the limelights in the following order {shooterLimelight, * frontLeftLimelight, frontRightLimelight} */ private PoseEstimate[] limelightEstimates = - new PoseEstimate[] {new PoseEstimate(), new PoseEstimate(), new PoseEstimate()}; + new PoseEstimate[Limelight.values().length]; public PhysicalVision() { for (Limelight limelight : Limelight.values()) { VisionInputs inputs = new VisionInputs(); // Create vision inputs for each Limelight limelightThreads.put(limelight, new AtomicReference<>(inputs)); + + // Initialize limelightEstimates with default PoseEstimate objects + limelightEstimates[limelight.getId()] = new PoseEstimate(); + + + // Start a vision input task for each Limelight threadManager.startVisionInputTask( limelight.getName(), inputs, () -> visionThreadTask(limelight, inputs)); @@ -50,27 +56,25 @@ public void updateInputs(VisionInputs inputs) { (limelight, inputRef) -> { VisionInputs limelightInputs = inputRef.get(); // Combine inputs into the main inputs object - switch (limelight) { - case SHOOTER -> { - inputs.isShooterLimelightConnected = limelightInputs.isShooterLimelightConnected; - inputs.shooterMegaTag1Pose = limelightInputs.shooterMegaTag1Pose; - inputs.shooterLatency = limelightInputs.shooterLatency; - inputs.shooterMegaTag2Pose = limelightInputs.shooterMegaTag2Pose; - } - case FRONT_LEFT -> { - inputs.isFrontLeftLimelightConnected = limelightInputs.isFrontLeftLimelightConnected; - inputs.frontLeftMegaTag1Pose = limelightInputs.frontLeftMegaTag1Pose; - inputs.frontLeftLatency = limelightInputs.frontLeftLatency; - inputs.frontLeftMegaTag2Pose = limelightInputs.frontLeftMegaTag2Pose; - } - case FRONT_RIGHT -> { - inputs.isFrontRightLimelightConnected = - limelightInputs.isFrontRightLimelightConnected; - inputs.frontRightMegaTag1Pose = limelightInputs.frontRightMegaTag1Pose; - inputs.frontRightLatency = limelightInputs.frontRightLatency; - inputs.frontRightMegaTag2Pose = limelightInputs.frontRightMegaTag2Pose; + synchronized (limelightInputs) { + // checkAndUpdatePose(limelight, limelightInputs); + // if (inputs != null) { + // for (Limelight limelight : Limelight.values()) { + inputs.isLimelightConnected[limelight.getId()] = limelightInputs.isLimelightConnected[limelight.getId()]; + // inputs.limelightMegatag1Pose[limelight.getId()] = limelightInputs.limelightMegatag1Pose[limelight.getId()]; + // inputs.limelightMegatag2Pose[limelight.getId()] = limelightInputs.limelightMegatag2Pose[limelight.getId()]; + inputs.limelightLatency[limelight.getId()] = limelightInputs.limelightLatency[limelight.getId()]; + inputs.limelightTargets[limelight.getId()] = limelightInputs.limelightTargets[limelight.getId()]; + inputs.limelightCameraToTargetPose[limelight.getId()] = limelightInputs.limelightCameraToTargetPose[limelight.getId()]; + inputs.limelightRobotToTargetPose[limelight.getId()] = limelightInputs.limelightRobotToTargetPose[limelight.getId()]; + inputs.limelightSeesAprilTags[limelight.getId()] = limelightInputs.limelightSeesAprilTags[limelight.getId()]; + inputs.limelightAprilTagDistance[limelight.getId()] = limelightInputs.limelightAprilTagDistance[limelight.getId()]; + inputs.limelightCalculatedPose[limelight.getId()] = limelightInputs.limelightCalculatedPose[limelight.getId()]; + inputs.limelightTimestamp[limelight.getId()] = limelightInputs.limelightTimestamp[limelight.getId()]; + // } + inputs.limelightLastSeenPose = limelightInputs.limelightLastSeenPose; + } - } }); latestInputs.set(inputs); } @@ -264,9 +268,10 @@ public Pose2d getLastSeenPose() { /** * Gets the average distance between the specified limelight and the April Tags it sees * - * @param limelightNumber a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return the average distance between the robot and the April Tag(s) in meters */ + @Override public double getLimelightAprilTagDistance(Limelight limelight) { if (canSeeAprilTags(limelight)) { return limelightEstimates[limelight.getId()].avgTagDist; @@ -283,6 +288,7 @@ public double getLimelightAprilTagDistance(Limelight limelight) { * alliance) * @param headingRateDegrees the rate the robot is rotating, CCW positive */ + @Override public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { this.headingDegrees = headingDegrees; this.headingRateDegreesPerSecond = headingRateDegrees; @@ -293,7 +299,7 @@ public boolean isLimelightConnected(Limelight limelight) { if (limelightTable.containsKey(limelight.getName())) { return true; } - return false; + return false; } /** @@ -323,28 +329,34 @@ public void checkAndUpdatePose(Limelight limelight) { // very demanding whereas this only has to get the Network Table entries for TX and TY. if (current_TX != last_TX || current_TY != last_TY) { updatePoseEstimate(limelight); - limelightThreads.computeIfPresent( - limelight, (key, value) -> latestInputs.get()); + // limelightThreads.computeIfPresent( + // limelight, (key, value) ->{return latestInputs;}); + // // Handle threading for Limelight (start or stop threads if needed) + // // Check if this Limelight thread exists in limelightThreads + // if (limelightThreads.get(limelight) != null) { + // // Update thread inputs or restart the thread if needed + // limelightThreads.get(limelight).set(latestInputs.get()); + // } + // This is to keep track of the last valid pose calculated by the limelights // it is used when the driver resets the robot odometry to the limelight calculated // position if (canSeeAprilTags(limelight)) { lastSeenPose = getMegaTag1PoseEstimate(limelight).pose; - } + // } } else { - // Retrieve the AtomicBoolean for the given limelight number - AtomicReference isThreadRunning = - limelightThreads.getOrDefault(limelight, new AtomicReference<>(new - VisionInputs())); - // // Only stop the thread if it's currently running - // if (isThreadRunning.get()) { + // Retrieve the AtomicReference for the given limelight number + AtomicReference isThreadRunning = + limelightThreads.getOrDefault(limelight, latestInputs); + // Only stop the thread if it's currently running + if (isThreadRunning.get() != null) { // stop the thread for the specified limelight stopLimelightThread(limelight); } - } + } last_TX = current_TX; last_TY = current_TY; - } catch (Exception e) { + } }catch (Exception e) { System.err.println( "Error communicating with the: " + limelight.getName() + ": " + e.getMessage()); } @@ -365,37 +377,30 @@ public void checkAndUpdatePose(Limelight limelight) { * * @param limelight the limelight number */ - private void visionThreadTask(Limelight limelight, VisionInputs inputs) { + public void visionThreadTask(Limelight limelight, VisionInputs inputs) { try { synchronized (inputs) { - switch (limelight) { - case SHOOTER -> { - inputs.isShooterLimelightConnected = isLimelightConnected(Limelight.SHOOTER); - inputs.shooterMegaTag1Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.SHOOTER)); - inputs.shooterLatency = getLatencySeconds(Limelight.SHOOTER); - inputs.shooterMegaTag2Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.SHOOTER)); - } - case FRONT_LEFT -> { - inputs.isFrontLeftLimelightConnected = isLimelightConnected(Limelight.FRONT_LEFT); - inputs.frontLeftMegaTag1Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.FRONT_LEFT)); - inputs.frontLeftLatency = getLatencySeconds(Limelight.FRONT_LEFT); - inputs.frontLeftMegaTag2Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.FRONT_LEFT)); - } - case FRONT_RIGHT -> { - inputs.isFrontRightLimelightConnected = isLimelightConnected(Limelight.FRONT_RIGHT); - inputs.frontRightMegaTag1Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(Limelight.FRONT_RIGHT)); - inputs.frontRightLatency = getLatencySeconds(Limelight.FRONT_RIGHT); - inputs.frontRightMegaTag2Pose = - MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(Limelight.FRONT_RIGHT)); - } - } - limelightThreads.get(limelight).set(inputs); + inputs.isLimelightConnected[limelight.getId()] = isLimelightConnected(limelight); + inputs.limelightCameraToTargetPose[limelight.getId()] = getAprilTagPositionToLimelight(limelight); + inputs.limelightRobotToTargetPose[limelight.getId()] = getAprilTagPositionToRobot(limelight); + inputs.limelightLatency[limelight.getId()] = getLatencySeconds(limelight); + inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); + // inputs.limelightMegatag1Pose[limelight.getId()] = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); + // inputs.limelightMegatag2Pose[limelight.getId()] = MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(limelight)); + inputs.limelightSeesAprilTags[limelight.getId()] = canSeeAprilTags(limelight); + inputs.limelightAprilTagDistance[limelight.getId()] = getLimelightAprilTagDistance(limelight); + inputs.limelightCalculatedPose[limelight.getId()] = getPoseFromAprilTags(limelight); + inputs.limelightTimestamp[limelight.getId()] = getTimeStampSeconds(limelight); + inputs.limelightLastSeenPose = getLastSeenPose(); + + latestInputs.set(inputs); + limelightThreads.get(limelight).set(latestInputs.get()); + + } + checkAndUpdatePose(limelight); + + } catch (Exception e) { System.err.println( "Error updating inputs for " + limelight.getName() + ": " + e.getMessage()); diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 22b63f63..f2c67ef4 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -33,6 +33,7 @@ public class SimulatedVision extends PhysicalVision { private final int kResHeight = 800; public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { + super(); this.robotSimulationPose = robotActualPoseInSimulationSupplier; // Create the vision system simulation which handles cameras and targets on the // field. diff --git a/src/main/java/frc/robot/subsystems/vision/ThreadManager.java b/src/main/java/frc/robot/subsystems/vision/ThreadManager.java index 83da90cd..f0113e80 100644 --- a/src/main/java/frc/robot/subsystems/vision/ThreadManager.java +++ b/src/main/java/frc/robot/subsystems/vision/ThreadManager.java @@ -47,7 +47,7 @@ public void stopThread(String threadName) { } } - // New method: Submit a vision input task + // Submit a vision input task public void startVisionInputTask(String threadName, VisionInputs inputs, Runnable visionTask) { startThread( threadName, @@ -55,7 +55,7 @@ public void startVisionInputTask(String threadName, VisionInputs inputs, Runnabl try { while (!Thread.currentThread().isInterrupted()) { visionTask.run(); - Thread.sleep(20); // Sleep to avoid excessive updates + Thread.sleep(VisionConstants.THREAD_SLEEP_MS); // Sleep to avoid excessive updates } } catch (InterruptedException e) { Thread.currentThread().interrupt(); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 788837ab..c8332f54 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -26,19 +26,19 @@ public void periodic() { // Add methods to support DriveCommand public int getNumberOfAprilTags(Limelight limelight) { - return visionInterface.getNumberOfAprilTags(limelight); + return inputs.limelightTargets[limelight.getId()]; } public double getLimelightAprilTagDistance(Limelight limelight) { - return visionInterface.getLimelightAprilTagDistance(limelight); + return inputs.limelightAprilTagDistance[limelight.getId()]; } public double getTimeStampSeconds(Limelight limelight) { - return visionInterface.getTimeStampSeconds(limelight); + return inputs.limelightTimestamp[limelight.getId()]; } public double getLatencySeconds(Limelight limelight) { - return visionInterface.getLatencySeconds(limelight); + return inputs.limelightLatency[limelight.getId()]; } public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { @@ -47,14 +47,14 @@ public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { @AutoLogOutput(key = "Vision/Has Targets") public boolean canSeeAprilTags(Limelight limelight) { - return visionInterface.canSeeAprilTags(limelight); + return inputs.limelightSeesAprilTags[limelight.getId()]; } public Pose2d getPoseFromAprilTags(Limelight limelight) { - return visionInterface.getPoseFromAprilTags(limelight); + return inputs.limelightCalculatedPose[limelight.getId()]; } public Pose2d getLastSeenPose() { - return visionInterface.getLastSeenPose(); + return inputs.limelightLastSeenPose; } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index f9c43eac..1ffcf7ad 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -8,38 +8,21 @@ public interface VisionInterface { @AutoLog class VisionInputs { - public boolean isShooterLimelightConnected; - public boolean isFrontLeftLimelightConnected; - public boolean isFrontRightLimelightConnected; + public boolean[] isLimelightConnected = new boolean[Limelight.values().length]; - public MegatagPoseEstimate shooterMegaTag1Pose; - public double shooterTagCount; - public MegatagPoseEstimate shooterMegaTag2Pose; - public double shooterLatency; - public double shooterTargets; - public Pose2d shooterCameraToTargets; - public Pose2d shooterRobotToTargets; - public Limelight shooter; + // public MegatagPoseEstimate[] limelightMegatag1Pose = new MegatagPoseEstimate[Limelight.values().length]; + // public MegatagPoseEstimate[] limelightMegatag2Pose = new MegatagPoseEstimate[Limelight.values().length]; + public double[] limelightLatency = new double[Limelight.values().length]; + public int[] limelightTargets = new int[Limelight.values().length]; + public Pose2d[] limelightCameraToTargetPose = new Pose2d[Limelight.values().length]; + public Pose2d[] limelightRobotToTargetPose = new Pose2d[Limelight.values().length]; + public boolean[] limelightSeesAprilTags = new boolean[Limelight.values().length]; - public MegatagPoseEstimate frontLeftMegaTag1Pose; - public double frontLeftTagCount; - public MegatagPoseEstimate frontLeftMegaTag2Pose; - public double frontLeftLatency; - public double frontLeftTargets; - public Pose2d frontLeftCameraToTargets; - public Pose2d frontLeftRobotToTargets; - public Limelight frontLeft; + public Pose2d[] limelightCalculatedPose = new Pose2d[Limelight.values().length]; + public Pose2d limelightLastSeenPose = new Pose2d(); + public double[] limelightAprilTagDistance = new double[Limelight.values().length]; - public MegatagPoseEstimate frontRightMegaTag1Pose; - public double frontRightTagCount; - public MegatagPoseEstimate frontRightMegaTag2Pose; - public double frontRightLatency; - public double frontRightTargets; - public Pose2d frontRightCameraToTargets; - public Pose2d frontRightRobotToTargets; - public Limelight frontRight; - - public double camerasAmount; + public double[] limelightTimestamp = new double[Limelight.values().length]; } default void updateInputs(VisionInputs inputs) {} From 7a51f3f2193612b40ea08664426246ce2921ce45 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 25 Nov 2024 14:23:33 -0500 Subject: [PATCH 44/75] im such a fucking dumbass --- .../subsystems/vision/PhysicalVision.java | 77 ++++++++++--------- .../subsystems/vision/VisionInterface.java | 4 +- 2 files changed, 41 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index f93abf76..85cda7bf 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -31,7 +31,7 @@ public class PhysicalVision implements VisionInterface { * frontLeftLimelight, frontRightLimelight} */ private PoseEstimate[] limelightEstimates = - new PoseEstimate[Limelight.values().length]; + new PoseEstimate[] {new PoseEstimate(), new PoseEstimate(), new PoseEstimate()}; public PhysicalVision() { for (Limelight limelight : Limelight.values()) { @@ -40,13 +40,13 @@ public PhysicalVision() { // Initialize limelightEstimates with default PoseEstimate objects - limelightEstimates[limelight.getId()] = new PoseEstimate(); + // limelightEstimates[limelight.getId()] = new PoseEstimate(); // Start a vision input task for each Limelight threadManager.startVisionInputTask( - limelight.getName(), inputs, () -> visionThreadTask(limelight, inputs)); + limelight.getName(), inputs, () -> visionThreadTask(inputs)); } } @@ -58,11 +58,11 @@ public void updateInputs(VisionInputs inputs) { // Combine inputs into the main inputs object synchronized (limelightInputs) { // checkAndUpdatePose(limelight, limelightInputs); - // if (inputs != null) { + if (limelightInputs != null) { // for (Limelight limelight : Limelight.values()) { inputs.isLimelightConnected[limelight.getId()] = limelightInputs.isLimelightConnected[limelight.getId()]; - // inputs.limelightMegatag1Pose[limelight.getId()] = limelightInputs.limelightMegatag1Pose[limelight.getId()]; - // inputs.limelightMegatag2Pose[limelight.getId()] = limelightInputs.limelightMegatag2Pose[limelight.getId()]; + inputs.limelightMegatag1Pose[limelight.getId()] = limelightInputs.limelightMegatag1Pose[limelight.getId()]; + inputs.limelightMegatag2Pose[limelight.getId()] = limelightInputs.limelightMegatag2Pose[limelight.getId()]; inputs.limelightLatency[limelight.getId()] = limelightInputs.limelightLatency[limelight.getId()]; inputs.limelightTargets[limelight.getId()] = limelightInputs.limelightTargets[limelight.getId()]; inputs.limelightCameraToTargetPose[limelight.getId()] = limelightInputs.limelightCameraToTargetPose[limelight.getId()]; @@ -73,10 +73,12 @@ public void updateInputs(VisionInputs inputs) { inputs.limelightTimestamp[limelight.getId()] = limelightInputs.limelightTimestamp[limelight.getId()]; // } inputs.limelightLastSeenPose = limelightInputs.limelightLastSeenPose; - } - }); + } + latestInputs.set(inputs); + limelightThreads.get(limelight).set(latestInputs.get()); + }); } /** @@ -109,8 +111,11 @@ && getNumberOfAprilTags(limelight) <= VisionConstants.APRIL_TAG_POSITIONS.length * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). */ public PoseEstimate enabledPoseUpdate(Limelight limelight) { - if (canSeeAprilTags(limelight) && isValidPoseEstimate(limelight)) { - if (isLargeDiscrepancyBetweenMegaTag1And2(limelight) + PoseEstimate megatag1Estimate = getMegaTag1PoseEstimate(limelight); + PoseEstimate megatag2Estimate = getMegaTag2PoseEstimate(limelight); + + if (canSeeAprilTags(limelight) && isValidPoseEstimate(limelight, megatag1Estimate, megatag2Estimate)) { + if (isLargeDiscrepancyBetweenMegaTag1And2(limelight, megatag1Estimate, megatag2Estimate) && getLimelightAprilTagDistance(limelight) < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { return limelightEstimates[limelight.getId()] = getMegaTag1PoseEstimate(limelight); @@ -130,11 +135,11 @@ && getLimelightAprilTagDistance(limelight) * * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). */ - public void updatePoseEstimate(Limelight limelight) { + public void updatePoseEstimate(Limelight limelight, VisionInputs inputs) { limelightEstimates[limelight.getId()] = DriverStation.isEnabled() - ? enabledPoseUpdate(limelight) - : getMegaTag1PoseEstimate(limelight); + ? inputs.limelightCalculatedPose[limelight.getId()] = MegatagPoseEstimate.fromLimelight(enabledPoseUpdate(limelight)).fieldToCamera + : inputs.limelightCalculatedPose[limelight.getId()] = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)).fieldToCamera; } /** @@ -143,17 +148,14 @@ public void updatePoseEstimate(Limelight limelight) { * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return true if the discrepancy is larger than the defined threshold, false otherwise */ - public boolean isLargeDiscrepancyBetweenMegaTag1And2(Limelight limelight) { - PoseEstimate megatag1Estimate = getMegaTag1PoseEstimate(limelight); - PoseEstimate megatag2Estimate = getMegaTag2PoseEstimate(limelight); - + public boolean isLargeDiscrepancyBetweenMegaTag1And2(Limelight limelight, PoseEstimate mt1, PoseEstimate mt2) { return !GeomUtil.isTranslationWithinThreshold( - megatag1Estimate.pose.getTranslation(), - megatag2Estimate.pose.getTranslation(), + mt1.pose.getTranslation(), + mt2.pose.getTranslation(), VisionConstants.MEGA_TAG_TRANSLATION_DISCREPANCY_THRESHOLD) || !GeomUtil.isRotationWithinThreshold( - megatag1Estimate.pose.getRotation().getDegrees(), - megatag2Estimate.pose.getRotation().getDegrees(), + mt1.pose.getRotation().getDegrees(), + mt2.pose.getRotation().getDegrees(), VisionConstants.MEGA_TAG_ROTATION_DISCREPANCY_THREASHOLD); } @@ -187,13 +189,10 @@ public PoseEstimate getMegaTag2PoseEstimate(Limelight limelight) { * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return true if the pose estimate exists within the field and the pose estimate is not null */ - public boolean isValidPoseEstimate(Limelight limelight) { - PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelight); - PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelight); - - return LimelightHelpers.isValidPoseEstimate(megaTag1Estimate) - && LimelightHelpers.isValidPoseEstimate(megaTag2Estimate) - && isWithinFieldBounds(megaTag1Estimate, megaTag2Estimate); + public boolean isValidPoseEstimate(Limelight limelight, PoseEstimate mt1, PoseEstimate mt2) { + return LimelightHelpers.isValidPoseEstimate(mt1) + && LimelightHelpers.isValidPoseEstimate(mt2) + && isWithinFieldBounds(mt1, mt2); } /** @@ -328,9 +327,9 @@ public void checkAndUpdatePose(Limelight limelight) { // because to get the timestamp of the reading, you need to parse the JSON dump which can be // very demanding whereas this only has to get the Network Table entries for TX and TY. if (current_TX != last_TX || current_TY != last_TY) { - updatePoseEstimate(limelight); - // limelightThreads.computeIfPresent( - // limelight, (key, value) ->{return latestInputs;}); + updatePoseEstimate(limelight, latestInputs.get()); + limelightThreads.computeIfPresent( + limelight, (key, value) -> latestInputs); // // Handle threading for Limelight (start or stop threads if needed) // // Check if this Limelight thread exists in limelightThreads // if (limelightThreads.get(limelight) != null) { @@ -377,16 +376,17 @@ public void checkAndUpdatePose(Limelight limelight) { * * @param limelight the limelight number */ - public void visionThreadTask(Limelight limelight, VisionInputs inputs) { + public void visionThreadTask(VisionInputs inputs) { //Limelight limelight try { synchronized (inputs) { + for (Limelight limelight : Limelight.values()) { inputs.isLimelightConnected[limelight.getId()] = isLimelightConnected(limelight); inputs.limelightCameraToTargetPose[limelight.getId()] = getAprilTagPositionToLimelight(limelight); inputs.limelightRobotToTargetPose[limelight.getId()] = getAprilTagPositionToRobot(limelight); inputs.limelightLatency[limelight.getId()] = getLatencySeconds(limelight); inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); - // inputs.limelightMegatag1Pose[limelight.getId()] = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); - // inputs.limelightMegatag2Pose[limelight.getId()] = MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(limelight)); + inputs.limelightMegatag1Pose[limelight.getId()] = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); + inputs.limelightMegatag2Pose[limelight.getId()] = MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(limelight)); inputs.limelightSeesAprilTags[limelight.getId()] = canSeeAprilTags(limelight); inputs.limelightAprilTagDistance[limelight.getId()] = getLimelightAprilTagDistance(limelight); inputs.limelightCalculatedPose[limelight.getId()] = getPoseFromAprilTags(limelight); @@ -395,15 +395,16 @@ public void visionThreadTask(Limelight limelight, VisionInputs inputs) { latestInputs.set(inputs); limelightThreads.get(limelight).set(latestInputs.get()); + - + checkAndUpdatePose(limelight); + } } - checkAndUpdatePose(limelight); } catch (Exception e) { - System.err.println( - "Error updating inputs for " + limelight.getName() + ": " + e.getMessage()); + // System.err.println( + // "Error updating inputs for " + limelight.getName() + ": " + e.getMessage()); } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index 1ffcf7ad..a91d7c8c 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -10,8 +10,8 @@ public interface VisionInterface { class VisionInputs { public boolean[] isLimelightConnected = new boolean[Limelight.values().length]; - // public MegatagPoseEstimate[] limelightMegatag1Pose = new MegatagPoseEstimate[Limelight.values().length]; - // public MegatagPoseEstimate[] limelightMegatag2Pose = new MegatagPoseEstimate[Limelight.values().length]; + public MegatagPoseEstimate[] limelightMegatag1Pose = new MegatagPoseEstimate[Limelight.values().length]; + public MegatagPoseEstimate[] limelightMegatag2Pose = new MegatagPoseEstimate[Limelight.values().length]; public double[] limelightLatency = new double[Limelight.values().length]; public int[] limelightTargets = new int[Limelight.values().length]; public Pose2d[] limelightCameraToTargetPose = new Pose2d[Limelight.values().length]; From 4247cb40365146df2620cb2ba09b30d2c639444b Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 25 Nov 2024 15:08:54 -0500 Subject: [PATCH 45/75] fuuuuuuuuuuuuuuuuuuuuuuuuuuck --- .../subsystems/vision/PhysicalVision.java | 108 +++++++++--------- .../subsystems/vision/VisionInterface.java | 4 +- 2 files changed, 59 insertions(+), 53 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 85cda7bf..e2470f21 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -57,8 +57,8 @@ public void updateInputs(VisionInputs inputs) { VisionInputs limelightInputs = inputRef.get(); // Combine inputs into the main inputs object synchronized (limelightInputs) { - // checkAndUpdatePose(limelight, limelightInputs); - if (limelightInputs != null) { + // checkAndUpdatePose(limelight); + // if (limelightInputs != null) { // for (Limelight limelight : Limelight.values()) { inputs.isLimelightConnected[limelight.getId()] = limelightInputs.isLimelightConnected[limelight.getId()]; inputs.limelightMegatag1Pose[limelight.getId()] = limelightInputs.limelightMegatag1Pose[limelight.getId()]; @@ -73,7 +73,7 @@ public void updateInputs(VisionInputs inputs) { inputs.limelightTimestamp[limelight.getId()] = limelightInputs.limelightTimestamp[limelight.getId()]; // } inputs.limelightLastSeenPose = limelightInputs.limelightLastSeenPose; - } + // } } latestInputs.set(inputs); @@ -94,14 +94,14 @@ public boolean canSeeAprilTags(Limelight limelight) { if (getNumberOfAprilTags(limelight) > 0 && getNumberOfAprilTags(limelight) <= VisionConstants.APRIL_TAG_POSITIONS.length) { if (limelight.getName().equals(VisionConstants.SHOOTER_LIMELIGHT_NAME)) { - return Math.abs(LimelightHelpers.getTX(limelight.getName())) + return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = Math.abs(LimelightHelpers.getTX(limelight.getName())) <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR; } else { - return Math.abs(LimelightHelpers.getTX(limelight.getName())) + return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = Math.abs(LimelightHelpers.getTX(limelight.getName())) <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR; } } - return false; + return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = false; } /** @@ -110,23 +110,23 @@ && getNumberOfAprilTags(limelight) <= VisionConstants.APRIL_TAG_POSITIONS.length * * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). */ - public PoseEstimate enabledPoseUpdate(Limelight limelight) { - PoseEstimate megatag1Estimate = getMegaTag1PoseEstimate(limelight); - PoseEstimate megatag2Estimate = getMegaTag2PoseEstimate(limelight); + public Pose2d enabledPoseUpdate(Limelight limelight) { + Pose2d megatag1Estimate = getMegaTag1PoseEstimate(limelight); + Pose2d megatag2Estimate = getMegaTag2PoseEstimate(limelight); if (canSeeAprilTags(limelight) && isValidPoseEstimate(limelight, megatag1Estimate, megatag2Estimate)) { if (isLargeDiscrepancyBetweenMegaTag1And2(limelight, megatag1Estimate, megatag2Estimate) && getLimelightAprilTagDistance(limelight) < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { - return limelightEstimates[limelight.getId()] = getMegaTag1PoseEstimate(limelight); + return latestInputs.get().limelightCalculatedPose[limelight.getId()] = getMegaTag1PoseEstimate(limelight); } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) { LimelightHelpers.SetRobotOrientation(limelight.getName(), headingDegrees, 0, 0, 0, 0, 0); - return limelightEstimates[limelight.getId()] = getMegaTag2PoseEstimate(limelight); + return latestInputs.get().limelightCalculatedPose[limelight.getId()] = getMegaTag2PoseEstimate(limelight); } else { - return limelightEstimates[limelight.getId()] = getMegaTag1PoseEstimate(limelight); + return latestInputs.get().limelightCalculatedPose[limelight.getId()] = getMegaTag1PoseEstimate(limelight); } } - return limelightEstimates[limelight.getId()] = new PoseEstimate(); + return latestInputs.get().limelightCalculatedPose[limelight.getId()] = new Pose2d(); } /** @@ -136,10 +136,12 @@ && getLimelightAprilTagDistance(limelight) * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). */ public void updatePoseEstimate(Limelight limelight, VisionInputs inputs) { - limelightEstimates[limelight.getId()] = + synchronized (inputs) { + inputs.limelightCalculatedPose[limelight.getId()] = DriverStation.isEnabled() - ? inputs.limelightCalculatedPose[limelight.getId()] = MegatagPoseEstimate.fromLimelight(enabledPoseUpdate(limelight)).fieldToCamera - : inputs.limelightCalculatedPose[limelight.getId()] = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)).fieldToCamera; + ? enabledPoseUpdate(limelight) + : getMegaTag1PoseEstimate(limelight); + } } /** @@ -148,15 +150,16 @@ public void updatePoseEstimate(Limelight limelight, VisionInputs inputs) { * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return true if the discrepancy is larger than the defined threshold, false otherwise */ - public boolean isLargeDiscrepancyBetweenMegaTag1And2(Limelight limelight, PoseEstimate mt1, PoseEstimate mt2) { + public boolean isLargeDiscrepancyBetweenMegaTag1And2(Limelight limelight, Pose2d mt1, Pose2d mt2) { return !GeomUtil.isTranslationWithinThreshold( - mt1.pose.getTranslation(), - mt2.pose.getTranslation(), + mt1.getTranslation(), + mt2.getTranslation(), VisionConstants.MEGA_TAG_TRANSLATION_DISCREPANCY_THRESHOLD) || !GeomUtil.isRotationWithinThreshold( - mt1.pose.getRotation().getDegrees(), - mt2.pose.getRotation().getDegrees(), + mt1.getRotation().getDegrees(), + mt2.getRotation().getDegrees(), VisionConstants.MEGA_TAG_ROTATION_DISCREPANCY_THREASHOLD); + // return true; } /** @@ -167,8 +170,8 @@ public boolean isLargeDiscrepancyBetweenMegaTag1And2(Limelight limelight, PoseEs * @return the MegaTag1 pose of the robot, if the limelight can't see any April Tags, it will * return 0 for x, y, and theta */ - public PoseEstimate getMegaTag1PoseEstimate(Limelight limelight) { - return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()); + public Pose2d getMegaTag1PoseEstimate(Limelight limelight) { + return latestInputs.get().limelightCalculatedPose[limelight.getId()] = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()).pose; } /** @@ -179,8 +182,8 @@ public PoseEstimate getMegaTag1PoseEstimate(Limelight limelight) { * @return the MegaTag2 pose of the robot, if the limelight can't see any April Tags, it will * return 0 for x, y, and theta */ - public PoseEstimate getMegaTag2PoseEstimate(Limelight limelight) { - return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName()); + public Pose2d getMegaTag2PoseEstimate(Limelight limelight) { + return latestInputs.get().limelightCalculatedPose[limelight.getId()] = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName()).pose; } /** @@ -189,10 +192,11 @@ public PoseEstimate getMegaTag2PoseEstimate(Limelight limelight) { * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return true if the pose estimate exists within the field and the pose estimate is not null */ - public boolean isValidPoseEstimate(Limelight limelight, PoseEstimate mt1, PoseEstimate mt2) { - return LimelightHelpers.isValidPoseEstimate(mt1) - && LimelightHelpers.isValidPoseEstimate(mt2) - && isWithinFieldBounds(mt1, mt2); + public boolean isValidPoseEstimate(Limelight limelight, Pose2d mt1, Pose2d mt2) { + // return LimelightHelpers.isValidPoseEstimate(mt1) + // && LimelightHelpers.isValidPoseEstimate(mt2) + // && + return isWithinFieldBounds(mt1, mt2); } /** @@ -202,15 +206,15 @@ public boolean isValidPoseEstimate(Limelight limelight, PoseEstimate mt1, PoseEs * @param megaTag2Estimate the MT2 pose estimate to check */ private boolean isWithinFieldBounds( - PoseEstimate megaTag1Estimate, PoseEstimate megaTag2Estimate) { - return (megaTag1Estimate.pose.getX() > 0 - && megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) - && (megaTag1Estimate.pose.getY() > 0 - && megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS) - && (megaTag2Estimate.pose.getX() > 0 - && megaTag2Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) - && (megaTag2Estimate.pose.getY() > 0 - && megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS); + Pose2d megaTag1Estimate, Pose2d megaTag2Estimate) { + return (megaTag1Estimate.getX() > 0 + && megaTag1Estimate.getX() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag1Estimate.getY() > 0 + && megaTag1Estimate.getY() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag2Estimate.getX() > 0 + && megaTag2Estimate.getX() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag2Estimate.getY() > 0 + && megaTag2Estimate.getY() <= FieldConstants.FIELD_WIDTH_METERS); } /** @@ -222,7 +226,7 @@ private boolean isWithinFieldBounds( */ @Override public Pose2d getPoseFromAprilTags(Limelight limelight) { - return limelightEstimates[limelight.getId()].pose; + return latestInputs.get().limelightCalculatedPose[limelight.getId()]; } public Pose2d getAprilTagPositionToLimelight(Limelight limelight) { @@ -230,13 +234,13 @@ public Pose2d getAprilTagPositionToLimelight(Limelight limelight) { } public Pose2d getAprilTagPositionToRobot(Limelight limelight) { - return LimelightHelpers.getTargetPose_RobotSpace(limelight.getName()); + return latestInputs.get().limelightRobotToTargetPose[limelight.getId()] =LimelightHelpers.getTargetPose_RobotSpace(limelight.getName()); } /** Returns how many april tags the limelight that is being used for pose estimation can see. */ @Override public int getNumberOfAprilTags(Limelight limelight) { - return limelightEstimates[limelight.getId()].tagCount; + return latestInputs.get().limelightTargets[limelight.getId()] = limelightEstimates[limelight.getId()].tagCount; } /** @@ -245,7 +249,7 @@ public int getNumberOfAprilTags(Limelight limelight) { */ @Override public double getTimeStampSeconds(Limelight limelight) { - return limelightEstimates[limelight.getId()].timestampSeconds / 1000.0; + return latestInputs.get().limelightTimestamp[limelight.getId()] = limelightEstimates[limelight.getId()].timestampSeconds / 1000.0; } /** @@ -255,13 +259,13 @@ public double getTimeStampSeconds(Limelight limelight) { */ @Override public double getLatencySeconds(Limelight limelight) { - return (limelightEstimates[limelight.getId()].latency) / 1000.0; + return latestInputs.get().limelightLatency[limelight.getId()] = (limelightEstimates[limelight.getId()].latency) / 1000.0; } /** Gets the pose calculated the last time a limelight saw an April Tag */ @Override public Pose2d getLastSeenPose() { - return lastSeenPose; + return latestInputs.get().limelightLastSeenPose = lastSeenPose; } /** @@ -327,21 +331,23 @@ public void checkAndUpdatePose(Limelight limelight) { // because to get the timestamp of the reading, you need to parse the JSON dump which can be // very demanding whereas this only has to get the Network Table entries for TX and TY. if (current_TX != last_TX || current_TY != last_TY) { + limelightThreads.get(limelight).set(latestInputs.get()); + updatePoseEstimate(limelight, latestInputs.get()); limelightThreads.computeIfPresent( limelight, (key, value) -> latestInputs); // // Handle threading for Limelight (start or stop threads if needed) // // Check if this Limelight thread exists in limelightThreads - // if (limelightThreads.get(limelight) != null) { - // // Update thread inputs or restart the thread if needed - // limelightThreads.get(limelight).set(latestInputs.get()); - // } + // if (limelightThreads.get(limelight) != null) { + // // Update thread inputs or restart the thread if needed + // limelightThreads.get(limelight).set(latestInputs.get()); + // } // This is to keep track of the last valid pose calculated by the limelights // it is used when the driver resets the robot odometry to the limelight calculated // position if (canSeeAprilTags(limelight)) { - lastSeenPose = getMegaTag1PoseEstimate(limelight).pose; + lastSeenPose = getMegaTag1PoseEstimate(limelight); // } } else { // Retrieve the AtomicReference for the given limelight number @@ -385,8 +391,8 @@ public void visionThreadTask(VisionInputs inputs) { //Limelight limelight inputs.limelightRobotToTargetPose[limelight.getId()] = getAprilTagPositionToRobot(limelight); inputs.limelightLatency[limelight.getId()] = getLatencySeconds(limelight); inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); - inputs.limelightMegatag1Pose[limelight.getId()] = MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); - inputs.limelightMegatag2Pose[limelight.getId()] = MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(limelight)); + inputs.limelightMegatag1Pose[limelight.getId()] = getMegaTag1PoseEstimate(limelight); + inputs.limelightMegatag2Pose[limelight.getId()] = getMegaTag2PoseEstimate(limelight); inputs.limelightSeesAprilTags[limelight.getId()] = canSeeAprilTags(limelight); inputs.limelightAprilTagDistance[limelight.getId()] = getLimelightAprilTagDistance(limelight); inputs.limelightCalculatedPose[limelight.getId()] = getPoseFromAprilTags(limelight); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index a91d7c8c..c583c8da 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -10,8 +10,8 @@ public interface VisionInterface { class VisionInputs { public boolean[] isLimelightConnected = new boolean[Limelight.values().length]; - public MegatagPoseEstimate[] limelightMegatag1Pose = new MegatagPoseEstimate[Limelight.values().length]; - public MegatagPoseEstimate[] limelightMegatag2Pose = new MegatagPoseEstimate[Limelight.values().length]; + public Pose2d[] limelightMegatag1Pose = new Pose2d[Limelight.values().length]; + public Pose2d[] limelightMegatag2Pose = new Pose2d[Limelight.values().length]; public double[] limelightLatency = new double[Limelight.values().length]; public int[] limelightTargets = new int[Limelight.values().length]; public Pose2d[] limelightCameraToTargetPose = new Pose2d[Limelight.values().length]; From ac7c3af8e8d71f055f14a4a336af3f217d7c0114 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 25 Nov 2024 15:51:11 -0500 Subject: [PATCH 46/75] ok im not stupid thank god --- .../subsystems/vision/PhysicalVision.java | 57 +++++++++---------- .../subsystems/vision/VisionInterface.java | 8 +-- 2 files changed, 32 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index e2470f21..2514342b 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.vision; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants.FieldConstants; @@ -23,7 +24,7 @@ public class PhysicalVision implements VisionInterface { private final ConcurrentHashMap> limelightThreads = new ConcurrentHashMap<>(); // private final ExecutorService executorService = Executors.newFixedThreadPool(3); - private final AtomicReference latestInputs = new AtomicReference<>(); + private final AtomicReference latestInputs = new AtomicReference<>(new VisionInputs()); private final ThreadManager threadManager = new ThreadManager(Limelight.values().length); /** @@ -61,12 +62,12 @@ public void updateInputs(VisionInputs inputs) { // if (limelightInputs != null) { // for (Limelight limelight : Limelight.values()) { inputs.isLimelightConnected[limelight.getId()] = limelightInputs.isLimelightConnected[limelight.getId()]; - inputs.limelightMegatag1Pose[limelight.getId()] = limelightInputs.limelightMegatag1Pose[limelight.getId()]; - inputs.limelightMegatag2Pose[limelight.getId()] = limelightInputs.limelightMegatag2Pose[limelight.getId()]; + // inputs.limelightMegatag1Pose[limelight.getId()] = limelightInputs.limelightMegatag1Pose[limelight.getId()]; + // inputs.limelightMegatag2Pose[limelight.getId()] = limelightInputs.limelightMegatag2Pose[limelight.getId()]; inputs.limelightLatency[limelight.getId()] = limelightInputs.limelightLatency[limelight.getId()]; inputs.limelightTargets[limelight.getId()] = limelightInputs.limelightTargets[limelight.getId()]; - inputs.limelightCameraToTargetPose[limelight.getId()] = limelightInputs.limelightCameraToTargetPose[limelight.getId()]; - inputs.limelightRobotToTargetPose[limelight.getId()] = limelightInputs.limelightRobotToTargetPose[limelight.getId()]; + // inputs.limelightCameraToTargetPose[limelight.getId()] = limelightInputs.limelightCameraToTargetPose[limelight.getId()]; + // inputs.limelightRobotToTargetPose[limelight.getId()] = limelightInputs.limelightRobotToTargetPose[limelight.getId()]; inputs.limelightSeesAprilTags[limelight.getId()] = limelightInputs.limelightSeesAprilTags[limelight.getId()]; inputs.limelightAprilTagDistance[limelight.getId()] = limelightInputs.limelightAprilTagDistance[limelight.getId()]; inputs.limelightCalculatedPose[limelight.getId()] = limelightInputs.limelightCalculatedPose[limelight.getId()]; @@ -94,14 +95,14 @@ public boolean canSeeAprilTags(Limelight limelight) { if (getNumberOfAprilTags(limelight) > 0 && getNumberOfAprilTags(limelight) <= VisionConstants.APRIL_TAG_POSITIONS.length) { if (limelight.getName().equals(VisionConstants.SHOOTER_LIMELIGHT_NAME)) { - return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = Math.abs(LimelightHelpers.getTX(limelight.getName())) + return Math.abs(LimelightHelpers.getTX(limelight.getName())) <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR; } else { - return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = Math.abs(LimelightHelpers.getTX(limelight.getName())) + return Math.abs(LimelightHelpers.getTX(limelight.getName())) <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR; } } - return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = false; + return false; } /** @@ -118,15 +119,15 @@ public Pose2d enabledPoseUpdate(Limelight limelight) { if (isLargeDiscrepancyBetweenMegaTag1And2(limelight, megatag1Estimate, megatag2Estimate) && getLimelightAprilTagDistance(limelight) < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { - return latestInputs.get().limelightCalculatedPose[limelight.getId()] = getMegaTag1PoseEstimate(limelight); + return limelightEstimates[limelight.getId()].pose = getMegaTag1PoseEstimate(limelight); } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) { LimelightHelpers.SetRobotOrientation(limelight.getName(), headingDegrees, 0, 0, 0, 0, 0); - return latestInputs.get().limelightCalculatedPose[limelight.getId()] = getMegaTag2PoseEstimate(limelight); + return limelightEstimates[limelight.getId()].pose = getMegaTag2PoseEstimate(limelight); } else { - return latestInputs.get().limelightCalculatedPose[limelight.getId()] = getMegaTag1PoseEstimate(limelight); + return limelightEstimates[limelight.getId()].pose = getMegaTag1PoseEstimate(limelight); } } - return latestInputs.get().limelightCalculatedPose[limelight.getId()] = new Pose2d(); + return limelightEstimates[limelight.getId()].pose = new Pose2d(); } /** @@ -171,7 +172,7 @@ public boolean isLargeDiscrepancyBetweenMegaTag1And2(Limelight limelight, Pose2d * return 0 for x, y, and theta */ public Pose2d getMegaTag1PoseEstimate(Limelight limelight) { - return latestInputs.get().limelightCalculatedPose[limelight.getId()] = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()).pose; + return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()).pose; } /** @@ -183,7 +184,7 @@ public Pose2d getMegaTag1PoseEstimate(Limelight limelight) { * return 0 for x, y, and theta */ public Pose2d getMegaTag2PoseEstimate(Limelight limelight) { - return latestInputs.get().limelightCalculatedPose[limelight.getId()] = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName()).pose; + return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName()).pose; } /** @@ -226,21 +227,21 @@ private boolean isWithinFieldBounds( */ @Override public Pose2d getPoseFromAprilTags(Limelight limelight) { - return latestInputs.get().limelightCalculatedPose[limelight.getId()]; + return limelightEstimates[limelight.getId()].pose; } - public Pose2d getAprilTagPositionToLimelight(Limelight limelight) { - return LimelightHelpers.getTargetPose_CameraSpace(limelight.getName()); - } + // public Pose2d getAprilTagPositionToLimelight(Limelight limelight) { + // return LimelightHelpers.getTargetPose_CameraSpace(limelight.getName()); + // } - public Pose2d getAprilTagPositionToRobot(Limelight limelight) { - return latestInputs.get().limelightRobotToTargetPose[limelight.getId()] =LimelightHelpers.getTargetPose_RobotSpace(limelight.getName()); - } + // public Pose2d getAprilTagPositionToRobot(Limelight limelight) { + // return latestInputs.get().limelightRobotToTargetPose[limelight.getId()] =LimelightHelpers.getTargetPose_RobotSpace(limelight.getName()); + // } /** Returns how many april tags the limelight that is being used for pose estimation can see. */ @Override public int getNumberOfAprilTags(Limelight limelight) { - return latestInputs.get().limelightTargets[limelight.getId()] = limelightEstimates[limelight.getId()].tagCount; + return limelightEstimates[limelight.getId()].tagCount; } /** @@ -249,7 +250,7 @@ public int getNumberOfAprilTags(Limelight limelight) { */ @Override public double getTimeStampSeconds(Limelight limelight) { - return latestInputs.get().limelightTimestamp[limelight.getId()] = limelightEstimates[limelight.getId()].timestampSeconds / 1000.0; + return limelightEstimates[limelight.getId()].timestampSeconds / 1000.0; } /** @@ -259,13 +260,13 @@ public double getTimeStampSeconds(Limelight limelight) { */ @Override public double getLatencySeconds(Limelight limelight) { - return latestInputs.get().limelightLatency[limelight.getId()] = (limelightEstimates[limelight.getId()].latency) / 1000.0; + return (limelightEstimates[limelight.getId()].latency) / 1000.0; } /** Gets the pose calculated the last time a limelight saw an April Tag */ @Override public Pose2d getLastSeenPose() { - return latestInputs.get().limelightLastSeenPose = lastSeenPose; + return lastSeenPose; } /** @@ -387,12 +388,10 @@ public void visionThreadTask(VisionInputs inputs) { //Limelight limelight synchronized (inputs) { for (Limelight limelight : Limelight.values()) { inputs.isLimelightConnected[limelight.getId()] = isLimelightConnected(limelight); - inputs.limelightCameraToTargetPose[limelight.getId()] = getAprilTagPositionToLimelight(limelight); - inputs.limelightRobotToTargetPose[limelight.getId()] = getAprilTagPositionToRobot(limelight); + // inputs.limelightCameraToTargetPose[limelight.getId()] = getAprilTagPositionToLimelight(limelight); + // inputs.limelightRobotToTargetPose[limelight.getId()] = getAprilTagPositionToRobot(limelight); inputs.limelightLatency[limelight.getId()] = getLatencySeconds(limelight); inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); - inputs.limelightMegatag1Pose[limelight.getId()] = getMegaTag1PoseEstimate(limelight); - inputs.limelightMegatag2Pose[limelight.getId()] = getMegaTag2PoseEstimate(limelight); inputs.limelightSeesAprilTags[limelight.getId()] = canSeeAprilTags(limelight); inputs.limelightAprilTagDistance[limelight.getId()] = getLimelightAprilTagDistance(limelight); inputs.limelightCalculatedPose[limelight.getId()] = getPoseFromAprilTags(limelight); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index c583c8da..cfad0751 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -10,12 +10,12 @@ public interface VisionInterface { class VisionInputs { public boolean[] isLimelightConnected = new boolean[Limelight.values().length]; - public Pose2d[] limelightMegatag1Pose = new Pose2d[Limelight.values().length]; - public Pose2d[] limelightMegatag2Pose = new Pose2d[Limelight.values().length]; + // public Pose2d[] limelightMegatag1Pose = new Pose2d[Limelight.values().length]; + // public Pose2d[] limelightMegatag2Pose = new Pose2d[Limelight.values().length]; public double[] limelightLatency = new double[Limelight.values().length]; public int[] limelightTargets = new int[Limelight.values().length]; - public Pose2d[] limelightCameraToTargetPose = new Pose2d[Limelight.values().length]; - public Pose2d[] limelightRobotToTargetPose = new Pose2d[Limelight.values().length]; + // public Pose2d[] limelightCameraToTargetPose = new Pose2d[Limelight.values().length]; + // public Pose2d[] limelightRobotToTargetPose = new Pose2d[Limelight.values().length]; public boolean[] limelightSeesAprilTags = new boolean[Limelight.values().length]; public Pose2d[] limelightCalculatedPose = new Pose2d[Limelight.values().length]; From 20d863ac627ad75b44a7ac809335f3d931929cd6 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 25 Nov 2024 17:19:15 -0500 Subject: [PATCH 47/75] i give uo :sob: --- .../subsystems/vision/PhysicalVision.java | 55 ++++++++++--------- .../subsystems/vision/SimulatedVision.java | 42 ++------------ .../frc/robot/subsystems/vision/Vision.java | 1 + .../subsystems/vision/VisionInterface.java | 3 +- 4 files changed, 38 insertions(+), 63 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 2514342b..216940be 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -36,8 +36,7 @@ public class PhysicalVision implements VisionInterface { public PhysicalVision() { for (Limelight limelight : Limelight.values()) { - VisionInputs inputs = new VisionInputs(); // Create vision inputs for each Limelight - limelightThreads.put(limelight, new AtomicReference<>(inputs)); + limelightThreads.put(limelight, new AtomicReference<>(latestInputs.get())); // Initialize limelightEstimates with default PoseEstimate objects @@ -46,9 +45,10 @@ public PhysicalVision() { // Start a vision input task for each Limelight - threadManager.startVisionInputTask( - limelight.getName(), inputs, () -> visionThreadTask(inputs)); } + + threadManager.startVisionInputTask( + Limelight.SHOOTER.getName(), latestInputs.get(), () -> visionThreadTask(latestInputs.get())); } @Override @@ -92,17 +92,19 @@ public void updateInputs(VisionInputs inputs) { public boolean canSeeAprilTags(Limelight limelight) { // First checks if it can see an april tag, then checks if it is fully in frame // Different Limelights have different FOVs - if (getNumberOfAprilTags(limelight) > 0 - && getNumberOfAprilTags(limelight) <= VisionConstants.APRIL_TAG_POSITIONS.length) { - if (limelight.getName().equals(VisionConstants.SHOOTER_LIMELIGHT_NAME)) { - return Math.abs(LimelightHelpers.getTX(limelight.getName())) - <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR; - } else { - return Math.abs(LimelightHelpers.getTX(limelight.getName())) - <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR; - } - } - return false; + // if (getNumberOfAprilTags(limelight) > 0 + // && getNumberOfAprilTags(limelight) <= VisionConstants.APRIL_TAG_POSITIONS.length) { + // if (limelight.getName().equals(VisionConstants.SHOOTER_LIMELIGHT_NAME)) { + // return latestInputs.get().limelightSeesAprilTags[limelight.getId()] =Math.abs(LimelightHelpers.getTX(limelight.getName())) + // <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR; + // } else { + // return latestInputs.get().limelightSeesAprilTags[limelight.getId()] =Math.abs(LimelightHelpers.getTX(limelight.getName())) + // <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR; + // } + // } + // return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = false; + return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = true; + // return true; } /** @@ -227,7 +229,7 @@ private boolean isWithinFieldBounds( */ @Override public Pose2d getPoseFromAprilTags(Limelight limelight) { - return limelightEstimates[limelight.getId()].pose; + return latestInputs.get().limelightCalculatedPose[limelight.getId()] =limelightEstimates[limelight.getId()].pose; } // public Pose2d getAprilTagPositionToLimelight(Limelight limelight) { @@ -241,7 +243,7 @@ public Pose2d getPoseFromAprilTags(Limelight limelight) { /** Returns how many april tags the limelight that is being used for pose estimation can see. */ @Override public int getNumberOfAprilTags(Limelight limelight) { - return limelightEstimates[limelight.getId()].tagCount; + return latestInputs.get().limelightTargets[limelight.getId()] = limelightEstimates[limelight.getId()].tagCount; } /** @@ -266,7 +268,7 @@ public double getLatencySeconds(Limelight limelight) { /** Gets the pose calculated the last time a limelight saw an April Tag */ @Override public Pose2d getLastSeenPose() { - return lastSeenPose; + return latestInputs.get().limelightLastSeenPose = lastSeenPose; } /** @@ -332,24 +334,23 @@ public void checkAndUpdatePose(Limelight limelight) { // because to get the timestamp of the reading, you need to parse the JSON dump which can be // very demanding whereas this only has to get the Network Table entries for TX and TY. if (current_TX != last_TX || current_TY != last_TY) { - limelightThreads.get(limelight).set(latestInputs.get()); updatePoseEstimate(limelight, latestInputs.get()); limelightThreads.computeIfPresent( limelight, (key, value) -> latestInputs); // // Handle threading for Limelight (start or stop threads if needed) // // Check if this Limelight thread exists in limelightThreads - // if (limelightThreads.get(limelight) != null) { - // // Update thread inputs or restart the thread if needed - // limelightThreads.get(limelight).set(latestInputs.get()); - // } + if (limelightThreads.get(limelight) != null) { + // Update thread inputs or restart the thread if needed + limelightThreads.get(limelight).set(latestInputs.get()); + } // This is to keep track of the last valid pose calculated by the limelights // it is used when the driver resets the robot odometry to the limelight calculated // position if (canSeeAprilTags(limelight)) { lastSeenPose = getMegaTag1PoseEstimate(limelight); - // } + } } else { // Retrieve the AtomicReference for the given limelight number AtomicReference isThreadRunning = @@ -362,7 +363,7 @@ public void checkAndUpdatePose(Limelight limelight) { } last_TX = current_TX; last_TY = current_TY; - } }catch (Exception e) { + } catch (Exception e) { System.err.println( "Error communicating with the: " + limelight.getName() + ": " + e.getMessage()); } @@ -387,6 +388,9 @@ public void visionThreadTask(VisionInputs inputs) { //Limelight limelight try { synchronized (inputs) { for (Limelight limelight : Limelight.values()) { + checkAndUpdatePose(limelight); + // updatePoseEstimate(limelight, inputs); + inputs.isLimelightConnected[limelight.getId()] = isLimelightConnected(limelight); // inputs.limelightCameraToTargetPose[limelight.getId()] = getAprilTagPositionToLimelight(limelight); // inputs.limelightRobotToTargetPose[limelight.getId()] = getAprilTagPositionToRobot(limelight); @@ -402,7 +406,6 @@ public void visionThreadTask(VisionInputs inputs) { //Limelight limelight limelightThreads.get(limelight).set(latestInputs.get()); - checkAndUpdatePose(limelight); } } diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index f2c67ef4..975c4c2c 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import frc.robot.extras.vision.LimelightHelpers; import frc.robot.subsystems.vision.VisionConstants.Limelight; import java.util.ArrayList; import java.util.Arrays; @@ -97,12 +98,9 @@ public void updateInputs(VisionInputs inputs) { Logger.recordOutput("Vision/SimIO/updateSimPose", robotSimulationPose.get()); } - NetworkTable shooterTable = - NetworkTableInstance.getDefault().getTable(Limelight.SHOOTER.getName()); - NetworkTable frontLeftTable = - NetworkTableInstance.getDefault().getTable(Limelight.FRONT_LEFT.getName()); - NetworkTable frontRightTable = - NetworkTableInstance.getDefault().getTable(Limelight.FRONT_RIGHT.getName()); + NetworkTable shooterTable = LimelightHelpers.getLimelightNTTable(Limelight.SHOOTER.getName()); + NetworkTable frontLeftTable = LimelightHelpers.getLimelightNTTable(Limelight.FRONT_LEFT.getName()); + NetworkTable frontRightTable = LimelightHelpers.getLimelightNTTable(Limelight.FRONT_RIGHT.getName()); // Write to limelight table writeToTable(shooterCamera.getAllUnreadResults(), shooterTable, Limelight.SHOOTER); writeToTable(frontLeftCamera.getAllUnreadResults(), frontLeftTable, Limelight.FRONT_LEFT); @@ -117,8 +115,6 @@ private void writeToTable( for (PhotonPipelineResult result : results) { if (result.getMultiTagResult().isPresent()) { Transform3d best = result.getMultiTagResult().get().estimatedPose.best; - Pose2d fieldToCamera = - new Pose2d(best.getTranslation().toTranslation2d(), best.getRotation().toRotation2d()); List pose_data = new ArrayList<>( Arrays.asList( @@ -127,7 +123,7 @@ private void writeToTable( best.getZ(), // 2: Z, best.getRotation().getX(), // 3: roll best.getRotation().getY(), // 4: pitch - fieldToCamera.getRotation().getDegrees(), // 5: yaw + best.getRotation().getZ(), // 5: yaw result.metadata.getLatencyMillis(), // 6: latency ms, (double) result.getMultiTagResult().get().fiducialIDsUsed.size(), // 7: tag count @@ -160,35 +156,9 @@ private void writeToTable( } } - public double getLatencySeconds(Limelight limelight) { - return super.getLatencySeconds(limelight); - } - - public double getTimeStampSeconds(Limelight limelight) { - return super.getTimeStampSeconds(limelight); - } - - public boolean canSeeAprilTags(Limelight limelight) { - return super.canSeeAprilTags(limelight); - } - - public double getLimelightAprilTagDistance(Limelight limelight) { - return super.getLimelightAprilTagDistance(limelight); - } - - public int getNumberOfAprilTags(Limelight limelight) { - return super.getNumberOfAprilTags(limelight); - } - - public Pose2d getPoseFromAprilTags(Limelight limelight) { - return super.getPoseFromAprilTags(limelight); - } - + @Override public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { super.setHeadingInfo(headingDegrees, headingRateDegrees); } - public Pose2d getLastSeenPose() { - return super.getLastSeenPose(); - } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index c8332f54..435842b8 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -57,4 +57,5 @@ public Pose2d getPoseFromAprilTags(Limelight limelight) { public Pose2d getLastSeenPose() { return inputs.limelightLastSeenPose; } + } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index cfad0751..4060e8c1 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.vision; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import frc.robot.extras.vision.MegatagPoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; import org.littletonrobotics.junction.AutoLog; @@ -30,7 +31,7 @@ default void updateInputs(VisionInputs inputs) {} default double getLatencySeconds(Limelight limelight) { return 0.0; } - + default double getTimeStampSeconds(Limelight limelight) { return 0.0; } From 8e68d1fc09b1b04756133ff5c31a7240ba082ca4 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 25 Nov 2024 22:45:06 -0500 Subject: [PATCH 48/75] un poquito de progresso --- .../java/frc/robot/subsystems/vision/PhysicalVision.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 216940be..3f54b1d7 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -45,10 +45,11 @@ public PhysicalVision() { // Start a vision input task for each Limelight - } + threadManager.startVisionInputTask( - Limelight.SHOOTER.getName(), latestInputs.get(), () -> visionThreadTask(latestInputs.get())); + limelight.getName(), latestInputs.get(), () -> visionThreadTask(latestInputs.get())); + } } @Override @@ -103,8 +104,9 @@ public boolean canSeeAprilTags(Limelight limelight) { // } // } // return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = false; - return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = true; + return LimelightHelpers.getTV(limelight.getName()); // return true; + //latestInputs.get().limelightSeesAprilTags[limelight.getId()] = } /** From ee832b22c1a0778d00d89eee0ec1d540b5e2c9d8 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Tue, 26 Nov 2024 16:55:04 -0500 Subject: [PATCH 49/75] it works thats fucking crazy bro --- .../subsystems/vision/PhysicalVision.java | 35 ++++++++++--------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 3f54b1d7..bc53ff22 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -54,33 +54,34 @@ public PhysicalVision() { @Override public void updateInputs(VisionInputs inputs) { - limelightThreads.forEach( - (limelight, inputRef) -> { - VisionInputs limelightInputs = inputRef.get(); + // limelightThreads.forEach( + // (limelight, inputRef) -> { + // VisionInputs limelightInputs = inputRef.get(); // Combine inputs into the main inputs object - synchronized (limelightInputs) { + synchronized (inputs) { // checkAndUpdatePose(limelight); // if (limelightInputs != null) { - // for (Limelight limelight : Limelight.values()) { - inputs.isLimelightConnected[limelight.getId()] = limelightInputs.isLimelightConnected[limelight.getId()]; + for (Limelight limelight : Limelight.values()) { + inputs.isLimelightConnected[limelight.getId()] = isLimelightConnected(limelight); // inputs.limelightMegatag1Pose[limelight.getId()] = limelightInputs.limelightMegatag1Pose[limelight.getId()]; // inputs.limelightMegatag2Pose[limelight.getId()] = limelightInputs.limelightMegatag2Pose[limelight.getId()]; - inputs.limelightLatency[limelight.getId()] = limelightInputs.limelightLatency[limelight.getId()]; - inputs.limelightTargets[limelight.getId()] = limelightInputs.limelightTargets[limelight.getId()]; + inputs.limelightLatency[limelight.getId()] = getLatencySeconds(limelight); + inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); // inputs.limelightCameraToTargetPose[limelight.getId()] = limelightInputs.limelightCameraToTargetPose[limelight.getId()]; // inputs.limelightRobotToTargetPose[limelight.getId()] = limelightInputs.limelightRobotToTargetPose[limelight.getId()]; - inputs.limelightSeesAprilTags[limelight.getId()] = limelightInputs.limelightSeesAprilTags[limelight.getId()]; - inputs.limelightAprilTagDistance[limelight.getId()] = limelightInputs.limelightAprilTagDistance[limelight.getId()]; - inputs.limelightCalculatedPose[limelight.getId()] = limelightInputs.limelightCalculatedPose[limelight.getId()]; - inputs.limelightTimestamp[limelight.getId()] = limelightInputs.limelightTimestamp[limelight.getId()]; - // } - inputs.limelightLastSeenPose = limelightInputs.limelightLastSeenPose; + inputs.limelightSeesAprilTags[limelight.getId()] = canSeeAprilTags(limelight); + inputs.limelightAprilTagDistance[limelight.getId()] = getLimelightAprilTagDistance(limelight); + inputs.limelightCalculatedPose[limelight.getId()] = enabledPoseUpdate(limelight); + inputs.limelightTimestamp[limelight.getId()] = getTimeStampSeconds(limelight); + inputs.limelightLastSeenPose = getLastSeenPose(); + + latestInputs.set(inputs); + limelightThreads.get(limelight).set(latestInputs.get()); + } // } } - latestInputs.set(inputs); - limelightThreads.get(limelight).set(latestInputs.get()); - }); + // }); } /** From da0217876474edff470a0ba39585f42cfd7078df Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Wed, 27 Nov 2024 09:19:17 -0500 Subject: [PATCH 50/75] actually doesnt work prolly gotta rewrite evrything --- .../commands/drive/DriveCommandBase.java | 8 +- .../robot/subsystems/swerve/SwerveDrive.java | 3 +- .../subsystems/vision/PhysicalVision.java | 234 ++++++++---------- .../subsystems/vision/SimulatedVision.java | 26 +- .../frc/robot/subsystems/vision/Vision.java | 1 - .../subsystems/vision/VisionInterface.java | 4 +- 6 files changed, 121 insertions(+), 155 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java index b47bdfdf..3de39d96 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java @@ -2,15 +2,13 @@ package frc.robot.commands.drive; -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.extras.interpolators.MultiLinearInterpolator; import frc.robot.subsystems.swerve.SwerveDrive; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionConstants; import frc.robot.subsystems.vision.VisionConstants.Limelight; +import org.littletonrobotics.junction.Logger; public abstract class DriveCommandBase extends Command { private final MultiLinearInterpolator oneAprilTagLookupTable = @@ -75,8 +73,8 @@ public void calculatePoseFromLimelight(Limelight limelight) { Logger.getTimestamp() - vision.getLatencySeconds(limelight)); } - - Logger.recordOutput("Odometry/CurrentVisionPose" + limelight.getName(), vision.getPoseFromAprilTags(limelight)); + Logger.recordOutput( + "Odometry/CurrentVisionPose" + limelight.getName(), vision.getPoseFromAprilTags(limelight)); Logger.recordOutput("Odometry/CurrentCalculatePose", swerveDrive.getPose()); lastTimeStampSeconds = currentTimeStampSeconds; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 0b578c37..a6a90a8f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -292,7 +292,8 @@ public void addPoseEstimatorSwerveMeasurement(int timestampIndex) { odometry.updateWithTime( odometryThreadInputs.measurementTimeStamps[timestampIndex], // Logger.getTimestamp(), - rawGyroRotation, modulePositions); + rawGyroRotation, + modulePositions); } /** diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index bc53ff22..282ed7a9 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -1,14 +1,12 @@ package frc.robot.subsystems.vision; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants.FieldConstants; import frc.robot.extras.util.GeomUtil; import frc.robot.extras.vision.LimelightHelpers; import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; -import frc.robot.extras.vision.MegatagPoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; // import frc.robot.subsystems.vision.VisionInterface.VisionInputs; import java.util.concurrent.ConcurrentHashMap; @@ -24,7 +22,8 @@ public class PhysicalVision implements VisionInterface { private final ConcurrentHashMap> limelightThreads = new ConcurrentHashMap<>(); // private final ExecutorService executorService = Executors.newFixedThreadPool(3); - private final AtomicReference latestInputs = new AtomicReference<>(new VisionInputs()); + private final AtomicReference latestInputs = + new AtomicReference<>(new VisionInputs()); private final ThreadManager threadManager = new ThreadManager(Limelight.values().length); /** @@ -32,56 +31,41 @@ public class PhysicalVision implements VisionInterface { * frontLeftLimelight, frontRightLimelight} */ private PoseEstimate[] limelightEstimates = - new PoseEstimate[] {new PoseEstimate(), new PoseEstimate(), new PoseEstimate()}; + new PoseEstimate[] {new PoseEstimate(), new PoseEstimate(), new PoseEstimate()}; public PhysicalVision() { for (Limelight limelight : Limelight.values()) { limelightThreads.put(limelight, new AtomicReference<>(latestInputs.get())); - - // Initialize limelightEstimates with default PoseEstimate objects - // limelightEstimates[limelight.getId()] = new PoseEstimate(); - - + // Initialize limelightEstimates with default PoseEstimate objects + // limelightEstimates[limelight.getId()] = new PoseEstimate(); // Start a vision input task for each Limelight - - - threadManager.startVisionInputTask( - limelight.getName(), latestInputs.get(), () -> visionThreadTask(latestInputs.get())); + + threadManager.startVisionInputTask( + limelight.getName(), latestInputs.get(), () -> visionThreadTask(latestInputs.get())); } } @Override public void updateInputs(VisionInputs inputs) { - // limelightThreads.forEach( - // (limelight, inputRef) -> { - // VisionInputs limelightInputs = inputRef.get(); - // Combine inputs into the main inputs object - synchronized (inputs) { - // checkAndUpdatePose(limelight); - // if (limelightInputs != null) { - for (Limelight limelight : Limelight.values()) { - inputs.isLimelightConnected[limelight.getId()] = isLimelightConnected(limelight); - // inputs.limelightMegatag1Pose[limelight.getId()] = limelightInputs.limelightMegatag1Pose[limelight.getId()]; - // inputs.limelightMegatag2Pose[limelight.getId()] = limelightInputs.limelightMegatag2Pose[limelight.getId()]; - inputs.limelightLatency[limelight.getId()] = getLatencySeconds(limelight); - inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); - // inputs.limelightCameraToTargetPose[limelight.getId()] = limelightInputs.limelightCameraToTargetPose[limelight.getId()]; - // inputs.limelightRobotToTargetPose[limelight.getId()] = limelightInputs.limelightRobotToTargetPose[limelight.getId()]; - inputs.limelightSeesAprilTags[limelight.getId()] = canSeeAprilTags(limelight); - inputs.limelightAprilTagDistance[limelight.getId()] = getLimelightAprilTagDistance(limelight); - inputs.limelightCalculatedPose[limelight.getId()] = enabledPoseUpdate(limelight); - inputs.limelightTimestamp[limelight.getId()] = getTimeStampSeconds(limelight); - inputs.limelightLastSeenPose = getLastSeenPose(); - - latestInputs.set(inputs); - limelightThreads.get(limelight).set(latestInputs.get()); - } - // } - } - - // }); + // Combine inputs into the main inputs object + synchronized (inputs) { + for (Limelight limelight : Limelight.values()) { + inputs.isLimelightConnected[limelight.getId()] = isLimelightConnected(limelight); + inputs.limelightLatency[limelight.getId()] = getLatencySeconds(limelight); + inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); + inputs.limelightSeesAprilTags[limelight.getId()] = canSeeAprilTags(limelight); + inputs.limelightAprilTagDistance[limelight.getId()] = + getLimelightAprilTagDistance(limelight); + inputs.limelightCalculatedPose[limelight.getId()] = getPoseFromAprilTags(limelight); + inputs.limelightTimestamp[limelight.getId()] = getTimeStampSeconds(limelight); + inputs.limelightLastSeenPose = getLastSeenPose(); + + latestInputs.set(inputs); + limelightThreads.get(limelight).set(latestInputs.get()); + } + } } /** @@ -94,20 +78,21 @@ public void updateInputs(VisionInputs inputs) { public boolean canSeeAprilTags(Limelight limelight) { // First checks if it can see an april tag, then checks if it is fully in frame // Different Limelights have different FOVs - // if (getNumberOfAprilTags(limelight) > 0 - // && getNumberOfAprilTags(limelight) <= VisionConstants.APRIL_TAG_POSITIONS.length) { - // if (limelight.getName().equals(VisionConstants.SHOOTER_LIMELIGHT_NAME)) { - // return latestInputs.get().limelightSeesAprilTags[limelight.getId()] =Math.abs(LimelightHelpers.getTX(limelight.getName())) - // <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR; - // } else { - // return latestInputs.get().limelightSeesAprilTags[limelight.getId()] =Math.abs(LimelightHelpers.getTX(limelight.getName())) - // <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR; - // } - // } + if (getNumberOfAprilTags(limelight) > 0 + && getNumberOfAprilTags(limelight) <= VisionConstants.APRIL_TAG_POSITIONS.length) { + if (limelight.getName().equals(Limelight.SHOOTER.getName())) { + return Math.abs(LimelightHelpers.getTX(limelight.getName())) + <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR; + } else { + // return false; + return Math.abs(LimelightHelpers.getTX(limelight.getName())) + <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR; + } + } // return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = false; - return LimelightHelpers.getTV(limelight.getName()); - // return true; - //latestInputs.get().limelightSeesAprilTags[limelight.getId()] = + // return LimelightHelpers.getTV(limelight.getName()); + return false; + // latestInputs.get().limelightSeesAprilTags[limelight.getId()] = } /** @@ -116,23 +101,24 @@ public boolean canSeeAprilTags(Limelight limelight) { * * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). */ - public Pose2d enabledPoseUpdate(Limelight limelight) { - Pose2d megatag1Estimate = getMegaTag1PoseEstimate(limelight); - Pose2d megatag2Estimate = getMegaTag2PoseEstimate(limelight); + public PoseEstimate enabledPoseUpdate(Limelight limelight) { + PoseEstimate megatag1Estimate = getMegaTag1PoseEstimate(limelight); + PoseEstimate megatag2Estimate = getMegaTag2PoseEstimate(limelight); - if (canSeeAprilTags(limelight) && isValidPoseEstimate(limelight, megatag1Estimate, megatag2Estimate)) { + if (canSeeAprilTags(limelight) + && isValidPoseEstimate(limelight, megatag1Estimate, megatag2Estimate)) { if (isLargeDiscrepancyBetweenMegaTag1And2(limelight, megatag1Estimate, megatag2Estimate) && getLimelightAprilTagDistance(limelight) < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { - return limelightEstimates[limelight.getId()].pose = getMegaTag1PoseEstimate(limelight); + return limelightEstimates[limelight.getId()] = getMegaTag1PoseEstimate(limelight); } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) { LimelightHelpers.SetRobotOrientation(limelight.getName(), headingDegrees, 0, 0, 0, 0, 0); - return limelightEstimates[limelight.getId()].pose = getMegaTag2PoseEstimate(limelight); + return limelightEstimates[limelight.getId()] = getMegaTag2PoseEstimate(limelight); } else { - return limelightEstimates[limelight.getId()].pose = getMegaTag1PoseEstimate(limelight); + return limelightEstimates[limelight.getId()] = getMegaTag1PoseEstimate(limelight); } } - return limelightEstimates[limelight.getId()].pose = new Pose2d(); + return limelightEstimates[limelight.getId()] = new PoseEstimate(); } /** @@ -143,10 +129,10 @@ && getLimelightAprilTagDistance(limelight) */ public void updatePoseEstimate(Limelight limelight, VisionInputs inputs) { synchronized (inputs) { - inputs.limelightCalculatedPose[limelight.getId()] = - DriverStation.isEnabled() - ? enabledPoseUpdate(limelight) - : getMegaTag1PoseEstimate(limelight); + limelightEstimates[limelight.getId()] = + DriverStation.isEnabled() + ? enabledPoseUpdate(limelight) + : getMegaTag1PoseEstimate(limelight); } } @@ -156,14 +142,15 @@ public void updatePoseEstimate(Limelight limelight, VisionInputs inputs) { * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return true if the discrepancy is larger than the defined threshold, false otherwise */ - public boolean isLargeDiscrepancyBetweenMegaTag1And2(Limelight limelight, Pose2d mt1, Pose2d mt2) { + public boolean isLargeDiscrepancyBetweenMegaTag1And2( + Limelight limelight, PoseEstimate mt1, PoseEstimate mt2) { return !GeomUtil.isTranslationWithinThreshold( - mt1.getTranslation(), - mt2.getTranslation(), + mt1.pose.getTranslation(), + mt2.pose.getTranslation(), VisionConstants.MEGA_TAG_TRANSLATION_DISCREPANCY_THRESHOLD) || !GeomUtil.isRotationWithinThreshold( - mt1.getRotation().getDegrees(), - mt2.getRotation().getDegrees(), + mt1.pose.getRotation().getDegrees(), + mt2.pose.getRotation().getDegrees(), VisionConstants.MEGA_TAG_ROTATION_DISCREPANCY_THREASHOLD); // return true; } @@ -176,8 +163,8 @@ public boolean isLargeDiscrepancyBetweenMegaTag1And2(Limelight limelight, Pose2d * @return the MegaTag1 pose of the robot, if the limelight can't see any April Tags, it will * return 0 for x, y, and theta */ - public Pose2d getMegaTag1PoseEstimate(Limelight limelight) { - return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()).pose; + public PoseEstimate getMegaTag1PoseEstimate(Limelight limelight) { + return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()); } /** @@ -188,8 +175,8 @@ public Pose2d getMegaTag1PoseEstimate(Limelight limelight) { * @return the MegaTag2 pose of the robot, if the limelight can't see any April Tags, it will * return 0 for x, y, and theta */ - public Pose2d getMegaTag2PoseEstimate(Limelight limelight) { - return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName()).pose; + public PoseEstimate getMegaTag2PoseEstimate(Limelight limelight) { + return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName()); } /** @@ -198,11 +185,11 @@ public Pose2d getMegaTag2PoseEstimate(Limelight limelight) { * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). * @return true if the pose estimate exists within the field and the pose estimate is not null */ - public boolean isValidPoseEstimate(Limelight limelight, Pose2d mt1, Pose2d mt2) { - // return LimelightHelpers.isValidPoseEstimate(mt1) - // && LimelightHelpers.isValidPoseEstimate(mt2) - // && - return isWithinFieldBounds(mt1, mt2); + public boolean isValidPoseEstimate(Limelight limelight, PoseEstimate mt1, PoseEstimate mt2) { + return LimelightHelpers.isValidPoseEstimate(mt1) + && LimelightHelpers.isValidPoseEstimate(mt2) + && + isWithinFieldBounds(mt1, mt2); } /** @@ -211,16 +198,15 @@ public boolean isValidPoseEstimate(Limelight limelight, Pose2d mt1, Pose2d mt2) * @param megaTag1Estimate the MT1 pose estimate to check * @param megaTag2Estimate the MT2 pose estimate to check */ - private boolean isWithinFieldBounds( - Pose2d megaTag1Estimate, Pose2d megaTag2Estimate) { - return (megaTag1Estimate.getX() > 0 - && megaTag1Estimate.getX() <= FieldConstants.FIELD_WIDTH_METERS) - && (megaTag1Estimate.getY() > 0 - && megaTag1Estimate.getY() <= FieldConstants.FIELD_WIDTH_METERS) - && (megaTag2Estimate.getX() > 0 - && megaTag2Estimate.getX() <= FieldConstants.FIELD_WIDTH_METERS) - && (megaTag2Estimate.getY() > 0 - && megaTag2Estimate.getY() <= FieldConstants.FIELD_WIDTH_METERS); + private boolean isWithinFieldBounds(PoseEstimate megaTag1Estimate, PoseEstimate megaTag2Estimate) { + return (megaTag1Estimate.pose.getX() > 0 + && megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag1Estimate.pose.getY() > 0 + && megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag2Estimate.pose.getX() > 0 + && megaTag2Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag2Estimate.pose.getY() > 0 + && megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS); } /** @@ -232,7 +218,7 @@ private boolean isWithinFieldBounds( */ @Override public Pose2d getPoseFromAprilTags(Limelight limelight) { - return latestInputs.get().limelightCalculatedPose[limelight.getId()] =limelightEstimates[limelight.getId()].pose; + return limelightEstimates[limelight.getId()].pose; } // public Pose2d getAprilTagPositionToLimelight(Limelight limelight) { @@ -240,13 +226,14 @@ public Pose2d getPoseFromAprilTags(Limelight limelight) { // } // public Pose2d getAprilTagPositionToRobot(Limelight limelight) { - // return latestInputs.get().limelightRobotToTargetPose[limelight.getId()] =LimelightHelpers.getTargetPose_RobotSpace(limelight.getName()); + // return latestInputs.get().limelightRobotToTargetPose[limelight.getId()] + // =LimelightHelpers.getTargetPose_RobotSpace(limelight.getName()); // } /** Returns how many april tags the limelight that is being used for pose estimation can see. */ @Override public int getNumberOfAprilTags(Limelight limelight) { - return latestInputs.get().limelightTargets[limelight.getId()] = limelightEstimates[limelight.getId()].tagCount; + return limelightEstimates[limelight.getId()].tagCount; } /** @@ -271,7 +258,7 @@ public double getLatencySeconds(Limelight limelight) { /** Gets the pose calculated the last time a limelight saw an April Tag */ @Override public Pose2d getLastSeenPose() { - return latestInputs.get().limelightLastSeenPose = lastSeenPose; + return lastSeenPose; } /** @@ -305,10 +292,7 @@ public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { public boolean isLimelightConnected(Limelight limelight) { NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(limelight.getName()); - if (limelightTable.containsKey(limelight.getName())) { - return true; - } - return false; + return limelightTable.containsKey("tv"); } /** @@ -316,7 +300,7 @@ public boolean isLimelightConnected(Limelight limelight) { * * @param limelight the limelight number */ - public void checkAndUpdatePose(Limelight limelight) { + public void checkAndUpdatePose(Limelight limelight, VisionInputs inputs) { double last_TX = 0; double last_TY = 0; @@ -338,32 +322,33 @@ public void checkAndUpdatePose(Limelight limelight) { // very demanding whereas this only has to get the Network Table entries for TX and TY. if (current_TX != last_TX || current_TY != last_TY) { - updatePoseEstimate(limelight, latestInputs.get()); - limelightThreads.computeIfPresent( - limelight, (key, value) -> latestInputs); + updatePoseEstimate(limelight, inputs); + latestInputs.set(inputs); + + limelightThreads.computeIfPresent(limelight, (key, value) -> latestInputs); // // Handle threading for Limelight (start or stop threads if needed) - // // Check if this Limelight thread exists in limelightThreads - if (limelightThreads.get(limelight) != null) { - // Update thread inputs or restart the thread if needed - limelightThreads.get(limelight).set(latestInputs.get()); - } + // Check if this Limelight thread exists in limelightThreads + if (limelightThreads.get(limelight) != null) { + // Update thread inputs or restart the thread if needed + limelightThreads.get(limelight).set(latestInputs.get()); + } // This is to keep track of the last valid pose calculated by the limelights // it is used when the driver resets the robot odometry to the limelight calculated // position if (canSeeAprilTags(limelight)) { - lastSeenPose = getMegaTag1PoseEstimate(limelight); + lastSeenPose = getMegaTag1PoseEstimate(limelight).pose; } } else { - // Retrieve the AtomicReference for the given limelight number + // // Retrieve the AtomicReference for the given limelight number AtomicReference isThreadRunning = limelightThreads.getOrDefault(limelight, latestInputs); // Only stop the thread if it's currently running if (isThreadRunning.get() != null) { - // stop the thread for the specified limelight - stopLimelightThread(limelight); - } + // stop the thread for the specified limelight + stopLimelightThread(limelight); } + } last_TX = current_TX; last_TY = current_TY; } catch (Exception e) { @@ -387,35 +372,16 @@ public void checkAndUpdatePose(Limelight limelight) { * * @param limelight the limelight number */ - public void visionThreadTask(VisionInputs inputs) { //Limelight limelight + public void visionThreadTask(VisionInputs inputs) { // Limelight limelight try { synchronized (inputs) { for (Limelight limelight : Limelight.values()) { - checkAndUpdatePose(limelight); - // updatePoseEstimate(limelight, inputs); - - inputs.isLimelightConnected[limelight.getId()] = isLimelightConnected(limelight); - // inputs.limelightCameraToTargetPose[limelight.getId()] = getAprilTagPositionToLimelight(limelight); - // inputs.limelightRobotToTargetPose[limelight.getId()] = getAprilTagPositionToRobot(limelight); - inputs.limelightLatency[limelight.getId()] = getLatencySeconds(limelight); - inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); - inputs.limelightSeesAprilTags[limelight.getId()] = canSeeAprilTags(limelight); - inputs.limelightAprilTagDistance[limelight.getId()] = getLimelightAprilTagDistance(limelight); - inputs.limelightCalculatedPose[limelight.getId()] = getPoseFromAprilTags(limelight); - inputs.limelightTimestamp[limelight.getId()] = getTimeStampSeconds(limelight); - inputs.limelightLastSeenPose = getLastSeenPose(); - - latestInputs.set(inputs); - limelightThreads.get(limelight).set(latestInputs.get()); - - + checkAndUpdatePose(limelight, inputs); } } - } catch (Exception e) { - // System.err.println( - // "Error updating inputs for " + limelight.getName() + ": " + e.getMessage()); + e.printStackTrace(); } } diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 975c4c2c..103f5dfb 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import frc.robot.extras.vision.LimelightHelpers; import frc.robot.subsystems.vision.VisionConstants.Limelight; import java.util.ArrayList; @@ -11,7 +10,6 @@ import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; import org.photonvision.PhotonCamera; -import org.photonvision.estimation.TargetModel; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; @@ -48,8 +46,8 @@ public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { // Create simulated camera properties. These can be set to mimic your actual // camera. var cameraProperties = new SimCameraProperties(); - cameraProperties.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7)); - cameraProperties.setCalibError(0.35, 0.10); + // cameraProperties.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7)); + // cameraProperties.setCalibError(0.35, 0.10); cameraProperties.setFPS(15); cameraProperties.setAvgLatencyMs(20); cameraProperties.setLatencyStdDevMs(5); @@ -63,14 +61,18 @@ public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { frontRightCameraSim = new PhotonCameraSim(frontRightCamera, cameraProperties); Transform3d robotToShooterCamera = - new Transform3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, 0.0, 0.0)); + new Transform3d(new Translation3d(-0.3119324724, 0.0, 0.1865472012), new Rotation3d(0.0, + 35, 180.0)); visionSim.addCamera(shooterCameraSim, robotToShooterCamera); - Transform3d robotToFrontLeftCamera = - new Transform3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, 0.0, 0.0)); - visionSim.addCamera(frontLeftCameraSim, robotToFrontLeftCamera); Transform3d robotToFrontRightCamera = - new Transform3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, 0.0, 0.0)); + new Transform3d(new Translation3d(0.2749477356, -0.269958439, 0.2318054546), new + Rotation3d(0.0, 25, -35)); + Transform3d robotToFrontLeftCamera = + new Transform3d(new Translation3d(0.2816630892, 0.2724405524, 0.232156), new + Rotation3d(0.0, 25, 35)); + visionSim.addCamera(frontLeftCameraSim, robotToFrontLeftCamera); + visionSim.addCamera(frontRightCameraSim, robotToFrontRightCamera); // Enable the raw and processed streams. (http://localhost:1181 / 1182) @@ -99,8 +101,10 @@ public void updateInputs(VisionInputs inputs) { } NetworkTable shooterTable = LimelightHelpers.getLimelightNTTable(Limelight.SHOOTER.getName()); - NetworkTable frontLeftTable = LimelightHelpers.getLimelightNTTable(Limelight.FRONT_LEFT.getName()); - NetworkTable frontRightTable = LimelightHelpers.getLimelightNTTable(Limelight.FRONT_RIGHT.getName()); + NetworkTable frontLeftTable = + LimelightHelpers.getLimelightNTTable(Limelight.FRONT_LEFT.getName()); + NetworkTable frontRightTable = + LimelightHelpers.getLimelightNTTable(Limelight.FRONT_RIGHT.getName()); // Write to limelight table writeToTable(shooterCamera.getAllUnreadResults(), shooterTable, Limelight.SHOOTER); writeToTable(frontLeftCamera.getAllUnreadResults(), frontLeftTable, Limelight.FRONT_LEFT); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 435842b8..c8332f54 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -57,5 +57,4 @@ public Pose2d getPoseFromAprilTags(Limelight limelight) { public Pose2d getLastSeenPose() { return inputs.limelightLastSeenPose; } - } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index 4060e8c1..5a20d8a6 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -1,8 +1,6 @@ package frc.robot.subsystems.vision; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import frc.robot.extras.vision.MegatagPoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; import org.littletonrobotics.junction.AutoLog; @@ -31,7 +29,7 @@ default void updateInputs(VisionInputs inputs) {} default double getLatencySeconds(Limelight limelight) { return 0.0; } - + default double getTimeStampSeconds(Limelight limelight) { return 0.0; } From cd269d18462db74ef9edc030a08a9ed23e46ef39 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 28 Nov 2024 18:46:37 -0500 Subject: [PATCH 51/75] fixes frfr --- .../extras/vision/MegatagPoseEstimate.java | 11 ++- .../subsystems/vision/PhysicalVision.java | 40 ++++---- .../subsystems/vision/SimulatedVision.java | 49 +++++----- .../frc/robot/subsystems/vision/Vision.java | 2 +- .../subsystems/vision/VisionConstants.java | 26 +++++ .../subsystems/vision/VisionInterface.java | 7 +- ...ta-2.json => Studica-2025.1.1-beta-3.json} | 14 +-- vendordeps/photonlib-v2025.0.0-beta-2.json | 97 ------------------- vendordeps/photonlib.json | 71 ++++++++++++++ 9 files changed, 167 insertions(+), 150 deletions(-) rename vendordeps/{Studica-2025.1.1-beta-2.json => Studica-2025.1.1-beta-3.json} (87%) delete mode 100644 vendordeps/photonlib-v2025.0.0-beta-2.json create mode 100644 vendordeps/photonlib.json diff --git a/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java b/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java index bf84f7ca..04a7d7a2 100644 --- a/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java +++ b/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java @@ -11,6 +11,8 @@ public static class MegatagPoseEstimateStruct implements Struct getTypeClass() { @@ -29,7 +31,7 @@ public int getSize() { @Override public String getSchema() { - return "Pose2d fieldToCamera;double timestampSeconds;double latency;double avgTagArea"; + return "Pose2d fieldToCamera;double timestampSeconds;double latency;double avgTagArea; int tagCount; double avgTagDist"; } @Override @@ -45,6 +47,8 @@ public MegatagPoseEstimate unpack(ByteBuffer bb) { rv.latency = bb.getDouble(); rv.avgTagArea = bb.getDouble(); rv.fiducialIds = new int[0]; + rv.avgTagDist = bb.getDouble(); + rv.timestampSeconds = bb.getInt(); return rv; } @@ -54,6 +58,7 @@ public void pack(ByteBuffer bb, MegatagPoseEstimate value) { bb.putDouble(value.timestampSeconds); bb.putDouble(value.latency); bb.putDouble(value.avgTagArea); + bb.putDouble(value.avgTagDist); } @Override @@ -68,6 +73,8 @@ public String getTypeName() { public double latency; public double avgTagArea; public int[] fiducialIds; + public int tagCount; + public double avgTagDist; public MegatagPoseEstimate() {} @@ -78,6 +85,8 @@ public static MegatagPoseEstimate fromLimelight(LimelightHelpers.PoseEstimate po rv.timestampSeconds = poseEstimate.timestampSeconds; rv.latency = poseEstimate.latency; rv.avgTagArea = poseEstimate.avgTagArea; + rv.avgTagDist = poseEstimate.avgTagDist; + rv.tagCount = poseEstimate.tagCount; rv.fiducialIds = new int[poseEstimate.rawFiducials.length]; for (int i = 0; i < rv.fiducialIds.length; ++i) { rv.fiducialIds[i] = poseEstimate.rawFiducials[i].id; diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 282ed7a9..0c1b392d 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -7,6 +7,7 @@ import frc.robot.extras.util.GeomUtil; import frc.robot.extras.vision.LimelightHelpers; import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; +import frc.robot.extras.vision.MegatagPoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; // import frc.robot.subsystems.vision.VisionInterface.VisionInputs; import java.util.concurrent.ConcurrentHashMap; @@ -30,18 +31,16 @@ public class PhysicalVision implements VisionInterface { * The pose estimates from the limelights in the following order {shooterLimelight, * frontLeftLimelight, frontRightLimelight} */ - private PoseEstimate[] limelightEstimates = - new PoseEstimate[] {new PoseEstimate(), new PoseEstimate(), new PoseEstimate()}; + private MegatagPoseEstimate[] limelightEstimates = + new MegatagPoseEstimate[] { + new MegatagPoseEstimate(), new MegatagPoseEstimate(), new MegatagPoseEstimate() + }; public PhysicalVision() { for (Limelight limelight : Limelight.values()) { limelightThreads.put(limelight, new AtomicReference<>(latestInputs.get())); - // Initialize limelightEstimates with default PoseEstimate objects - // limelightEstimates[limelight.getId()] = new PoseEstimate(); - // Start a vision input task for each Limelight - threadManager.startVisionInputTask( limelight.getName(), latestInputs.get(), () -> visionThreadTask(latestInputs.get())); } @@ -56,6 +55,7 @@ public void updateInputs(VisionInputs inputs) { inputs.limelightLatency[limelight.getId()] = getLatencySeconds(limelight); inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); inputs.limelightSeesAprilTags[limelight.getId()] = canSeeAprilTags(limelight); + inputs.limelightMegatagPose[limelight.getId()] = enabledPoseUpdate(limelight); inputs.limelightAprilTagDistance[limelight.getId()] = getLimelightAprilTagDistance(limelight); inputs.limelightCalculatedPose[limelight.getId()] = getPoseFromAprilTags(limelight); @@ -101,7 +101,7 @@ && getNumberOfAprilTags(limelight) <= VisionConstants.APRIL_TAG_POSITIONS.length * * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). */ - public PoseEstimate enabledPoseUpdate(Limelight limelight) { + public MegatagPoseEstimate enabledPoseUpdate(Limelight limelight) { PoseEstimate megatag1Estimate = getMegaTag1PoseEstimate(limelight); PoseEstimate megatag2Estimate = getMegaTag2PoseEstimate(limelight); @@ -110,15 +110,18 @@ && isValidPoseEstimate(limelight, megatag1Estimate, megatag2Estimate)) { if (isLargeDiscrepancyBetweenMegaTag1And2(limelight, megatag1Estimate, megatag2Estimate) && getLimelightAprilTagDistance(limelight) < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { - return limelightEstimates[limelight.getId()] = getMegaTag1PoseEstimate(limelight); + return limelightEstimates[limelight.getId()] = + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) { LimelightHelpers.SetRobotOrientation(limelight.getName(), headingDegrees, 0, 0, 0, 0, 0); - return limelightEstimates[limelight.getId()] = getMegaTag2PoseEstimate(limelight); + return limelightEstimates[limelight.getId()] = + MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(limelight)); } else { - return limelightEstimates[limelight.getId()] = getMegaTag1PoseEstimate(limelight); + return limelightEstimates[limelight.getId()] = + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); } } - return limelightEstimates[limelight.getId()] = new PoseEstimate(); + return limelightEstimates[limelight.getId()] = new MegatagPoseEstimate(); } /** @@ -132,7 +135,7 @@ public void updatePoseEstimate(Limelight limelight, VisionInputs inputs) { limelightEstimates[limelight.getId()] = DriverStation.isEnabled() ? enabledPoseUpdate(limelight) - : getMegaTag1PoseEstimate(limelight); + : MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); } } @@ -188,8 +191,7 @@ public PoseEstimate getMegaTag2PoseEstimate(Limelight limelight) { public boolean isValidPoseEstimate(Limelight limelight, PoseEstimate mt1, PoseEstimate mt2) { return LimelightHelpers.isValidPoseEstimate(mt1) && LimelightHelpers.isValidPoseEstimate(mt2) - && - isWithinFieldBounds(mt1, mt2); + && isWithinFieldBounds(mt1, mt2); } /** @@ -198,7 +200,8 @@ public boolean isValidPoseEstimate(Limelight limelight, PoseEstimate mt1, PoseEs * @param megaTag1Estimate the MT1 pose estimate to check * @param megaTag2Estimate the MT2 pose estimate to check */ - private boolean isWithinFieldBounds(PoseEstimate megaTag1Estimate, PoseEstimate megaTag2Estimate) { + private boolean isWithinFieldBounds( + PoseEstimate megaTag1Estimate, PoseEstimate megaTag2Estimate) { return (megaTag1Estimate.pose.getX() > 0 && megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) && (megaTag1Estimate.pose.getY() > 0 @@ -218,7 +221,7 @@ private boolean isWithinFieldBounds(PoseEstimate megaTag1Estimate, PoseEstimate */ @Override public Pose2d getPoseFromAprilTags(Limelight limelight) { - return limelightEstimates[limelight.getId()].pose; + return limelightEstimates[limelight.getId()].fieldToCamera; } // public Pose2d getAprilTagPositionToLimelight(Limelight limelight) { @@ -234,6 +237,8 @@ public Pose2d getPoseFromAprilTags(Limelight limelight) { @Override public int getNumberOfAprilTags(Limelight limelight) { return limelightEstimates[limelight.getId()].tagCount; + // latestInputs.get().limelightMegatagPose[limelight.getId()].fiducialIds.length; + // return limelightEstimates[limelight.getId()].tagCount; } /** @@ -270,7 +275,8 @@ public Pose2d getLastSeenPose() { @Override public double getLimelightAprilTagDistance(Limelight limelight) { if (canSeeAprilTags(limelight)) { - return limelightEstimates[limelight.getId()].avgTagDist; + // return limelightEstimates[limelight.getId()].; + return 2; } // To be safe returns a big distance from the april tags if it can't see any return Double.MAX_VALUE; diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 103f5dfb..06c3763b 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -19,9 +19,9 @@ // Please see the following link for example code // https://github.com/PhotonVision/photonvision/blob/2a6fa1b6ac81f239c59d724da5339f608897c510/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java public class SimulatedVision extends PhysicalVision { - private final PhotonCamera shooterCamera = new PhotonCamera("shooterCamera"); - private final PhotonCamera frontLeftCamera = new PhotonCamera("frontLeftCamera"); - private final PhotonCamera frontRightCamera = new PhotonCamera("frontRightCamera"); + // private final PhotonCamera shooterCamera = new PhotonCamera("shooterCamera"); + // private final PhotonCamera frontLeftCamera = new PhotonCamera("frontLeftCamera"); + // private final PhotonCamera frontRightCamera = new PhotonCamera("frontRightCamera"); PhotonCameraSim shooterCameraSim; PhotonCameraSim frontLeftCameraSim; PhotonCameraSim frontRightCameraSim; @@ -31,6 +31,8 @@ public class SimulatedVision extends PhysicalVision { private final int kResWidth = 1280; private final int kResHeight = 800; + private int[] tagCount = new int[Limelight.values().length]; + public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { super(); this.robotSimulationPose = robotActualPoseInSimulationSupplier; @@ -56,21 +58,22 @@ public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { // with visible // targets. // Instance variables - shooterCameraSim = new PhotonCameraSim(shooterCamera, cameraProperties); - frontLeftCameraSim = new PhotonCameraSim(frontLeftCamera, cameraProperties); - frontRightCameraSim = new PhotonCameraSim(frontRightCamera, cameraProperties); + shooterCameraSim = new PhotonCameraSim(Limelight.SHOOTER.getSimulationCamera(Limelight.SHOOTER), cameraProperties); + frontLeftCameraSim = new PhotonCameraSim(Limelight.FRONT_LEFT.getSimulationCamera(Limelight.FRONT_LEFT), cameraProperties); + frontRightCameraSim = new PhotonCameraSim(Limelight.FRONT_RIGHT.getSimulationCamera(Limelight.FRONT_RIGHT), cameraProperties); Transform3d robotToShooterCamera = - new Transform3d(new Translation3d(-0.3119324724, 0.0, 0.1865472012), new Rotation3d(0.0, - 35, 180.0)); + new Transform3d( + new Translation3d(-0.3119324724, 0.0, 0.1865472012), new Rotation3d(0.0, 35, 180.0)); visionSim.addCamera(shooterCameraSim, robotToShooterCamera); Transform3d robotToFrontRightCamera = - new Transform3d(new Translation3d(0.2749477356, -0.269958439, 0.2318054546), new - Rotation3d(0.0, 25, -35)); - Transform3d robotToFrontLeftCamera = - new Transform3d(new Translation3d(0.2816630892, 0.2724405524, 0.232156), new - Rotation3d(0.0, 25, 35)); + new Transform3d( + new Translation3d(0.2749477356, -0.269958439, 0.2318054546), + new Rotation3d(0.0, 25, -35)); + Transform3d robotToFrontLeftCamera = + new Transform3d( + new Translation3d(0.2816630892, 0.2724405524, 0.232156), new Rotation3d(0.0, 25, 35)); visionSim.addCamera(frontLeftCameraSim, robotToFrontLeftCamera); visionSim.addCamera(frontRightCameraSim, robotToFrontRightCamera); @@ -100,16 +103,10 @@ public void updateInputs(VisionInputs inputs) { Logger.recordOutput("Vision/SimIO/updateSimPose", robotSimulationPose.get()); } - NetworkTable shooterTable = LimelightHelpers.getLimelightNTTable(Limelight.SHOOTER.getName()); - NetworkTable frontLeftTable = - LimelightHelpers.getLimelightNTTable(Limelight.FRONT_LEFT.getName()); - NetworkTable frontRightTable = - LimelightHelpers.getLimelightNTTable(Limelight.FRONT_RIGHT.getName()); - // Write to limelight table - writeToTable(shooterCamera.getAllUnreadResults(), shooterTable, Limelight.SHOOTER); - writeToTable(frontLeftCamera.getAllUnreadResults(), frontLeftTable, Limelight.FRONT_LEFT); - writeToTable(frontRightCamera.getAllUnreadResults(), frontRightTable, Limelight.FRONT_RIGHT); - + for (Limelight limelight : Limelight.values()) { + writeToTable(limelight.getSimulationCamera(limelight).getAllUnreadResults(), limelight.getLimelightTable(limelight), limelight); + inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); + } super.updateInputs(inputs); } @@ -153,6 +150,7 @@ private void writeToTable( table .getEntry("botpose_orb_wpiblue") .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); + tagCount[limelight.getId()] = result.getMultiTagResult().get().fiducialIDsUsed.size(); } table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); @@ -165,4 +163,9 @@ public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { super.setHeadingInfo(headingDegrees, headingRateDegrees); } + @Override + public int getNumberOfAprilTags(Limelight limelight) { + // TODO Auto-generated method stub + return tagCount[limelight.getId()]; + } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index c8332f54..4f34c456 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -26,7 +26,7 @@ public void periodic() { // Add methods to support DriveCommand public int getNumberOfAprilTags(Limelight limelight) { - return inputs.limelightTargets[limelight.getId()]; + return visionInterface.getNumberOfAprilTags(limelight); } public double getLimelightAprilTagDistance(Limelight limelight) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 51679e36..d5d3da18 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -1,8 +1,13 @@ package frc.robot.subsystems.vision; +import org.photonvision.PhotonCamera; +import org.photonvision.proto.Photon; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTable; +import frc.robot.extras.vision.LimelightHelpers; public final class VisionConstants { public enum Limelight { @@ -34,8 +39,29 @@ public static Limelight fromId(int id) { default -> throw new IllegalArgumentException("Invalid Limelight ID: " + id); }; } + + public PhotonCamera getSimulationCamera(Limelight limelight) { + return switch (limelight) { + case SHOOTER -> SHOOTER_CAMERA; + case FRONT_LEFT -> FRONT_LEFT_CAMERA; + case FRONT_RIGHT -> FRONT_RIGHT_CAMERA; + default -> throw new IllegalArgumentException("Invalid limelight camera " + limelight); + }; + } + + public NetworkTable getLimelightTable(Limelight limelight) { + return switch (limelight) { + case SHOOTER -> LimelightHelpers.getLimelightNTTable(SHOOTER.getName()); + case FRONT_LEFT -> LimelightHelpers.getLimelightNTTable(FRONT_LEFT.getName()); + case FRONT_RIGHT -> LimelightHelpers.getLimelightNTTable(FRONT_RIGHT.getName()); + default -> throw new IllegalArgumentException("Invalid limelight " + limelight); + }; + } } + public static final PhotonCamera SHOOTER_CAMERA = new PhotonCamera(Limelight.SHOOTER.getName()); + public static final PhotonCamera FRONT_LEFT_CAMERA = new PhotonCamera(Limelight.FRONT_LEFT.getName()); + public static final PhotonCamera FRONT_RIGHT_CAMERA = new PhotonCamera(Limelight.FRONT_RIGHT.getName()); public static final int THREAD_SLEEP_MS = 20; public static final AprilTagFieldLayout FIELD_LAYOUT = diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index 5a20d8a6..895ff312 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.vision; import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.extras.vision.MegatagPoseEstimate; import frc.robot.subsystems.vision.VisionConstants.Limelight; import org.littletonrobotics.junction.AutoLog; @@ -9,12 +10,10 @@ public interface VisionInterface { class VisionInputs { public boolean[] isLimelightConnected = new boolean[Limelight.values().length]; - // public Pose2d[] limelightMegatag1Pose = new Pose2d[Limelight.values().length]; - // public Pose2d[] limelightMegatag2Pose = new Pose2d[Limelight.values().length]; + public MegatagPoseEstimate[] limelightMegatagPose = + new MegatagPoseEstimate[Limelight.values().length]; public double[] limelightLatency = new double[Limelight.values().length]; public int[] limelightTargets = new int[Limelight.values().length]; - // public Pose2d[] limelightCameraToTargetPose = new Pose2d[Limelight.values().length]; - // public Pose2d[] limelightRobotToTargetPose = new Pose2d[Limelight.values().length]; public boolean[] limelightSeesAprilTags = new boolean[Limelight.values().length]; public Pose2d[] limelightCalculatedPose = new Pose2d[Limelight.values().length]; diff --git a/vendordeps/Studica-2025.1.1-beta-2.json b/vendordeps/Studica-2025.1.1-beta-3.json similarity index 87% rename from vendordeps/Studica-2025.1.1-beta-2.json rename to vendordeps/Studica-2025.1.1-beta-3.json index b9aa624c..2f64aaf4 100644 --- a/vendordeps/Studica-2025.1.1-beta-2.json +++ b/vendordeps/Studica-2025.1.1-beta-3.json @@ -1,13 +1,13 @@ { - "fileName": "Studica-2025.1.1-beta-2.json", + "fileName": "Studica-2025.1.1-beta-3.json", "name": "Studica", - "version": "2025.1.1-beta-2", + "version": "2025.1.1-beta-3", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "frcYear": "2025", "mavenUrls": [ "https://dev.studica.com/maven/release/2025/" ], - "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-2.json", + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-3.json", "cppDependencies": [ { "artifactId": "Studica-cpp", @@ -24,7 +24,7 @@ "libName": "Studica", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.1.1-beta-2" + "version": "2025.1.1-beta-3" }, { "artifactId": "Studica-driver", @@ -41,14 +41,14 @@ "libName": "StudicaDriver", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.1.1-beta-2" + "version": "2025.1.1-beta-3" } ], "javaDependencies": [ { "artifactId": "Studica-java", "groupId": "com.studica.frc", - "version": "2025.1.1-beta-2" + "version": "2025.1.1-beta-3" } ], "jniDependencies": [ @@ -65,7 +65,7 @@ "osxuniversal", "windowsx86-64" ], - "version": "2025.1.1-beta-2" + "version": "2025.1.1-beta-3" } ] } \ No newline at end of file diff --git a/vendordeps/photonlib-v2025.0.0-beta-2.json b/vendordeps/photonlib-v2025.0.0-beta-2.json deleted file mode 100644 index 73fcefd1..00000000 --- a/vendordeps/photonlib-v2025.0.0-beta-2.json +++ /dev/null @@ -1,97 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2025.0.0-beta-2", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2025", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [ - { - "groupId": "edu.wpi.first.wpilibc", - "artifactId": "wpilibc-cpp", - "version": "2025.1.1-beta-1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-2", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-jni", - "version": "v2025.0.0-beta-2", - "skipInvalidPlatforms": true, - "isJar": true, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2025.0.0-beta-2", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-2", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2025.0.0-beta-2" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2025.0.0-beta-2" - } - ] -} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 00000000..ad3a530c --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.0.0-beta-5", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.0.0-beta-5", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-5", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.0.0-beta-5" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.0.0-beta-5" + } + ] +} \ No newline at end of file From ac8226b3345dafa1d203b4002ef557b6afa9ab94 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 28 Nov 2024 18:47:16 -0500 Subject: [PATCH 52/75] typo --- src/main/java/frc/robot/commands/drive/DriveCommandBase.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java index 3de39d96..83f8aab0 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java @@ -75,7 +75,7 @@ public void calculatePoseFromLimelight(Limelight limelight) { Logger.recordOutput( "Odometry/CurrentVisionPose" + limelight.getName(), vision.getPoseFromAprilTags(limelight)); - Logger.recordOutput("Odometry/CurrentCalculatePose", swerveDrive.getPose()); + Logger.recordOutput("Odometry/CurrentCalculatedPose", swerveDrive.getPose()); lastTimeStampSeconds = currentTimeStampSeconds; } From 75cf72402ca647d4fbd4b90bbb67b2e07d01ea3f Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 28 Nov 2024 18:48:11 -0500 Subject: [PATCH 53/75] format --- .../subsystems/vision/SimulatedVision.java | 23 ++++++++++++------- .../subsystems/vision/VisionConstants.java | 14 +++++------ 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 06c3763b..054cb08d 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -2,14 +2,12 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.networktables.NetworkTable; -import frc.robot.extras.vision.LimelightHelpers; import frc.robot.subsystems.vision.VisionConstants.Limelight; import java.util.ArrayList; import java.util.Arrays; import java.util.List; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; -import org.photonvision.PhotonCamera; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; @@ -58,9 +56,15 @@ public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { // with visible // targets. // Instance variables - shooterCameraSim = new PhotonCameraSim(Limelight.SHOOTER.getSimulationCamera(Limelight.SHOOTER), cameraProperties); - frontLeftCameraSim = new PhotonCameraSim(Limelight.FRONT_LEFT.getSimulationCamera(Limelight.FRONT_LEFT), cameraProperties); - frontRightCameraSim = new PhotonCameraSim(Limelight.FRONT_RIGHT.getSimulationCamera(Limelight.FRONT_RIGHT), cameraProperties); + shooterCameraSim = + new PhotonCameraSim( + Limelight.SHOOTER.getSimulationCamera(Limelight.SHOOTER), cameraProperties); + frontLeftCameraSim = + new PhotonCameraSim( + Limelight.FRONT_LEFT.getSimulationCamera(Limelight.FRONT_LEFT), cameraProperties); + frontRightCameraSim = + new PhotonCameraSim( + Limelight.FRONT_RIGHT.getSimulationCamera(Limelight.FRONT_RIGHT), cameraProperties); Transform3d robotToShooterCamera = new Transform3d( @@ -104,8 +108,11 @@ public void updateInputs(VisionInputs inputs) { } for (Limelight limelight : Limelight.values()) { - writeToTable(limelight.getSimulationCamera(limelight).getAllUnreadResults(), limelight.getLimelightTable(limelight), limelight); - inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); + writeToTable( + limelight.getSimulationCamera(limelight).getAllUnreadResults(), + limelight.getLimelightTable(limelight), + limelight); + inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); } super.updateInputs(inputs); } @@ -150,7 +157,7 @@ private void writeToTable( table .getEntry("botpose_orb_wpiblue") .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); - tagCount[limelight.getId()] = result.getMultiTagResult().get().fiducialIDsUsed.size(); + tagCount[limelight.getId()] = result.getMultiTagResult().get().fiducialIDsUsed.size(); } table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index d5d3da18..e884336c 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -1,13 +1,11 @@ package frc.robot.subsystems.vision; -import org.photonvision.PhotonCamera; -import org.photonvision.proto.Photon; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import frc.robot.extras.vision.LimelightHelpers; +import org.photonvision.PhotonCamera; public final class VisionConstants { public enum Limelight { @@ -46,7 +44,7 @@ public PhotonCamera getSimulationCamera(Limelight limelight) { case FRONT_LEFT -> FRONT_LEFT_CAMERA; case FRONT_RIGHT -> FRONT_RIGHT_CAMERA; default -> throw new IllegalArgumentException("Invalid limelight camera " + limelight); - }; + }; } public NetworkTable getLimelightTable(Limelight limelight) { @@ -56,12 +54,14 @@ public NetworkTable getLimelightTable(Limelight limelight) { case FRONT_RIGHT -> LimelightHelpers.getLimelightNTTable(FRONT_RIGHT.getName()); default -> throw new IllegalArgumentException("Invalid limelight " + limelight); }; - } + } } public static final PhotonCamera SHOOTER_CAMERA = new PhotonCamera(Limelight.SHOOTER.getName()); - public static final PhotonCamera FRONT_LEFT_CAMERA = new PhotonCamera(Limelight.FRONT_LEFT.getName()); - public static final PhotonCamera FRONT_RIGHT_CAMERA = new PhotonCamera(Limelight.FRONT_RIGHT.getName()); + public static final PhotonCamera FRONT_LEFT_CAMERA = + new PhotonCamera(Limelight.FRONT_LEFT.getName()); + public static final PhotonCamera FRONT_RIGHT_CAMERA = + new PhotonCamera(Limelight.FRONT_RIGHT.getName()); public static final int THREAD_SLEEP_MS = 20; public static final AprilTagFieldLayout FIELD_LAYOUT = From 6751da8181c635c809840d2c2b8fc21e3eeee8fb Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Fri, 29 Nov 2024 06:31:47 -0500 Subject: [PATCH 54/75] push fixes --- .../subsystems/vision/SimulatedVision.java | 34 ++++++++++++++----- .../subsystems/vision/VisionConstants.java | 17 ---------- 2 files changed, 25 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 054cb08d..a82fedff 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.networktables.NetworkTable; +import frc.robot.extras.vision.LimelightHelpers; import frc.robot.subsystems.vision.VisionConstants.Limelight; import java.util.ArrayList; import java.util.Arrays; @@ -57,14 +58,11 @@ public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { // targets. // Instance variables shooterCameraSim = - new PhotonCameraSim( - Limelight.SHOOTER.getSimulationCamera(Limelight.SHOOTER), cameraProperties); + new PhotonCameraSim(getSimulationCamera(Limelight.SHOOTER), cameraProperties); frontLeftCameraSim = - new PhotonCameraSim( - Limelight.FRONT_LEFT.getSimulationCamera(Limelight.FRONT_LEFT), cameraProperties); + new PhotonCameraSim(getSimulationCamera(Limelight.FRONT_LEFT), cameraProperties); frontRightCameraSim = - new PhotonCameraSim( - Limelight.FRONT_RIGHT.getSimulationCamera(Limelight.FRONT_RIGHT), cameraProperties); + new PhotonCameraSim(getSimulationCamera(Limelight.FRONT_RIGHT), cameraProperties); Transform3d robotToShooterCamera = new Transform3d( @@ -108,9 +106,8 @@ public void updateInputs(VisionInputs inputs) { } for (Limelight limelight : Limelight.values()) { - writeToTable( - limelight.getSimulationCamera(limelight).getAllUnreadResults(), - limelight.getLimelightTable(limelight), + writeToTable(getSimulationCamera(limelight).getAllUnreadResults(), + getLimelightTable(limelight), limelight); inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); } @@ -165,6 +162,25 @@ private void writeToTable( } } + + public PhotonCamera getSimulationCamera(Limelight limelight) { + return switch (limelight) { + case SHOOTER -> VisionConstants.SHOOTER_CAMERA; + case FRONT_LEFT -> VisionConstants.FRONT_LEFT_CAMERA; + case FRONT_RIGHT -> VisionConstants.FRONT_RIGHT_CAMERA; + default -> throw new IllegalArgumentException("Invalid limelight camera " + limelight); + }; + } + + public NetworkTable getLimelightTable(Limelight limelight) { + return switch (limelight) { + case SHOOTER -> LimelightHelpers.getLimelightNTTable(Limelight.SHOOTER.getName()); + case FRONT_LEFT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_LEFT.getName()); + case FRONT_RIGHT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_RIGHT.getName()); + default -> throw new IllegalArgumentException("Invalid limelight " + limelight); + }; + } + @Override public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { super.setHeadingInfo(headingDegrees, headingRateDegrees); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index e884336c..f864b030 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -38,23 +38,6 @@ public static Limelight fromId(int id) { }; } - public PhotonCamera getSimulationCamera(Limelight limelight) { - return switch (limelight) { - case SHOOTER -> SHOOTER_CAMERA; - case FRONT_LEFT -> FRONT_LEFT_CAMERA; - case FRONT_RIGHT -> FRONT_RIGHT_CAMERA; - default -> throw new IllegalArgumentException("Invalid limelight camera " + limelight); - }; - } - - public NetworkTable getLimelightTable(Limelight limelight) { - return switch (limelight) { - case SHOOTER -> LimelightHelpers.getLimelightNTTable(SHOOTER.getName()); - case FRONT_LEFT -> LimelightHelpers.getLimelightNTTable(FRONT_LEFT.getName()); - case FRONT_RIGHT -> LimelightHelpers.getLimelightNTTable(FRONT_RIGHT.getName()); - default -> throw new IllegalArgumentException("Invalid limelight " + limelight); - }; - } } public static final PhotonCamera SHOOTER_CAMERA = new PhotonCamera(Limelight.SHOOTER.getName()); From bfb0199311e6884921c8e6a92a763181aa347d6e Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Fri, 29 Nov 2024 06:53:04 -0500 Subject: [PATCH 55/75] fix ci --- .../subsystems/vision/SimulatedVision.java | 55 ++++++++----------- .../subsystems/vision/VisionConstants.java | 18 +++++- 2 files changed, 37 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index a82fedff..d9597cb6 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -9,6 +9,7 @@ import java.util.List; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; +import org.photonvision.PhotonCamera; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; @@ -64,21 +65,9 @@ public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { frontRightCameraSim = new PhotonCameraSim(getSimulationCamera(Limelight.FRONT_RIGHT), cameraProperties); - Transform3d robotToShooterCamera = - new Transform3d( - new Translation3d(-0.3119324724, 0.0, 0.1865472012), new Rotation3d(0.0, 35, 180.0)); - visionSim.addCamera(shooterCameraSim, robotToShooterCamera); - - Transform3d robotToFrontRightCamera = - new Transform3d( - new Translation3d(0.2749477356, -0.269958439, 0.2318054546), - new Rotation3d(0.0, 25, -35)); - Transform3d robotToFrontLeftCamera = - new Transform3d( - new Translation3d(0.2816630892, 0.2724405524, 0.232156), new Rotation3d(0.0, 25, 35)); - visionSim.addCamera(frontLeftCameraSim, robotToFrontLeftCamera); - - visionSim.addCamera(frontRightCameraSim, robotToFrontRightCamera); + visionSim.addCamera(shooterCameraSim, VisionConstants.SHOOTER_TRANSFORM); + visionSim.addCamera(frontLeftCameraSim, VisionConstants.FRONT_LEFT_TRANSFORM); + visionSim.addCamera(frontRightCameraSim, VisionConstants.FRONT_RIGHT_TRANSFORM); // Enable the raw and processed streams. (http://localhost:1181 / 1182) shooterCameraSim.enableRawStream(true); @@ -106,7 +95,8 @@ public void updateInputs(VisionInputs inputs) { } for (Limelight limelight : Limelight.values()) { - writeToTable(getSimulationCamera(limelight).getAllUnreadResults(), + writeToTable( + getSimulationCamera(limelight).getAllUnreadResults(), getLimelightTable(limelight), limelight); inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); @@ -162,24 +152,23 @@ private void writeToTable( } } - - public PhotonCamera getSimulationCamera(Limelight limelight) { - return switch (limelight) { - case SHOOTER -> VisionConstants.SHOOTER_CAMERA; - case FRONT_LEFT -> VisionConstants.FRONT_LEFT_CAMERA; - case FRONT_RIGHT -> VisionConstants.FRONT_RIGHT_CAMERA; - default -> throw new IllegalArgumentException("Invalid limelight camera " + limelight); - }; - } + public PhotonCamera getSimulationCamera(Limelight limelight) { + return switch (limelight) { + case SHOOTER -> VisionConstants.SHOOTER_CAMERA; + case FRONT_LEFT -> VisionConstants.FRONT_LEFT_CAMERA; + case FRONT_RIGHT -> VisionConstants.FRONT_RIGHT_CAMERA; + default -> throw new IllegalArgumentException("Invalid limelight camera " + limelight); + }; + } - public NetworkTable getLimelightTable(Limelight limelight) { - return switch (limelight) { - case SHOOTER -> LimelightHelpers.getLimelightNTTable(Limelight.SHOOTER.getName()); - case FRONT_LEFT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_LEFT.getName()); - case FRONT_RIGHT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_RIGHT.getName()); - default -> throw new IllegalArgumentException("Invalid limelight " + limelight); - }; - } + public NetworkTable getLimelightTable(Limelight limelight) { + return switch (limelight) { + case SHOOTER -> LimelightHelpers.getLimelightNTTable(Limelight.SHOOTER.getName()); + case FRONT_LEFT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_LEFT.getName()); + case FRONT_RIGHT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_RIGHT.getName()); + default -> throw new IllegalArgumentException("Invalid limelight " + limelight); + }; + } @Override public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index f864b030..7b7e35ec 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -2,9 +2,10 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTable; -import frc.robot.extras.vision.LimelightHelpers; import org.photonvision.PhotonCamera; public final class VisionConstants { @@ -37,14 +38,25 @@ public static Limelight fromId(int id) { default -> throw new IllegalArgumentException("Invalid Limelight ID: " + id); }; } - } + public static final Transform3d SHOOTER_TRANSFORM = + new Transform3d( + new Translation3d(-0.3119324724, 0.0, 0.1865472012), new Rotation3d(0.0, 35, 180.0)); + public static final Transform3d FRONT_LEFT_TRANSFORM = + new Transform3d( + new Translation3d(0.2749477356, -0.269958439, 0.2318054546), + new Rotation3d(0.0, 25, -35)); + public static final Transform3d FRONT_RIGHT_TRANSFORM = + new Transform3d( + new Translation3d(0.2816630892, 0.2724405524, 0.232156), new Rotation3d(0.0, 25, 35)); + public static final PhotonCamera SHOOTER_CAMERA = new PhotonCamera(Limelight.SHOOTER.getName()); public static final PhotonCamera FRONT_LEFT_CAMERA = new PhotonCamera(Limelight.FRONT_LEFT.getName()); public static final PhotonCamera FRONT_RIGHT_CAMERA = new PhotonCamera(Limelight.FRONT_RIGHT.getName()); + public static final int THREAD_SLEEP_MS = 20; public static final AprilTagFieldLayout FIELD_LAYOUT = From 2a4080f05f2dea13d7cb62bbe022cd84f0706108 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Fri, 29 Nov 2024 15:40:51 -0500 Subject: [PATCH 56/75] update --- ascopeAssets/Robot_Offseason/config.json | 50 +++++++++++++++++- .../{MAIN_ASSEMBLY.glb => model.glb} | Bin 2 files changed, 48 insertions(+), 2 deletions(-) rename ascopeAssets/Robot_Offseason/{MAIN_ASSEMBLY.glb => model.glb} (100%) diff --git a/ascopeAssets/Robot_Offseason/config.json b/ascopeAssets/Robot_Offseason/config.json index cbb3f88d..9176ce8c 100644 --- a/ascopeAssets/Robot_Offseason/config.json +++ b/ascopeAssets/Robot_Offseason/config.json @@ -1,3 +1,49 @@ { - "name": "offseason" - } \ No newline at end of file + "name": "butch", + "sourceUrl": "https://cad.onshape.com/documents/8f873ac3a74d8fd06cda742a/w/88e5df6f1994d26987d1f315/e/eb5a3c451ca2580d6f4169f7", + "rotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 0 + } + ], + "position": [ + 0, + 0, + 0.04 + ], + "cameras": [ + {"name":"FrontCam","position":[0.3,0.0,0.2],"fov":59.99999999999999,"rotations":[{"axis":"y","degrees":-29.999999999999996},{"axis":"z","degrees":0.0}],"resolution":[640,480]}, + {"name":"FrontLeftCam","position":[0.25,0.3,0.2],"fov":59.99999999999999,"rotations":[{"axis":"y","degrees":-30.000000000000004},{"axis":"z","degrees":29.999999999999996}],"resolution":[640,480]}, + {"name":"FrontRightCam","position":[0.25,-0.3,0.2],"fov":59.99999999999999,"rotations":[{"axis":"y","degrees":-29.999999999999996},{"axis":"z","degrees":-30.000000000000004}],"resolution":[640,480]}, + {"name":"BackLeftCam","position":[0.25,0.05,0.2],"fov":59.99999999999999,"rotations":[{"axis":"y","degrees":-35.000000000000014},{"axis":"z","degrees":150.0}],"resolution":[640,480]}, + {"name":"BackRightCam","position":[0.25,-0.05,0.2],"fov":59.99999999999999,"rotations":[{"axis":"y","degrees":-35.00000000000001},{"axis":"z","degrees":-150.0}],"resolution":[640,480]} + ], + "components": [ + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 180 + }, + { + "axis": "y", + "degrees": 60 + }, + { + "axis": "z", + "degrees": 0 + } + ], + "zeroedPosition": [ + 0.25, + 0, + -0.02 + ] + } + ] +} \ No newline at end of file diff --git a/ascopeAssets/Robot_Offseason/MAIN_ASSEMBLY.glb b/ascopeAssets/Robot_Offseason/model.glb similarity index 100% rename from ascopeAssets/Robot_Offseason/MAIN_ASSEMBLY.glb rename to ascopeAssets/Robot_Offseason/model.glb From 5d6b0c55ca422d0ac266ca665a2bfe8d7625a79e Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 2 Dec 2024 08:05:05 -0500 Subject: [PATCH 57/75] fixy (#82) --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/subsystems/swerve/odometryThread/OdometryThread.java | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fc3c3499..a1c4740c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -11,7 +11,7 @@ public static final class LogPaths { public static final String PHYSICS_SIMULATION_PATH = "MaplePhysicsSimulation/"; } - public static final Mode currentMode = Mode.SIM; + public static final Mode CURRENT_MODE = Mode.SIM; public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 49b3d6dd..235783b8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -48,7 +48,7 @@ public void robotInit() { } // Set up data receivers & replay source - switch (Constants.currentMode) { + switch (Constants.CURRENT_MODE) { case REAL: // Running on a real robot, log to a USB stick ("/U/logs") Logger.addDataReceiver(new WPILOGWriter()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 348f7131..1a9ecee0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -52,7 +52,7 @@ public class RobotContainer { // private final XboxController driverController = new XboxController(0); public RobotContainer() { - switch (Constants.currentMode) { + switch (Constants.CURRENT_MODE) { case REAL -> { /* Real robot, instantiate hardware IO implementations */ diff --git a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java index a4a98ea3..32e83bfa 100644 --- a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java +++ b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java @@ -48,7 +48,7 @@ static Queue registerInput(Supplier supplier) { } static OdometryThread createInstance(DeviceCANBus canBus) { - return switch (Constants.currentMode) { + return switch (Constants.CURRENT_MODE) { case REAL -> new OdometryThreadReal( canBus, From 4737dfb6950c532e2abaf197d79d09d05618a047 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 2 Dec 2024 17:44:36 -0500 Subject: [PATCH 58/75] i fixed a thing --- .../frc/robot/subsystems/vision/PhysicalVision.java | 4 ++-- .../frc/robot/subsystems/vision/SimulatedVision.java | 12 +++++++++--- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 0c1b392d..b1a69313 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -275,8 +275,8 @@ public Pose2d getLastSeenPose() { @Override public double getLimelightAprilTagDistance(Limelight limelight) { if (canSeeAprilTags(limelight)) { - // return limelightEstimates[limelight.getId()].; - return 2; + return limelightEstimates[limelight.getId()].avgTagDist; + // return 2; } // To be safe returns a big distance from the april tags if it can't see any return Double.MAX_VALUE; diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index d9597cb6..822a70e8 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -19,9 +19,6 @@ // Please see the following link for example code // https://github.com/PhotonVision/photonvision/blob/2a6fa1b6ac81f239c59d724da5339f608897c510/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java public class SimulatedVision extends PhysicalVision { - // private final PhotonCamera shooterCamera = new PhotonCamera("shooterCamera"); - // private final PhotonCamera frontLeftCamera = new PhotonCamera("frontLeftCamera"); - // private final PhotonCamera frontRightCamera = new PhotonCamera("frontRightCamera"); PhotonCameraSim shooterCameraSim; PhotonCameraSim frontLeftCameraSim; PhotonCameraSim frontRightCameraSim; @@ -32,6 +29,7 @@ public class SimulatedVision extends PhysicalVision { private final int kResHeight = 800; private int[] tagCount = new int[Limelight.values().length]; + private double[] apriltagDist = new double[Limelight.values().length]; public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { super(); @@ -100,6 +98,7 @@ public void updateInputs(VisionInputs inputs) { getLimelightTable(limelight), limelight); inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); + inputs.limelightAprilTagDistance[limelight.getId()] = getLimelightAprilTagDistance(limelight); } super.updateInputs(inputs); } @@ -145,6 +144,7 @@ private void writeToTable( .getEntry("botpose_orb_wpiblue") .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); tagCount[limelight.getId()] = result.getMultiTagResult().get().fiducialIDsUsed.size(); + apriltagDist[limelight.getId()] = result.getMultiTagResult().get().estimatedPose.best.getX(); } table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); @@ -180,4 +180,10 @@ public int getNumberOfAprilTags(Limelight limelight) { // TODO Auto-generated method stub return tagCount[limelight.getId()]; } + + @Override + public double getLimelightAprilTagDistance(Limelight limelight) { + // TODO Auto-generated method stub + return apriltagDist[limelight.getId()]; + } } From 94d1b2f202b692fe8723fd385a65af4fb40276d5 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Wed, 4 Dec 2024 17:31:01 -0500 Subject: [PATCH 59/75] cleanup --- .../frc/robot/subsystems/vision/PhysicalVision.java | 1 - .../frc/robot/subsystems/vision/SimulatedVision.java | 10 ++++++++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index b1a69313..fd500eed 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -276,7 +276,6 @@ public Pose2d getLastSeenPose() { public double getLimelightAprilTagDistance(Limelight limelight) { if (canSeeAprilTags(limelight)) { return limelightEstimates[limelight.getId()].avgTagDist; - // return 2; } // To be safe returns a big distance from the april tags if it can't see any return Double.MAX_VALUE; diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 822a70e8..19c9fc4b 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -31,9 +31,9 @@ public class SimulatedVision extends PhysicalVision { private int[] tagCount = new int[Limelight.values().length]; private double[] apriltagDist = new double[Limelight.values().length]; - public SimulatedVision(Supplier robotActualPoseInSimulationSupplier) { + public SimulatedVision(Supplier robotSimulationPose) { super(); - this.robotSimulationPose = robotActualPoseInSimulationSupplier; + this.robotSimulationPose = robotSimulationPose; // Create the vision system simulation which handles cameras and targets on the // field. visionSim = new VisionSystemSim("main"); @@ -186,4 +186,10 @@ public double getLimelightAprilTagDistance(Limelight limelight) { // TODO Auto-generated method stub return apriltagDist[limelight.getId()]; } + + @Override + public Pose2d getLastSeenPose() { + // TODO Auto-generated method stub + return super.getLastSeenPose(); + } } From cf5ef88a6be790cc8a40e10370628d0161c26d53 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 9 Dec 2024 14:34:16 -0500 Subject: [PATCH 60/75] thingy --- ascopeAssets/Robot_Offseason/config.json | 126 +++++++++++++++- .../swerve/SwerveModuleSimulation.java | 1 - .../robot/subsystems/swerve/SwerveModule.java | 2 - .../swerve/module/SimulatedModule.java | 1 - .../subsystems/vision/PhysicalVision.java | 4 +- .../subsystems/vision/SimulatedVision.java | 21 +-- vendordeps/photonlib.json | 140 +++++++++--------- 7 files changed, 201 insertions(+), 94 deletions(-) diff --git a/ascopeAssets/Robot_Offseason/config.json b/ascopeAssets/Robot_Offseason/config.json index 798e050e..d77295ef 100644 --- a/ascopeAssets/Robot_Offseason/config.json +++ b/ascopeAssets/Robot_Offseason/config.json @@ -1,7 +1,7 @@ { - "name": "butch", - "sourceUrl": "https://cad.onshape.com/documents/8f873ac3a74d8fd06cda742a/w/88e5df6f1994d26987d1f315/e/eb5a3c451ca2580d6f4169f7", - "rotations": [ + "name": "butch", + "sourceUrl": "https://cad.onshape.com/documents/8f873ac3a74d8fd06cda742a/w/88e5df6f1994d26987d1f315/e/eb5a3c451ca2580d6f4169f7", + "rotations": [ { "axis": "x", "degrees": 90 @@ -17,11 +17,121 @@ 0.04 ], "cameras": [ - {"name":"FrontCam","position":[0.3,0.0,0.2],"fov":59.99999999999999,"rotations":[{"axis":"y","degrees":-29.999999999999996},{"axis":"z","degrees":0.0}],"resolution":[640,480]}, - {"name":"FrontLeftCam","position":[0.25,0.3,0.2],"fov":59.99999999999999,"rotations":[{"axis":"y","degrees":-30.000000000000004},{"axis":"z","degrees":29.999999999999996}],"resolution":[640,480]}, - {"name":"FrontRightCam","position":[0.25,-0.3,0.2],"fov":59.99999999999999,"rotations":[{"axis":"y","degrees":-29.999999999999996},{"axis":"z","degrees":-30.000000000000004}],"resolution":[640,480]}, - {"name":"BackLeftCam","position":[0.25,0.05,0.2],"fov":59.99999999999999,"rotations":[{"axis":"y","degrees":-35.000000000000014},{"axis":"z","degrees":150.0}],"resolution":[640,480]}, - {"name":"BackRightCam","position":[0.25,-0.05,0.2],"fov":59.99999999999999,"rotations":[{"axis":"y","degrees":-35.00000000000001},{"axis":"z","degrees":-150.0}],"resolution":[640,480]} + { + "name": "FrontCam", + "position": [ + 0.3, + 0.0, + 0.2 + ], + "fov": 59.99999999999999, + "rotations": [ + { + "axis": "y", + "degrees": -29.999999999999996 + }, + { + "axis": "z", + "degrees": 0.0 + } + ], + "resolution": [ + 640, + 480 + ] + }, + { + "name": "FrontLeftCam", + "position": [ + 0.25, + 0.3, + 0.2 + ], + "fov": 59.99999999999999, + "rotations": [ + { + "axis": "y", + "degrees": -30.000000000000004 + }, + { + "axis": "z", + "degrees": 29.999999999999996 + } + ], + "resolution": [ + 640, + 480 + ] + }, + { + "name": "FrontRightCam", + "position": [ + 0.25, + -0.3, + 0.2 + ], + "fov": 59.99999999999999, + "rotations": [ + { + "axis": "y", + "degrees": -29.999999999999996 + }, + { + "axis": "z", + "degrees": -30.000000000000004 + } + ], + "resolution": [ + 640, + 480 + ] + }, + { + "name": "BackLeftCam", + "position": [ + 0.25, + 0.05, + 0.2 + ], + "fov": 59.99999999999999, + "rotations": [ + { + "axis": "y", + "degrees": -35.000000000000014 + }, + { + "axis": "z", + "degrees": 150.0 + } + ], + "resolution": [ + 640, + 480 + ] + }, + { + "name": "BackRightCam", + "position": [ + 0.25, + -0.05, + 0.2 + ], + "fov": 59.99999999999999, + "rotations": [ + { + "axis": "y", + "degrees": -35.00000000000001 + }, + { + "axis": "z", + "degrees": -150.0 + } + ], + "resolution": [ + 640, + 480 + ] + } ], "components": [ { diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java index 8197f25e..87037b71 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java @@ -16,7 +16,6 @@ import edu.wpi.first.units.measure.Voltage; import frc.robot.extras.simulation.field.SimulatedField; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; - import java.util.Queue; import java.util.concurrent.ConcurrentLinkedQueue; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 779cb296..51ab9f4b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -7,12 +7,10 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // import edu.wpi.first.w; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.SimulationConstants; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; import frc.robot.subsystems.swerve.module.ModuleInputsAutoLogged; import frc.robot.subsystems.swerve.module.ModuleInterface; diff --git a/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java index deafc55a..249138a8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java @@ -13,7 +13,6 @@ import frc.robot.extras.simulation.OdometryTimestampsSim; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; -import java.util.Arrays; /** Wrapper class around {@link SwerveModuleSimulation} */ public class SimulatedModule implements ModuleInterface { diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index fd500eed..33e481df 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -78,8 +78,8 @@ public void updateInputs(VisionInputs inputs) { public boolean canSeeAprilTags(Limelight limelight) { // First checks if it can see an april tag, then checks if it is fully in frame // Different Limelights have different FOVs - if (getNumberOfAprilTags(limelight) > 0 - && getNumberOfAprilTags(limelight) <= VisionConstants.APRIL_TAG_POSITIONS.length) { + if (getNumberOfAprilTags(limelight) > VisionConstants.MIN_APRIL_TAG_ID + && getNumberOfAprilTags(limelight) <= VisionConstants.MAX_APRIL_TAG_ID) { if (limelight.getName().equals(Limelight.SHOOTER.getName())) { return Math.abs(LimelightHelpers.getTX(limelight.getName())) <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR; diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index 19c9fc4b..b937a3d6 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -144,7 +144,8 @@ private void writeToTable( .getEntry("botpose_orb_wpiblue") .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); tagCount[limelight.getId()] = result.getMultiTagResult().get().fiducialIDsUsed.size(); - apriltagDist[limelight.getId()] = result.getMultiTagResult().get().estimatedPose.best.getX(); + apriltagDist[limelight.getId()] = + result.getMultiTagResult().get().estimatedPose.best.getX(); } table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); @@ -182,14 +183,14 @@ public int getNumberOfAprilTags(Limelight limelight) { } @Override - public double getLimelightAprilTagDistance(Limelight limelight) { - // TODO Auto-generated method stub - return apriltagDist[limelight.getId()]; - } + public double getLimelightAprilTagDistance(Limelight limelight) { + // TODO Auto-generated method stub + return apriltagDist[limelight.getId()]; + } - @Override - public Pose2d getLastSeenPose() { - // TODO Auto-generated method stub - return super.getLastSeenPose(); - } + @Override + public Pose2d getLastSeenPose() { + // TODO Auto-generated method stub + return super.getLastSeenPose(); + } } diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index ad3a530c..2e8e6b00 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,71 +1,71 @@ { - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2025.0.0-beta-5", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2025", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-5", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2025.0.0-beta-5", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-5", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2025.0.0-beta-5" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2025.0.0-beta-5" - } - ] -} \ No newline at end of file + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.0.0-beta-5", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.0.0-beta-5", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-5", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.0.0-beta-5" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.0.0-beta-5" + } + ] +} From 6af9077a9a1bef414b31e6e4321a4dc7af438452 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 9 Dec 2024 14:41:16 -0500 Subject: [PATCH 61/75] remove buildconstants from gitignore bc it is cooking CI --- .gitignore | 2 +- src/main/java/frc/robot/BuildConstants.java | 17 +++++++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/BuildConstants.java diff --git a/.gitignore b/.gitignore index da5ea016..fb243301 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,6 @@ # This gitignore has been specially created by the WPILib team. # If you remove items from this file, intellisense might break. -*src/main/java/frc/robot/BuildConstants.java +# *src/main/java/frc/robot/BuildConstants.java simgui-ds.json simgui.json networktables.json diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java new file mode 100644 index 00000000..09f0aadb --- /dev/null +++ b/src/main/java/frc/robot/BuildConstants.java @@ -0,0 +1,17 @@ +package frc.robot; + +/** Automatically generated file containing build version information. */ +public final class BuildConstants { + public static final String MAVEN_GROUP = ""; + public static final String MAVEN_NAME = "offseason-robot-code-2024"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 464; + public static final String GIT_SHA = "e85bebc1ff41f2682e3d4ccbe67f1f5ce0f8a0f3"; + public static final String GIT_DATE = "2024-12-08 17:15:05 EST"; + public static final String GIT_BRANCH = "add-aquila-constants"; + public static final String BUILD_DATE = "2024-12-08 17:29:19 EST"; + public static final long BUILD_UNIX_TIME = 1733696959570L; + public static final int DIRTY = 0; + + private BuildConstants() {} +} From f711faf91bd0535113de80c330fe825c8fb9e9e9 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 9 Dec 2024 14:47:57 -0500 Subject: [PATCH 62/75] add it back --- .gitignore | 2 +- src/main/java/frc/robot/extras/util/MathUtil.java | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index fb243301..da5ea016 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,6 @@ # This gitignore has been specially created by the WPILib team. # If you remove items from this file, intellisense might break. -# *src/main/java/frc/robot/BuildConstants.java +*src/main/java/frc/robot/BuildConstants.java simgui-ds.json simgui.json networktables.json diff --git a/src/main/java/frc/robot/extras/util/MathUtil.java b/src/main/java/frc/robot/extras/util/MathUtil.java index 65acfdfe..f67401aa 100644 --- a/src/main/java/frc/robot/extras/util/MathUtil.java +++ b/src/main/java/frc/robot/extras/util/MathUtil.java @@ -1,7 +1,6 @@ package frc.robot.extras.util; import edu.wpi.first.math.geometry.Rotation3d; -import frc.robot.BuildConstants; import java.util.Random; public class MathUtil { @@ -9,7 +8,7 @@ public class MathUtil { * random object that generates random variables the seed is the hash of GIT_SHA this way when you * do log-replay even the generated random numbers are the same */ - private static final Random random = new Random(BuildConstants.GIT_SHA.hashCode()); + private static final Random random = new Random((long) Math.random()); public static double linearInterpretationWithBounding( double x1, double y1, double x2, double y2, double x) { From faf0b4c6b51fc7713c6f4c598ae34ef7c4ff379d Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 9 Dec 2024 14:49:47 -0500 Subject: [PATCH 63/75] testing code --- .../frc/robot/subsystems/swerve/SwerveModule.java | 5 +++-- .../subsystems/swerve/module/PhysicalModule.java | 12 ------------ 2 files changed, 3 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 51ab9f4b..72d99337 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -72,6 +72,7 @@ public double getCharacterizationVelocity() { /** Runs the module with the specified setpoint state. Returns optimized setpoint */ public void runSetPoint(SwerveModuleState state) { state.optimize(getTurnRotation()); + state.cosineScale(getTurnRotation()); if (Math.abs(state.speedMetersPerSecond) < 0.01) { io.stopModule(); @@ -91,12 +92,12 @@ public double getTurnVelocity() { /** Returns the current drive position of the module in meters. */ public double getDrivePositionMeters() { - return ModuleConstants.WHEEL_CIRCUMFERENCE_METERS * inputs.drivePosition; + return ModuleConstants.DRIVE_TO_METERS * inputs.drivePosition; //Wheel circumference } /** Returns the current drive velocity of the module in meters per second. */ public double getDriveVelocityMetersPerSec() { - return ModuleConstants.WHEEL_CIRCUMFERENCE_METERS * inputs.driveVelocity; + return ModuleConstants.DRIVE_TO_METERS_PER_SECOND * inputs.driveVelocity; } /** Returns the module state (turn angle and drive velocity). */ diff --git a/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java index a987556e..15092505 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java @@ -173,18 +173,6 @@ public void setTurnVoltage(Voltage volts) { @Override public void setDesiredState(SwerveModuleState desiredState) { - double turnRotations = getTurnRotations(); - // Optimize the reference state to avoid spinning further than 90 degrees - - // desiredState.optimize(Rotation2d.fromRotations(turnRotations)); - // setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); - - // if (Math.abs(desiredState.speedMetersPerSecond) < 0.01) { - // driveMotor.set(0); - // turnMotor.set(0); - // return; - // } - // Converts meters per second to rotations per second double desiredDriveRPS = desiredState.speedMetersPerSecond From 2285b42e528cb7fe98479c388bf8594442cf9401 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Tue, 10 Dec 2024 12:48:35 -0500 Subject: [PATCH 64/75] what is sdis shit --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 6 +- .../frc/robot/extras/util/JoystickUtil.java | 30 -------- .../robot/subsystems/swerve/SwerveDrive.java | 61 ++++++--------- .../robot/subsystems/swerve/SwerveModule.java | 2 +- .../swerve/module/PhysicalModule.java | 9 +-- ...json => Phoenix6-frc2025-beta-latest.json} | 77 +++++++++++-------- 8 files changed, 80 insertions(+), 109 deletions(-) rename vendordeps/{Phoenix6-25.0.0-beta-2.json => Phoenix6-frc2025-beta-latest.json} (85%) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 38742b18..ac55d717 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,7 +14,7 @@ public static final class LogPaths { public static final String PHYSICS_SIMULATION_PATH = "MaplePhysicsSimulation/"; } - public static final Mode CURRENT_MODE = Mode.SIM; + public static final Mode CURRENT_MODE = Mode.REAL; public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1856df2c..a0a69672 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -43,7 +43,7 @@ public Robot() { switch (Constants.CURRENT_MODE) { case REAL: // Running on a real robot, log to a USB stick ("/U/logs") - Logger.addDataReceiver(new WPILOGWriter()); + // Logger.addDataReceiver(new WPILOGWriter()); Logger.addDataReceiver(new NT4Publisher()); break; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ec1f056e..01be9a40 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -167,8 +167,8 @@ private void configureButtonBindings() { DoubleSupplier driverRightStickX = driverController::getRightX; DoubleSupplier driverLeftStick[] = new DoubleSupplier[] { - () -> JoystickUtil.modifyAxisCubedPolar(driverLeftStickX, driverLeftStickY)[0], - () -> JoystickUtil.modifyAxisCubedPolar(driverLeftStickX, driverLeftStickY)[1] + () -> JoystickUtil.modifyAxisPolar(driverLeftStickX, driverLeftStickY, 3)[0], + () -> JoystickUtil.modifyAxisPolar(driverLeftStickX, driverLeftStickY, 3)[1] }; DoubleSupplier operatorLeftStickX = operatorController::getLeftX; @@ -217,7 +217,7 @@ private void configureButtonBindings() { visionSubsystem, driverLeftStick[1], driverLeftStick[0], - () -> JoystickUtil.modifyAxisCubed(driverRightStickX), + () -> JoystickUtil.modifyAxis(driverRightStickX, 3), () -> !driverRightBumper.getAsBoolean(), () -> driverLeftBumper.getAsBoolean()); swerveDrive.setDefaultCommand(driveCommand); diff --git a/src/main/java/frc/robot/extras/util/JoystickUtil.java b/src/main/java/frc/robot/extras/util/JoystickUtil.java index 1ffc4bb9..444024e1 100644 --- a/src/main/java/frc/robot/extras/util/JoystickUtil.java +++ b/src/main/java/frc/robot/extras/util/JoystickUtil.java @@ -43,18 +43,6 @@ public static double modifyAxis(DoubleSupplier supplierValue, int exponent) { return value; } - public static double modifyAxisCubed(DoubleSupplier supplierValue) { - double value = supplierValue.getAsDouble(); - - // Deadband - value = JoystickUtil.deadband(value, JoystickConstants.DEADBAND_VALUE); - - // Cube the axis - value = Math.copySign(value * value * value, value); - - return value; - } - /** * Converts the two axis coordinates to polar to get both the angle and radius, so they can work * in a double[] list. @@ -81,22 +69,4 @@ public static double[] modifyAxisPolar( Math.copySign(Math.pow(yInput, exponent), yInput) }; } - - public static double[] modifyAxisCubedPolar(DoubleSupplier xJoystick, DoubleSupplier yJoystick) { - double xInput = - JoystickUtil.deadband(xJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE); - double yInput = deadband(yJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE); - if (Math.abs(xInput) > 0 && Math.abs(yInput) > 0) { - double theta = Math.atan(xInput / yInput); - double hypotenuse = Math.sqrt(xInput * xInput + yInput * yInput); - double cubedHypotenuse = Math.pow(hypotenuse, 3); - xInput = Math.copySign(Math.sin(theta) * cubedHypotenuse, xInput); - yInput = Math.copySign(Math.cos(theta) * cubedHypotenuse, yInput); - return new double[] {xInput, yInput}; - } - return new double[] { - Math.copySign(xInput * xInput * xInput, xInput), - Math.copySign(yInput * yInput * yInput, yInput) - }; - } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index a6a90a8f..88982eca 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -82,19 +82,19 @@ public SwerveDrive( VisionConstants.VISION_Y_POS_TRUST, VisionConstants.VISION_ANGLE_TRUST)); - this.odometryThread = OdometryThread.createInstance(DeviceCANBus.RIO); + this.odometryThread = OdometryThread.createInstance(DeviceCANBus.CANIVORE); this.odometryThreadInputs = new OdometryThreadInputsAutoLogged(); this.odometryThread.start(); gyroDisconnectedAlert.set(false); } - // /** Updates the pose estimator with the pose calculated from the swerve modules. */ - public void addPoseEstimatorSwerveMeasurement() { - for (int timestampIndex = 0; - timestampIndex < odometryThreadInputs.measurementTimeStamps.length; - timestampIndex++) addPoseEstimatorSwerveMeasurement(timestampIndex); - } + // // /** Updates the pose estimator with the pose calculated from the swerve modules. */ + // public void addPoseEstimatorSwerveMeasurement() { + // for (int timestampIndex = 0; + // timestampIndex < odometryThreadInputs.measurementTimeStamps.length; + // timestampIndex++) addPoseEstimatorSwerveMeasurement(timestampIndex); + // } /* * Updates the pose estimator with the pose calculated from the april tags. How much it @@ -180,15 +180,19 @@ private void modulesPeriodic() { * @param fieldRelative Whether the provided x and y speeds are relative to the field. */ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative) { - ChassisSpeeds discreteSpeeds = - // ChassisSpeeds.discretize( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d()) - : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); - // , 0.02); + ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); + if (fieldRelative) { + speeds.toRobotRelativeSpeeds(getOdometryAllianceRelativeRotation2d()); + } + // ChassisSpeeds discreteSpeeds = + // // ChassisSpeeds.discretize( + // fieldRelative + // ? ChassisSpeeds.fromFieldRelativeSpeeds( + // xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d()) + // : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); + // // , 0.02); SwerveModuleState[] swerveModuleStates = - DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(discreteSpeeds); + DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(speeds); SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DriveConstants.MAX_SPEED_METERS_PER_SECOND); @@ -277,38 +281,21 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { * * @param timestampIndex index of the timestamp to sample the pose at */ - public void addPoseEstimatorSwerveMeasurement(int timestampIndex) { + public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex final SwerveModulePosition[] modulePositions = getModulePositions(), moduleDeltas = getModulesDelta(modulePositions); if (gyroInputs.isConnected) { - rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; - // rawGyroRotation = getGyroRotation2d(); + // rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; + rawGyroRotation = getGyroRotation2d(); } else { Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas); rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } odometry.updateWithTime( - odometryThreadInputs.measurementTimeStamps[timestampIndex], - // Logger.getTimestamp(), - rawGyroRotation, - modulePositions); - } - - /** - * Gets the modules positions, sampled at the indexed timestamp. - * - * @param timestampIndex the timestamp index to sample. - * @return a list of SwerveModulePosition, containing relative drive position and absolute turn - * rotation at the sampled timestamp. - */ - private SwerveModulePosition[] getModulesPosition(int timestampIndex) { - SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[swerveModules.length]; - for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) - swerveModulePositions[moduleIndex] = - swerveModules[moduleIndex].getOdometryPositions()[timestampIndex]; - return swerveModulePositions; + // odometryThreadInputs.measurementTimeStamps[timestampIndex], + Logger.getTimestamp(), rawGyroRotation, modulePositions); } private SwerveModulePosition[] getModulesDelta(SwerveModulePosition[] freshModulesPosition) { diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 72d99337..c4a2317a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -92,7 +92,7 @@ public double getTurnVelocity() { /** Returns the current drive position of the module in meters. */ public double getDrivePositionMeters() { - return ModuleConstants.DRIVE_TO_METERS * inputs.drivePosition; //Wheel circumference + return ModuleConstants.DRIVE_TO_METERS * inputs.drivePosition; // Wheel circumference } /** Returns the current drive velocity of the module in meters per second. */ diff --git a/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java index 15092505..f89a9bfb 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java @@ -11,7 +11,6 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; @@ -49,15 +48,13 @@ public class PhysicalModule implements ModuleInterface { private final BaseStatusSignal[] periodicallyRefreshedSignals; public PhysicalModule(ModuleConfig moduleConfig) { - driveMotor = new TalonFX(moduleConfig.driveMotorChannel(), DeviceCANBus.RIO.name); - turnMotor = new TalonFX(moduleConfig.turnMotorChannel(), DeviceCANBus.RIO.name); - turnEncoder = new CANcoder(moduleConfig.turnEncoderChannel(), DeviceCANBus.RIO.name); + driveMotor = new TalonFX(moduleConfig.driveMotorChannel(), DeviceCANBus.CANIVORE.name); + turnMotor = new TalonFX(moduleConfig.turnMotorChannel(), DeviceCANBus.CANIVORE.name); + turnEncoder = new CANcoder(moduleConfig.turnEncoderChannel(), DeviceCANBus.CANIVORE.name); CANcoderConfiguration turnEncoderConfig = new CANcoderConfiguration(); turnEncoderConfig.MagnetSensor.MagnetOffset = -moduleConfig.angleZero(); turnEncoderConfig.MagnetSensor.SensorDirection = moduleConfig.encoderReversed(); - turnEncoderConfig.MagnetSensor.AbsoluteSensorRange = - AbsoluteSensorRangeValue.Signed_PlusMinusHalf; turnEncoder.getConfigurator().apply(turnEncoderConfig, HardwareConstants.TIMEOUT_S); TalonFXConfiguration driveConfig = new TalonFXConfiguration(); diff --git a/vendordeps/Phoenix6-25.0.0-beta-2.json b/vendordeps/Phoenix6-frc2025-beta-latest.json similarity index 85% rename from vendordeps/Phoenix6-25.0.0-beta-2.json rename to vendordeps/Phoenix6-frc2025-beta-latest.json index b602fcb2..cd9d153a 100644 --- a/vendordeps/Phoenix6-25.0.0-beta-2.json +++ b/vendordeps/Phoenix6-frc2025-beta-latest.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.0.0-beta-2.json", + "fileName": "Phoenix6-frc2025-beta-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -9,11 +9,6 @@ ], "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json", "conflictsWith": [ - { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" - }, { "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", @@ -24,19 +19,20 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.0.0-beta-2" + "version": "25.0.0-beta-3" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -44,12 +40,13 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -57,12 +54,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -70,12 +68,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -83,12 +82,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -96,12 +96,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -109,12 +110,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -122,12 +124,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -135,12 +138,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -148,12 +152,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -161,12 +166,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -176,7 +182,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -184,6 +190,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -191,7 +198,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -199,6 +206,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -206,7 +214,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -214,6 +222,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -221,7 +230,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -229,6 +238,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -236,7 +246,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,6 +254,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -251,7 +262,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -259,6 +270,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -266,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,6 +286,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -281,7 +294,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -289,6 +302,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -296,7 +310,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,6 +318,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -311,7 +326,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -319,6 +334,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -326,7 +342,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -334,6 +350,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" From e33b2a1765ceb13f8c55cce69b87affbdb9286f5 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Fri, 13 Dec 2024 13:09:44 -0500 Subject: [PATCH 65/75] redid setpoint generation --- src/main/java/frc/robot/Robot.java | 32 +- .../java/frc/robot/extras/util/MathUtil.java | 3 +- .../util/ProceduralStructGenerator.java | 857 ++++++++++++++++++ .../robot/subsystems/swerve/SwerveDrive.java | 39 +- .../AdvancedSwerveModuleState.java | 27 + .../swerve/setpointGen/SPGCalcs.java | 280 ++++++ .../swerve/setpointGen/SwerveSetpoint.java | 30 + .../setpointGen/SwerveSetpointGenerator.java | 379 ++++++++ 8 files changed, 1617 insertions(+), 30 deletions(-) create mode 100644 src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java create mode 100644 src/main/java/frc/robot/subsystems/swerve/setpointGen/AdvancedSwerveModuleState.java create mode 100644 src/main/java/frc/robot/subsystems/swerve/setpointGen/SPGCalcs.java create mode 100644 src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpoint.java create mode 100644 src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4627dfb6..b8386af4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -22,22 +22,22 @@ public class Robot extends LoggedRobot { public Robot() { // Record metadata - Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); - Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); - Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); - Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); - switch (BuildConstants.DIRTY) { - case 0: - Logger.recordMetadata("GitDirty", "All changes committed"); - break; - case 1: - Logger.recordMetadata("GitDirty", "Uncomitted changes"); - break; - default: - Logger.recordMetadata("GitDirty", "Unknown"); - break; - } + // Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + // Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + // Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + // Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + // Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + // switch (BuildConstants.DIRTY) { + // case 0: + // Logger.recordMetadata("GitDirty", "All changes committed"); + // break; + // case 1: + // Logger.recordMetadata("GitDirty", "Uncomitted changes"); + // break; + // default: + // Logger.recordMetadata("GitDirty", "Unknown"); + // break; + // } // Set up data receivers & replay source switch (Constants.currentMode) { diff --git a/src/main/java/frc/robot/extras/util/MathUtil.java b/src/main/java/frc/robot/extras/util/MathUtil.java index 65acfdfe..f67401aa 100644 --- a/src/main/java/frc/robot/extras/util/MathUtil.java +++ b/src/main/java/frc/robot/extras/util/MathUtil.java @@ -1,7 +1,6 @@ package frc.robot.extras.util; import edu.wpi.first.math.geometry.Rotation3d; -import frc.robot.BuildConstants; import java.util.Random; public class MathUtil { @@ -9,7 +8,7 @@ public class MathUtil { * random object that generates random variables the seed is the hash of GIT_SHA this way when you * do log-replay even the generated random numbers are the same */ - private static final Random random = new Random(BuildConstants.GIT_SHA.hashCode()); + private static final Random random = new Random((long) Math.random()); public static double linearInterpretationWithBounding( double x1, double y1, double x2, double y2, double x) { diff --git a/src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java b/src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java new file mode 100644 index 00000000..7a47a72e --- /dev/null +++ b/src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java @@ -0,0 +1,857 @@ +package frc.robot.extras.util; + +import edu.wpi.first.units.Measure; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import java.lang.annotation.Documented; +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; +import java.lang.reflect.AnnotatedElement; +import java.lang.reflect.Field; +import java.lang.reflect.InvocationTargetException; +import java.lang.reflect.Modifier; +import java.lang.reflect.RecordComponent; +import java.nio.ByteBuffer; +import java.util.ArrayList; +import java.util.Collection; +import java.util.HashMap; +import java.util.HashSet; +import java.util.Iterator; +import java.util.List; +import java.util.Optional; +import java.util.OptionalInt; +import java.util.Set; +import java.util.function.Supplier; +import java.util.stream.BaseStream; + +/** A utility class for procedurally generating {@link Struct}s from records and enums. */ +public final class ProceduralStructGenerator { + private ProceduralStructGenerator() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * A functional interface representing a method that retrives a value from a {@link ByteBuffer}. + */ + @FunctionalInterface + private interface Unpacker { + T unpack(ByteBuffer buffer); + } + + /** A functional interface representing a method that packs a value into a {@link ByteBuffer}. */ + @FunctionalInterface + private interface Packer { + ByteBuffer pack(ByteBuffer buffer, T value); + + static Packer fromStruct(Struct struct) { + return (buffer, value) -> { + struct.pack(buffer, value); + return buffer; + }; + } + } + + private record PrimType(String name, int size, Unpacker unpacker, Packer packer) {} + + /** A map of primitive types to their schema types. */ + private static final HashMap, PrimType> primitiveTypeMap = new HashMap<>(); + + private static void addPrimType( + Class boxedClass, + Class primitiveClass, + String name, + int size, + Unpacker unpacker, + Packer packer) { + PrimType primType = new PrimType<>(name, size, unpacker, packer); + primitiveTypeMap.put(boxedClass, primType); + primitiveTypeMap.put(primitiveClass, primType); + } + + // Add primitive types to the map + static { + addPrimType( + Integer.class, int.class, "int32", Integer.BYTES, ByteBuffer::getInt, ByteBuffer::putInt); + addPrimType( + Double.class, + double.class, + "float64", + Double.BYTES, + ByteBuffer::getDouble, + ByteBuffer::putDouble); + addPrimType( + Float.class, + float.class, + "float32", + Float.BYTES, + ByteBuffer::getFloat, + ByteBuffer::putFloat); + addPrimType( + Boolean.class, + boolean.class, + "bool", + Byte.BYTES, + buffer -> buffer.get() != 0, + (buffer, value) -> buffer.put((byte) (value ? 1 : 0))); + addPrimType( + Character.class, + char.class, + "char", + Character.BYTES, + ByteBuffer::getChar, + ByteBuffer::putChar); + addPrimType(Byte.class, byte.class, "uint8", Byte.BYTES, ByteBuffer::get, ByteBuffer::put); + addPrimType( + Short.class, short.class, "int16", Short.BYTES, ByteBuffer::getShort, ByteBuffer::putShort); + addPrimType( + Long.class, long.class, "int64", Long.BYTES, ByteBuffer::getLong, ByteBuffer::putLong); + } + + /** + * A map of types to their custom struct schemas. + * + *

    This allows adding custom struct implementations for types that are not supported by + * default. Think of vendor-specific. + */ + private static final HashMap, Struct> customStructTypeMap = new HashMap<>(); + + /** + * Add a custom struct to the structifier. + * + * @param The type the struct is for. + * @param clazz The class of the type. + * @param struct The struct to add. + * @param override Whether to override an existing struct. An existing struct could mean the type + * already has a {@code struct} field and implemnts {@link StructSerializable} or that the + * type is already in the custom struct map. + */ + public static void addCustomStruct(Class clazz, Struct struct, boolean override) { + if (override) { + customStructTypeMap.put(clazz, struct); + } else if (!StructSerializable.class.isAssignableFrom(clazz)) { + customStructTypeMap.putIfAbsent(clazz, struct); + } + } + + /** + * Returns a {@link Struct} for the given {@link StructSerializable} marked class. Due to the + * non-contractual nature of the marker this can fail. If the {@code struct} field could not be + * accessed for any reason, an empty {@link Optional} is returned. + * + * @param The type of the class. + * @param clazz The class object to extract the struct from. + * @return An optional containing the struct if it could be extracted. + */ + @SuppressWarnings("unchecked") + public static Optional> extractClassStruct( + Class clazz) { + try { + var possibleField = Optional.ofNullable(clazz.getDeclaredField("struct")); + return possibleField.flatMap( + field -> { + field.setAccessible(true); + if (Struct.class.isAssignableFrom(field.getType())) { + try { + return Optional.ofNullable((Struct) field.get(null)); + } catch (IllegalAccessException e) { + return Optional.empty(); + } + } else { + return Optional.empty(); + } + }); + } catch (NoSuchFieldException e) { + return Optional.empty(); + } + } + + /** + * Returns a {@link Struct} for the given class. This does not do compile time checking that the + * class is a {@link StructSerializable}. Whenever possible it is reccomended to use {@link + * #extractClassStruct(Class)}. + * + * @param clazz The class object to extract the struct from. + * @return An optional containing the struct if it could be extracted. + */ + @SuppressWarnings("unchecked") + public static Optional> extractClassStructDynamic(Class clazz) { + if (StructSerializable.class.isAssignableFrom(clazz)) { + return extractClassStruct((Class) clazz).map(struct -> struct); + } else { + return Optional.empty(); + } + } + + @Retention(RetentionPolicy.RUNTIME) + @Target({ElementType.FIELD, ElementType.RECORD_COMPONENT}) + @Documented + public @interface IgnoreStructField {} + + @Retention(RetentionPolicy.RUNTIME) + @Target({ElementType.FIELD, ElementType.RECORD_COMPONENT}) + @Documented + public @interface FixedSizeArray { + int size(); + } + + private static OptionalInt arraySize(AnnotatedElement field) { + return Optional.ofNullable(field.getAnnotation(FixedSizeArray.class)) + .map(FixedSizeArray::size) + .map(OptionalInt::of) + .orElse(OptionalInt.empty()); + } + + private static boolean shouldIgnore(AnnotatedElement field) { + return field.isAnnotationPresent(IgnoreStructField.class); + } + + private record StructField( + String name, + String type, + int size, + boolean immutable, + Set> structsToLoad, + Unpacker unpacker, + Packer packer) { + + public static StructField fromField(Field field) { + return StructField.fromNameAndClass( + field.getName(), + field.getType(), + arraySize(field), + Modifier.isFinal(field.getModifiers())); + } + + public static StructField fromRecordComponent(RecordComponent component) { + return StructField.fromNameAndClass( + component.getName(), component.getType(), arraySize(component), true); + } + + @SuppressWarnings("unchecked") + public static StructField fromNameAndClass( + String name, Class clazz, OptionalInt arraySize, boolean isFinal) { + if (!isFixedSize(clazz, arraySize)) { + return null; + } + if (clazz.isArray() && arraySize.isPresent()) { + final Class componentType = clazz.getComponentType(); + final int size = arraySize.getAsInt(); + final StructField componentField = + fromNameAndClass( + componentType.getSimpleName(), componentType, OptionalInt.empty(), false); + return new StructField( + name + "[" + size + "]", + componentField.type, + componentField.size * size, + isFinal, + componentField.structsToLoad, + buffer -> { + Object[] array = new Object[size]; + for (int i = 0; i < size; i++) { + array[i] = componentField.unpacker.unpack(buffer); + } + return array; + }, + (buffer, value) -> { + for (Object obj : (Object[]) value) { + ((Packer) componentField.packer).pack(buffer, obj); + } + return buffer; + }); + } else if (Measure.class.isAssignableFrom(clazz)) { + return new StructField( + name, + "float64", + Double.BYTES, + isFinal, + Set.of(), + buffer -> { + throw new UnsupportedOperationException("Cannot unpack Measure"); + }, + (buffer, value) -> buffer.putDouble(((Measure) value).baseUnitMagnitude())); + } else if (primitiveTypeMap.containsKey(clazz)) { + PrimType primType = primitiveTypeMap.get(clazz); + return new StructField( + name, + primType.name, + primType.size, + isFinal, + Set.of(), + primType.unpacker, + primType.packer); + } else { + Struct struct = null; + if (customStructTypeMap.containsKey(clazz)) { + struct = customStructTypeMap.get(clazz); + } else if (StructSerializable.class.isAssignableFrom(clazz)) { + struct = extractClassStructDynamic(clazz).orElse(null); + } + if (struct == null) { + return null; + } + Set> structsToLoad = new HashSet<>(Set.of(struct.getNested())); + structsToLoad.add(struct); + return new StructField( + name, + struct.getTypeName(), + struct.getSize(), + struct.isImmutable() && isFinal, + structsToLoad, + struct::unpack, + Packer.fromStruct(struct)); + } + } + } + + /** + * Introspects a class to determine if it's a fixed size. + * + *

    Fixed size means no collections, no strings, no arrays, etc. + * + * @param clazz The class to introspect. + * @return Whether the class is fixed size. + */ + public static boolean isFixedSize(Class clazz, OptionalInt arraySize) { + if (clazz.isArray()) { + if (arraySize.isEmpty()) { + return false; + } else { + Class componentType = clazz.getComponentType(); + return isFixedSize(componentType, OptionalInt.empty()); + } + } else if (clazz.isRecord()) { + for (RecordComponent component : clazz.getRecordComponents()) { + if (!isFixedSize(component.getType(), arraySize(component))) { + return false; + } + } + } else { + for (Field field : clazz.getDeclaredFields()) { + Class fieldClass = field.getType(); + if (field.isEnumConstant() || Modifier.isStatic(field.getModifiers())) { + continue; + } + if (Collection.class.isAssignableFrom(fieldClass) + || Iterator.class.isAssignableFrom(fieldClass) + || Iterable.class.isAssignableFrom(fieldClass) + || BaseStream.class.isAssignableFrom(fieldClass) + || fieldClass.isArray() + || fieldClass == String.class + || fieldClass == Optional.class) { + return false; + } + if (!primitiveTypeMap.containsKey(fieldClass) + && !isFixedSize(fieldClass, arraySize(field))) { + return false; + } + } + } + return true; + } + + /** + * Introspects a class to determine if it's interiorly mutable. + * + *

    Interior mutability means that the class has fields that are mutable. + * + * @param clazz The class to introspect. + * @return Whether the class is interiorly mutable. + */ + public static boolean isInteriorlyMutable(Class clazz) { + if (clazz.isArray()) { + return true; + } else if (clazz.isRecord()) { + for (RecordComponent component : clazz.getRecordComponents()) { + if (isInteriorlyMutable(component.getType())) { + return true; + } + } + } else { + for (Field field : clazz.getDeclaredFields()) { + if (field.isEnumConstant() || Modifier.isStatic(field.getModifiers())) { + continue; + } + if (!Modifier.isFinal(field.getModifiers())) { + return true; + } + if (!primitiveTypeMap.containsKey(field.getType()) + && isInteriorlyMutable(field.getType())) { + return true; + } + } + } + return false; + } + + /** A utility for building schema syntax in a procedural manner. */ + @SuppressWarnings("PMD.AvoidStringBufferField") + public static class SchemaBuilder { + /** A utility for building enum fields in a procedural manner. */ + public static final class EnumFieldBuilder { + private final StringBuilder m_builder = new StringBuilder(); + private final String m_fieldName; + private boolean m_firstVariant = true; + + /** + * Creates a new enum field builder. + * + * @param fieldName The name of the field. + */ + public EnumFieldBuilder(String fieldName) { + this.m_fieldName = fieldName; + m_builder.append("enum {"); + } + + /** + * Adds a variant to the enum field. + * + * @param name The name of the variant. + * @param value The value of the variant. + * @return The builder for chaining. + */ + public EnumFieldBuilder addVariant(String name, int value) { + if (!m_firstVariant) { + m_builder.append(','); + } + m_firstVariant = false; + m_builder.append(name).append('=').append(value); + return this; + } + + /** + * Builds the enum field. If this object is being used with {@link SchemaBuilder#addEnumField} + * then {@link #build()} does not have to be called by the user. + * + * @return The built enum field. + */ + public String build() { + m_builder.append("} int8 ").append(m_fieldName).append(';'); + return m_builder.toString(); + } + } + + /** Creates a new schema builder. */ + public SchemaBuilder() {} + + private final StringBuilder m_builder = new StringBuilder(); + + /** + * Adds a field to the schema. + * + * @param name The name of the field. + * @param type The type of the field. + * @return The builder for chaining. + */ + public SchemaBuilder addField(StructField field) { + m_builder.append(field.type).append(' ').append(field.name).append(';'); + return this; + } + + /** + * Adds an inline enum field to the schema. + * + * @param enumFieldBuilder The builder for the enum field. + * @return The builder for chaining. + */ + public SchemaBuilder addEnumField(EnumFieldBuilder enumFieldBuilder) { + m_builder.append(enumFieldBuilder.build()); + return this; + } + + /** + * Builds the schema. + * + * @return The built schema. + */ + public String build() { + return m_builder.toString(); + } + } + + public static Struct noopStruct(Class cls) { + return new Struct<>() { + @Override + public Class getTypeClass() { + return cls; + } + + @Override + public String getTypeName() { + return cls.getSimpleName(); + } + + @Override + public String getSchema() { + return ""; + } + + @Override + public int getSize() { + return 0; + } + + @Override + public void pack(ByteBuffer buffer, T value) {} + + @Override + public T unpack(ByteBuffer buffer) { + return null; + } + }; + } + + private abstract static class ProcStruct implements Struct { + protected final Class typeClass; + protected final List fields; + private final String schema; + + // stored values so we never recompute them + private final int size; + private final boolean isImmutable; + private final Struct[] nested; + + public ProcStruct(Class typeClass, List fields, String schema) { + this.typeClass = typeClass; + this.fields = fields; + this.schema = schema; + + this.size = fields.stream().mapToInt(StructField::size).sum(); + this.isImmutable = fields.stream().allMatch(StructField::immutable); + this.nested = + fields.stream() + .map(StructField::structsToLoad) + .flatMap(Collection::stream) + .toArray(Struct[]::new); + + ProceduralStructGenerator.customStructTypeMap.put(typeClass, this); + } + + @Override + public Class getTypeClass() { + return typeClass; + } + + @Override + public String getTypeName() { + return typeClass.getSimpleName(); + } + + @Override + public String getSchema() { + return schema; + } + + @Override + public int getSize() { + return size; + } + + @Override + public boolean isCloneable() { + return Cloneable.class.isAssignableFrom(typeClass); + } + + @Override + @SuppressWarnings("unchecked") + public T clone(T obj) throws CloneNotSupportedException { + if (isCloneable()) { + try { + return (T) typeClass.getMethod("clone").invoke(obj); + } catch (IllegalAccessException | InvocationTargetException | NoSuchMethodException e) { + throw new CloneNotSupportedException(); + } + } else { + throw new CloneNotSupportedException(); + } + } + + @Override + public boolean isImmutable() { + return isImmutable; + } + + @Override + public Struct[] getNested() { + return nested; + } + + @Override + public String toString() { + return this.getTypeName() + "<" + this.getSize() + ">" + " {" + this.schema + "}"; + } + } + + /** + * Generates a {@link Struct} for the given {@link Record} class. If a {@link Struct} cannot be + * generated from the {@link Record}, the errors encountered will be printed and a no-op {@link + * Struct} will be returned. + * + * @param The type of the record. + * @param recordClass The class of the record. + * @return The generated struct. + */ + @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) + public static Struct genRecord(final Class recordClass) { + final RecordComponent[] components = recordClass.getRecordComponents(); + final SchemaBuilder schemaBuilder = new SchemaBuilder(); + final ArrayList fields = new ArrayList<>(); + + for (final RecordComponent component : components) { + if (shouldIgnore(component)) { + continue; + } + component.getAccessor().setAccessible(true); + fields.add(StructField.fromRecordComponent(component)); + } + + if (fields.stream().anyMatch(f -> f == null)) { + return noopStruct(recordClass); + } + fields.forEach(schemaBuilder::addField); + + return new ProcStruct<>(recordClass, fields, schemaBuilder.build()) { + @Override + public void pack(ByteBuffer buffer, R value) { + boolean failed = false; + int startingPosition = buffer.position(); + for (int i = 0; i < components.length; i++) { + Packer packer = (Packer) fields.get(i).packer(); + try { + Object componentValue = components[i].getAccessor().invoke(value); + if (componentValue == null) { + throw new IllegalArgumentException("Component is null"); + } + packer.pack(buffer, componentValue); + } catch (IllegalAccessException + | IllegalArgumentException + | InvocationTargetException e) { + failed = true; + break; + } + } + if (failed) { + buffer.put(startingPosition, new byte[this.getSize()]); + } + } + + @Override + public R unpack(ByteBuffer buffer) { + try { + Object[] args = new Object[components.length]; + Class[] argTypes = new Class[components.length]; + for (int i = 0; i < components.length; i++) { + args[i] = fields.get(i).unpacker().unpack(buffer); + argTypes[i] = components[i].getType(); + } + return recordClass.getConstructor(argTypes).newInstance(args); + } catch (InstantiationException + | IllegalAccessException + | InvocationTargetException + | NoSuchMethodException + | SecurityException e) { + System.err.println( + "Could not unpack record: " + + recordClass.getSimpleName() + + "\n " + + e.getMessage()); + return null; + } + } + }; + } + + /** + * Generates a {@link Struct} for the given {@link Enum} class. If a {@link Struct} cannot be + * generated from the {@link Enum}, the errors encountered will be printed and a no-op {@link + * Struct} will be returned. + * + * @param The type of the enum. + * @param enumClass The class of the enum. + * @return The generated struct. + */ + @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) + public static > Struct genEnum(Class enumClass) { + final E[] enumVariants = enumClass.getEnumConstants(); + final Field[] allEnumFields = enumClass.getDeclaredFields(); + final SchemaBuilder schemaBuilder = new SchemaBuilder(); + final SchemaBuilder.EnumFieldBuilder enumFieldBuilder = + new SchemaBuilder.EnumFieldBuilder("variant"); + final HashMap enumMap = new HashMap<>(); + final ArrayList fields = new ArrayList<>(); + + if (enumVariants == null || enumVariants.length == 0) { + return noopStruct(enumClass); + } + + for (final E constant : enumVariants) { + final String name = constant.name(); + final int ordinal = constant.ordinal(); + + enumFieldBuilder.addVariant(name, ordinal); + enumMap.put(ordinal, constant); + } + schemaBuilder.addEnumField(enumFieldBuilder); + fields.add( + new StructField( + "variant", + "int8", + 1, + true, + Set.of(), + ByteBuffer::get, + (buffer, value) -> buffer.put((byte) ((Enum) value).ordinal()))); + + final List enumFields = + List.of(allEnumFields).stream() + .filter( + f -> + !f.isEnumConstant() && !Modifier.isStatic(f.getModifiers()) && !shouldIgnore(f)) + .toList(); + + for (final Field field : enumFields) { + field.setAccessible(true); + fields.add(StructField.fromField(field)); + } + if (fields.stream().anyMatch(f -> f == null)) { + return noopStruct(enumClass); + } + for (int i = 1; i < fields.size(); i++) { + // do this to skip the variant field + schemaBuilder.addField(fields.get(i)); + } + + return new ProcStruct<>(enumClass, fields, schemaBuilder.build()) { + @Override + public void pack(ByteBuffer buffer, E value) { + boolean failed = false; + int startingPosition = buffer.position(); + buffer.put((byte) value.ordinal()); + for (int i = 0; i < enumFields.size(); i++) { + Packer packer = (Packer) fields.get(i + 1).packer(); + Field field = enumFields.get(i); + try { + Object fieldValue = field.get(value); + if (fieldValue == null) { + throw new IllegalArgumentException("Field is null"); + } + packer.pack(buffer, fieldValue); + } catch (IllegalArgumentException | IllegalAccessException e) { + System.err.println( + "Could not pack enum field: " + + enumClass.getSimpleName() + + "#" + + field.getName() + + "\n " + + e.getMessage()); + failed = true; + break; + } + } + if (failed) { + buffer.put(startingPosition, new byte[this.getSize()]); + } + } + + final byte[] m_spongeBuffer = new byte[this.getSize() - 1]; + + @Override + public E unpack(ByteBuffer buffer) { + int ordinal = buffer.get(); + buffer.get(m_spongeBuffer); + return enumMap.getOrDefault(ordinal, null); + } + + public boolean isCloneable() { + return true; + } + ; + + public E clone(E obj) throws CloneNotSupportedException { + return obj; + } + ; + }; + } + + /** + * Generates a {@link Struct} for the given {@link Object} class. If a {@link Struct} cannot be + * generated from the {@link Object}, the errors encountered will be printed and a no-op {@link + * Struct} will be returned. + * + * @param The type of the object. + * @param objectClass The class of the object. + * @param objectSupplier A supplier for the object. + * @return The generated struct. + */ + @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) + public static Struct genObject(Class objectClass, Supplier objectSupplier) { + final SchemaBuilder schemaBuilder = new SchemaBuilder(); + final Field[] allFields = + List.of(objectClass.getDeclaredFields()).stream() + .filter(f -> !shouldIgnore(f) && !Modifier.isStatic(f.getModifiers())) + .toArray(Field[]::new); + final ArrayList fields = new ArrayList<>(allFields.length); + + for (final Field field : allFields) { + field.setAccessible(true); + fields.add(StructField.fromField(field)); + } + + if (fields.stream().anyMatch(f -> f == null)) { + return noopStruct(objectClass); + } + fields.forEach(schemaBuilder::addField); + + return new ProcStruct<>(objectClass, fields, schemaBuilder.build()) { + @Override + public void pack(ByteBuffer buffer, O value) { + boolean failed = false; + int startingPosition = buffer.position(); + for (int i = 0; i < allFields.length; i++) { + Packer packer = (Packer) fields.get(i).packer(); + try { + Object fieldValue = allFields[i].get(value); + if (fieldValue == null) { + throw new IllegalArgumentException("Field is null"); + } + packer.pack(buffer, fieldValue); + } catch (IllegalArgumentException | IllegalAccessException e) { + System.err.println( + "Could not pack object field: " + + objectClass.getSimpleName() + + "#" + + allFields[i].getName() + + "\n " + + e.getMessage()); + failed = true; + break; + } + } + if (failed) { + buffer.put(startingPosition, new byte[this.getSize()]); + } + } + + @Override + public O unpack(ByteBuffer buffer) { + try { + O obj = objectSupplier.get(); + for (int i = 0; i < allFields.length; i++) { + Object fieldValue = fields.get(i).unpacker().unpack(buffer); + allFields[i].set(obj, fieldValue); + } + return obj; + } catch (IllegalArgumentException | IllegalAccessException e) { + System.err.println( + "Could not unpack object: " + + objectClass.getSimpleName() + + "\n " + + e.getMessage()); + return null; + } + } + }; + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index ff38a99c..47eefbce 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -8,21 +8,25 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP; import frc.robot.extras.util.DeviceCANBus; import frc.robot.extras.util.TimeUtil; import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; +import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; import frc.robot.subsystems.swerve.gyroIO.GyroInputsAutoLogged; import frc.robot.subsystems.swerve.gyroIO.GyroInterface; import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; import frc.robot.subsystems.swerve.odometryThread.OdometryThread; import frc.robot.subsystems.swerve.odometryThread.OdometryThreadInputsAutoLogged; +import frc.robot.subsystems.swerve.setpointGen.SwerveSetpoint; +import frc.robot.subsystems.swerve.setpointGen.SwerveSetpointGenerator; import frc.robot.subsystems.vision.VisionConstants; import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; @@ -38,6 +42,19 @@ public class SwerveDrive extends SubsystemBase { private final SwerveModulePosition[] lastModulePositions; private final SwerveDrivePoseEstimator poseEstimator; + private final SwerveSetpointGenerator setpointGenerator = + new SwerveSetpointGenerator( + DriveConstants.MODULE_TRANSLATIONS, + DCMotor.getKrakenX60(1).withReduction(ModuleConstants.DRIVE_GEAR_RATIO), + DCMotor.getFalcon500(1).withReduction(11), + 60, + 58, + 11, + ModuleConstants.WHEEL_DIAMETER_METERS, + WHEEL_GRIP.TIRE_WHEEL.cof, + 0.0); + private SwerveSetpoint setpoint = SwerveSetpoint.zeroed(); + private final OdometryThread odometryThread; private final Alert gyroDisconnectedAlert = @@ -187,17 +204,15 @@ private void modulesPeriodic() { * @param fieldRelative Whether the provided x and y speeds are relative to the field. */ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative) { - SwerveModuleState[] swerveModuleStates = - DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rotationSpeed, getPose().getRotation()) - : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed)); - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DriveConstants.MAX_SPEED_METERS_PER_SECOND); - - setModuleStates(swerveModuleStates); - Logger.recordOutput("SwerveStates/DesiredStates", swerveModuleStates); + ChassisSpeeds desiredSpeeds = + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rotationSpeed, getPose().getRotation()) + : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); + + setpoint = setpointGenerator.generateSetpoint(setpoint, desiredSpeeds, 0.02); + setModuleStates(setpoint.moduleStates()); + Logger.recordOutput("SwerveStates/DesiredStates", setpoint.moduleStates()); } /** Returns 0 degrees if the robot is on the blue alliance, 180 if on the red alliance. */ diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/AdvancedSwerveModuleState.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/AdvancedSwerveModuleState.java new file mode 100644 index 00000000..064f2dd0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/AdvancedSwerveModuleState.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems.swerve.setpointGen; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.struct.Struct; + +public final class AdvancedSwerveModuleState extends SwerveModuleState { + public double steerVelocityFF; + public double driveAccelerationFF; + + public AdvancedSwerveModuleState( + double speedMetersPerSecond, + Rotation2d angle, + double steerVelocityFF, + double driveAccelerationFF) { + super(speedMetersPerSecond, angle); + this.steerVelocityFF = steerVelocityFF; + this.driveAccelerationFF = driveAccelerationFF; + } + + // todo: implement custom struct + public static final Struct struct = SwerveModuleState.struct; + + public static AdvancedSwerveModuleState fromBase(SwerveModuleState base) { + return new AdvancedSwerveModuleState(base.speedMetersPerSecond, base.angle, 0.0, 0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SPGCalcs.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SPGCalcs.java new file mode 100644 index 00000000..4c6d37d1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SPGCalcs.java @@ -0,0 +1,280 @@ +package frc.robot.subsystems.swerve.setpointGen; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import frc.robot.extras.util.ProceduralStructGenerator; +import frc.robot.extras.util.ProceduralStructGenerator.FixedSizeArray; +import frc.robot.extras.util.ProceduralStructGenerator.IgnoreStructField; +import java.nio.ByteBuffer; +import java.util.Arrays; + +class SPGCalcs { + private static final double kEpsilon = 1E-8; + private static final int MAX_STEER_ITERATIONS = 8; + private static final int MAX_DRIVE_ITERATIONS = 10; + static final int NUM_MODULES = 4; + + static double unwrapAngle(double ref, double angle) { + double diff = angle - ref; + if (diff > Math.PI) { + return angle - 2.0 * Math.PI; + } else if (diff < -Math.PI) { + return angle + 2.0 * Math.PI; + } else { + return angle; + } + } + + @FunctionalInterface + interface Function2d { + double f(double x, double y); + } + + /** + * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. + * This is a pretty naive way to do root finding, but it's usually faster than simple bisection + * while being robust in ways that e.g. the Newton-Raphson method isn't. + * + * @param func The Function2d to take the root of. + * @param x_0 x value of the lower bracket. + * @param y_0 y value of the lower bracket. + * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during + * recursion) + * @param x_1 x value of the upper bracket. + * @param y_1 y value of the upper bracket. + * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during + * recursion) + * @param iterations_left Number of iterations of root finding left. + * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the + * (approximate) root. + */ + static double findRoot( + Function2d func, + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + int iterations_left) { + var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); + + if (iterations_left < 0 || epsilonEquals(f_0, f_1)) { + return s_guess; + } + + var x_guess = (x_1 - x_0) * s_guess + x_0; + var y_guess = (y_1 - y_0) * s_guess + y_0; + var f_guess = func.f(x_guess, y_guess); + if (Math.signum(f_0) == Math.signum(f_guess)) { + // 0 and guess on same side of root, so use upper bracket. + return s_guess + + (1.0 - s_guess) + * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1); + } else { + // Use lower bracket. + return s_guess + * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); + } + } + + static double findSteeringMaxS( + double prevVx, + double prevVy, + double prevHeading, + double desiredVx, + double desiredVy, + double desiredHeading, + double maxDeviation) { + desiredHeading = unwrapAngle(prevHeading, desiredHeading); + double diff = desiredHeading - prevHeading; + if (Math.abs(diff) <= maxDeviation) { + // Can go all the way to s=1. + return 1.0; + } + double offset = prevHeading + Math.signum(diff) * maxDeviation; + Function2d func = (x, y) -> unwrapAngle(prevHeading, Math.atan2(y, x)) - offset; + return findRoot( + func, + prevVx, + prevVy, + prevHeading - offset, + desiredVx, + desiredVy, + desiredHeading - offset, + MAX_STEER_ITERATIONS); + } + + static double findDriveMaxS( + double x_0, double y_0, double f_0, double x_1, double y_1, double f_1, double max_vel_step) { + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_vel_step) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_vel_step; + Function2d func = (x, y) -> Math.hypot(x, y) - offset; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, MAX_DRIVE_ITERATIONS); + } + + static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, kEpsilon); + } + + static boolean epsilonEquals(ChassisSpeeds s1, ChassisSpeeds s2) { + return epsilonEquals(s1.vxMetersPerSecond, s2.vxMetersPerSecond) + && epsilonEquals(s1.vyMetersPerSecond, s2.vyMetersPerSecond) + && epsilonEquals(s1.omegaRadiansPerSecond, s2.omegaRadiansPerSecond); + } + + /** + * Check if it would be faster to go to the opposite of the goal heading (and reverse drive + * direction). + * + * @param prevToGoal The rotation from the previous state to the goal state (i.e. + * prev.inverse().rotateBy(goal)). + * @return True if the shortest path to achieve this rotation involves flipping the drive + * direction. + */ + static boolean flipHeading(double prevToGoal) { + return Math.abs(prevToGoal) > Math.PI / 2.0; + } + + private static final Struct structVectors; + private static final Struct structVars; + + static { + final var structVectorsProc = + ProceduralStructGenerator.genObject(LocalVectors.class, LocalVectors::new); + structVectors = + new Struct() { + @Override + public String getSchema() { + return "float64 vx;float64 vy;float64 cos;float64 sin;"; + } + + @Override + public int getSize() { + return 32; + } + + @Override + public Class getTypeClass() { + return LocalVectors.class; + } + + @Override + public String getTypeName() { + return "LocalVectors"; + } + + @Override + public void pack(ByteBuffer bb, LocalVectors value) { + bb.putDouble(value.vx); + bb.putDouble(value.vy); + bb.putDouble(value.cos); + bb.putDouble(value.sin); + } + + @Override + public LocalVectors unpack(ByteBuffer bb) { + return structVectorsProc.unpack(bb); + } + + @Override + public String toString() { + return this.getTypeName() + "<" + this.getSize() + ">" + " {" + this.getSchema() + "}"; + } + }; + structVars = ProceduralStructGenerator.genObject(LocalVars.class, LocalVars::new); + System.out.println(structVectors); + System.out.println(structVars); + } + + static final class LocalVectors implements StructSerializable { + public double vx, vy, cos, sin; + + public LocalVectors() {} + + public void reset() { + vx = vy = cos = sin = 0.0; + } + + public void applyModuleState(SwerveModuleState state) { + cos = state.angle.getCos(); + sin = state.angle.getSin(); + vx = cos * state.speedMetersPerSecond; + vy = sin * state.speedMetersPerSecond; + if (state.speedMetersPerSecond < 0.0) { + applyRotation(Rotation2d.k180deg.getCos(), Rotation2d.k180deg.getSin()); + } + } + + public LocalVectors applyRotation(double otherCos, double otherSin) { + double newCos = cos * otherCos - sin * otherSin; + double newSin = cos * otherSin + sin * otherCos; + cos = newCos; + sin = newSin; + + return this; + } + + public double radians() { + return Math.atan2(sin, cos); + } + + public static final Struct struct = structVectors; + } + + static final class LocalVars implements StructSerializable { + @FixedSizeArray(size = NUM_MODULES) + public LocalVectors[] prev; + + @FixedSizeArray(size = NUM_MODULES) + public LocalVectors[] desired; + + public boolean needToSteer = true, allModulesShouldFlip = true; + public double minS, dt; + public double dx, dy, dtheta; + public ChassisSpeeds prevSpeeds, desiredSpeeds; + + @FixedSizeArray(size = NUM_MODULES) + public SwerveModuleState[] prevModuleStates; + + @FixedSizeArray(size = NUM_MODULES) + public SwerveModuleState[] desiredModuleStates; + + @IgnoreStructField public Rotation2d[] steeringOverride; + + public LocalVars() { + desiredSpeeds = prevSpeeds = new ChassisSpeeds(); + prev = new LocalVectors[NUM_MODULES]; + desired = new LocalVectors[NUM_MODULES]; + steeringOverride = new Rotation2d[NUM_MODULES]; + for (int i = 0; i < NUM_MODULES; i++) { + prev[i] = new LocalVectors(); + desired[i] = new LocalVectors(); + } + } + + public LocalVars reset() { + needToSteer = allModulesShouldFlip = true; + Arrays.fill(steeringOverride, null); + for (int i = 0; i < NUM_MODULES; i++) { + prev[i].reset(); + desired[i].reset(); + } + + return this; + } + + public static final Struct struct = structVars; + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpoint.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpoint.java new file mode 100644 index 00000000..9ec12667 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpoint.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.swerve.setpointGen; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import frc.robot.extras.util.ProceduralStructGenerator; +import frc.robot.extras.util.ProceduralStructGenerator.FixedSizeArray; + +public record SwerveSetpoint( + ChassisSpeeds chassisSpeeds, + @FixedSizeArray(size = 4) AdvancedSwerveModuleState[] moduleStates) // + implements StructSerializable { + public static SwerveSetpoint zeroed() { + return new SwerveSetpoint( + new ChassisSpeeds(), + new AdvancedSwerveModuleState[] { + new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0), + new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0), + new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0), + new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0) + }); + } + + public static final Struct struct; + + static { + struct = ProceduralStructGenerator.genRecord(SwerveSetpoint.class); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java new file mode 100644 index 00000000..f92dc608 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java @@ -0,0 +1,379 @@ +package frc.robot.subsystems.swerve.setpointGen; + +import static frc.robot.subsystems.swerve.setpointGen.SPGCalcs.*; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.RobotController; + +/** + * Swerve setpoint generatoR that has been passed around so many times its hard to keep track, just + * know i didn't write most the logic in this code that credit goes to 254 and mjansen + * + *

    Takes a prior setpoint, a desired setpoint, and outputs a new setpoint that respects all the + * kinematic constraints on module rotation and wheel velocity/torque, as well as preventing any + * forces acting on a module's wheel from exceeding the force of friction. + */ +public class SwerveSetpointGenerator { + private static final ChassisSpeeds ZERO_SPEEDS = new ChassisSpeeds(); + + private static final int NUM_MODULES = SPGCalcs.NUM_MODULES; + + private final SwerveDriveKinematics kinematics; + private final Translation2d[] moduleLocations; + private final DCMotor driveMotor; + private final double driveCurrentLimitAmps, + maxDriveVelocityMPS, + maxSteerVelocityRadsPerSec, + massKg, + moiKgMetersSquared, + wheelRadiusMeters, + wheelFrictionForce, + // maxTorqueFriction, + torqueLoss; + + public SwerveSetpointGenerator( + final Translation2d[] moduleLocations, + final DCMotor driveMotor, + final DCMotor angleMotor, + final double driveCurrentLimitAmps, + final double massKg, + final double moiKgMetersSquared, + final double wheelDiameterMeters, + final double wheelCoF, + final double torqueLoss) { + + if (moduleLocations.length != NUM_MODULES) { + throw new IllegalArgumentException("Module locations must have 4 elements"); + } + + this.driveMotor = driveMotor; + this.driveCurrentLimitAmps = driveCurrentLimitAmps; + this.maxSteerVelocityRadsPerSec = angleMotor.freeSpeedRadPerSec; + kinematics = new SwerveDriveKinematics(moduleLocations); + this.moduleLocations = moduleLocations; + this.massKg = massKg; + this.moiKgMetersSquared = moiKgMetersSquared; + this.wheelRadiusMeters = wheelDiameterMeters / 2; + this.maxDriveVelocityMPS = driveMotor.freeSpeedRadPerSec * wheelRadiusMeters; + + wheelFrictionForce = wheelCoF * ((massKg / 4) * 9.81); + // maxTorqueFriction = this.wheelFrictionForce * wheelRadiusMeters; + this.torqueLoss = torqueLoss; + } + + // alot of work was done to reduce allocations in this hot loop, + // migrating everything over to a vars object that gets reused + // was the best way to do this. + private static final LocalVars VARS_TEMPLATE = new LocalVars(); + + /** + * Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from + * this method. This method will discretize the speeds for you. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredRobotRelativeSpeeds The desired state of motion, such as from the driver sticks + * or a path following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging to + * desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) { + // https://github.com/wpilibsuite/allwpilib/issues/7332 + SwerveModuleState[] desiredModuleStates = + kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds); + // Make sure desiredState respects velocity limits. + SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleStates, maxDriveVelocityMPS); + desiredRobotRelativeSpeeds = kinematics.toChassisSpeeds(desiredModuleStates); + + final LocalVars vars = VARS_TEMPLATE.reset(); + vars.dt = dt; + vars.prevSpeeds = prevSetpoint.chassisSpeeds(); + vars.desiredSpeeds = desiredRobotRelativeSpeeds; + vars.desiredModuleStates = desiredModuleStates; + vars.prevModuleStates = prevSetpoint.moduleStates(); + vars.dx = + desiredRobotRelativeSpeeds.vxMetersPerSecond + - prevSetpoint.chassisSpeeds().vxMetersPerSecond; + vars.dy = + desiredRobotRelativeSpeeds.vyMetersPerSecond + - prevSetpoint.chassisSpeeds().vyMetersPerSecond; + vars.dtheta = + desiredRobotRelativeSpeeds.omegaRadiansPerSecond + - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; + vars.minS = 1.0; + + // Logger.recordOutput("beginningVars", vars); + + checkNeedToSteer(vars); + // Logger.recordOutput("postCheckNeedToSteer", vars); + makeVectors(vars); + // Logger.recordOutput("pastMakeVectors", vars); + + // if (vars.allModulesShouldFlip + // && !epsilonEquals(prevSetpoint.chassisSpeeds(), ZERO_SPEEDS) + // && !epsilonEquals(desiredRobotRelativeSpeeds, ZERO_SPEEDS)) { + // // It will (likely) be faster to stop the robot, rotate the modules in place to the + // complement + // // of the desired angle, and accelerate again. + // return generateSetpoint(prevSetpoint, ZERO_SPEEDS, dt); + // } + + solveSteering(vars); + // Logger.recordOutput("postSolveSteering", vars); + + solveDriving(vars); + // Logger.recordOutput("postSolveDriving", vars); + + ChassisSpeeds retSpeeds = + new ChassisSpeeds( + vars.prevSpeeds.vxMetersPerSecond + vars.minS * vars.dx, + vars.prevSpeeds.vyMetersPerSecond + vars.minS * vars.dy, + vars.prevSpeeds.omegaRadiansPerSecond + vars.minS * vars.dtheta); + retSpeeds = ChassisSpeeds.discretize(retSpeeds, dt); + + double chassisAccelX = (retSpeeds.vxMetersPerSecond - vars.prevSpeeds.vxMetersPerSecond) / dt; + double chassisAccelY = (retSpeeds.vyMetersPerSecond - vars.prevSpeeds.vyMetersPerSecond) / dt; + double angularAccel = + (retSpeeds.omegaRadiansPerSecond - vars.prevSpeeds.omegaRadiansPerSecond) / dt; + + SwerveModuleState[] retStates = kinematics.toSwerveModuleStates(retSpeeds); + SwerveModuleState[] accelStates = + kinematics.toSwerveModuleStates( + new ChassisSpeeds(chassisAccelX, chassisAccelY, angularAccel)); + + AdvancedSwerveModuleState[] outputStates = new AdvancedSwerveModuleState[NUM_MODULES]; + for (int m = 0; m < NUM_MODULES; m++) { + retStates[m].optimize(vars.prevModuleStates[m].angle); + double steerVelocity = + (vars.prevModuleStates[m].angle.getRadians() - retStates[m].angle.getRadians()) / dt; + outputStates[m] = + new AdvancedSwerveModuleState( + retStates[m].speedMetersPerSecond, + retStates[m].angle, + steerVelocity, + accelStates[m].speedMetersPerSecond); + } + + // Logger.recordOutput("finalVars", vars); + + return new SwerveSetpoint( // Logger.recordOutput("output", + retSpeeds, outputStates); + } + + public SwerveSetpoint generateSimpleSetpoint( + final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) { + AdvancedSwerveModuleState[] outputStates = new AdvancedSwerveModuleState[NUM_MODULES]; + SwerveModuleState[] desiredModuleStates = + kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleStates, maxDriveVelocityMPS); + for (int m = 0; m < NUM_MODULES; m++) { + desiredModuleStates[m].optimize(prevSetpoint.moduleStates()[m].angle); + outputStates[m] = AdvancedSwerveModuleState.fromBase(desiredModuleStates[m]); + } + + return new SwerveSetpoint(kinematics.toChassisSpeeds(desiredModuleStates), outputStates); + } + + private static void checkNeedToSteer(LocalVars vars) { + if (epsilonEquals(vars.desiredSpeeds, ZERO_SPEEDS)) { + vars.needToSteer = false; + for (int m = 0; m < NUM_MODULES; m++) { + vars.desiredModuleStates[m].angle = vars.prevModuleStates[m].angle; + vars.desiredModuleStates[m].speedMetersPerSecond = 0.0; + } + } + } + + private static double rotateBy(double rad, double otherCos, double otherSin) { + return Math.atan2( + Math.sin(rad) * otherCos + Math.cos(rad) * otherSin, + Math.cos(rad) * otherCos - Math.sin(rad) * otherSin); + } + + private static double requiredRotation(double prevRadians, double desiredRads) { + // this looks messy without using Rotation2d methods. + // this is roughly equivalent to: + // + // double r = vars.prev[m].rot2d().unaryMinus() + // .rotateBy(vars.desired[m].rot2d()).getRadians(); + double unaryMinusPrevRads = -prevRadians; + return rotateBy(unaryMinusPrevRads, Math.cos(desiredRads), Math.sin(desiredRads)); + } + + private static void makeVectors(LocalVars vars) { + for (int m = 0; m < NUM_MODULES; m++) { + vars.prev[m].applyModuleState(vars.prevModuleStates[m]); + vars.desired[m].applyModuleState(vars.desiredModuleStates[m]); + if (vars.allModulesShouldFlip) { + double requiredRots = requiredRotation(vars.prev[m].radians(), vars.desired[m].radians()); + if (flipHeading(requiredRots)) { + vars.allModulesShouldFlip = false; + } + } + } + } + + private void solveSteering(LocalVars vars) { + // In cases where an individual module is stopped, we want to remember the right steering angle + // to command (since inverse kinematics doesn't care about angle, we can be opportunistically + // lazy). + // Enforce steering velocity limits. We do this by taking the derivative of steering angle at + // the current angle, and then backing out the maximum interpolant between start and goal + // states. We remember the minimum across all modules, since that is the active constraint. + for (int m = 0; m < NUM_MODULES; m++) { + if (!vars.needToSteer) { + vars.steeringOverride[m] = vars.prevModuleStates[m].angle; + continue; + } + + double maxThetaStep = vars.dt * maxSteerVelocityRadsPerSec; + + if (epsilonEquals(vars.prevModuleStates[m].speedMetersPerSecond, 0.0)) { + // If module is stopped, we know that we will need to move straight to the final steering + // angle, so limit based purely on rotation in place. + if (epsilonEquals(vars.desiredModuleStates[m].speedMetersPerSecond, 0.0)) { + // Goal angle doesn't matter. Just leave module at its current angle. + vars.steeringOverride[m] = vars.prevModuleStates[m].angle; + continue; + } + + double requiredRots = requiredRotation(vars.prev[m].radians(), vars.desired[m].radians()); + if (flipHeading(requiredRots)) { + requiredRots = + rotateBy(requiredRots, Rotation2d.k180deg.getCos(), Rotation2d.k180deg.getSin()); + } + + // radians bounds to +/- Pi. + final double numStepsNeeded = Math.abs(requiredRots) / maxThetaStep; + + if (numStepsNeeded <= 1.0) { + // Steer directly to goal angle. + vars.steeringOverride[m] = vars.desiredModuleStates[m].angle; + } else { + // Adjust steering by max_theta_step. + // there really is no way to avoid this allocation. + Rotation2d adjusted = + vars.prevModuleStates[m].angle.rotateBy( + Rotation2d.fromRadians(Math.signum(requiredRots) * maxThetaStep)); + vars.steeringOverride[m] = adjusted; + vars.minS = 0.0; + } + continue; + } + if (vars.minS == 0.0) { + // s can't get any lower. Save some CPU. + continue; + } + + double maxS = + findSteeringMaxS( + vars.prev[m].vx, + vars.prev[m].vy, + vars.prev[m].radians(), + vars.desired[m].vx, + vars.desired[m].vy, + vars.desired[m].radians(), + maxThetaStep); + vars.minS = Math.min(vars.minS, maxS); + } + } + + private void solveDriving(LocalVars vars) { + // Enforce drive wheel torque limits + Translation2d chassisForceVec = Translation2d.kZero; + double chassisTorque = 0.0; + for (int m = 0; m < NUM_MODULES; m++) { + double lastVelRadPerSec = vars.prevModuleStates[m].speedMetersPerSecond / wheelRadiusMeters; + // Use the current battery voltage since we won't be able to supply 12v if the + // battery is sagging down to 11v, which will affect the max torque output + double currentDraw = + driveMotor.getCurrent(Math.abs(lastVelRadPerSec), RobotController.getInputVoltage()); + currentDraw = Math.min(currentDraw, driveCurrentLimitAmps); + double moduleTorque = driveMotor.getTorque(currentDraw); + + double prevSpeed = vars.prevModuleStates[m].speedMetersPerSecond; + vars.desiredModuleStates[m].optimize(vars.prevModuleStates[m].angle); + double desiredSpeed = vars.desiredModuleStates[m].speedMetersPerSecond; + + int forceSign; + Rotation2d forceAngle = vars.prevModuleStates[m].angle; + if (epsilonEquals(prevSpeed, 0.0) + || (prevSpeed > 0 && desiredSpeed >= prevSpeed) + || (prevSpeed < 0 && desiredSpeed <= prevSpeed)) { + // Torque loss will be fighting motor + moduleTorque -= torqueLoss; + forceSign = 1; // Force will be applied in direction of module + if (prevSpeed < 0) { + forceAngle = forceAngle.plus(Rotation2d.k180deg); + } + } else { + // Torque loss will be helping the motor + moduleTorque += torqueLoss; + forceSign = -1; // Force will be applied in opposite direction of module + if (prevSpeed > 0) { + forceAngle = forceAngle.plus(Rotation2d.k180deg); + } + } + + // Limit torque to prevent wheel slip + moduleTorque = Math.min(moduleTorque, wheelFrictionForce * wheelRadiusMeters); + + double forceAtCarpet = moduleTorque / wheelRadiusMeters; + Translation2d moduleForceVec = new Translation2d(forceAtCarpet * forceSign, forceAngle); + + // Add the module force vector to the chassis force vector + chassisForceVec = chassisForceVec.plus(moduleForceVec); + + // Calculate the torque this module will apply to the chassis + Rotation2d angleToModule = moduleLocations[m].getAngle(); + Rotation2d theta = moduleForceVec.getAngle().minus(angleToModule); + chassisTorque += forceAtCarpet * moduleLocations[m].getNorm() * theta.getSin(); + } + + Translation2d chassisAccelVec = chassisForceVec.div(massKg); + double chassisAngularAccel = chassisTorque / moiKgMetersSquared; + + // Use kinematics to convert chassis accelerations to module accelerations + ChassisSpeeds chassisAccel = + new ChassisSpeeds(chassisAccelVec.getX(), chassisAccelVec.getY(), chassisAngularAccel); + var accelStates = kinematics.toSwerveModuleStates(chassisAccel); + + for (int m = 0; m < NUM_MODULES; m++) { + if (vars.minS == 0.0) { + // No need to carry on. + break; + } + + double maxVelStep = Math.abs(accelStates[m].speedMetersPerSecond * vars.dt); + + double vxMinS; + double vyMinS; + if (vars.minS == 1.0) { + vxMinS = vars.desired[m].vx; + vyMinS = vars.desired[m].vy; + } else { + vxMinS = (vars.desired[m].vx - vars.prev[m].vx) * vars.minS + vars.prev[m].vx; + vyMinS = (vars.desired[m].vy - vars.prev[m].vy) * vars.minS + vars.prev[m].vy; + } + // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we + // already know we can't go faster than that. + double s = + findDriveMaxS( + vars.prev[m].vx, + vars.prev[m].vy, + Math.hypot(vars.prev[m].vx, vars.prev[m].vy), + vxMinS, + vyMinS, + Math.hypot(vxMinS, vyMinS), + maxVelStep); + vars.minS = Math.min(vars.minS, s); + } + } +} From 4973c6be587b866d992946002361f5669701577f Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Sat, 21 Dec 2024 05:58:49 -0500 Subject: [PATCH 66/75] update GradleRIO version to beta-3 and refactor voltage calculations in simulation classes --- build.gradle | 20 +++++++++---------- src/main/java/frc/robot/Robot.java | 6 ++++-- .../swerve/BrushlessMotorSim.java | 20 +++++++++++++++---- .../swerve/moduleIO/SimulatedModule.java | 4 ++-- 4 files changed, 32 insertions(+), 18 deletions(-) diff --git a/build.gradle b/build.gradle index cc290c39..2adb2a29 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2" + id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.25.0" id "pmd" @@ -151,15 +151,15 @@ tasks.withType(JavaCompile) { } // Create version file -// project.compileJava.dependsOn(createVersionFile) -// gversion { -// srcDir = "src/main/java/" -// classPackage = "frc.robot" -// className = "BuildConstants" -// dateFormat = "yyyy-MM-dd HH:mm:ss z" -// timeZone = "America/New_York" -// indent = " " -// } +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc.robot" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/New_York" + indent = " " +} // TODO: set this shit up // Create commit with working changes on event branches diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b8386af4..6086a926 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,5 +1,6 @@ package frc.robot; +import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.extras.simulation.field.SimulatedField; @@ -74,7 +75,7 @@ public Robot() { public void robotPeriodic() { // TODO: test this! // Switch thread to high priority to improve loop timing - // Threads.setCurrentThreadPriority(true, 99); + Threads.setCurrentThreadPriority(true, 99); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled commands, running already-scheduled commands, removing @@ -84,7 +85,7 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); // Return to normal thread priority - // Threads.setCurrentThreadPriority(false, 10); + Threads.setCurrentThreadPriority(false, 10); } /** This function is called once when the robot is disabled. */ @@ -120,6 +121,7 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_robotContainer.teleopInit(); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java index 54348897..76f1e64a 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java @@ -359,14 +359,20 @@ public void update() { Voltage voltage = Volts.of(poseVoltController.calculate(getPosition().in(Radians), output)); Voltage feedforwardVoltage = - feedforward.calculate(getVelocity(), velocityForVolts(voltage)); + Volts.of( + feedforward.calculate( + getVelocity().in(RadiansPerSecond), + velocityForVolts(voltage).in(RadiansPerSecond))); driveAtVoltage(feedforwardVoltage.plus(voltage)); } case VELOCITY -> { Voltage voltage = Volts.of(veloVoltController.calculate(getVelocity().in(RadiansPerSecond), output)); Voltage feedforwardVoltage = - feedforward.calculate(getVelocity(), RadiansPerSecond.of(output)); + Volts.of( + feedforward.calculate( + getVelocity().in(RadiansPerSecond), + RadiansPerSecond.of(output).in(RadiansPerSecond))); driveAtVoltage(voltage.plus(feedforwardVoltage)); } } @@ -381,14 +387,20 @@ public void update() { Amps.of(poseCurrentController.calculate(getPosition().in(Radians), output)); Voltage voltage = voltsForAmps(current, getVelocity()); Voltage feedforwardVoltage = - feedforward.calculate(getVelocity(), velocityForVolts(voltage)); + Volts.of( + feedforward.calculate( + getVelocity().in(RadiansPerSecond), + velocityForVolts(voltage).in(RadiansPerSecond))); driveAtVoltage(feedforwardVoltage.plus(voltage)); } case VELOCITY -> { Current current = Amps.of(veloCurrentController.calculate(getPosition().in(Radians), output)); Voltage feedforwardVoltage = - feedforward.calculate(getVelocity(), RadiansPerSecond.of(output)); + Volts.of( + feedforward.calculate( + getVelocity().in(RadiansPerSecond), + RadiansPerSecond.of(output).in(RadiansPerSecond))); Voltage voltage = voltsForAmps(current, getVelocity()).plus(feedforwardVoltage); driveAtVoltage(voltage); } diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index 312853b9..48c7a09f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -108,13 +108,13 @@ public void setDesiredState(SwerveModuleState desiredState) { RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) .in(RotationsPerSecond), desiredDriveRPS)) - .plus(driveFF.calculate(RotationsPerSecond.of(desiredDriveRPS)))); + .plus(Volts.of(driveFF.calculate((desiredDriveRPS))))); moduleSimulation.requestTurnVoltageOut( Volts.of( turnPID.calculate( moduleSimulation.getTurnAbsolutePosition().getRotations(), desiredState.angle.getRotations())) - .plus(turnFF.calculate(RotationsPerSecond.of(turnPID.getSetpoint().velocity)))); + .plus(Volts.of(turnFF.calculate(turnPID.getSetpoint().velocity)))); } @Override From f90e65a8e7b466202adf92be63d77369bd2d1b4f Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Tue, 24 Dec 2024 11:44:05 -0500 Subject: [PATCH 67/75] refactor gyro interface and simulation classes; move to new package structure --- build.gradle | 62 ++++---- src/main/java/frc/robot/RobotContainer.java | 14 +- .../swerve/SwerveModuleSimulation.java | 6 +- .../robot/subsystems/swerve/SwerveDrive.java | 150 ++++++++++++------ .../robot/subsystems/swerve/SwerveModule.java | 59 +++---- .../{gyroIO => gyro}/GyroInterface.java | 5 +- .../swerve/{gyroIO => gyro}/PhysicalGyro.java | 13 +- .../{gyroIO => gyro}/SimulatedGyro.java | 9 +- .../{moduleIO => module}/ModuleInterface.java | 2 +- .../{moduleIO => module}/PhysicalModule.java | 2 +- .../{moduleIO => module}/SimulatedModule.java | 60 ++----- .../swerve/setpointGen/SPGCalcs.java | 149 ++++++++--------- .../setpointGen/SwerveSetpointGenerator.java | 71 ++++++--- 13 files changed, 329 insertions(+), 273 deletions(-) rename src/main/java/frc/robot/subsystems/swerve/{gyroIO => gyro}/GyroInterface.java (86%) rename src/main/java/frc/robot/subsystems/swerve/{gyroIO => gyro}/PhysicalGyro.java (85%) rename src/main/java/frc/robot/subsystems/swerve/{gyroIO => gyro}/SimulatedGyro.java (78%) rename src/main/java/frc/robot/subsystems/swerve/{moduleIO => module}/ModuleInterface.java (96%) rename src/main/java/frc/robot/subsystems/swerve/{moduleIO => module}/PhysicalModule.java (99%) rename src/main/java/frc/robot/subsystems/swerve/{moduleIO => module}/SimulatedModule.java (63%) diff --git a/build.gradle b/build.gradle index 2adb2a29..20f2ef4d 100644 --- a/build.gradle +++ b/build.gradle @@ -163,37 +163,37 @@ gversion { // TODO: set this shit up // Create commit with working changes on event branches -// task(eventDeploy) { -// doLast { -// if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) { -// def branchPrefix = "event" -// def branch = 'git branch --show-current'.execute().text.trim() -// def commitMessage = "Update at '${new Date().toString()}'" - -// if (branch.startsWith(branchPrefix)) { -// exec { -// workingDir(projectDir) -// executable 'git' -// args 'add', '-A' -// } -// exec { -// workingDir(projectDir) -// executable 'git' -// args 'commit', '-m', commitMessage -// ignoreExitValue = true -// } - -// println "Committed to branch: '$branch'" -// println "Commit message: '$commitMessage'" -// } else { -// println "Not on an event branch, skipping commit" -// } -// } else { -// println "Not running deploy task, skipping commit" -// } -// } -// } -// createVersionFile.dependsOn(eventDeploy) +task(eventDeploy) { + doLast { + if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) { + def branchPrefix = "event" + def branch = 'git branch --show-current'.execute().text.trim() + def commitMessage = "Update at '${new Date().toString()}'" + + if (branch.startsWith(branchPrefix)) { + exec { + workingDir(projectDir) + executable 'git' + args 'add', '-A' + } + exec { + workingDir(projectDir) + executable 'git' + args 'commit', '-m', commitMessage + ignoreExitValue = true + } + + println "Committed to branch: '$branch'" + println "Commit message: '$commitMessage'" + } else { + println "Not on an event branch, skipping commit" + } + } else { + println "Not running deploy task, skipping commit" + } + } +} +createVersionFile.dependsOn(eventDeploy) // Spotless formatting project.compileJava.dependsOn(spotlessApply) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9c866910..65f87f71 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,12 +19,12 @@ import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; import frc.robot.subsystems.swerve.SwerveDrive; -import frc.robot.subsystems.swerve.gyroIO.GyroInterface; -import frc.robot.subsystems.swerve.gyroIO.PhysicalGyro; -import frc.robot.subsystems.swerve.gyroIO.SimulatedGyro; -import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; -import frc.robot.subsystems.swerve.moduleIO.PhysicalModule; -import frc.robot.subsystems.swerve.moduleIO.SimulatedModule; +import frc.robot.subsystems.swerve.gyro.GyroInterface; +import frc.robot.subsystems.swerve.gyro.PhysicalGyro; +import frc.robot.subsystems.swerve.gyro.SimulatedGyro; +import frc.robot.subsystems.swerve.module.ModuleInterface; +import frc.robot.subsystems.swerve.module.PhysicalModule; +import frc.robot.subsystems.swerve.module.SimulatedModule; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOReal; @@ -79,7 +79,7 @@ public RobotContainer() { /* create a swerve drive simulation */ this.swerveDriveSimulation = new SwerveDriveSimulation( - 45, + 60, DriveConstants.TRACK_WIDTH, DriveConstants.WHEEL_BASE, DriveConstants.TRACK_WIDTH + .2, diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java index 87037b71..5c2c25e8 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java @@ -295,8 +295,7 @@ public double getDriveEncoderUnGearedPositionRad() { * @return the final position of the drive encoder (wheel rotations), in radians */ public double getDriveEncoderFinalPositionRad() { - return getDriveEncoderUnGearedPositionRad(); - // / DRIVE_GEAR_RATIO; + return getDriveEncoderUnGearedPositionRad() / DRIVE_GEAR_RATIO; } /** @@ -323,8 +322,7 @@ public double getDriveEncoderUnGearedSpeedRadPerSec() { * @return the final speed of the drive wheel, in radians per second */ public double getDriveWheelFinalSpeedRadPerSec() { - return getDriveEncoderUnGearedSpeedRadPerSec(); - // / DRIVE_GEAR_RATIO; + return getDriveEncoderUnGearedSpeedRadPerSec() / DRIVE_GEAR_RATIO; } /** diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 47eefbce..4e2823c3 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -13,16 +13,15 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP; import frc.robot.extras.util.DeviceCANBus; import frc.robot.extras.util.TimeUtil; import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; -import frc.robot.subsystems.swerve.gyroIO.GyroInputsAutoLogged; -import frc.robot.subsystems.swerve.gyroIO.GyroInterface; -import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; +import frc.robot.subsystems.swerve.gyro.GyroInputsAutoLogged; +import frc.robot.subsystems.swerve.gyro.GyroInterface; +import frc.robot.subsystems.swerve.module.ModuleInterface; import frc.robot.subsystems.swerve.odometryThread.OdometryThread; import frc.robot.subsystems.swerve.odometryThread.OdometryThreadInputsAutoLogged; import frc.robot.subsystems.swerve.setpointGen.SwerveSetpoint; @@ -49,7 +48,7 @@ public class SwerveDrive extends SubsystemBase { DCMotor.getFalcon500(1).withReduction(11), 60, 58, - 11, + 7, ModuleConstants.WHEEL_DIAMETER_METERS, WHEEL_GRIP.TIRE_WHEEL.cof, 0.0); @@ -57,6 +56,8 @@ public class SwerveDrive extends SubsystemBase { private final OdometryThread odometryThread; + private Optional alliance; + private final Alert gyroDisconnectedAlert = new Alert("Gyro Hardware Fault", Alert.AlertType.kError); @@ -105,21 +106,12 @@ public SwerveDrive( gyroDisconnectedAlert.set(false); } - /** - * Gets the current velocity of the gyro's yaw - * - * @return the yaw velocity - */ - public double getGyroRate() { - return gyroInputs.yawVelocity; - } - /** Updates the pose estimator with the pose calculated from the swerve modules. */ - public void addPoseEstimatorSwerveMeasurement() { - for (int timestampIndex = 0; - timestampIndex < odometryThreadInputs.measurementTimeStamps.length; - timestampIndex++) addPoseEstimatorSwerveMeasurement(timestampIndex); - } + // public void addPoseEstimatorSwerveMeasurement() { + // for (int timestampIndex = 0; + // timestampIndex < odometryThreadInputs.measurementTimeStamps.length; + // timestampIndex++) addPoseEstimatorSwerveMeasurement(timestampIndex); + // } /* * Updates the pose estimator with the pose calculated from the april tags. How much it @@ -207,22 +199,79 @@ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fi ChassisSpeeds desiredSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rotationSpeed, getPose().getRotation()) + xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d()) : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); - setpoint = setpointGenerator.generateSetpoint(setpoint, desiredSpeeds, 0.02); + setpoint = setpointGenerator.generateSimpleSetpoint(setpoint, desiredSpeeds, 0.02); + setModuleStates(setpoint.moduleStates()); Logger.recordOutput("SwerveStates/DesiredStates", setpoint.moduleStates()); } + /** + * Returns the heading of the robot in degrees from 0 to 360. + * + * @return Value is Counter-clockwise positive. + */ + public double getHeading() { + return -gyroInputs.yawDegrees; + } + + /** + * Gets the rate of rotation of the NavX. + * + * @return The current rate in degrees per second. + */ + public double getGyroRate() { + return gyroInputs.yawVelocity; + } + + /** Returns a Rotation2d for the heading of the robot. */ + public Rotation2d getGyroRotation2d() { + return Rotation2d.fromDegrees(getHeading()); + } + + /** Returns a Rotation2d for the heading of the robot. */ + public Rotation2d getGyroFieldRelativeRotation2d() { + return Rotation2d.fromDegrees(getHeading() + getAllianceAngleOffset()); + } + /** Returns 0 degrees if the robot is on the blue alliance, 180 if on the red alliance. */ public double getAllianceAngleOffset() { - Optional alliance = DriverStation.getAlliance(); + alliance = DriverStation.getAlliance(); double offset = alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red ? 180.0 : 0.0; return offset; } + /** Zeroes the heading of the robot. */ + public void zeroHeading() { + gyroIO.reset(); + } + + /** + * Returns the estimated field-relative pose of the robot. Positive x being forward, positive y + * being left. + */ + @AutoLogOutput(key = "Odometry/Odometry") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** Returns a Rotation2d for the heading of the robot */ + public Rotation2d getOdometryRotation2d() { + return getPose().getRotation(); + } + + /** + * Returns a Rotation2d for the heading of the robot relative to the field from the driver's + * perspective. This method is needed so that the drive command and poseEstimator don't fight each + * other. It uses odometry rotation. + */ + public Rotation2d getOdometryAllianceRelativeRotation2d() { + return getPose().getRotation().plus(Rotation2d.fromDegrees(getAllianceAngleOffset())); + } + /** * Sets the modules to the specified states. * @@ -231,7 +280,7 @@ public double getAllianceAngleOffset() { */ public void setModuleStates(SwerveModuleState[] desiredStates) { for (int i = 0; i < 4; i++) { - swerveModules[i].runSetPoint(desiredStates[i]); + swerveModules[i].runSetPoint((desiredStates[i])); } } @@ -240,21 +289,38 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { * * @param timestampIndex index of the timestamp to sample the pose at */ - private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { - final SwerveModulePosition[] modulePositions = getModulesPosition(timestampIndex), + // private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { + // final SwerveModulePosition[] modulePositions = getModulePositions(), + // moduleDeltas = getModulesDelta(modulePositions); + + // if (gyroInputs.isConnected) { + // rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; + // } else { + // Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas); + // rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + // } + + // poseEstimator.updateWithTime( + // odometryThreadInputs.measurementTimeStamps[timestampIndex], + // rawGyroRotation, + // modulePositions); + // } + + public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex + final SwerveModulePosition[] modulePositions = getModulePositions(), moduleDeltas = getModulesDelta(modulePositions); if (gyroInputs.isConnected) { - rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; + // rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; + rawGyroRotation = getGyroRotation2d(); } else { Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas); rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } poseEstimator.updateWithTime( - odometryThreadInputs.measurementTimeStamps[timestampIndex], - rawGyroRotation, - modulePositions); + // odometryThreadInputs.measurementTimeStamps[timestampIndex], + Logger.getTimestamp(), rawGyroRotation, modulePositions); } /** @@ -264,13 +330,14 @@ private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { * @return a list of SwerveModulePosition, containing relative drive position and absolute turn * rotation at the sampled timestamp. */ - private SwerveModulePosition[] getModulesPosition(int timestampIndex) { - SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[swerveModules.length]; - for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) - swerveModulePositions[moduleIndex] = - swerveModules[moduleIndex].getOdometryPositions()[timestampIndex]; - return swerveModulePositions; - } + // private SwerveModulePosition[] getModulesPosition(int timestampIndex) { + // SwerveModulePosition[] swerveModulePositions = new + // SwerveModulePosition[swerveModules.length]; + // for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) + // swerveModulePositions[moduleIndex] = + // swerveModules[moduleIndex].getOdometryPositions()[timestampIndex]; + // return swerveModulePositions; + // } private SwerveModulePosition[] getModulesDelta(SwerveModulePosition[] freshModulesPosition) { SwerveModulePosition[] deltas = new SwerveModulePosition[swerveModules.length]; @@ -300,17 +367,6 @@ private SwerveModulePosition[] getModulePositions() { return positions; } - /** Gets the fused pose from the pose estimator. */ - @AutoLogOutput(key = "Odometry/RobotPosition") - public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); - } - - /** Gets the current gyro yaw */ - public Rotation2d getRawGyroYaw() { - return gyroInputs.yawDegreesRotation2d; - } - /** * Sets the pose * diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index cbb21264..8dbc1ec4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -7,15 +7,11 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.w; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.SimulationConstants; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; -import frc.robot.subsystems.swerve.moduleIO.ModuleInputsAutoLogged; -import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; +import frc.robot.subsystems.swerve.module.ModuleInputsAutoLogged; +import frc.robot.subsystems.swerve.module.ModuleInterface; import org.littletonrobotics.junction.Logger; public class SwerveModule extends SubsystemBase { @@ -23,8 +19,6 @@ public class SwerveModule extends SubsystemBase { private final String name; private final ModuleInputsAutoLogged inputs = new ModuleInputsAutoLogged(); - private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; - private final Alert hardwareFaultAlert; public SwerveModule(ModuleInterface io, String name) { @@ -46,25 +40,25 @@ public void updateOdometryInputs() { @Override public void periodic() { - updateOdometryPositions(); + // updateOdometryPositions(); } - private void updateOdometryPositions() { - odometryPositions = new SwerveModulePosition[inputs.odometryDriveWheelRevolutions.length]; - for (int i = 0; i < odometryPositions.length; i++) { - double positionMeters = inputs.odometryDriveWheelRevolutions[i]; - Rotation2d angle = inputs.odometrySteerPositions[i]; - odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + // private void updateOdometryPositions() { + // odometryPositions = new SwerveModulePosition[inputs.odometryDriveWheelRevolutions.length]; + // for (int i = 0; i < odometryPositions.length; i++) { + // double positionMeters = inputs.odometryDriveWheelRevolutions[i]; + // Rotation2d angle = inputs.odometrySteerPositions[i]; + // odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); - SmartDashboard.putNumber("updated drive position", positionMeters); - SmartDashboard.putNumber("updated angle", angle.getDegrees()); - } - } + // SmartDashboard.putNumber("updated drive position", positionMeters); + // SmartDashboard.putNumber("updated angle", angle.getDegrees()); + // } + // } - private double driveRevolutionsToMeters(double driveWheelRevolutions) { - return Rotations.of(driveWheelRevolutions).in(Radians) - * SimulationConstants.WHEEL_RADIUS_METERS; - } + // private double driveRevolutionsToMeters(double driveWheelRevolutions) { + // return Rotations.of(driveWheelRevolutions).in(Radians) + // * SimulationConstants.WHEEL_RADIUS_METERS; + // } public void setVoltage(Voltage volts) { io.setDriveVoltage(volts); @@ -79,8 +73,12 @@ public double getCharacterizationVelocity() { return inputs.driveVelocity; } - /** Runs the module with the specified setpoint state. Returns the optimized state. */ + /** Runs the module with the specified setpoint state. Returns optimized setpoint */ public void runSetPoint(SwerveModuleState state) { + // state.optimize(getTurnRotation()); + // if (state.speedMetersPerSecond < 0.01) { + // io.stopModule(); + // } io.setDesiredState(state); } @@ -95,12 +93,12 @@ public double getTurnVelocity() { /** Returns the current drive position of the module in meters. */ public double getDrivePositionMeters() { - return ModuleConstants.DRIVE_TO_METERS * inputs.drivePosition; + return ModuleConstants.WHEEL_CIRCUMFERENCE_METERS * inputs.drivePosition; } /** Returns the current drive velocity of the module in meters per second. */ public double getDriveVelocityMetersPerSec() { - return ModuleConstants.DRIVE_TO_METERS_PER_SECOND * inputs.driveVelocity; + return ModuleConstants.WHEEL_CIRCUMFERENCE_METERS * inputs.driveVelocity; } /** Returns the module state (turn angle and drive velocity). */ @@ -108,11 +106,6 @@ public SwerveModuleState getMeasuredState() { return new SwerveModuleState(getDriveVelocityMetersPerSec(), getTurnRotation()); } - /** Returns the module positions received this cycle. */ - public SwerveModulePosition[] getOdometryPositions() { - return odometryPositions; - } - /** * Gets the module position consisting of the distance it has traveled and the angle it is * rotated. @@ -120,8 +113,6 @@ public SwerveModulePosition[] getOdometryPositions() { * @return a SwerveModulePosition object containing position and rotation */ public SwerveModulePosition getPosition() { - double position = ModuleConstants.DRIVE_TO_METERS * getDrivePositionMeters(); - Rotation2d rotation = getTurnRotation(); - return new SwerveModulePosition(position, rotation); + return new SwerveModulePosition(getDrivePositionMeters(), getTurnRotation()); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java similarity index 86% rename from src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java rename to src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java index b2fa5669..2d45260e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.swerve.gyroIO; +package frc.robot.subsystems.swerve.gyro; import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; @@ -22,4 +22,7 @@ public static class GyroInputs { * @param inputs inputs to update */ default void updateInputs(GyroInputs inputs) {} + + /** Resets the gyro yaw */ + default void reset() {} } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java similarity index 85% rename from src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java rename to src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java index 79c44d11..0dfc028a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.swerve.gyroIO; +package frc.robot.subsystems.swerve.gyro; import static edu.wpi.first.units.Units.*; @@ -15,15 +15,15 @@ public class PhysicalGyro implements GyroInterface { private final Queue yawPositionInput; public PhysicalGyro() { - yawPositionInput = OdometryThread.registerInput(() -> Degrees.of(-gyro.getAngle())); + yawPositionInput = OdometryThread.registerInput(() -> Degrees.of(gyro.getAngle())); } @Override public void updateInputs(GyroInputs inputs) { inputs.isConnected = gyro.isConnected(); inputs.yawDegreesRotation2d = gyro.getRotation2d(); - inputs.yawVelocity = -gyro.getRate(); - inputs.yawDegrees = -gyro.getAngle(); + inputs.yawVelocity = gyro.getRate(); + inputs.yawDegrees = gyro.getAngle(); // Handle odometry yaw positions if (!yawPositionInput.isEmpty()) { @@ -36,4 +36,9 @@ public void updateInputs(GyroInputs inputs) { yawPositionInput.clear(); } } + + @Override + public void reset() { + gyro.reset(); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java similarity index 78% rename from src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java rename to src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java index cea6b996..4197e452 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java @@ -1,8 +1,9 @@ -package frc.robot.subsystems.swerve.gyroIO; +package frc.robot.subsystems.swerve.gyro; import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; +import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.extras.simulation.OdometryTimestampsSim; import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation; @@ -19,8 +20,14 @@ public void updateInputs(GyroInputs inputs) { inputs.odometryYawPositions = gyroSimulation.getCachedGyroReadings(); inputs.odometryYawTimestamps = OdometryTimestampsSim.getTimestamps(); inputs.yawDegreesRotation2d = gyroSimulation.getGyroReading(); + inputs.yawDegrees = gyroSimulation.getGyroReading().getDegrees(); inputs.yawVelocity = RadiansPerSecond.of(gyroSimulation.getMeasuredAngularVelocityRadPerSec()) .in(DegreesPerSecond); } + + @Override + public void reset() { + gyroSimulation.setRotation(Rotation2d.kZero); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java similarity index 96% rename from src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java rename to src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java index 54d60aef..7fdbbcd4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.swerve.moduleIO; +package frc.robot.subsystems.swerve.module; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java similarity index 99% rename from src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java rename to src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java index 41dcdcef..2d052fe2 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.swerve.moduleIO; +package frc.robot.subsystems.swerve.module; import static edu.wpi.first.units.Units.*; diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java similarity index 63% rename from src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java rename to src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java index 48c7a09f..2105c497 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java @@ -1,72 +1,58 @@ -package frc.robot.subsystems.swerve.moduleIO; +package frc.robot.subsystems.swerve.module; import static edu.wpi.first.units.Units.*; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.extras.simulation.OdometryTimestampsSim; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; -import java.util.Arrays; /** Wrapper class around {@link SwerveModuleSimulation} */ public class SimulatedModule implements ModuleInterface { private final SwerveModuleSimulation moduleSimulation; - private final PIDController drivePID = new PIDController(.07, 0, 0); - private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.0, .0, 0.0); + private final PIDController drivePID = new PIDController(.27, 0, 0); + private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(1, 1.5); private final Constraints turnConstraints = new Constraints( - RadiansPerSecond.of(2 * Math.PI).in(RotationsPerSecond), - RadiansPerSecondPerSecond.of(4 * Math.PI).in(RotationsPerSecondPerSecond)); + ModuleConstants.MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND, + ModuleConstants.MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED); private final ProfiledPIDController turnPID = - new ProfiledPIDController(Radians.of(3.5).in(Rotations), 0, 0, turnConstraints); - private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(0.77, 0.75, 0); + new ProfiledPIDController(3.5, 0, 0, turnConstraints); + private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(4, 10, 0); public SimulatedModule(SwerveModuleSimulation moduleSimulation) { this.moduleSimulation = moduleSimulation; - turnPID.enableContinuousInput(-Math.PI, Math.PI); + // turnPID.enableContinuousInput(-Math.PI, Math.PI); } @Override public void updateInputs(ModuleInputs inputs) { inputs.drivePosition = - Units.radiansToRotations(moduleSimulation.getDriveEncoderFinalPositionRad()); + Radians.of(moduleSimulation.getDriveEncoderFinalPositionRad()).in(Rotations); inputs.driveVelocity = - Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()); + RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) + .in(RotationsPerSecond); inputs.driveAppliedVolts = moduleSimulation.getDriveMotorAppliedVolts(); inputs.driveCurrentAmps = moduleSimulation.getDriveMotorSupplyCurrentAmps(); inputs.turnAbsolutePosition = moduleSimulation.getTurnAbsolutePosition(); - inputs.turnVelocity = moduleSimulation.getTurnAbsoluteEncoderSpeedRadPerSec(); + inputs.turnVelocity = + Radians.of(moduleSimulation.getTurnAbsoluteEncoderSpeedRadPerSec()).in(Rotations); inputs.turnAppliedVolts = moduleSimulation.getTurnMotorAppliedVolts(); inputs.turnCurrentAmps = moduleSimulation.getTurnMotorSupplyCurrentAmps(); - inputs.odometryDriveWheelRevolutions = - Arrays.stream(moduleSimulation.getCachedDriveWheelFinalPositionsRad()) - .map(Units::radiansToRotations) - .toArray(); - inputs.odometrySteerPositions = moduleSimulation.getCachedTurnAbsolutePositions(); inputs.odometryTimestamps = OdometryTimestampsSim.getTimestamps(); inputs.isConnected = true; - - SmartDashboard.putNumber( - "turn absolute position", moduleSimulation.getTurnAbsolutePosition().getDegrees()); - SmartDashboard.putNumber( - "drive pos", - Radians.of(moduleSimulation.getDriveEncoderFinalPositionRad()).in(Rotations) - * ModuleConstants.DRIVE_TO_METERS); } @Override @@ -81,34 +67,20 @@ public void setTurnVoltage(Voltage volts) { @Override public void setDesiredState(SwerveModuleState desiredState) { - double turnRotations = getTurnRotations(); - // setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); - desiredState.optimize(Rotation2d.fromRotations(turnRotations)); - - if (Math.abs(desiredState.speedMetersPerSecond) < 0.01) { - stopModule(); - return; - } - // Converts meters per second to rotations per second double desiredDriveRPS = desiredState.speedMetersPerSecond * ModuleConstants.DRIVE_GEAR_RATIO / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; - SmartDashboard.putNumber("desired drive RPS", desiredDriveRPS); - SmartDashboard.putNumber( - "current drive RPS", - RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) - .in(RotationsPerSecond)); - SmartDashboard.putNumber("desired angle", desiredState.angle.getDegrees()); moduleSimulation.requestDriveVoltageOut( Volts.of( drivePID.calculate( RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) - .in(RotationsPerSecond), + .in(RotationsPerSecond) + * ModuleConstants.WHEEL_CIRCUMFERENCE_METERS, desiredDriveRPS)) - .plus(Volts.of(driveFF.calculate((desiredDriveRPS))))); + .plus(Volts.of(driveFF.calculate(desiredDriveRPS)))); moduleSimulation.requestTurnVoltageOut( Volts.of( turnPID.calculate( diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SPGCalcs.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SPGCalcs.java index 4c6d37d1..23cf036e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SPGCalcs.java +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SPGCalcs.java @@ -13,8 +13,6 @@ class SPGCalcs { private static final double kEpsilon = 1E-8; - private static final int MAX_STEER_ITERATIONS = 8; - private static final int MAX_DRIVE_ITERATIONS = 10; static final int NUM_MODULES = 4; static double unwrapAngle(double ref, double angle) { @@ -28,59 +26,6 @@ static double unwrapAngle(double ref, double angle) { } } - @FunctionalInterface - interface Function2d { - double f(double x, double y); - } - - /** - * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. - * This is a pretty naive way to do root finding, but it's usually faster than simple bisection - * while being robust in ways that e.g. the Newton-Raphson method isn't. - * - * @param func The Function2d to take the root of. - * @param x_0 x value of the lower bracket. - * @param y_0 y value of the lower bracket. - * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during - * recursion) - * @param x_1 x value of the upper bracket. - * @param y_1 y value of the upper bracket. - * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during - * recursion) - * @param iterations_left Number of iterations of root finding left. - * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the - * (approximate) root. - */ - static double findRoot( - Function2d func, - double x_0, - double y_0, - double f_0, - double x_1, - double y_1, - double f_1, - int iterations_left) { - var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); - - if (iterations_left < 0 || epsilonEquals(f_0, f_1)) { - return s_guess; - } - - var x_guess = (x_1 - x_0) * s_guess + x_0; - var y_guess = (y_1 - y_0) * s_guess + y_0; - var f_guess = func.f(x_guess, y_guess); - if (Math.signum(f_0) == Math.signum(f_guess)) { - // 0 and guess on same side of root, so use upper bracket. - return s_guess - + (1.0 - s_guess) - * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1); - } else { - // Use lower bracket. - return s_guess - * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); - } - } - static double findSteeringMaxS( double prevVx, double prevVy, @@ -95,29 +40,89 @@ static double findSteeringMaxS( // Can go all the way to s=1. return 1.0; } - double offset = prevHeading + Math.signum(diff) * maxDeviation; - Function2d func = (x, y) -> unwrapAngle(prevHeading, Math.atan2(y, x)) - offset; - return findRoot( - func, - prevVx, - prevVy, - prevHeading - offset, - desiredVx, - desiredVy, - desiredHeading - offset, - MAX_STEER_ITERATIONS); + + double target = prevHeading + Math.copySign(maxDeviation, diff); + + // Rotate the velocity vectors such that the target angle becomes the +X + // axis. We only need find the Y components, h_0 and h_1, since they are + // proportional to the distances from the two points to the solution + // point (x_0 + (x_1 - x_0)s, y_0 + (y_1 - y_0)s). + double sin = Math.sin(-target); + double cos = Math.cos(-target); + double h_0 = sin * prevVx + cos * prevVy; + double h_1 = sin * desiredVx + cos * desiredVy; + + // Undo linear interpolation from h_0 to h_1: + // 0 = h_0 + (h_1 - h_0) * s + // -h_0 = (h_1 - h_0) * s + // -h_0 / (h_1 - h_0) = s + // h_0 / (h_0 - h_1) = s + // Guaranteed to not divide by zero, since if h_0 was equal to h_1, theta_0 + // would be equal to theta_1, which is caught by the difference check. + return h_0 / (h_0 - h_1); + } + + private static boolean isValidS(double s) { + return Double.isFinite(s) && s >= 0 && s <= 1; } - static double findDriveMaxS( - double x_0, double y_0, double f_0, double x_1, double y_1, double f_1, double max_vel_step) { - double diff = f_1 - f_0; + static double findDriveMaxS(double x_0, double y_0, double x_1, double y_1, double max_vel_step) { + // Derivation: + // Want to find point P(s) between (x_0, y_0) and (x_1, y_1) where the + // length of P(s) is the target T. P(s) is linearly interpolated between the + // points, so P(s) = (x_0 + (x_1 - x_0) * s, y_0 + (y_1 - y_0) * s). + // Then, + // T = sqrt(P(s).x^2 + P(s).y^2) + // T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2 + // T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2 + // + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2 + // T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2 + // + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2 + // 0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2 + // + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s + // + (x_0^2 + y_0^2 - T^2). + // + // To simplify, we can factor out some common parts: + // Let l_0 = x_0^2 + y_0^2, l_1 = x_1^2 + y_1^2, and + // p = x_0 * x_1 + y_0 * y_1. + // Then we have + // 0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2), + // with which we can solve for s using the quadratic formula. + + double l_0 = x_0 * x_0 + y_0 * y_0; + double l_1 = x_1 * x_1 + y_1 * y_1; + double sqrt_l_0 = Math.sqrt(l_0); + double diff = Math.sqrt(l_1) - sqrt_l_0; if (Math.abs(diff) <= max_vel_step) { // Can go all the way to s=1. return 1.0; } - double offset = f_0 + Math.signum(diff) * max_vel_step; - Function2d func = (x, y) -> Math.hypot(x, y) - offset; - return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, MAX_DRIVE_ITERATIONS); + + double target = sqrt_l_0 + Math.copySign(max_vel_step, diff); + double p = x_0 * x_1 + y_0 * y_1; + + // Quadratic of s + double a = l_0 + l_1 - 2 * p; + double b = 2 * (p - l_0); + double c = l_0 - target * target; + double root = Math.sqrt(b * b - 4 * a * c); + + // Check if either of the solutions are valid + // Won't divide by zero because it is only possible for a to be zero if the + // target velocity is exactly the same or the reverse of the current + // velocity, which would be caught by the difference check. + double s_1 = (-b + root) / (2 * a); + if (isValidS(s_1)) { + return s_1; + } + double s_2 = (-b - root) / (2 * a); + if (isValidS(s_2)) { + return s_2; + } + + // Since we passed the initial max_vel_step check, a solution should exist, + // but if no solution was found anyway, just don't limit movement + return 1.0; } static boolean epsilonEquals(double a, double b, double epsilon) { diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java index f92dc608..008d2df4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java @@ -9,6 +9,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.RobotController; +import frc.robot.subsystems.swerve.setpointGen.SPGCalcs.LocalVars; +import org.littletonrobotics.junction.Logger; /** * Swerve setpoint generatoR that has been passed around so many times its hard to keep track, just @@ -22,6 +24,7 @@ public class SwerveSetpointGenerator { private static final ChassisSpeeds ZERO_SPEEDS = new ChassisSpeeds(); private static final int NUM_MODULES = SPGCalcs.NUM_MODULES; + // private final double brownoutVoltage; private final SwerveDriveKinematics kinematics; private final Translation2d[] moduleLocations; @@ -60,6 +63,7 @@ public SwerveSetpointGenerator( this.moiKgMetersSquared = moiKgMetersSquared; this.wheelRadiusMeters = wheelDiameterMeters / 2; this.maxDriveVelocityMPS = driveMotor.freeSpeedRadPerSec * wheelRadiusMeters; + // this.brownoutVoltage = RobotController.getBrownoutVoltage(); wheelFrictionForce = wheelCoF * ((massKg / 4) * 9.81); // maxTorqueFriction = this.wheelFrictionForce * wheelRadiusMeters; @@ -85,6 +89,13 @@ public SwerveSetpointGenerator( */ public SwerveSetpoint generateSetpoint( final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) { + // Add logging for inputs + // if (Double.isNaN(inputVoltage)) { + // inputVoltage = 12.0; + // } else { + // inputVoltage = Math.max(inputVoltage, brownoutVoltage); + // } + // https://github.com/wpilibsuite/allwpilib/issues/7332 SwerveModuleState[] desiredModuleStates = kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds); @@ -109,27 +120,30 @@ public SwerveSetpoint generateSetpoint( - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; vars.minS = 1.0; - // Logger.recordOutput("beginningVars", vars); + Logger.recordOutput("beginningVars", vars); checkNeedToSteer(vars); - // Logger.recordOutput("postCheckNeedToSteer", vars); + + Logger.recordOutput("postCheckNeedToSteer", vars); + makeVectors(vars); - // Logger.recordOutput("pastMakeVectors", vars); - - // if (vars.allModulesShouldFlip - // && !epsilonEquals(prevSetpoint.chassisSpeeds(), ZERO_SPEEDS) - // && !epsilonEquals(desiredRobotRelativeSpeeds, ZERO_SPEEDS)) { - // // It will (likely) be faster to stop the robot, rotate the modules in place to the - // complement - // // of the desired angle, and accelerate again. - // return generateSetpoint(prevSetpoint, ZERO_SPEEDS, dt); - // } + + Logger.recordOutput("pastMakeVectors", vars); + + if (vars.allModulesShouldFlip + && !epsilonEquals(prevSetpoint.chassisSpeeds(), ZERO_SPEEDS) + && !epsilonEquals(desiredRobotRelativeSpeeds, ZERO_SPEEDS)) { + // It will (likely) be faster to stop the robot, rotate the modules in place to the + // complement + // of the desired angle, and accelerate again. + return generateSetpoint(prevSetpoint, ZERO_SPEEDS, dt); + } solveSteering(vars); - // Logger.recordOutput("postSolveSteering", vars); + Logger.recordOutput("postSolveSteering", vars); solveDriving(vars); - // Logger.recordOutput("postSolveDriving", vars); + Logger.recordOutput("postSolveDriving", vars); ChassisSpeeds retSpeeds = new ChassisSpeeds( @@ -161,7 +175,7 @@ public SwerveSetpoint generateSetpoint( accelStates[m].speedMetersPerSecond); } - // Logger.recordOutput("finalVars", vars); + Logger.recordOutput("finalVars", vars); return new SwerveSetpoint( // Logger.recordOutput("output", retSpeeds, outputStates); @@ -169,6 +183,7 @@ public SwerveSetpoint generateSetpoint( public SwerveSetpoint generateSimpleSetpoint( final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) { + AdvancedSwerveModuleState[] outputStates = new AdvancedSwerveModuleState[NUM_MODULES]; SwerveModuleState[] desiredModuleStates = kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds); @@ -177,7 +192,6 @@ public SwerveSetpoint generateSimpleSetpoint( desiredModuleStates[m].optimize(prevSetpoint.moduleStates()[m].angle); outputStates[m] = AdvancedSwerveModuleState.fromBase(desiredModuleStates[m]); } - return new SwerveSetpoint(kinematics.toChassisSpeeds(desiredModuleStates), outputStates); } @@ -295,8 +309,16 @@ private void solveDriving(LocalVars vars) { // battery is sagging down to 11v, which will affect the max torque output double currentDraw = driveMotor.getCurrent(Math.abs(lastVelRadPerSec), RobotController.getInputVoltage()); + double reverseCurrentDraw = + Math.abs( + driveMotor.getCurrent( + Math.abs(lastVelRadPerSec), -RobotController.getInputVoltage())); currentDraw = Math.min(currentDraw, driveCurrentLimitAmps); - double moduleTorque = driveMotor.getTorque(currentDraw); + + reverseCurrentDraw = Math.min(reverseCurrentDraw, driveCurrentLimitAmps); + double forwardModuleTorque = driveMotor.getTorque(currentDraw); + double reverseModuleTorque = driveMotor.getTorque(reverseCurrentDraw); + // double moduleTorque = driveMotor.getTorque(currentDraw); double prevSpeed = vars.prevModuleStates[m].speedMetersPerSecond; vars.desiredModuleStates[m].optimize(vars.prevModuleStates[m].angle); @@ -304,9 +326,13 @@ private void solveDriving(LocalVars vars) { int forceSign; Rotation2d forceAngle = vars.prevModuleStates[m].angle; + double moduleTorque; + if (epsilonEquals(prevSpeed, 0.0) || (prevSpeed > 0 && desiredSpeed >= prevSpeed) || (prevSpeed < 0 && desiredSpeed <= prevSpeed)) { + moduleTorque = forwardModuleTorque; + // Torque loss will be fighting motor moduleTorque -= torqueLoss; forceSign = 1; // Force will be applied in direction of module @@ -314,6 +340,7 @@ private void solveDriving(LocalVars vars) { forceAngle = forceAngle.plus(Rotation2d.k180deg); } } else { + moduleTorque = reverseModuleTorque; // Torque loss will be helping the motor moduleTorque += torqueLoss; forceSign = -1; // Force will be applied in opposite direction of module @@ -364,15 +391,7 @@ private void solveDriving(LocalVars vars) { } // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we // already know we can't go faster than that. - double s = - findDriveMaxS( - vars.prev[m].vx, - vars.prev[m].vy, - Math.hypot(vars.prev[m].vx, vars.prev[m].vy), - vxMinS, - vyMinS, - Math.hypot(vxMinS, vyMinS), - maxVelStep); + double s = findDriveMaxS(vars.prev[m].vx, vars.prev[m].vy, vxMinS, vyMinS, maxVelStep); vars.minS = Math.min(vars.minS, s); } } From e04dec9ce159fda0feafd3f5d2a55e104b42d396 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Tue, 24 Dec 2024 20:15:57 -0500 Subject: [PATCH 68/75] refactor SwerveDrive and SwerveModule; adjust motor configurations and clean up commented code --- .../robot/subsystems/swerve/SwerveDrive.java | 4 +-- .../robot/subsystems/swerve/SwerveModule.java | 25 +++---------------- .../swerve/module/SimulatedModule.java | 6 ++--- 3 files changed, 9 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 4e2823c3..c988e709 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -45,7 +45,7 @@ public class SwerveDrive extends SubsystemBase { new SwerveSetpointGenerator( DriveConstants.MODULE_TRANSLATIONS, DCMotor.getKrakenX60(1).withReduction(ModuleConstants.DRIVE_GEAR_RATIO), - DCMotor.getFalcon500(1).withReduction(11), + DCMotor.getFalcon500(1).withReduction(1), 60, 58, 7, @@ -214,7 +214,7 @@ xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d()) * @return Value is Counter-clockwise positive. */ public double getHeading() { - return -gyroInputs.yawDegrees; + return gyroInputs.yawDegrees; } /** diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 8dbc1ec4..b3ab1eb8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -43,23 +43,6 @@ public void periodic() { // updateOdometryPositions(); } - // private void updateOdometryPositions() { - // odometryPositions = new SwerveModulePosition[inputs.odometryDriveWheelRevolutions.length]; - // for (int i = 0; i < odometryPositions.length; i++) { - // double positionMeters = inputs.odometryDriveWheelRevolutions[i]; - // Rotation2d angle = inputs.odometrySteerPositions[i]; - // odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); - - // SmartDashboard.putNumber("updated drive position", positionMeters); - // SmartDashboard.putNumber("updated angle", angle.getDegrees()); - // } - // } - - // private double driveRevolutionsToMeters(double driveWheelRevolutions) { - // return Rotations.of(driveWheelRevolutions).in(Radians) - // * SimulationConstants.WHEEL_RADIUS_METERS; - // } - public void setVoltage(Voltage volts) { io.setDriveVoltage(volts); io.setTurnVoltage(Volts.zero()); @@ -75,10 +58,10 @@ public double getCharacterizationVelocity() { /** Runs the module with the specified setpoint state. Returns optimized setpoint */ public void runSetPoint(SwerveModuleState state) { - // state.optimize(getTurnRotation()); - // if (state.speedMetersPerSecond < 0.01) { - // io.stopModule(); - // } + state.optimize(getTurnRotation()); + if (state.speedMetersPerSecond < 0.01) { + io.stopModule(); + } io.setDesiredState(state); } diff --git a/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java index 2105c497..10a8ec57 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java @@ -24,12 +24,12 @@ public class SimulatedModule implements ModuleInterface { ModuleConstants.MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND, ModuleConstants.MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED); private final ProfiledPIDController turnPID = - new ProfiledPIDController(3.5, 0, 0, turnConstraints); - private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(4, 10, 0); + new ProfiledPIDController(5, 0, 0, turnConstraints); + private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(6, 8, 0); public SimulatedModule(SwerveModuleSimulation moduleSimulation) { this.moduleSimulation = moduleSimulation; - // turnPID.enableContinuousInput(-Math.PI, Math.PI); + turnPID.enableContinuousInput(-Math.PI, Math.PI); } @Override From 94c7ff770e0089d41c5d63ecc8982ebcffdc089d Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 2 Jan 2025 15:57:46 -0500 Subject: [PATCH 69/75] tuned sim pid --- src/main/java/frc/robot/subsystems/swerve/SwerveModule.java | 2 ++ .../frc/robot/subsystems/swerve/module/SimulatedModule.java | 5 ++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index b3ab1eb8..033af949 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -41,6 +41,7 @@ public void updateOdometryInputs() { @Override public void periodic() { // updateOdometryPositions(); + Logger.recordOutput("Drive/current turn angle", getTurnRotation().getRotations()); } public void setVoltage(Voltage volts) { @@ -63,6 +64,7 @@ public void runSetPoint(SwerveModuleState state) { io.stopModule(); } io.setDesiredState(state); + Logger.recordOutput("Drive/desired turn angle", state.angle.getRotations()); } /** Returns the current turn angle of the module. */ diff --git a/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java index 10a8ec57..e4e54601 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java @@ -23,9 +23,8 @@ public class SimulatedModule implements ModuleInterface { new Constraints( ModuleConstants.MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND, ModuleConstants.MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED); - private final ProfiledPIDController turnPID = - new ProfiledPIDController(5, 0, 0, turnConstraints); - private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(6, 8, 0); + private final ProfiledPIDController turnPID = new ProfiledPIDController(18, 0, 0, turnConstraints); + private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(0, 0, 0); public SimulatedModule(SwerveModuleSimulation moduleSimulation) { this.moduleSimulation = moduleSimulation; From 3c7b3acddd43f4166cf281cfe41fd54aa957dde2 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 2 Jan 2025 16:24:53 -0500 Subject: [PATCH 70/75] idk msthn --- src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java | 2 +- .../frc/robot/subsystems/swerve/module/SimulatedModule.java | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index c988e709..81e12f54 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -202,7 +202,7 @@ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fi xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d()) : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); - setpoint = setpointGenerator.generateSimpleSetpoint(setpoint, desiredSpeeds, 0.02); + setpoint = setpointGenerator.generateSetpoint(setpoint, desiredSpeeds, 0.02); setModuleStates(setpoint.moduleStates()); Logger.recordOutput("SwerveStates/DesiredStates", setpoint.moduleStates()); diff --git a/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java index e4e54601..44094b5d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java @@ -23,7 +23,8 @@ public class SimulatedModule implements ModuleInterface { new Constraints( ModuleConstants.MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND, ModuleConstants.MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED); - private final ProfiledPIDController turnPID = new ProfiledPIDController(18, 0, 0, turnConstraints); + private final ProfiledPIDController turnPID = + new ProfiledPIDController(18, 0, 0, turnConstraints); private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(0, 0, 0); public SimulatedModule(SwerveModuleSimulation moduleSimulation) { From 509eeebbbf1baf55cef85c5e7b481d953fbede6e Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 2 Jan 2025 18:34:57 -0500 Subject: [PATCH 71/75] it works lmao --- .../setpointGen/SwerveSetpointGenerator.java | 65 +++++-------------- 1 file changed, 17 insertions(+), 48 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java index 008d2df4..392f9e1c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java @@ -9,8 +9,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.RobotController; -import frc.robot.subsystems.swerve.setpointGen.SPGCalcs.LocalVars; -import org.littletonrobotics.junction.Logger; /** * Swerve setpoint generatoR that has been passed around so many times its hard to keep track, just @@ -24,7 +22,6 @@ public class SwerveSetpointGenerator { private static final ChassisSpeeds ZERO_SPEEDS = new ChassisSpeeds(); private static final int NUM_MODULES = SPGCalcs.NUM_MODULES; - // private final double brownoutVoltage; private final SwerveDriveKinematics kinematics; private final Translation2d[] moduleLocations; @@ -63,7 +60,6 @@ public SwerveSetpointGenerator( this.moiKgMetersSquared = moiKgMetersSquared; this.wheelRadiusMeters = wheelDiameterMeters / 2; this.maxDriveVelocityMPS = driveMotor.freeSpeedRadPerSec * wheelRadiusMeters; - // this.brownoutVoltage = RobotController.getBrownoutVoltage(); wheelFrictionForce = wheelCoF * ((massKg / 4) * 9.81); // maxTorqueFriction = this.wheelFrictionForce * wheelRadiusMeters; @@ -89,13 +85,6 @@ public SwerveSetpointGenerator( */ public SwerveSetpoint generateSetpoint( final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) { - // Add logging for inputs - // if (Double.isNaN(inputVoltage)) { - // inputVoltage = 12.0; - // } else { - // inputVoltage = Math.max(inputVoltage, brownoutVoltage); - // } - // https://github.com/wpilibsuite/allwpilib/issues/7332 SwerveModuleState[] desiredModuleStates = kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds); @@ -120,37 +109,32 @@ public SwerveSetpoint generateSetpoint( - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; vars.minS = 1.0; - Logger.recordOutput("beginningVars", vars); - checkNeedToSteer(vars); - - Logger.recordOutput("postCheckNeedToSteer", vars); - makeVectors(vars); - Logger.recordOutput("pastMakeVectors", vars); - - if (vars.allModulesShouldFlip - && !epsilonEquals(prevSetpoint.chassisSpeeds(), ZERO_SPEEDS) - && !epsilonEquals(desiredRobotRelativeSpeeds, ZERO_SPEEDS)) { - // It will (likely) be faster to stop the robot, rotate the modules in place to the - // complement - // of the desired angle, and accelerate again. - return generateSetpoint(prevSetpoint, ZERO_SPEEDS, dt); - } + // if (vars.allModulesShouldFlip + // && !epsilonEquals(prevSetpoint.chassisSpeeds(), ZERO_SPEEDS) + // && !epsilonEquals(desiredRobotRelativeSpeeds, ZERO_SPEEDS)) { + // // It will (likely) be faster to stop the robot, rotate the modules in place to the + // complement + // // of the desired angle, and accelerate again. + // return generateSetpoint(prevSetpoint, ZERO_SPEEDS, dt); + // } solveSteering(vars); - Logger.recordOutput("postSolveSteering", vars); solveDriving(vars); - Logger.recordOutput("postSolveDriving", vars); ChassisSpeeds retSpeeds = new ChassisSpeeds( vars.prevSpeeds.vxMetersPerSecond + vars.minS * vars.dx, vars.prevSpeeds.vyMetersPerSecond + vars.minS * vars.dy, vars.prevSpeeds.omegaRadiansPerSecond + vars.minS * vars.dtheta); - retSpeeds = ChassisSpeeds.discretize(retSpeeds, dt); + retSpeeds.discretize( + retSpeeds.vxMetersPerSecond, + retSpeeds.vyMetersPerSecond, + retSpeeds.omegaRadiansPerSecond, + dt); double chassisAccelX = (retSpeeds.vxMetersPerSecond - vars.prevSpeeds.vxMetersPerSecond) / dt; double chassisAccelY = (retSpeeds.vyMetersPerSecond - vars.prevSpeeds.vyMetersPerSecond) / dt; @@ -175,15 +159,12 @@ public SwerveSetpoint generateSetpoint( accelStates[m].speedMetersPerSecond); } - Logger.recordOutput("finalVars", vars); - - return new SwerveSetpoint( // Logger.recordOutput("output", - retSpeeds, outputStates); + // log("output", + return new SwerveSetpoint(retSpeeds, outputStates); } public SwerveSetpoint generateSimpleSetpoint( final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) { - AdvancedSwerveModuleState[] outputStates = new AdvancedSwerveModuleState[NUM_MODULES]; SwerveModuleState[] desiredModuleStates = kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds); @@ -192,6 +173,7 @@ public SwerveSetpoint generateSimpleSetpoint( desiredModuleStates[m].optimize(prevSetpoint.moduleStates()[m].angle); outputStates[m] = AdvancedSwerveModuleState.fromBase(desiredModuleStates[m]); } + return new SwerveSetpoint(kinematics.toChassisSpeeds(desiredModuleStates), outputStates); } @@ -309,16 +291,8 @@ private void solveDriving(LocalVars vars) { // battery is sagging down to 11v, which will affect the max torque output double currentDraw = driveMotor.getCurrent(Math.abs(lastVelRadPerSec), RobotController.getInputVoltage()); - double reverseCurrentDraw = - Math.abs( - driveMotor.getCurrent( - Math.abs(lastVelRadPerSec), -RobotController.getInputVoltage())); currentDraw = Math.min(currentDraw, driveCurrentLimitAmps); - - reverseCurrentDraw = Math.min(reverseCurrentDraw, driveCurrentLimitAmps); - double forwardModuleTorque = driveMotor.getTorque(currentDraw); - double reverseModuleTorque = driveMotor.getTorque(reverseCurrentDraw); - // double moduleTorque = driveMotor.getTorque(currentDraw); + double moduleTorque = driveMotor.getTorque(currentDraw); double prevSpeed = vars.prevModuleStates[m].speedMetersPerSecond; vars.desiredModuleStates[m].optimize(vars.prevModuleStates[m].angle); @@ -326,13 +300,9 @@ private void solveDriving(LocalVars vars) { int forceSign; Rotation2d forceAngle = vars.prevModuleStates[m].angle; - double moduleTorque; - if (epsilonEquals(prevSpeed, 0.0) || (prevSpeed > 0 && desiredSpeed >= prevSpeed) || (prevSpeed < 0 && desiredSpeed <= prevSpeed)) { - moduleTorque = forwardModuleTorque; - // Torque loss will be fighting motor moduleTorque -= torqueLoss; forceSign = 1; // Force will be applied in direction of module @@ -340,7 +310,6 @@ private void solveDriving(LocalVars vars) { forceAngle = forceAngle.plus(Rotation2d.k180deg); } } else { - moduleTorque = reverseModuleTorque; // Torque loss will be helping the motor moduleTorque += torqueLoss; forceSign = -1; // Force will be applied in opposite direction of module From ad8a3acc542d078c469f805ef9e81151035ee5ec Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 2 Jan 2025 19:14:59 -0500 Subject: [PATCH 72/75] update robot mode to simulation and improve thread priority handling --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 6 ++++-- .../frc/robot/extras/simulation/field/SimulatedField.java | 8 ++++---- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ac55d717..38742b18 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,7 +14,7 @@ public static final class LogPaths { public static final String PHYSICS_SIMULATION_PATH = "MaplePhysicsSimulation/"; } - public static final Mode CURRENT_MODE = Mode.REAL; + public static final Mode CURRENT_MODE = Mode.SIM; public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a0a69672..00ed33c5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,5 +1,6 @@ package frc.robot; +import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.extras.simulation.field.SimulatedField; @@ -74,7 +75,7 @@ public Robot() { public void robotPeriodic() { // TODO: test this! // Switch thread to high priority to improve loop timing - // Threads.setCurrentThreadPriority(true, 99); + Threads.setCurrentThreadPriority(true, 99); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled commands, running already-scheduled commands, removing @@ -84,7 +85,7 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); // Return to normal thread priority - // Threads.setCurrentThreadPriority(false, 10); + Threads.setCurrentThreadPriority(false, 10); } /** This function is called once when the robot is disabled. */ @@ -120,6 +121,7 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_robotContainer.teleopInit(); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java b/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java index 317cbc71..e73bc04a 100644 --- a/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java +++ b/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java @@ -13,7 +13,7 @@ import frc.robot.extras.simulation.mechanismSim.swerve.AbstractDriveTrainSimulation; import frc.robot.extras.simulation.mechanismSim.swerve.BrushlessMotorSim; import frc.robot.extras.util.GeomUtil; -import java.lang.ref.WeakReference; +// import java.lang.ref.WeakReference; import java.util.*; import org.dyn4j.dynamics.Body; import org.dyn4j.dynamics.BodyFixture; @@ -130,7 +130,7 @@ public static void overrideSimulationTimings( protected final Set driveTrainSimulations; protected final Set gamePieces; protected final List simulationSubTickActions; - protected final List> motors; + protected final List motors; private final List intakeSimulations; /** @@ -284,7 +284,7 @@ public void simulationPeriodic() { } public void addMotor(BrushlessMotorSim motor) { - motors.add(new WeakReference<>(motor)); + motors.add(motor); } /** @@ -306,7 +306,7 @@ public void addMotor(BrushlessMotorSim motor) { private void simulationSubTick() { ArrayList motorCurrents = new ArrayList<>(); for (var motor : motors) { - BrushlessMotorSim motorRef = motor.get(); + BrushlessMotorSim motorRef = motor; if (motorRef != null) { motorRef.update(); } From bf661f9f8d1233875f154280c54da2e4c3473fed Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Fri, 3 Jan 2025 21:20:40 -0500 Subject: [PATCH 73/75] wtf why does it work :sob: --- src/main/java/frc/robot/BuildConstants.java | 16 ++-- src/main/java/frc/robot/Robot.java | 30 +++---- src/main/java/frc/robot/RobotContainer.java | 16 ++-- .../commands/drive/DriveCommandBase.java | 3 +- .../java/frc/robot/extras/util/TimeUtil.java | 3 +- .../robot/subsystems/swerve/SwerveDrive.java | 21 +++-- .../setpointGen/SwerveSetpointGenerator.java | 6 +- .../subsystems/vision/PhysicalVision.java | 2 + .../subsystems/vision/SimulatedVision.java | 47 ++++++----- ...atest.json => Phoenix6-25.0.0-beta-4.json} | 80 +++++++++++++------ ...ta-3.json => Studica-2025.1.1-beta-4.json} | 14 ++-- ...b.json => photonlib-v2025.0.0-beta-8.json} | 12 +-- 12 files changed, 142 insertions(+), 108 deletions(-) rename vendordeps/{Phoenix6-frc2025-beta-latest.json => Phoenix6-25.0.0-beta-4.json} (83%) rename vendordeps/{Studica-2025.1.1-beta-3.json => Studica-2025.1.1-beta-4.json} (86%) rename vendordeps/{photonlib.json => photonlib-v2025.0.0-beta-8.json} (88%) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 09f0aadb..8cbe8222 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,15 +3,15 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "offseason-robot-code-2024"; + public static final String MAVEN_NAME = "offseason-robot-code-2024-1"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 464; - public static final String GIT_SHA = "e85bebc1ff41f2682e3d4ccbe67f1f5ce0f8a0f3"; - public static final String GIT_DATE = "2024-12-08 17:15:05 EST"; - public static final String GIT_BRANCH = "add-aquila-constants"; - public static final String BUILD_DATE = "2024-12-08 17:29:19 EST"; - public static final long BUILD_UNIX_TIME = 1733696959570L; - public static final int DIRTY = 0; + public static final int GIT_REVISION = 533; + public static final String GIT_SHA = "79a886e543767e4320fa8f9f31ac613afa9377c3"; + public static final String GIT_DATE = "2025-01-02 19:18:09 EST"; + public static final String GIT_BRANCH = "vision-sim"; + public static final String BUILD_DATE = "2025-01-03 21:17:04 EST"; + public static final long BUILD_UNIX_TIME = 1735957024697L; + public static final int DIRTY = 1; private BuildConstants() {} } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f2cac57f..b3a2417a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,21 +24,21 @@ public class Robot extends LoggedRobot { public Robot() { // Record metadata // Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); - // Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); - // Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - // Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); - // Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); - // switch (BuildConstants.DIRTY) { - // case 0: - // Logger.recordMetadata("GitDirty", "All changes committed"); - // break; - // case 1: - // Logger.recordMetadata("GitDirty", "Uncomitted changes"); - // break; - // default: - // Logger.recordMetadata("GitDirty", "Unknown"); - // break; - // } + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } // Set up data receivers & replay source switch (Constants.CURRENT_MODE) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f8742040..b3c60e7e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.SimulationConstants; import frc.robot.commands.drive.DriveCommand; import frc.robot.extras.simulation.field.SimulatedField; import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation; @@ -87,8 +86,8 @@ public RobotContainer() { DriveConstants.TRACK_WIDTH + .2, DriveConstants.WHEEL_BASE + .2, SwerveModuleSimulation.getModule( - DCMotor.getFalcon500(1), - DCMotor.getFalcon500(1), + DCMotor.getKrakenX60(1).withReduction(ModuleConstants.DRIVE_GEAR_RATIO), + DCMotor.getFalcon500(1).withReduction(11), 60, WHEEL_GRIP.TIRE_WHEEL, ModuleConstants.DRIVE_GEAR_RATIO), @@ -145,7 +144,7 @@ private void resetFieldAndOdometryForAuto(Pose2d robotStartingPoseAtBlueAlliance } // swerveDrive.periodic(); - swerveDrive.resetPosition(startingPose); + swerveDrive.setPose(startingPose); } public void teleopInit() { @@ -246,7 +245,7 @@ private void configureButtonBindings() { driverRightDirectionPad.onTrue( new InstantCommand( () -> - swerveDrive.resetPosition( + swerveDrive.setPose( new Pose2d( swerveDrive.getPose().getX(), swerveDrive.getPose().getY(), @@ -255,11 +254,10 @@ private void configureButtonBindings() { .x() .onTrue( new InstantCommand( - () -> - swerveDrive.resetPosition(swerveDriveSimulation.getSimulatedDriveTrainPose()))); + () -> swerveDrive.setPose(swerveDriveSimulation.getSimulatedDriveTrainPose()))); // // // Reset robot odometry based on vision pose measurement from april tags driverLeftDirectionPad.onTrue( - new InstantCommand(() -> swerveDrive.resetPosition(visionSubsystem.getLastSeenPose()))); + new InstantCommand(() -> swerveDrive.setPose(visionSubsystem.getLastSeenPose()))); // // driverLeftDpad.onTrue(new InstantCommand(() -> swerveDrive.resetOdometry(new // Pose2d(15.251774787902832, 5.573054313659668, Rotation2d.fromRadians(3.14159265))))); // // driverBButton.whileTrue(new ShootPass(swerveDrive, shooterSubsystem, pivotSubsystem, @@ -308,7 +306,7 @@ private void configureButtonBindings() { public Command getAutonomousCommand() { // Resets the pose factoring in the robot side // This is just a failsafe, pose should be reset at the beginning of auto - swerveDrive.resetPosition( + swerveDrive.setPose( new Pose2d( swerveDrive.getPose().getX(), swerveDrive.getPose().getY(), diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java index 83f8aab0..73ae4529 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.extras.interpolators.MultiLinearInterpolator; +import frc.robot.extras.util.TimeUtil; import frc.robot.subsystems.swerve.SwerveDrive; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionConstants; @@ -70,7 +71,7 @@ public void calculatePoseFromLimelight(Limelight limelight) { swerveDrive.addPoseEstimatorVisionMeasurement( vision.getPoseFromAprilTags(limelight), - Logger.getTimestamp() - vision.getLatencySeconds(limelight)); + TimeUtil.getLogTimeSeconds() - vision.getLatencySeconds(limelight)); } Logger.recordOutput( diff --git a/src/main/java/frc/robot/extras/util/TimeUtil.java b/src/main/java/frc/robot/extras/util/TimeUtil.java index 754968af..3cad1701 100644 --- a/src/main/java/frc/robot/extras/util/TimeUtil.java +++ b/src/main/java/frc/robot/extras/util/TimeUtil.java @@ -1,5 +1,6 @@ package frc.robot.extras.util; +import edu.wpi.first.wpilibj.RobotController; import org.littletonrobotics.junction.Logger; public class TimeUtil { @@ -26,6 +27,6 @@ public static double getLogTimeSeconds() { } public static double getRealTimeSeconds() { - return Logger.getRealTimestamp() / 1_000_000.0; + return RobotController.getFPGATime() / 1_000_000.0; } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 5efcdc95..81e12f54 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -39,7 +39,7 @@ public class SwerveDrive extends SubsystemBase { private Rotation2d rawGyroRotation; private final SwerveModulePosition[] lastModulePositions; - private final SwerveDrivePoseEstimator odometry; + private final SwerveDrivePoseEstimator poseEstimator; private final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator( @@ -86,10 +86,10 @@ public SwerveDrive( new SwerveModulePosition(), new SwerveModulePosition() }; - this.odometry = + this.poseEstimator = new SwerveDrivePoseEstimator( DriveConstants.DRIVE_KINEMATICS, - getGyroRotation2d(), + rawGyroRotation, lastModulePositions, new Pose2d(), VecBuilder.fill( @@ -99,7 +99,7 @@ public SwerveDrive( VisionConstants.VISION_Y_POS_TRUST, VisionConstants.VISION_ANGLE_TRUST)); - this.odometryThread = OdometryThread.createInstance(DeviceCANBus.CANIVORE); + this.odometryThread = OdometryThread.createInstance(DeviceCANBus.RIO); this.odometryThreadInputs = new OdometryThreadInputsAutoLogged(); this.odometryThread.start(); @@ -123,7 +123,7 @@ public SwerveDrive( */ public void addPoseEstimatorVisionMeasurement( Pose2d visionMeasurement, double currentTimeStampSeconds) { - odometry.addVisionMeasurement(visionMeasurement, currentTimeStampSeconds); + poseEstimator.addVisionMeasurement(visionMeasurement, currentTimeStampSeconds); } /** @@ -136,7 +136,7 @@ public void addPoseEstimatorVisionMeasurement( */ public void setPoseEstimatorVisionConfidence( double xStandardDeviation, double yStandardDeviation, double thetaStandardDeviation) { - odometry.setVisionMeasurementStdDevs( + poseEstimator.setVisionMeasurementStdDevs( VecBuilder.fill(xStandardDeviation, yStandardDeviation, thetaStandardDeviation)); } @@ -160,7 +160,6 @@ public void runCharacterization(double volts) { } /** Returns the average drive velocity in rotations/sec. */ - // TODO: fix method public double getCharacterizationVelocity() { double velocity = 0.0; for (SwerveModule module : swerveModules) { @@ -256,7 +255,7 @@ public void zeroHeading() { */ @AutoLogOutput(key = "Odometry/Odometry") public Pose2d getPose() { - return odometry.getEstimatedPosition(); + return poseEstimator.getEstimatedPosition(); } /** Returns a Rotation2d for the heading of the robot */ @@ -319,7 +318,7 @@ public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } - odometry.updateWithTime( + poseEstimator.updateWithTime( // odometryThreadInputs.measurementTimeStamps[timestampIndex], Logger.getTimestamp(), rawGyroRotation, modulePositions); } @@ -373,7 +372,7 @@ private SwerveModulePosition[] getModulePositions() { * * @param pose pose to set */ - public void resetPosition(Pose2d pose) { - odometry.resetPosition(getGyroRotation2d(), getModulePositions(), pose); + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java index 392f9e1c..80fc630a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java @@ -130,11 +130,7 @@ public SwerveSetpoint generateSetpoint( vars.prevSpeeds.vxMetersPerSecond + vars.minS * vars.dx, vars.prevSpeeds.vyMetersPerSecond + vars.minS * vars.dy, vars.prevSpeeds.omegaRadiansPerSecond + vars.minS * vars.dtheta); - retSpeeds.discretize( - retSpeeds.vxMetersPerSecond, - retSpeeds.vyMetersPerSecond, - retSpeeds.omegaRadiansPerSecond, - dt); + retSpeeds = ChassisSpeeds.discretize(retSpeeds, dt); double chassisAccelX = (retSpeeds.vxMetersPerSecond - vars.prevSpeeds.vxMetersPerSecond) / dt; double chassisAccelY = (retSpeeds.vyMetersPerSecond - vars.prevSpeeds.vyMetersPerSecond) / dt; diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 33e481df..4ceab058 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -61,6 +61,8 @@ public void updateInputs(VisionInputs inputs) { inputs.limelightCalculatedPose[limelight.getId()] = getPoseFromAprilTags(limelight); inputs.limelightTimestamp[limelight.getId()] = getTimeStampSeconds(limelight); inputs.limelightLastSeenPose = getLastSeenPose(); + inputs.limelightAprilTagDistance[limelight.getId()] = + getLimelightAprilTagDistance(limelight); latestInputs.set(inputs); limelightThreads.get(limelight).set(latestInputs.get()); diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index b937a3d6..f2c5bd29 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -46,8 +46,8 @@ public SimulatedVision(Supplier robotSimulationPose) { // Create simulated camera properties. These can be set to mimic your actual // camera. var cameraProperties = new SimCameraProperties(); - // cameraProperties.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7)); - // cameraProperties.setCalibError(0.35, 0.10); + cameraProperties.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7)); + cameraProperties.setCalibError(0.35, 0.10); cameraProperties.setFPS(15); cameraProperties.setAvgLatencyMs(20); cameraProperties.setLatencyStdDevMs(5); @@ -63,23 +63,23 @@ public SimulatedVision(Supplier robotSimulationPose) { frontRightCameraSim = new PhotonCameraSim(getSimulationCamera(Limelight.FRONT_RIGHT), cameraProperties); - visionSim.addCamera(shooterCameraSim, VisionConstants.SHOOTER_TRANSFORM); - visionSim.addCamera(frontLeftCameraSim, VisionConstants.FRONT_LEFT_TRANSFORM); - visionSim.addCamera(frontRightCameraSim, VisionConstants.FRONT_RIGHT_TRANSFORM); + visionSim.addCamera(shooterCameraSim, VisionConstants.SHOOTER_TRANSFORM.inverse()); + visionSim.addCamera(frontLeftCameraSim, VisionConstants.FRONT_LEFT_TRANSFORM.inverse()); + visionSim.addCamera(frontRightCameraSim, VisionConstants.FRONT_RIGHT_TRANSFORM.inverse()); // Enable the raw and processed streams. (http://localhost:1181 / 1182) - shooterCameraSim.enableRawStream(true); - shooterCameraSim.enableProcessedStream(true); - frontLeftCameraSim.enableRawStream(true); - frontLeftCameraSim.enableProcessedStream(true); - frontRightCameraSim.enableRawStream(true); - frontRightCameraSim.enableProcessedStream(true); - - // Enable drawing a wireframe visualization of the field to the camera streams. - // This is extremely resource-intensive and is disabled by default. - shooterCameraSim.enableDrawWireframe(true); - frontLeftCameraSim.enableDrawWireframe(true); - frontRightCameraSim.enableDrawWireframe(true); + // shooterCameraSim.enableRawStream(true); + // shooterCameraSim.enableProcessedStream(true); + // frontLeftCameraSim.enableRawStream(true); + // frontLeftCameraSim.enableProcessedStream(true); + // frontRightCameraSim.enableRawStream(true); + // frontRightCameraSim.enableProcessedStream(true); + + // // Enable drawing a wireframe visualization of the field to the camera streams. + // // This is extremely resource-intensive and is disabled by default. + // shooterCameraSim.enableDrawWireframe(true); + // frontLeftCameraSim.enableDrawWireframe(true); + // frontRightCameraSim.enableDrawWireframe(true); } @Override @@ -97,8 +97,9 @@ public void updateInputs(VisionInputs inputs) { getSimulationCamera(limelight).getAllUnreadResults(), getLimelightTable(limelight), limelight); - inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); - inputs.limelightAprilTagDistance[limelight.getId()] = getLimelightAprilTagDistance(limelight); + // inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); + // inputs.limelightAprilTagDistance[limelight.getId()] = + // getLimelightAprilTagDistance(limelight); } super.updateInputs(inputs); } @@ -145,7 +146,13 @@ private void writeToTable( .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); tagCount[limelight.getId()] = result.getMultiTagResult().get().fiducialIDsUsed.size(); apriltagDist[limelight.getId()] = - result.getMultiTagResult().get().estimatedPose.best.getX(); + result + .getMultiTagResult() + .get() + .estimatedPose + .best + .getTranslation() + .getDistance(result.getBestTarget().bestCameraToTarget.getTranslation()); } table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); diff --git a/vendordeps/Phoenix6-frc2025-beta-latest.json b/vendordeps/Phoenix6-25.0.0-beta-4.json similarity index 83% rename from vendordeps/Phoenix6-frc2025-beta-latest.json rename to vendordeps/Phoenix6-25.0.0-beta-4.json index cd9d153a..5a588aab 100644 --- a/vendordeps/Phoenix6-frc2025-beta-latest.json +++ b/vendordeps/Phoenix6-25.0.0-beta-4.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-frc2025-beta-latest.json", + "fileName": "Phoenix6-25.0.0-beta-4.json", "name": "CTRE-Phoenix (v6)", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.0.0-beta-3" + "version": "25.0.0-beta-4" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -182,7 +196,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -198,7 +212,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -214,7 +228,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -230,7 +244,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -246,7 +260,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -262,7 +276,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +292,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -294,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -310,7 +324,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -326,7 +340,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -339,10 +353,26 @@ ], "simMode": "swsim" }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.0.0-beta-4", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-3", + "version": "25.0.0-beta-4", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/Studica-2025.1.1-beta-3.json b/vendordeps/Studica-2025.1.1-beta-4.json similarity index 86% rename from vendordeps/Studica-2025.1.1-beta-3.json rename to vendordeps/Studica-2025.1.1-beta-4.json index 92a57d64..facfc721 100644 --- a/vendordeps/Studica-2025.1.1-beta-3.json +++ b/vendordeps/Studica-2025.1.1-beta-4.json @@ -1,13 +1,13 @@ { - "fileName": "Studica-2025.1.1-beta-3.json", + "fileName": "Studica-2025.1.1-beta-4.json", "name": "Studica", - "version": "2025.1.1-beta-3", + "version": "2025.1.1-beta-4", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "frcYear": "2025", "mavenUrls": [ "https://dev.studica.com/maven/release/2025/" ], - "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-3.json", + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-4.json", "cppDependencies": [ { "artifactId": "Studica-cpp", @@ -24,7 +24,7 @@ "libName": "Studica", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.1.1-beta-3" + "version": "2025.1.1-beta-4" }, { "artifactId": "Studica-driver", @@ -41,14 +41,14 @@ "libName": "StudicaDriver", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.1.1-beta-3" + "version": "2025.1.1-beta-4" } ], "javaDependencies": [ { "artifactId": "Studica-java", "groupId": "com.studica.frc", - "version": "2025.1.1-beta-3" + "version": "2025.1.1-beta-4" } ], "jniDependencies": [ @@ -65,7 +65,7 @@ "osxuniversal", "windowsx86-64" ], - "version": "2025.1.1-beta-3" + "version": "2025.1.1-beta-4" } ] } diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib-v2025.0.0-beta-8.json similarity index 88% rename from vendordeps/photonlib.json rename to vendordeps/photonlib-v2025.0.0-beta-8.json index 2e8e6b00..1163429e 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib-v2025.0.0-beta-8.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.0.0-beta-5", + "version": "v2025.0.0-beta-8", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-5", + "version": "v2025.0.0-beta-8", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.0.0-beta-5", + "version": "v2025.0.0-beta-8", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-5", + "version": "v2025.0.0-beta-8", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.0.0-beta-5" + "version": "v2025.0.0-beta-8" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.0.0-beta-5" + "version": "v2025.0.0-beta-8" } ] } From c1d909fe2509e78b3b041ad5232b46727452a1d6 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Sat, 4 Jan 2025 02:32:10 -0500 Subject: [PATCH 74/75] im gonna need to rewrite this shit - vision sim works but is high key incredibly jank --- src/main/java/frc/robot/BuildConstants.java | 10 ++-- .../commands/drive/DriveCommandBase.java | 4 +- .../subsystems/vision/SimulatedVision.java | 51 ++++++++----------- .../frc/robot/subsystems/vision/Vision.java | 4 +- 4 files changed, 30 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 8cbe8222..fcb85e86 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "offseason-robot-code-2024-1"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 533; - public static final String GIT_SHA = "79a886e543767e4320fa8f9f31ac613afa9377c3"; - public static final String GIT_DATE = "2025-01-02 19:18:09 EST"; + public static final int GIT_REVISION = 534; + public static final String GIT_SHA = "bf661f9f8d1233875f154280c54da2e4c3473fed"; + public static final String GIT_DATE = "2025-01-03 21:20:40 EST"; public static final String GIT_BRANCH = "vision-sim"; - public static final String BUILD_DATE = "2025-01-03 21:17:04 EST"; - public static final long BUILD_UNIX_TIME = 1735957024697L; + public static final String BUILD_DATE = "2025-01-04 02:29:02 EST"; + public static final long BUILD_UNIX_TIME = 1735975742416L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java index 73ae4529..efadafa3 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java @@ -42,8 +42,8 @@ public void execute() { vision.setHeadingInfo( swerveDrive.getPose().getRotation().getDegrees(), swerveDrive.getGyroRate()); calculatePoseFromLimelight(Limelight.SHOOTER); - calculatePoseFromLimelight(Limelight.FRONT_LEFT); - calculatePoseFromLimelight(Limelight.FRONT_RIGHT); + // calculatePoseFromLimelight(Limelight.FRONT_LEFT); + // calculatePoseFromLimelight(Limelight.FRONT_RIGHT); } public void calculatePoseFromLimelight(Limelight limelight) { diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index f2c5bd29..a4e46773 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -24,6 +24,7 @@ public class SimulatedVision extends PhysicalVision { PhotonCameraSim frontRightCameraSim; private final VisionSystemSim visionSim; private final Supplier robotSimulationPose; + private Pose2d lastSeenPose = new Pose2d(); private final int kResWidth = 1280; private final int kResHeight = 800; @@ -63,9 +64,10 @@ public SimulatedVision(Supplier robotSimulationPose) { frontRightCameraSim = new PhotonCameraSim(getSimulationCamera(Limelight.FRONT_RIGHT), cameraProperties); - visionSim.addCamera(shooterCameraSim, VisionConstants.SHOOTER_TRANSFORM.inverse()); - visionSim.addCamera(frontLeftCameraSim, VisionConstants.FRONT_LEFT_TRANSFORM.inverse()); - visionSim.addCamera(frontRightCameraSim, VisionConstants.FRONT_RIGHT_TRANSFORM.inverse()); + visionSim.addCamera( + shooterCameraSim, VisionConstants.SHOOTER_TRANSFORM); // check inverse things + visionSim.addCamera(frontLeftCameraSim, VisionConstants.FRONT_LEFT_TRANSFORM); + visionSim.addCamera(frontRightCameraSim, VisionConstants.FRONT_RIGHT_TRANSFORM); // Enable the raw and processed streams. (http://localhost:1181 / 1182) // shooterCameraSim.enableRawStream(true); @@ -91,6 +93,7 @@ public void updateInputs(VisionInputs inputs) { visionSim.update(robotSimulationPose.get()); Logger.recordOutput("Vision/SimIO/updateSimPose", robotSimulationPose.get()); } + super.updateInputs(inputs); for (Limelight limelight : Limelight.values()) { writeToTable( @@ -101,7 +104,6 @@ public void updateInputs(VisionInputs inputs) { // inputs.limelightAprilTagDistance[limelight.getId()] = // getLimelightAprilTagDistance(limelight); } - super.updateInputs(inputs); } private void writeToTable( @@ -144,19 +146,16 @@ private void writeToTable( table .getEntry("botpose_orb_wpiblue") .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); - tagCount[limelight.getId()] = result.getMultiTagResult().get().fiducialIDsUsed.size(); - apriltagDist[limelight.getId()] = - result - .getMultiTagResult() - .get() - .estimatedPose - .best - .getTranslation() - .getDistance(result.getBestTarget().bestCameraToTarget.getTranslation()); - } - table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); - table.getEntry("cl").setDouble(result.metadata.getLatencyMillis()); + table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); + table.getEntry("cl").setDouble(result.metadata.getLatencyMillis()); + lastSeenPose = + new Pose2d( + result.getMultiTagResult().get().estimatedPose.best.getTranslation().getX(), + result.getMultiTagResult().get().estimatedPose.best.getY(), + new Rotation2d( + result.getMultiTagResult().get().estimatedPose.best.getRotation().getAngle())); + } } } @@ -178,26 +177,18 @@ public NetworkTable getLimelightTable(Limelight limelight) { }; } + // @Override + // public boolean canSeeAprilTags(Limelight limelight) { + // table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); + + // } @Override public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { super.setHeadingInfo(headingDegrees, headingRateDegrees); } - @Override - public int getNumberOfAprilTags(Limelight limelight) { - // TODO Auto-generated method stub - return tagCount[limelight.getId()]; - } - - @Override - public double getLimelightAprilTagDistance(Limelight limelight) { - // TODO Auto-generated method stub - return apriltagDist[limelight.getId()]; - } - @Override public Pose2d getLastSeenPose() { - // TODO Auto-generated method stub - return super.getLastSeenPose(); + return lastSeenPose; } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 4f34c456..cf714238 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -26,7 +26,7 @@ public void periodic() { // Add methods to support DriveCommand public int getNumberOfAprilTags(Limelight limelight) { - return visionInterface.getNumberOfAprilTags(limelight); + return inputs.limelightTargets[limelight.getId()]; } public double getLimelightAprilTagDistance(Limelight limelight) { @@ -55,6 +55,6 @@ public Pose2d getPoseFromAprilTags(Limelight limelight) { } public Pose2d getLastSeenPose() { - return inputs.limelightLastSeenPose; + return visionInterface.getLastSeenPose(); } } From 46a77b0f2cc0d9f5f7a396d30cb096c06f898cac Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sat, 4 Jan 2025 07:38:29 +0000 Subject: [PATCH 75/75] Formatting fixes --- .../swerve/setpointGen/SwerveSetpointGenerator.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java index 581e42f6..4f2f230d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java @@ -130,9 +130,7 @@ public SwerveSetpoint generateSetpoint( vars.prevSpeeds.vxMetersPerSecond + vars.minS * vars.dx, vars.prevSpeeds.vyMetersPerSecond + vars.minS * vars.dy, vars.prevSpeeds.omegaRadiansPerSecond + vars.minS * vars.dtheta); - retSpeeds.discretize( - retSpeeds, - dt); + retSpeeds.discretize(retSpeeds, dt); double chassisAccelX = (retSpeeds.vxMetersPerSecond - vars.prevSpeeds.vxMetersPerSecond) / dt; double chassisAccelY = (retSpeeds.vyMetersPerSecond - vars.prevSpeeds.vyMetersPerSecond) / dt;