From 74fd100faef98ea5c88365428fa9e2cf2b23e400 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Fri, 10 Oct 2025 21:46:39 -0700 Subject: [PATCH 01/36] Remove entire robot/lib/swerve folder and replace all references with subsystem/ctre/CtreDriveConstants --- src/main/java/frc/robot/Constants.java | 16 +- .../swerve/COTSTalonFXSwerveConstants.java | 311 ------------------ .../frc/robot/lib/swerve/DriveController.java | 32 -- .../swerve/PIDHolonomicDriveController.java | 150 --------- .../ProfiledPIDHolonomicDriveController.java | 149 --------- .../lib/swerve/SwerveModuleConstants.java | 28 -- .../drive/ctre/CtreDriveConstants.java | 6 +- 7 files changed, 12 insertions(+), 680 deletions(-) delete mode 100644 src/main/java/frc/robot/lib/swerve/COTSTalonFXSwerveConstants.java delete mode 100644 src/main/java/frc/robot/lib/swerve/DriveController.java delete mode 100644 src/main/java/frc/robot/lib/swerve/PIDHolonomicDriveController.java delete mode 100644 src/main/java/frc/robot/lib/swerve/ProfiledPIDHolonomicDriveController.java delete mode 100644 src/main/java/frc/robot/lib/swerve/SwerveModuleConstants.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 23f1b86..d8daa75 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,6 +1,7 @@ package frc.robot; import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Seconds; @@ -28,7 +29,8 @@ import edu.wpi.first.wpilibj.DriverStation; import frc.robot.lib.control.ControlConstants.PIDFConstants; import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; -import frc.robot.lib.swerve.COTSTalonFXSwerveConstants; +// import frc.robot.lib.swerve.COTSTalonFXSwerveConstants; +import frc.robot.subsystems.drive.ctre.CtreDriveConstants; /** * All constants belong here. @@ -55,13 +57,13 @@ public static final class Odometry { public static final class Drive { - public static final COTSTalonFXSwerveConstants SWERVE_MODULE_TYPE = - COTSTalonFXSwerveConstants.SDS.MK4i.KrakenX60(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L3); + // public static final COTSTalonFXSwerveConstants SWERVE_MODULE_TYPE = + // COTSTalonFXSwerveConstants.SDS.MK4i.KrakenX60(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L3); public static final double TRACK_WIDTH = Units.Inches.of(24).in(Units.Meters); public static final double WHEEL_BASE = Units.Inches.of(24).in(Units.Meters); - public static final double WHEEL_CIRCUMFERENCE = SWERVE_MODULE_TYPE.wheelCircumference; - public static final double WHEEL_DIAMETER = SWERVE_MODULE_TYPE.wheelDiameter; + public static final double WHEEL_DIAMETER = 2 * CtreDriveConstants.kWheelRadius.in(Meters);//SWERVE_MODULE_TYPE.wheelDiameter; + public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER*Math.PI;//SWERVE_MODULE_TYPE.wheelCircumference; public static final Translation2d[] MODULE_LOCATIONS = { new Translation2d(-WHEEL_BASE / 2.0, TRACK_WIDTH / 2.0), @@ -70,8 +72,8 @@ public static final class Drive { new Translation2d(WHEEL_BASE / 2.0, -TRACK_WIDTH / 2.0) }; - public static final double DRIVE_GEAR_RATIO = SWERVE_MODULE_TYPE.driveGearRatio; - public static final double ANGLE_GEAR_RATIO = SWERVE_MODULE_TYPE.angleGearRatio; + public static final double DRIVE_GEAR_RATIO = CtreDriveConstants.kDriveGearRatio;//SWERVE_MODULE_TYPE.driveGearRatio; + public static final double ANGLE_GEAR_RATIO = CtreDriveConstants.kSteerGearRatio;//SWERVE_MODULE_TYPE.angleGearRatio; public static final double MAX_SPEED = 5.0; public static final double MAX_ACCEL = 5.0; diff --git a/src/main/java/frc/robot/lib/swerve/COTSTalonFXSwerveConstants.java b/src/main/java/frc/robot/lib/swerve/COTSTalonFXSwerveConstants.java deleted file mode 100644 index e6d3381..0000000 --- a/src/main/java/frc/robot/lib/swerve/COTSTalonFXSwerveConstants.java +++ /dev/null @@ -1,311 +0,0 @@ -package frc.robot.lib.swerve; - -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; - -import edu.wpi.first.math.util.Units; - -/** Contains values and required settings for common COTS swerve modules. */ -public class COTSTalonFXSwerveConstants { - public final double wheelDiameter; - public final double wheelCircumference; - public final double angleGearRatio; - public final double driveGearRatio; - public final double angleKP; - public final double angleKI; - public final double angleKD; - public final InvertedValue driveMotorInvert; - public final InvertedValue angleMotorInvert; - public final SensorDirectionValue cancoderInvert; - - public COTSTalonFXSwerveConstants(double wheelDiameter, double angleGearRatio, double driveGearRatio, double angleKP, double angleKI, double angleKD, InvertedValue driveMotorInvert, InvertedValue angleMotorInvert, SensorDirectionValue cancoderInvert){ - this.wheelDiameter = wheelDiameter; - this.wheelCircumference = wheelDiameter * Math.PI; - this.angleGearRatio = angleGearRatio; - this.driveGearRatio = driveGearRatio; - this.angleKP = angleKP; - this.angleKI = angleKI; - this.angleKD = angleKD; - this.driveMotorInvert = driveMotorInvert; - this.angleMotorInvert = angleMotorInvert; - this.cancoderInvert = cancoderInvert; - } - - /** West Coast Products */ - public static final class WCP { - /** West Coast Products - SwerveX Standard*/ - public static final class SwerveXStandard{ - /** West Coast Products - SwerveX Standard (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (396 / 35) : 1 */ - double angleGearRatio = ((396.0 / 35.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** West Coast Products - SwerveX Standard (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (396 / 35) : 1 */ - double angleGearRatio = ((150.0/7.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** WCP SwerveX Standard X1 - 10 Tooth - (7.85 : 1) */ - public static final double X1_10 = (7.85 / 1.0); - - /** WCP SwerveX Standard X1 - 11 Tooth - (7.13 : 1) */ - public static final double X1_11 = (7.13 / 1.0); - - /** WCP SwerveX Standard X1 - 12 Tooth - (6.54 : 1) */ - public static final double X1_12 = (6.54 / 1.0); - - /** WCP SwerveX Standard X2 - 10 Tooth - (6.56 : 1) */ - public static final double X2_10 = (6.56 / 1.0); - - /** WCP SwerveX Standard X2 - 11 Tooth - (5.96 : 1) */ - public static final double X2_11 = (5.96 / 1.0); - - /** WCP SwerveX Standard X2 - 12 Tooth - (5.46 : 1) */ - public static final double X2_12 = (5.46 / 1.0); - - /** WCP SwerveX Standard X3 - 12 Tooth - (5.14 : 1) */ - public static final double X3_12 = (5.14 / 1.0); - - /** WCP SwerveX Standard X3 - 13 Tooth - (4.75 : 1) */ - public static final double X3_13 = (4.75 / 1.0); - - /** WCP SwerveX Standard X3 - 14 Tooth - (4.41 : 1) */ - public static final double X3_14 = (4.41 / 1.0); - } - } - - /** West Coast Products - SwerveX Flipped*/ - public static final class SwerveXFlipped{ - /** West Coast Products - SwerveX Flipped (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (468 / 35) : 1 */ - double angleGearRatio = ((468.0 / 35.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** West Coast Products - SwerveX Flipped (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (468 / 35) : 1 */ - double angleGearRatio = ((468.0 / 35.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** WCP SwerveX Flipped X1 - 10 Tooth - (8.10 : 1) */ - public static final double X1_10 = (8.10 / 1.0); - - /** WCP SwerveX Flipped X1 - 11 Tooth - (7.36 : 1) */ - public static final double X1_11 = (7.36 / 1.0); - - /** WCP SwerveX Flipped X1 - 12 Tooth - (6.75 : 1) */ - public static final double X1_12 = (6.75 / 1.0); - - /** WCP SwerveX Flipped X2 - 10 Tooth - (6.72 : 1) */ - public static final double X2_10 = (6.72 / 1.0); - - /** WCP SwerveX Flipped X2 - 11 Tooth - (6.11 : 1) */ - public static final double X2_11 = (6.11 / 1.0); - - /** WCP SwerveX Flipped X2 - 12 Tooth - (5.60 : 1) */ - public static final double X2_12 = (5.60 / 1.0); - - /** WCP SwerveX Flipped X3 - 10 Tooth - (5.51 : 1) */ - public static final double X3_10 = (5.51 / 1.0); - - /** WCP SwerveX Flipped X3 - 11 Tooth - (5.01 : 1) */ - public static final double X3_11 = (5.01 / 1.0); - - /** WCP SwerveX Flipped X3 - 12 Tooth - (4.59 : 1) */ - public static final double X3_12 = (4.59 / 1.0); - } - } - } - - /** Swerve Drive Specialities */ - public static final class SDS { - /** Swerve Drive Specialties - MK3 Module*/ - public static final class MK3{ - /** Swerve Drive Specialties - MK3 Module (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** 12.8 : 1 */ - double angleGearRatio = (12.8 / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** Swerve Drive Specialties - MK3 Module (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** 12.8 : 1 */ - double angleGearRatio = (12.8 / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** SDS MK3 - (8.16 : 1) */ - public static final double Standard = (8.16 / 1.0); - /** SDS MK3 - (6.86 : 1) */ - public static final double Fast = (6.86 / 1.0); - } - } - - /** Swerve Drive Specialties - MK4 Module*/ - public static final class MK4{ - /** Swerve Drive Specialties - MK4 Module (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** 12.8 : 1 */ - double angleGearRatio = (12.8 / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** Swerve Drive Specialties - MK4 Module (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** 12.8 : 1 */ - double angleGearRatio = ((150.0/7.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** SDS MK4 - (8.14 : 1) */ - public static final double L1 = (8.14 / 1.0); - /** SDS MK4 - (6.75 : 1) */ - public static final double L2 = (6.75 / 1.0); - /** SDS MK4 - (6.12 : 1) */ - public static final double L3 = (6.12 / 1.0); - /** SDS MK4 - (5.14 : 1) */ - public static final double L4 = (5.14 / 1.0); - } - } - - /** Swerve Drive Specialties - MK4i Module*/ - public static final class MK4i{ - /** Swerve Drive Specialties - MK4i Module (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (150 / 7) : 1 */ - double angleGearRatio = ((150.0 / 7.0) / 1.0); - - double angleKP = 100.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** Swerve Drive Specialties - MK4i Module (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (150 / 7) : 1 */ - double angleGearRatio = ((150.0 / 7.0) / 1.0); - - double angleKP = 9.0; // Change to correct speed - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.Clockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** SDS MK4i - (8.14 : 1) */ - public static final double L1 = (8.14 / 1.0); - /** SDS MK4i - (6.75 : 1) */ - public static final double L2 = (6.75 / 1.0); - /** SDS MK4i - (6.12 : 1) */ - public static final double L3 = (6.12 / 1.0); - } - } - } -} - - \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/swerve/DriveController.java b/src/main/java/frc/robot/lib/swerve/DriveController.java deleted file mode 100644 index 3409f97..0000000 --- a/src/main/java/frc/robot/lib/swerve/DriveController.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.robot.lib.swerve; - -import frc.robot.lib.trajectory.RedTrajectory; - -/** - * An interface for a drive controller which traverses a trajectory. - * It takes the current speed and position and returns the calculated {@code ChassisSpeeds}. - */ -@Deprecated -public interface DriveController { - /** - * Sets the trajectory. - * @param trajectory The trajectory to traverse. - */ - public void setTrajectory(RedTrajectory trajectory); - - /** - * Whether the traversal is done. - * @return Whether the traversal is done. - */ - public boolean isDone(); - - /** - * Resets the drive controller. - */ - public void reset(); - - /** - * Gets the current trajectory. - */ - public RedTrajectory getTrajectory(); -} diff --git a/src/main/java/frc/robot/lib/swerve/PIDHolonomicDriveController.java b/src/main/java/frc/robot/lib/swerve/PIDHolonomicDriveController.java deleted file mode 100644 index bcd76fa..0000000 --- a/src/main/java/frc/robot/lib/swerve/PIDHolonomicDriveController.java +++ /dev/null @@ -1,150 +0,0 @@ -package frc.robot.lib.swerve; - -import frc.robot.lib.control.ControlConstants.*; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants; -import frc.robot.lib.control.PIDVController; -import frc.robot.lib.control.ProfiledPIDVController; -import frc.robot.lib.trajectory.RedTrajectory; - -@Deprecated -public class PIDHolonomicDriveController implements DriveController { - private final PIDVController xController; - private final PIDVController yController; - private final ProfiledPIDVController thetaController; - private double accelConstant; - - private RedTrajectory trajectory; - private Pose2d currentPose; - private ChassisSpeeds currentSpeeds; - - private Timer timer = null; - - static Field2d field = new Field2d(); - static { - SmartDashboard.putData("debug", field); - } - - /** - * A drive controller that works with 2 {@link PIDVController}s for translation and one {@link ProfiledPIDVController} for rotation. - * @param translationConstants The {@link PIDFConstants} for the translation of the robot. - * @param rotationConstants The {@link ProfiledPIDFConstants} for the rotation of the robot. - * @param accelConstant The acceleration feedforwards (useful for traversing sharp turns on a trajectory). - */ - public PIDHolonomicDriveController(PIDFConstants translationConstants, ProfiledPIDFConstants rotationConstants, double accelConstant) { - xController = new PIDVController(translationConstants); - yController = new PIDVController(translationConstants); - thetaController = new ProfiledPIDVController(rotationConstants); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - this.accelConstant = accelConstant; - - timer = new Timer(); - } - - @Override - public void setTrajectory(RedTrajectory trajectory) { - this.trajectory = trajectory; - timer.reset(); - timer.start(); - } - - public void setRobotState(Pose2d pose, Twist2d speeds) { - this.currentPose = pose; - this.currentSpeeds = new ChassisSpeeds(speeds.dx, speeds.dy, speeds.dtheta); - } - - public void setInput(Pair current) { - setRobotState(current.getFirst(), current.getSecond()); - } - - public ChassisSpeeds getOutput() { - if (trajectory == null || currentPose == null || currentSpeeds == null || trajectory.isDone()) { - return new ChassisSpeeds(); - } - - RedTrajectory.State targetState = trajectory.advanceTo(timer.get()); - - double vxFF = targetState.speeds.vxMetersPerSecond; - double vyFF = targetState.speeds.vyMetersPerSecond; - - double xAccelFF = MathUtil.applyDeadband( - targetState.accels.ax, - Constants.Drive.MAX_ACCEL * 0.5); - double yAccelFF = MathUtil.applyDeadband( - targetState.accels.ay, - Constants.Drive.MAX_ACCEL * 0.5); - - double angularAccel = MathUtil.applyDeadband( - targetState.accels.alpha, - Constants.Drive.MAX_ROTATION_ACCEL * 0.5); - xAccelFF += -angularAccel * targetState.pose.getRotation().getSin(); - yAccelFF += angularAccel * targetState.pose.getRotation().getCos(); - - xController.setTarget(targetState.pose.getX()); - yController.setTarget(targetState.pose.getY()); - - xController.setFeedforward(vxFF); - yController.setFeedforward(vyFF); - - xController.setInput(new Pair(currentPose.getX(), currentSpeeds.vxMetersPerSecond)); - yController.setInput(new Pair(currentPose.getY(), currentSpeeds.vyMetersPerSecond)); - - double vx = xController.getOutput(); - double vy = yController.getOutput(); - - thetaController.setTarget(targetState.pose.getRotation().getRadians()); - thetaController.setFeedforward(targetState.speeds.omegaRadiansPerSecond); - thetaController.setInput( - new Pair( - currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond)); - - double rotation = thetaController.getOutput(); - - field.setRobotPose(targetState.pose); - SmartDashboard.putData("debug", field); - - return ChassisSpeeds.fromFieldRelativeSpeeds( - vx + xAccelFF * accelConstant, - vy + yAccelFF * accelConstant, - rotation, - currentPose.getRotation()); - } - - @Override - public boolean isDone() { - if (trajectory == null) return true; - if (trajectory.isDone()) { - System.out.println("Done with trajectory, error: " + Math.hypot(xController.error, yController.error)); - return true; - } - return false; - } - - @Override - public void reset() { - xController.reset(); - yController.reset(); - thetaController.reset(); - - trajectory = null; - currentPose = null; - currentSpeeds = null; - - if (timer != null) { - timer.stop(); - timer.reset(); - } - } - - @Override - public RedTrajectory getTrajectory() { - return trajectory; - } -} diff --git a/src/main/java/frc/robot/lib/swerve/ProfiledPIDHolonomicDriveController.java b/src/main/java/frc/robot/lib/swerve/ProfiledPIDHolonomicDriveController.java deleted file mode 100644 index 354dfc2..0000000 --- a/src/main/java/frc/robot/lib/swerve/ProfiledPIDHolonomicDriveController.java +++ /dev/null @@ -1,149 +0,0 @@ -package frc.robot.lib.swerve; - -import frc.robot.lib.control.ControlConstants.*; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants; -import frc.robot.lib.control.PIDVController; -import frc.robot.lib.control.ProfiledPIDVController; -import frc.robot.lib.trajectory.RedTrajectory; - -@Deprecated -public class ProfiledPIDHolonomicDriveController implements DriveController { - private final ProfiledPIDVController xController; - private final ProfiledPIDVController yController; - private final ProfiledPIDVController thetaController; - private double accelConstant; - - private RedTrajectory trajectory; - private Pose2d currentPose; - private ChassisSpeeds currentSpeeds; - - private Timer timer = null; - - static Field2d field = new Field2d(); - static { - SmartDashboard.putData("debug", field); - } - - /** - * A drive controller that works with 2 {@link PIDVController}s for translation and one {@link ProfiledPIDVController} for rotation. - * @param translationConstants The {@link PIDFConstants} for the translation of the robot. - * @param rotationConstants The {@link ProfiledPIDFConstants} for the rotation of the robot. - * @param accelConstant The acceleration feedforwards (useful for traversing sharp turns on a trajectory). - */ - public ProfiledPIDHolonomicDriveController(ProfiledPIDFConstants translationConstants, ProfiledPIDFConstants rotationConstants, double accelConstant) { - xController = new ProfiledPIDVController(translationConstants); - yController = new ProfiledPIDVController(translationConstants); - thetaController = new ProfiledPIDVController(rotationConstants); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - this.accelConstant = accelConstant; - timer = new Timer(); - } - - @Override - public void setTrajectory(RedTrajectory trajectory) { - this.trajectory = trajectory; - timer.reset(); - timer.start(); - } - - public void setRobotState(Pose2d pose, Twist2d speeds) { - this.currentPose = pose; - this.currentSpeeds = new ChassisSpeeds(speeds.dx, speeds.dy, speeds.dtheta); - } - - public void setInput(Pair current) { - setRobotState(current.getFirst(), current.getSecond()); - } - - public ChassisSpeeds getOutput() { - if (trajectory == null || currentPose == null || currentSpeeds == null || trajectory.isDone()) { - return new ChassisSpeeds(); - } - - RedTrajectory.State targetState = trajectory.advanceTo(timer.get()); - - double vxFF = targetState.speeds.vxMetersPerSecond; - double vyFF = targetState.speeds.vyMetersPerSecond; - - double xAccelFF = MathUtil.applyDeadband( - targetState.accels.ax, - Constants.Drive.MAX_ACCEL * 0.5); - double yAccelFF = MathUtil.applyDeadband( - targetState.accels.ay, - Constants.Drive.MAX_ACCEL * 0.5); - - double angularAccel = MathUtil.applyDeadband( - targetState.accels.alpha, - Constants.Drive.MAX_ROTATION_ACCEL * 0.5); - xAccelFF += -angularAccel * targetState.pose.getRotation().getSin(); - yAccelFF += angularAccel * targetState.pose.getRotation().getCos(); - - xController.setTarget(targetState.pose.getX()); - yController.setTarget(targetState.pose.getY()); - - xController.setFeedforward(vxFF); - yController.setFeedforward(vyFF); - - xController.setInput(new Pair(currentPose.getX(), currentSpeeds.vxMetersPerSecond)); - yController.setInput(new Pair(currentPose.getY(), currentSpeeds.vyMetersPerSecond)); - - double vx = xController.getOutput(); - double vy = yController.getOutput(); - - thetaController.setTarget(targetState.pose.getRotation().getRadians()); - thetaController.setFeedforward(targetState.speeds.omegaRadiansPerSecond); - thetaController.setInput( - new Pair( - currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond)); - - double rotation = thetaController.getOutput(); - - field.setRobotPose(targetState.pose); - SmartDashboard.putData("debug", field); - - return ChassisSpeeds.fromFieldRelativeSpeeds( - vx + xAccelFF * accelConstant, - vy + yAccelFF * accelConstant, - rotation, - currentPose.getRotation()); - } - - @Override - public boolean isDone() { - if (trajectory == null) return true; - if (trajectory.isDone()) { - System.out.println("Done with trajectory, error: " + Math.hypot(xController.error, yController.error)); - return true; - } - return false; - } - - @Override - public void reset() { - xController.reset(); - yController.reset(); - thetaController.reset(); - - trajectory = null; - currentPose = null; - currentSpeeds = null; - - if (timer != null) { - timer.stop(); - timer.reset(); - } - } - - @Override - public RedTrajectory getTrajectory() { - return trajectory; - } -} diff --git a/src/main/java/frc/robot/lib/swerve/SwerveModuleConstants.java b/src/main/java/frc/robot/lib/swerve/SwerveModuleConstants.java deleted file mode 100644 index 70ab187..0000000 --- a/src/main/java/frc/robot/lib/swerve/SwerveModuleConstants.java +++ /dev/null @@ -1,28 +0,0 @@ -package frc.robot.lib.swerve; - -import edu.wpi.first.math.geometry.Rotation2d; - -public class SwerveModuleConstants { - public final int driveMotorID; - public final int angleMotorID; - public final int cancoderID; - public final Rotation2d angleOffset; - public final boolean driveInvert; - public final boolean angleInvert; - - /** - * Swerve Module Constants to be used when creating swerve modules. - * @param driveMotorID - * @param angleMotorID - * @param canCoderID - * @param angleOffset - */ - public SwerveModuleConstants(int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset, boolean driveInvert, boolean angleInvert) { - this.driveMotorID = driveMotorID; - this.angleMotorID = angleMotorID; - this.cancoderID = canCoderID; - this.angleOffset = angleOffset; - this.driveInvert = driveInvert; - this.angleInvert = angleInvert; - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 2d74a84..6de0650 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -84,9 +84,9 @@ public class CtreDriveConstants { // This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.5714285714285716; - private static final double kDriveGearRatio = 6.122448979591837; - private static final double kSteerGearRatio = 21.428571428571427; - private static final Distance kWheelRadius = Inches.of(2); + public static final double kDriveGearRatio = 6.122448979591837; + public static final double kSteerGearRatio = 21.428571428571427; + public static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; From 4e698d89a27a52b14454cd6345ee136cd70aee5b Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Fri, 10 Oct 2025 21:55:03 -0700 Subject: [PATCH 02/36] Remove WheelTracker, Pigeon, SwerveModule, and TalonFxManager from codebase. --- .../frc/robot/subsystems/TalonFXManager.java | 172 -------- .../frc/robot/subsystems/drive/Pigeon.java | 132 ------- .../robot/subsystems/drive/SwerveModule.java | 369 ------------------ .../robot/subsystems/drive/WheelTracker.java | 280 ------------- 4 files changed, 953 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/TalonFXManager.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/Pigeon.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/SwerveModule.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/WheelTracker.java diff --git a/src/main/java/frc/robot/subsystems/TalonFXManager.java b/src/main/java/frc/robot/subsystems/TalonFXManager.java deleted file mode 100644 index e92723b..0000000 --- a/src/main/java/frc/robot/subsystems/TalonFXManager.java +++ /dev/null @@ -1,172 +0,0 @@ -package frc.robot.subsystems; - -import java.util.Map; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import frc.robot.Constants; -import frc.robot.lib.drivers.Phoenix6Util; - -import java.util.HashMap; - -public class TalonFXManager { - /** - * Holds all created TalonFX motor instances, keyed by a unique name - */ - private static final Map motors = new HashMap<>(); - - /** - * Creates and registers a motor with the given name, CAN ID, and applies a named default config. - * @param name Unique identifier for this motor - * @param canId CAN device ID - * @param configKey Key of a default configuration to apply - * @return The newly created TalonFX instance - */ - public static TalonFX createMotor(String name, int canId, String configKey) { - if (motors.containsKey(name)) { - throw new IllegalArgumentException("Motor with name '" + name + "' already exists."); - } - TalonFX motor = new TalonFX(canId, "CV"); - TalonFXConfiguration config = DefaultConfigs.getConfig(configKey); - Phoenix6Util.checkErrorAndRetry(()-> - motor.getConfigurator().apply(config, Constants.LONG_CANT_TIMEOUT_MS)); - motors.put(name, motor); - return motor; - } - - /** - * Retrieves a previously registered motor by its name. - * @param name Unique identifier - * @return TalonFX instance - */ - public static TalonFX getMotor(String name) { - if (!motors.containsKey(name)) { - throw new IllegalArgumentException("No motor registered under name '" + name + "'."); - } - return motors.get(name); - } - - /** - * Applies one of the default configs to an existing motor. - * @param name Registered motor name - * @param configKey Key of the default configuration - */ - public static void applyConfig(String name, String configKey) { - TalonFX motor = getMotor(name); - TalonFXConfiguration config = DefaultConfigs.getConfig(configKey); - Phoenix6Util.checkErrorAndRetry(()-> - motor.getConfigurator().apply(config, Constants.LONG_CANT_TIMEOUT_MS)); - } - - /** - * Static container for default motor configurations. - */ - public static class DefaultConfigs { - private static final Map configs = new HashMap<>(); - - public static TalonFXConfiguration AngleMotorConfig () { - TalonFXConfiguration ANGLE_CONFIG = new TalonFXConfiguration(); - ANGLE_CONFIG.Slot0.kP = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kP; - ANGLE_CONFIG.Slot0.kI = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kI; - ANGLE_CONFIG.Slot0.kD = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kD; - ANGLE_CONFIG.Slot0.kS = 0.0; - ANGLE_CONFIG.Slot0.kV = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kF; - - ANGLE_CONFIG.CurrentLimits.StatorCurrentLimitEnable = true; - ANGLE_CONFIG.CurrentLimits.StatorCurrentLimit = Constants.Drive.ANGLE_CURRENT_LIMIT; - - ANGLE_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true; - ANGLE_CONFIG.CurrentLimits.SupplyCurrentLimit = Constants.Drive.ANGLE_CURRENT_LIMIT; - ANGLE_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = Constants.Drive.ANGLE_CURRENT_THRESHOLD; - ANGLE_CONFIG.CurrentLimits.SupplyCurrentLowerTime = Constants.Drive.ANGLE_CURRENT_THRESHOLD_TIME; - - ANGLE_CONFIG.Voltage.PeakForwardVoltage = 12.0; - ANGLE_CONFIG.Voltage.PeakReverseVoltage = -12.0; - - ANGLE_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; -// configs.put("Angle", ANGLE_CONFIG); - return ANGLE_CONFIG; - } - - public static TalonFXConfiguration DriveMotorConfig(){ - TalonFXConfiguration DRIVE_CONFIG = new TalonFXConfiguration(); - - DRIVE_CONFIG.Slot0.kP = Constants.Drive.DRIVE_MOTOR_PIDF_CONSTANTS.kP; - DRIVE_CONFIG.Slot0.kI = Constants.Drive.DRIVE_MOTOR_PIDF_CONSTANTS.kI; - DRIVE_CONFIG.Slot0.kD = Constants.Drive.DRIVE_MOTOR_PIDF_CONSTANTS.kD; - DRIVE_CONFIG.Slot0.kS = 0.1; - DRIVE_CONFIG.Slot0.kV = Constants.Drive.DRIVE_MOTOR_PIDF_CONSTANTS.kF; - - DRIVE_CONFIG.CurrentLimits.StatorCurrentLimitEnable = true; - DRIVE_CONFIG.CurrentLimits.StatorCurrentLimit = Constants.Drive.DRIVE_CURRENT_LIMIT; - - DRIVE_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true; - DRIVE_CONFIG.CurrentLimits.SupplyCurrentLimit = Constants.Drive.DRIVE_CURRENT_LIMIT; - DRIVE_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = Constants.Drive.DRIVE_CURRENT_THRESHOLD; - DRIVE_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 0.5; - - DRIVE_CONFIG.Voltage.PeakForwardVoltage = 12.0; - DRIVE_CONFIG.Voltage.PeakReverseVoltage = -12.0; - - DRIVE_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - DRIVE_CONFIG.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = Constants.Drive.OPEN_LOOP_RAMP; - DRIVE_CONFIG.OpenLoopRamps.VoltageOpenLoopRampPeriod = Constants.Drive.OPEN_LOOP_RAMP; - //configs.put("Drive", DRIVE_CONFIG); - return DRIVE_CONFIG; - } - { - TalonFXConfiguration DEFAULT_CONFIG = new TalonFXConfiguration(); - DEFAULT_CONFIG.Slot0.kP = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kP; - DEFAULT_CONFIG.Slot0.kI = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kI; - DEFAULT_CONFIG.Slot0.kD = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kD; - DEFAULT_CONFIG.Slot0.kS = 0.0; - DEFAULT_CONFIG.Slot0.kV = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kF; - - DEFAULT_CONFIG.CurrentLimits.StatorCurrentLimitEnable = true; - DEFAULT_CONFIG.CurrentLimits.StatorCurrentLimit = Constants.Drive.ANGLE_CURRENT_LIMIT; - - DEFAULT_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true; - DEFAULT_CONFIG.CurrentLimits.SupplyCurrentLimit = Constants.Drive.ANGLE_CURRENT_LIMIT; - DEFAULT_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = Constants.Drive.ANGLE_CURRENT_THRESHOLD; - DEFAULT_CONFIG.CurrentLimits.SupplyCurrentLowerTime = Constants.Drive.ANGLE_CURRENT_THRESHOLD_TIME; - - DEFAULT_CONFIG.Voltage.PeakForwardVoltage = 12.0; - DEFAULT_CONFIG.Voltage.PeakReverseVoltage = -12.0; - - DEFAULT_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - configs.put("Default", DEFAULT_CONFIG); - } - - /** - * Retrieves a named default config, or throws if not found. - */ - public static TalonFXConfiguration getConfig(String key) { - if (key.matches("Angle")){ - return AngleMotorConfig(); - }else if (key.matches("Drive")){ - return DriveMotorConfig(); - }else{ - return null; - } - // TalonFXConfiguration cfg = configs.get(key); - // if (cfg == null) { - // throw new IllegalArgumentException("No default config found for key '" + key + "'."); - // } - // return cfg; - } - - /** - * Allows registration of additional default configs at runtime. - */ - public static void registerConfig(String key, TalonFXConfiguration config) { - if (configs.containsKey(key)) { - throw new IllegalArgumentException("Config key '" + key + "' already registered."); - } - configs.put(key, config); - } - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/Pigeon.java b/src/main/java/frc/robot/subsystems/drive/Pigeon.java deleted file mode 100644 index e1d1327..0000000 --- a/src/main/java/frc/robot/subsystems/drive/Pigeon.java +++ /dev/null @@ -1,132 +0,0 @@ -package frc.robot.subsystems.drive; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.sim.Pigeon2SimState; - -import frc.robot.Constants; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - - -public class Pigeon extends SubsystemBase { - private static Pigeon pigeonInstance; - public static Pigeon getInstance() { - if (pigeonInstance == null) { - pigeonInstance = new Pigeon(Constants.Port.PIGEON); - } - return pigeonInstance; - } - - private final Pigeon2 gyro; - - private boolean inverted = Constants.Drive.INVERT_GYRO; - private Rotation2d yawAdjustmentAngle = new Rotation2d(); - private Rotation2d rollAdjustmentAngle = new Rotation2d(); - private Rotation2d pitchAdjustmentAngle = new Rotation2d(); - - private double simAngularVelocity = 0.0; - - private Pigeon(Constants.Port constants) { - gyro = new Pigeon2(constants.id, constants.bus); - gyro.getConfigurator().apply(new Pigeon2Configuration()); - } - - public Rotation2d getYaw() { - Rotation2d angle = getUnadjustedYaw().rotateBy(yawAdjustmentAngle.unaryMinus()); - if (inverted) { - return angle.unaryMinus(); - } - return angle; - } - - public Rotation2d getRoll() { - return getUnadjustedRoll().rotateBy(rollAdjustmentAngle.unaryMinus()); - } - - public Rotation2d getPitch() { - return getUnadjustedPitch().rotateBy(pitchAdjustmentAngle.unaryMinus()).unaryMinus(); - } - - /** - * Sets the yaw register to read the specified value. - * - * @param angleDeg New yaw in degrees - */ - public void setYaw(double angleDeg) { - yawAdjustmentAngle = Rotation2d.fromDegrees(getYawStatusSignal().getValueAsDouble()) - .rotateBy(Rotation2d.fromDegrees(angleDeg).unaryMinus()); - } - - /** - * Sets the roll register to read the specified value. - * - * @param angleDeg New yaw in degrees - */ - public void setRoll(double angleDeg) { - rollAdjustmentAngle = - getUnadjustedRoll().rotateBy(Rotation2d.fromDegrees(angleDeg).unaryMinus()); - } - - /** - * Sets the roll register to read the specified value. - * - * @param angleDeg New yaw in degrees - */ - public void setPitch(double angleDeg) { - pitchAdjustmentAngle = - getUnadjustedPitch().rotateBy(Rotation2d.fromDegrees(angleDeg).unaryMinus()); - System.out.println("Reset gyro to " + getPitch().getDegrees()); - } - - public Rotation2d getUnadjustedYaw() { - return Rotation2d.fromDegrees( - BaseStatusSignal.getLatencyCompensatedValueAsDouble(getYawStatusSignal(), getRateStatusSignal())); - } - - public Rotation2d getUnadjustedPitch() { - return Rotation2d.fromDegrees(gyro.getRoll().getValueAsDouble()); - } - - public Rotation2d getUnadjustedRoll() { - return Rotation2d.fromDegrees(gyro.getPitch().getValueAsDouble()); - } - - public StatusSignal getYawStatusSignal() { - return gyro.getYaw(); - } - - public StatusSignal getRateStatusSignal() { - return gyro.getAngularVelocityZDevice(); - } - - public void setSimAngularVelocity(double angularVelocity) { - this.simAngularVelocity = angularVelocity; - } - - @Override - public void simulationPeriodic() { - Pigeon2SimState gyroSimState = gyro.getSimState(); - - gyroSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); - - Rotation2d angleChange = Rotation2d.fromRadians(simAngularVelocity * TimedRobot.kDefaultPeriod); - Rotation2d angle = getUnadjustedYaw().plus(angleChange); - gyroSimState.setRawYaw(angle.getDegrees()); - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Subsystem"); - builder.addDoubleProperty("/RollDegrees", this.getRoll()::getDegrees, null); - builder.addDoubleProperty("/PitchDegrees", this.getPitch()::getDegrees, null); - builder.addDoubleProperty("/YawDegrees", this.getYaw()::getDegrees, null); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java deleted file mode 100644 index 09721a6..0000000 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ /dev/null @@ -1,369 +0,0 @@ -package frc.robot.subsystems.drive; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.controls.*; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.sim.CANcoderSimState; -import com.ctre.phoenix6.sim.TalonFXSimState; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.Units; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants; -import frc.robot.lib.util.Conversions; -import frc.robot.subsystems.TalonFXManager; - -public class SwerveModule extends SubsystemBase { - public final String name; - private final BaseStatusSignal[] signals = new BaseStatusSignal[4]; - - private final SwerveModuleIO io = new SwerveModuleIO(); - public static class SwerveModuleIO { - public double rotationPosition = 0.0; - public double rotationVelocity = 0.0; - public double drivePosition = 0.0; - public double driveVelocity = 0.0; - - SwerveModuleState currentState = new SwerveModuleState(); - SwerveModuleState targetState = new SwerveModuleState(); - double targetDriveVelocity = 0; - ControlRequest angleRequest = new NeutralOut(); - ControlRequest driveRequest = new NeutralOut(); - } - - private State state = State.DEFAULT; - public static enum State { - DEFAULT, - ANGLE_TUNING, - DRIVE_TUNING - } - - private final TalonFX angleMotor; - private final TalonFX driveMotor; - private final CANcoder cancoder; - - private final DCMotorSim angleMotorSim; - private final DCMotorSim driveMotorSim; - - public SwerveModule(Constants.Drive.ModuleConstants constants, CANcoder cancoder) { - name = constants.name(); - setName("Module " + name); - - angleMotor = TalonFXManager.createMotor(name + " Angle Motor", constants.angleMotorID, "Angle"); - driveMotor = TalonFXManager.createMotor(name + " Drive Motor", constants.driveMotorID, "Drive"); - - driveMotorSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(1), 0.001, Constants.Drive.DRIVE_GEAR_RATIO), - DCMotor.getKrakenX60Foc(1)); - angleMotorSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(1), 0.001, Constants.Drive.ANGLE_GEAR_RATIO), - DCMotor.getFalcon500Foc(1)); - - this.cancoder = cancoder; - - signals[0] = driveMotor.getRotorPosition(); - signals[1] = driveMotor.getRotorVelocity(); - signals[2] = angleMotor.getRotorPosition(); - signals[3] = angleMotor.getRotorVelocity(); - SmartDashboard.putData(this); - } - - /** - * Sets the target state of this module. - * @param state The target {@code SwerveModuleState}. - */ - public void setTargetState(SwerveModuleState state) { - io.targetState = state; - } - - @Override - public void periodic() { - switch (state) { - case DEFAULT: - setVelocity(io.targetState); - break; - case ANGLE_TUNING: - break; - case DRIVE_TUNING: - break; - default: - setVelocity(io.targetState); - break; - } - - angleMotor.setControl(io.angleRequest); - driveMotor.setControl(io.driveRequest); - - Rotation2d angleRotation = new Rotation2d(cancoder.getPosition().getValue()); - double wheelSpeed = Conversions.RPSToMPS( - driveMotor.getVelocity().getValueAsDouble(), - Constants.Drive.WHEEL_CIRCUMFERENCE, - Constants.Drive.DRIVE_GEAR_RATIO); - - io.currentState = new SwerveModuleState(wheelSpeed, angleRotation); - } - - @Override - public void simulationPeriodic() { - TalonFXSimState mDriveMotorSimState = driveMotor.getSimState(); - TalonFXSimState mAngleMotorSimState = angleMotor.getSimState(); - CANcoderSimState angleEncoderSimState = cancoder.getSimState(); - double batteryVoltage = RobotController.getBatteryVoltage(); - mDriveMotorSimState.setSupplyVoltage(batteryVoltage); - mAngleMotorSimState.setSupplyVoltage(batteryVoltage); - angleEncoderSimState.setSupplyVoltage(batteryVoltage); - - driveMotorSim.setInputVoltage(mDriveMotorSimState.getMotorVoltageMeasure().in(Units.Volts)); - driveMotorSim.update(TimedRobot.kDefaultPeriod); - mDriveMotorSimState.setRawRotorPosition( - driveMotorSim.getAngularPosition().times(Constants.Drive.DRIVE_GEAR_RATIO)); - mDriveMotorSimState.setRotorVelocity( - driveMotorSim.getAngularVelocity().times(Constants.Drive.DRIVE_GEAR_RATIO)); - - angleMotorSim.setInputVoltage(mAngleMotorSimState.getMotorVoltageMeasure().in(Units.Volts)); - angleMotorSim.update(Constants.DT); - mAngleMotorSimState.setRawRotorPosition( - angleMotorSim.getAngularPosition().times(Constants.Drive.ANGLE_GEAR_RATIO)); - mAngleMotorSimState.setRotorVelocity( - angleMotorSim.getAngularVelocity().times(Constants.Drive.ANGLE_GEAR_RATIO)); - - angleEncoderSimState.setRawPosition( - angleMotorSim.getAngularPosition()); - angleEncoderSimState.setVelocity( - angleMotorSim.getAngularVelocity()); - } - - /** - * Gets the current state of the module. - */ - public SwerveModuleState getState() { - return io.currentState; - } - - /** - * Sets the target velocities of the swerve module. - * @param targetState The target {@code SwerveModuleState}. - */ - public void setVelocity(SwerveModuleState targetState) { - boolean flip = setSteeringAngleOptimized(targetState.angle.unaryMinus()); - io.targetDriveVelocity = targetState.speedMetersPerSecond * (flip ? -1.0 : 1.0); - - double rotorSpeed = Conversions.MPSToRPS( - io.targetDriveVelocity, - Constants.Drive.WHEEL_CIRCUMFERENCE, - Constants.Drive.DRIVE_GEAR_RATIO); - - if (Math.abs(rotorSpeed) < 0.01) { - io.driveRequest = new NeutralOut(); - } else { - io.driveRequest = new VelocityVoltage(rotorSpeed); - } - } - - /** - * Sets the angle of the swerve module, and flips it if it is more efficient to. - * @param targetAngle The desire angle of the swerve module. - * @return Whether to flip the drive direction. - */ - private boolean setSteeringAngleOptimized(Rotation2d targetAngle) { - boolean flip = false; - - double currentAngleDegrees = cancoder.getPosition().getValueAsDouble() * 360.0; - Rotation2d currentAngle = Rotation2d.fromDegrees(currentAngleDegrees); - - Rotation2d delta = targetAngle.minus(currentAngle); - double deltaDegrees = delta.getDegrees(); - - if (deltaDegrees > 90.0) { - deltaDegrees -= 180.0; - flip = true; - } else if (deltaDegrees < -90.0) { - deltaDegrees += 180.0; - flip = true; - } - - setSteeringAngleRaw(currentAngleDegrees + deltaDegrees); - return flip; - } - - /** - * Sets the target angle position of the swerve module. - * @param angleDegrees The angle position, in degrees. - */ - private void setSteeringAngleRaw(double angleDegrees) { - double rotorPosition = Conversions.degreesToRotation(angleDegrees, Constants.Drive.ANGLE_GEAR_RATIO); - io.angleRequest = new PositionVoltage(rotorPosition); - } - - public synchronized void refreshSignals() { - io.rotationVelocity = angleMotor.getRotorVelocity().getValueAsDouble(); - io.driveVelocity = driveMotor.getRotorVelocity().getValueAsDouble(); - - io.rotationPosition = BaseStatusSignal.getLatencyCompensatedValueAsDouble( - angleMotor.getRotorPosition(), angleMotor.getRotorVelocity()); - io.drivePosition = BaseStatusSignal.getLatencyCompensatedValueAsDouble( - driveMotor.getRotorPosition(), driveMotor.getRotorVelocity()); - } - - /** - * Gets the distance the drive wheel has traversed. - * @return The distance, in meters. - */ - public double getDriveDistance() { - return Conversions.rotationsToMeters( - io.drivePosition, - Constants.Drive.WHEEL_CIRCUMFERENCE, - Constants.Drive.DRIVE_GEAR_RATIO); - } - - /** - * Gets the speed of the drive wheel; - * @return The speed, in meters per second. - */ - public double getDriveSpeed() { - return Conversions.RPSToMPS( - io.driveVelocity, - Constants.Drive.WHEEL_CIRCUMFERENCE, - Constants.Drive.DRIVE_GEAR_RATIO); - } - - /** - * Gets the current angle of the swerve module. - * @return The angle, as a {@code Rotation2d}. - */ - public Rotation2d getModuleAngle() { - return Rotation2d.fromDegrees(getModuleAngleDegrees()); - } - - /** - * Gets the current angle of the swerve module. - * @return The angle, in degrees. - */ - public double getModuleAngleDegrees() { - return Conversions.rotationsToDegrees(io.rotationPosition, Constants.Drive.ANGLE_GEAR_RATIO); - } - - /** - * Gets the speed of the angle. - * @return The speed, in degrees per second. - */ - public double getModuleAngleSpeed() { - return Units.RotationsPerSecond.of(io.rotationVelocity).in(Units.DegreesPerSecond); - } - - /** - * Retrieves the status signals. - * @return The status signals from this swerve module. - */ - public BaseStatusSignal[] getUsedStatusSignals() { - return signals; - } - - /** - * The system identification routine for the angle motor. - * @return The angle motor routine. - */ - public SysIdRoutine angleRoutine() { - return new SysIdRoutine( - new SysIdRoutine.Config( - Constants.Calibration.AngleMotor.RAMP_RATE, - Constants.Calibration.AngleMotor.DYNAMIC_VOLTAGE, - null, - recordState -> SignalLogger.writeString("/Sysid/Angle/State", recordState.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> io.angleRequest = new VoltageOut(volts), null, this)); - } - - /** - * The system identification routine for the drive motor. - * @return The drive motor routine. - */ - public SysIdRoutine driveRoutine() { - return new SysIdRoutine( - new SysIdRoutine.Config( - Constants.Calibration.DriveMotor.RAMP_RATE, - Constants.Calibration.DriveMotor.DYNAMIC_VOLTAGE, - null, - recordState -> SignalLogger.writeString("/Sysid/Drive/State", recordState.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> io.driveRequest = new VoltageOut(volts), null, this)); - } - - /** - * The command to run a sysid routine of the specified type and direction. - * @param type The type of routine (DRIVE or ANGLE). - * @param direction The direction to run in. - * @return The command. - */ - public Command sysIdDynamic(State type, SysIdRoutine.Direction direction) { - return switch (type) { - case ANGLE_TUNING -> - Commands.runOnce(() -> state = type) - .andThen(angleRoutine().dynamic(direction)) - .andThen(Commands.runOnce(() -> state = State.DEFAULT)); - case DRIVE_TUNING -> - Commands.runOnce(() -> state = type) - .andThen(driveRoutine().dynamic(direction)) - .andThen(Commands.runOnce(() -> state = State.DEFAULT)); - default -> { - DriverStation.reportWarning("Did not provide a valid tuning type", false); - yield Commands.none(); - } - }; - } - - /** - * The command to run a sysid routine of the specified type and direction. - * @param type The type of routine (DRIVE or ANGLE). - * @param direction The direction to run in. - * @return The command. - */ - public Command sysIdQuasistatic(State type, SysIdRoutine.Direction direction) { - return switch (type) { - case ANGLE_TUNING -> - Commands.runOnce(() -> state = type) - .andThen(Commands.runOnce( - () -> io.driveRequest = new NeutralOut())) - .andThen(angleRoutine().quasistatic(direction)) - .andThen(Commands.runOnce(() -> state = State.DEFAULT)); - case DRIVE_TUNING -> - Commands.runOnce(() -> state = type) - .andThen(Commands.runOnce( - () -> io.angleRequest = new NeutralOut())) - .andThen(driveRoutine().quasistatic(direction)) - .andThen(Commands.runOnce(() -> state = State.DEFAULT)); - default -> { - DriverStation.reportWarning("Did not provide a valid tuning type", false); - yield Commands.none(); - } - }; - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.addStringProperty("/State", () -> state.name(), null); - builder.addDoubleProperty("/AngleDegrees", () -> getModuleAngleDegrees(), null); - builder.addDoubleProperty("/AngleSpeedDegreesPerSecond", () -> getModuleAngleSpeed(), null); - builder.addDoubleProperty("/DriveSpeedMetersPerSecond", () -> getDriveSpeed(), null); - builder.addDoubleProperty("/TargetAngleDegrees", () -> io.targetState.angle.getDegrees(), null); - builder.addDoubleProperty("/TargetDriveSpeedMetersPerSecond", () -> io.targetState.speedMetersPerSecond, null); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/WheelTracker.java b/src/main/java/frc/robot/subsystems/drive/WheelTracker.java deleted file mode 100644 index 1240b9b..0000000 --- a/src/main/java/frc/robot/subsystems/drive/WheelTracker.java +++ /dev/null @@ -1,280 +0,0 @@ -package frc.robot.subsystems.drive; - -import com.ctre.phoenix6.BaseStatusSignal; -import frc.robot.Constants; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import java.util.ArrayList; -import java.util.List; - -public class WheelTracker { - private final SwerveModule[] modules; - private final Pigeon pigeon; - - private WheelProperties[] wheelProperties = new WheelProperties[4]; - private Pose2d robotPose = new Pose2d(10,10,new Rotation2d(0)); - private Translation2d robotVelocity = new Translation2d(0, 0); - private BaseStatusSignal[] allSignals; - - private double robotHeading; - - private double timestamp; - - private final OdometryThread odometryThread; - - private Field2d robotField = new Field2d(); - - public WheelTracker(SwerveModule[] modules) { - if (modules.length != 4) { - throw new IllegalArgumentException("Odometry needs 4 modules to run"); - } - - this.modules = modules; - pigeon = Pigeon.getInstance(); - - for (int i = 0; i < wheelProperties.length; i++) { - WheelProperties w = new WheelProperties(); - Translation2d robotToWheel = new Translation2d( - Constants.Drive.MODULE_LOCATIONS[i].getX(), - Constants.Drive.MODULE_LOCATIONS[i].getY()); - w.startingPosition = robotToWheel; - wheelProperties[i] = w; - } - - resetModulePoses(); - - allSignals = new BaseStatusSignal[(4 * 4) + 2]; - for (int i = 0; i < 4; ++i) { - var signals = modules[i].getUsedStatusSignals(); - allSignals[(i * 4) + 0] = signals[0]; - allSignals[(i * 4) + 1] = signals[1]; - allSignals[(i * 4) + 2] = signals[2]; - allSignals[(i * 4) + 3] = signals[3]; - } - allSignals[allSignals.length - 2] = pigeon.getYawStatusSignal(); - allSignals[allSignals.length - 1] = pigeon.getRateStatusSignal(); - - for (BaseStatusSignal sig : allSignals) { - sig.setUpdateFrequency(50); - } - odometryThread = new OdometryThread(); - odometryThread.setDaemon(true); - odometryThread.start(); - - robotField.setRobotPose(robotPose); - - SmartDashboard.putData("WheelTracker", robotField); - } - private class OdometryThread extends Thread { - @Override - public void run() { - while (true) { - try { - BaseStatusSignal.waitForAll(1.0, allSignals); - - for (SwerveModule m : modules) { - m.refreshSignals(); // No downside to refreshing io reads from multiple threads - } - - robotHeading = pigeon.getYaw().getRadians(); - updateRobotPose(Timer.getFPGATimestamp()); - } catch (Exception e) { - e.printStackTrace(); - System.out.println("Wheeltracker failed, see above error"); - } - } - } - } - - private Pose2d last_velocity_sample = new Pose2d(); - private double last_sample_timestamp = 0.0; - - private void updateRobotPose(double timestamp) { - double x = 0.0; - double y = 0.0; - Rotation2d heading = Rotation2d.fromRadians(robotHeading); - - double avg_delta = 0.0; - double[] deltas = new double[4]; - for (int i = 0; i < modules.length; i++) { - SwerveModule m = modules[i]; - WheelProperties w = wheelProperties[i]; - updateWheelOdometry(m, w); - double delta = w.estimatedRobotPose - .getTranslation() - .plus(robotPose.getTranslation().unaryMinus()) - .getNorm(); - deltas[i] = delta; - avg_delta += delta; - } - avg_delta /= 4; - - int min__dev_idx = 0; - double min_dev = Double.MAX_VALUE; - List accurateModules = new ArrayList<>(); - for (int i = 0; i < deltas.length; i++) { - WheelProperties w = wheelProperties[i]; - double dev = Math.abs(deltas[i] - avg_delta); - if (dev < min_dev) { - min_dev = dev; - min__dev_idx = i; - } - if (dev <= 0.01) { - accurateModules.add(w); - } - } - - if (accurateModules.isEmpty()) { - accurateModules.add(wheelProperties[min__dev_idx]); - } - - int n = accurateModules.size(); - - for (WheelProperties w : accurateModules) { - x += w.estimatedRobotPose.getTranslation().getX(); - y += w.estimatedRobotPose.getTranslation().getY(); - } - final Pose2d new_pose = new Pose2d(new Translation2d(x / n, y / n), heading); - - // Velocity calcs - double sample_window = timestamp - last_sample_timestamp; - if (sample_window > 0.02) { - final Translation2d translation = (new_pose.transformBy(new Transform2d( - new Translation2d(-last_velocity_sample.getTranslation().getX(), - -last_velocity_sample.getTranslation().getY()), - last_velocity_sample.getRotation().unaryMinus())).getTranslation()); - robotVelocity = new Translation2d( - translation.getX() * (1.0 / sample_window), - translation.getY() * (1.0 / sample_window) - ); - last_sample_timestamp = timestamp; - last_velocity_sample = new_pose; - } - - robotPose = new_pose; - - robotField.setRobotPose(new_pose); - - resetModulePoses(robotPose); - } - - - private void updateWheelOdometry(SwerveModule module, WheelProperties props) { - double currentEncDistance = module.getDriveDistance(); - double deltaEncDistance = currentEncDistance - props.previousEncDistance; - Rotation2d wheelAngle = module.getModuleAngle().rotateBy(Rotation2d.fromRadians(-robotHeading)); - Translation2d deltaPosition = new Translation2d( - wheelAngle.getCos() * deltaEncDistance, - -wheelAngle.getSin() * deltaEncDistance); - double xCorrectionFactor = 1.0; - double yCorrectionFactor = 1.0; - - if (Math.signum(deltaPosition.getX()) == 1.0) { - xCorrectionFactor = (8.6 / 9.173); - // TODO: Put this in constants - - } else if (Math.signum(deltaPosition.getX()) == -1.0) { - xCorrectionFactor = (8.27 / 9.173); - } - - if (Math.signum(deltaPosition.getY()) == 1.0) { - yCorrectionFactor = (3.638 / 4.0); - } else if (Math.signum(deltaPosition.getY()) == -1.0) { - yCorrectionFactor = (3.660 / 4.0); - } - - deltaPosition = new Translation2d(deltaPosition.getX() * xCorrectionFactor, deltaPosition.getY() * yCorrectionFactor); - Translation2d updatedPosition = props.position.plus(deltaPosition); - Pose2d wheelPose = new Pose2d(updatedPosition, Rotation2d.fromRadians(robotHeading)); - props.estimatedRobotPose = new Pose2d(wheelPose.getTranslation().plus(props.startingPosition.unaryMinus()), wheelPose.getRotation()); - - props.position = updatedPosition; - props.previousEncDistance = currentEncDistance; - } - - public void resetModulePoses(Pose2d mRobotPose) { - for (int i = 0; i < modules.length; i++) { - WheelProperties props = wheelProperties[i]; - Translation2d modulePosition = new Pose2d(mRobotPose.getTranslation().plus(props.startingPosition),mRobotPose.getRotation()) - .getTranslation(); - props.position = modulePosition; - } - } - - private void resetModulePoses() { - for (int i = 0; i < modules.length; i++) { - WheelProperties props = wheelProperties[i]; - props.position = props.startingPosition; - } - } - - public synchronized void resetPose(Pose2d pose) { - robotPose = new Pose2d(pose.getTranslation(), pose.getRotation()); - resetModulePoses(robotPose); - } - - public class WheelProperties { - private double previousEncDistance = 0; - private Translation2d position; - private Translation2d startingPosition; - private Pose2d estimatedRobotPose = new Pose2d(); - } - - public synchronized Pose2d getRobotPose() { - return robotPose; - } - - public synchronized Translation2d getMeasuredVelocity() { - return robotVelocity; - } - - public double getTimestamp() { - return timestamp; - } - - public double wheel0_x() { - return wheelProperties[0].position.getX(); - } - - public double wheel0_y() { - return wheelProperties[0].position.getY(); - } - - public double wheel1_x() { - return wheelProperties[1].position.getX(); - } - - public double wheel1_y() { - return wheelProperties[1].position.getY(); - } - - public double wheel2_x() { - return wheelProperties[2].position.getX(); - } - - public double wheel2_y() { - return wheelProperties[2].position.getY(); - } - - public double wheel3_x() { - return wheelProperties[3].position.getX(); - } - - public double wheel3_y() { - return wheelProperties[3].position.getY(); - } - - public double robot_x() { - return robotPose.getTranslation().getX(); - } - - public double robot_y() { - return robotPose.getTranslation().getY(); - } -} From c208b63c48a2aa4d5d552f6213c8bce4327c3d28 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Fri, 10 Oct 2025 22:49:15 -0700 Subject: [PATCH 03/36] Deprecate RobotState, CancoderManager, AutoPilotCommand and SnapCommand etc --- simgui-ds.json | 5 - src/main/java/frc/robot/Robot.java | 6 +- src/main/java/frc/robot/RobotState.java | 348 ------------------ .../robot/lib/trajectory/RedTrajectory.java | 9 +- .../frc/robot/subsystems/CancoderManager.java | 125 ------- .../drive/commands/AutopilotCommand.java | 88 ----- .../drive/commands/SnapCommand.java | 152 -------- .../drive/commands/TrajectoryCommand.java | 2 +- 8 files changed, 9 insertions(+), 726 deletions(-) delete mode 100644 src/main/java/frc/robot/RobotState.java delete mode 100644 src/main/java/frc/robot/subsystems/CancoderManager.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/commands/SnapCommand.java diff --git a/simgui-ds.json b/simgui-ds.json index 7efccf5..2296812 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -4,11 +4,6 @@ "visible": true } }, - "Keyboard 1 Settings": { - "window": { - "visible": true - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6de2f9b..4b660fb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -40,8 +40,8 @@ public Robot() { } - RobotState.reset(Timer.getFPGATimestamp(), new Pose2d()); - RobotState.resetKalman(); + // RobotState.reset(Timer.getFPGATimestamp(), new Pose2d()); + // RobotState.resetKalman(); Drive.getInstance(); VisionDeviceManager.getInstance(); @@ -81,7 +81,7 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected. */ @Override public void autonomousInit() { - RobotState.setAlliance(DriverStation.getAlliance()); + // RobotState.setAlliance(DriverStation.getAlliance()); autoCommand = autoChooser.getAuto(); if (autoCommand != null) { diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java deleted file mode 100644 index 12a01a1..0000000 --- a/src/main/java/frc/robot/RobotState.java +++ /dev/null @@ -1,348 +0,0 @@ -package frc.robot; - -import java.util.Map; -import java.util.Optional; - -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; -import edu.wpi.first.math.estimator.ExtendedKalmanFilter; -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.math.numbers.*; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.StateSpaceUtil; -import frc.robot.lib.localization.VisionPoseAcceptor; -import frc.robot.lib.util.MovingAverage; -import frc.robot.lib.util.Util; -import frc.robot.lib.util.interpolation.InterpolatingTreeMap; -import frc.robot.lib.util.interpolation.InterpolationUtil; -// import frc.robot.subsystems.drive.LegacyDrive; -import edu.wpi.first.math.interpolation.InverseInterpolator; - - -public class RobotState { - public static Optional mLatestVisionUpdate; - - private static Optional mInitialFieldToOdom = Optional.empty(); - private static InterpolatingTreeMap mOdometryToVehicle; - private static InterpolatingTreeMap mFieldToOdometry; - private static ExtendedKalmanFilter mKalmanFilter; - private static VisionPoseAcceptor mPoseAcceptor; - - private static Twist2d mVehicleVelocityMeasured; - private static Twist2d mVehichleVelocityPredicted; - private static MovingAverage mVehicleVelocityMeasuredFiltered; - - private static boolean mHasRecievedVisionUpdate = false; - private static boolean mIsInAuto = false; - private static Optional mAlliance = null; - - public static double mLastTimestamp = 0; - - /** - * Adds new odometry pose update. - * - * @param now Timestamp of observation. - * @param odometryPose Reported pose from odometry. - * @param measuredVelocity Measured field-relative velocity. - * @param predictedVelocity Predicted field-relative velocity (usually swerve - * setpoint). - */ - public static synchronized void reset(double now, Pose2d initialOdomToVehicle) { - mOdometryToVehicle = new InterpolatingTreeMap( - InverseInterpolator.forDouble(), - InterpolationUtil.poseInterpolator, - Constants.Odometry.OBSERVATION_BUFFER_SIZE); - mOdometryToVehicle.put(now, initialOdomToVehicle); - mFieldToOdometry = new InterpolatingTreeMap( - InverseInterpolator.forDouble(), - InterpolationUtil.translationInterpolator, - Constants.Odometry.OBSERVATION_BUFFER_SIZE); - mFieldToOdometry.put(now, getmInitialFieldToOdom()); - mVehicleVelocityMeasured = new Twist2d(); - mVehichleVelocityPredicted = new Twist2d(); - mVehicleVelocityMeasuredFiltered = new MovingAverage( - 25, new Twist2d(), - (Twist2d x, Twist2d y) -> { return new Twist2d(x.dx + y.dx, x.dy + y.dy, x.dtheta + y.dtheta); }, - (Twist2d x, Integer y) -> { return new Twist2d(x.dx / y, x.dy / y, x.dtheta / y); } - ); - - mLastTimestamp = now; - mLatestVisionUpdate = Optional.empty(); - mPoseAcceptor = new VisionPoseAcceptor(); - } - - /** - * Reconstructs Kalman Filter. - */ - public static synchronized void resetKalman() { - mKalmanFilter = new ExtendedKalmanFilter ( - Nat.N2(), // Dimensions of output (x, y) - Nat.N2(), // Dimensions of predicted error shift (dx, dy) (always 0) - Nat.N2(), // Dimensions of vision (x, y) - (x, u) -> u, // The derivative of the output is predicted shift (always 0) - (x, u) -> x, // The output is position (x, y) - Constants.Odometry.STATE_STD_DEVS, // Standard deviation of position (uncertainty propagation with no vision) - Constants.Odometry.LOCAL_MEASUREMENT_STD_DEVS, // Standard deviation of vision measurements - Constants.DT); - } - - public static synchronized void addOdometryUpdate( - double now, Pose2d odometryPose, Twist2d measuredVelocity, Twist2d predictedVelocity) { - mOdometryToVehicle.put(now, odometryPose); - mKalmanFilter.predict( - VecBuilder.fill(0.0, 0.0), Constants.DT); // Propagate error of current vision prediction - mVehicleVelocityMeasured = measuredVelocity; - mVehicleVelocityMeasuredFiltered.add(measuredVelocity); - mVehichleVelocityPredicted = predictedVelocity; - - mLastTimestamp = now; - } - - /** - * Adds new vision pose update. - * - * @param update Info about vision update. - */ - public static synchronized void addVisionUpdate(VisionUpdate update) { - // If it's the first update don't do filtering - if (mLatestVisionUpdate.isEmpty() || mInitialFieldToOdom.isEmpty()) { - double visionTimestamp = update.mTimestamp; - mLastTimestamp = update.mTimestamp; - Pose2d proximateDtPose = mOdometryToVehicle.get((visionTimestamp)); - Translation2d fieldToVision = update.mFieldToCamera - .plus(update.getmRobotToCamera() - .rotateBy(getLatestOdomToVehicle().getValue().getRotation()).unaryMinus()); - Translation2d odomToVehicleTranslation = proximateDtPose.getTranslation(); - Translation2d fieldToOdom = fieldToVision - .plus(odomToVehicleTranslation.unaryMinus()); - mFieldToOdometry.put((visionTimestamp), fieldToOdom); - mInitialFieldToOdom = Optional.of(mFieldToOdometry.lastEntry().getValue()); - mKalmanFilter.setXhat(0, fieldToOdom.getX()); - mKalmanFilter.setXhat(1, fieldToOdom.getY()); - mLatestVisionUpdate = Optional.ofNullable(update); - - // LegacyDrive.getInstance().setOdometry(new Pose2d(fieldToOdom, getLatestOdomToVehicle().getValue().getRotation())); - } else { - double visionTimestamp = mLatestVisionUpdate.get().mTimestamp; - mLastTimestamp = mLatestVisionUpdate.get().mTimestamp; - Pose2d proximateDtPose = mOdometryToVehicle.get((visionTimestamp)); - mLatestVisionUpdate = Optional.ofNullable(update); - Translation2d fieldToVision = mLatestVisionUpdate - .get().mFieldToCamera - .plus(mLatestVisionUpdate - .get() - .getmRobotToCamera() - .rotateBy(proximateDtPose.getRotation()).unaryMinus()); - - if (mPoseAcceptor.shouldAcceptVision( - visionTimestamp, - new Pose2d(fieldToVision, new Rotation2d()), - getLatestFieldToVehicle(), - mVehicleVelocityMeasured, - mIsInAuto)) { - Translation2d fieldToOdom = fieldToVision.plus( - proximateDtPose.getTranslation().unaryMinus()); - try { - Vector stdevs = update.mXyStdev; - mKalmanFilter.correct( - VecBuilder.fill(0.0, 0.0), - VecBuilder.fill( - fieldToOdom.getX(), - fieldToOdom.getY()), - StateSpaceUtil.makeCovarianceMatrix(Nat.N2(), stdevs)); - mFieldToOdometry.put( - (visionTimestamp), - new Translation2d(mKalmanFilter.getXhat(0), mKalmanFilter.getXhat(1))); - if (!getHasRecievedVisionUpdate()) { - mHasRecievedVisionUpdate = true; - } - } catch (Exception e) { - DriverStation.reportError(update.mXyStdev + "//QR Decomposition failed: ", e.getStackTrace()); - } - } - } - } - - /** - * Gets initial odometry error. Odometry initializes to the origin, eile the - * robot starts at an unknown position on the field. - * - * @return Initial odometry error translation. - */ - public static synchronized Translation2d getmInitialFieldToOdom() { - if (mInitialFieldToOdom.isEmpty()) - return new Translation2d(); - return mInitialFieldToOdom.get(); - } - - /** - * @return Latest field relative robot pose. - */ - public static synchronized Pose2d getLatestFieldToVehicle() { - Pose2d odomToVehicle = getLatestOdomToVehicle().getValue(); - - Translation2d fieldToOdom = getLatestFieldToOdom(); - return new Pose2d(Util.translateBy(fieldToOdom, odomToVehicle.getTranslation()), odomToVehicle.getRotation()); - } - - /** - * Gets field relative robot pose from history. Linearly interpolates between - * gaps. - * - * @param timestamp Timestamp to look up. - * @return Field relative robot pose at timestamp. - */ - public static synchronized Pose2d getFieldToVehicle(double timestamp) { - Pose2d odomToVehicle = getOdomToVehicle(timestamp); - - Translation2d fieldToOdom = getFieldToOdom(timestamp); - return new Pose2d(Util.translateBy(fieldToOdom, odomToVehicle.getTranslation()), odomToVehicle.getRotation()); - - } - - /** - * Gets interpolated robot pose using predicted robot velocity from latest - * odometry update. - * - * @param lookaheadTime Scalar for predicted velocity. - * @return Predcited robot pose at lookahead time. - */ - public static synchronized Pose2d getPredictedFieldToVehicle(double lookaheadTime) { - Pose2d odomToVehicle = getPredictedOdomToVehicle(lookaheadTime); - - Translation2d fieldToOdom = getLatestFieldToOdom(); - return new Pose2d(Util.translateBy(fieldToOdom, odomToVehicle.getTranslation()), odomToVehicle.getRotation()); - } - - /** - * @return Latest odometry pose. - */ - public static synchronized Map.Entry getLatestOdomToVehicle() { - return mOdometryToVehicle.lastEntry(); - } - - /** - * Gets odometry pose from history. Linearly interpolates between gaps. - * - * @param timestamp Timestamp to loop up. - * @return Odometry relative robot pose at timestamp. - */ - public static synchronized Pose2d getOdomToVehicle(double timestamp) { - return mOdometryToVehicle.get((timestamp)); - } - - /** - * Gets interpolated odometry pose using predicted robot velocity from latest - * odometry update. - * - * @param lookaheadTime Scalar for predicted velocity. - * @return Predcited odometry pose at lookahead time. - */ - public static synchronized Pose2d getPredictedOdomToVehicle(double lookaheadTime) { - return getLatestOdomToVehicle() - .getValue() - .transformBy(new Transform2d(new Pose2d(), - Util.expMap(Util.scaledTwist2d(mVehichleVelocityPredicted, lookaheadTime)))); - } - - /** - * @return Latest odometry error translation. - */ - public static synchronized Translation2d getLatestFieldToOdom() { - return getFieldToOdom(mFieldToOdometry.lastKey()); - } - - /** - * Gets odometry error translation at timestamp. Linearly interpolates between - * gaps. - * - * @param timestamp Timestamp to look up. - * @return Odometry error at timestamp. - */ - public static synchronized Translation2d getFieldToOdom(double timestamp) { - if (mFieldToOdometry.isEmpty()) - return new Translation2d(); - return mFieldToOdometry.get((timestamp)); - } - - /** - * @return Predicted robot velocity from last odometry update. - */ - public static synchronized Twist2d getPredictedVelocity() { - return mVehichleVelocityPredicted; - } - - /** - * @return Measured robot velocity from last odometry update. - */ - public static synchronized Twist2d getMeasuredVelocity() { - return mVehicleVelocityMeasured; - } - - /** - * @return Measured robot velocity smoothed using a moving average filter. - */ - public static synchronized Twist2d getSmoothedVelocity() { - return mVehicleVelocityMeasuredFiltered.getAverage(); - } - - /** - * @return Gets if estimator has recieved a vision update. - */ - public static synchronized boolean getHasRecievedVisionUpdate() { - return mHasRecievedVisionUpdate; - } - - /** - * Updates tracker to use stricter auto vision filtering. - * @param inAuto If auto filters should be used. - */ - public static synchronized void setIsInAuto(boolean inAuto) { - mIsInAuto = inAuto; - } - - /** - * Class to hold information about a vision update. - */ - public static class VisionUpdate { - private double mTimestamp; - private Translation2d mFieldToCamera; - private Translation2d mRobotToCamera; - private Vector mXyStdev; - - public VisionUpdate( - double timestamp, Translation2d fieldToCamera, Translation2d robotToCamera, Vector xyStdev) { - mTimestamp = timestamp; - mFieldToCamera = fieldToCamera; - mRobotToCamera = robotToCamera; - mXyStdev = xyStdev; - } - - public double getmTimestamp() { - return mTimestamp; - } - - public Translation2d getFieldToVehicle() { - return mFieldToCamera; - } - - public Translation2d getmRobotToCamera() { - return mRobotToCamera; - } - - public Vector getXYStdev() { - return mXyStdev; - } - } - - public static void setAlliance(Optional alliance) { - mAlliance = alliance; - System.out.println(alliance.toString()); - } - - public static Optional getAlliance() { - return mAlliance; - } -} diff --git a/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java b/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java index add8fbd..1875085 100644 --- a/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java +++ b/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java @@ -7,9 +7,10 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.Constants; -import frc.robot.RobotState; +// import frc.robot.RobotState; import frc.robot.lib.localization.FieldUtil; public class RedTrajectory { @@ -155,7 +156,7 @@ public static ChassisAccels estimate(ChassisSpeeds first, ChassisSpeeds second, public RedTrajectory(Trajectory traj, boolean flipForAlliance) { choreoTrajectory = traj; this.type = TrajectoryType.CHOREO; - this.flipped = flipForAlliance && RobotState.getAlliance().orElse(Alliance.Blue) == Alliance.Red; + this.flipped = flipForAlliance && DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;//RobotState.getAlliance().orElse(Alliance.Blue) == Alliance.Red; this.name = "ChoreoTrajectory#" + traj.name(); } @@ -167,7 +168,7 @@ public RedTrajectory(Trajectory traj, boolean flipForAlliance) { public RedTrajectory(PathPlannerTrajectory traj, boolean flipForAlliance) { pathplannerTrajectory = traj; this.type = TrajectoryType.PATHPLANNER; - this.flipped = flipForAlliance && RobotState.getAlliance().orElse(Alliance.Blue) == Alliance.Red; + this.flipped = flipForAlliance && DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;//RobotState.getAlliance().orElse(Alliance.Blue) == Alliance.Red; this.name = "PathPlannerTrajectory#" + traj.hashCode(); } @@ -179,7 +180,7 @@ public RedTrajectory(PathPlannerTrajectory traj, boolean flipForAlliance) { public RedTrajectory(PathPlannerTrajectory traj, boolean flipForAlliance, String name) { pathplannerTrajectory = traj; this.type = TrajectoryType.PATHPLANNER; - this.flipped = flipForAlliance && RobotState.getAlliance().orElse(Alliance.Blue) == Alliance.Red; + this.flipped = flipForAlliance && DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;//RobotState.getAlliance().orElse(Alliance.Blue) == Alliance.Red; this.name = "PathPlannerTrajectory#" + name; } diff --git a/src/main/java/frc/robot/subsystems/CancoderManager.java b/src/main/java/frc/robot/subsystems/CancoderManager.java deleted file mode 100644 index bcd451b..0000000 --- a/src/main/java/frc/robot/subsystems/CancoderManager.java +++ /dev/null @@ -1,125 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CANcoderConfigurator; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants; - -import java.util.Optional; - -public class CancoderManager { - public static CancoderManager cancodersInstance; - public static CancoderManager getInstance() { - if (cancodersInstance == null) { - cancodersInstance = new CancoderManager(); - } - return cancodersInstance; - } - - private final CANcoder fl; - private final CANcoder fr; - private final CANcoder bl; - private final CANcoder br; - - private final CanTimeObserver frObs; - private final CanTimeObserver flObs; - private final CanTimeObserver blObs; - private final CanTimeObserver brObs; - - private static final double BOOT_UP_ERROR_ALLOWANCE_TIME = 10.0; - - private static class CanTimeObserver { - private final CANcoder cancoder; - private Optional lastTimestamp = Optional.empty(); - private int validUpdates = 0; - private static final int kRequiredValidTimestamps = 10; - - public CanTimeObserver(CANcoder cancoder) { - this.cancoder = cancoder; - } - - public boolean hasUpdate() { - StatusSignal absolutePositionSignal = cancoder.getAbsolutePosition(); - - double timestamp = absolutePositionSignal.getTimestamp().getTime(); - if (lastTimestamp.isEmpty()) { - lastTimestamp = Optional.of(timestamp); - } - if (timestamp > lastTimestamp.get()) { - validUpdates++; - lastTimestamp = Optional.of(timestamp); - } - return validUpdates > kRequiredValidTimestamps; - } - } - - private CANcoder build(Constants.Port canDeviceId) { - CANcoder thisCancoder = new CANcoder(canDeviceId.id, canDeviceId.bus); - CANcoderConfigurator configurator = thisCancoder.getConfigurator(); - CANcoderConfiguration canCoderConfig = new CANcoderConfiguration(); - - canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1.0; - canCoderConfig.MagnetSensor.MagnetOffset = 0.0; - canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - - double startTime = Timer.getFPGATimestamp(); - boolean timedOut = false; - boolean goodInit = false; - int attempt = 1; - while (!goodInit && !timedOut && attempt < 20) { - System.out.println("Initing CANCoder " + canDeviceId.id + " / attempt: " + attempt + " / " - + (Timer.getFPGATimestamp() - startTime) + " seconds elapsed"); - StatusCode settingsCode = configurator.apply(canCoderConfig); - StatusCode sensorCode = thisCancoder.getAbsolutePosition().setUpdateFrequency(20); - - goodInit = settingsCode == StatusCode.OK && sensorCode == StatusCode.OK; - - timedOut = (Timer.getFPGATimestamp()) - startTime >= BOOT_UP_ERROR_ALLOWANCE_TIME; - attempt++; - } - - return thisCancoder; - } - - public CancoderManager() { - fl = build(Constants.Port.FL_CANCODER); - flObs = new CanTimeObserver(fl); - - fr = build(Constants.Port.FR_CANCODER); - frObs = new CanTimeObserver(fr); - - bl = build(Constants.Port.BL_CANCODER); - blObs = new CanTimeObserver(bl); - - br = build(Constants.Port.BR_CANCODER); - brObs = new CanTimeObserver(br); - } - - public boolean allHaveBeenInitialized() { - return flObs.hasUpdate() - && frObs.hasUpdate() - && blObs.hasUpdate() - && brObs.hasUpdate(); - } - - public CANcoder getFrontLeft() { - return fl; - } - - public CANcoder getFrontRight() { - return fr; - } - - public CANcoder getBackLeft() { - return bl; - } - - public CANcoder getBackRight() { - return br; - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java deleted file mode 100644 index c9c4f6a..0000000 --- a/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java +++ /dev/null @@ -1,88 +0,0 @@ -package frc.robot.subsystems.drive.commands; - -import static edu.wpi.first.units.Units.Centimeters; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.MetersPerSecond; - -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.therekrab.autopilot.*; -import com.therekrab.autopilot.Autopilot.APResult; - -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.lib.control.ProfiledPIDVController; -import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; -import frc.robot.subsystems.drive.Drive; - -/** - * I've tried this enough times according to the documents - * I don't think we need this anymore. - */ -@Deprecated -public class AutopilotCommand extends Command { - public final Drive drive; - private static final APConstraints kConstraints = new APConstraints() - .withAcceleration(5.0) - .withJerk(2.0); - private static final APProfile kProfile = new APProfile(kConstraints) - .withErrorXY(Centimeters.of(2)) - .withErrorTheta(Degrees.of(0.5)) - .withBeelineRadius(Centimeters.of(8)); - public static final Autopilot kAutopilot = new Autopilot(kProfile); - public final APTarget target; - - private final ProfiledPIDVController thetaController; - - public AutopilotCommand(Pose2d target) { - this(Drive.getInstance(), new APTarget(target), Constants.Auto.ROTATION_CONSTANTS); - } - - public AutopilotCommand(Drive drive, APTarget target, ProfiledPIDFConstants rotationConstants) { - this.drive = drive; - this.target = target; - thetaController = new ProfiledPIDVController(rotationConstants); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - addRequirements(drive); - setName("Autopilot to " + target.getReference().toString()); - } - - @Override - public void execute() { - ChassisSpeeds robotRelativeSpeeds = drive.getState().Speeds; - Pose2d pose = drive.getState().Pose; - ChassisSpeeds speeds = drive.getState().Speeds; - - APResult output = kAutopilot.calculate(pose, robotRelativeSpeeds, target); - - /* these speeds are field relative */ - double veloX = output.vx().in(MetersPerSecond); - double veloY = output.vy().in(MetersPerSecond); - Rotation2d headingReference = output.targetAngle(); - - thetaController.setTarget(headingReference.getRadians()); - thetaController.setInput( - new Pair( - pose.getRotation().getRadians(), speeds.omegaRadiansPerSecond)); - - double rotation = thetaController.getOutput(); - - drive.setSwerveRequest(new SwerveRequest.RobotCentric() - .withVelocityX(veloX) - .withVelocityY(veloY) - .withRotationalRate(rotation)); - } - - @Override - public boolean isFinished() { - return kAutopilot.atTarget(drive.getState().Pose, target); - } - - @Override - public void end(boolean interrupted) { - drive.setSwerveRequest(new SwerveRequest.FieldCentric()); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/commands/SnapCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/SnapCommand.java deleted file mode 100644 index cac32e0..0000000 --- a/src/main/java/frc/robot/subsystems/drive/commands/SnapCommand.java +++ /dev/null @@ -1,152 +0,0 @@ -package frc.robot.subsystems.drive.commands; - -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.lib.util.Util; -import frc.robot.subsystems.drive.Drive; - -/** - * A re-implementation of the autopilot. Please don't kill me - */ -public class SnapCommand extends Command { - private final Drive drive; - private final Pose2d target; - private final Rotation2d finalEntry; - private double maxSpeed; - private double maxAccel; - private double rotationRadius; - private SwerveRequest.FieldCentricFacingAngle request = new SwerveRequest.FieldCentricFacingAngle(); - private Transform2d errorTolerance; - - public SnapCommand(Pose2d target, Rotation2d finalEntry) { - this( - Drive.getInstance(), - target, - finalEntry, - Constants.Drive.MAX_SPEED, - Constants.Drive.MAX_ACCEL, - 1.5, - 0.04, - 2 * Math.PI / 180); - } - - public SnapCommand( - Drive drive, - Pose2d target, - Rotation2d finalEntry, - double maxSpeed, - double maxAccel, - double rotationRadius, - double errorXY, - double errorTheta) - { - this.drive = drive; - this.target = target; - this.finalEntry = finalEntry; - this.maxSpeed = maxSpeed; - this.maxAccel = maxAccel; - this.rotationRadius = rotationRadius; - errorTolerance = new Transform2d(errorXY, errorXY, new Rotation2d(errorTheta)); - } - - @Override - public void execute() { - calculate( - drive.getPose(), - drive.getState().Speeds, - target, - finalEntry); - drive.setSwerveRequest(request); - } - - @Override - public boolean isFinished() { - var delta = drive.getPose().minus(target); - return Math.abs(delta.getTranslation().getX()) < errorTolerance.getTranslation().getX() - && Math.abs(delta.getTranslation().getY()) < errorTolerance.getTranslation().getY() - && Math.abs(delta.getRotation().getRadians()) < errorTolerance.getRotation().getRadians(); - } - - @Override - public void end(boolean interrupted) { - drive.setSwerveRequest(new SwerveRequest.FieldCentric()); - } - - private void calculate( - Pose2d pose, - ChassisSpeeds speeds, - Pose2d target, - Rotation2d finalEntry) - { - Transform2d delta = target.minus(pose); - Translation2d offset = delta.getTranslation(); - Rotation2d entryRotation; - if (!Util.chassisSpeedsEpsilonEquals(speeds, new ChassisSpeeds(), 0.2)) { - entryRotation = new Rotation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - } else { - entryRotation = offset.getAngle(); - } - var deltaRotation = finalEntry.minus(entryRotation); - var robotRelativeSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(speeds, pose.getRotation()); - - double disp; - - double theta = - new Rotation2d(offset.getX(), offset.getY()) - .getRadians(); - double radius = offset.getNorm(); - if (theta == 0) { - disp = radius; - } - theta = Math.abs(theta); - double hypot = Math.hypot(theta, 1); - double u1 = radius * hypot; - double u2 = radius * Math.log(theta + hypot) / theta; - disp = 0.5 * (u1 + u2); - - var vx = Math.cos(theta) - theta * Math.sin(theta); - var vy = theta * Math.cos(theta) + Math.sin(theta); - - var vels = new Translation2d(vx, vy); - if (vels.getNorm() > Constants.DEADBAND) { - vels = vels.div(vels.getNorm()); - } else { - vels = new Translation2d(); - } - - double mag; - double stoppingLimit = Math.sqrt(2.0 * maxAccel * disp); - double goalV = Math.min(maxSpeed, stoppingLimit); - double maxDelta = maxAccel * Constants.DT; - if (goalV > vels.getNorm() + maxDelta) { - mag = vels.getNorm() + maxDelta; - } else if (goalV < vels.getNorm() - maxDelta) { - mag = vels.getNorm() - maxDelta; - } else { - mag = goalV; - } - vels = vels.times(mag); - - Rotation2d entryAngle = new Rotation2d(delta.getX(), delta.getY()); - vels = vels.rotateBy(entryAngle); - - ChassisSpeeds fieldRelativeSpeeds = - new ChassisSpeeds(vels.getX(), vels.getY(), 0.0); - - double blend = Math.min(1.0, offset.getNorm() / rotationRadius); - Rotation2d targetHeading = pose.getRotation().interpolate(finalEntry, 1.0 - blend); - - request.withVelocityX(fieldRelativeSpeeds.vxMetersPerSecond) - .withVelocityY(fieldRelativeSpeeds.vyMetersPerSecond) - .withTargetDirection(targetHeading); - } -} - diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index b8895a2..d275ba3 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; -import frc.robot.RobotState; +// import frc.robot.RobotState; import frc.robot.lib.control.ControlConstants.PIDFConstants; import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; import frc.robot.lib.control.PIDVController; From d850d4529947d9da126ad3ab14ef35bd9e6b44ba Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Fri, 10 Oct 2025 22:54:28 -0700 Subject: [PATCH 04/36] Deprecate limelight-based vision localization code --- .../robot/lib/localization/CustomADStar.java | 713 ------- .../lib/localization/LimelightHelpers.java | 1642 ----------------- .../lib/localization/VisionPoseAcceptor.java | 85 - 3 files changed, 2440 deletions(-) delete mode 100644 src/main/java/frc/robot/lib/localization/CustomADStar.java delete mode 100644 src/main/java/frc/robot/lib/localization/LimelightHelpers.java delete mode 100644 src/main/java/frc/robot/lib/localization/VisionPoseAcceptor.java diff --git a/src/main/java/frc/robot/lib/localization/CustomADStar.java b/src/main/java/frc/robot/lib/localization/CustomADStar.java deleted file mode 100644 index e82cbfc..0000000 --- a/src/main/java/frc/robot/lib/localization/CustomADStar.java +++ /dev/null @@ -1,713 +0,0 @@ -package frc.robot.lib.localization; - -import com.pathplanner.lib.path.*; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.Filesystem; -import java.io.BufferedReader; -import java.io.File; -import java.io.FileReader; -import java.util.*; -import java.util.concurrent.locks.ReadWriteLock; -import java.util.concurrent.locks.ReentrantReadWriteLock; -import org.json.simple.JSONArray; -import org.json.simple.JSONObject; -import org.json.simple.parser.JSONParser; - -/** - * Currently the same as the pathplanner one. - */ -public class CustomADStar { - private static final double SMOOTHING_ANCHOR_PCT = 0.8; - private static final double EPS = 2.5; - - private double fieldLength = 16.54; - private double fieldWidth = 8.02; - - private double nodeSize = 0.2; - - private int nodesX = (int) Math.ceil(fieldLength / nodeSize); - private int nodesY = (int) Math.ceil(fieldWidth / nodeSize); - - private final HashMap g = new HashMap<>(); - private final HashMap rhs = new HashMap<>(); - private final HashMap> open = new HashMap<>(); - private final HashMap> incons = new HashMap<>(); - private final Set closed = new HashSet<>(); - private final Set staticObstacles = new HashSet<>(); - private final Set dynamicObstacles = new HashSet<>(); - private final Set requestObstacles = new HashSet<>(); - - private GridPosition requestStart; - private Translation2d requestRealStartPos; - private GridPosition requestGoal; - private Translation2d requestRealGoalPos; - - private double eps; - - private final Thread planningThread; - private boolean requestMinor = true; - private boolean requestMajor = true; - private boolean requestReset = true; - private boolean newPathAvailable = false; - - private final ReadWriteLock pathLock = new ReentrantReadWriteLock(); - private final ReadWriteLock requestLock = new ReentrantReadWriteLock(); - - private List currentWaypoints = new ArrayList<>(); - private List currentPathFull = new ArrayList<>(); - - private IdealStartingState idealStartingState; - - /** Create a new pathfinder that runs AD* locally in a background thread */ - public CustomADStar() { - planningThread = new Thread(this::runThread); - - requestStart = new GridPosition(0, 0); - requestRealStartPos = Translation2d.kZero; - requestGoal = new GridPosition(0, 0); - requestRealGoalPos = Translation2d.kZero; - - staticObstacles.clear(); - dynamicObstacles.clear(); - - File navGridFile = new File(Filesystem.getDeployDirectory(), "pathplanner/navgrid.json"); - if (navGridFile.exists()) { - try (BufferedReader br = new BufferedReader(new FileReader(navGridFile))) { - StringBuilder fileContentBuilder = new StringBuilder(); - String line; - while ((line = br.readLine()) != null) { - fileContentBuilder.append(line); - } - - String fileContent = fileContentBuilder.toString(); - JSONObject json = (JSONObject) new JSONParser().parse(fileContent); - - nodeSize = ((Number) json.get("nodeSizeMeters")).doubleValue(); - JSONArray grid = (JSONArray) json.get("grid"); - nodesY = grid.size(); - for (int row = 0; row < grid.size(); row++) { - JSONArray rowArray = (JSONArray) grid.get(row); - if (row == 0) { - nodesX = rowArray.size(); - } - for (int col = 0; col < rowArray.size(); col++) { - boolean isObstacle = (boolean) rowArray.get(col); - if (isObstacle) { - staticObstacles.add(new GridPosition(col, row)); - } - } - } - - JSONObject fieldSize = (JSONObject) json.get("field_size"); - fieldLength = ((Number) fieldSize.get("x")).doubleValue(); - fieldWidth = ((Number) fieldSize.get("y")).doubleValue(); - } catch (Exception e) { - // Do nothing, use defaults - } - } - - requestObstacles.clear(); - requestObstacles.addAll(staticObstacles); - requestObstacles.addAll(dynamicObstacles); - - requestReset = true; - requestMajor = true; - requestMinor = true; - - newPathAvailable = false; - - planningThread.setDaemon(true); - planningThread.setName("ADStar Planning Thread"); - planningThread.start(); - } - - /** - * Get if a new path has been calculated since the last time a path was retrieved - * - * @return True if a new path is available - */ - public boolean isNewPathAvailable() { - return newPathAvailable; - } - - /** - * Get the most recently calculated path - * - * @param constraints The path constraints to use when creating the path - * @param goalEndState The goal end state to use when creating the path - * @return The PathPlannerPath created from the points calculated by the pathfinder - */ - public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { - List waypoints; - - pathLock.readLock().lock(); - waypoints = new ArrayList<>(currentWaypoints); - pathLock.readLock().unlock(); - - newPathAvailable = false; - - if (waypoints.size() < 2) { - // Not enough points. Something got borked somewhere - System.out.println("bork"); - return null; - } - - return new PathPlannerPath(waypoints, constraints, idealStartingState, goalEndState); - } - - /** - * Set the start position to pathfind from - * - * @param startPosition Start position on the field. If this is within an obstacle it will be - * moved to the nearest non-obstacle node. - */ - public void setStartPosition(Translation2d startPosition) { - GridPosition startPos = findClosestNonObstacle(getGridPos(startPosition), requestObstacles); - - if (startPos != null && !startPos.equals(requestStart)) { - requestLock.writeLock().lock(); - requestStart = startPos; - requestRealStartPos = startPosition; - - requestMinor = true; - newPathAvailable = false; - requestLock.writeLock().unlock(); - } - } - - public void setIdealStartingState(IdealStartingState idealStartingState) { - this.idealStartingState = idealStartingState; - } - - /** - * Set the goal position to pathfind to - * - * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved - * to the nearest non-obstacle node. - */ - public void setGoalPosition(Translation2d goalPosition) { - GridPosition gridPos = findClosestNonObstacle(getGridPos(goalPosition), requestObstacles); - - if (gridPos != null) { - requestLock.writeLock().lock(); - requestGoal = gridPos; - requestRealGoalPos = goalPosition; - - requestMinor = true; - requestMajor = true; - requestReset = true; - newPathAvailable = false; - requestLock.writeLock().unlock(); - } - } - - /** - * Set the dynamic obstacles that should be avoided while pathfinding. - * - * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents - * opposite corners of a bounding box. - * @param currentRobotPos The current position of the robot. This is needed to change the start - * position of the path if the robot is now within an obstacle. - */ - public void setDynamicObstacles( - List> obs, Translation2d currentRobotPos) { - Set newObs = new HashSet<>(); - - for (var obstacle : obs) { - var gridPos1 = getGridPos(obstacle.getFirst()); - var gridPos2 = getGridPos(obstacle.getSecond()); - - int minX = Math.min(gridPos1.x, gridPos2.x); - int maxX = Math.max(gridPos1.x, gridPos2.x); - - int minY = Math.min(gridPos1.y, gridPos2.y); - int maxY = Math.max(gridPos1.y, gridPos2.y); - - for (int x = minX; x <= maxX; x++) { - for (int y = minY; y <= maxY; y++) { - newObs.add(new GridPosition(x, y)); - } - } - } - - dynamicObstacles.clear(); - dynamicObstacles.addAll(newObs); - requestLock.writeLock().lock(); - requestObstacles.clear(); - requestObstacles.addAll(staticObstacles); - requestObstacles.addAll(dynamicObstacles); - requestLock.writeLock().unlock(); - - pathLock.readLock().lock(); - boolean recalculate = false; - for (GridPosition pos : currentPathFull) { - if (requestObstacles.contains(pos)) { - recalculate = true; - break; - } - } - pathLock.readLock().unlock(); - - if (recalculate) { - setStartPosition(currentRobotPos); - setGoalPosition(requestRealGoalPos); - } - } - - @SuppressWarnings("BusyWait") - private void runThread() { - while (true) { - try { - requestLock.readLock().lock(); - boolean reset = requestReset; - boolean minor = requestMinor; - boolean major = requestMajor; - GridPosition start = requestStart; - Translation2d realStart = requestRealStartPos; - GridPosition goal = requestGoal; - Translation2d realGoal = requestRealGoalPos; - Set obstacles = new HashSet<>(requestObstacles); - - // Change the request booleans based on what will be done this loop - if (reset) { - requestReset = false; - } - - if (minor) { - requestMinor = false; - } else if (major && (eps - 0.5) <= 1.0) { - requestMajor = false; - } - requestLock.readLock().unlock(); - - if (reset || minor || major) { - doWork(reset, minor, major, start, goal, realStart, realGoal, obstacles); - } else { - try { - Thread.sleep(10); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } - } - } catch (Exception e) { - // Something messed up. Reset and hope for the best - requestLock.writeLock().lock(); - requestReset = true; - requestLock.writeLock().unlock(); - } - } - } - - private void doWork( - boolean needsReset, - boolean doMinor, - boolean doMajor, - GridPosition sStart, - GridPosition sGoal, - Translation2d realStartPos, - Translation2d realGoalPos, - Set obstacles) { - if (needsReset) { - reset(sStart, sGoal); - } - - if (doMinor) { - computeOrImprovePath(sStart, sGoal, obstacles); - - List pathPositions = extractPath(sStart, sGoal, obstacles); - List waypoints = - createWaypoints(pathPositions, realStartPos, realGoalPos, obstacles); - - pathLock.writeLock().lock(); - currentPathFull = pathPositions; - currentWaypoints = waypoints; - pathLock.writeLock().unlock(); - - newPathAvailable = true; - } else if (doMajor) { - if (eps > 1.0) { - eps -= 0.5; - open.putAll(incons); - - open.replaceAll((s, v) -> key(s, sStart)); - closed.clear(); - computeOrImprovePath(sStart, sGoal, obstacles); - - List pathPositions = extractPath(sStart, sGoal, obstacles); - List waypoints = - createWaypoints(pathPositions, realStartPos, realGoalPos, obstacles); - - pathLock.writeLock().lock(); - currentPathFull = pathPositions; - currentWaypoints = waypoints; - pathLock.writeLock().unlock(); - - newPathAvailable = true; - } - } - } - - private List extractPath( - GridPosition sStart, GridPosition sGoal, Set obstacles) { - if (sGoal.equals(sStart)) { - return new ArrayList<>(); - } - - List path = new ArrayList<>(); - path.add(sStart); - - var s = sStart; - - for (int k = 0; k < 200; k++) { - HashMap gList = new HashMap<>(); - - for (GridPosition x : getOpenNeighbors(s, obstacles)) { - gList.put(x, g.get(x)); - } - - Map.Entry min = Map.entry(sGoal, Double.POSITIVE_INFINITY); - for (var entry : gList.entrySet()) { - if (entry.getValue() < min.getValue()) { - min = entry; - } - } - s = min.getKey(); - - path.add(s); - if (s.equals(sGoal)) { - break; - } - } - - return path; - } - - private List createWaypoints( - List path, - Translation2d realStartPos, - Translation2d realGoalPos, - Set obstacles) { - if (path.isEmpty()) { - return new ArrayList<>(); - } - - List simplifiedPath = new ArrayList<>(); - simplifiedPath.add(path.get(0)); - for (int i = 1; i < path.size() - 1; i++) { - if (!walkable(simplifiedPath.get(simplifiedPath.size() - 1), path.get(i + 1), obstacles)) { - simplifiedPath.add(path.get(i)); - } - } - simplifiedPath.add(path.get(path.size() - 1)); - - List fieldPosPath = new ArrayList<>(); - for (GridPosition pos : simplifiedPath) { - fieldPosPath.add(gridPosToTranslation2d(pos)); - } - - if (fieldPosPath.size() < 2) { - return new ArrayList<>(); - } - - // Replace start and end positions with their real positions - fieldPosPath.set(0, realStartPos); - fieldPosPath.set(fieldPosPath.size() - 1, realGoalPos); - - List pathPoses = new ArrayList<>(); - pathPoses.add( - new Pose2d(fieldPosPath.get(0), fieldPosPath.get(1).minus(fieldPosPath.get(0)).getAngle())); - for (int i = 1; i < fieldPosPath.size() - 1; i++) { - Translation2d last = fieldPosPath.get(i - 1); - Translation2d current = fieldPosPath.get(i); - Translation2d next = fieldPosPath.get(i + 1); - - Translation2d anchor1 = current.minus(last).times(SMOOTHING_ANCHOR_PCT).plus(last); - Rotation2d heading1 = current.minus(last).getAngle(); - Translation2d anchor2 = current.minus(next).times(SMOOTHING_ANCHOR_PCT).plus(next); - Rotation2d heading2 = next.minus(anchor2).getAngle(); - - pathPoses.add(new Pose2d(anchor1, heading1)); - pathPoses.add(new Pose2d(anchor2, heading2)); - } - pathPoses.add( - new Pose2d( - fieldPosPath.get(fieldPosPath.size() - 1), - fieldPosPath - .get(fieldPosPath.size() - 1) - .minus(fieldPosPath.get(fieldPosPath.size() - 2)) - .getAngle())); - - return PathPlannerPath.waypointsFromPoses(pathPoses); - } - - private GridPosition findClosestNonObstacle(GridPosition pos, Set obstacles) { - if (!obstacles.contains(pos)) { - return pos; - } - - Set visited = new HashSet<>(); - - Queue queue = new LinkedList<>(getAllNeighbors(pos)); - - while (!queue.isEmpty()) { - GridPosition check = queue.poll(); - if (!obstacles.contains(check)) { - return check; - } - visited.add(check); - - for (GridPosition neighbor : getAllNeighbors(check)) { - if (!visited.contains(neighbor) && !queue.contains(neighbor)) { - queue.add(neighbor); - } - } - } - return null; - } - - private boolean walkable(GridPosition s1, GridPosition s2, Set obstacles) { - int x0 = s1.x; - int y0 = s1.y; - int x1 = s2.x; - int y1 = s2.y; - - int dx = Math.abs(x1 - x0); - int dy = Math.abs(y1 - y0); - int x = x0; - int y = y0; - int n = 1 + dx + dy; - int xInc = (x1 > x0) ? 1 : -1; - int yInc = (y1 > y0) ? 1 : -1; - int error = dx - dy; - dx *= 2; - dy *= 2; - - for (; n > 0; n--) { - if (obstacles.contains(new GridPosition(x, y))) { - return false; - } - - if (error > 0) { - x += xInc; - error -= dy; - } else if (error < 0) { - y += yInc; - error += dx; - } else { - x += xInc; - y += yInc; - error -= dy; - error += dx; - n--; - } - } - - return true; - } - - private void reset(GridPosition sStart, GridPosition sGoal) { - g.clear(); - rhs.clear(); - open.clear(); - incons.clear(); - closed.clear(); - - for (int x = 0; x < nodesX; x++) { - for (int y = 0; y < nodesY; y++) { - g.put(new GridPosition(x, y), Double.POSITIVE_INFINITY); - rhs.put(new GridPosition(x, y), Double.POSITIVE_INFINITY); - } - } - - rhs.put(sGoal, 0.0); - - eps = EPS; - - open.put(sGoal, key(sGoal, sStart)); - } - - private void computeOrImprovePath( - GridPosition sStart, GridPosition sGoal, Set obstacles) { - while (true) { - var sv = topKey(); - if (sv == null) { - break; - } - var s = sv.getFirst(); - var v = sv.getSecond(); - - if (comparePair(v, key(sStart, sStart)) >= 0 && rhs.get(sStart).equals(g.get(sStart))) { - break; - } - - open.remove(s); - - if (g.get(s) > rhs.get(s)) { - g.put(s, rhs.get(s)); - closed.add(s); - - for (GridPosition sn : getOpenNeighbors(s, obstacles)) { - updateState(sn, sStart, sGoal, obstacles); - } - } else { - g.put(s, Double.POSITIVE_INFINITY); - for (GridPosition sn : getOpenNeighbors(s, obstacles)) { - updateState(sn, sStart, sGoal, obstacles); - } - updateState(s, sStart, sGoal, obstacles); - } - } - } - - private void updateState( - GridPosition s, GridPosition sStart, GridPosition sGoal, Set obstacles) { - if (!s.equals(sGoal)) { - rhs.put(s, Double.POSITIVE_INFINITY); - - for (GridPosition x : getOpenNeighbors(s, obstacles)) { - rhs.put(s, Math.min(rhs.get(s), g.get(x) + cost(s, x, obstacles))); - } - } - - open.remove(s); - - if (!g.get(s).equals(rhs.get(s))) { - if (!closed.contains(s)) { - open.put(s, key(s, sStart)); - } else { - incons.put(s, Pair.of(0.0, 0.0)); - } - } - } - - private double cost(GridPosition sStart, GridPosition sGoal, Set obstacles) { - if (isCollision(sStart, sGoal, obstacles)) { - return Double.POSITIVE_INFINITY; - } - - return heuristic(sStart, sGoal); - } - - private boolean isCollision(GridPosition sStart, GridPosition sEnd, Set obstacles) { - if (obstacles.contains(sStart) || obstacles.contains(sEnd)) { - return true; - } - - if (sStart.x != sEnd.x && sStart.y != sEnd.y) { - GridPosition s1; - GridPosition s2; - - if (sEnd.x - sStart.x == sStart.y - sEnd.y) { - s1 = new GridPosition(Math.min(sStart.x, sEnd.x), Math.min(sStart.y, sEnd.y)); - s2 = new GridPosition(Math.max(sStart.x, sEnd.x), Math.max(sStart.y, sEnd.y)); - } else { - s1 = new GridPosition(Math.min(sStart.x, sEnd.x), Math.max(sStart.y, sEnd.y)); - s2 = new GridPosition(Math.max(sStart.x, sEnd.x), Math.min(sStart.y, sEnd.y)); - } - - return obstacles.contains(s1) || obstacles.contains(s2); - } - - return false; - } - - private List getOpenNeighbors(GridPosition s, Set obstacles) { - List ret = new ArrayList<>(); - - for (int xMove = -1; xMove <= 1; xMove++) { - for (int yMove = -1; yMove <= 1; yMove++) { - GridPosition sNext = new GridPosition(s.x + xMove, s.y + yMove); - if (!obstacles.contains(sNext) - && sNext.x >= 0 - && sNext.x < nodesX - && sNext.y >= 0 - && sNext.y < nodesY) { - ret.add(sNext); - } - } - } - return ret; - } - - private List getAllNeighbors(GridPosition s) { - List ret = new ArrayList<>(); - - for (int xMove = -1; xMove <= 1; xMove++) { - for (int yMove = -1; yMove <= 1; yMove++) { - GridPosition sNext = new GridPosition(s.x + xMove, s.y + yMove); - if (sNext.x >= 0 && sNext.x < nodesX && sNext.y >= 0 && sNext.y < nodesY) { - ret.add(sNext); - } - } - } - return ret; - } - - private Pair key(GridPosition s, GridPosition sStart) { - if (g.get(s) > rhs.get(s)) { - return Pair.of(rhs.get(s) + eps * heuristic(sStart, s), rhs.get(s)); - } else { - return Pair.of(g.get(s) + heuristic(sStart, s), g.get(s)); - } - } - - private Pair> topKey() { - Map.Entry> min = null; - for (var entry : open.entrySet()) { - if (min == null || comparePair(entry.getValue(), min.getValue()) < 0) { - min = entry; - } - } - - if (min == null) { - return null; - } - - return Pair.of(min.getKey(), min.getValue()); - } - - private double heuristic(GridPosition sStart, GridPosition sGoal) { - return Math.hypot(sGoal.x - sStart.x, sGoal.y - sStart.y); - } - - private int comparePair(Pair a, Pair b) { - int first = Double.compare(a.getFirst(), b.getFirst()); - if (first == 0) { - return Double.compare(a.getSecond(), b.getSecond()); - } else { - return first; - } - } - - private GridPosition getGridPos(Translation2d pos) { - int x = (int) Math.floor(pos.getX() / nodeSize); - int y = (int) Math.floor(pos.getY() / nodeSize); - - return new GridPosition(x, y); - } - - private Translation2d gridPosToTranslation2d(GridPosition pos) { - return new Translation2d( - (pos.x * nodeSize) + (nodeSize / 2.0), (pos.y * nodeSize) + (nodeSize / 2.0)); - } - - /** - * Represents a node in the pathfinding grid - * - * @param x X index in the grid - * @param y Y index in the grid - */ - public record GridPosition(int x, int y) implements Comparable { - @Override - public int compareTo(GridPosition o) { - if (x == o.x) { - return Integer.compare(y, o.y); - } else { - return Integer.compare(x, o.x); - } - } - } -} diff --git a/src/main/java/frc/robot/lib/localization/LimelightHelpers.java b/src/main/java/frc/robot/lib/localization/LimelightHelpers.java deleted file mode 100644 index f627307..0000000 --- a/src/main/java/frc/robot/lib/localization/LimelightHelpers.java +++ /dev/null @@ -1,1642 +0,0 @@ -package frc.robot.lib.localization; - -import edu.wpi.first.networktables.DoubleArrayEntry; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.TimestampedDoubleArray; -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.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; - -import java.io.IOException; -import java.net.HttpURLConnection; -import java.net.MalformedURLException; -import java.net.URL; -import java.util.Map; -import java.util.concurrent.CompletableFuture; - -import com.fasterxml.jackson.annotation.JsonFormat; -import com.fasterxml.jackson.annotation.JsonFormat.Shape; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationFeature; -import com.fasterxml.jackson.databind.ObjectMapper; -import java.util.concurrent.ConcurrentHashMap; - -/** - * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. - * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. - */ -public class LimelightHelpers { - private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); - - /** - * Represents a Color/Retroreflective Target Result extracted from JSON Output - */ - public static class LimelightTarget_Retro { - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() - { - return toPose3D(cameraPose_TargetSpace); - } - public Pose3d getRobotPose_FieldSpace() - { - return toPose3D(robotPose_FieldSpace); - } - public Pose3d getRobotPose_TargetSpace() - { - return toPose3D(robotPose_TargetSpace); - } - public Pose3d getTargetPose_CameraSpace() - { - return toPose3D(targetPose_CameraSpace); - } - public Pose3d getTargetPose_RobotSpace() - { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() - { - return toPose2D(cameraPose_TargetSpace); - } - public Pose2d getRobotPose_FieldSpace2D() - { - return toPose2D(robotPose_FieldSpace); - } - public Pose2d getRobotPose_TargetSpace2D() - { - return toPose2D(robotPose_TargetSpace); - } - public Pose2d getTargetPose_CameraSpace2D() - { - return toPose2D(targetPose_CameraSpace); - } - public Pose2d getTargetPose_RobotSpace2D() - { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Retro() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - - } - - /** - * Represents an AprilTag/Fiducial Target Result extracted from JSON Output - */ - public static class LimelightTarget_Fiducial { - - @JsonProperty("fID") - public double fiducialID; - - @JsonProperty("fam") - public String fiducialFamily; - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() - { - return toPose3D(cameraPose_TargetSpace); - } - public Pose3d getRobotPose_FieldSpace() - { - return toPose3D(robotPose_FieldSpace); - } - public Pose3d getRobotPose_TargetSpace() - { - return toPose3D(robotPose_TargetSpace); - } - public Pose3d getTargetPose_CameraSpace() - { - return toPose3D(targetPose_CameraSpace); - } - public Pose3d getTargetPose_RobotSpace() - { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() - { - return toPose2D(cameraPose_TargetSpace); - } - public Pose2d getRobotPose_FieldSpace2D() - { - return toPose2D(robotPose_FieldSpace); - } - public Pose2d getRobotPose_TargetSpace2D() - { - return toPose2D(robotPose_TargetSpace); - } - public Pose2d getTargetPose_CameraSpace2D() - { - return toPose2D(targetPose_CameraSpace); - } - public Pose2d getTargetPose_RobotSpace2D() - { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Fiducial() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - /** - * Represents a Barcode Target Result extracted from JSON Output - */ - public static class LimelightTarget_Barcode { - - /** - * Barcode family type (e.g. "QR", "DataMatrix", etc.) - */ - @JsonProperty("fam") - public String family; - - /** - * Gets the decoded data content of the barcode - */ - @JsonProperty("data") - public String data; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("pts") - public double[][] corners; - - public LimelightTarget_Barcode() { - } - - public String getFamily() { - return family; - } - } - - /** - * Represents a Neural Classifier Pipeline Result extracted from JSON Output - */ - public static class LimelightTarget_Classifier { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("zone") - public double zone; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Classifier() { - } - } - - /** - * Represents a Neural Detector Pipeline Result extracted from JSON Output - */ - public static class LimelightTarget_Detector { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - public LimelightTarget_Detector() { - } - } - - /** - * Limelight Results object, parsed from a Limelight's JSON results output. - */ - public static class LimelightResults { - - public String error; - - @JsonProperty("pID") - public double pipelineID; - - @JsonProperty("tl") - public double latency_pipeline; - - @JsonProperty("cl") - public double latency_capture; - - public double latency_jsonParse; - - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; - - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; - - @JsonProperty("v") - @JsonFormat(shape = Shape.NUMBER) - public boolean valid; - - @JsonProperty("botpose") - public double[] botpose; - - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; - - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; - - @JsonProperty("botpose_tagcount") - public double botpose_tagcount; - - @JsonProperty("botpose_span") - public double botpose_span; - - @JsonProperty("botpose_avgdist") - public double botpose_avgdist; - - @JsonProperty("botpose_avgarea") - public double botpose_avgarea; - - @JsonProperty("t6c_rs") - public double[] camerapose_robotspace; - - public Pose3d getBotPose3d() { - return toPose3D(botpose); - } - - public Pose3d getBotPose3d_wpiRed() { - return toPose3D(botpose_wpired); - } - - public Pose3d getBotPose3d_wpiBlue() { - return toPose3D(botpose_wpiblue); - } - - public Pose2d getBotPose2d() { - return toPose2D(botpose); - } - - public Pose2d getBotPose2d_wpiRed() { - return toPose2D(botpose_wpired); - } - - public Pose2d getBotPose2d_wpiBlue() { - return toPose2D(botpose_wpiblue); - } - - @JsonProperty("Retro") - public LimelightTarget_Retro[] targets_Retro; - - @JsonProperty("Fiducial") - public LimelightTarget_Fiducial[] targets_Fiducials; - - @JsonProperty("Classifier") - public LimelightTarget_Classifier[] targets_Classifier; - - @JsonProperty("Detector") - public LimelightTarget_Detector[] targets_Detector; - - @JsonProperty("Barcode") - public LimelightTarget_Barcode[] targets_Barcode; - - public LimelightResults() { - botpose = new double[6]; - botpose_wpired = new double[6]; - botpose_wpiblue = new double[6]; - camerapose_robotspace = new double[6]; - targets_Retro = new LimelightTarget_Retro[0]; - targets_Fiducials = new LimelightTarget_Fiducial[0]; - targets_Classifier = new LimelightTarget_Classifier[0]; - targets_Detector = new LimelightTarget_Detector[0]; - targets_Barcode = new LimelightTarget_Barcode[0]; - - } - - - } - - /** - * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. - */ - public static class RawFiducial { - public int id = 0; - public double txnc = 0; - public double tync = 0; - public double ta = 0; - public double distToCamera = 0; - public double distToRobot = 0; - public double ambiguity = 0; - - - public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { - this.id = id; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.distToCamera = distToCamera; - this.distToRobot = distToRobot; - this.ambiguity = ambiguity; - } - } - - /** - * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. - */ - public static class RawDetection { - public int classId = 0; - public double txnc = 0; - public double tync = 0; - public double ta = 0; - public double corner0_X = 0; - public double corner0_Y = 0; - public double corner1_X = 0; - public double corner1_Y = 0; - public double corner2_X = 0; - public double corner2_Y = 0; - public double corner3_X = 0; - public double corner3_Y = 0; - - - public RawDetection(int classId, double txnc, double tync, double ta, - double corner0_X, double corner0_Y, - double corner1_X, double corner1_Y, - double corner2_X, double corner2_Y, - double corner3_X, double corner3_Y ) { - this.classId = classId; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.corner0_X = corner0_X; - this.corner0_Y = corner0_Y; - this.corner1_X = corner1_X; - this.corner1_Y = corner1_Y; - this.corner2_X = corner2_X; - this.corner2_Y = corner2_Y; - this.corner3_X = corner3_X; - this.corner3_Y = corner3_Y; - } - } - - /** - * Represents a 3D Pose Estimate. - */ - public static class PoseEstimate { - public Pose2d pose; - public double timestampSeconds; - public double latency; - public int tagCount; - public double tagSpan; - public double avgTagDist; - public double avgTagArea; - - public RawFiducial[] rawFiducials; - public boolean isMegaTag2; - - /** - * Instantiates a PoseEstimate object with default values - */ - public PoseEstimate() { - this.pose = new Pose2d(); - this.timestampSeconds = 0; - this.latency = 0; - this.tagCount = 0; - this.tagSpan = 0; - this.avgTagDist = 0; - this.avgTagArea = 0; - this.rawFiducials = new RawFiducial[]{}; - this.isMegaTag2 = false; - } - - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, - int tagCount, double tagSpan, double avgTagDist, - double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { - - this.pose = pose; - this.timestampSeconds = timestampSeconds; - this.latency = latency; - this.tagCount = tagCount; - this.tagSpan = tagSpan; - this.avgTagDist = avgTagDist; - this.avgTagArea = avgTagArea; - this.rawFiducials = rawFiducials; - this.isMegaTag2 = isMegaTag2; - } - - } - - /** - * Encapsulates the state of an internal Limelight IMU. - */ - public static class IMUData { - public double robotYaw = 0.0; - public double Roll = 0.0; - public double Pitch = 0.0; - public double Yaw = 0.0; - public double gyroX = 0.0; - public double gyroY = 0.0; - public double gyroZ = 0.0; - public double accelX = 0.0; - public double accelY = 0.0; - public double accelZ = 0.0; - - public IMUData() {} - - public IMUData(double[] imuData) { - if (imuData != null && imuData.length >= 10) { - this.robotYaw = imuData[0]; - this.Roll = imuData[1]; - this.Pitch = imuData[2]; - this.Yaw = imuData[3]; - this.gyroX = imuData[4]; - this.gyroY = imuData[5]; - this.gyroZ = imuData[6]; - this.accelX = imuData[7]; - this.accelY = imuData[8]; - this.accelZ = imuData[9]; - } - } - } - - - private static ObjectMapper mapper; - - /** - * Print JSON Parse time to the console in milliseconds - */ - static boolean profileJSON = false; - - static final String sanitizeName(String name) { - if ("".equals(name) || name == null) { - return "limelight"; - } - return name; - } - - /** - * Takes a 6-length array of pose data and converts it to a Pose3d object. - * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. - * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] - * @return Pose3d object representing the pose, or empty Pose3d if invalid data - */ - public static Pose3d toPose3D(double[] inData){ - if(inData.length < 6) - { - //System.err.println("Bad LL 3D Pose Data!"); - return new Pose3d(); - } - return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), - Units.degreesToRadians(inData[5]))); - } - - /** - * Takes a 6-length array of pose data and converts it to a Pose2d object. - * Uses only x, y, and yaw components, ignoring z, roll, and pitch. - * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. - * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] - * @return Pose2d object representing the pose, or empty Pose2d if invalid data - */ - public static Pose2d toPose2D(double[] inData){ - if(inData.length < 6) - { - //System.err.println("Bad LL 2D Pose Data!"); - return new Pose2d(); - } - Translation2d tran2d = new Translation2d(inData[0], inData[1]); - Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); - return new Pose2d(tran2d, r2d); - } - - /** - * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. - * Translation components are in meters, rotation components are in degrees. - * - * @param pose The Pose3d object to convert - * @return A 6-element array containing [x, y, z, roll, pitch, yaw] - */ - public static double[] pose3dToArray(Pose3d pose) { - double[] result = new double[6]; - result[0] = pose.getTranslation().getX(); - result[1] = pose.getTranslation().getY(); - result[2] = pose.getTranslation().getZ(); - result[3] = Units.radiansToDegrees(pose.getRotation().getX()); - result[4] = Units.radiansToDegrees(pose.getRotation().getY()); - result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); - return result; - } - - /** - * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. - * Translation components are in meters, rotation components are in degrees. - * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. - * - * @param pose The Pose2d object to convert - * @return A 6-element array containing [x, y, 0, 0, 0, yaw] - */ - public static double[] pose2dToArray(Pose2d pose) { - double[] result = new double[6]; - result[0] = pose.getTranslation().getX(); - result[1] = pose.getTranslation().getY(); - result[2] = 0; - result[3] = Units.radiansToDegrees(0); - result[4] = Units.radiansToDegrees(0); - result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); - return result; - } - - private static double extractArrayEntry(double[] inData, int position){ - if(inData.length < position+1) - { - return 0; - } - return inData[position]; - } - - private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { - DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); - - TimestampedDoubleArray tsValue = poseEntry.getAtomic(); - double[] poseArray = tsValue.value; - long timestamp = tsValue.timestamp; - - if (poseArray.length == 0) { - // Handle the case where no data is available - return null; // or some default PoseEstimate - } - - var pose = toPose2D(poseArray); - double latency = extractArrayEntry(poseArray, 6); - int tagCount = (int)extractArrayEntry(poseArray, 7); - double tagSpan = extractArrayEntry(poseArray, 8); - double tagDist = extractArrayEntry(poseArray, 9); - double tagArea = extractArrayEntry(poseArray, 10); - - // Convert server timestamp from microseconds to seconds and adjust for latency - double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); - - RawFiducial[] rawFiducials = new RawFiducial[tagCount]; - int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial * tagCount; - - if (poseArray.length != expectedTotalVals) { - // Don't populate fiducials - } else { - for(int i = 0; i < tagCount; i++) { - int baseIndex = 11 + (i * valsPerFiducial); - int id = (int)poseArray[baseIndex]; - double txnc = poseArray[baseIndex + 1]; - double tync = poseArray[baseIndex + 2]; - double ta = poseArray[baseIndex + 3]; - double distToCamera = poseArray[baseIndex + 4]; - double distToRobot = poseArray[baseIndex + 5]; - double ambiguity = poseArray[baseIndex + 6]; - rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - } - - return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); - } - - /** - * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. - * - * @param limelightName Name/identifier of the Limelight - * @return Array of RawFiducial objects containing detection details - */ - public static RawFiducial[] getRawFiducials(String limelightName) { - var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); - var rawFiducialArray = entry.getDoubleArray(new double[0]); - int valsPerEntry = 7; - if (rawFiducialArray.length % valsPerEntry != 0) { - return new RawFiducial[0]; - } - - int numFiducials = rawFiducialArray.length / valsPerEntry; - RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; - - for (int i = 0; i < numFiducials; i++) { - int baseIndex = i * valsPerEntry; - int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); - double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); - double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); - double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); - double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); - double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); - double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); - - rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - - return rawFiducials; - } - - /** - * Gets the latest raw neural detector results from NetworkTables - * - * @param limelightName Name/identifier of the Limelight - * @return Array of RawDetection objects containing detection details - */ - public static RawDetection[] getRawDetections(String limelightName) { - var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); - var rawDetectionArray = entry.getDoubleArray(new double[0]); - int valsPerEntry = 12; - if (rawDetectionArray.length % valsPerEntry != 0) { - return new RawDetection[0]; - } - - int numDetections = rawDetectionArray.length / valsPerEntry; - RawDetection[] rawDetections = new RawDetection[numDetections]; - - for (int i = 0; i < numDetections; i++) { - int baseIndex = i * valsPerEntry; // Starting index for this detection's data - int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); - double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); - double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); - double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); - double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); - double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); - double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); - double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); - double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); - double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); - double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); - double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); - - rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); - } - - return rawDetections; - } - - /** - * Prints detailed information about a PoseEstimate to standard output. - * Includes timestamp, latency, tag count, tag span, average tag distance, - * average tag area, and detailed information about each detected fiducial. - * - * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." - */ - public static void printPoseEstimate(PoseEstimate pose) { - if (pose == null) { - System.out.println("No PoseEstimate available."); - return; - } - - System.out.printf("Pose Estimate Information:%n"); - System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); - System.out.printf("Latency: %.3f ms%n", pose.latency); - System.out.printf("Tag Count: %d%n", pose.tagCount); - System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); - System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); - System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); - System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); - System.out.println(); - - if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { - System.out.println("No RawFiducials data available."); - return; - } - - System.out.println("Raw Fiducials Details:"); - for (int i = 0; i < pose.rawFiducials.length; i++) { - RawFiducial fiducial = pose.rawFiducials[i]; - System.out.printf(" Fiducial #%d:%n", i + 1); - System.out.printf(" ID: %d%n", fiducial.id); - System.out.printf(" TXNC: %.2f%n", fiducial.txnc); - System.out.printf(" TYNC: %.2f%n", fiducial.tync); - System.out.printf(" TA: %.2f%n", fiducial.ta); - System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); - System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); - System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); - System.out.println(); - } - } - - public static Boolean validPoseEstimate(PoseEstimate pose) { - return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; - } - - public static NetworkTable getLimelightNTTable(String tableName) { - return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); - } - - public static void Flush() { - NetworkTableInstance.getDefault().flush(); - } - - public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { - return getLimelightNTTable(tableName).getEntry(entryName); - } - - public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { - String key = tableName + "/" + entryName; - return doubleArrayEntries.computeIfAbsent(key, k -> { - NetworkTable table = getLimelightNTTable(tableName); - return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); - }); - } - - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); - } - - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); - } - - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); - } - - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); - } - - - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); - } - - public static String[] getLimelightNTStringArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); - } - - - public static URL getLimelightURLString(String tableName, String request) { - String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; - URL url; - try { - url = new URL(urlString); - return url; - } catch (MalformedURLException e) { - System.err.println("bad LL URL"); - } - return null; - } - ///// - ///// - - /** - * Does the Limelight have a valid target? - * @param limelightName Name of the Limelight camera ("" for default) - * @return True if a valid target is present, false otherwise - */ - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); - } - - /** - * Gets the horizontal offset from the crosshair to the target in degrees. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees - */ - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); - } - - /** - * Gets the vertical offset from the crosshair to the target in degrees. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees - */ - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } - - /** - * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees - */ - public static double getTXNC(String limelightName) { - return getLimelightNTDouble(limelightName, "txnc"); - } - - /** - * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees - */ - public static double getTYNC(String limelightName) { - return getLimelightNTDouble(limelightName, "tync"); - } - - /** - * Gets the target area as a percentage of the image (0-100%). - * @param limelightName Name of the Limelight camera ("" for default) - * @return Target area percentage (0-100) - */ - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } - - /** - * T2D is an array that contains several targeting metrcis - * @param limelightName Name of the Limelight camera - * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, - * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] - */ - public static double[] getT2DArray(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "t2d"); - } - - /** - * Gets the number of targets currently detected. - * @param limelightName Name of the Limelight camera - * @return Number of detected targets - */ - public static int getTargetCount(String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[1]; - } - return 0; - } - - /** - * Gets the classifier class index from the currently running neural classifier pipeline - * @param limelightName Name of the Limelight camera - * @return Class index from classifier pipeline - */ - public static int getClassifierClassIndex (String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[10]; - } - return 0; - } - - /** - * Gets the detector class index from the primary result of the currently running neural detector pipeline. - * @param limelightName Name of the Limelight camera - * @return Class index from detector pipeline - */ - public static int getDetectorClassIndex (String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[11]; - } - return 0; - } - - /** - * Gets the current neural classifier result class name. - * @param limelightName Name of the Limelight camera - * @return Class name string from classifier pipeline - */ - public static String getClassifierClass (String limelightName) { - return getLimelightNTString(limelightName, "tcclass"); - } - - /** - * Gets the primary neural detector result class name. - * @param limelightName Name of the Limelight camera - * @return Class name string from detector pipeline - */ - public static String getDetectorClass (String limelightName) { - return getLimelightNTString(limelightName, "tdclass"); - } - - /** - * Gets the pipeline's processing latency contribution. - * @param limelightName Name of the Limelight camera - * @return Pipeline latency in milliseconds - */ - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); - } - - /** - * Gets the capture latency. - * @param limelightName Name of the Limelight camera - * @return Capture latency in milliseconds - */ - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "cl"); - } - - /** - * Gets the active pipeline index. - * @param limelightName Name of the Limelight camera - * @return Current pipeline index (0-9) - */ - public static double getCurrentPipelineIndex(String limelightName) { - return getLimelightNTDouble(limelightName, "getpipe"); - } - - /** - * Gets the current pipeline type. - * @param limelightName Name of the Limelight camera - * @return Pipeline type string (e.g. "retro", "apriltag", etc) - */ - public static String getCurrentPipelineType(String limelightName) { - return getLimelightNTString(limelightName, "getpipetype"); - } - - /** - * Gets the full JSON results dump. - * @param limelightName Name of the Limelight camera - * @return JSON string containing all current results - */ - public static String getJSONDump(String limelightName) { - return getLimelightNTString(limelightName, "json"); - } - - /** - * Switch to getBotPose - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - public static double[] getBotPose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - public static double[] getBotPose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } - - 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 double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - } - - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } - - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } - - public static String getNeuralClassID(String limelightName) { - return getLimelightNTString(limelightName, "tclass"); - } - - public static String[] getRawBarcodeData(String limelightName) { - return getLimelightNTStringArray(limelightName, "rawbarcodes"); - } - - ///// - ///// - - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - return toPose3D(poseArray); - } - - /** - * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Red Alliance field space - */ - public static Pose3d getBotPose3d_wpiRed(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - return toPose3D(poseArray); - } - - /** - * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space - */ - public static Pose3d getBotPose3d_wpiBlue(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - return toPose3D(poseArray); - } - - /** - * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation relative to the target - */ - public static Pose3d getBotPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - return toPose3D(poseArray); - } - - /** - * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the target - */ - public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - return toPose3D(poseArray); - } - - /** - * Gets the target's 3D pose with respect to the camera's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the camera - */ - public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - return toPose3D(poseArray); - } - - /** - * Gets the target's 3D pose with respect to the robot's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the robot - */ - public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - return toPose3D(poseArray); - } - - /** - * Gets the camera's 3D pose with respect to the robot's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the robot - */ - public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); - return toPose3D(poseArray); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiBlue(String limelightName) { - - double[] result = getBotPose_wpiBlue(limelightName); - return toPose2D(result); - } - - /** - * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); - } - - /** - * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. - * Make sure you are calling setRobotOrientation() before calling this method. - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiRed(String limelightName) { - - double[] result = getBotPose_wpiRed(limelightName); - return toPose2D(result); - - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED - * alliance - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpired", false); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED - * alliance - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d(String limelightName) { - - double[] result = getBotPose(limelightName); - return toPose2D(result); - - } - - /** - * Gets the current IMU data from NetworkTables. - * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. - * Returns all zeros if data is invalid or unavailable. - * - * @param limelightName Name/identifier of the Limelight - * @return IMUData object containing all current IMU data - */ - public static IMUData getIMUData(String limelightName) { - double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); - if (imuData == null || imuData.length < 10) { - return new IMUData(); // Returns object with all zeros - } - return new IMUData(imuData); - } - - ///// - ///// - - public static void setPipelineIndex(String limelightName, int pipelineIndex) { - setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); - } - - - public static void setPriorityTagID(String limelightName, int ID) { - setLimelightNTDouble(limelightName, "priorityid", ID); - } - - /** - * Sets LED mode to be controlled by the current pipeline. - * @param limelightName Name of the Limelight camera - */ - public static void setLEDMode_PipelineControl(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 0); - } - - public static void setLEDMode_ForceOff(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 1); - } - - public static void setLEDMode_ForceBlink(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 2); - } - - public static void setLEDMode_ForceOn(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 3); - } - - /** - * Enables standard side-by-side stream mode. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); - } - - /** - * Enables Picture-in-Picture mode with secondary stream in the corner. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } - - /** - * Enables Picture-in-Picture mode with primary stream in the corner. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } - - - /** - * Sets the crop window for the camera. The crop window in the UI must be completely open. - * @param limelightName Name of the Limelight camera - * @param cropXMin Minimum X value (-1 to 1) - * @param cropXMax Maximum X value (-1 to 1) - * @param cropYMin Minimum Y value (-1 to 1) - * @param cropYMax Maximum Y value (-1 to 1) - */ - public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { - double[] entries = new double[4]; - entries[0] = cropXMin; - entries[1] = cropXMax; - entries[2] = cropYMin; - entries[3] = cropYMax; - setLimelightNTDoubleArray(limelightName, "crop", entries); - } - - /** - * Sets 3D offset point for easy 3D targeting. - */ - public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { - double[] entries = new double[3]; - entries[0] = offsetX; - entries[1] = offsetY; - entries[2] = offsetZ; - setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); - } - - /** - * Sets robot orientation values used by MegaTag2 localization algorithm. - * - * @param limelightName Name/identifier of the Limelight - * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC - * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second - * @param pitch (Unnecessary) Robot pitch in degrees - * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second - * @param roll (Unnecessary) Robot roll in degrees - * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second - */ - public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate) { - SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); - } - - public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate) { - SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); - } - - private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate, boolean flush) { - - double[] entries = new double[6]; - entries[0] = yaw; - entries[1] = yawRate; - entries[2] = pitch; - entries[3] = pitchRate; - entries[4] = roll; - entries[5] = rollRate; - setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); - if(flush) - { - Flush(); - } - } - - /** - * Configures the IMU mode for MegaTag2 Localization - * - * @param limelightName Name/identifier of the Limelight - * @param mode IMU mode. - */ - public static void SetIMUMode(String limelightName, int mode) { - setLimelightNTDouble(limelightName, "imumode_set", mode); - } - - /** - * Sets the 3D point-of-interest offset for the current fiducial pipeline. - * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking - * - * @param limelightName Name/identifier of the Limelight - * @param x X offset in meters - * @param y Y offset in meters - * @param z Z offset in meters - */ - public static void SetFidcuial3DOffset(String limelightName, double x, double y, - double z) { - - double[] entries = new double[3]; - entries[0] = x; - entries[1] = y; - entries[2] = z; - setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); - } - - /** - * Overrides the valid AprilTag IDs that will be used for localization. - * Tags not in this list will be ignored for robot pose estimation. - * - * @param limelightName Name/identifier of the Limelight - * @param validIDs Array of valid AprilTag IDs to track - */ - public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { - double[] validIDsDouble = new double[validIDs.length]; - for (int i = 0; i < validIDs.length; i++) { - validIDsDouble[i] = validIDs[i]; - } - setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); - } - - /** - * Sets the downscaling factor for AprilTag detection. - * Increasing downscale can improve performance at the cost of potentially reduced detection range. - * - * @param limelightName Name/identifier of the Limelight - * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. - */ - public static void SetFiducialDownscalingOverride(String limelightName, float downscale) - { - int d = 0; // pipeline - if (downscale == 1.0) - { - d = 1; - } - if (downscale == 1.5) - { - d = 2; - } - if (downscale == 2) - { - d = 3; - } - if (downscale == 3) - { - d = 4; - } - if (downscale == 4) - { - d = 5; - } - setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); - } - - /** - * Sets the camera pose relative to the robot. - * @param limelightName Name of the Limelight camera - * @param forward Forward offset in meters - * @param side Side offset in meters - * @param up Up offset in meters - * @param roll Roll angle in degrees - * @param pitch Pitch angle in degrees - * @param yaw Yaw angle in degrees - */ - public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { - double[] entries = new double[6]; - entries[0] = forward; - entries[1] = side; - entries[2] = up; - entries[3] = roll; - entries[4] = pitch; - entries[5] = yaw; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); - } - - ///// - ///// - - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); - } - - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); - } - - ///// - ///// - - /** - * Asynchronously take snapshot. - */ - public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { - return CompletableFuture.supplyAsync(() -> { - return SYNCH_TAKESNAPSHOT(tableName, snapshotName); - }); - } - - private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { - URL url = getLimelightURLString(tableName, "capturesnapshot"); - try { - HttpURLConnection connection = (HttpURLConnection) url.openConnection(); - connection.setRequestMethod("GET"); - if (snapshotName != null && !"".equals(snapshotName)) { - connection.setRequestProperty("snapname", snapshotName); - } - - int responseCode = connection.getResponseCode(); - if (responseCode == 200) { - return true; - } else { - System.err.println("Bad LL Request"); - } - } catch (IOException e) { - System.err.println(e.getMessage()); - } - return false; - } - - /** - * Gets the latest JSON results output and returns a LimelightResults object. - * @param limelightName Name of the Limelight camera - * @return LimelightResults object containing all current target data - */ - public static LimelightResults getLatestResults(String limelightName) { - - long start = System.nanoTime(); - LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); - if (mapper == null) { - mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); - } - - try { - results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); - } catch (JsonProcessingException e) { - results.error = "lljson error: " + e.getMessage(); - } - - long end = System.nanoTime(); - double millis = (end - start) * .000001; - results.latency_jsonParse = millis; - if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); - } - - return results; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/localization/VisionPoseAcceptor.java b/src/main/java/frc/robot/lib/localization/VisionPoseAcceptor.java deleted file mode 100644 index 461c56b..0000000 --- a/src/main/java/frc/robot/lib/localization/VisionPoseAcceptor.java +++ /dev/null @@ -1,85 +0,0 @@ -package frc.robot.lib.localization; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class VisionPoseAcceptor { - private static final double kFieldBorderMargin = 0.5; - private static final double kMaxVisionCorrection = 2.0; // Jump from fused pose - - Pose2d mLastVisionFieldToVehicle = null; - - public static Twist2d log(final Pose2d transform) { - final double dtheta = transform.getRotation().getRadians(); - final double half_dtheta = 0.5 * dtheta; - final double cos_minus_one = transform.getRotation().getCos() - 1.0; - double halftheta_by_tan_of_halfdtheta; - if (Math.abs(cos_minus_one) < 1E-9) { - halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; - } else { - halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().getSin()) / cos_minus_one; - } - final Translation2d translation_part = transform.getTranslation() - .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta)); - return new Twist2d(translation_part.getX(), translation_part.getY(), dtheta); - } - - public Pose2d inverse(Translation2d translation, Rotation2d rotation) { - Rotation2d rotation_inverted = Rotation2d.fromRadians(-rotation.getRadians()); - return new Pose2d(new Translation2d(-translation.getX(), -translation.getY()).rotateBy(rotation_inverted), - rotation_inverted); - } - - public boolean shouldAcceptVision( - double timestamp, - Pose2d visionFieldToVehicle, - Pose2d lastFieldToVehicle, - Twist2d robotVelocity, - boolean isInAuto) { - - // If first update, trust - if (mLastVisionFieldToVehicle == null) { - mLastVisionFieldToVehicle = visionFieldToVehicle; - return true; - } - - // Write last pose early because we return out of the method - mLastVisionFieldToVehicle = visionFieldToVehicle; - - // Check out of field - if (visionFieldToVehicle.getTranslation().getX() < -kFieldBorderMargin - || visionFieldToVehicle.getTranslation().getX() > FieldLayout.FIELD_LENGTH + kFieldBorderMargin - || visionFieldToVehicle.getTranslation().getY() < -kFieldBorderMargin - || visionFieldToVehicle.getTranslation().getY() > FieldLayout.FIELD_WIDTH + kFieldBorderMargin) { - SmartDashboard.putString("Vision validation", "Outside field"); - return false; - } - - if ((robotVelocity.dy == 0 - ? Math.abs(robotVelocity.dx) - : Math.hypot(robotVelocity.dx, robotVelocity.dy)) > 4.0) { - SmartDashboard.putString("Vision validation", "Max velocity"); - return false; - } - - if (isInAuto) { - // Check max correction - final Pose2d inverse = inverse(visionFieldToVehicle.getTranslation(), visionFieldToVehicle.getRotation()); - final Twist2d logTwist = log(new Pose2d( - inverse.getTranslation().plus(lastFieldToVehicle.getTranslation().rotateBy(inverse.getRotation())), - inverse.getRotation().rotateBy(lastFieldToVehicle.getRotation()))); - if ((logTwist.dy == 0 - ? Math.abs(logTwist.dx) - : Math.hypot(logTwist.dx, logTwist.dy)) > kMaxVisionCorrection) { - SmartDashboard.putString("Vision validation", "Max correction"); - return false; - } - } - - SmartDashboard.putString("Vision validation", "OK"); - return true; - } -} From bf549970835d5cdc09bf565c2f00d3481cd997d8 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Fri, 10 Oct 2025 22:58:28 -0700 Subject: [PATCH 05/36] Deprecated robot/lib/drivers folder and its classes (CanDeviceId and Phoenix6Util) --- .../frc/robot/lib/drivers/CanDeviceId.java | 24 --------- .../frc/robot/lib/drivers/Phoenix6Util.java | 53 ------------------- 2 files changed, 77 deletions(-) delete mode 100644 src/main/java/frc/robot/lib/drivers/CanDeviceId.java delete mode 100644 src/main/java/frc/robot/lib/drivers/Phoenix6Util.java diff --git a/src/main/java/frc/robot/lib/drivers/CanDeviceId.java b/src/main/java/frc/robot/lib/drivers/CanDeviceId.java deleted file mode 100644 index be7c444..0000000 --- a/src/main/java/frc/robot/lib/drivers/CanDeviceId.java +++ /dev/null @@ -1,24 +0,0 @@ -package frc.robot.lib.drivers; - -public class CanDeviceId { - private final int deviceNumber; - private final String bus; - - public CanDeviceId(int deviceNumber, String bus) { - this.deviceNumber = deviceNumber; - this.bus = bus; - } - - // Use the default bus name (empty string). - public CanDeviceId(int deviceNumber) { - this(deviceNumber, ""); - } - - public int getDeviceNumber() { return deviceNumber; } - - public String getBus() { return bus; } - - public boolean equals(CanDeviceId other) { - return other.deviceNumber == deviceNumber && other.bus == bus; - } -} diff --git a/src/main/java/frc/robot/lib/drivers/Phoenix6Util.java b/src/main/java/frc/robot/lib/drivers/Phoenix6Util.java deleted file mode 100644 index 2e8deb5..0000000 --- a/src/main/java/frc/robot/lib/drivers/Phoenix6Util.java +++ /dev/null @@ -1,53 +0,0 @@ -package frc.robot.lib.drivers; - -import com.ctre.phoenix6.StatusCode; - -import edu.wpi.first.wpilibj.DriverStation; - -import java.util.function.Supplier; - -public class Phoenix6Util { - /** - * Checks the specified error code for issues - * - * @param statusCode error code - * @param message message to print if error happens - */ - public static void checkError(StatusCode statusCode, String message) { - if (statusCode != StatusCode.OK) { - DriverStation.reportError(message + " " + statusCode, false); - } - } - - public static boolean checkErrorAndRetry(Supplier function, int numTries) { - StatusCode code = function.get(); - int tries = 0; - while (code != StatusCode.OK && tries < numTries) { - DriverStation.reportWarning("Retrying CTRE Device Config " + code.getName(), false); - code = function.get(); - tries++; - } - if (code != StatusCode.OK) { - DriverStation.reportError("Failed to execute phoenix pro api call after " + numTries + " attempts, status code " + code.getName() + " description: " + code.getDescription(), false); - return false; - } - return true; - } - - /** - * checks the specified error code and throws an exception if there are any - * issues - * - * @param statusCode error code - * @param message message to print if error happens - */ - public static void checkErrorWithThrow(StatusCode statusCode, String message) { - if (statusCode != StatusCode.OK) { - throw new RuntimeException(message + " " + statusCode); - } - } - - public static boolean checkErrorAndRetry(Supplier function) { - return checkErrorAndRetry(function, 1); - } -} From c37ed3c6120d13fd0f00be30f7c32b958a585222 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Sun, 12 Oct 2025 10:12:58 -0700 Subject: [PATCH 06/36] reformat to prepare for sysID tuning --- .../subsystems/drive/ctre/CtreDriveConstants.java | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 6de0650..986f8ef 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -28,14 +28,21 @@ public class CtreDriveConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) - .withKS(0.1).withKV(2.66).withKA(0) + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(2.66) + .withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); + .withKP(0.1) + .withKI(0) + .withKD(0) + .withKS(0) + .withKV(0.124); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors From 78c351d69598f011bdd189de3d62896a2fbf2e03 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sun, 12 Oct 2025 18:19:35 -0700 Subject: [PATCH 07/36] feat: automode scaffolding and PIDV, validation on first complex path on the field. --- simgui-ds.json | 8 +- .../deploy/pathplanner/paths/testPath.path | 91 +++++++++++++++ src/main/deploy/pathplanner/paths/zisen.path | 87 +++++++++------ src/main/java/frc/robot/Robot.java | 4 +- .../java/frc/robot/auto/AutoRoutines.java | 4 +- .../frc/robot/lib/control/PIDVController.java | 1 + .../lib/control/ProfiledPIDVController.java | 104 +++++++++++++----- src/main/java/frc/robot/lib/util/Util.java | 4 + .../drive/commands/PIDToPoseCommand.java | 40 ++++--- .../drive/commands/TrajectoryCommand.java | 21 ++-- 10 files changed, 264 insertions(+), 100 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/testPath.path diff --git a/simgui-ds.json b/simgui-ds.json index 2296812..e12022f 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -16,13 +16,17 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], - "axisCount": 3, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 90, diff --git a/src/main/deploy/pathplanner/paths/testPath.path b/src/main/deploy/pathplanner/paths/testPath.path new file mode 100644 index 0000000..7ecb284 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/testPath.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.6543032786885246, + "y": 0.15896516393442653 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.870081967215316, + "y": 0.7583504098420167 + }, + "prevControl": { + "x": 2.025922131149742, + "y": 0.386731557383 + }, + "nextControl": { + "x": 1.5354564647707203, + "y": 1.5563035310560531 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4265368852459013, + "y": 1.6094774590163934 + }, + "prevControl": { + "x": 1.798155737707119, + "y": 1.6574282786944752 + }, + "nextControl": { + "x": 0.3915104179993174, + "y": 1.4759256567752452 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": { + "x": 0.45553278688744697, + "y": 0.9501536885305403 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.57, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/zisen.path b/src/main/deploy/pathplanner/paths/zisen.path index cfcaeb2..092f40d 100644 --- a/src/main/deploy/pathplanner/paths/zisen.path +++ b/src/main/deploy/pathplanner/paths/zisen.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 0.25, - "y": 0.0 + "x": 0.38428120678715416, + "y": 0.028820911848564158 }, "isLocked": false, "linkedName": null @@ -20,28 +20,28 @@ "y": 4.102920081967213 }, "prevControl": { - "x": 0.05027796255960154, - "y": 3.8580280108810805 + "x": 0.05037403257065813, + "y": 3.702002286192967 }, "nextControl": { - "x": -0.050277962559601555, - "y": 4.3478121530533445 + "x": -0.03116664423415783, + "y": 4.350969754990947 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": -0.13186475409099274, - "y": 8.298616803285734 + "x": 0.1, + "y": 6.299 }, "prevControl": { - "x": -0.09533771451619764, - "y": 7.877164046497824 + "x": 0.10557581830210679, + "y": 5.920374832566059 }, "nextControl": { - "x": -0.16839179366578771, - "y": 8.720069560073641 + "x": 0.09434292934831419, + "y": 6.683142596947763 }, "isLocked": false, "linkedName": null @@ -52,12 +52,12 @@ "y": 3.647387295081966 }, "prevControl": { - "x": 0.2877049180298839, - "y": 3.8272028688595094 + "x": 0.11181482097517709, + "y": 4.140432055584381 }, "nextControl": { - "x": 1.7690012112975215, - "y": 2.9726088534753377 + "x": 1.7288144064715079, + "y": 2.505277147468517 }, "isLocked": false, "linkedName": null @@ -68,28 +68,28 @@ "y": 3.647387295081966 }, "prevControl": { - "x": 4.894373757716017, - "y": 3.898766036258368 + "x": 4.930116297867843, + "y": 3.8558936606051346 }, "nextControl": { - "x": 5.271200012763925, - "y": 3.396008553905564 + "x": 5.230480503895737, + "y": 3.445678110838003 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.082786885239971, - "y": 7.435502049180025 + "x": 5.22663934425693, + "y": 6.404559426232486 }, "prevControl": { - "x": 5.111974375425232, - "y": 8.2308611566828 + "x": 5.22034016514926, + "y": 6.860885487783113 }, "nextControl": { - "x": 5.03483606556481, - "y": 6.128842213106792 + "x": 5.248769225007602, + "y": 4.801423342477989 }, "isLocked": false, "linkedName": null @@ -100,12 +100,12 @@ "y": 4.39062499999939 }, "prevControl": { - "x": 5.166700819660269, - "y": 6.2607069672118945 + "x": 5.356425699711494, + "y": 6.167810932873377 }, "nextControl": { - "x": 4.976840630520012, - "y": 2.029537037502777 + "x": 4.717422490835596, + "y": 2.0177143317431057 }, "isLocked": false, "linkedName": null @@ -116,15 +116,36 @@ "y": 0.7103995901636302 }, "prevControl": { - "x": 5.154713114748167, - "y": 1.9810963114751057 + "x": 5.013218512474755, + "y": 1.7634618654482566 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 1.3462614954018397, + "rotationDegrees": 147.49861635916233 + }, + { + "waypointRelativePos": 2.1523390643742495, + "rotationDegrees": -91.35349039382996 + }, + { + "waypointRelativePos": 2.9696121551379466, + "rotationDegrees": 62.38625776485705 + }, + { + "waypointRelativePos": 4.648940423830468, + "rotationDegrees": -153.59782498299396 + }, + { + "waypointRelativePos": 6.563374650139956, + "rotationDegrees": 0.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4b660fb..b5b19d5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -44,7 +44,9 @@ public Robot() { // RobotState.resetKalman(); Drive.getInstance(); - VisionDeviceManager.getInstance(); + if (Robot.isReal()) { + VisionDeviceManager.getInstance(); + } TelemetryManager.getInstance(); autoChooser = new AutoSelector(); diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index 09128d2..df3e4da 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -11,12 +11,12 @@ public final class AutoRoutines { public static Command testPidToPose() { - return new PIDToPoseCommand(new Pose2d(3, 0.5, Rotation2d.fromDegrees(50))); + return new PIDToPoseCommand(new Pose2d(3, 3, Rotation2d.fromDegrees(50))); } public static Command testTrajectoryAuto() { RedTrajectory traj = TrajectoryLoader.loadAutoTrajectory(TrajectoryType.PATHPLANNER, - "zisen").get(); + "testPath").get(); return new TrajectoryCommand(traj); } } diff --git a/src/main/java/frc/robot/lib/control/PIDVController.java b/src/main/java/frc/robot/lib/control/PIDVController.java index 9d57226..38a852a 100644 --- a/src/main/java/frc/robot/lib/control/PIDVController.java +++ b/src/main/java/frc/robot/lib/control/PIDVController.java @@ -34,6 +34,7 @@ public PIDVController(PIDFConstants constants) { public PIDVController(PIDConstants constants) { this(new PIDFConstants(constants)); } + /** * Makes the controller continuous, which means that values repeat. * @param minInput The minimum value. diff --git a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java index 2fb6fc0..e467aff 100644 --- a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java +++ b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java @@ -2,56 +2,76 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Timer; import frc.robot.lib.control.ControlConstants.*; +/** + * A profiled version of {@link PIDVController} that uses a TrapezoidProfile + * to smoothly reach the target position with velocity and acceleration limits. + * The derivative term is based on velocity feedback (like PIDV). + */ public class ProfiledPIDVController { - private final ProfiledPIDFConstants constants; + private final PIDFConstants constants; + private TrapezoidProfile.Constraints constraints; + private TrapezoidProfile profile; + + private TrapezoidProfile.State goal = new TrapezoidProfile.State(); + private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); - private double target = 0.0; private Pair measurement = new Pair<>(0.0, 0.0); private double feedforward = 0.0; private double integral = 0.0; public double error = 0.0; - private boolean continuous = false; + private boolean isContinuous = false; private double minRange = 0.0; private double maxRange = 0.0; private final Timer timer = new Timer(); /** - * A {@link PIDVController} with a trapezoid profile, used for limiting speed and acceleration. - * @param constants The {@link ProfiledPIDFConstants}. + * Creates a profiled PIDV controller. + * @param constants The {@link PIDFConstants}. + * @param constraints The trapezoidal motion constraints (max velocity and acceleration). */ public ProfiledPIDVController(ProfiledPIDFConstants constants) { - this.constants = constants; + this.constants = new PIDFConstants( + constants.kP, + constants.kI, + constants.kD, + constants.kF); + this.constraints = constants.constraints; + this.profile = new TrapezoidProfile(constraints); timer.start(); } - /** - * Makes the controller continuous, which means that values repeat. - * @param minInput The minimum value. - * @param maxInput The maximum value. - */ + /** Enables continuous input. */ public void enableContinuousInput(double minInput, double maxInput) { - continuous = true; - this.minRange = minInput; - this.maxRange = maxInput; + isContinuous = true; + minRange = minInput; + maxRange = maxInput; } - /** Makes the controller discontinuous */ + /** Disables continuous input. */ public void disableContinuousInput() { - continuous = false; + isContinuous = false; } + /** Sets the current position and velocity measurement. */ public void setInput(Pair input) { - measurement = input; + this.measurement = input; } + /** Sets the goal position for the profile (end velocity = 0). */ public void setTarget(Double target) { - this.target = target; + this.goal = new TrapezoidProfile.State(target, 0.0); + } + + /** Sets the goal with both position and end velocity. */ + public void setTarget(Double position, Double velocity) { + this.goal = new TrapezoidProfile.State(position, velocity); } /** Sets the feedforward value. */ @@ -59,38 +79,66 @@ public void setFeedforward(Double feedforward) { this.feedforward = feedforward; } + /** + * Computes the controller output using the current profile and measurements. + * @return Control output (PID + feedforward). + */ public Double getOutput() { double dt = timer.get(); timer.reset(); - if (dt <= 0.0) return 0.0; + if (dt <= 0.0) { + return 0.0; + } + + setpoint = profile.calculate(dt, goal, setpoint); double position = measurement.getFirst(); double velocity = measurement.getSecond(); - error = continuous - ? MathUtil.inputModulus(target - position, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) - : target - position; + double targetPosition = setpoint.position; + double targetVelocity = setpoint.velocity; + + error = isContinuous + ? MathUtil.inputModulus(targetPosition - position, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) + : targetPosition - position; integral += error * dt; - double derivative = feedforward - velocity; + + double derivative = targetVelocity - velocity; return constants.kP * error - + constants.kI * integral - + constants.kD * derivative - + constants.kF * feedforward; + + constants.kI * integral + + constants.kD * derivative + + constants.kF * feedforward; } - /** Sets the integral value. */ + /** Sets the integral term directly. */ public void setIntegral(double integral) { this.integral = integral; } - /** Resets the controller. */ + /** Resets the controller state. */ public void reset() { integral = 0.0; feedforward = 0.0; error = 0.0; + setpoint = new TrapezoidProfile.State(measurement.getFirst(), measurement.getSecond()); timer.reset(); timer.start(); } + + /** Returns the current motion profile setpoint. */ + public TrapezoidProfile.State getSetpoint() { + return setpoint; + } + + /** Returns the goal state. */ + public TrapezoidProfile.State getGoal() { + return goal; + } + + /** Updates motion constraints. */ + public void setConstraints(TrapezoidProfile.Constraints constraints) { + this.constraints = constraints; + } } diff --git a/src/main/java/frc/robot/lib/util/Util.java b/src/main/java/frc/robot/lib/util/Util.java index 703f41c..7f802b4 100644 --- a/src/main/java/frc/robot/lib/util/Util.java +++ b/src/main/java/frc/robot/lib/util/Util.java @@ -232,6 +232,10 @@ public static boolean chassisSpeedsEpsilonEquals(ChassisSpeeds speed1, ChassisSp && MathUtils.epsilonEquals(speed1.omegaRadiansPerSecond, other.omegaRadiansPerSecond, epsilon); } + public static double chassisSpeedsMagnitude(ChassisSpeeds other) { + return Math.sqrt(other.vxMetersPerSecond * other.vxMetersPerSecond + + other.vyMetersPerSecond * other.vyMetersPerSecond); + } public static Translation2d translateBy(Translation2d a, Translation2d b){ return new Translation2d(a.getX()+b.getX(),a.getY()+b.getY()); diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index 06272da..e975609 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; +import frc.robot.lib.util.Util; import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.subsystems.drive.Drive; @@ -18,8 +19,7 @@ public class PIDToPoseCommand extends Command { private final SwerveRequest.ApplyFieldSpeeds request = new SwerveRequest.ApplyFieldSpeeds(); - private final ProfiledPIDVController xController; - private final ProfiledPIDVController yController; + private final ProfiledPIDVController translationController; private final ProfiledPIDVController thetaController; private final Pose2d target; @@ -37,8 +37,7 @@ public PIDToPoseCommand(Pose2d target) { public PIDToPoseCommand(Drive drive, Pose2d target, ProfiledPIDFConstants translationConstants, ProfiledPIDFConstants rotationConstants) { this.drive = drive; this.target = target; - xController = new ProfiledPIDVController(translationConstants); - yController = new ProfiledPIDVController(translationConstants); + translationController = new ProfiledPIDVController(translationConstants); thetaController = new ProfiledPIDVController(rotationConstants); thetaController.enableContinuousInput(-Math.PI, Math.PI); @@ -68,15 +67,19 @@ public ChassisSpeeds calculateSpeeds() { if (target == null || currentPose == null || currentSpeeds == null) { return new ChassisSpeeds(); } + + var delta = target.relativeTo(currentPose); - xController.setTarget(target.getX()); - yController.setTarget(target.getY()); + translationController.setTarget(delta.getTranslation().getNorm()); - xController.setInput(new Pair(currentPose.getX(), currentSpeeds.vxMetersPerSecond)); - yController.setInput(new Pair(currentPose.getY(), currentSpeeds.vyMetersPerSecond)); + translationController.setInput(new Pair( + 0.0, + Util.chassisSpeedsMagnitude( + currentSpeeds))); + + double vMagnitude = translationController.getOutput(); - double vx = xController.getOutput(); - double vy = yController.getOutput(); + var deltaRotation = delta.getTranslation().getAngle(); thetaController.setTarget(target.getRotation().getRadians()); thetaController.setInput( @@ -85,21 +88,16 @@ public ChassisSpeeds calculateSpeeds() { double rotation = thetaController.getOutput(); - return ChassisSpeeds.fromFieldRelativeSpeeds( - vx, - vy, - rotation, - currentPose.getRotation()); + return new ChassisSpeeds( + vMagnitude * deltaRotation.getCos(), + vMagnitude * deltaRotation.getSin(), + rotation); } @Override public boolean isFinished() { - if (Math.hypot(xController.error, yController.error) < 0.03 - && MathUtil.isNear(thetaController.error / Math.PI * 180, 0, 3.0)) - { - return true; - } - return false; + return translationController.error < 0.03 + && MathUtil.isNear(thetaController.error / Math.PI * 180, 0, 3.0); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index d275ba3..1760731 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -5,19 +5,17 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; -// import frc.robot.RobotState; import frc.robot.lib.control.ControlConstants.PIDFConstants; import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; import frc.robot.lib.control.PIDVController; import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.lib.trajectory.RedTrajectory; +import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.Drive; /** @@ -37,14 +35,10 @@ public class TrajectoryCommand extends Command { private final RedTrajectory trajectory; private Pose2d currentPose; private ChassisSpeeds currentSpeeds; + private RedTrajectory.State targetState = new RedTrajectory.State(); private Timer timer = null; - static Field2d field = new Field2d(); - static { - SmartDashboard.putData("debug", field); - } - public TrajectoryCommand(RedTrajectory trajectory) { this( Drive.getInstance(), @@ -71,6 +65,10 @@ public TrajectoryCommand(Drive drive, RedTrajectory trajectory, PIDFConstants tr timer = new Timer(); addRequirements(drive); + TelemetryManager.getInstance() + .addStructPublisher("Debug/TrajectoryCommand", + Pose3d.struct, () -> new Pose3d( + targetState.pose)); setName("Trajectory " + trajectory.name); } @@ -98,7 +96,7 @@ public ChassisSpeeds calculateSpeeds() { return new ChassisSpeeds(); } - RedTrajectory.State targetState = trajectory.advanceTo(timer.get()); + targetState = trajectory.advanceTo(timer.get()); double vxFF = targetState.speeds.vxMetersPerSecond; double vyFF = targetState.speeds.vyMetersPerSecond; @@ -136,9 +134,6 @@ public ChassisSpeeds calculateSpeeds() { double rotation = thetaController.getOutput(); - field.setRobotPose(targetState.pose); - SmartDashboard.putData("debug", field); - return new ChassisSpeeds( vx + xAccelFF * accelConstant, vy + yAccelFF * accelConstant, From fd640cbaa85ab8604d9cce2e8113643e5b860dc3 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sun, 19 Oct 2025 15:02:26 -0700 Subject: [PATCH 08/36] fix: pid to pose --- .../deploy/pathplanner/paths/testPath2.path | 91 ++++++++++ src/main/deploy/pathplanner/paths/zisen.path | 171 ------------------ src/main/java/frc/robot/Robot.java | 7 + .../java/frc/robot/auto/AutoRoutines.java | 5 +- .../drive/commands/PIDToPoseCommand.java | 12 +- 5 files changed, 108 insertions(+), 178 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/testPath2.path delete mode 100644 src/main/deploy/pathplanner/paths/zisen.path diff --git a/src/main/deploy/pathplanner/paths/testPath2.path b/src/main/deploy/pathplanner/paths/testPath2.path new file mode 100644 index 0000000..bb4596e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/testPath2.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.6543032786885246, + "y": 0.15896516393442653 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.194, + "y": 0.9022028688524597 + }, + "prevControl": { + "x": 2.1700245901639343, + "y": 0.4107069672131138 + }, + "nextControl": { + "x": 2.236158480441017, + "y": 1.7664517178933052 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4265368852459013, + "y": 1.6094774590163934 + }, + "prevControl": { + "x": 1.798155737707119, + "y": 1.6574282786944752 + }, + "nextControl": { + "x": 0.3915104179993174, + "y": 1.4759256567752452 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": { + "x": 0.45553278688744697, + "y": 0.9501536885305403 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.57, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/zisen.path b/src/main/deploy/pathplanner/paths/zisen.path deleted file mode 100644 index 092f40d..0000000 --- a/src/main/deploy/pathplanner/paths/zisen.path +++ /dev/null @@ -1,171 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.0, - "y": 0.0 - }, - "prevControl": null, - "nextControl": { - "x": 0.38428120678715416, - "y": 0.028820911848564158 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.0, - "y": 4.102920081967213 - }, - "prevControl": { - "x": 0.05037403257065813, - "y": 3.702002286192967 - }, - "nextControl": { - "x": -0.03116664423415783, - "y": 4.350969754990947 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.1, - "y": 6.299 - }, - "prevControl": { - "x": 0.10557581830210679, - "y": 5.920374832566059 - }, - "nextControl": { - "x": 0.09434292934831419, - "y": 6.683142596947763 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.5993852458972976, - "y": 3.647387295081966 - }, - "prevControl": { - "x": 0.11181482097517709, - "y": 4.140432055584381 - }, - "nextControl": { - "x": 1.7288144064715079, - "y": 2.505277147468517 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.082786885239971, - "y": 3.647387295081966 - }, - "prevControl": { - "x": 4.930116297867843, - "y": 3.8558936606051346 - }, - "nextControl": { - "x": 5.230480503895737, - "y": 3.445678110838003 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.22663934425693, - "y": 6.404559426232486 - }, - "prevControl": { - "x": 5.22034016514926, - "y": 6.860885487783113 - }, - "nextControl": { - "x": 5.248769225007602, - "y": 4.801423342477989 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.082786885239971, - "y": 4.39062499999939 - }, - "prevControl": { - "x": 5.356425699711494, - "y": 6.167810932873377 - }, - "nextControl": { - "x": 4.717422490835596, - "y": 2.0177143317431057 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.082786885239971, - "y": 0.7103995901636302 - }, - "prevControl": { - "x": 5.013218512474755, - "y": 1.7634618654482566 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.3462614954018397, - "rotationDegrees": 147.49861635916233 - }, - { - "waypointRelativePos": 2.1523390643742495, - "rotationDegrees": -91.35349039382996 - }, - { - "waypointRelativePos": 2.9696121551379466, - "rotationDegrees": 62.38625776485705 - }, - { - "waypointRelativePos": 4.648940423830468, - "rotationDegrees": -153.59782498299396 - }, - { - "waypointRelativePos": 6.563374650139956, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -171.98615915245963 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b5b19d5..32e5b66 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,6 +1,11 @@ package frc.robot; import frc.robot.auto.AutoSelector; + +import com.pathplanner.lib.commands.FollowPathCommand; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.commands.PathfindingCommand; + import edu.wpi.first.hal.AllianceStationID; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -49,6 +54,8 @@ public Robot() { } TelemetryManager.getInstance(); + FollowPathCommand.warmupCommand().schedule();; + autoChooser = new AutoSelector(); } diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index df3e4da..5a36154 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -11,12 +11,13 @@ public final class AutoRoutines { public static Command testPidToPose() { - return new PIDToPoseCommand(new Pose2d(3, 3, Rotation2d.fromDegrees(50))); + return new PIDToPoseCommand(new Pose2d(1.5, 1, Rotation2d.fromDegrees(90))); } public static Command testTrajectoryAuto() { RedTrajectory traj = TrajectoryLoader.loadAutoTrajectory(TrajectoryType.PATHPLANNER, - "testPath").get(); + "testPath2").get(); return new TrajectoryCommand(traj); } } + diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index e975609..c70761c 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -5,7 +5,9 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; @@ -68,9 +70,9 @@ public ChassisSpeeds calculateSpeeds() { return new ChassisSpeeds(); } - var delta = target.relativeTo(currentPose); + var delta = target.getTranslation().minus(currentPose.getTranslation()); - translationController.setTarget(delta.getTranslation().getNorm()); + translationController.setTarget(delta.getNorm()); translationController.setInput(new Pair( 0.0, @@ -78,8 +80,8 @@ public ChassisSpeeds calculateSpeeds() { currentSpeeds))); double vMagnitude = translationController.getOutput(); - - var deltaRotation = delta.getTranslation().getAngle(); + SmartDashboard.putNumber("Debug/PIDToPoseCommand/vmag", vMagnitude); + var deltaRotation = delta.getAngle(); thetaController.setTarget(target.getRotation().getRadians()); thetaController.setInput( @@ -102,7 +104,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - drive.setSwerveRequest(new SwerveRequest.FieldCentric()); + drive.setSwerveRequest(new SwerveRequest.ApplyRobotSpeeds()); } public Pose2d getTarget() { From 67d078738e6b698417db8c547fba4c8086fec3b8 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sun, 19 Oct 2025 15:34:22 -0700 Subject: [PATCH 09/36] tested snap and it work :D --- src/main/java/frc/robot/ControlsMapping.java | 4 ++++ src/main/java/frc/robot/auto/AutoRoutines.java | 3 ++- src/main/java/frc/robot/subsystems/drive/Drive.java | 6 +++--- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 6665969..e116766 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -3,8 +3,10 @@ import static frc.robot.Robot.controller; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.commands.PIDToPoseCommand; //import frc.robot.subsystems.drive.commands.TeleopCommand; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; @@ -15,6 +17,8 @@ public static void mapTeleopCommand() { Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); + controller.x().whileTrue(new PIDToPoseCommand( + new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120.0)))); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index 5a36154..f2567db 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -11,7 +11,8 @@ public final class AutoRoutines { public static Command testPidToPose() { - return new PIDToPoseCommand(new Pose2d(1.5, 1, Rotation2d.fromDegrees(90))); + return new PIDToPoseCommand( + new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120))); } public static Command testTrajectoryAuto() { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 9176245..54e6758 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -114,9 +114,9 @@ public Command teleopCommand() { teleopRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0); setSwerveRequest(teleopRequest); }).andThen(run(() -> { - double xDesiredRaw = Robot.controller.getLeftY(); - double yDesiredRaw = Robot.controller.getLeftX(); - double rotDesiredRaw = Robot.controller.getRightX(); + double xDesiredRaw = -Robot.controller.getLeftY(); + double yDesiredRaw = -Robot.controller.getLeftX(); + double rotDesiredRaw = -Robot.controller.getRightX(); double xFancy = Util.applyJoystickDeadband(xDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); double yFancy = Util.applyJoystickDeadband(yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); double rotFancy = Util.applyJoystickDeadband(rotDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); From 206d515e6422a23946d137d69c3e2abf810a6b6a Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Wed, 29 Oct 2025 17:36:38 -0700 Subject: [PATCH 10/36] minor refactor --- src/main/java/frc/robot/ControlsMapping.java | 4 ++- .../frc/robot/lib/control/PIDController.java | 8 ++--- .../frc/robot/lib/control/PIDVController.java | 30 +++++++++---------- .../lib/control/ProfiledPIDVController.java | 27 ++++++++--------- .../robot/lib/localization/FieldLayout.java | 20 +++++++++++++ .../frc/robot/subsystems/drive/Drive.java | 16 ++++++++++ .../commands/ExtendedTrajectoryCommand.java | 4 +-- .../drive/commands/PIDToPoseCommand.java | 7 ++--- .../drive/commands/TrajectoryCommand.java | 7 ++--- 9 files changed, 79 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index e116766..51a6994 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -17,7 +17,9 @@ public static void mapTeleopCommand() { Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); - controller.x().whileTrue(new PIDToPoseCommand( + controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); + controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); + controller.b().whileTrue(new PIDToPoseCommand( new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120.0)))); } diff --git a/src/main/java/frc/robot/lib/control/PIDController.java b/src/main/java/frc/robot/lib/control/PIDController.java index c2d6ec6..464f113 100644 --- a/src/main/java/frc/robot/lib/control/PIDController.java +++ b/src/main/java/frc/robot/lib/control/PIDController.java @@ -52,20 +52,20 @@ public void disableContinuousInput() { isContinuousInputEnabled = false; } - public void setInput(Double input) { + public void setInput(double input) { this.measurement = input; } - public void setTarget(Double target) { + public void setTarget(double target) { this.target = target; } /** Sets the feedforward value. */ - public void setFeedforward(Double feedforward) { + public void setFeedforward(double feedforward) { this.feedforward = feedforward; } - public Double getOutput() { + public double getOutput() { double dt = timer.get(); timer.reset(); if (dt <= 0.0) { diff --git a/src/main/java/frc/robot/lib/control/PIDVController.java b/src/main/java/frc/robot/lib/control/PIDVController.java index 38a852a..d1de0f3 100644 --- a/src/main/java/frc/robot/lib/control/PIDVController.java +++ b/src/main/java/frc/robot/lib/control/PIDVController.java @@ -8,8 +8,10 @@ public class PIDVController { private final PIDFConstants constants; + private double positionMeasurement = 0.0; + private double velocityMeasurement = 0.0; + private double target = 0.0; - private Pair measurement = new Pair<>(0.0, 0.0); private double feedforward = 0.0; private double integral = 0.0; @@ -51,39 +53,37 @@ public void disableContinuousInput() { isContinuous = false; } - public void setInput(Pair input) { - this.measurement = input; + public void setInput(double positionMeasurement, double velocityMeasurement) { + this.positionMeasurement = positionMeasurement; + this.velocityMeasurement = velocityMeasurement; } - public void setTarget(Double target) { + public void setTarget(double target) { this.target = target; } /** Sets the feedforward value. */ - public void setFeedforward(Double feedforward) { + public void setFeedforward(double feedforward) { this.feedforward = feedforward; } - public Double getOutput() { + public double getOutput() { double dt = timer.get(); timer.reset(); if (dt <= 0.0) return 0.0; - - double position = measurement.getFirst(); - double velocity = measurement.getSecond(); error = isContinuous - ? MathUtil.inputModulus(target - position, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) - : target - position; + ? MathUtil.inputModulus(target - positionMeasurement, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) + : target - positionMeasurement; integral += error * dt; - double derivative = feedforward - velocity; + double derivative = feedforward - velocityMeasurement; return constants.kP * error - + constants.kI * integral - + constants.kD * derivative - + constants.kF * feedforward; + + constants.kI * integral + + constants.kD * derivative + + constants.kF * feedforward; } /** Sets the integral value. */ diff --git a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java index e467aff..438d037 100644 --- a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java +++ b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java @@ -19,7 +19,8 @@ public class ProfiledPIDVController { private TrapezoidProfile.State goal = new TrapezoidProfile.State(); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); - private Pair measurement = new Pair<>(0.0, 0.0); + private double positionMeasurement = 0.0; + private double velocityMeasurement = 0.0; private double feedforward = 0.0; private double integral = 0.0; @@ -60,22 +61,23 @@ public void disableContinuousInput() { } /** Sets the current position and velocity measurement. */ - public void setInput(Pair input) { - this.measurement = input; + public void setInput(double positionMeasurement, double velocityMeasurement) { + this.positionMeasurement = positionMeasurement; + this.velocityMeasurement = velocityMeasurement; } /** Sets the goal position for the profile (end velocity = 0). */ - public void setTarget(Double target) { + public void setTarget(double target) { this.goal = new TrapezoidProfile.State(target, 0.0); } /** Sets the goal with both position and end velocity. */ - public void setTarget(Double position, Double velocity) { + public void setTarget(double position, double velocity) { this.goal = new TrapezoidProfile.State(position, velocity); } /** Sets the feedforward value. */ - public void setFeedforward(Double feedforward) { + public void setFeedforward(double feedforward) { this.feedforward = feedforward; } @@ -83,7 +85,7 @@ public void setFeedforward(Double feedforward) { * Computes the controller output using the current profile and measurements. * @return Control output (PID + feedforward). */ - public Double getOutput() { + public double getOutput() { double dt = timer.get(); timer.reset(); if (dt <= 0.0) { @@ -92,19 +94,16 @@ public Double getOutput() { setpoint = profile.calculate(dt, goal, setpoint); - double position = measurement.getFirst(); - double velocity = measurement.getSecond(); - double targetPosition = setpoint.position; double targetVelocity = setpoint.velocity; error = isContinuous - ? MathUtil.inputModulus(targetPosition - position, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) - : targetPosition - position; + ? MathUtil.inputModulus(targetPosition - positionMeasurement, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) + : targetPosition - positionMeasurement; integral += error * dt; - double derivative = targetVelocity - velocity; + double derivative = targetVelocity - velocityMeasurement; return constants.kP * error + constants.kI * integral @@ -122,7 +121,7 @@ public void reset() { integral = 0.0; feedforward = 0.0; error = 0.0; - setpoint = new TrapezoidProfile.State(measurement.getFirst(), measurement.getSecond()); + setpoint = new TrapezoidProfile.State(positionMeasurement, velocityMeasurement); timer.reset(); timer.start(); } diff --git a/src/main/java/frc/robot/lib/localization/FieldLayout.java b/src/main/java/frc/robot/lib/localization/FieldLayout.java index 8d47028..37032d4 100644 --- a/src/main/java/frc/robot/lib/localization/FieldLayout.java +++ b/src/main/java/frc/robot/lib/localization/FieldLayout.java @@ -1,15 +1,21 @@ package frc.robot.lib.localization; import java.io.IOException; +import java.util.ArrayList; +import java.util.HashSet; +import java.util.List; +import java.util.Set; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; /** * Contains various field dimensions and useful reference points. Dimensions are @@ -35,11 +41,25 @@ public class FieldLayout { public static final double APRITAG_WIDTH = Units.inchesToMeters(6.50); public static final AprilTagFieldLayout APRILTAG_MAP; + public static final List ALIGN_POSES_RIGHT = new ArrayList<>(); + public static final List ALIGN_POSES_LEFT = new ArrayList<>(); + static { try { APRILTAG_MAP = AprilTagFieldLayout.loadFromResource(AprilTagFields.kDefaultField.m_resourceFile); field = new Field2d(); SmartDashboard.putData(field); + int[] reefIds = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; + Transform2d leftReefTransform = new Transform2d( + Constants.Drive.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), -Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); + for (int i : reefIds) { + ALIGN_POSES_LEFT.add(APRILTAG_MAP.getTagPose(i).get().toPose2d().transformBy(leftReefTransform)); + } + Transform2d rightReefTransform = new Transform2d( + Constants.Drive.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); + for (int i : reefIds) { + ALIGN_POSES_RIGHT.add(APRILTAG_MAP.getTagPose(i).get().toPose2d().transformBy(rightReefTransform)); + } } catch (IOException e) { throw new RuntimeException(e); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 54e6758..17c9703 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.drive; +import java.util.Set; + import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; @@ -21,9 +23,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; +import frc.robot.lib.localization.FieldLayout; import frc.robot.lib.util.Util; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.ctre.CtreDriveConstants; +import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.ctre.CtreDrive; import frc.robot.subsystems.drive.ctre.CtreDriveTelemetry; @@ -132,6 +136,18 @@ public Command teleopCommand() { }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Teleop"); } + public Command autoAlign(boolean left) { + return Commands.defer(() -> { + Pose2d pose; + if (left) { + pose = getPose().nearest(FieldLayout.ALIGN_POSES_LEFT); + } else { + pose = getPose().nearest(FieldLayout.ALIGN_POSES_RIGHT); + } + return new PIDToPoseCommand(pose).withName("Auto Align"); + }, Set.of(this)); + } + public void addVisionUpdate(Pose2d pose, Time timestamp) { getCtreDrive().addVisionMeasurement(pose, timestamp.in(Units.Seconds)); } diff --git a/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java index 8e354d8..5688318 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java @@ -15,11 +15,10 @@ public class ExtendedTrajectoryCommand extends TrajectoryCommand { @SafeVarargs public ExtendedTrajectoryCommand(Drive drive, RedTrajectory trajectory, Pair... triggers) { super(trajectory); - addRequirements(drive); setName("Extended Trajectory Command"); this.triggers = new ArrayList<>(List.of(triggers)); - for (Pair trigger : triggers) { + for (Pair trigger : triggers) { new Trigger(() -> trajectory.progress > trigger.getFirst()).onTrue(trigger.getSecond()); addRequirements(trigger.getSecond().getRequirements()); } @@ -27,6 +26,7 @@ public ExtendedTrajectoryCommand(Drive drive, RedTrajectory trajectory, Pair trigger : triggers) { if (trigger.getSecond().isScheduled() && !trigger.getSecond().isFinished()){ diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index c70761c..34c38be 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -74,10 +74,10 @@ public ChassisSpeeds calculateSpeeds() { translationController.setTarget(delta.getNorm()); - translationController.setInput(new Pair( + translationController.setInput( 0.0, Util.chassisSpeedsMagnitude( - currentSpeeds))); + currentSpeeds)); double vMagnitude = translationController.getOutput(); SmartDashboard.putNumber("Debug/PIDToPoseCommand/vmag", vMagnitude); @@ -85,8 +85,7 @@ public ChassisSpeeds calculateSpeeds() { thetaController.setTarget(target.getRotation().getRadians()); thetaController.setInput( - new Pair( - currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond)); + currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond); double rotation = thetaController.getOutput(); diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index 1760731..b2b1ee2 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -120,8 +120,8 @@ public ChassisSpeeds calculateSpeeds() { xController.setFeedforward(vxFF); yController.setFeedforward(vyFF); - xController.setInput(new Pair(currentPose.getX(), currentSpeeds.vxMetersPerSecond)); - yController.setInput(new Pair(currentPose.getY(), currentSpeeds.vyMetersPerSecond)); + xController.setInput(currentPose.getX(), currentSpeeds.vxMetersPerSecond); + yController.setInput(currentPose.getY(), currentSpeeds.vyMetersPerSecond); double vx = xController.getOutput(); double vy = yController.getOutput(); @@ -129,8 +129,7 @@ public ChassisSpeeds calculateSpeeds() { thetaController.setTarget(targetState.pose.getRotation().getRadians()); thetaController.setFeedforward(targetState.speeds.omegaRadiansPerSecond); thetaController.setInput( - new Pair( - currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond)); + currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond); double rotation = thetaController.getOutput(); From 9f4c42ce1131c2ccfa9d94bc5f487df394565e4d Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Wed, 29 Oct 2025 17:39:49 -0700 Subject: [PATCH 11/36] refactor of the refactor --- .../frc/robot/subsystems/drive/commands/PIDToPoseCommand.java | 2 -- .../frc/robot/subsystems/drive/commands/TrajectoryCommand.java | 1 - 2 files changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index 34c38be..4dca0f1 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -3,9 +3,7 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index b2b1ee2..e4acfa2 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -3,7 +3,6 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; From db9d7da4b241b60b9b35e9b2677b9fbf75f33275 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Mon, 24 Nov 2025 17:10:40 -0800 Subject: [PATCH 12/36] feat: sysid tuning for drive-motors, verified on robot. --- src/main/java/frc/robot/ControlsMapping.java | 49 +++++++++++++++---- src/main/java/frc/robot/Robot.java | 15 ++++++ .../subsystems/drive/ctre/CtreDrive.java | 30 ++++++++++-- .../drive/ctre/CtreDriveConstants.java | 32 ++++++++---- 4 files changed, 102 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index e116766..af87f1c 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -2,19 +2,23 @@ import static frc.robot.Robot.controller; +import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.commands.PIDToPoseCommand; //import frc.robot.subsystems.drive.commands.TeleopCommand; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; +import edu.wpi.first.wpilibj2.command.ProxyCommand; public class ControlsMapping { public static void mapTeleopCommand() { Drive.getInstance().setDefaultCommand((Drive.getInstance().teleopCommand())); - // run sysID functions - Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); + // // run sysID functions + // Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); controller.x().whileTrue(new PIDToPoseCommand( @@ -22,13 +26,38 @@ public static void mapTeleopCommand() { } public static void mapSysId() { - controller.a().onTrue( - Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward)); - controller.b().onTrue( - Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse)); - controller.x().onTrue( - Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward)); - controller.y().onTrue( - Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse)); + //set up sysID routine type + controller.a().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.TRANSLATION))); + controller.b().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.ROTATION))); + controller.back().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER))); + //map the sysid routine movement directions + controller.leftBumper().and(controller.x()) + .whileTrue( + new ProxyCommand( + ()->Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward) + .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) + ) + ); + controller.leftBumper().and(controller.y()) + .whileTrue( + new ProxyCommand( + ()->Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse) + .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) + ) + ); + controller.rightBumper().and(controller.x()) + .whileTrue( + new ProxyCommand( + ()->Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward) + .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) + ) + ); + controller.rightBumper().and(controller.y()) + .whileTrue( + new ProxyCommand( + ()->Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse) + .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) + ) + ); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 32e5b66..074bae6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,7 @@ import edu.wpi.first.hal.AllianceStationID; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; @@ -57,6 +58,17 @@ public Robot() { FollowPathCommand.warmupCommand().schedule();; autoChooser = new AutoSelector(); + + //robot data loggers + boolean usbPresent = new java.io.File("/u").exists(); + if (usbPresent) { + DataLogManager.start("/u/logs"); // USB stick + System.out.println("Log/USB mounts OK"); + } else { + DataLogManager.start(); // falls back to /home/lvuser/logs + System.out.println("Log/USB mounts NOT OK"); + } + DriverStation.startDataLog(DataLogManager.getLog()); } /** @@ -129,6 +141,9 @@ public void teleopPeriodic() { public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + + //map test commands + ControlsMapping.mapSysId(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java index f72acfc..183b5ff 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java @@ -52,6 +52,9 @@ public static enum SysIdRoutineType { private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + //class field + private double m_lastAppliedVolts =0.; + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( @@ -59,12 +62,23 @@ public static enum SysIdRoutineType { Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout null, // Use default timeout (10 s) // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), + // state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) + null + ), new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this + output -> { + m_lastAppliedVolts = output.in(Volts); + setControl(m_translationCharacterization.withVolts(output)); + }, + log -> { + var s = getStateCopy(); + log.motor("drive") + .voltage(Volts.of(m_lastAppliedVolts)) + .linearPosition(Meters.of(s.Pose.getTranslation().getX())) // or avg wheel distance + .linearVelocity(MetersPerSecond.of(getKinematics() + .toChassisSpeeds(s.ModuleStates).vxMetersPerSecond)); + }, + this ) ); @@ -208,12 +222,18 @@ public void setSysIdRoutine(SysIdRoutineType type) { switch(type) { case ROTATION: m_sysIdRoutineToApply = m_sysIdRoutineRotation; + System.out.println("SysID/Rotation routine set"); + break; case STEER: m_sysIdRoutineToApply = m_sysIdRoutineSteer; + System.out.println("SysID/Steer routine set"); + break; case TRANSLATION: m_sysIdRoutineToApply = m_sysIdRoutineTranslation; + System.out.println("SysID/Translation routine set"); + break; default: break; diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 986f8ef..d73ba5d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -23,7 +23,25 @@ // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class CtreDriveConstants { - // Both sets of gains need to be tuned to your individual robot. + // mechanical and geometric parameters of drive train + public static final double kDriveGearRatio = 6.122448979591837; + public static final double kSteerGearRatio = 21.428571428571427; + public static final Distance kWheelRadius = Inches.of(2); + + // Both sets of gains need to be tuned to your individual robot via sysid. + + // translation sysid results + static double kS_sysid_drive = 0.12903; + static double kV_sysid_drive = 2.3293; + static double kA_sysid_drive = 0.41181; + static double kP_sysid_drive = 2.2622; + static double kD_sysid_drive = 0.; + // covnert sysid gains into CTRE (rotation/s) units + static double kS_ctre_drive = kS_sysid_drive; + static double kV_ctre_drive = kV_sysid_drive * (2.0*Math.PI*kWheelRadius.in(Meters)/kDriveGearRatio); + static double kA_ctre_drive = kA_sysid_drive * (2.0*Math.PI*kWheelRadius.in(Meters)/kDriveGearRatio); + static double kP_ctre_drive = kP_sysid_drive * (2.0*Math.PI*kWheelRadius.in(Meters)/kDriveGearRatio); + static double kD_ctre_drive = kD_sysid_drive * (2.0*Math.PI*kWheelRadius.in(Meters)/kDriveGearRatio); // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput @@ -38,11 +56,11 @@ public class CtreDriveConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1) + .withKP(kP_ctre_drive) .withKI(0) - .withKD(0) - .withKS(0) - .withKV(0.124); + .withKD(kD_ctre_drive) + .withKS(kS_ctre_drive) + .withKV(kV_ctre_drive); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -91,10 +109,6 @@ public class CtreDriveConstants { // This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.5714285714285716; - public static final double kDriveGearRatio = 6.122448979591837; - public static final double kSteerGearRatio = 21.428571428571427; - public static final Distance kWheelRadius = Inches.of(2); - private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; From d3d92457d6bbecab20f7b5e84853bc04adbc5bbb Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Tue, 25 Nov 2025 15:35:45 -0800 Subject: [PATCH 13/36] finetuning steer-drive coupling ratio --- .../frc/robot/subsystems/drive/ctre/CtreDriveConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index d73ba5d..2e9d6f6 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -29,7 +29,7 @@ public class CtreDriveConstants { public static final Distance kWheelRadius = Inches.of(2); // Both sets of gains need to be tuned to your individual robot via sysid. - + // translation sysid results static double kS_sysid_drive = 0.12903; static double kV_sysid_drive = 2.3293; @@ -107,7 +107,7 @@ public class CtreDriveConstants { // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.5714285714285716; + private static final double kCoupleRatio = kDriveGearRatio /3.; //(drive gear ratio)/(bevel gear ratio) 3.5714285714285716; private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; From 28e5885a967627b095a459ee1b969e21ce4d933b Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 6 Dec 2025 21:13:03 -0800 Subject: [PATCH 14/36] Merged from 2025RobotTest --- simgui-ds.json | 21 +- src/main/deploy/choreo/jason.traj | 488 +++++++++++------- src/main/java/frc/robot/Constants.java | 188 +------ src/main/java/frc/robot/ControlsMapping.java | 20 +- src/main/java/frc/robot/Robot.java | 29 +- .../java/frc/robot/auto/AutoRoutines.java | 3 + .../java/frc/robot/auto/AutoSelector.java | 40 +- src/main/java/frc/robot/auto/Automation.java | 3 - .../frc/robot/lib/control/PIDVController.java | 54 +- .../lib/control/ProfiledPIDVController.java | 276 +++++----- .../{localization => field}/FieldLayout.java | 10 +- .../{localization => field}/FieldUtil.java | 2 +- .../java/frc/robot/lib/sim/Mechanisms.java | 11 + .../robot/lib/trajectory/RedTrajectory.java | 3 +- src/main/java/frc/robot/lib/util/Util.java | 39 ++ .../robot/subsystems/DefaultSubsystem.java | 7 - .../frc/robot/subsystems/LoggingManager.java | 7 - .../robot/subsystems/TelemetryManager.java | 48 +- .../frc/robot/subsystems/drive/Drive.java | 154 ++---- .../subsystems/drive/DriveConstants.java | 55 ++ .../commands/ExtendedTrajectoryCommand.java | 2 +- .../drive/commands/PIDToPoseCommand.java | 98 +++- .../drive/commands/TrajectoryCommand.java | 69 +-- .../drive/ctre/CtreDriveConstants.java | 6 +- .../robot/subsystems/vision/VisionDevice.java | 3 +- .../vision/VisionDeviceManager.java | 1 + 26 files changed, 866 insertions(+), 771 deletions(-) rename src/main/java/frc/robot/lib/{localization => field}/FieldLayout.java (90%) rename src/main/java/frc/robot/lib/{localization => field}/FieldUtil.java (99%) create mode 100644 src/main/java/frc/robot/lib/sim/Mechanisms.java delete mode 100644 src/main/java/frc/robot/subsystems/DefaultSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/LoggingManager.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveConstants.java diff --git a/simgui-ds.json b/simgui-ds.json index e12022f..23c0a81 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -12,27 +12,29 @@ "incKey": 68 }, { - "decKey": 87, - "incKey": 83 + "decKey": 83, + "incKey": 87 }, { "decayRate": 0.0, - "incKey": 82, "keyRate": 0.009999999776482582 }, {}, { - "decKey": 81, - "incKey": 69 + "decKey": 74, + "incKey": 76 } ], "axisCount": 5, - "buttonCount": 4, + "buttonCount": 7, "buttonKeys": [ 90, 88, 67, - 86 + 86, + 81, + 69, + 82 ], "povConfig": [ { @@ -50,10 +52,7 @@ }, { "axisConfig": [ - { - "decKey": 74, - "incKey": 76 - }, + {}, { "decKey": 73, "incKey": 75 diff --git a/src/main/deploy/choreo/jason.traj b/src/main/deploy/choreo/jason.traj index a562371..b7b18b6 100644 --- a/src/main/deploy/choreo/jason.traj +++ b/src/main/deploy/choreo/jason.traj @@ -3,31 +3,38 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":0.0, "y":0.0, "heading":0.0, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.4578442573547363, "y":4.232089042663574, "heading":0.0, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.869084358215332, "y":2.1505911350250244, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.280324459075928, "y":3.2428622245788574, "heading":0.0, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.434422016143799, "y":3.1810357570648193, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":7.125288963317871, "y":1.5220201015472412, "heading":2.297438401528901, "intervals":64, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.6629953384399414, "y":3.0058600902557373, "heading":1.0010398833246623, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.1074931621551514, "y":0.9861889481544496, "heading":0.8709035133255352, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.951519966125488, "y":2.7791624069213867, "heading":1.0427221780766034, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.0868842601776123, "y":1.0067977905273438, "heading":0.8818720101391697, "intervals":60, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.147773265838623, "y":3.87143349647522, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":3, "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"0 mm", "val":0.0}, "y":{"exp":"0 mm", "val":0.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.4578442573547363 m", "val":1.4578442573547363}, "y":{"exp":"4.232089042663574 m", "val":4.232089042663574}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.869084358215332 m", "val":3.869084358215332}, "y":{"exp":"2.1505911350250244 m", "val":2.1505911350250244}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.280324459075928 m", "val":6.280324459075928}, "y":{"exp":"3.2428622245788574 m", "val":3.2428622245788574}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.434422016143799 m", "val":7.434422016143799}, "y":{"exp":"3.1810357570648193 m", "val":3.1810357570648193}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"7.125288963317871 m", "val":7.125288963317871}, "y":{"exp":"1.5220201015472412 m", "val":1.5220201015472412}, "heading":{"exp":"2.297438401528901 rad", "val":2.297438401528901}, "intervals":64, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.6629953384399414 m", "val":3.6629953384399414}, "y":{"exp":"3.0058600902557373 m", "val":3.0058600902557373}, "heading":{"exp":"1.0010398833246623 rad", "val":1.0010398833246623}, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.1074931621551514 m", "val":1.1074931621551514}, "y":{"exp":"0.9861889481544495 m", "val":0.9861889481544496}, "heading":{"exp":"0.8709035133255352 rad", "val":0.8709035133255352}, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.9515199661254883 m", "val":3.951519966125488}, "y":{"exp":"2.7791624069213867 m", "val":2.7791624069213867}, "heading":{"exp":"1.0427221780766034 rad", "val":1.0427221780766034}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.0868842601776123 m", "val":1.0868842601776123}, "y":{"exp":"1.0067977905273438 m", "val":1.0067977905273438}, "heading":{"exp":"0.8818720101391697 rad", "val":0.8818720101391697}, "intervals":60, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.147773265838623 m", "val":3.147773265838623}, "y":{"exp":"3.8714334964752197 m", "val":3.87143349647522}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":3, "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}, - {"from":3, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}], + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -35,186 +42,283 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,2.47152,4.09774,5.36975,6.91421], + "waypoints":[0.0,2.31607,4.46155,6.64137,8.82436,11.06268], "samples":[ - {"t":0.0, "x":0.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.20669, "ay":2.82309, "alpha":0.0, "fx":[3.51567,3.51567,3.51567,3.51567], "fy":[48.01996,48.01996,48.01996,48.01996]}, - {"t":0.04846, "x":0.00024, "y":0.00331, "heading":0.0, "vx":0.01002, "vy":0.13681, "omega":0.0, "ax":0.21133, "ay":2.82265, "alpha":0.0, "fx":[3.59474,3.59474,3.59474,3.59474], "fy":[48.01241,48.01241,48.01241,48.01241]}, - {"t":0.09692, "x":0.00098, "y":0.01326, "heading":0.0, "vx":0.02026, "vy":0.2736, "omega":0.0, "ax":0.21639, "ay":2.82216, "alpha":0.0, "fx":[3.68067,3.68067,3.68066,3.68066], "fy":[48.00406,48.00406,48.00406,48.00406]}, - {"t":0.14538, "x":0.00221, "y":0.02983, "heading":0.0, "vx":0.03074, "vy":0.41036, "omega":0.0, "ax":0.2219, "ay":2.82161, "alpha":0.0, "fx":[3.7744,3.7744,3.7744,3.7744], "fy":[47.99477,47.99477,47.99477,47.99477]}, - {"t":0.19384, "x":0.00396, "y":0.05303, "heading":0.0, "vx":0.0415, "vy":0.5471, "omega":0.0, "ax":0.22793, "ay":2.821, "alpha":0.0, "fx":[3.87704,3.87704,3.87704,3.87704], "fy":[47.98439,47.98439,47.98439,47.98439]}, - {"t":0.24231, "x":0.00624, "y":0.08286, "heading":0.0, "vx":0.05254, "vy":0.68381, "omega":0.0, "ax":0.23457, "ay":2.82031, "alpha":0.0, "fx":[3.98993,3.98993,3.98993,3.98993], "fy":[47.97271,47.97271,47.97271,47.97271]}, - {"t":0.29077, "x":0.00906, "y":0.11931, "heading":0.0, "vx":0.06391, "vy":0.82049, "omega":0.0, "ax":0.2419, "ay":2.81954, "alpha":0.0, "fx":[4.11467,4.11467,4.11467,4.11467], "fy":[47.9595,47.9595,47.9595,47.9595]}, - {"t":0.33923, "x":0.01244, "y":0.16238, "heading":0.0, "vx":0.07563, "vy":0.95712, "omega":0.0, "ax":0.25005, "ay":2.81865, "alpha":0.0, "fx":[4.25324,4.25324,4.25324,4.25324], "fy":[47.94444,47.94444,47.94444,47.94444]}, - {"t":0.38769, "x":0.0164, "y":0.21207, "heading":0.0, "vx":0.08775, "vy":1.09372, "omega":0.0, "ax":0.25915, "ay":2.81763, "alpha":0.0, "fx":[4.40806,4.40806,4.40806,4.40806], "fy":[47.92713,47.92713,47.92713,47.92713]}, - {"t":0.43615, "x":0.02096, "y":0.26838, "heading":0.0, "vx":0.10031, "vy":1.23026, "omega":0.0, "ax":0.26939, "ay":2.81645, "alpha":0.0, "fx":[4.58217,4.58217,4.58217,4.58217], "fy":[47.90706,47.90706,47.90706,47.90706]}, - {"t":0.48461, "x":0.02614, "y":0.33131, "heading":0.0, "vx":0.11336, "vy":1.36675, "omega":0.0, "ax":0.28098, "ay":2.81507, "alpha":0.0, "fx":[4.77939,4.77939,4.77939,4.77939], "fy":[47.88355,47.88355,47.88355,47.88355]}, - {"t":0.53307, "x":0.03196, "y":0.40085, "heading":0.0, "vx":0.12698, "vy":1.50317, "omega":0.0, "ax":0.29422, "ay":2.81343, "alpha":0.0, "fx":[5.00463,5.00463,5.00463,5.00463], "fy":[47.85568,47.85568,47.85568,47.85568]}, - {"t":0.58153, "x":0.03846, "y":0.477, "heading":0.0, "vx":0.14124, "vy":1.63952, "omega":0.0, "ax":0.30949, "ay":2.81147, "alpha":0.0, "fx":[5.26432,5.26432,5.26432,5.26432], "fy":[47.82223,47.82223,47.82223,47.82223]}, - {"t":0.62999, "x":0.04567, "y":0.55976, "heading":0.0, "vx":0.15624, "vy":1.77576, "omega":0.0, "ax":0.32728, "ay":2.80907, "alpha":0.0, "fx":[5.56697,5.56697,5.56697,5.56697], "fy":[47.78142,47.78142,47.78142,47.78142]}, - {"t":0.67846, "x":0.05362, "y":0.64911, "heading":0.0, "vx":0.1721, "vy":1.91189, "omega":0.0, "ax":0.34828, "ay":2.80609, "alpha":0.0, "fx":[5.92416,5.92416,5.92415,5.92415], "fy":[47.73075,47.73075,47.73075,47.73075]}, - {"t":0.72692, "x":0.06237, "y":0.74506, "heading":0.0, "vx":0.18898, "vy":2.04788, "omega":0.0, "ax":0.37343, "ay":2.80231, "alpha":0.0, "fx":[6.35198,6.35198,6.35198,6.35198], "fy":[47.66646,47.66646,47.66646,47.66646]}, - {"t":0.77538, "x":0.07197, "y":0.84759, "heading":0.0, "vx":0.20707, "vy":2.18368, "omega":0.0, "ax":0.40409, "ay":2.79739, "alpha":0.0, "fx":[6.87354,6.87354,6.87354,6.87354], "fy":[47.58276,47.58276,47.58276,47.58276]}, - {"t":0.82384, "x":0.08248, "y":0.9567, "heading":0.0, "vx":0.22666, "vy":2.31925, "omega":0.0, "ax":0.44229, "ay":2.79078, "alpha":0.0, "fx":[7.52315,7.52315,7.52315,7.52315], "fy":[47.47029,47.47029,47.47029,47.47029]}, - {"t":0.8723, "x":0.09398, "y":1.07237, "heading":0.0, "vx":0.24809, "vy":2.45449, "omega":0.0, "ax":0.49113, "ay":2.78153, "alpha":0.0, "fx":[8.35399,8.35399,8.35399,8.35399], "fy":[47.31304,47.31304,47.31304,47.31304]}, - {"t":0.92076, "x":0.10658, "y":1.19458, "heading":0.0, "vx":0.27189, "vy":2.58929, "omega":0.0, "ax":0.55573, "ay":2.76793, "alpha":0.0, "fx":[9.45289,9.45289,9.45289,9.45289], "fy":[47.08173,47.08173,47.08173,47.08173]}, - {"t":0.96922, "x":0.12041, "y":1.32331, "heading":0.0, "vx":0.29882, "vy":2.72342, "omega":0.0, "ax":0.645, "ay":2.74653, "alpha":0.0, "fx":[10.9713,10.9713,10.9713,10.9713], "fy":[46.71769,46.71769,46.71769,46.71769]}, - {"t":1.01768, "x":0.13565, "y":1.45852, "heading":0.0, "vx":0.33008, "vy":2.85652, "omega":0.0, "ax":0.77582, "ay":2.70955, "alpha":0.0, "fx":[13.1964,13.1964,13.1964,13.1964], "fy":[46.08868,46.08868,46.08868,46.08868]}, - {"t":1.06614, "x":0.15256, "y":1.60013, "heading":0.0, "vx":0.36768, "vy":2.98783, "omega":0.0, "ax":0.9838, "ay":2.63635, "alpha":0.0, "fx":[16.73414,16.73414,16.73414,16.73414], "fy":[44.84363,44.84363,44.84363,44.84363]}, - {"t":1.11461, "x":0.17153, "y":1.74802, "heading":0.0, "vx":0.41535, "vy":3.11559, "omega":0.0, "ax":1.35376, "ay":2.4577, "alpha":0.0, "fx":[23.02715,23.02715,23.02715,23.02715], "fy":[41.80482,41.80482,41.80482,41.80482]}, - {"t":1.16307, "x":0.19325, "y":1.90189, "heading":0.0, "vx":0.48096, "vy":3.2347, "omega":0.0, "ax":2.07939, "ay":1.85996, "alpha":0.0, "fx":[35.3698,35.3698,35.3698,35.3698], "fy":[31.63742,31.63742,31.63742,31.63742]}, - {"t":1.21153, "x":0.219, "y":2.06083, "heading":0.0, "vx":0.58173, "vy":3.32483, "omega":0.0, "ax":2.76297, "ay":-0.24201, "alpha":0.0, "fx":[46.99738,46.99738,46.99738,46.99738], "fy":[-4.11649,-4.11649,-4.11649,-4.11648]}, - {"t":1.25999, "x":0.25043, "y":2.22167, "heading":0.0, "vx":0.71562, "vy":3.3131, "omega":0.0, "ax":1.84329, "ay":-2.09719, "alpha":0.0, "fx":[31.35387,31.35387,31.35387,31.35387], "fy":[-35.67253,-35.67253,-35.67253,-35.67253]}, - {"t":1.30845, "x":0.28728, "y":2.37977, "heading":0.0, "vx":0.80495, "vy":3.21147, "omega":0.0, "ax":1.13393, "ay":-2.56779, "alpha":0.0, "fx":[19.2878,19.2878,19.2878,19.2878], "fy":[-43.67745,-43.67745,-43.67745,-43.67745]}, - {"t":1.35691, "x":0.32762, "y":2.53238, "heading":0.0, "vx":0.8599, "vy":3.08703, "omega":0.0, "ax":0.77828, "ay":-2.70479, "alpha":0.0, "fx":[13.23825,13.23825,13.23825,13.23825], "fy":[-46.00773,-46.00773,-46.00773,-46.00773]}, - {"t":1.40537, "x":0.3702, "y":2.67881, "heading":0.0, "vx":0.89762, "vy":2.95596, "omega":0.0, "ax":0.57718, "ay":-2.75907, "alpha":0.0, "fx":[9.8177,9.8177,9.8177,9.8177], "fy":[-46.93095,-46.93095,-46.93095,-46.93095]}, - {"t":1.45383, "x":0.41438, "y":2.81882, "heading":0.0, "vx":0.92559, "vy":2.82225, "omega":0.0, "ax":0.44993, "ay":-2.78538, "alpha":0.0, "fx":[7.65322,7.65322,7.65322,7.65322], "fy":[-47.37858,-47.37858,-47.37858,-47.37858]}, - {"t":1.50229, "x":0.45976, "y":2.95232, "heading":0.0, "vx":0.94739, "vy":2.68727, "omega":0.0, "ax":0.36268, "ay":-2.79995, "alpha":0.0, "fx":[6.16915,6.16915,6.16915,6.16915], "fy":[-47.62629,-47.62629,-47.62629,-47.62629]}, - {"t":1.55076, "x":0.5061, "y":3.07926, "heading":0.0, "vx":0.96497, "vy":2.55158, "omega":0.0, "ax":0.29931, "ay":-2.80878, "alpha":0.0, "fx":[5.09116,5.09116,5.09116,5.09116], "fy":[-47.77654,-47.77654,-47.77654,-47.77654]}, - {"t":1.59922, "x":0.55322, "y":3.19961, "heading":0.0, "vx":0.97947, "vy":2.41546, "omega":0.0, "ax":0.25126, "ay":-2.81451, "alpha":0.0, "fx":[4.2738,4.2738,4.2738,4.2738], "fy":[-47.87398,-47.87398,-47.87398,-47.87398]}, - {"t":1.64768, "x":0.60098, "y":3.31336, "heading":0.0, "vx":0.99165, "vy":2.27907, "omega":0.0, "ax":0.2136, "ay":-2.81842, "alpha":0.0, "fx":[3.63325,3.63325,3.63325,3.63325], "fy":[-47.94048,-47.94048,-47.94048,-47.94048]}, - {"t":1.69614, "x":0.64928, "y":3.4205, "heading":0.0, "vx":1.002, "vy":2.14248, "omega":0.0, "ax":0.18331, "ay":-2.82119, "alpha":0.0, "fx":[3.11799,3.11799,3.11799,3.11799], "fy":[-47.98769,-47.98769,-47.98769,-47.98769]}, - {"t":1.7446, "x":0.69806, "y":3.52101, "heading":0.0, "vx":1.01089, "vy":2.00577, "omega":0.0, "ax":0.15842, "ay":-2.82323, "alpha":0.0, "fx":[2.69468,2.69468,2.69468,2.69468], "fy":[-48.02232,-48.02232,-48.02232,-48.02232]}, - {"t":1.79306, "x":0.74723, "y":3.6149, "heading":0.0, "vx":1.01856, "vy":1.86895, "omega":0.0, "ax":0.13761, "ay":-2.82476, "alpha":0.0, "fx":[2.34078,2.34078,2.34078,2.34078], "fy":[-48.04838,-48.04838,-48.04838,-48.04838]}, - {"t":1.84152, "x":0.79676, "y":3.70215, "heading":0.0, "vx":1.02523, "vy":1.73206, "omega":0.0, "ax":0.11997, "ay":-2.82594, "alpha":0.0, "fx":[2.04057,2.04057,2.04057,2.04057], "fy":[-48.06844,-48.06844,-48.06844,-48.06844]}, - {"t":1.88998, "x":0.84658, "y":3.78277, "heading":0.0, "vx":1.03105, "vy":1.59511, "omega":0.0, "ax":0.10481, "ay":-2.82687, "alpha":0.0, "fx":[1.78272,1.78272,1.78272,1.78272], "fy":[-48.08417,-48.08417,-48.08417,-48.08417]}, - {"t":1.93844, "x":0.89667, "y":3.85675, "heading":0.0, "vx":1.03612, "vy":1.45812, "omega":0.0, "ax":0.09165, "ay":-2.8276, "alpha":0.0, "fx":[1.55887,1.55887,1.55887,1.55887], "fy":[-48.09669,-48.09669,-48.09669,-48.09669]}, - {"t":1.98691, "x":0.94699, "y":3.92409, "heading":0.0, "vx":1.04057, "vy":1.32109, "omega":0.0, "ax":0.08011, "ay":-2.8282, "alpha":0.0, "fx":[1.36272,1.36272,1.36272,1.36272], "fy":[-48.10681,-48.10681,-48.10681,-48.10681]}, - {"t":2.03537, "x":0.99751, "y":3.9848, "heading":0.0, "vx":1.04445, "vy":1.18403, "omega":0.0, "ax":0.06993, "ay":-2.82868, "alpha":0.0, "fx":[1.18945,1.18945,1.18945,1.18945], "fy":[-48.11508,-48.11508,-48.11508,-48.11508]}, - {"t":2.08383, "x":1.04821, "y":4.03885, "heading":0.0, "vx":1.04784, "vy":1.04695, "omega":0.0, "ax":0.06086, "ay":-2.82908, "alpha":0.0, "fx":[1.03527,1.03527,1.03527,1.03527], "fy":[-48.12192,-48.12192,-48.12192,-48.12192]}, - {"t":2.13229, "x":1.09906, "y":4.08627, "heading":0.0, "vx":1.05079, "vy":0.90985, "omega":0.0, "ax":0.05275, "ay":-2.82942, "alpha":0.0, "fx":[0.89719,0.89719,0.89719,0.89719], "fy":[-48.12762,-48.12762,-48.12762,-48.12762]}, - {"t":2.18075, "x":1.15004, "y":4.12704, "heading":0.0, "vx":1.05334, "vy":0.77273, "omega":0.0, "ax":0.04544, "ay":-2.8297, "alpha":0.0, "fx":[0.77284,0.77284,0.77284,0.77284], "fy":[-48.13241,-48.13241,-48.13241,-48.13241]}, - {"t":2.22921, "x":1.20114, "y":4.16116, "heading":0.0, "vx":1.05554, "vy":0.6356, "omega":0.0, "ax":0.03882, "ay":-2.82994, "alpha":0.0, "fx":[0.66025,0.66025,0.66025,0.66025], "fy":[-48.13648,-48.13648,-48.13648,-48.13648]}, - {"t":2.27767, "x":1.25234, "y":4.18864, "heading":0.0, "vx":1.05743, "vy":0.49846, "omega":0.0, "ax":0.0328, "ay":-2.83014, "alpha":0.0, "fx":[0.55784,0.55784,0.55784,0.55784], "fy":[-48.13994,-48.13994,-48.13994,-48.13994]}, - {"t":2.32613, "x":1.30362, "y":4.20947, "heading":0.0, "vx":1.05901, "vy":0.36131, "omega":0.0, "ax":0.0273, "ay":-2.83032, "alpha":0.0, "fx":[0.46429,0.46429,0.46429,0.46429], "fy":[-48.14292,-48.14292,-48.14292,-48.14292]}, - {"t":2.37459, "x":1.35497, "y":4.22366, "heading":0.0, "vx":1.06034, "vy":0.22415, "omega":0.0, "ax":0.02225, "ay":-2.83047, "alpha":0.0, "fx":[0.3785,0.3785,0.3785,0.3785], "fy":[-48.14549,-48.14549,-48.14549,-48.14549]}, - {"t":2.42306, "x":1.40639, "y":4.2312, "heading":0.0, "vx":1.06142, "vy":0.08698, "omega":0.0, "ax":0.01761, "ay":-2.8306, "alpha":0.0, "fx":[0.29955,0.29955,0.29955,0.29955], "fy":[-48.14772,-48.14772,-48.14772,-48.14772]}, - {"t":2.47152, "x":1.45784, "y":4.23209, "heading":0.0, "vx":1.06227, "vy":-0.0502, "omega":0.0, "ax":0.01941, "ay":-2.82991, "alpha":0.0, "fx":[0.33008,0.33008,0.33009,0.33009], "fy":[-48.13603,-48.13603,-48.13603,-48.13603]}, - {"t":2.50934, "x":1.49803, "y":4.22817, "heading":0.0, "vx":1.063, "vy":-0.15722, "omega":0.0, "ax":0.02795, "ay":-2.8297, "alpha":0.0, "fx":[0.47541,0.47541,0.47541,0.47541], "fy":[-48.13231,-48.13231,-48.13231,-48.13231]}, - {"t":2.54716, "x":1.53825, "y":4.2202, "heading":0.0, "vx":1.06406, "vy":-0.26424, "omega":0.0, "ax":0.03736, "ay":-2.82943, "alpha":0.0, "fx":[0.63545,0.63545,0.63545,0.63545], "fy":[-48.12771,-48.12771,-48.12771,-48.12771]}, - {"t":2.58497, "x":1.57852, "y":4.20818, "heading":0.0, "vx":1.06547, "vy":-0.37124, "omega":0.0, "ax":0.04777, "ay":-2.82909, "alpha":0.0, "fx":[0.81254,0.81254,0.81254,0.81254], "fy":[-48.12201,-48.12201,-48.12201,-48.12201]}, - {"t":2.62279, "x":1.61885, "y":4.19212, "heading":0.0, "vx":1.06728, "vy":-0.47824, "omega":0.0, "ax":0.05935, "ay":-2.82867, "alpha":0.0, "fx":[1.00955,1.00955,1.00955,1.00955], "fy":[-48.11489,-48.11489,-48.11489,-48.11489]}, - {"t":2.66061, "x":1.65926, "y":4.17201, "heading":0.0, "vx":1.06952, "vy":-0.58521, "omega":0.0, "ax":0.07231, "ay":-2.82815, "alpha":0.0, "fx":[1.23004,1.23004,1.23004,1.23004], "fy":[-48.10597,-48.10597,-48.10597,-48.10597]}, - {"t":2.69843, "x":1.69976, "y":4.14785, "heading":0.0, "vx":1.07226, "vy":-0.69217, "omega":0.0, "ax":0.08692, "ay":-2.82748, "alpha":0.0, "fx":[1.47845,1.47845,1.47845,1.47845], "fy":[-48.09471,-48.09471,-48.09471,-48.09471]}, - {"t":2.73625, "x":1.74037, "y":4.11965, "heading":0.0, "vx":1.07555, "vy":-0.79911, "omega":0.0, "ax":0.10349, "ay":-2.82664, "alpha":0.0, "fx":[1.76042,1.76042,1.76042,1.76042], "fy":[-48.08037,-48.08037,-48.08037,-48.08037]}, - {"t":2.77407, "x":1.78112, "y":4.08741, "heading":0.0, "vx":1.07946, "vy":-0.90601, "omega":0.0, "ax":0.12247, "ay":-2.82556, "alpha":0.0, "fx":[2.08319,2.08319,2.08319,2.08319], "fy":[-48.06192,-48.06192,-48.06192,-48.06192]}, - {"t":2.81189, "x":1.82203, "y":4.05113, "heading":0.0, "vx":1.08409, "vy":-1.01287, "omega":0.0, "ax":0.1444, "ay":-2.82414, "alpha":0.0, "fx":[2.45627,2.45627,2.45627,2.45627], "fy":[-48.03789,-48.03789,-48.03789,-48.03789]}, - {"t":2.84971, "x":1.86314, "y":4.0108, "heading":0.0, "vx":1.08955, "vy":-1.11967, "omega":0.0, "ax":0.17004, "ay":-2.82228, "alpha":0.0, "fx":[2.89231,2.89231,2.89231,2.89231], "fy":[-48.00612,-48.00612,-48.00612,-48.00612]}, - {"t":2.88753, "x":1.90446, "y":3.96644, "heading":0.0, "vx":1.09598, "vy":-1.22641, "omega":0.0, "ax":0.20039, "ay":-2.81976, "alpha":0.0, "fx":[3.40858,3.40858,3.40858,3.40858], "fy":[-47.96336,-47.96336,-47.96336,-47.96336]}, - {"t":2.92535, "x":1.94606, "y":3.91804, "heading":0.0, "vx":1.10356, "vy":-1.33305, "omega":0.0, "ax":0.23688, "ay":-2.8163, "alpha":0.0, "fx":[4.0292,4.0292,4.0292,4.0292], "fy":[-47.90454,-47.90454,-47.90454,-47.90454]}, - {"t":2.96316, "x":1.98796, "y":3.86561, "heading":0.0, "vx":1.11252, "vy":-1.43956, "omega":0.0, "ax":0.28154, "ay":-2.81142, "alpha":0.0, "fx":[4.78892,4.78892,4.78892,4.78892], "fy":[-47.8215,-47.8215,-47.8215,-47.8215]}, - {"t":3.00098, "x":2.03024, "y":3.80916, "heading":0.0, "vx":1.12317, "vy":-1.54588, "omega":0.0, "ax":0.33743, "ay":-2.8043, "alpha":0.0, "fx":[5.73956,5.73956,5.73956,5.73956], "fy":[-47.70035,-47.70035,-47.70035,-47.70035]}, - {"t":3.0388, "x":2.07296, "y":3.74869, "heading":0.0, "vx":1.13593, "vy":-1.65194, "omega":0.0, "ax":0.40928, "ay":-2.79348, "alpha":0.0, "fx":[6.96167,6.96167,6.96167,6.96167], "fy":[-47.51626,-47.51626,-47.51626,-47.51626]}, - {"t":3.07662, "x":2.11621, "y":3.68422, "heading":0.0, "vx":1.15141, "vy":-1.75759, "omega":0.0, "ax":0.50482, "ay":-2.77614, "alpha":0.0, "fx":[8.58689,8.58689,8.58689,8.58689], "fy":[-47.22141,-47.22141,-47.22141,-47.22141]}, - {"t":3.11444, "x":2.16011, "y":3.61576, "heading":0.0, "vx":1.1705, "vy":-1.86258, "omega":0.0, "ax":0.63751, "ay":-2.74639, "alpha":0.0, "fx":[10.84378,10.84378,10.84378,10.84378], "fy":[-46.71531,-46.71531,-46.71531,-46.71531]}, - {"t":3.15226, "x":2.20484, "y":3.54335, "heading":0.0, "vx":1.19461, "vy":-1.96644, "omega":0.0, "ax":0.83234, "ay":-2.69028, "alpha":0.0, "fx":[14.15784,14.15784,14.15784,14.15784], "fy":[-45.76097,-45.76097,-45.76097,-45.76097]}, - {"t":3.19008, "x":2.25061, "y":3.46706, "heading":0.0, "vx":1.22609, "vy":-2.06819, "omega":0.0, "ax":1.13917, "ay":-2.56973, "alpha":0.0, "fx":[19.37688,19.37688,19.37688,19.37688], "fy":[-43.71041,-43.71041,-43.71041,-43.71041]}, - {"t":3.2279, "x":2.2978, "y":3.38701, "heading":0.0, "vx":1.26917, "vy":-2.16537, "omega":0.0, "ax":1.65554, "ay":-2.26094, "alpha":0.0, "fx":[28.16021,28.16021,28.16021,28.1602], "fy":[-38.45802,-38.45802,-38.45802,-38.45802]}, - {"t":3.26572, "x":2.34698, "y":3.3035, "heading":0.0, "vx":1.33178, "vy":-2.25088, "omega":0.0, "ax":2.44315, "ay":-1.34613, "alpha":0.0, "fx":[41.55724,41.55724,41.55724,41.55723], "fy":[-22.89737,-22.89737,-22.89737,-22.89737]}, - {"t":3.30354, "x":2.39909, "y":3.21741, "heading":0.0, "vx":1.42418, "vy":-2.30179, "omega":0.0, "ax":2.73359, "ay":0.539, "alpha":0.0, "fx":[46.49764,46.49764,46.49764,46.49764], "fy":[9.16825,9.16826,9.16826,9.16825]}, - {"t":3.34136, "x":2.45491, "y":3.13074, "heading":0.0, "vx":1.52756, "vy":-2.28141, "omega":0.0, "ax":2.09391, "ay":1.85688, "alpha":0.0, "fx":[35.61686,35.61686,35.61686,35.61686], "fy":[31.58505,31.58506,31.58506,31.58506]}, - {"t":3.37917, "x":2.51418, "y":3.04579, "heading":0.0, "vx":1.60675, "vy":-2.21118, "omega":0.0, "ax":1.54052, "ay":2.34858, "alpha":0.0, "fx":[26.20372,26.20372,26.20372,26.20372], "fy":[39.94865,39.94865,39.94866,39.94865]}, - {"t":3.41699, "x":2.57604, "y":2.96384, "heading":0.0, "vx":1.66501, "vy":-2.12236, "omega":0.0, "ax":1.20271, "ay":2.54488, "alpha":0.0, "fx":[20.45774,20.45774,20.45774,20.45774], "fy":[43.28775,43.28775,43.28775,43.28775]}, - {"t":3.45481, "x":2.63987, "y":2.8854, "heading":0.0, "vx":1.7105, "vy":-2.02611, "omega":0.0, "ax":0.9899, "ay":2.63899, "alpha":0.0, "fx":[16.83784,16.83784,16.83784,16.83784], "fy":[44.88847,44.88847,44.88847,44.88847]}, - {"t":3.49263, "x":2.70527, "y":2.81066, "heading":0.0, "vx":1.74793, "vy":-1.92631, "omega":0.0, "ax":0.84659, "ay":2.69104, "alpha":0.0, "fx":[14.4003,14.4003,14.4003,14.4003], "fy":[45.77374,45.77374,45.77374,45.77374]}, - {"t":3.53045, "x":2.77198, "y":2.73973, "heading":0.0, "vx":1.77995, "vy":-1.82454, "omega":0.0, "ax":0.74441, "ay":2.72293, "alpha":0.0, "fx":[12.66219,12.66219,12.66219,12.66219], "fy":[46.31629,46.31629,46.31629,46.31629]}, - {"t":3.56827, "x":2.83983, "y":2.67268, "heading":0.0, "vx":1.8081, "vy":-1.72156, "omega":0.0, "ax":0.66819, "ay":2.744, "alpha":0.0, "fx":[11.36564,11.36564,11.36564,11.36564], "fy":[46.67473,46.67473,46.67473,46.67473]}, - {"t":3.60609, "x":2.90869, "y":2.60953, "heading":0.0, "vx":1.83337, "vy":-1.61778, "omega":0.0, "ax":0.60928, "ay":2.75874, "alpha":0.0, "fx":[10.3636,10.3636,10.36359,10.36359], "fy":[46.92532,46.92532,46.92532,46.92532]}, - {"t":3.64391, "x":2.97846, "y":2.55032, "heading":0.0, "vx":1.85642, "vy":-1.51345, "omega":0.0, "ax":0.56244, "ay":2.7695, "alpha":0.0, "fx":[9.56699,9.56698,9.56698,9.56698], "fy":[47.10836,47.10836,47.10836,47.10836]}, - {"t":3.68173, "x":3.04907, "y":2.49507, "heading":0.0, "vx":1.87769, "vy":-1.40871, "omega":0.0, "ax":0.52435, "ay":2.77764, "alpha":0.0, "fx":[8.91904,8.91904,8.91904,8.91904], "fy":[47.24681,47.24681,47.24681,47.24681]}, - {"t":3.71955, "x":3.12046, "y":2.44378, "heading":0.0, "vx":1.89752, "vy":-1.30366, "omega":0.0, "ax":0.49278, "ay":2.78397, "alpha":0.0, "fx":[8.38198,8.38198,8.38198,8.38198], "fy":[47.35452,47.35452,47.35452,47.35452]}, - {"t":3.75737, "x":3.19257, "y":2.39646, "heading":0.0, "vx":1.91615, "vy":-1.19838, "omega":0.0, "ax":0.46619, "ay":2.78901, "alpha":0.0, "fx":[7.92975,7.92975,7.92975,7.92975], "fy":[47.44032,47.44032,47.44032,47.44032]}, - {"t":3.79518, "x":3.26537, "y":2.35314, "heading":0.0, "vx":1.93379, "vy":-1.0929, "omega":0.0, "ax":0.4435, "ay":2.79311, "alpha":0.0, "fx":[7.54382,7.54382,7.54382,7.54382], "fy":[47.51,47.51,47.51,47.51]}, - {"t":3.833, "x":3.33883, "y":2.3138, "heading":0.0, "vx":1.95056, "vy":-0.98726, "omega":0.0, "ax":0.42392, "ay":2.79649, "alpha":0.0, "fx":[7.21068,7.21068,7.21068,7.21068], "fy":[47.56755,47.56755,47.56755,47.56755]}, - {"t":3.87082, "x":3.4129, "y":2.27846, "heading":0.0, "vx":1.96659, "vy":-0.8815, "omega":0.0, "ax":0.40684, "ay":2.79933, "alpha":0.0, "fx":[6.92022,6.92022,6.92022,6.92022], "fy":[47.61577,47.61577,47.61577,47.61577]}, - {"t":3.90864, "x":3.48756, "y":2.24713, "heading":0.0, "vx":1.98198, "vy":-0.77564, "omega":0.0, "ax":0.39182, "ay":2.80173, "alpha":0.0, "fx":[6.66477,6.66477,6.66477,6.66477], "fy":[47.65667,47.65667,47.65667,47.65667]}, - {"t":3.94646, "x":3.5628, "y":2.2198, "heading":0.0, "vx":1.99679, "vy":-0.66968, "omega":0.0, "ax":0.37851, "ay":2.80379, "alpha":0.0, "fx":[6.43838,6.43838,6.43838,6.43838], "fy":[47.69174,47.69174,47.69174,47.69174]}, - {"t":3.98428, "x":3.63859, "y":2.19648, "heading":0.0, "vx":2.01111, "vy":-0.56364, "omega":0.0, "ax":0.36664, "ay":2.80558, "alpha":0.0, "fx":[6.23637,6.23636,6.23636,6.23636], "fy":[47.72211,47.72211,47.72211,47.72211]}, - {"t":4.0221, "x":3.71491, "y":2.17717, "heading":0.0, "vx":2.02498, "vy":-0.45754, "omega":0.0, "ax":0.35597, "ay":2.80714, "alpha":0.0, "fx":[6.05501,6.05501,6.05501,6.05501], "fy":[47.74862,47.74862,47.74862,47.74862]}, - {"t":4.05992, "x":3.79174, "y":2.16187, "heading":0.0, "vx":2.03844, "vy":-0.35137, "omega":0.0, "ax":0.34635, "ay":2.80851, "alpha":0.0, "fx":[5.8913,5.8913,5.8913,5.8913], "fy":[47.77195,47.77195,47.77195,47.77195]}, - {"t":4.09774, "x":3.86908, "y":2.15059, "heading":0.0, "vx":2.05154, "vy":-0.24516, "omega":0.0, "ax":0.3382, "ay":2.80899, "alpha":0.0, "fx":[5.75263,5.75263,5.75262,5.75262], "fy":[47.78004,47.78004,47.78004,47.78004]}, - {"t":4.13035, "x":3.93618, "y":2.14409, "heading":0.0, "vx":2.06257, "vy":-0.15354, "omega":0.0, "ax":0.33041, "ay":2.80972, "alpha":0.0, "fx":[5.62013,5.62013,5.62013,5.62013], "fy":[47.7926,47.7926,47.7926,47.7926]}, - {"t":4.16297, "x":4.00362, "y":2.14058, "heading":0.0, "vx":2.07334, "vy":-0.0619, "omega":0.0, "ax":0.3218, "ay":2.81051, "alpha":0.0, "fx":[5.47366,5.47366,5.47366,5.47366], "fy":[47.80604,47.80604,47.80604,47.80604]}, - {"t":4.19558, "x":4.07142, "y":2.14005, "heading":0.0, "vx":2.08384, "vy":0.02977, "omega":0.0, "ax":0.31223, "ay":2.81136, "alpha":0.0, "fx":[5.31092,5.31092,5.31092,5.31092], "fy":[47.82046,47.82046,47.82046,47.82046]}, - {"t":4.2282, "x":4.13955, "y":2.14252, "heading":0.0, "vx":2.09402, "vy":0.12146, "omega":0.0, "ax":0.30153, "ay":2.81227, "alpha":0.0, "fx":[5.12902,5.12902,5.12902,5.12902], "fy":[47.83591,47.83591,47.83591,47.83591]}, - {"t":4.26082, "x":4.20801, "y":2.14798, "heading":0.0, "vx":2.10386, "vy":0.21319, "omega":0.0, "ax":0.2895, "ay":2.81324, "alpha":0.0, "fx":[4.92439,4.92439,4.92438,4.92438], "fy":[47.85247,47.85247,47.85247,47.85247]}, - {"t":4.29343, "x":4.27678, "y":2.15643, "heading":0.0, "vx":2.1133, "vy":0.30494, "omega":0.0, "ax":0.27587, "ay":2.81428, "alpha":0.0, "fx":[4.69249,4.69249,4.69249,4.69249], "fy":[47.87017,47.87017,47.87017,47.87017]}, - {"t":4.32605, "x":4.34586, "y":2.16787, "heading":0.0, "vx":2.1223, "vy":0.39673, "omega":0.0, "ax":0.26029, "ay":2.81539, "alpha":0.0, "fx":[4.42751,4.42751,4.42751,4.42751], "fy":[47.88902,47.88902,47.88902,47.88902]}, - {"t":4.35866, "x":4.41521, "y":2.18231, "heading":0.0, "vx":2.13079, "vy":0.48856, "omega":0.0, "ax":0.24232, "ay":2.81656, "alpha":0.0, "fx":[4.12185,4.12185,4.12185,4.12185], "fy":[47.90895,47.90895,47.90895,47.90895]}, - {"t":4.39128, "x":4.48484, "y":2.19974, "heading":0.0, "vx":2.13869, "vy":0.58042, "omega":0.0, "ax":0.22137, "ay":2.81778, "alpha":0.0, "fx":[3.76543,3.76543,3.76543,3.76543], "fy":[47.92972,47.92972,47.92972,47.92972]}, - {"t":4.42389, "x":4.55471, "y":2.22017, "heading":0.0, "vx":2.14591, "vy":0.67233, "omega":0.0, "ax":0.19662, "ay":2.81903, "alpha":0.0, "fx":[3.34451,3.34451,3.34451,3.34451], "fy":[47.95083,47.95083,47.95083,47.95083]}, - {"t":4.45651, "x":4.62481, "y":2.2436, "heading":0.0, "vx":2.15232, "vy":0.76427, "omega":0.0, "ax":0.16696, "ay":2.82023, "alpha":0.0, "fx":[2.83999,2.83999,2.83999,2.83999], "fy":[47.97129,47.97129,47.97129,47.97129]}, - {"t":4.48913, "x":4.6951, "y":2.27002, "heading":0.0, "vx":2.15777, "vy":0.85625, "omega":0.0, "ax":0.13077, "ay":2.82127, "alpha":0.0, "fx":[2.22443,2.22443,2.22443,2.22443], "fy":[47.98908,47.98908,47.98908,47.98908]}, - {"t":4.52174, "x":4.76554, "y":2.29945, "heading":0.0, "vx":2.16203, "vy":0.94827, "omega":0.0, "ax":0.08566, "ay":2.82193, "alpha":0.0, "fx":[1.45705,1.45705,1.45705,1.45705], "fy":[48.00024,48.00024,48.00024,48.00024]}, - {"t":4.55436, "x":4.8361, "y":2.33188, "heading":0.0, "vx":2.16483, "vy":1.04031, "omega":0.0, "ax":0.02791, "ay":2.82172, "alpha":0.0, "fx":[0.47468,0.47468,0.47468,0.47468], "fy":[47.99669,47.99669,47.99669,47.99669]}, - {"t":4.58697, "x":4.90673, "y":2.36731, "heading":0.0, "vx":2.16574, "vy":1.13234, "omega":0.0, "ax":-0.04855, "ay":2.81963, "alpha":0.0, "fx":[-0.82584,-0.82584,-0.82584,-0.82584], "fy":[47.96118,47.96118,47.96118,47.96118]}, - {"t":4.61959, "x":4.97734, "y":2.40574, "heading":0.0, "vx":2.16416, "vy":1.22431, "omega":0.0, "ax":-0.15427, "ay":2.81334, "alpha":0.0, "fx":[-2.624,-2.624,-2.624,-2.624], "fy":[47.8541,47.8541,47.8541,47.8541]}, - {"t":4.6522, "x":5.04784, "y":2.44717, "heading":0.0, "vx":2.15912, "vy":1.31607, "omega":0.0, "ax":-0.30915, "ay":2.79692, "alpha":0.0, "fx":[-5.25848,-5.25848,-5.25848,-5.25848], "fy":[47.57473,47.57473,47.57473,47.57473]}, - {"t":4.68482, "x":5.1181, "y":2.49158, "heading":0.0, "vx":2.14904, "vy":1.40729, "omega":0.0, "ax":-0.55467, "ay":2.75296, "alpha":0.0, "fx":[-9.43486,-9.43486,-9.43486,-9.43486], "fy":[46.82702,46.82702,46.82702,46.82702]}, - {"t":4.71744, "x":5.1879, "y":2.53895, "heading":0.0, "vx":2.13095, "vy":1.49708, "omega":0.0, "ax":-0.98651, "ay":2.61888, "alpha":0.0, "fx":[-16.78022,-16.78022,-16.78022,-16.78022], "fy":[44.54646,44.54646,44.54646,44.54646]}, - {"t":4.75005, "x":5.25687, "y":2.58917, "heading":0.0, "vx":2.09877, "vy":1.5825, "omega":0.0, "ax":-1.80643, "ay":2.1144, "alpha":0.0, "fx":[-30.7269,-30.72691,-30.7269,-30.72689], "fy":[35.9653,35.96529,35.9653,35.96531]}, - {"t":4.78267, "x":5.32436, "y":2.64191, "heading":0.0, "vx":2.03986, "vy":1.65146, "omega":0.0, "ax":-2.7403, "ay":0.37503, "alpha":0.0, "fx":[-46.61169,-46.61169,-46.61169,-46.61169], "fy":[6.37911,6.37907,6.37908,6.37912]}, - {"t":4.81528, "x":5.38944, "y":2.69597, "heading":0.0, "vx":1.95048, "vy":1.66369, "omega":0.0, "ax":-2.36037, "ay":-1.47423, "alpha":0.0, "fx":[-40.1493,-40.14929,-40.14928,-40.14929], "fy":[-25.07618,-25.07619,-25.0762,-25.07619]}, - {"t":4.8479, "x":5.4518, "y":2.74945, "heading":0.0, "vx":1.87349, "vy":1.61561, "omega":0.0, "ax":-1.78403, "ay":-2.15763, "alpha":0.0, "fx":[-30.34579,-30.34578,-30.34578,-30.34578], "fy":[-36.70073,-36.70073,-36.70073,-36.70073]}, - {"t":4.88051, "x":5.51196, "y":2.801, "heading":0.0, "vx":1.81531, "vy":1.54524, "omega":0.0, "ax":-1.4416, "ay":-2.41077, "alpha":0.0, "fx":[-24.52124,-24.52124,-24.52123,-24.52123], "fy":[-41.00649,-41.00649,-41.00649,-41.00649]}, - {"t":4.91313, "x":5.5704, "y":2.85011, "heading":0.0, "vx":1.76829, "vy":1.46661, "omega":0.0, "ax":-1.235, "ay":-2.52889, "alpha":0.0, "fx":[-21.00702,-21.00702,-21.00701,-21.00701], "fy":[-43.01571,-43.01571,-43.01572,-43.01572]}, - {"t":4.94574, "x":5.62741, "y":2.8966, "heading":0.0, "vx":1.72801, "vy":1.38412, "omega":0.0, "ax":-1.10006, "ay":-2.59423, "alpha":0.0, "fx":[-18.71168,-18.71168,-18.71168,-18.71168], "fy":[-44.12711,-44.12711,-44.12712,-44.12712]}, - {"t":4.97836, "x":5.68319, "y":2.94037, "heading":0.0, "vx":1.69213, "vy":1.29951, "omega":0.0, "ax":-1.00584, "ay":-2.63478, "alpha":0.0, "fx":[-17.10903,-17.10903,-17.10903,-17.10903], "fy":[-44.81678,-44.81678,-44.81678,-44.81678]}, - {"t":5.01098, "x":5.73784, "y":2.98135, "heading":0.0, "vx":1.65932, "vy":1.21358, "omega":0.0, "ax":-0.9366, "ay":-2.66204, "alpha":0.0, "fx":[-15.93136,-15.93136,-15.93136,-15.93136], "fy":[-45.28055,-45.28055,-45.28055,-45.28055]}, - {"t":5.04359, "x":5.79147, "y":3.01951, "heading":0.0, "vx":1.62877, "vy":1.12675, "omega":0.0, "ax":-0.88369, "ay":-2.68148, "alpha":0.0, "fx":[-15.0313,-15.0313,-15.0313,-15.0313], "fy":[-45.61121,-45.61121,-45.61121,-45.61121]}, - {"t":5.07621, "x":5.84412, "y":3.05484, "heading":0.0, "vx":1.59995, "vy":1.03929, "omega":0.0, "ax":-0.84198, "ay":-2.69597, "alpha":0.0, "fx":[-14.32189,-14.32189,-14.32189,-14.32189], "fy":[-45.85765,-45.85765,-45.85765,-45.85765]}, - {"t":5.10882, "x":5.89585, "y":3.0873, "heading":0.0, "vx":1.57249, "vy":0.95136, "omega":0.0, "ax":-0.80829, "ay":-2.70714, "alpha":0.0, "fx":[-13.74879,-13.74879,-13.74879,-13.74879], "fy":[-46.04775,-46.04775,-46.04775,-46.04775]}, - {"t":5.14144, "x":5.94671, "y":3.11689, "heading":0.0, "vx":1.54613, "vy":0.86307, "omega":0.0, "ax":-0.78052, "ay":-2.71601, "alpha":0.0, "fx":[-13.27637,-13.27637,-13.27637,-13.27637], "fy":[-46.1985,-46.1985,-46.1985,-46.1985]}, - {"t":5.17405, "x":5.99673, "y":3.1436, "heading":0.0, "vx":1.52067, "vy":0.77448, "omega":0.0, "ax":-0.75724, "ay":-2.72319, "alpha":0.0, "fx":[-12.88038,-12.88038,-12.88038,-12.88038], "fy":[-46.32075,-46.32075,-46.32075,-46.32075]}, - {"t":5.20667, "x":6.04592, "y":3.16741, "heading":0.0, "vx":1.49597, "vy":0.68567, "omega":0.0, "ax":-0.73745, "ay":-2.72913, "alpha":0.0, "fx":[-12.54374,-12.54374,-12.54374,-12.54374], "fy":[-46.42176,-46.42176,-46.42176,-46.42176]}, - {"t":5.23929, "x":6.09432, "y":3.18832, "heading":0.0, "vx":1.47192, "vy":0.59665, "omega":0.0, "ax":-0.72042, "ay":-2.73412, "alpha":0.0, "fx":[-12.25408,-12.25408,-12.25408,-12.25408], "fy":[-46.50654,-46.50654,-46.50654,-46.50654]}, - {"t":5.2719, "x":6.14194, "y":3.20633, "heading":0.0, "vx":1.44842, "vy":0.50748, "omega":0.0, "ax":-0.70561, "ay":-2.73836, "alpha":0.0, "fx":[-12.00225,-12.00225,-12.00225,-12.00225], "fy":[-46.57865,-46.57865,-46.57865,-46.57865]}, - {"t":5.30452, "x":6.18881, "y":3.22142, "heading":0.0, "vx":1.42541, "vy":0.41816, "omega":0.0, "ax":-0.69262, "ay":-2.742, "alpha":0.0, "fx":[-11.7813,-11.7813,-11.7813,-11.7813], "fy":[-46.6407,-46.6407,-46.6407,-46.6407]}, - {"t":5.33713, "x":6.23493, "y":3.2336, "heading":0.0, "vx":1.40282, "vy":0.32873, "omega":0.0, "ax":-0.68113, "ay":-2.74517, "alpha":0.0, "fx":[-11.5859,-11.5859,-11.5859,-11.5859], "fy":[-46.69464,-46.69464,-46.69464,-46.69464]}, - {"t":5.36975, "x":6.28032, "y":3.24286, "heading":0.0, "vx":1.3806, "vy":0.2392, "omega":0.0, "ax":-0.27897, "ay":-0.95811, "alpha":0.00421, "fx":[-4.74764,-4.74219,-4.74532,-4.7454], "fy":[-16.27328,-16.36655,-16.27527,-16.27335]}, - {"t":5.40485, "x":6.32861, "y":3.25067, "heading":0.0, "vx":1.37081, "vy":0.20557, "omega":0.00015, "ax":-0.36115, "ay":-0.93136, "alpha":-0.00136, "fx":[-6.14668,-6.14911,-6.14783,-6.12858], "fy":[-15.87984,-15.82964,-15.82965,-15.82939]}, - {"t":5.43995, "x":6.37651, "y":3.25731, "heading":0.00001, "vx":1.35813, "vy":0.17287, "omega":0.0001, "ax":-0.44243, "ay":-0.8956, "alpha":0.00239, "fx":[-7.52787,-7.5235,-7.52542,-7.52555], "fy":[-15.22046,-15.27177,-15.22253,-15.22057]}, - {"t":5.47505, "x":6.42391, "y":3.26283, "heading":0.00001, "vx":1.3426, "vy":0.14144, "omega":0.00018, "ax":-0.52106, "ay":-0.85225, "alpha":-0.00201, "fx":[-8.86141,-8.86441,-8.86283,-8.86377], "fy":[-14.53053,-14.48544,-14.48547,-14.48457]}, - {"t":5.51015, "x":6.47071, "y":3.26727, "heading":0.00002, "vx":1.32431, "vy":0.11152, "omega":0.00011, "ax":-0.59513, "ay":-0.80229, "alpha":0.00203, "fx":[-10.12529,-10.12107,-10.12277,-10.12292], "fy":[-13.6354,-13.67833,-13.63745,-13.63553]}, - {"t":5.54526, "x":6.51683, "y":3.27069, "heading":0.00002, "vx":1.30342, "vy":0.08336, "omega":0.00018, "ax":-0.66307, "ay":-0.74714, "alpha":-0.00225, "fx":[-11.27289,-11.27562,-11.27375,-11.29243], "fy":[-12.73503,-12.70033,-12.70039,-12.69851]}, - {"t":5.58036, "x":6.56218, "y":3.27315, "heading":0.00003, "vx":1.28015, "vy":0.05714, "omega":0.00011, "ax":-0.72382, "ay":-0.68848, "alpha":0.00155, "fx":[-12.31391,-12.31112,-12.31138,-12.31151], "fy":[-11.70267,-11.73334,-11.70463,-11.70279]}, - {"t":5.61546, "x":6.60667, "y":3.27473, "heading":0.00003, "vx":1.25474, "vy":0.03297, "omega":0.00016, "ax":-0.77687, "ay":-0.62805, "alpha":-0.00213, "fx":[-13.20637,-13.20809,-13.20599,-13.23685], "fy":[-10.70029,-10.67816,-10.67824,-10.67541]}, - {"t":5.65056, "x":6.65023, "y":3.2755, "heading":0.00003, "vx":1.22747, "vy":0.01092, "omega":0.00009, "ax":-0.82222, "ay":-0.56745, "alpha":0.00111, "fx":[-13.98741,-13.98569,-13.98486,-13.98498], "fy":[-9.64664,-9.66655,-9.64856,-9.64676]}, - {"t":5.68566, "x":6.69281, "y":3.27554, "heading":0.00004, "vx":1.19861, "vy":-0.009, "omega":0.00012, "ax":-0.86027, "ay":-0.50798, "alpha":-0.00184, "fx":[-14.62422,-14.62519,-14.62294,-14.65956], "fy":[-8.65001,-8.63855,-8.63864,-8.6354]}, - {"t":5.72076, "x":6.73435, "y":3.27491, "heading":0.00004, "vx":1.16842, "vy":-0.02683, "omega":0.00006, "ax":-0.89168, "ay":-0.45066, "alpha":0.00076, "fx":[-15.16868,-15.16785,-15.16612,-15.16623], "fy":[-7.66235,-7.67342,-7.66424,-7.66245]}, - {"t":5.75586, "x":6.77482, "y":3.27369, "heading":0.00004, "vx":1.13712, "vy":-0.04265, "omega":0.00009, "ax":-0.91723, "ay":-0.39616, "alpha":-0.00151, "fx":[-15.59346,-15.59387,-15.59154,-15.62871], "fy":[-6.74238,-6.73833,-6.73842,-6.73532]}, - {"t":5.79097, "x":6.81417, "y":3.27195, "heading":0.00005, "vx":1.10492, "vy":-0.05655, "omega":0.00003, "ax":-0.93776, "ay":-0.34489, "alpha":0.00051, "fx":[-15.95231,-15.95196,-15.94975,-15.94984], "fy":[-5.8648,-5.86976,-5.8667,-5.86489]}, - {"t":5.82607, "x":6.85237, "y":3.26975, "heading":0.00005, "vx":1.072, "vy":-0.06866, "omega":0.00005, "ax":-0.95403, "ay":-0.29704, "alpha":-0.00121, "fx":[-16.22002,-16.22014,-16.21776,-16.25342], "fy":[-5.05253,-5.05343,-5.05351,-5.05053]}, - {"t":5.86117, "x":6.88941, "y":3.26716, "heading":0.00005, "vx":1.03852, "vy":-0.07908, "omega":0.00001, "ax":-0.96678, "ay":-0.25261, "alpha":0.00036, "fx":[-16.44597,-16.44584,-16.44341,-16.44349], "fy":[-4.29607,-4.29731,-4.29801,-4.29615]}, - {"t":5.89627, "x":6.92527, "y":3.26423, "heading":0.00005, "vx":1.00458, "vy":-0.08795, "omega":0.00002, "ax":-0.97663, "ay":-0.21153, "alpha":-0.00094, "fx":[-16.60518,-16.60513,-16.60272,-16.63557], "fy":[-3.59569,-3.5998,-3.59987,-3.59717]}, - {"t":5.93137, "x":6.95993, "y":3.26101, "heading":0.00005, "vx":0.9703, "vy":-0.09538, "omega":-0.00001, "ax":-0.9841, "ay":-0.17364, "alpha":0.00026, "fx":[-16.74059,-16.74057,-16.73804,-16.7381], "fy":[-2.95327,-2.95219,-2.95527,-2.95333]}, - {"t":5.96647, "x":6.99338, "y":3.25755, "heading":0.00005, "vx":0.93576, "vy":-0.10147, "omega":0.0, "ax":-0.98966, "ay":-0.13872, "alpha":-0.00074, "fx":[-16.82759,-16.82752,-16.8251,-16.85512], "fy":[-2.35576,-2.36167,-2.36172,-2.3592]}, - {"t":6.00157, "x":7.02562, "y":3.25391, "heading":0.00005, "vx":0.90102, "vy":-0.10634, "omega":-0.00003, "ax":-0.99367, "ay":-0.10656, "alpha":0.00021, "fx":[-16.90322,-16.90323,-16.90068,-16.90072], "fy":[-1.81268,-1.81029,-1.81474,-1.81273]}, - {"t":6.03667, "x":7.05664, "y":3.25011, "heading":0.00005, "vx":0.86614, "vy":-0.11008, "omega":-0.00002, "ax":-0.99642, "ay":-0.07694, "alpha":-0.00059, "fx":[-16.94332,-16.94326,-16.94085,-16.96806], "fy":[-1.30428,-1.31103,-1.31106,-1.30873]}, - {"t":6.07178, "x":7.08642, "y":3.2462, "heading":0.00005, "vx":0.83116, "vy":-0.11278, "omega":-0.00004, "ax":-0.99818, "ay":-0.04964, "alpha":0.00019, "fx":[-16.98002,-16.98003,-16.9775,-16.97752], "fy":[-0.84454,-0.84171,-0.84667,-0.84457]}, - {"t":6.10688, "x":7.11499, "y":3.24221, "heading":0.00005, "vx":0.79613, "vy":-0.11452, "omega":-0.00004, "ax":-0.99914, "ay":-0.02445, "alpha":-0.00047, "fx":[-16.99009,-16.99007,-16.98766,-17.01241], "fy":[-0.41103,-0.41822,-0.41823,-0.41602]}, - {"t":6.14198, "x":7.14231, "y":3.23817, "heading":0.00005, "vx":0.76105, "vy":-0.11538, "omega":-0.00005, "ax":-0.99946, "ay":-0.00118, "alpha":0.00019, "fx":[-17.00175,-17.00175,-16.99926,-16.99926], "fy":[-0.02017,-0.01734,-0.02236,-0.02017]}, - {"t":6.17708, "x":7.16841, "y":3.23412, "heading":0.00004, "vx":0.72597, "vy":-0.11542, "omega":-0.00004, "ax":-0.99927, "ay":0.02036, "alpha":-0.00038, "fx":[-16.99286,-16.99288,-16.99049,-17.01311], "fy":[0.35105,0.34393,0.34393,0.34609]}, - {"t":6.21218, "x":7.19328, "y":3.23008, "heading":0.00004, "vx":0.6909, "vy":-0.11471, "omega":-0.00006, "ax":-0.99869, "ay":0.04031, "alpha":0.00021, "fx":[-16.9886,-16.98859,-16.98613,-16.98611], "fy":[0.68556,0.6881,0.6833,0.68558]}, - {"t":6.24728, "x":7.21692, "y":3.22608, "heading":0.00004, "vx":0.65584, "vy":-0.11329, "omega":-0.00005, "ax":-0.99778, "ay":0.05883, "alpha":-0.00043, "fx":[-16.96743,-16.96749,-16.96514,-16.98802], "fy":[1.00531,0.99846,0.99849,1.00031]}, - {"t":6.28238, "x":7.23932, "y":3.22214, "heading":0.00004, "vx":0.62082, "vy":-0.11123, "omega":-0.00007, "ax":-0.99664, "ay":0.07605, "alpha":0.00023, "fx":[-16.95374,-16.95373,-16.95129,-16.95125], "fy":[1.29357,1.29562,1.29125,1.29361]}, - {"t":6.31749, "x":7.2605, "y":3.21828, "heading":0.00004, "vx":0.58583, "vy":-0.10856, "omega":-0.00006, "ax":-0.9953, "ay":0.09208, "alpha":-0.00038, "fx":[-16.92553,-16.92563,-16.9233,-16.94453], "fy":[1.57072,1.56419,1.56423,1.5659]}, - {"t":6.35259, "x":7.28045, "y":3.21453, "heading":0.00004, "vx":0.5509, "vy":-0.10533, "omega":-0.00007, "ax":-0.99382, "ay":0.10704, "alpha":0.00026, "fx":[-16.90576,-16.90575,-16.90334,-16.90328], "fy":[1.82089,1.82243,1.81853,1.82096]}, - {"t":6.38769, "x":7.29918, "y":3.2109, "heading":0.00003, "vx":0.51601, "vy":-0.10157, "omega":-0.00006, "ax":-0.99223, "ay":0.12102, "alpha":-0.00033, "fx":[-16.87364,-16.87377,-16.87146,-16.89101], "fy":[2.06259,2.0565,2.05657,2.05819]}, - {"t":6.42279, "x":7.31668, "y":3.20741, "heading":0.00003, "vx":0.48119, "vy":-0.09732, "omega":-0.00007, "ax":-0.99056, "ay":0.1341, "alpha":0.00029, "fx":[-16.85028,-16.85029,-16.84789,-16.84781], "fy":[2.28135,2.28226,2.27895,2.28143]}, - {"t":6.45789, "x":7.33296, "y":3.20407, "heading":0.00003, "vx":0.44642, "vy":-0.09262, "omega":-0.00006, "ax":-0.98883, "ay":0.14636, "alpha":-0.0003, "fx":[-16.81615,-16.8163,-16.81402,-16.83241], "fy":[2.4934,2.48779,2.48787,2.48944]}, - {"t":6.49299, "x":7.34802, "y":3.20091, "heading":0.00003, "vx":0.41171, "vy":-0.08748, "omega":-0.00007, "ax":-0.98707, "ay":0.15788, "alpha":0.00031, "fx":[-16.79097,-16.79098,-16.7886,-16.7885], "fy":[2.68601,2.68633,2.68359,2.68612]}, - {"t":6.52809, "x":7.36186, "y":3.19794, "heading":0.00002, "vx":0.37706, "vy":-0.08194, "omega":-0.00006, "ax":-0.98529, "ay":0.16871, "alpha":-0.00029, "fx":[-16.75608,-16.75625,-16.75398,-16.77149], "fy":[2.8731,2.86803,2.86813,2.86964]}, - {"t":6.5632, "x":7.37449, "y":3.19517, "heading":0.00002, "vx":0.34247, "vy":-0.07601, "omega":-0.00007, "ax":-0.9835, "ay":0.17891, "alpha":0.00034, "fx":[-16.73019,-16.73023,-16.72785,-16.72773], "fy":[3.04386,3.04357,3.04142,3.04399]}, - {"t":6.5983, "x":7.3859, "y":3.19261, "heading":0.00002, "vx":0.30795, "vy":-0.06973, "omega":-0.00006, "ax":-0.98171, "ay":0.18853, "alpha":-0.00028, "fx":[-16.6954,-16.69559,-16.69335,-16.70999], "fy":[3.20985,3.20524,3.20536,3.20686]}, - {"t":6.6334, "x":7.39611, "y":3.19028, "heading":0.00002, "vx":0.27349, "vy":-0.06312, "omega":-0.00007, "ax":-0.97993, "ay":0.19761, "alpha":0.00037, "fx":[-16.66951,-16.66957,-16.66719,-16.66706], "fy":[3.36216,3.36124,3.35969,3.3623]}, - {"t":6.6685, "x":7.40511, "y":3.18818, "heading":0.00001, "vx":0.2391, "vy":-0.05618, "omega":-0.00006, "ax":-0.97817, "ay":0.2062, "alpha":-0.00027, "fx":[-16.63534,-16.63553,-16.63331,-16.64921], "fy":[3.51007,3.50603,3.50616,3.50766]}, - {"t":6.7036, "x":7.4129, "y":3.18634, "heading":0.00001, "vx":0.20476, "vy":-0.04894, "omega":-0.00007, "ax":-0.97643, "ay":0.21434, "alpha":0.0004, "fx":[-16.60989,-16.60998,-16.6076,-16.60745], "fy":[3.6468,3.64533,3.64431,3.64696]}, - {"t":6.7387, "x":7.41948, "y":3.18475, "heading":0.00001, "vx":0.17049, "vy":-0.04142, "omega":-0.00005, "ax":-0.97471, "ay":0.22205, "alpha":-0.00027, "fx":[-16.57667,-16.57686,-16.57467,-16.58992], "fy":[3.77927,3.77569,3.77584,3.77736]}, - {"t":6.7738, "x":7.42487, "y":3.18344, "heading":0.00001, "vx":0.13627, "vy":-0.03362, "omega":-0.00006, "ax":-0.97302, "ay":0.22937, "alpha":0.00043, "fx":[-16.55195,-16.55207,-16.54968,-16.54952], "fy":[3.90266,3.90062,3.90015,3.90283]}, - {"t":6.80891, "x":7.42905, "y":3.1824, "heading":0.00001, "vx":0.10212, "vy":-0.02557, "omega":-0.00005, "ax":-0.97136, "ay":0.23633, "alpha":-0.00026, "fx":[-16.51986,-16.52006,-16.51788,-16.53256], "fy":[4.02181,4.01864,4.0188,4.02035]}, - {"t":6.84401, "x":7.43204, "y":3.18165, "heading":0.0, "vx":0.06802, "vy":-0.01728, "omega":-0.00006, "ax":-0.96974, "ay":0.24295, "alpha":0.00045, "fx":[-16.49605,-16.4962,-16.4938,-16.49362], "fy":[4.1337,4.13116,4.13117,4.1339]}, - {"t":6.87911, "x":7.43383, "y":3.18119, "heading":0.0, "vx":0.03398, "vy":-0.00875, "omega":-0.00004, "ax":-0.96814, "ay":0.24925, "alpha":0.00117, "fx":[-16.46526,-16.47961,-16.46327,-16.46307], "fy":[4.24152,4.23711,4.23848,4.24171]}, - {"t":6.91421, "x":7.43442, "y":3.18104, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.12529, "y":1.52202, "heading":2.29744, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.5846, "ay":1.10795, "alpha":-0.96298, "fx":[-41.1586,-45.08819,-46.65187,-42.95469], "fy":[24.9646,16.87258,11.84569,21.70113]}, + {"t":0.03619, "x":7.1236, "y":1.52275, "heading":2.29744, "vx":-0.09353, "vy":0.0401, "omega":-0.03485, "ax":-2.5845, "ay":1.10791, "alpha":-0.96315, "fx":[-41.15663,-45.08687,-46.6504,-42.95257], "fy":[24.96463,16.87171,11.84393,21.7007]}, + {"t":0.07238, "x":7.11852, "y":1.52492, "heading":2.29618, "vx":-0.18706, "vy":0.08019, "omega":-0.0697, "ax":-2.58439, "ay":1.10786, "alpha":-0.96323, "fx":[-41.15238,-45.08264,-46.64927,-42.95498], "fy":[24.96818,16.8783,11.84036,21.69096]}, + {"t":0.10857, "x":7.11006, "y":1.52855, "heading":2.29365, "vx":-0.28059, "vy":0.12028, "omega":-0.10456, "ax":-2.58428, "ay":1.10781, "alpha":-0.96323, "fx":[-41.14586,-45.07549,-46.64846,-42.96194], "fy":[24.9752,16.89235,11.83502,21.67185]}, + {"t":0.14475, "x":7.09821, "y":1.53363, "heading":2.28987, "vx":-0.37411, "vy":0.16037, "omega":-0.13942, "ax":-2.58417, "ay":1.10776, "alpha":-0.96316, "fx":[-41.13708,-45.0654,-46.64794,-42.97342], "fy":[24.98565,16.91388,11.82798,21.64332]}, + {"t":0.18094, "x":7.08298, "y":1.54016, "heading":2.28483, "vx":-0.46763, "vy":0.20046, "omega":-0.17428, "ax":-2.58405, "ay":1.10771, "alpha":-0.963, "fx":[-41.12606,-45.05234,-46.64765,-42.98944], "fy":[24.99945,16.94288,11.81935,21.6053]}, + {"t":0.21713, "x":7.06437, "y":1.54814, "heading":2.27852, "vx":-0.56114, "vy":0.24055, "omega":-0.20913, "ax":-2.58392, "ay":1.10764, "alpha":-0.96277, "fx":[-41.11284,-45.03626,-46.64756,-43.00997], "fy":[25.0165,16.97938,11.80928,21.55768]}, + {"t":0.25332, "x":7.04237, "y":1.55757, "heading":2.27095, "vx":-0.65465, "vy":0.28063, "omega":-0.24397, "ax":-2.58378, "ay":1.10758, "alpha":-0.96247, "fx":[-41.09746,-45.01712,-46.64758,-43.03502], "fy":[25.03667,17.02339,11.79795,21.50034]}, + {"t":0.28951, "x":7.01698, "y":1.56845, "heading":2.26212, "vx":-0.74815, "vy":0.32071, "omega":-0.2788, "ax":-2.58363, "ay":1.10751, "alpha":-0.96209, "fx":[-41.07998,-44.99486,-46.64763,-43.06457], "fy":[25.05982,17.07492,11.78559,21.43314]}, + {"t":0.3257, "x":6.98822, "y":1.58078, "heading":2.25203, "vx":-0.84165, "vy":0.36079, "omega":-0.31361, "ax":-2.58347, "ay":1.10743, "alpha":-0.96164, "fx":[-41.06046,-44.96942,-46.64762,-43.0986], "fy":[25.08574,17.13399,11.77243,21.35592]}, + {"t":0.36189, "x":6.95607, "y":1.59456, "heading":2.24068, "vx":-0.93514, "vy":0.40087, "omega":-0.34841, "ax":-2.58329, "ay":1.10734, "alpha":-0.96112, "fx":[-41.03897,-44.94073,-46.64744,-43.13709], "fy":[25.11425,17.20062,11.75878,21.26851]}, + {"t":0.39807, "x":6.92054, "y":1.60979, "heading":2.22807, "vx":-1.02863, "vy":0.44094, "omega":-0.3832, "ax":-2.5831, "ay":1.10724, "alpha":-0.96055, "fx":[-41.01562,-44.90869,-46.64697,-43.18], "fy":[25.14509,17.2748,11.74496,21.17072]}, + {"t":0.43426, "x":6.88162, "y":1.62647, "heading":2.21421, "vx":-1.12211, "vy":0.48101, "omega":-0.41796, "ax":-2.58289, "ay":1.10714, "alpha":-0.95993, "fx":[-40.99047,-44.87322,-46.64606,-43.22726], "fy":[25.178,17.35655,11.73131,21.06235]}, + {"t":0.47045, "x":6.83932, "y":1.64461, "heading":2.19908, "vx":-1.21558, "vy":0.52108, "omega":-0.4527, "ax":-2.58266, "ay":1.10701, "alpha":-0.95926, "fx":[-40.96365,-44.8342,-46.64456,-43.2788], "fy":[25.21267,17.44587,11.71821,20.9432]}, + {"t":0.50664, "x":6.79364, "y":1.66419, "heading":2.1827, "vx":-1.30904, "vy":0.56114, "omega":-0.48741, "ax":-2.5824, "ay":1.10688, "alpha":-0.95857, "fx":[-40.93526,-44.79151,-46.64229,-43.33451], "fy":[25.24877,17.54274,11.70608,20.81306]}, + {"t":0.54283, "x":6.74458, "y":1.68522, "heading":2.16506, "vx":-1.40249, "vy":0.60119, "omega":-0.5221, "ax":-2.58211, "ay":1.10672, "alpha":-0.95786, "fx":[-40.90539,-44.74502,-46.63905,-43.39425], "fy":[25.2859,17.64714,11.69532,20.6717]}, + {"t":0.57902, "x":6.69213, "y":1.7077, "heading":2.14617, "vx":-1.49594, "vy":0.64124, "omega":-0.55676, "ax":-2.58178, "ay":1.10654, "alpha":-0.95715, "fx":[-40.87417,-44.69455,-46.6346,-43.45785], "fy":[25.32367,17.75904,11.68639,20.51891]}, + {"t":0.61521, "x":6.6363, "y":1.73163, "heading":2.12602, "vx":-1.58937, "vy":0.68129, "omega":-0.5914, "ax":-2.5814, "ay":1.10634, "alpha":-0.95647, "fx":[-40.84168,-44.63991,-46.62868,-43.52504], "fy":[25.36159,17.87837,11.67975,20.35448]}, + {"t":0.65139, "x":6.5771, "y":1.75701, "heading":2.10462, "vx":-1.68278, "vy":0.72133, "omega":-0.62601, "ax":-2.58096, "ay":1.10611, "alpha":-0.95583, "fx":[-40.80799,-44.58088,-46.62098,-43.59553], "fy":[25.39916,18.00504,11.67584,20.17818]}, + {"t":0.68758, "x":6.51451, "y":1.78384, "heading":2.08196, "vx":-1.77619, "vy":0.76135, "omega":-0.6606, "ax":-2.58044, "ay":1.10583, "alpha":-0.95527, "fx":[-40.77313,-44.51717,-46.61111,-43.6689], "fy":[25.43579,18.13893,11.67512,19.9898]}, + {"t":0.72377, "x":6.44854, "y":1.81212, "heading":2.05806, "vx":-1.86957, "vy":0.80137, "omega":-0.69517, "ax":-2.57983, "ay":1.10551, "alpha":-0.95481, "fx":[-40.73708,-44.44842,-46.59863,-43.74462], "fy":[25.47083,18.27989,11.67802,19.78911]}, + {"t":0.75996, "x":6.3792, "y":1.84184, "heading":2.0329, "vx":-1.96293, "vy":0.84138, "omega":-0.72973, "ax":-2.5791, "ay":1.10513, "alpha":-0.9545, "fx":[-40.69968,-44.37417,-46.58293,-43.82197], "fy":[25.50355,18.42768,11.68491,19.57587]}, + {"t":0.79615, "x":6.30647, "y":1.87301, "heading":2.00649, "vx":-2.05626, "vy":0.88137, "omega":-0.76427, "ax":-2.5782, "ay":1.10468, "alpha":-0.95438, "fx":[-40.66062,-44.2938,-46.56324,-43.89998], "fy":[25.5331,18.58199,11.69611,19.34976]}, + {"t":0.83234, "x":6.23037, "y":1.90563, "heading":1.97883, "vx":-2.14956, "vy":0.92135, "omega":-0.79881, "ax":-2.57708, "ay":1.10412, "alpha":-0.95451, "fx":[-40.61927,-44.2064,-46.5385,-43.97727], "fy":[25.55844,18.74236,11.71177,19.11038]}, + {"t":0.86853, "x":6.15089, "y":1.9397, "heading":1.94993, "vx":-2.24283, "vy":0.96131, "omega":-0.83335, "ax":-2.57565, "ay":1.10342, "alpha":-0.95497, "fx":[-40.57448,-44.11065,-46.50714,-44.05184], "fy":[25.57833,18.90814,11.73181,18.85713]}, + {"t":0.90472, "x":6.06804, "y":1.97521, "heading":1.91977, "vx":-2.33603, "vy":1.00124, "omega":-0.86791, "ax":-2.57376, "ay":1.10252, "alpha":-0.95585, "fx":[-40.52416,-44.00444,-46.46676,-44.12062], "fy":[25.59112,19.07833,11.7557,18.58897]}, + {"t":0.9409, "x":5.98182, "y":2.01216, "heading":1.88836, "vx":-2.42918, "vy":1.04114, "omega":-0.9025, "ax":-2.57118, "ay":1.10131, "alpha":-0.95733, "fx":[-40.46437,-43.88418,-46.41334,-44.17844], "fy":[25.59445,19.25125,11.78206,18.30404]}, + {"t":0.97709, "x":5.89223, "y":2.05056, "heading":1.8557, "vx":-2.52222, "vy":1.08099, "omega":-0.93714, "ax":-2.56744, "ay":1.09959, "alpha":-0.95966, "fx":[-40.38728,-43.74314,-46.33944,-44.2159], "fy":[25.58457,19.42392,11.80768,17.99856]}, + {"t":1.01328, "x":5.79927, "y":2.0904, "heading":1.82178, "vx":-2.61513, "vy":1.12078, "omega":-0.97187, "ax":-2.56156, "ay":1.09693, "alpha":-0.96345, "fx":[-40.27556,-43.56702,-46.22943,-44.21329], "fy":[25.55439,19.59013,11.825,17.66406]}, + {"t":1.04947, "x":5.70295, "y":2.13168, "heading":1.78661, "vx":-2.70783, "vy":1.16048, "omega":-1.00674, "ax":-2.55098, "ay":1.0922, "alpha":-0.97016, "fx":[-40.08345,-43.3189,-46.04325,-44.12026], "fy":[25.48696,19.73423,11.81334,17.27773]}, + {"t":1.08566, "x":5.60329, "y":2.17439, "heading":1.75018, "vx":-2.80015, "vy":1.2, "omega":-1.04185, "ax":-2.5265, "ay":1.08137, "alpha":-0.9852, "fx":[-39.64354,-42.86527,-45.6363,-43.75486], "fy":[25.32333,19.80033,11.69632,16.75498]}, + {"t":1.12185, "x":5.5003, "y":2.21852, "heading":1.71248, "vx":-2.89158, "vy":1.23914, "omega":-1.0775, "ax":-2.40851, "ay":1.02944, "alpha":-1.05585, "fx":[-37.50972,-41.06487,-43.76459,-41.53308], "fy":[24.55282,19.31179,10.82083,15.35644]}, + {"t":1.15804, "x":5.39408, "y":2.26404, "heading":1.67349, "vx":-2.97874, "vy":1.27639, "omega":-1.11571, "ax":2.41014, "ay":-1.03437, "alpha":0.83965, "fx":[38.3031,40.93627,43.26184,41.48165], "fy":[-23.13155,-19.22049,-12.3303,-15.69477]}, + {"t":1.19422, "x":5.28786, "y":2.30955, "heading":1.63311, "vx":-2.89152, "vy":1.23896, "omega":-1.08532, "ax":2.52783, "ay":-1.08331, "alpha":0.91804, "fx":[39.96487,42.5726,45.42379,44.0295], "fy":[-24.7578,-20.37327,-12.5106,-16.06575]}, + {"t":1.23041, "x":5.18488, "y":2.35368, "heading":1.59383, "vx":-2.80004, "vy":1.19976, "omega":-1.0521, "ax":2.5518, "ay":-1.09325, "alpha":0.93563, "fx":[40.32492,42.81459,45.83566,44.64635], "fy":[-25.05715,-20.78317,-12.64083,-15.90235]}, + {"t":1.2666, "x":5.08522, "y":2.39638, "heading":1.55576, "vx":-2.7077, "vy":1.16019, "omega":-1.01824, "ax":2.56208, "ay":-1.09751, "alpha":0.94468, "fx":[40.50095,42.85939,45.99243,44.96806], "fy":[-25.15256,-21.07817,-12.76947,-15.67315]}, + {"t":1.30279, "x":4.98891, "y":2.43765, "heading":1.51891, "vx":-2.61498, "vy":1.12047, "omega":-0.98406, "ax":2.56777, "ay":-1.09988, "alpha":0.95079, "fx":[40.61874,42.84074,46.06309,45.1854], "fy":[-25.17397,-21.3285,-12.89967,-15.43226]}, + {"t":1.33898, "x":4.89596, "y":2.47748, "heading":1.4833, "vx":-2.52206, "vy":1.08067, "omega":-0.94965, "ax":2.57137, "ay":-1.10139, "alpha":0.95544, "fx":[40.71224,42.79524,46.0941,45.35133], "fy":[-25.15787,-21.55335,-13.03142,-15.19454]}, + {"t":1.37517, "x":4.80637, "y":2.51586, "heading":1.44893, "vx":-2.429, "vy":1.04081, "omega":-0.91507, "ax":2.57384, "ay":-1.10244, "alpha":0.95921, "fx":[40.79414,42.73704,46.10351,45.48657], "fy":[-25.11896,-21.76018,-13.16411,-14.9656]}, + {"t":1.41136, "x":4.72016, "y":2.55281, "heading":1.41582, "vx":-2.33586, "vy":1.00092, "omega":-0.88036, "ax":2.57564, "ay":-1.10322, "alpha":0.96235, "fx":[40.87003,42.67277,46.09989,45.60101], "fy":[-25.06463,-21.95253,-13.29703,-14.74788]}, + {"t":1.44754, "x":4.63731, "y":2.58831, "heading":1.38396, "vx":-2.24265, "vy":0.96099, "omega":-0.84553, "ax":2.57701, "ay":-1.10383, "alpha":0.96498, "fx":[40.94258,42.606,46.08788,45.70011], "fy":[-24.99923,-22.13236,-13.42945,-14.54244]}, + {"t":1.48373, "x":4.55784, "y":2.62236, "heading":1.35336, "vx":-2.14939, "vy":0.92105, "omega":-0.81061, "ax":2.57808, "ay":-1.10432, "alpha":0.9672, "fx":[41.01311,42.53877,46.07025,45.78715], "fy":[-24.92564,-22.30088,-13.56068,-14.3497]}, + {"t":1.51992, "x":4.48175, "y":2.65497, "heading":1.32402, "vx":-2.05609, "vy":0.88108, "omega":-0.77561, "ax":2.57893, "ay":-1.10473, "alpha":0.96906, "fx":[41.08224,42.47236,46.04879,45.86434], "fy":[-24.846,-22.4589,-13.69008,-14.16968]}, + {"t":1.55611, "x":4.40903, "y":2.68613, "heading":1.29596, "vx":-1.96277, "vy":0.84111, "omega":-0.74054, "ax":2.57964, "ay":-1.10508, "alpha":0.97061, "fx":[41.15018,42.40759,46.02473,45.93321], "fy":[-24.76195,-22.60705,-13.81706,-14.00218]}, + {"t":1.5923, "x":4.33969, "y":2.71585, "heading":1.26916, "vx":-1.86941, "vy":0.80111, "omega":-0.70542, "ax":2.58023, "ay":-1.10538, "alpha":0.97187, "fx":[41.21693,42.34503,45.99895,45.99492], "fy":[-24.67489,-22.74582,-13.94108,-13.84686]}, + {"t":1.62849, "x":4.27373, "y":2.74411, "heading":1.24363, "vx":-1.77604, "vy":0.76111, "omega":-0.67025, "ax":2.58073, "ay":-1.10564, "alpha":0.97288, "fx":[41.28239,42.28504,45.97214,46.05035], "fy":[-24.58598,-22.87562,-14.06166,-13.7033]}, + {"t":1.66468, "x":4.21114, "y":2.77093, "heading":1.21937, "vx":-1.68264, "vy":0.7211, "omega":-0.63504, "ax":2.58116, "ay":-1.10587, "alpha":0.97367, "fx":[41.34637,42.22787,45.9448,46.10022], "fy":[-24.49628,-22.99683,-14.17834,-13.571]}, + {"t":1.70086, "x":4.15194, "y":2.79631, "heading":1.19639, "vx":-1.58924, "vy":0.68108, "omega":-0.5998, "ax":2.58154, "ay":-1.10608, "alpha":0.97427, "fx":[41.40863,42.17369,45.91737,46.14514], "fy":[-24.4067,-23.10977,-14.29074,-13.44945]}, + {"t":1.73705, "x":4.09612, "y":2.82023, "heading":1.17469, "vx":-1.49581, "vy":0.64105, "omega":-0.56455, "ax":2.58187, "ay":-1.10627, "alpha":0.9747, "fx":[41.46893,42.12262,45.8902,46.18561], "fy":[-24.3181,-23.21476,-14.39848,-13.33811]}, + {"t":1.77324, "x":4.04368, "y":2.8427, "heading":1.15426, "vx":-1.40238, "vy":0.60102, "omega":-0.52927, "ax":2.58216, "ay":-1.10644, "alpha":0.975, "fx":[41.52704,42.07472,45.86357,46.22206], "fy":[-24.23124,-23.31209,-14.50124,-13.23646]}, + {"t":1.80943, "x":3.99462, "y":2.86373, "heading":1.1351, "vx":-1.30893, "vy":0.56098, "omega":-0.49399, "ax":2.58243, "ay":-1.1066, "alpha":0.97519, "fx":[41.58271,42.03002,45.83774,46.25486], "fy":[-24.14682,-23.40203,-14.59874,-13.14395]}, + {"t":1.84562, "x":3.94894, "y":2.8833, "heading":1.11723, "vx":-1.21548, "vy":0.52093, "omega":-0.4587, "ax":2.58266, "ay":-1.10674, "alpha":0.97528, "fx":[41.63571,41.98854,45.81293,46.28435], "fy":[-24.06548,-23.48484,-14.69071,-13.06007]}, + {"t":1.88181, "x":3.90665, "y":2.90143, "heading":1.10063, "vx":-1.12202, "vy":0.48088, "omega":-0.4234, "ax":2.58288, "ay":-1.10686, "alpha":0.9753, "fx":[41.68583,41.95027,45.78931,46.31082], "fy":[-23.98779,-23.56078,-14.77693,-12.98432]}, + {"t":1.918, "x":3.86773, "y":2.91811, "heading":1.0853, "vx":-1.02855, "vy":0.44082, "omega":-0.38811, "ax":2.58308, "ay":-1.10698, "alpha":0.97527, "fx":[41.73287,41.91518,45.76707,46.33452], "fy":[-23.91428,-23.63005,-14.85719,-12.91623]}, + {"t":1.95418, "x":3.8322, "y":2.93334, "heading":1.07126, "vx":-0.93507, "vy":0.40076, "omega":-0.35282, "ax":2.58326, "ay":-1.10709, "alpha":0.9752, "fx":[41.77666,41.88325,45.74633,46.35568], "fy":[-23.84544,-23.6929,-14.9313,-12.85535]}, + {"t":1.99037, "x":3.80006, "y":2.94712, "heading":1.05849, "vx":-0.84158, "vy":0.3607, "omega":-0.31752, "ax":2.58342, "ay":-1.10718, "alpha":0.97512, "fx":[41.81704,41.85445,45.72724,46.3745], "fy":[-23.78169,-23.7495,-14.99912,-12.80127]}, + {"t":2.02656, "x":3.77129, "y":2.95944, "heading":1.047, "vx":-0.74809, "vy":0.32063, "omega":-0.28224, "ax":2.58358, "ay":-1.10727, "alpha":0.97502, "fx":[41.85386,41.82873,45.7099,46.39115], "fy":[-23.72343,-23.80006,-15.0605,-12.75361]}, + {"t":2.06275, "x":3.74591, "y":2.97032, "heading":1.03679, "vx":-0.6546, "vy":0.28056, "omega":-0.24695, "ax":2.58372, "ay":-1.10735, "alpha":0.97493, "fx":[41.88699,41.80607,45.69441,46.4058], "fy":[-23.67099,-23.84473,-15.11533,-12.71202]}, + {"t":2.09894, "x":3.72391, "y":2.97975, "heading":1.02785, "vx":-0.5611, "vy":0.24049, "omega":-0.21167, "ax":2.58385, "ay":-1.10743, "alpha":0.97486, "fx":[41.91634,41.78641,45.68085,46.41857], "fy":[-23.62467,-23.88367,-15.16349,-12.6762]}, + {"t":2.13513, "x":3.7053, "y":2.98773, "heading":1.02019, "vx":-0.46759, "vy":0.20041, "omega":-0.17639, "ax":2.58397, "ay":-1.10749, "alpha":0.97481, "fx":[41.94181,41.76974,45.66931,46.42959], "fy":[-23.58475,-23.91702,-15.2049,-12.64586]}, + {"t":2.17132, "x":3.69007, "y":2.99426, "heading":1.01381, "vx":-0.37408, "vy":0.16033, "omega":-0.14111, "ax":2.58408, "ay":-1.10755, "alpha":0.97479, "fx":[41.96331,41.75602,45.65984,46.43895], "fy":[-23.55143,-23.94489,-15.2395,-12.62079]}, + {"t":2.2075, "x":3.67823, "y":2.99933, "heading":1.0087, "vx":-0.28056, "vy":0.12025, "omega":-0.10584, "ax":2.58419, "ay":-1.10761, "alpha":0.9748, "fx":[41.98079,41.74522,45.65251,46.44674], "fy":[-23.52492,-23.96737,-15.26721,-12.60077]}, + {"t":2.24369, "x":3.66976, "y":3.00296, "heading":1.00487, "vx":-0.18705, "vy":0.08017, "omega":-0.07056, "ax":2.58429, "ay":-1.10765, "alpha":0.97486, "fx":[41.9942,41.73732,45.64734,46.45303], "fy":[-23.50534,-23.98456,-15.28798,-12.58567]}, + {"t":2.27988, "x":3.66469, "y":3.00513, "heading":1.00232, "vx":-0.09353, "vy":0.04009, "omega":-0.03528, "ax":2.58438, "ay":-1.1077, "alpha":0.97495, "fx":[42.00351,41.73232,45.64439,46.45786], "fy":[-23.49283,-23.99649,-15.30179,-12.57536]}, + {"t":2.31607, "x":3.663, "y":3.00586, "heading":1.00104, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.3017, "ay":-1.64665, "alpha":-0.12964, "fx":[-38.91341,-39.70123,-39.38614,-38.60453], "fy":[-28.34641,-27.23297,-27.68886,-28.76778]}, + {"t":2.35986, "x":3.66079, "y":3.00428, "heading":1.00104, "vx":-0.10078, "vy":-0.0721, "omega":-0.00568, "ax":-2.30141, "ay":-1.64687, "alpha":-0.1296, "fx":[-38.90828,-39.69602,-39.38113,-38.59961], "fy":[-28.35006,-27.23709,-27.69267,-28.77114]}, + {"t":2.40364, "x":3.65417, "y":2.99955, "heading":1.00079, "vx":-0.20155, "vy":-0.14421, "omega":-0.01135, "ax":-2.30108, "ay":-1.64711, "alpha":-0.12955, "fx":[-38.90255,-39.69028,-39.37579,-38.59431], "fy":[-28.35424,-27.24166,-27.69663,-28.77473]}, + {"t":2.44743, "x":3.64314, "y":2.99165, "heading":1.00029, "vx":-0.3023, "vy":-0.21633, "omega":-0.01702, "ax":-2.30073, "ay":-1.64737, "alpha":-0.1295, "fx":[-38.89615,-39.68395,-39.37007,-38.58855], "fy":[-28.35899,-27.24673,-27.70079,-28.77859]}, + {"t":2.49121, "x":3.6277, "y":2.9806, "heading":0.99955, "vx":-0.40304, "vy":-0.28846, "omega":-0.02269, "ax":-2.30033, "ay":-1.64766, "alpha":-0.12945, "fx":[-38.889,-39.67694,-39.36387,-38.58227], "fy":[-28.36437,-27.25238,-27.70523,-28.78276]}, + {"t":2.535, "x":3.60785, "y":2.96639, "heading":0.99856, "vx":-0.50376, "vy":-0.3606, "omega":-0.02836, "ax":-2.2999, "ay":-1.64798, "alpha":-0.12939, "fx":[-38.88099,-39.66915,-39.35712,-38.57537], "fy":[-28.37045,-27.25867,-27.71,-28.78732]}, + {"t":2.57878, "x":3.58358, "y":2.94902, "heading":0.99731, "vx":-0.60446, "vy":-0.43276, "omega":-0.03403, "ax":-2.29942, "ay":-1.64833, "alpha":-0.12932, "fx":[-38.87202,-39.66047,-39.34968,-38.56774], "fy":[-28.3773,-27.2657,-27.71519,-28.79233]}, + {"t":2.62257, "x":3.55491, "y":2.9285, "heading":0.99582, "vx":-0.70514, "vy":-0.50493, "omega":-0.03969, "ax":-2.29888, "ay":-1.64873, "alpha":-0.12925, "fx":[-38.86193,-39.65074,-39.34142,-38.55923], "fy":[-28.38503,-27.27359,-27.72092,-28.7979]}, + {"t":2.66635, "x":3.52183, "y":2.90481, "heading":0.99409, "vx":-0.8058, "vy":-0.57712, "omega":-0.04535, "ax":-2.29828, "ay":-1.64917, "alpha":-0.12917, "fx":[-38.85055,-39.63977,-39.33217,-38.54967], "fy":[-28.39377,-27.28248,-27.72732,-28.80414]}, + {"t":2.71014, "x":3.48435, "y":2.87796, "heading":0.9921, "vx":-0.90643, "vy":-0.64933, "omega":-0.051, "ax":-2.29759, "ay":-1.64968, "alpha":-0.12908, "fx":[-38.83765,-39.62734,-39.32168,-38.53885], "fy":[-28.40368,-27.29256,-27.73456,-28.81121]}, + {"t":2.75392, "x":3.44246, "y":2.84794, "heading":0.98987, "vx":-1.00703, "vy":-0.72156, "omega":-0.05666, "ax":-2.29681, "ay":-1.65025, "alpha":-0.12897, "fx":[-38.82292,-39.61314,-39.30967,-38.52647], "fy":[-28.41498,-27.30407,-27.74287,-28.81929]}, + {"t":2.79771, "x":3.39616, "y":2.81477, "heading":0.98739, "vx":-1.1076, "vy":-0.79382, "omega":-0.0623, "ax":-2.2959, "ay":-1.65092, "alpha":-0.12885, "fx":[-38.80598,-39.59676,-39.29575,-38.51215], "fy":[-28.42792,-27.31731,-27.75252,-28.82865]}, + {"t":2.84149, "x":3.34547, "y":2.77843, "heading":0.98466, "vx":-1.20813, "vy":-0.8661, "omega":-0.06794, "ax":-2.29485, "ay":-1.65169, "alpha":-0.12871, "fx":[-38.78631,-39.57769,-39.27942,-38.49541], "fy":[-28.44287,-27.33268,-27.7639,-28.83961]}, + {"t":2.88528, "x":3.29037, "y":2.73892, "heading":0.98168, "vx":-1.30861, "vy":-0.93842, "omega":-0.07358, "ax":-2.2936, "ay":-1.6526, "alpha":-0.12855, "fx":[-38.7632,-39.55521,-39.25998,-38.47555], "fy":[-28.46034,-27.35077,-27.77754,-28.85265]}, + {"t":2.92907, "x":3.23087, "y":2.69625, "heading":0.97846, "vx":-1.40903, "vy":-1.01078, "omega":-0.07921, "ax":-2.2921, "ay":-1.6537, "alpha":-0.12835, "fx":[-38.73566,-39.52829,-39.23645,-38.45163], "fy":[-28.481,-27.37233,-27.79416,-28.8684]}, + {"t":2.97285, "x":3.16698, "y":2.65041, "heading":0.97499, "vx":-1.50939, "vy":-1.08319, "omega":-0.08483, "ax":-2.29027, "ay":-1.65504, "alpha":-0.12811, "fx":[-38.70226,-39.49549,-39.20744,-38.42226], "fy":[-28.50586,-27.39849,-27.81482,-28.88779]}, + {"t":3.01664, "x":3.09869, "y":2.60139, "heading":0.97128, "vx":-1.60967, "vy":-1.15566, "omega":-0.09044, "ax":-2.28798, "ay":-1.65671, "alpha":-0.12781, "fx":[-38.66085,-39.45461,-39.17081,-38.38538], "fy":[-28.53641,-27.43094,-27.84108,-28.91223]}, + {"t":3.06042, "x":3.02602, "y":2.5492, "heading":0.96732, "vx":-1.70985, "vy":-1.2282, "omega":-0.09603, "ax":-2.28504, "ay":-1.65885, "alpha":-0.12743, "fx":[-38.60805,-39.40222,-39.12326,-38.33771], "fy":[-28.57497,-27.47231,-27.87541,-28.94388]}, + {"t":3.10421, "x":2.94896, "y":2.49383, "heading":0.96311, "vx":-1.80991, "vy":-1.30083, "omega":-0.10161, "ax":-2.28111, "ay":-1.66171, "alpha":-0.12693, "fx":[-38.53823,-39.33257,-39.05924,-38.27384], "fy":[-28.62544,-27.52695,-27.92188,-28.98638]}, + {"t":3.14799, "x":2.86753, "y":2.43528, "heading":0.95867, "vx":-1.90978, "vy":-1.37359, "omega":-0.10717, "ax":-2.2756, "ay":-1.66569, "alpha":-0.12623, "fx":[-38.44123,-39.23533,-38.96876,-38.18395], "fy":[-28.69475,-27.60268,-27.98783,-29.04621]}, + {"t":3.19178, "x":2.78173, "y":2.37354, "heading":0.95397, "vx":-2.00942, "vy":-1.44652, "omega":-0.1127, "ax":-2.26733, "ay":-1.67163, "alpha":-0.12518, "fx":[-38.29673,-39.08977,-38.83179,-38.04844], "fy":[-28.79667,-27.715,-28.08779,-29.13628]}, + {"t":3.23556, "x":2.69157, "y":2.30861, "heading":0.94904, "vx":-2.1087, "vy":-1.51972, "omega":-0.11818, "ax":-2.25353, "ay":-1.68145, "alpha":-0.12345, "fx":[-38.05725,-38.84745,-38.60148,-37.82146], "fy":[-28.96293,-27.89964,-28.25528,-29.28631]}, + {"t":3.27935, "x":2.59708, "y":2.24045, "heading":0.94386, "vx":-2.20737, "vy":-1.59334, "omega":-0.12358, "ax":-2.22585, "ay":-1.7008, "alpha":-0.12001, "fx":[-37.58014,-38.36244,-38.13663,-37.36509], "fy":[-29.28655,-28.26156,-28.58899,-29.58364]}, + {"t":3.32313, "x":2.4983, "y":2.16906, "heading":0.93845, "vx":-2.30483, "vy":-1.66781, "omega":-0.12884, "ax":-2.14233, "ay":-1.75643, "alpha":-0.10966, "fx":[-36.1507,-36.89969,-36.72448,-35.9868], "fy":[-30.20668,-29.29816,-29.55852,-30.44236]}, + {"t":3.36692, "x":2.39533, "y":2.09435, "heading":0.93281, "vx":-2.39863, "vy":-1.74472, "omega":-0.13364, "ax":1.18299, "ay":-1.68186, "alpha":0.27897, "fx":[18.73385,19.79067,21.47107,20.49347], "fy":[-29.29422,-29.41564,-27.94382,-27.77831]}, + {"t":3.4107, "x":2.29143, "y":2.01634, "heading":0.92696, "vx":-2.34684, "vy":-1.81836, "omega":-0.12143, "ax":2.36244, "ay":1.44644, "alpha":0.15003, "fx":[39.92156,40.73422,40.44433,39.63735], "fy":[24.99566,23.67646,24.23349,25.50826]}, + {"t":3.45449, "x":2.19094, "y":1.93811, "heading":0.92164, "vx":-2.24339, "vy":-1.75502, "omega":-0.11486, "ax":2.33738, "ay":1.54375, "alpha":0.14026, "fx":[39.48443,40.30189,40.02753,39.21884], "fy":[26.6599,25.41857,25.87817,27.07841]}, + {"t":3.49828, "x":2.09495, "y":1.86275, "heading":0.91661, "vx":-2.14105, "vy":-1.68743, "omega":-0.10872, "ax":2.32821, "ay":1.57629, "alpha":0.13698, "fx":[39.32334,40.14175,39.87622,39.06765], "fy":[27.2176,26.00234,26.42696,27.60232]}, + {"t":3.54206, "x":2.00344, "y":1.79037, "heading":0.91185, "vx":-2.03911, "vy":-1.61841, "omega":-0.10272, "ax":2.32348, "ay":1.59258, "alpha":0.13535, "fx":[39.239,40.05824,39.79906,38.99029], "fy":[27.49812,26.29527,26.70014,27.86365]}, + {"t":3.58585, "x":1.91638, "y":1.72104, "heading":0.90736, "vx":-1.93738, "vy":-1.54868, "omega":-0.09679, "ax":2.32058, "ay":1.60235, "alpha":0.13437, "fx":[39.18667,40.00679,39.75275,38.94357], "fy":[27.66764,26.47169,26.86296,28.01988]}, + {"t":3.62963, "x":1.83378, "y":1.65476, "heading":0.90312, "vx":-1.83577, "vy":-1.47852, "omega":-0.09091, "ax":2.31863, "ay":1.60887, "alpha":0.13372, "fx":[39.15075,39.97177,39.72215,38.91246], "fy":[27.78158,26.58981,26.97064,28.12358]}, + {"t":3.67342, "x":1.75562, "y":1.59157, "heading":0.89914, "vx":-1.73425, "vy":-1.40808, "omega":-0.08505, "ax":2.31723, "ay":1.61353, "alpha":0.13326, "fx":[39.12441,39.94631,39.7006,38.89035], "fy":[27.86367,26.67457,27.0469,28.19729]}, + {"t":3.7172, "x":1.68191, "y":1.53146, "heading":0.89541, "vx":-1.63279, "vy":-1.33743, "omega":-0.07922, "ax":2.31617, "ay":1.61702, "alpha":0.13292, "fx":[39.10416,39.9269,39.68471,38.87391], "fy":[27.92578,26.73844,27.10356,28.25228]}, + {"t":3.76099, "x":1.61264, "y":1.47445, "heading":0.89195, "vx":-1.53137, "vy":-1.26662, "omega":-0.0734, "ax":2.31535, "ay":1.61973, "alpha":0.13266, "fx":[39.08804,39.91159,39.67259,38.86124], "fy":[27.97451,26.78835,27.14723,28.29482]}, + {"t":3.80477, "x":1.5478, "y":1.42055, "heading":0.88873, "vx":-1.42999, "vy":-1.1957, "omega":-0.06759, "ax":2.31468, "ay":1.62191, "alpha":0.13245, "fx":[39.07487,39.89917,39.66308,38.8512], "fy":[28.01381,26.82847,27.18186,28.32868]}, + {"t":3.84856, "x":1.48741, "y":1.36975, "heading":0.88577, "vx":-1.32864, "vy":-1.12469, "omega":-0.06179, "ax":2.31414, "ay":1.62369, "alpha":0.13228, "fx":[39.0639,39.88889,39.65543,38.84306], "fy":[28.04622,26.86143,27.20996,28.35625]}, + {"t":3.89234, "x":1.43145, "y":1.32206, "heading":0.88307, "vx":-1.22732, "vy":-1.05359, "omega":-0.056, "ax":2.31368, "ay":1.62517, "alpha":0.13214, "fx":[39.05461,39.88024,39.64916,38.83634], "fy":[28.07339,26.889,27.23321,28.37912]}, + {"t":3.93613, "x":1.37993, "y":1.27748, "heading":0.88061, "vx":-1.12601, "vy":-0.98244, "omega":-0.05021, "ax":2.3133, "ay":1.62642, "alpha":0.13202, "fx":[39.04665,39.87285,39.64393,38.83069], "fy":[28.0965,26.9124,27.25277,28.39841]}, + {"t":3.97991, "x":1.33285, "y":1.23603, "heading":0.87842, "vx":-1.02472, "vy":-0.91122, "omega":-0.04443, "ax":2.31297, "ay":1.6275, "alpha":0.13192, "fx":[39.03976,39.86648,39.63948,38.82588], "fy":[28.11638,26.9325,27.26947,28.4149]}, + {"t":4.0237, "x":1.2902, "y":1.19769, "heading":0.87647, "vx":-0.92345, "vy":-0.83996, "omega":-0.03866, "ax":2.31268, "ay":1.62843, "alpha":0.13183, "fx":[39.03377,39.86094,39.63563,38.82171], "fy":[28.13363,26.94994,27.28392,28.42918]}, + {"t":4.06748, "x":1.25198, "y":1.16247, "heading":0.87478, "vx":-0.82219, "vy":-0.76866, "omega":-0.03288, "ax":2.31243, "ay":1.62925, "alpha":0.13175, "fx":[39.02853,39.85609,39.63226,38.81805], "fy":[28.14872,26.9652,27.29658,28.44168]}, + {"t":4.11127, "x":1.2182, "y":1.13038, "heading":0.87334, "vx":-0.72094, "vy":-0.69732, "omega":-0.02712, "ax":2.31221, "ay":1.62997, "alpha":0.13168, "fx":[39.02392,39.85182,39.62925,38.8148], "fy":[28.16199,26.97864,27.30779,28.45273]}, + {"t":4.15506, "x":1.18885, "y":1.10141, "heading":0.87215, "vx":-0.6197, "vy":-0.62595, "omega":-0.02135, "ax":2.31201, "ay":1.63061, "alpha":0.13162, "fx":[39.01988,39.84805,39.62653,38.81188], "fy":[28.17372,26.99054,27.31782,28.4626]}, + {"t":4.19884, "x":1.16393, "y":1.07556, "heading":0.87122, "vx":-0.51847, "vy":-0.55456, "omega":-0.01559, "ax":2.31183, "ay":1.63118, "alpha":0.13157, "fx":[39.01633,39.84471,39.62402,38.80923], "fy":[28.18412,27.00115,27.32689,28.47148]}, + {"t":4.24263, "x":1.14344, "y":1.05284, "heading":0.87053, "vx":-0.41724, "vy":-0.48314, "omega":-0.00983, "ax":2.31167, "ay":1.6317, "alpha":0.13152, "fx":[39.01321,39.84174,39.62168,38.80679], "fy":[28.19336,27.01062,27.33517,28.47955]}, + {"t":4.28641, "x":1.12739, "y":1.03325, "heading":0.8701, "vx":-0.31602, "vy":-0.41169, "omega":-0.00407, "ax":2.31153, "ay":1.63216, "alpha":0.13148, "fx":[39.01049,39.83912,39.61947,38.80451], "fy":[28.20159,27.01911,27.34281,28.48693]}, + {"t":4.3302, "x":1.11577, "y":1.01679, "heading":0.86993, "vx":-0.21481, "vy":-0.34023, "omega":0.00169, "ax":2.31139, "ay":1.63259, "alpha":0.13144, "fx":[39.00812,39.83679,39.61736,38.80238], "fy":[28.20891,27.02674,27.3499,28.49373]}, + {"t":4.37398, "x":1.10858, "y":1.00346, "heading":0.87, "vx":-0.11361, "vy":-0.26874, "omega":0.00745, "ax":2.31127, "ay":1.63297, "alpha":0.13141, "fx":[39.00609,39.83473,39.6153,38.80035], "fy":[28.21542,27.0336,27.35655,28.50004]}, + {"t":4.41777, "x":1.10582, "y":0.99326, "heading":0.87033, "vx":-0.01241, "vy":-0.19724, "omega":0.0132, "ax":2.31116, "ay":1.63333, "alpha":0.13137, "fx":[39.00436,39.83292,39.61329,38.79841], "fy":[28.22119,27.03978,27.36283,28.50594]}, + {"t":4.46155, "x":1.10749, "y":0.98619, "heading":0.8709, "vx":0.08879, "vy":-0.12573, "omega":0.01895, "ax":2.31095, "ay":1.63357, "alpha":0.12618, "fx":[39.01315,39.80868,39.59761,38.81468], "fy":[28.20727,27.07359,27.38363,28.48198]}, + {"t":4.50429, "x":1.1134, "y":0.98231, "heading":0.87171, "vx":0.18756, "vy":-0.05591, "omega":0.02434, "ax":2.31066, "ay":1.63379, "alpha":0.12613, "fx":[39.00861,39.80383,39.59233,38.80966], "fy":[28.21022,27.07728,27.38797,28.48562]}, + {"t":4.54704, "x":1.12353, "y":0.98141, "heading":0.87275, "vx":0.28632, "vy":0.01392, "omega":0.02974, "ax":2.31034, "ay":1.63402, "alpha":0.12609, "fx":[39.00376,39.79861,39.58651,38.80415], "fy":[28.21332,27.08122,27.39279,28.48964]}, + {"t":4.58978, "x":1.13787, "y":0.9835, "heading":0.87403, "vx":0.38507, "vy":0.08377, "omega":0.03513, "ax":2.31, "ay":1.63428, "alpha":0.12604, "fx":[38.99853,39.79295,39.58009,38.79811], "fy":[28.21661,27.08547,27.39815,28.49409]}, + {"t":4.63252, "x":1.15644, "y":0.98857, "heading":0.87553, "vx":0.4838, "vy":0.15362, "omega":0.04051, "ax":2.30962, "ay":1.63456, "alpha":0.12599, "fx":[38.99286,39.78678,39.57301,38.79145], "fy":[28.22012,27.09007,27.40409,28.499]}, + {"t":4.67526, "x":1.17923, "y":0.99663, "heading":0.87726, "vx":0.58252, "vy":0.22348, "omega":0.0459, "ax":2.30921, "ay":1.63486, "alpha":0.12593, "fx":[38.98667,39.78002,39.56518,38.7841], "fy":[28.22394,27.0951,27.41067,28.50442]}, + {"t":4.718, "x":1.20624, "y":1.00767, "heading":0.87922, "vx":0.68122, "vy":0.29336, "omega":0.05128, "ax":2.30875, "ay":1.6352, "alpha":0.12586, "fx":[38.97984,39.77256,39.55651,38.77597], "fy":[28.22812,27.10064,27.41799,28.51044]}, + {"t":4.76074, "x":1.23746, "y":1.02171, "heading":0.88141, "vx":0.7799, "vy":0.36325, "omega":0.05666, "ax":2.30824, "ay":1.63558, "alpha":0.12579, "fx":[38.97227,39.76427,39.54686,38.76693], "fy":[28.23275,27.10679,27.42611,28.51713]}, + {"t":4.80348, "x":1.2729, "y":1.03873, "heading":0.88383, "vx":0.87855, "vy":0.43315, "omega":0.06204, "ax":2.30768, "ay":1.636, "alpha":0.12571, "fx":[38.96379,39.75499,39.5361,38.75683], "fy":[28.23795,27.11368,27.43517,28.52458]}, + {"t":4.84623, "x":1.31256, "y":1.05873, "heading":0.88648, "vx":0.97719, "vy":0.50308, "omega":0.06741, "ax":2.30704, "ay":1.63647, "alpha":0.12563, "fx":[38.95421,39.74454,39.52402,38.7455], "fy":[28.24385,27.12145,27.44531,28.53293]}, + {"t":4.88897, "x":1.35644, "y":1.08173, "heading":0.88937, "vx":1.07579, "vy":0.57302, "omega":0.07278, "ax":2.30631, "ay":1.637, "alpha":0.12553, "fx":[38.94329,39.73265,39.51039,38.73269], "fy":[28.25061,27.13031,27.4567,28.54233]}, + {"t":4.93171, "x":1.40452, "y":1.10772, "heading":0.89248, "vx":1.17437, "vy":0.64299, "omega":0.07814, "ax":2.30549, "ay":1.63762, "alpha":0.12541, "fx":[38.93071,39.71901,39.49491,38.71811], "fy":[28.25847,27.1405,27.46957,28.55299]}, + {"t":4.97445, "x":1.45683, "y":1.1367, "heading":0.89582, "vx":1.27291, "vy":0.71299, "omega":0.0835, "ax":2.30453, "ay":1.63832, "alpha":0.12528, "fx":[38.91607,39.70321,39.47716,38.70136], "fy":[28.26771,27.15235,27.48424,28.56518]}, + {"t":5.01719, "x":1.51334, "y":1.16867, "heading":0.89939, "vx":1.37141, "vy":0.78301, "omega":0.08886, "ax":2.30342, "ay":1.63914, "alpha":0.12513, "fx":[38.89882,39.68467,39.45661,38.68192], "fy":[28.27872,27.1663,27.5011,28.57925]}, + {"t":5.05993, "x":1.57406, "y":1.20363, "heading":0.90318, "vx":1.46986, "vy":0.85307, "omega":0.09421, "ax":2.3021, "ay":1.64011, "alpha":0.12495, "fx":[38.87821,39.66266,39.43253,38.65906], "fy":[28.29201,27.18294,27.52071,28.59568]}, + {"t":5.10267, "x":1.63898, "y":1.24159, "heading":0.90721, "vx":1.56826, "vy":0.92317, "omega":0.09955, "ax":2.30052, "ay":1.64127, "alpha":0.12474, "fx":[38.8532,39.63609,39.40389,38.63178], "fy":[28.30833,27.20308,27.54384,28.61517]}, + {"t":5.14542, "x":1.70811, "y":1.28255, "heading":0.91146, "vx":1.66658, "vy":0.99332, "omega":0.10488, "ax":2.29859, "ay":1.64269, "alpha":0.12448, "fx":[38.82227,39.60343,39.36921,38.59864], "fy":[28.32873,27.22793,27.57158,28.6387]}, + {"t":5.18816, "x":1.78145, "y":1.32651, "heading":0.91595, "vx":1.76483, "vy":1.06353, "omega":0.1102, "ax":2.29618, "ay":1.64446, "alpha":0.12417, "fx":[38.78317,39.56239,39.32627,38.55743], "fy":[28.3548,27.25925,27.6056,28.66773]}, + {"t":5.2309, "x":1.85897, "y":1.37346, "heading":0.92066, "vx":1.86297, "vy":1.13382, "omega":0.11551, "ax":2.29307, "ay":1.64673, "alpha":0.12376, "fx":[38.73233,39.50932,39.27155,38.50473], "fy":[28.389,27.29982,27.64849,28.70457]}, + {"t":5.27364, "x":1.94069, "y":1.42343, "heading":0.92559, "vx":1.96098, "vy":1.2042, "omega":0.1208, "ax":2.28893, "ay":1.64975, "alpha":0.12322, "fx":[38.6638,39.43819,39.19922,38.4348], "fy":[28.43545,27.35426,27.70455,28.75305]}, + {"t":5.31638, "x":2.0266, "y":1.47641, "heading":0.93076, "vx":2.05881, "vy":1.27472, "omega":0.12606, "ax":2.28312, "ay":1.65396, "alpha":0.12248, "fx":[38.56689,39.3381,39.09875,38.33733], "fy":[28.5015,27.43081,27.7815,28.82003]}, + {"t":5.35912, "x":2.11668, "y":1.5324, "heading":0.93615, "vx":2.1564, "vy":1.34541, "omega":0.1313, "ax":2.27441, "ay":1.66025, "alpha":0.12137, "fx":[38.42017,39.18722,38.94904,38.19161], "fy":[28.60175,27.54583,27.89463,28.91912]}, + {"t":5.40187, "x":2.21093, "y":1.59142, "heading":0.94176, "vx":2.25361, "vy":1.41637, "omega":0.13649, "ax":2.25986, "ay":1.67063, "alpha":0.11953, "fx":[38.17356,38.93447,38.70067,37.9493], "fy":[28.76976,27.73701,28.07925,29.0817]}, + {"t":5.44461, "x":2.30931, "y":1.65348, "heading":0.94759, "vx":2.3502, "vy":1.48778, "omega":0.14159, "ax":2.23067, "ay":1.69106, "alpha":0.11589, "fx":[37.67658,38.42598,38.20472,37.4649], "fy":[29.10389,28.11496,28.4391,29.39984]}, + {"t":5.48735, "x":2.4118, "y":1.71862, "heading":0.95364, "vx":2.44554, "vy":1.56005, "omega":0.14655, "ax":2.14256, "ay":1.74966, "alpha":0.10495, "fx":[36.17509,36.88618,36.70888,36.0075], "fy":[30.06708,29.20246,29.46673,30.30891]}, + {"t":5.53009, "x":2.51829, "y":1.7869, "heading":0.95991, "vx":2.53712, "vy":1.63484, "omega":0.15103, "ax":-1.18537, "ay":1.67074, "alpha":-0.28005, "fx":[-18.76694,-19.86803,-21.52031,-20.49593], "fy":[29.12774,29.21764,27.73102,27.59869]}, + {"t":5.57283, "x":2.62564, "y":1.8583, "heading":0.96636, "vx":2.48645, "vy":1.70625, "omega":0.13906, "ax":-2.37231, "ay":-1.42239, "alpha":-0.14751, "fx":[-40.12345,-40.89398,-40.58018,-39.81134], "fy":[-24.53579,-23.26248,-23.87322,-25.10603]}, + {"t":5.61557, "x":2.72975, "y":1.92993, "heading":0.97231, "vx":2.38505, "vy":1.64545, "omega":0.13276, "ax":-2.34716, "ay":-1.52508, "alpha":-0.13724, "fx":[-39.69112,-40.46323,-40.15586,-39.38813], "fy":[-26.28627,-25.09534,-25.6148,-26.76843]}, + {"t":5.65831, "x":2.82955, "y":1.99886, "heading":0.97798, "vx":2.28473, "vy":1.58027, "omega":0.12689, "ax":-2.33788, "ay":-1.55946, "alpha":-0.13379, "fx":[-39.53385,-40.30367,-39.99699,-39.23209], "fy":[-26.86809,-25.70762,-26.20182,-27.3263]}, + {"t":5.70106, "x":2.92507, "y":2.06498, "heading":0.9834, "vx":2.18481, "vy":1.51361, "omega":0.12117, "ax":-2.33307, "ay":-1.57667, "alpha":-0.13208, "fx":[-39.45376,-40.22133,-39.91331,-39.15074], "fy":[-27.1571,-26.01316,-26.49777,-27.60648]}, + {"t":5.7438, "x":3.01632, "y":2.12823, "heading":0.98858, "vx":2.08509, "vy":1.44623, "omega":0.11553, "ax":-2.33012, "ay":-1.58699, "alpha":-0.13105, "fx":[-39.40584,-40.17136,-39.86117,-39.10059], "fy":[-27.32905,-26.19588,-26.6769,-27.77535]}, + {"t":5.78654, "x":3.10331, "y":2.1886, "heading":0.99352, "vx":1.9855, "vy":1.37839, "omega":0.10993, "ax":-2.32814, "ay":-1.59388, "alpha":-0.13037, "fx":[-39.37432,-40.13796,-39.82527,-39.06643], "fy":[-27.44256,-26.31722,-26.79747,-27.88846]}, + {"t":5.82928, "x":3.18604, "y":2.24606, "heading":0.99822, "vx":1.88599, "vy":1.31027, "omega":0.10436, "ax":-2.32671, "ay":-1.5988, "alpha":-0.12989, "fx":[-39.35226,-40.11416,-39.79883,-39.04157], "fy":[-27.52273,-26.40352,-26.88448,-27.96967]}, + {"t":5.87202, "x":3.26453, "y":2.3006, "heading":1.00268, "vx":1.78654, "vy":1.24194, "omega":0.0988, "ax":-2.32563, "ay":-1.60249, "alpha":-0.12954, "fx":[-39.33612,-40.0964,-39.77841,-39.0226], "fy":[-27.58214,-26.46794,-26.95044,-28.03088]}, + {"t":5.91476, "x":3.33876, "y":2.35222, "heading":1.0069, "vx":1.68714, "vy":1.17344, "omega":0.09327, "ax":-2.32479, "ay":-1.60536, "alpha":-0.12926, "fx":[-39.32393,-40.08267,-39.76208,-39.00761], "fy":[-27.62777,-26.51782,-27.00232,-28.07874]}, + {"t":5.9575, "x":3.40875, "y":2.40091, "heading":1.01089, "vx":1.58778, "vy":1.10483, "omega":0.08774, "ax":-2.32412, "ay":-1.60765, "alpha":-0.12904, "fx":[-39.31447,-40.07178,-39.74866,-38.99545], "fy":[-27.66381,-26.55753,-27.04427,-28.11722]}, + {"t":6.00025, "x":3.47449, "y":2.44666, "heading":1.01464, "vx":1.48844, "vy":1.03611, "omega":0.08223, "ax":-2.32356, "ay":-1.60953, "alpha":-0.12886, "fx":[-39.30698,-40.06294,-39.7374,-38.98536], "fy":[-27.69292,-26.58988,-27.07897,-28.14886]}, + {"t":6.04299, "x":3.53599, "y":2.48948, "heading":1.01815, "vx":1.38913, "vy":0.96732, "omega":0.07672, "ax":-2.3231, "ay":-1.6111, "alpha":-0.12872, "fx":[-39.30093,-40.05563,-39.72779,-38.97685], "fy":[-27.71687,-26.61672,-27.10817,-28.17535]}, + {"t":6.08573, "x":3.59324, "y":2.52935, "heading":1.02143, "vx":1.28983, "vy":0.89846, "omega":0.07122, "ax":-2.32271, "ay":-1.61242, "alpha":-0.12859, "fx":[-39.29598,-40.04949,-39.71949,-38.96957], "fy":[-27.7369,-26.63934,-27.13311,-28.19785]}, + {"t":6.12847, "x":3.64625, "y":2.56628, "heading":1.02448, "vx":1.19056, "vy":0.82954, "omega":0.06572, "ax":-2.32237, "ay":-1.61355, "alpha":-0.12849, "fx":[-39.29185,-40.04426,-39.71225,-38.96327], "fy":[-27.75389,-26.65866,-27.15466,-28.2172]}, + {"t":6.17121, "x":3.69501, "y":2.60026, "heading":1.02729, "vx":1.0913, "vy":0.76058, "omega":0.06023, "ax":-2.32208, "ay":-1.61454, "alpha":-0.12839, "fx":[-39.28836,-40.03976,-39.70587,-38.95776], "fy":[-27.76848,-26.67536,-27.17347,-28.23403]}, + {"t":6.21395, "x":3.73953, "y":2.63129, "heading":1.02986, "vx":0.99205, "vy":0.69157, "omega":0.05474, "ax":-2.32183, "ay":-1.6154, "alpha":-0.12832, "fx":[-39.28537,-40.03583,-39.70023,-38.95292], "fy":[-27.78116,-26.68994,-27.19001,-28.24878]}, + {"t":6.25669, "x":3.77982, "y":2.65938, "heading":1.0322, "vx":0.89281, "vy":0.62252, "omega":0.04926, "ax":-2.3216, "ay":-1.61616, "alpha":-0.12825, "fx":[-39.28276,-40.03237,-39.69521,-38.94863], "fy":[-27.79229,-26.70279,-27.20466,-28.26182]}, + {"t":6.29944, "x":3.81586, "y":2.68451, "heading":1.03431, "vx":0.79358, "vy":0.55345, "omega":0.04378, "ax":-2.3214, "ay":-1.61683, "alpha":-0.12818, "fx":[-39.28046,-40.0293,-39.69073,-38.94481], "fy":[-27.80216,-26.7142,-27.2177,-28.27342]}, + {"t":6.34218, "x":3.84765, "y":2.70669, "heading":1.03618, "vx":0.69436, "vy":0.48434, "omega":0.0383, "ax":-2.32122, "ay":-1.61744, "alpha":-0.12813, "fx":[-39.27839,-40.02655,-39.68672,-38.94139], "fy":[-27.81101,-26.72441,-27.22935,-28.28379]}, + {"t":6.38492, "x":3.87521, "y":2.72591, "heading":1.03781, "vx":0.59515, "vy":0.41521, "omega":0.03282, "ax":-2.32106, "ay":-1.61798, "alpha":-0.12808, "fx":[-39.2765,-40.02406,-39.68314,-38.93833], "fy":[-27.81901,-26.73362,-27.2398,-28.2931]}, + {"t":6.42766, "x":3.89853, "y":2.74218, "heading":1.03922, "vx":0.49594, "vy":0.34605, "omega":0.02735, "ax":-2.32091, "ay":-1.61847, "alpha":-0.12804, "fx":[-39.27474,-40.02179,-39.67994,-38.93557], "fy":[-27.82631,-26.74197,-27.2492,-28.30151]}, + {"t":6.4704, "x":3.91761, "y":2.75549, "heading":1.04039, "vx":0.39674, "vy":0.27688, "omega":0.02187, "ax":-2.32078, "ay":-1.61892, "alpha":-0.128, "fx":[-39.27308,-40.01971,-39.67709,-38.93308], "fy":[-27.83303,-26.7496,-27.25765,-28.30912]}, + {"t":6.51314, "x":3.93244, "y":2.76585, "heading":1.04132, "vx":0.29755, "vy":0.20768, "omega":0.0164, "ax":-2.32065, "ay":-1.61933, "alpha":-0.12796, "fx":[-39.27149,-40.01778,-39.67455,-38.93084], "fy":[-27.83928,-26.75659,-27.26527,-28.31603]}, + {"t":6.55589, "x":3.94304, "y":2.77324, "heading":1.04202, "vx":0.19836, "vy":0.13847, "omega":0.01093, "ax":-2.32054, "ay":-1.6197, "alpha":-0.12793, "fx":[-39.26995,-40.01598,-39.67231,-38.92882], "fy":[-27.84512,-26.76305,-27.27213,-28.32231]}, + {"t":6.59863, "x":3.9494, "y":2.77768, "heading":1.04249, "vx":0.09918, "vy":0.06924, "omega":0.00547, "ax":-2.32044, "ay":-1.62005, "alpha":-0.1279, "fx":[-39.26843,-40.01429,-39.67034,-38.92699], "fy":[-27.85064,-26.76903,-27.27831,-28.32804]}, + {"t":6.64137, "x":3.95152, "y":2.77916, "heading":1.04272, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.40665, "ay":-1.48901, "alpha":-0.13507, "fx":[-40.7716,-41.47344,-41.10256,-40.39798], "fy":[-25.60135,-24.44938,-25.07022,-26.18927]}, + {"t":6.68503, "x":3.94923, "y":2.77774, "heading":1.04272, "vx":-0.10507, "vy":-0.06501, "omega":-0.0059, "ax":-2.40655, "ay":-1.48895, "alpha":-0.13507, "fx":[-40.76995,-41.47176,-41.10095,-40.39641], "fy":[-25.6003,-24.44844,-25.06925,-26.1882]}, + {"t":6.72869, "x":3.94235, "y":2.77349, "heading":1.04246, "vx":-0.21014, "vy":-0.13002, "omega":-0.01179, "ax":-2.40645, "ay":-1.48888, "alpha":-0.13506, "fx":[-40.76801,-41.4699,-41.09934,-40.39475], "fy":[-25.59938,-24.44748,-25.06798,-26.18697]}, + {"t":6.77235, "x":3.93088, "y":2.76639, "heading":1.04195, "vx":-0.31521, "vy":-0.19502, "omega":-0.01769, "ax":-2.40633, "ay":-1.48881, "alpha":-0.13506, "fx":[-40.76576,-41.46782,-41.0977,-40.39297], "fy":[-25.59859,-24.44649,-25.06639,-26.18556]}, + {"t":6.81601, "x":3.91482, "y":2.75646, "heading":1.04118, "vx":-0.42027, "vy":-0.26002, "omega":-0.02359, "ax":-2.40621, "ay":-1.48873, "alpha":-0.13505, "fx":[-40.76318,-41.46551,-41.096,-40.39105], "fy":[-25.5979,-24.44545,-25.06447,-26.18395]}, + {"t":6.85967, "x":3.89418, "y":2.74369, "heading":1.04015, "vx":-0.52532, "vy":-0.32502, "omega":-0.02948, "ax":-2.40607, "ay":-1.48865, "alpha":-0.13504, "fx":[-40.76024,-41.46294,-41.09423,-40.38897], "fy":[-25.59729,-24.44435,-25.06219,-26.18213]}, + {"t":6.90333, "x":3.86895, "y":2.72808, "heading":1.03886, "vx":-0.63037, "vy":-0.39001, "omega":-0.03538, "ax":-2.40592, "ay":-1.48855, "alpha":-0.13504, "fx":[-40.75689,-41.46007,-41.09234,-40.38669], "fy":[-25.59675,-24.44316,-25.05955,-26.18007]}, + {"t":6.94699, "x":3.83914, "y":2.70963, "heading":1.03732, "vx":-0.73542, "vy":-0.45501, "omega":-0.04128, "ax":-2.40575, "ay":-1.48845, "alpha":-0.13503, "fx":[-40.75311,-41.45685,-41.09029,-40.38417], "fy":[-25.59625,-24.44187,-25.05651,-26.17775]}, + {"t":6.99065, "x":3.80473, "y":2.68835, "heading":1.03551, "vx":-0.84045, "vy":-0.51999, "omega":-0.04717, "ax":-2.40556, "ay":-1.48833, "alpha":-0.13502, "fx":[-40.74882,-41.45324,-41.08803,-40.38136], "fy":[-25.59575,-24.44044,-25.05304,-26.17512]}, + {"t":7.03431, "x":3.76575, "y":2.66422, "heading":1.03345, "vx":-0.94548, "vy":-0.58497, "omega":-0.05307, "ax":-2.40534, "ay":-1.4882, "alpha":-0.13501, "fx":[-40.74397,-41.44915,-41.08549,-40.37819], "fy":[-25.5952,-24.43883,-25.0491,-26.17215]}, + {"t":7.07797, "x":3.72218, "y":2.63727, "heading":1.03114, "vx":-1.05049, "vy":-0.64995, "omega":-0.05896, "ax":-2.4051, "ay":-1.48805, "alpha":-0.135, "fx":[-40.73847,-41.4445,-41.08258,-40.37458], "fy":[-25.59455,-24.437,-25.04465,-26.16878]}, + {"t":7.12163, "x":3.67402, "y":2.60747, "heading":1.02856, "vx":-1.1555, "vy":-0.71491, "omega":-0.06485, "ax":-2.40482, "ay":-1.48787, "alpha":-0.13499, "fx":[-40.7322,-41.43918,-41.0792,-40.37043], "fy":[-25.59373,-24.43487,-25.03961,-26.16493]}, + {"t":7.16529, "x":3.62128, "y":2.57484, "heading":1.02573, "vx":-1.26049, "vy":-0.77987, "omega":-0.07075, "ax":-2.40449, "ay":-1.48767, "alpha":-0.13498, "fx":[-40.72501,-41.43302,-41.0752,-40.36558], "fy":[-25.59265,-24.43237,-25.03389,-26.16051]}, + {"t":7.20895, "x":3.56395, "y":2.53937, "heading":1.02264, "vx":-1.36547, "vy":-0.84483, "omega":-0.07664, "ax":-2.40411, "ay":-1.48743, "alpha":-0.13497, "fx":[-40.71671,-41.42584,-41.07038,-40.35984], "fy":[-25.59116,-24.42936,-25.02739,-26.15538]}, + {"t":7.25261, "x":3.50205, "y":2.50107, "heading":1.0193, "vx":-1.47044, "vy":-0.90977, "omega":-0.08253, "ax":-2.40365, "ay":-1.48715, "alpha":-0.13495, "fx":[-40.707,-41.41733,-41.06446,-40.35295], "fy":[-25.5891,-24.4257,-25.01993,-26.14938]}, + {"t":7.29627, "x":3.43556, "y":2.45993, "heading":1.01569, "vx":-1.57538, "vy":-0.9747, "omega":-0.08843, "ax":-2.4031, "ay":-1.48681, "alpha":-0.13494, "fx":[-40.69551,-41.40711,-41.05708,-40.34453], "fy":[-25.58621,-24.42115,-25.01128,-26.14225]}, + {"t":7.33993, "x":3.36448, "y":2.41596, "heading":1.01183, "vx":-1.6803, "vy":-1.03961, "omega":-0.09432, "ax":-2.40242, "ay":-1.48639, "alpha":-0.13492, "fx":[-40.68164,-41.39458,-41.04764,-40.33401], "fy":[-25.58213,-24.41536,-25.0011,-26.13362]}, + {"t":7.38359, "x":3.28883, "y":2.36915, "heading":1.00771, "vx":-1.78519, "vy":-1.1045, "omega":-0.10021, "ax":-2.40156, "ay":-1.48586, "alpha":-0.1349, "fx":[-40.6645,-41.37884,-41.0353,-40.32054], "fy":[-25.57629,-24.40782,-24.98887,-26.12293]}, + {"t":7.42725, "x":3.2086, "y":2.31952, "heading":1.00334, "vx":-1.89004, "vy":-1.16938, "omega":-0.1061, "ax":-2.40043, "ay":-1.48516, "alpha":-0.13488, "fx":[-40.64265,-41.35844,-41.01865,-40.30273], "fy":[-25.56777,-24.39767,-24.97373,-26.10927]}, + {"t":7.47091, "x":3.1238, "y":2.26705, "heading":0.99871, "vx":-1.99484, "vy":-1.23422, "omega":-0.11199, "ax":-2.39889, "ay":-1.48421, "alpha":-0.13485, "fx":[-40.6136,-41.33087,-40.99526,-40.27818], "fy":[-25.55501,-24.38343,-24.95422,-26.09109]}, + {"t":7.51457, "x":3.03441, "y":2.21174, "heading":0.99382, "vx":-2.09958, "vy":-1.29902, "omega":-0.11787, "ax":-2.39667, "ay":-1.48283, "alpha":-0.13482, "fx":[-40.57264,-41.2914,-40.96058,-40.24235], "fy":[-25.53503,-24.36233,-24.92757,-26.06543]}, + {"t":7.55823, "x":2.94046, "y":2.15362, "heading":0.98867, "vx":-2.20422, "vy":-1.36376, "omega":-0.12376, "ax":-2.39319, "ay":-1.48068, "alpha":-0.13479, "fx":[-40.50973,-41.22992,-40.90486,-40.18557], "fy":[-25.5015,-24.32841,-24.88786,-26.02603]}, + {"t":7.60189, "x":2.84195, "y":2.09266, "heading":0.98327, "vx":-2.3087, "vy":-1.42841, "omega":-0.12965, "ax":-2.38694, "ay":-1.47681, "alpha":-0.13475, "fx":[-40.39879,-41.12023,-40.80282,-40.08269], "fy":[-25.43799,-24.2662,-24.81972,-25.95659]}, + {"t":7.64555, "x":2.73887, "y":2.02889, "heading":0.97761, "vx":-2.41292, "vy":-1.49288, "omega":-0.13553, "ax":-2.37241, "ay":-1.46782, "alpha":-0.13469, "fx":[-40.14477,-40.8668,-40.56226,-39.84205], "fy":[-25.28459,-24.11932,-24.66711,-25.79779]}, + {"t":7.68921, "x":2.63127, "y":1.96231, "heading":0.97169, "vx":-2.5165, "vy":-1.55697, "omega":-0.14141, "ax":-2.3011, "ay":-1.4237, "alpha":-0.13454, "fx":[-38.91022,-39.62788,-39.37029,-38.65553], "fy":[-24.51372,-23.39073,-23.93584,-25.02661]}, + {"t":7.73287, "x":2.5192, "y":1.89298, "heading":0.96552, "vx":-2.61696, "vy":-1.61913, "omega":-0.14728, "ax":2.3011, "ay":1.4237, "alpha":0.13438, "fx":[38.90742,39.62596,39.37294,38.65763], "fy":[24.51845,23.39346,23.93119,25.0238]}, + {"t":7.77652, "x":2.40714, "y":1.82365, "heading":0.95909, "vx":-2.5165, "vy":-1.55697, "omega":-0.14142, "ax":2.37241, "ay":1.46782, "alpha":0.13464, "fx":[40.13511,40.86293,40.57135,39.84649], "fy":[25.30014,24.12532,24.65194,25.79142]}, + {"t":7.82018, "x":2.29953, "y":1.75707, "heading":0.95291, "vx":-2.41292, "vy":-1.49288, "omega":-0.13554, "ax":2.38694, "ay":1.47681, "alpha":0.13472, "fx":[40.3827,41.11421,40.81797,40.08965], "fy":[25.46373,24.27584,24.7946,25.94633]}, + {"t":7.86384, "x":2.19646, "y":1.6933, "heading":0.94699, "vx":-2.3087, "vy":-1.42841, "omega":-0.12966, "ax":2.39319, "ay":1.48068, "alpha":0.13477, "fx":[40.48751,41.22181,40.92578,40.19499], "fy":[25.53698,24.34161,24.85325,26.01196]}, + {"t":7.9075, "x":2.09794, "y":1.63234, "heading":0.94133, "vx":-2.20422, "vy":-1.36376, "omega":-0.12377, "ax":2.39667, "ay":1.48283, "alpha":0.13481, "fx":[40.54457,41.28127,40.98701,40.25414], "fy":[25.57979,24.37896,24.88391,26.0477]}, + {"t":7.95116, "x":2.00399, "y":1.57422, "heading":0.93593, "vx":-2.09958, "vy":-1.29902, "omega":-0.11789, "ax":2.39889, "ay":1.48421, "alpha":0.13485, "fx":[40.57995,41.31881,41.02692,40.29222], "fy":[25.60861,24.40334,24.90194,26.06985]}, + {"t":7.99482, "x":1.91461, "y":1.51891, "heading":0.93078, "vx":-1.99484, "vy":-1.23422, "omega":-0.112, "ax":2.40043, "ay":1.48516, "alpha":0.13488, "fx":[40.60371,41.34454,41.05529,40.31893], "fy":[25.62976,24.42071,24.91326,26.0847]}, + {"t":8.03848, "x":1.8298, "y":1.46644, "heading":0.92589, "vx":-1.89004, "vy":-1.16938, "omega":-0.10611, "ax":2.40156, "ay":1.48586, "alpha":0.1349, "fx":[40.62055,41.36319,41.07666,40.33879], "fy":[25.64623,24.43384,24.92065,26.09519]}, + {"t":8.08214, "x":1.74957, "y":1.41681, "heading":0.92126, "vx":-1.78519, "vy":-1.1045, "omega":-0.10022, "ax":2.40242, "ay":1.48639, "alpha":0.13492, "fx":[40.63295,41.37727,41.09346,40.3542], "fy":[25.65958,24.4442,24.92556,26.10287]}, + {"t":8.1258, "x":1.67392, "y":1.37, "heading":0.91688, "vx":-1.6803, "vy":-1.03961, "omega":-0.09433, "ax":2.4031, "ay":1.48681, "alpha":0.13494, "fx":[40.64237,41.38824,41.10708,40.36654], "fy":[25.67073,24.45265,24.92885,26.10867]}, + {"t":8.16946, "x":1.60285, "y":1.32603, "heading":0.91277, "vx":-1.57538, "vy":-0.9747, "omega":-0.08844, "ax":2.40365, "ay":1.48715, "alpha":0.13496, "fx":[40.64969,41.39699,41.11839,40.37668], "fy":[25.68023,24.4597,24.93104,26.11314]}, + {"t":8.21312, "x":1.53636, "y":1.28489, "heading":0.90891, "vx":-1.47044, "vy":-0.90977, "omega":-0.08255, "ax":2.40411, "ay":1.48743, "alpha":0.13498, "fx":[40.6555,41.40412,41.12797,40.38517], "fy":[25.68847,24.4657,24.93247,26.11666]}, + {"t":8.25678, "x":1.47445, "y":1.24659, "heading":0.9053, "vx":-1.36547, "vy":-0.84483, "omega":-0.07665, "ax":2.40449, "ay":1.48767, "alpha":0.13499, "fx":[40.66018,41.41003,41.13619,40.3924], "fy":[25.69569,24.47088,24.93338,26.11946]}, + {"t":8.30044, "x":1.41713, "y":1.21112, "heading":0.90195, "vx":-1.26049, "vy":-0.77987, "omega":-0.07076, "ax":2.40482, "ay":1.48787, "alpha":0.13501, "fx":[40.66403,41.415,41.14334,40.39863], "fy":[25.70206,24.4754,24.93393,26.12174]}, + {"t":8.3441, "x":1.36439, "y":1.17849, "heading":0.89887, "vx":-1.1555, "vy":-0.71491, "omega":-0.06486, "ax":2.4051, "ay":1.48805, "alpha":0.13502, "fx":[40.66724,41.41923,41.14959,40.40406], "fy":[25.70773,24.47938,24.93425,26.12362]}, + {"t":8.38776, "x":1.31623, "y":1.14869, "heading":0.89603, "vx":-1.05049, "vy":-0.64995, "omega":-0.05897, "ax":2.40534, "ay":1.4882, "alpha":0.13503, "fx":[40.66997,41.42289,41.15511,40.40882], "fy":[25.71278,24.48291,24.9344,26.1252]}, + {"t":8.43142, "x":1.27266, "y":1.12174, "heading":0.89346, "vx":-0.94548, "vy":-0.58497, "omega":-0.05307, "ax":2.40556, "ay":1.48833, "alpha":0.13504, "fx":[40.67232,41.42608,41.16001,40.41303], "fy":[25.71729,24.48604,24.93447,26.12655]}, + {"t":8.47508, "x":1.23367, "y":1.09762, "heading":0.89114, "vx":-0.84045, "vy":-0.51999, "omega":-0.04718, "ax":2.40575, "ay":1.48845, "alpha":0.13505, "fx":[40.67438,41.4289,41.16436,40.41677], "fy":[25.72132,24.48884,24.9345,26.12772]}, + {"t":8.51874, "x":1.19927, "y":1.07633, "heading":0.88908, "vx":-0.73542, "vy":-0.45501, "omega":-0.04128, "ax":2.40592, "ay":1.48855, "alpha":0.13506, "fx":[40.67622,41.43142,41.16824,40.42011], "fy":[25.7249,24.49133,24.93454,26.12877]}, + {"t":8.5624, "x":1.16945, "y":1.05788, "heading":0.88728, "vx":-0.63037, "vy":-0.39001, "omega":-0.03539, "ax":2.40607, "ay":1.48865, "alpha":0.13506, "fx":[40.6779,41.43368,41.17169,40.42309], "fy":[25.72808,24.49355,24.9346,26.12973]}, + {"t":8.60606, "x":1.14423, "y":1.04227, "heading":0.88573, "vx":-0.52532, "vy":-0.32502, "omega":-0.02949, "ax":2.40621, "ay":1.48873, "alpha":0.13507, "fx":[40.67946,41.43575,41.17477,40.42576], "fy":[25.73088,24.49552,24.93473,26.13063]}, + {"t":8.64972, "x":1.12358, "y":1.0295, "heading":0.88445, "vx":-0.42027, "vy":-0.26002, "omega":-0.02359, "ax":2.40633, "ay":1.48881, "alpha":0.13508, "fx":[40.68093,41.43764,41.17751,40.42816], "fy":[25.73332,24.49727,24.93494,26.13149]}, + {"t":8.69338, "x":1.10753, "y":1.01957, "heading":0.88342, "vx":-0.31521, "vy":-0.19502, "omega":-0.01769, "ax":2.40645, "ay":1.48888, "alpha":0.13508, "fx":[40.68235,41.43941,41.17994,40.4303], "fy":[25.73543,24.4988,24.93524,26.13233]}, + {"t":8.73704, "x":1.09606, "y":1.01247, "heading":0.88264, "vx":-0.21014, "vy":-0.13002, "omega":-0.0118, "ax":2.40655, "ay":1.48895, "alpha":0.13509, "fx":[40.68373,41.44106,41.18207,40.43221], "fy":[25.73722,24.50014,24.93566,26.13317]}, + {"t":8.7807, "x":1.08918, "y":1.00822, "heading":0.88213, "vx":-0.10507, "vy":-0.06501, "omega":-0.0059, "ax":2.40665, "ay":1.48901, "alpha":0.1351, "fx":[40.6851,41.44262,41.18394,40.43392], "fy":[25.73871,24.50129,24.9362,26.13402]}, + {"t":8.82436, "x":1.08688, "y":1.0068, "heading":0.88187, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.64709, "ay":2.28942, "alpha":-0.70923, "fx":[31.16593,25.31202,24.32813,31.25989], "fy":[36.69018,40.9491,41.53143,36.59875]}, + {"t":8.86167, "x":1.08803, "y":1.00839, "heading":0.88187, "vx":0.06145, "vy":0.08541, "omega":-0.02646, "ax":1.64702, "ay":2.28933, "alpha":-0.70919, "fx":[31.16467,25.31126,24.32734,31.2584], "fy":[36.68901,40.94757,41.52959,36.59737]}, + {"t":8.89897, "x":1.09147, "y":1.01317, "heading":0.88088, "vx":0.12289, "vy":0.17081, "omega":-0.05292, "ax":1.64696, "ay":2.28924, "alpha":-0.70915, "fx":[31.16609,25.31387,24.32352,31.25362], "fy":[36.68539,40.94383,41.52936,36.5986]}, + {"t":8.93628, "x":1.0972, "y":1.02114, "heading":0.87891, "vx":0.18433, "vy":0.25621, "omega":-0.07937, "ax":1.64689, "ay":2.28914, "alpha":-0.70909, "fx":[31.17018,25.31984,24.31667,31.24555], "fy":[36.67931,40.93784,41.53073,36.60242]}, + {"t":8.97358, "x":1.10522, "y":1.03229, "heading":0.87595, "vx":0.24577, "vy":0.34161, "omega":-0.10582, "ax":1.64681, "ay":2.28903, "alpha":-0.70902, "fx":[31.1769,25.32917,24.30681,31.23413], "fy":[36.67079,40.9296,41.53366,36.60885]}, + {"t":9.01089, "x":1.11554, "y":1.04662, "heading":0.872, "vx":0.3072, "vy":0.427, "omega":-0.13227, "ax":1.64673, "ay":2.28891, "alpha":-0.70893, "fx":[31.18623,25.34188,24.29397,31.21933], "fy":[36.65982,40.91907,41.5381,36.61788]}, + {"t":9.04819, "x":1.12814, "y":1.06415, "heading":0.86707, "vx":0.36863, "vy":0.51239, "omega":-0.15872, "ax":1.64664, "ay":2.28878, "alpha":-0.70883, "fx":[31.1981,25.35797,24.27822,31.20106], "fy":[36.6464,40.90622,41.54399,36.62954]}, + {"t":9.0855, "x":1.14304, "y":1.08485, "heading":0.86115, "vx":0.43006, "vy":0.59778, "omega":-0.18516, "ax":1.64654, "ay":2.28864, "alpha":-0.70872, "fx":[31.21245,25.37745,24.25959,31.17925], "fy":[36.63056,40.89099,41.55126,36.64384]}, + {"t":9.12281, "x":1.16023, "y":1.10875, "heading":0.85424, "vx":0.49149, "vy":0.68315, "omega":-0.2116, "ax":1.64644, "ay":2.28849, "alpha":-0.70859, "fx":[31.22923,25.40036,24.23818,31.1538], "fy":[36.6123,40.87334,41.55983,36.66081]}, + {"t":9.16011, "x":1.17971, "y":1.13582, "heading":0.84635, "vx":0.55291, "vy":0.76853, "omega":-0.23804, "ax":1.64632, "ay":2.28832, "alpha":-0.70843, "fx":[31.24834,25.4267,24.21406,31.12461], "fy":[36.59163,40.8532,41.56958,36.68048]}, + {"t":9.19742, "x":1.20148, "y":1.16609, "heading":0.83747, "vx":0.61432, "vy":0.85389, "omega":-0.26446, "ax":1.64619, "ay":2.28814, "alpha":-0.70826, "fx":[31.26969,25.45649,24.18734,31.09153], "fy":[36.56858,40.83049,41.58041,36.70286]}, + {"t":9.23472, "x":1.22554, "y":1.19953, "heading":0.8276, "vx":0.67574, "vy":0.93925, "omega":-0.29089, "ax":1.64605, "ay":2.28793, "alpha":-0.70805, "fx":[31.29316,25.48975,24.15811,31.05443], "fy":[36.54316,40.80514,41.59217,36.72799]}, + {"t":9.27203, "x":1.2519, "y":1.23616, "heading":0.81675, "vx":0.73714, "vy":1.02461, "omega":-0.3173, "ax":1.64589, "ay":2.28771, "alpha":-0.70781, "fx":[31.31861,25.52651,24.12651,31.01313], "fy":[36.51538,40.77705,41.6047,36.75588]}, + {"t":9.30933, "x":1.28054, "y":1.27598, "heading":0.80491, "vx":0.79854, "vy":1.10995, "omega":-0.34371, "ax":1.64572, "ay":2.28745, "alpha":-0.70754, "fx":[31.34589,25.56676,24.09266,30.96744], "fy":[36.48526,40.74609,41.61781,36.78655]}, + {"t":9.34664, "x":1.31148, "y":1.31898, "heading":0.79209, "vx":0.85994, "vy":1.19528, "omega":-0.3701, "ax":1.64552, "ay":2.28717, "alpha":-0.70723, "fx":[31.37482,25.61052,24.05669,30.91715], "fy":[36.4528,40.71213,41.63129,36.81997]}, + {"t":9.38394, "x":1.3447, "y":1.36516, "heading":0.77828, "vx":0.92132, "vy":1.28061, "omega":-0.39648, "ax":1.64529, "ay":2.28684, "alpha":-0.70687, "fx":[31.40518,25.65776,24.01874,30.86201], "fy":[36.41799,40.67501,41.64489,36.85614]}, + {"t":9.42125, "x":1.38022, "y":1.41453, "heading":0.76349, "vx":0.9827, "vy":1.36592, "omega":-0.42285, "ax":1.64503, "ay":2.28647, "alpha":-0.70645, "fx":[31.43671,25.70846,23.97896,30.80172], "fy":[36.38082,40.63452,41.65829,36.89498]}, + {"t":9.45855, "x":1.41802, "y":1.46707, "heading":0.74772, "vx":1.04407, "vy":1.45122, "omega":-0.44921, "ax":1.64472, "ay":2.28603, "alpha":-0.70598, "fx":[31.4691,25.76256,23.93748,30.73597], "fy":[36.34123,40.59043,41.67112,36.93639]}, + {"t":9.49586, "x":1.45812, "y":1.5228, "heading":0.73096, "vx":1.10543, "vy":1.5365, "omega":-0.47555, "ax":1.64436, "ay":2.28553, "alpha":-0.70544, "fx":[31.50197,25.81996,23.89441,30.66433], "fy":[36.29913,40.54241,41.68292,36.98018]}, + {"t":9.53316, "x":1.5005, "y":1.58171, "heading":0.71322, "vx":1.16677, "vy":1.62176, "omega":-0.50186, "ax":1.64394, "ay":2.28492, "alpha":-0.70482, "fx":[31.53482,25.8805,23.84985,30.58632], "fy":[36.25435,40.49006,41.69311,37.02607]}, + {"t":9.57047, "x":1.54517, "y":1.6438, "heading":0.6945, "vx":1.2281, "vy":1.707, "omega":-0.52816, "ax":1.64341, "ay":2.28419, "alpha":-0.70412, "fx":[31.56702,25.94391,23.80382,30.50129], "fy":[36.20662,40.43282,41.70089,37.07359]}, + {"t":9.60778, "x":1.59213, "y":1.70907, "heading":0.67479, "vx":1.28941, "vy":1.79221, "omega":-0.55442, "ax":1.64277, "ay":2.28329, "alpha":-0.7033, "fx":[31.59769,26.0098,23.75623,30.40837], "fy":[36.15547,40.3699,41.70518,37.12203]}, + {"t":9.64508, "x":1.64137, "y":1.77752, "heading":0.65411, "vx":1.35069, "vy":1.87739, "omega":-0.58066, "ax":1.64195, "ay":2.28215, "alpha":-0.70235, "fx":[31.62557,26.0775,23.70684,30.30634], "fy":[36.10015,40.30011,41.70441,37.17022]}, + {"t":9.68239, "x":1.6929, "y":1.84914, "heading":0.63245, "vx":1.41194, "vy":1.96253, "omega":-0.60686, "ax":1.64087, "ay":2.28066, "alpha":-0.70123, "fx":[31.64875,26.14593,23.65504,30.19331], "fy":[36.03932,40.2216,41.69615,37.21625]}, + {"t":9.71969, "x":1.74672, "y":1.92394, "heading":0.60981, "vx":1.47316, "vy":2.04761, "omega":-0.63302, "ax":1.6394, "ay":2.27862, "alpha":-0.69988, "fx":[31.66405,26.21317,23.59956,30.06617], "fy":[35.97056,40.13121,41.6763,37.25672]}, + {"t":9.757, "x":1.80282, "y":2.00192, "heading":0.58619, "vx":1.53432, "vy":2.13261, "omega":-0.65913, "ax":1.63727, "ay":2.27568, "alpha":-0.69818, "fx":[31.66569,26.27554,23.53772,29.9192], "fy":[35.88916,40.02301,41.63736,37.28517]}, + {"t":9.7943, "x":1.86119, "y":2.08306, "heading":0.5616, "vx":1.5954, "vy":2.21751, "omega":-0.68518, "ax":1.63393, "ay":2.27106, "alpha":-0.69587, "fx":[31.64158,26.32515,23.46336,29.74041], "fy":[35.78476,39.88444,41.5635,37.28769]}, + {"t":9.83161, "x":1.92185, "y":2.16736, "heading":0.53604, "vx":1.65635, "vy":2.30223, "omega":-0.71114, "ax":1.62791, "ay":2.26276, "alpha":-0.6923, "fx":[31.56079,26.3415,23.35986,29.49896], "fy":[35.63018,39.68294,41.41403,37.22825]}, + {"t":9.86891, "x":1.98477, "y":2.25482, "heading":0.50951, "vx":1.71708, "vy":2.38665, "omega":-0.73696, "ax":1.61393, "ay":2.24347, "alpha":-0.68504, "fx":[31.31152,26.24961,23.16554,29.08342], "fy":[35.3255,39.29994,41.04164,36.97587]}, + {"t":9.90622, "x":2.04995, "y":2.34542, "heading":0.48202, "vx":1.77729, "vy":2.47034, "omega":-0.76252, "ax":1.54564, "ay":2.14919, "alpha":-0.6517, "fx":[29.91516,25.38582,22.33322,27.52965], "fy":[33.98625,37.68739,39.16142,35.39327]}, + {"t":9.94352, "x":2.11733, "y":2.43907, "heading":0.45358, "vx":1.83495, "vy":2.55052, "omega":-0.78683, "ax":-1.54589, "ay":-2.14768, "alpha":0.74403, "fx":[-30.44689,-25.39744,-21.72664,-27.60979], "fy":[-33.55309,-37.75021,-39.52974,-35.29265]}, + {"t":9.98083, "x":2.18471, "y":2.53273, "heading":0.42422, "vx":1.77728, "vy":2.4704, "omega":-0.75907, "ax":-1.61386, "ay":-2.24274, "alpha":0.71431, "fx":[-31.60438,-26.5442,-22.88548,-28.77084], "fy":[-35.05852,-39.11211,-41.21233,-37.21042]}, + {"t":10.01813, "x":2.24989, "y":2.62332, "heading":0.39591, "vx":1.71707, "vy":2.38673, "omega":-0.73243, "ax":-1.62785, "ay":-2.26233, "alpha":0.70802, "fx":[-31.87422,-26.87051,-23.10929,-28.90296], "fy":[-35.34098,-39.33216,-41.56688,-37.68616]}, + {"t":10.05544, "x":2.31281, "y":2.71079, "heading":0.36858, "vx":1.65635, "vy":2.30233, "omega":-0.70601, "ax":-1.63388, "ay":-2.27079, "alpha":0.70521, "fx":[-32.00987,-27.07237,-23.19871,-28.88656], "fy":[-35.44566,-39.38514,-41.7238,-37.94711]}, + {"t":10.09275, "x":2.37346, "y":2.7951, "heading":0.34224, "vx":1.59539, "vy":2.21762, "omega":-0.67971, "ax":-1.63724, "ay":-2.2755, "alpha":0.70358, "fx":[-32.0979,-27.22995,-23.24563,-28.82255], "fy":[-35.4928,-39.38333,-41.81282,-38.13376]}, + {"t":10.13005, "x":2.43184, "y":2.87624, "heading":0.31689, "vx":1.53431, "vy":2.13273, "omega":-0.65346, "ax":-1.63938, "ay":-2.27852, "alpha":0.70251, "fx":[-32.16231,-27.36524,-23.27506,-28.73885], "fy":[-35.51537,-39.35775,-41.8698,-38.28466]}, + {"t":10.16736, "x":2.48794, "y":2.95422, "heading":0.29251, "vx":1.47316, "vy":2.04773, "omega":-0.62725, "ax":-1.64086, "ay":-2.2806, "alpha":0.70176, "fx":[-32.21253,-27.48671,-23.29639,-28.64638], "fy":[-35.52596,-39.32042,-41.90874,-38.41462]}, + {"t":10.20466, "x":2.54175, "y":3.02902, "heading":0.26911, "vx":1.41194, "vy":1.96265, "omega":-0.60107, "ax":-1.64194, "ay":-2.28214, "alpha":0.70122, "fx":[-32.25311,-27.5983,-23.31383,-28.55037], "fy":[-35.53037,-39.27702,-41.9363,-38.53053]}, + {"t":10.24197, "x":2.59328, "y":3.10065, "heading":0.24669, "vx":1.35069, "vy":1.87752, "omega":-0.57491, "ax":-1.64276, "ay":-2.28332, "alpha":0.70082, "fx":[-32.28656,-27.70208,-23.32945,-28.45364], "fy":[-35.53156,-39.23058,-41.95609,-38.63601]}, + {"t":10.27927, "x":2.64253, "y":3.16911, "heading":0.22524, "vx":1.28941, "vy":1.79234, "omega":-0.54877, "ax":-1.64341, "ay":-2.28425, "alpha":0.70053, "fx":[-32.31446,-27.79924,-23.34429,-28.3579], "fy":[-35.53118,-39.18289,-41.97031,-38.73314]}, + {"t":10.31658, "x":2.68949, "y":3.23438, "heading":0.20477, "vx":1.2281, "vy":1.70712, "omega":-0.52263, "ax":-1.64394, "ay":-2.285, "alpha":0.70032, "fx":[-32.33784,-27.89052,-23.3589,-28.26424], "fy":[-35.53015,-39.13509,-41.98036,-38.82321]}, + {"t":10.35388, "x":2.73416, "y":3.29647, "heading":0.18527, "vx":1.16677, "vy":1.62188, "omega":-0.49651, "ax":-1.64437, "ay":-2.28562, "alpha":0.70019, "fx":[-32.35747,-27.97638,-23.37354,-28.17342], "fy":[-35.52903,-39.08791,-41.98722,-38.90707]}, + {"t":10.39119, "x":2.77654, "y":3.35539, "heading":0.16675, "vx":1.10543, "vy":1.53661, "omega":-0.47039, "ax":-1.64473, "ay":-2.28615, "alpha":0.70011, "fx":[-32.37393,-28.05715,-23.3883,-28.08595], "fy":[-35.52814,-39.04188,-41.99159,-38.98528]}, + {"t":10.42849, "x":2.81663, "y":3.41112, "heading":0.1492, "vx":1.04407, "vy":1.45133, "omega":-0.44427, "ax":-1.64503, "ay":-2.28659, "alpha":0.70007, "fx":[-32.38767,-28.13306,-23.40318,-28.00221], "fy":[-35.52764,-38.99736,-41.99401,-39.05825]}, + {"t":10.4658, "x":2.85444, "y":3.46367, "heading":0.13263, "vx":0.9827, "vy":1.36602, "omega":-0.41815, "ax":-1.64529, "ay":-2.28698, "alpha":0.70007, "fx":[-32.39908,-28.20428,-23.41812,-27.9225], "fy":[-35.52762,-38.95464,-41.99491,-39.12629]}, + {"t":10.5031, "x":2.88995, "y":3.51304, "heading":0.11703, "vx":0.92132, "vy":1.28071, "omega":-0.39204, "ax":-1.64552, "ay":-2.28731, "alpha":0.7001, "fx":[-32.40849,-28.27093,-23.43302,-27.84702], "fy":[-35.5281,-38.91393,-41.99461,-39.18963]}, + {"t":10.54041, "x":2.92318, "y":3.55923, "heading":0.1024, "vx":0.85994, "vy":1.19538, "omega":-0.36592, "ax":-1.64572, "ay":-2.28761, "alpha":0.70015, "fx":[-32.41618,-28.33313,-23.44778,-27.77593], "fy":[-35.52906,-38.87539,-41.99341,-39.24845]}, + {"t":10.57772, "x":2.95411, "y":3.60223, "heading":0.08875, "vx":0.79854, "vy":1.11004, "omega":-0.3398, "ax":-1.6459, "ay":-2.28787, "alpha":0.70022, "fx":[-32.42241,-28.39095,-23.46227,-27.70938], "fy":[-35.53046,-38.83913,-41.99154,-39.30291]}, + {"t":10.61502, "x":2.98276, "y":3.64205, "heading":0.07607, "vx":0.73714, "vy":1.02469, "omega":-0.31368, "ax":-1.64605, "ay":-2.2881, "alpha":0.70029, "fx":[-32.4274,-28.44447,-23.47638,-27.64744], "fy":[-35.53224,-38.80528,-41.9892,-39.35314]}, + {"t":10.65233, "x":3.00911, "y":3.67868, "heading":0.06437, "vx":0.67573, "vy":0.93933, "omega":-0.28755, "ax":-1.6462, "ay":-2.28831, "alpha":0.70038, "fx":[-32.43134,-28.49373,-23.48997,-27.5902], "fy":[-35.53434,-38.7739,-41.98656,-39.39925]}, + {"t":10.68963, "x":3.03318, "y":3.71213, "heading":0.05365, "vx":0.61432, "vy":0.85396, "omega":-0.26142, "ax":-1.64632, "ay":-2.2885, "alpha":0.70047, "fx":[-32.43442,-28.53879,-23.50293,-27.53771], "fy":[-35.53667,-38.74505,-41.98378,-39.44132]}, + {"t":10.72694, "x":3.05495, "y":3.7424, "heading":0.04389, "vx":0.55291, "vy":0.76859, "omega":-0.23529, "ax":-1.64644, "ay":-2.28867, "alpha":0.70055, "fx":[-32.4368,-28.57969,-23.51516,-27.49001], "fy":[-35.53918,-38.7188,-41.98097,-39.47944]}, + {"t":10.76424, "x":3.07443, "y":3.76948, "heading":0.03512, "vx":0.49149, "vy":0.68321, "omega":-0.20916, "ax":-1.64654, "ay":-2.28882, "alpha":0.70064, "fx":[-32.43863,-28.61648,-23.52655,-27.44713], "fy":[-35.54179,-38.69519,-41.97827,-39.51369]}, + {"t":10.80155, "x":3.09162, "y":3.79337, "heading":0.02731, "vx":0.43006, "vy":0.59782, "omega":-0.18302, "ax":-1.64664, "ay":-2.28897, "alpha":0.70071, "fx":[-32.44003,-28.64919,-23.53701,-27.40909], "fy":[-35.54443,-38.67425,-41.97577,-39.54413]}, + {"t":10.83885, "x":3.10652, "y":3.81408, "heading":0.02048, "vx":0.36863, "vy":0.51243, "omega":-0.15688, "ax":-1.64673, "ay":-2.2891, "alpha":0.70078, "fx":[-32.44112,-28.67784,-23.54646,-27.37592], "fy":[-35.54705,-38.65602,-41.97354,-39.57082]}, + {"t":10.87616, "x":3.11912, "y":3.83161, "heading":0.01463, "vx":0.3072, "vy":0.42704, "omega":-0.13074, "ax":-1.64681, "ay":-2.28922, "alpha":0.70084, "fx":[-32.44201,-28.70246,-23.55482,-27.34761], "fy":[-35.54958,-38.64051,-41.97168,-39.5938]}, + {"t":10.91346, "x":3.12944, "y":3.84594, "heading":0.00975, "vx":0.24577, "vy":0.34164, "omega":-0.10459, "ax":-1.64688, "ay":-2.28933, "alpha":0.70089, "fx":[-32.44276,-28.72308,-23.56203,-27.32419], "fy":[-35.55198,-38.62774,-41.97024,-39.61313]}, + {"t":10.95077, "x":3.13746, "y":3.85709, "heading":0.00585, "vx":0.18433, "vy":0.25623, "omega":-0.07845, "ax":-1.64695, "ay":-2.28943, "alpha":0.70092, "fx":[-32.44347,-28.73972,-23.56804,-27.30565], "fy":[-35.55422,-38.61775,-41.96928,-39.62883]}, + {"t":10.98807, "x":3.14319, "y":3.86506, "heading":0.00293, "vx":0.12289, "vy":0.17083, "omega":-0.0523, "ax":-1.64702, "ay":-2.28952, "alpha":0.70094, "fx":[-32.44418,-28.75239,-23.57281,-27.29201], "fy":[-35.55625,-38.61052,-41.96884,-39.64095]}, + {"t":11.02538, "x":3.14663, "y":3.86984, "heading":0.00098, "vx":0.06145, "vy":0.08541, "omega":-0.02615, "ax":-1.64708, "ay":-2.28961, "alpha":0.70095, "fx":[-32.44495,-28.76111,-23.57631,-27.28326], "fy":[-35.55806,-38.60609,-41.96895,-39.6495]}, + {"t":11.06268, "x":3.14777, "y":3.87143, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cc3effd..01b782d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,41 +1,24 @@ package frc.robot; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Seconds; - import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.VoltageUnit; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Time; -import edu.wpi.first.units.measure.Velocity; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.lib.control.ControlConstants.PIDFConstants; -import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; -import frc.robot.subsystems.drive.ctre.CtreDriveConstants; +import frc.robot.subsystems.drive.DriveConstants; /** - * All constants belong here. + * Miscallenous constants */ public final class Constants { public static final double DT = 0.02; - public static final double DEADBAND = 1e-6; + public static final double EPSILON = 1e-6; public static final double LONG_CANT_TIMEOUT_MS = 0; public static final double TAU = Math.PI * 2; @@ -52,122 +35,6 @@ public static final class Odometry { Math.pow(0.02, 1), // vision Math.pow(0.02, 1)); } - - - public static final class Drive { - // public static final COTSTalonFXSwerveConstants SWERVE_MODULE_TYPE = - // COTSTalonFXSwerveConstants.SDS.MK4i.KrakenX60(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L3); - - public static final double TRACK_WIDTH = Units.Inches.of(24).in(Units.Meters); - public static final double WHEEL_BASE = Units.Inches.of(24).in(Units.Meters); - public static final double WHEEL_DIAMETER = 2 * CtreDriveConstants.kWheelRadius.in(Meters);//SWERVE_MODULE_TYPE.wheelDiameter; - public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER*Math.PI;//SWERVE_MODULE_TYPE.wheelCircumference; - - public static final Translation2d[] MODULE_LOCATIONS = { - new Translation2d(-WHEEL_BASE / 2.0, TRACK_WIDTH / 2.0), - new Translation2d(WHEEL_BASE / 2.0, TRACK_WIDTH / 2.0), - new Translation2d(-WHEEL_BASE / 2.0, -TRACK_WIDTH / 2.0), - new Translation2d(WHEEL_BASE / 2.0, -TRACK_WIDTH / 2.0) - }; - - public static final double DRIVE_GEAR_RATIO = CtreDriveConstants.kDriveGearRatio;//SWERVE_MODULE_TYPE.driveGearRatio; - public static final double ANGLE_GEAR_RATIO = CtreDriveConstants.kSteerGearRatio;//SWERVE_MODULE_TYPE.angleGearRatio; - - public static final double MAX_SPEED = 5.0; - public static final double MAX_ACCEL = 5.0; - public static final double MAX_ROTATION_SPEED = DegreesPerSecond.of(540.0).in(RadiansPerSecond); - public static final double MAX_ROTATION_ACCEL = DegreesPerSecond.of(2880.0).in(RadiansPerSecond); - - public static final int ANGLE_CURRENT_LIMIT = 20; - public static final int ANGLE_CURRENT_THRESHOLD = 30; - public static final double ANGLE_CURRENT_THRESHOLD_TIME = 0.1; - public static final boolean ANGLE_ENABLE_CURRENT_LIMIT = true; - - public static final int DRIVE_CURRENT_LIMIT = 30; - public static final int DRIVE_CURRENT_THRESHOLD = 45; - public static final double DRIVE_CURRENT_THRESHOLD_TIME = 0.1; - public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true; - - public static final double DRIVE_MOTOR_KV = 12 * Math.PI * WHEEL_DIAMETER / (DRIVE_GEAR_RATIO * MAX_SPEED); - - public static final PIDFConstants ANGLE_MOTOR_PIDF_CONSTANTS = new PIDFConstants( - 2.0, 0.0, 0.0, 0); - public static final PIDFConstants DRIVE_MOTOR_PIDF_CONSTANTS = new PIDFConstants( - 1.2, 0.005, 0.0, DRIVE_MOTOR_KV); - - public static final double OPEN_LOOP_RAMP = 0.25; - public static final double CLOSED_LOOP_RAMP = 0.0; - - public static final boolean INVERT_GYRO = false; - - public static final double MAX_VELOCITY_STABLE = 10; // degrees per second - - public static final double MAX_PITCH_STABLE = 5; // degrees - - public static final LinearVelocity kScoringTranslationMaxSpeed = - Units.Centimeters.of(15.0).per(Units.Seconds); - public static final AngularVelocity kScoringRotationMaxSpeed = - Units.Degrees.of(7.0).per(Units.Seconds); // oh god - - public static final Time POSE_RESET_PREVENTION_TIME = Seconds.of(0.15); - - public static enum ModuleConstants { - /** Module 0 */ - FRONT_LEFT ( - 8, 10, 7, Rotation2d.fromRotations(0.142334), true, false), - /** Module 1 */ - FRONT_RIGHT ( - 9, 11, 6, Rotation2d.fromRotations(0.427246), true, false), - /** Module 2 */ - BACK_LEFT ( - 3, 5, 0, Rotation2d.fromRotations(0.174316), true, false), - /** Module 3 */ - BACK_RIGHT ( - 4, 2, 1, Rotation2d.fromRotations(0.413330), false, false); - - public final int driveMotorID; - public final int angleMotorID; - public final int cancoderID; - public final Rotation2d angleOffset; - public final boolean driveInvert; - public final boolean angleInvert; - - /** - * Swerve Module Constants to be used when creating swerve modules. - * @param driveMotorID - * @param angleMotorID - * @param canCoderID - * @param angleOffset - */ - private ModuleConstants(int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset, boolean driveInvert, boolean angleInvert) { - this.driveMotorID = driveMotorID; - this.angleMotorID = angleMotorID; - this.cancoderID = canCoderID; - this.angleOffset = angleOffset; - this.driveInvert = driveInvert; - this.angleInvert = angleInvert; - } - } - } - - public static final class Auto { - public static final PIDFConstants TRANSLATION_CONSTANTS = - new PIDFConstants(3.5, 0.0, 0.1, 1.0); - - public static final ProfiledPIDFConstants PROFILED_TRANSLATION_CONSTANTS = - new ProfiledPIDFConstants(3.5, 0.0, 0.1, 1.0, - new TrapezoidProfile.Constraints( - Drive.MAX_SPEED * 0.7, - Drive.MAX_ACCEL * 0.7)); - - public static final ProfiledPIDFConstants ROTATION_CONSTANTS = - new ProfiledPIDFConstants(4.0, 0.0, 0.0, 1.0, - new TrapezoidProfile.Constraints( - Drive.MAX_ROTATION_SPEED, - Drive.MAX_ROTATION_ACCEL)); - public static final double ACCELERATION_CONSTANT = 0.1; - } - public static final class Limelight { //TODO: this must be tuned to specific robot public static enum VisionDeviceConstants { // L_CONSTANTS ( @@ -240,47 +107,6 @@ private VisionDeviceConstants( } } - public static enum Calibration { - DriveRotation ( - Units.Volts.of(1).per(Units.Second), - Units.Volts.of(7)), - - DriveTranslation ( - Units.Volts.of(1).per(Units.Second), - Units.Volts.of(7)), - - AngleMotor ( - Units.Volts.of(1).per(Units.Second), - Units.Volts.of(7)), - - DriveMotor ( - Units.Volts.of(1).per(Units.Second), - Units.Volts.of(4)); - - public final Velocity RAMP_RATE; - public final Voltage DYNAMIC_VOLTAGE; - private Calibration(Velocity rampRate, Voltage dynamicVoltage) { - RAMP_RATE = rampRate; - DYNAMIC_VOLTAGE = dynamicVoltage; - } - } - - public static enum Port { // TODO: this must be tuned to the specific robot - FL_CANCODER (7, "CV"), - FR_CANCODER (6, "CV"), - BL_CANCODER (14, "CV"), - BR_CANCODER (1, "CV"), - LEDS (21, "CV"), - PIGEON (60, "CV"); - - public final int id; - public final String bus; - private Port(int id, String bus) { - this.id = id; - this.bus = bus; - } - } - public static final class Pathplanner { public static RobotConfig config; static { @@ -291,10 +117,10 @@ public static final class Pathplanner { } } public static final PathConstraints GLOBAL_CONSTRAINTS = - new PathConstraints(Drive.MAX_SPEED * 0.85, - Drive.MAX_ACCEL * 0.85, - Drive.MAX_ROTATION_SPEED * 0.85, - Drive.MAX_ROTATION_ACCEL * 0.85); + new PathConstraints(DriveConstants.MAX_SPEED * 0.85, + DriveConstants.MAX_ACCEL * 0.85, + DriveConstants.MAX_ROTATION_SPEED * 0.85, + DriveConstants.MAX_ROTATION_ACCEL * 0.85); public static final double GENERATION_WAIT_TIME = 5; } } diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 353828b..b59c59f 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -3,33 +3,27 @@ import static frc.robot.Robot.controller; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; public class ControlsMapping { public static void mapTeleopCommand() { - Drive.getInstance().setDefaultCommand((Drive.getInstance().teleopCommand())); - // run sysID functions - Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); - - controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); + controller.back().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); - controller.b().whileTrue(new PIDToPoseCommand( - new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120.0)))); } public static void mapSysId() { - controller.a().onTrue( + // run sysID functions + Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); + controller.a().whileTrue( Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward)); - controller.b().onTrue( + controller.b().whileTrue( Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse)); - controller.x().onTrue( + controller.x().whileTrue( Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward)); - controller.y().onTrue( + controller.y().whileTrue( Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse)); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f8ef887..9c88a50 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,12 +2,18 @@ import frc.robot.auto.AutoSelector; +import java.util.Optional; + import com.pathplanner.lib.commands.FollowPathCommand; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.logging.EpilogueBackend; import edu.wpi.first.hal.AllianceStationID; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.Controllers; @@ -20,9 +26,10 @@ * the TimedRobot documentation. If you change the name of this class or the package after creating * this project, you must also update the Main.java file in the project. */ +@SuppressWarnings("unused") public class Robot extends TimedRobot { private static final CommandScheduler commandScheduler = CommandScheduler.getInstance(); - public AutoSelector autoChooser; + private AutoSelector autoChooser; private Command autoCommand; public static final CommandXboxController controller = @@ -33,22 +40,12 @@ public class Robot extends TimedRobot { * initialization code. */ public Robot() { - if (Robot.isSimulation()) { - // TODO: remove this - - } - - // RobotState.reset(Timer.getFPGATimestamp(), new Pose2d()); - // RobotState.resetKalman(); - - Drive.getInstance(); if (Robot.isReal()) { VisionDeviceManager.getInstance(); } + Drive.getInstance(); TelemetryManager.getInstance(); - - FollowPathCommand.warmupCommand().schedule();; - + FollowPathCommand.warmupCommand().schedule(); autoChooser = new AutoSelector(); } @@ -71,7 +68,6 @@ public void robotPeriodic() { /** This function is called once each time the robot enters Disabled mode. */ @Override public void disabledInit() { - } /** This function is called periodically during disabled. */ @@ -83,9 +79,7 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected. */ @Override public void autonomousInit() { - // RobotState.setAlliance(DriverStation.getAlliance()); autoCommand = autoChooser.getAuto(); - if (autoCommand != null) { autoCommand.schedule(); } else { @@ -109,7 +103,8 @@ public void teleopInit() { if (autoCommand != null) { autoCommand.cancel(); } - + Drive.getInstance().setDefaultCommand(Drive.getInstance().teleopCommand()); + ControlsMapping.mapTeleopCommand(); } diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index f2567db..8f5ca36 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -1,5 +1,6 @@ package frc.robot.auto; +import frc.robot.auto.AutoSelector.Auto; import frc.robot.lib.trajectory.RedTrajectory; import frc.robot.lib.trajectory.TrajectoryLoader; import frc.robot.lib.trajectory.RedTrajectory.TrajectoryType; @@ -10,11 +11,13 @@ import edu.wpi.first.wpilibj2.command.Command; public final class AutoRoutines { + @Auto(name = "Pid Test") public static Command testPidToPose() { return new PIDToPoseCommand( new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120))); } + @Auto(name = "Trajectory Test") public static Command testTrajectoryAuto() { RedTrajectory traj = TrajectoryLoader.loadAutoTrajectory(TrajectoryType.PATHPLANNER, "testPath2").get(); diff --git a/src/main/java/frc/robot/auto/AutoSelector.java b/src/main/java/frc/robot/auto/AutoSelector.java index fbc7916..4cb4551 100644 --- a/src/main/java/frc/robot/auto/AutoSelector.java +++ b/src/main/java/frc/robot/auto/AutoSelector.java @@ -1,8 +1,13 @@ package frc.robot.auto; +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; import java.lang.reflect.Method; import java.util.function.Supplier; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -11,20 +16,37 @@ * A class to select autos */ public class AutoSelector { + @Retention(RetentionPolicy.RUNTIME) + @Target(ElementType.METHOD) + /** An autonomous routine command. */ + public static @interface Auto { + String name() default ""; + } + private final SendableChooser> chooser = new SendableChooser<>(); public AutoSelector() { Method[] autos = AutoRoutines.class.getMethods(); + // Warning: dark and evil magic below for (Method auto : autos) { - var name = auto.getName(); - if (auto.getReturnType().equals(Command.class)) { - chooser.addOption(name, () -> { - try{ - return (Command) auto.invoke(null); - } catch (Exception e) { - return null; - }}); - // System.out.println(name); + if (auto.isAnnotationPresent(Auto.class)) { + String name = auto.getAnnotation(Auto.class).name(); + if (name.equals("")) { + name = auto.getName(); + } + + if (auto.getReturnType() == Command.class) { + chooser.addOption(name, () -> { + try { + return (Command) auto.invoke(null); + } catch (Exception e) { + return null; + } + }); + } else { + DriverStation.reportWarning( + "@Auto annotation for this element is not supported", true); + } } } diff --git a/src/main/java/frc/robot/auto/Automation.java b/src/main/java/frc/robot/auto/Automation.java index 2fe192f..8de0739 100644 --- a/src/main/java/frc/robot/auto/Automation.java +++ b/src/main/java/frc/robot/auto/Automation.java @@ -1,8 +1,5 @@ package frc.robot.auto; -import frc.robot.subsystems.*; - -@SuppressWarnings("unused") public class Automation { } diff --git a/src/main/java/frc/robot/lib/control/PIDVController.java b/src/main/java/frc/robot/lib/control/PIDVController.java index d1de0f3..7f1019c 100644 --- a/src/main/java/frc/robot/lib/control/PIDVController.java +++ b/src/main/java/frc/robot/lib/control/PIDVController.java @@ -1,8 +1,7 @@ package frc.robot.lib.control; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; -import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; import frc.robot.lib.control.ControlConstants.*; public class PIDVController { @@ -15,14 +14,12 @@ public class PIDVController { private double feedforward = 0.0; private double integral = 0.0; - public double error = 0.0; + private double error = 0.0; private boolean isContinuous = false; private double minRange = 0.0; private double maxRange = 0.0; - private final Timer timer = new Timer(); - /** * Creates a PIDV controller, which is a PID controller * where the derivative is replaced by accurate velocity measurements. @@ -30,7 +27,6 @@ public class PIDVController { */ public PIDVController(PIDFConstants constants) { this.constants = constants; - timer.start(); } public PIDVController(PIDConstants constants) { @@ -42,41 +38,49 @@ public PIDVController(PIDConstants constants) { * @param minInput The minimum value. * @param maxInput The maximum value. */ - public void enableContinuousInput(double minInput, double maxInput) { + public PIDVController enableContinuousInput(double minInput, double maxInput) { isContinuous = true; minRange = minInput; maxRange = maxInput; + return this; } /** Makes the controller discontinuous */ - public void disableContinuousInput() { + public PIDVController disableContinuousInput() { isContinuous = false; + return this; } - public void setInput(double positionMeasurement, double velocityMeasurement) { + /** Sets the current position and velocity measurement. */ + public PIDVController setMeasurement(double positionMeasurement, double velocityMeasurement) { this.positionMeasurement = positionMeasurement; this.velocityMeasurement = velocityMeasurement; + return this; } - public void setTarget(double target) { + /** Sets the goal */ + public PIDVController setTarget(double target) { this.target = target; + return this; } /** Sets the feedforward value. */ - public void setFeedforward(double feedforward) { + public PIDVController setFeedforward(double feedforward) { this.feedforward = feedforward; + return this; } public double getOutput() { - double dt = timer.get(); - timer.reset(); - if (dt <= 0.0) return 0.0; - - error = isContinuous - ? MathUtil.inputModulus(target - positionMeasurement, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) - : target - positionMeasurement; + if (isContinuous) { + error = MathUtil.inputModulus( + target - positionMeasurement, + -(maxRange - minRange) / 2.0, + (maxRange - minRange) / 2.0); + } else { + error = target - positionMeasurement; + } - integral += error * dt; + integral += error * Constants.DT; double derivative = feedforward - velocityMeasurement; @@ -86,17 +90,21 @@ public double getOutput() { + constants.kF * feedforward; } + public double getError() { + return error; + } + /** Sets the integral value. */ - public void setIntegral(double integral) { + public PIDVController setIntegral(double integral) { this.integral = integral; + return this; } /** Resets the controller. */ - public void reset() { + public PIDVController reset() { integral = 0.0; feedforward = 0.0; error = 0.0; - timer.reset(); - timer.start(); + return this; } } diff --git a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java index 438d037..e88480f 100644 --- a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java +++ b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java @@ -1,143 +1,167 @@ package frc.robot.lib.control; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; import frc.robot.lib.control.ControlConstants.*; -/** - * A profiled version of {@link PIDVController} that uses a TrapezoidProfile - * to smoothly reach the target position with velocity and acceleration limits. - * The derivative term is based on velocity feedback (like PIDV). - */ +/** Note: broken do not use rn */ public class ProfiledPIDVController { - private final PIDFConstants constants; - private TrapezoidProfile.Constraints constraints; - private TrapezoidProfile profile; + private final PIDFConstants constants; + private TrapezoidProfile.Constraints constraints; + private TrapezoidProfile profile; - private TrapezoidProfile.State goal = new TrapezoidProfile.State(); - private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); + private TrapezoidProfile.State goal = new TrapezoidProfile.State(); + private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); - private double positionMeasurement = 0.0; - private double velocityMeasurement = 0.0; - private double feedforward = 0.0; - private double integral = 0.0; + private double positionMeasurement = 0.0; + private double velocityMeasurement = 0.0; + private double feedforward = 0.0; + private boolean isFeedforwardSet = false; + private double integral = 0.0; - public double error = 0.0; + public double error = 0.0; - private boolean isContinuous = false; - private double minRange = 0.0; - private double maxRange = 0.0; - - private final Timer timer = new Timer(); + private boolean isContinuous = false; + private double minRange = 0.0; + private double maxRange = 0.0; + private double positionTolerance = 0.01; + private double velocityTolerance = Double.POSITIVE_INFINITY; /** - * Creates a profiled PIDV controller. - * @param constants The {@link PIDFConstants}. - * @param constraints The trapezoidal motion constraints (max velocity and acceleration). - */ - public ProfiledPIDVController(ProfiledPIDFConstants constants) { - this.constants = new PIDFConstants( - constants.kP, - constants.kI, - constants.kD, - constants.kF); - this.constraints = constants.constraints; - this.profile = new TrapezoidProfile(constraints); - timer.start(); - } - - /** Enables continuous input. */ - public void enableContinuousInput(double minInput, double maxInput) { - isContinuous = true; - minRange = minInput; - maxRange = maxInput; - } - - /** Disables continuous input. */ - public void disableContinuousInput() { - isContinuous = false; - } - - /** Sets the current position and velocity measurement. */ - public void setInput(double positionMeasurement, double velocityMeasurement) { - this.positionMeasurement = positionMeasurement; - this.velocityMeasurement = velocityMeasurement; - } - - /** Sets the goal position for the profile (end velocity = 0). */ - public void setTarget(double target) { - this.goal = new TrapezoidProfile.State(target, 0.0); - } - - /** Sets the goal with both position and end velocity. */ - public void setTarget(double position, double velocity) { - this.goal = new TrapezoidProfile.State(position, velocity); - } - - /** Sets the feedforward value. */ - public void setFeedforward(double feedforward) { - this.feedforward = feedforward; - } + * Creates a Profiled PIDV controller, which is a PID controller + * where the derivative is replaced by accurate velocity measurements, + * and it's also profiled + * @param constants The {@link ProfiledPIDFConstants}. + */ + public ProfiledPIDVController(ProfiledPIDFConstants constants) { + this.constants = new PIDFConstants(constants.kP, constants.kI, constants.kD, constants.kF); + this.constraints = constants.constraints; + this.profile = new TrapezoidProfile(constraints); + } /** - * Computes the controller output using the current profile and measurements. - * @return Control output (PID + feedforward). + * Makes the controller continuous, which means that values repeat. + * @param minInput The minimum value. + * @param maxInput The maximum value. */ - public double getOutput() { - double dt = timer.get(); - timer.reset(); - if (dt <= 0.0) { - return 0.0; - } - - setpoint = profile.calculate(dt, goal, setpoint); - - double targetPosition = setpoint.position; - double targetVelocity = setpoint.velocity; - - error = isContinuous - ? MathUtil.inputModulus(targetPosition - positionMeasurement, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) - : targetPosition - positionMeasurement; - - integral += error * dt; - - double derivative = targetVelocity - velocityMeasurement; - - return constants.kP * error - + constants.kI * integral - + constants.kD * derivative - + constants.kF * feedforward; - } - - /** Sets the integral term directly. */ - public void setIntegral(double integral) { - this.integral = integral; - } - - /** Resets the controller state. */ - public void reset() { - integral = 0.0; - feedforward = 0.0; - error = 0.0; - setpoint = new TrapezoidProfile.State(positionMeasurement, velocityMeasurement); - timer.reset(); - timer.start(); - } - - /** Returns the current motion profile setpoint. */ - public TrapezoidProfile.State getSetpoint() { - return setpoint; - } - - /** Returns the goal state. */ - public TrapezoidProfile.State getGoal() { - return goal; - } - - /** Updates motion constraints. */ - public void setConstraints(TrapezoidProfile.Constraints constraints) { - this.constraints = constraints; - } + public ProfiledPIDVController enableContinuousInput(double minInput, double maxInput) { + isContinuous = true; + minRange = minInput; + maxRange = maxInput; + return this; + } + + /** Makes the controller discontinuous */ + public ProfiledPIDVController disableContinuousInput() { + isContinuous = false; + return this; + } + + /** Sets the current position and velocity measurement. */ + public ProfiledPIDVController setMeasurement(double positionMeasurement, double velocityMeasurement) { + this.positionMeasurement = positionMeasurement; + this.velocityMeasurement = velocityMeasurement; + return this; + } + + /** Sets the goal */ + public ProfiledPIDVController setTarget(double target) { + this.goal = new TrapezoidProfile.State(target, 0.0); + return this; + } + + /** Sets the goal */ + public void setTarget(double position, double velocity) { + this.goal = new TrapezoidProfile.State(position, velocity); + } + + /** Sets the feedforward value. */ + public ProfiledPIDVController setFeedforward(double feedforward) { + this.feedforward = feedforward; + isFeedforwardSet = true; + return this; + } + + public double getOutput() { + if (isContinuous) { + double errorBound = (maxRange - minRange) / 2.0; + double goalMinDistance = MathUtil.inputModulus( + goal.position - positionMeasurement, -errorBound, errorBound); + double setpointMinDistance = MathUtil.inputModulus( + setpoint.position - positionMeasurement, -errorBound, errorBound); + goal.position = goalMinDistance + positionMeasurement; + setpoint.position = setpointMinDistance + positionMeasurement; + } + + setpoint = profile.calculate(Constants.DT, setpoint, goal); + + double targetPosition = setpoint.position; + double targetVelocity = setpoint.velocity; + + error = targetPosition - positionMeasurement; + integral += error * Constants.DT; + double derivative = targetVelocity - velocityMeasurement; + + if (!isFeedforwardSet) { + feedforward = targetVelocity; + } + isFeedforwardSet = false; + + return constants.kP * error + + constants.kI * integral + + constants.kD * derivative + + constants.kF * feedforward; + } + + /** Sets the integral value. */ + public ProfiledPIDVController setIntegral(double integral) { + this.integral = integral; + return this; + } + + /** Resets the controller. */ + public ProfiledPIDVController reset() { + integral = 0.0; + feedforward = 0.0; + error = 0.0; + setpoint = new TrapezoidProfile.State(positionMeasurement, velocityMeasurement); + return this; + } + + public TrapezoidProfile.State getSetpoint() { + return setpoint; + } + + public TrapezoidProfile.State getGoal() { + return goal; + } + + public double getError() { + return error; + } + + public ProfiledPIDVController setConstraints(TrapezoidProfile.Constraints constraints) { + this.constraints = constraints; + this.profile = new TrapezoidProfile(constraints); + return this; + } + + public ProfiledPIDVController setTolerance(double positionTolerance, double velocityTolerance) { + this.positionTolerance = positionTolerance; + this.velocityTolerance = velocityTolerance; + return this; + } + + public ProfiledPIDVController setTolerance(double positionTolerance) { + return setTolerance(positionTolerance, Double.POSITIVE_INFINITY); + } + + public boolean atSetpoint() { + return Math.abs(error) < positionTolerance && Math.abs(setpoint.velocity) < velocityTolerance; + } + + public boolean atGoal() { + return atSetpoint() && goal.equals(setpoint); + } } diff --git a/src/main/java/frc/robot/lib/localization/FieldLayout.java b/src/main/java/frc/robot/lib/field/FieldLayout.java similarity index 90% rename from src/main/java/frc/robot/lib/localization/FieldLayout.java rename to src/main/java/frc/robot/lib/field/FieldLayout.java index 37032d4..c16c84f 100644 --- a/src/main/java/frc/robot/lib/localization/FieldLayout.java +++ b/src/main/java/frc/robot/lib/field/FieldLayout.java @@ -1,10 +1,8 @@ -package frc.robot.lib.localization; +package frc.robot.lib.field; import java.io.IOException; import java.util.ArrayList; -import java.util.HashSet; import java.util.List; -import java.util.Set; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -15,7 +13,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants; +import frc.robot.subsystems.drive.DriveConstants; /** * Contains various field dimensions and useful reference points. Dimensions are @@ -51,12 +49,12 @@ public class FieldLayout { SmartDashboard.putData(field); int[] reefIds = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; Transform2d leftReefTransform = new Transform2d( - Constants.Drive.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), -Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); + DriveConstants.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), -Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); for (int i : reefIds) { ALIGN_POSES_LEFT.add(APRILTAG_MAP.getTagPose(i).get().toPose2d().transformBy(leftReefTransform)); } Transform2d rightReefTransform = new Transform2d( - Constants.Drive.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); + DriveConstants.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); for (int i : reefIds) { ALIGN_POSES_RIGHT.add(APRILTAG_MAP.getTagPose(i).get().toPose2d().transformBy(rightReefTransform)); } diff --git a/src/main/java/frc/robot/lib/localization/FieldUtil.java b/src/main/java/frc/robot/lib/field/FieldUtil.java similarity index 99% rename from src/main/java/frc/robot/lib/localization/FieldUtil.java rename to src/main/java/frc/robot/lib/field/FieldUtil.java index 4c2d272..c8666d4 100644 --- a/src/main/java/frc/robot/lib/localization/FieldUtil.java +++ b/src/main/java/frc/robot/lib/field/FieldUtil.java @@ -1,4 +1,4 @@ -package frc.robot.lib.localization; +package frc.robot.lib.field; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/lib/sim/Mechanisms.java b/src/main/java/frc/robot/lib/sim/Mechanisms.java new file mode 100644 index 0000000..8c2ac13 --- /dev/null +++ b/src/main/java/frc/robot/lib/sim/Mechanisms.java @@ -0,0 +1,11 @@ +package frc.robot.lib.sim; + +public class Mechanisms { + private static Mechanisms ssmechInstance; + public static Mechanisms getInstance() { + if (ssmechInstance == null) { + ssmechInstance = new Mechanisms(); + } + return ssmechInstance; + } +} diff --git a/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java b/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java index 1875085..6f39a58 100644 --- a/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java +++ b/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java @@ -10,8 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.Constants; -// import frc.robot.RobotState; -import frc.robot.lib.localization.FieldUtil; +import frc.robot.lib.field.FieldUtil; public class RedTrajectory { public TrajectoryType type; diff --git a/src/main/java/frc/robot/lib/util/Util.java b/src/main/java/frc/robot/lib/util/Util.java index 7f802b4..b5592f0 100644 --- a/src/main/java/frc/robot/lib/util/Util.java +++ b/src/main/java/frc/robot/lib/util/Util.java @@ -251,4 +251,43 @@ public static double twist2dMagnitude(Twist2d t) { public static ChassisSpeeds fromTwist2d(Twist2d t) { return new ChassisSpeeds(t.dx, t.dy, t.dtheta); } + + public static double trapezoidProfileTimeToTarget( + double currentPosition, double currentSpeed, + double target, double maxSpeed, double maxAccel + ) { + double delta = target - currentPosition; + double distance = Math.abs(delta); + double direction = Math.signum(delta); + double v0 = currentSpeed * direction; + // Decelerating line + double stopDist = (v0 * v0) / (2 * maxAccel); + if (distance < stopDist) { + // Triangle + return v0 / maxAccel; // time to stop + } + double accelDist = (maxSpeed * maxSpeed - v0 * v0) / (2 * maxAccel); + if (accelDist < 0) accelDist = 0; + double decelDist = (maxSpeed * maxSpeed) / (2 * maxAccel); + double minDistance = accelDist + decelDist; + + // Triangle + if (distance < minDistance) { + // Solve for peak velocity vp + double vp = Math.sqrt((2 * maxAccel * distance + v0 * v0) / 2); + + double accelTime = (vp - v0) / maxAccel; + double decelTime = vp / maxAccel; + + return accelTime + decelTime; + } + + // Trapezoid + double accelTime = (maxSpeed - v0) / maxAccel; + double cruiseDist = distance - minDistance; + double cruiseTime = cruiseDist / maxSpeed; + double decelTime = maxSpeed / maxAccel; + + return accelTime + cruiseTime + decelTime; + } } diff --git a/src/main/java/frc/robot/subsystems/DefaultSubsystem.java b/src/main/java/frc/robot/subsystems/DefaultSubsystem.java deleted file mode 100644 index 36e79ba..0000000 --- a/src/main/java/frc/robot/subsystems/DefaultSubsystem.java +++ /dev/null @@ -1,7 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class DefaultSubsystem extends SubsystemBase { - // Unfinished -} diff --git a/src/main/java/frc/robot/subsystems/LoggingManager.java b/src/main/java/frc/robot/subsystems/LoggingManager.java deleted file mode 100644 index 22e3edf..0000000 --- a/src/main/java/frc/robot/subsystems/LoggingManager.java +++ /dev/null @@ -1,7 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj.DataLogManager; - -public class LoggingManager { - DataLogManager x; -} diff --git a/src/main/java/frc/robot/subsystems/TelemetryManager.java b/src/main/java/frc/robot/subsystems/TelemetryManager.java index 4034238..6a1047d 100644 --- a/src/main/java/frc/robot/subsystems/TelemetryManager.java +++ b/src/main/java/frc/robot/subsystems/TelemetryManager.java @@ -3,10 +3,14 @@ import java.util.ArrayList; import java.util.function.Supplier; +import com.ctre.phoenix6.hardware.TalonFX; + import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.units.Units; import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.struct.Struct; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -30,7 +34,6 @@ public void publish() { } } - private final ArrayList> structPublishers; public static record StructPublisherEntry ( @@ -75,6 +78,49 @@ public void addStructArrayPublisher(String name, Struct struct, Supplier< supplier)); } + public static void makeSendableTalonFX(String name, TalonFX motor, SendableBuilder builder) { + builder.addDoubleProperty( + name + "/Rotations", + () -> motor + .getPosition() + .getValueAsDouble(), + null); + builder.addDoubleProperty( + name + "/Velocity RPS", + () -> motor + .getVelocity() + .getValueAsDouble(), + null); + builder.addDoubleProperty( + name + "/Volts", + () -> motor + .getMotorVoltage() + .getValue() + .in(Units.Volts), + null); + builder.addDoubleProperty( + name + "/Stator Current", + () -> motor + .getStatorCurrent() + .getValue() + .in(Units.Amps), + null); + builder.addDoubleProperty( + name + "/Supply Current", + () -> motor + .getSupplyCurrent() + .getValue() + .in(Units.Amps), + null); + builder.addDoubleProperty( + name + "/Temperature Celsius", + () -> motor + .getDeviceTemp() + .getValue() + .in(Units.Celsius), + null); + } + public void addSendable(Sendable sendable) { sendables.add(sendable); SmartDashboard.putData(sendable); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 17c9703..71e1dcd 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,10 +1,9 @@ package frc.robot.subsystems.drive; -import java.util.Set; +import static frc.robot.subsystems.drive.DriveConstants.*; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; @@ -16,14 +15,13 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Time; import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; -import frc.robot.lib.localization.FieldLayout; +import frc.robot.lib.field.FieldLayout; import frc.robot.lib.util.Util; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.ctre.CtreDriveConstants; @@ -41,16 +39,13 @@ public static Drive getInstance() { } private SwerveDriveState lastReadState; - public static final SwerveRequest.FieldCentric teleopRequest = - new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + public static final SwerveRequest.FieldCentric teleopRequest = new SwerveRequest.FieldCentric(); public SwerveRequest driveRequest = teleopRequest; private final CtreDrive drivetrain = CtreDriveConstants.createDrivetrain(); - private final CtreDriveTelemetry telemetry = new CtreDriveTelemetry(Constants.Drive.MAX_SPEED); - // Citrus what are you doing - private Time lastPoseResetTime = BaseUnits.TimeUnit.of(0.0); - - private final Field2d elasticPose = new Field2d(); + private final CtreDriveTelemetry telemetry = new CtreDriveTelemetry(MAX_SPEED); + @SuppressWarnings("unused") + private Time lastPoseResetTime = BaseUnits.TimeUnit.of(0.0); // Citrus what are you doing private Drive() { lastReadState = drivetrain.getState(); @@ -58,15 +53,12 @@ private Drive() { return driveRequest; })); - if (!Robot.isReal()) { - // drivetrain.resetPose((new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90.0)))); - } - drivetrain.getOdometryThread().setThreadPriority(31); TelemetryManager.getInstance().addStructPublisher("Mechanisms/Drive", Pose3d.struct, () -> new Pose3d(getPose())); TelemetryManager.getInstance().addSendable(this); } + /** @return the ctre generated drivetrain */ public CtreDrive getCtreDrive() { return drivetrain; } @@ -79,8 +71,7 @@ public void periodic() { public void outputTelemetry() { telemetry.telemeterize(lastReadState); - elasticPose.setRobotPose(getPose()); - SmartDashboard.putData("Elastic Field 2D", elasticPose); + FieldLayout.field.setRobotPose(getPose()); } /** @@ -97,6 +88,9 @@ public Pose2d getPose() { return lastReadState.Pose; } + /** + * @return the chassis speeds, field relative + */ public ChassisSpeeds getFieldSpeeds() { return ChassisSpeeds.fromRobotRelativeSpeeds(lastReadState.Speeds, lastReadState.Pose.getRotation()); } @@ -109,10 +103,14 @@ public void setSwerveRequest(SwerveRequest request) { driveRequest = request; } + /** + * @return the current swerve request + */ public SwerveRequest getSwerveRequest() { return driveRequest; } + /** Open loop during teleop mode */ public Command teleopCommand() { return runOnce(() -> { teleopRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0); @@ -130,63 +128,74 @@ public Command teleopCommand() { SmartDashboard.putNumber("Sticks/vW", rotDesiredRaw); teleopRequest - .withVelocityX(xFancy * Constants.Drive.MAX_SPEED) - .withVelocityY(yFancy * Constants.Drive.MAX_SPEED) - .withRotationalRate(rotFancy * Constants.Drive.MAX_ROTATION_SPEED); + .withVelocityX(xFancy * MAX_SPEED) + .withVelocityY(yFancy * MAX_SPEED) + .withRotationalRate(rotFancy * MAX_ROTATION_SPEED); }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Teleop"); } + /** + * Auto aligns to the nearest reef face + * @param left chooses the left or right face + */ public Command autoAlign(boolean left) { - return Commands.defer(() -> { + return defer(() -> { Pose2d pose; if (left) { pose = getPose().nearest(FieldLayout.ALIGN_POSES_LEFT); } else { pose = getPose().nearest(FieldLayout.ALIGN_POSES_RIGHT); } - return new PIDToPoseCommand(pose).withName("Auto Align"); - }, Set.of(this)); + return new PIDToPoseCommand(pose).withName("Auto Align to " + pose); + }); } + /** Adds a vision update */ public void addVisionUpdate(Pose2d pose, Time timestamp) { getCtreDrive().addVisionMeasurement(pose, timestamp.in(Units.Seconds)); } + /** Adds a vision update with standard deviations */ public void addVisionUpdate(Pose2d pose, Time timestamp, Matrix stdDevs) { getCtreDrive().addVisionMeasurement(pose, timestamp.in(Units.Seconds), stdDevs); } + /** Resets pose estimator to a pose */ public void resetPose(Pose2d pose) { getCtreDrive().resetPose(pose); lastPoseResetTime = - Units.Seconds.of(Utils.getCurrentTimeSeconds()).plus(Constants.Drive.POSE_RESET_PREVENTION_TIME); + Units.Seconds.of(Utils.getCurrentTimeSeconds()).plus(POSE_RESET_PREVENTION_TIME); } + /** A command that resets the pose */ public Command resetPoseCommand(Pose2d pose) { return Commands.runOnce(() -> resetPose(pose)); } + /** Whether the pitch is stable */ public boolean isPitchStable() { return drivetrain.getPigeon2().getAngularVelocityYDevice().getValue().abs(Units.DegreesPerSecond) - < Constants.Drive.MAX_VELOCITY_STABLE + < MAX_VELOCITY_STABLE && drivetrain.getPigeon2().getPitch().getValue().abs(BaseUnits.AngleUnit) - < Constants.Drive.MAX_PITCH_STABLE; + < MAX_PITCH_STABLE; } + /** Whether the roll is stable */ public boolean isRollStable() { return drivetrain.getPigeon2().getAngularVelocityXDevice().getValue().abs(Units.DegreesPerSecond) - < Constants.Drive.MAX_VELOCITY_STABLE + < MAX_VELOCITY_STABLE && drivetrain.getPigeon2().getRoll().getValue().abs(BaseUnits.AngleUnit) - < Constants.Drive.MAX_PITCH_STABLE; + < MAX_PITCH_STABLE; } + /** Whether the robot is stable */ public boolean isStable() { ChassisSpeeds speeds = getState().Speeds; return isPitchStable() && isRollStable() && Units.MetersPerSecond.of(Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond)) - .lte(Constants.Drive.kScoringTranslationMaxSpeed) - && Units.RadiansPerSecond.of(speeds.omegaRadiansPerSecond).lte(Constants.Drive.kScoringRotationMaxSpeed); + .lte(MAX_SPEED_SCORING_TRANSLATION) + && Units.RadiansPerSecond.of(speeds.omegaRadiansPerSecond).lte(MAX_ROTATION_SPEED_SCORING); } @Override @@ -224,85 +233,12 @@ public void initSendable(SendableBuilder builder) { addModuleToBuilder(builder, 3); } + /** Telemeterizes a module */ private void addModuleToBuilder(SendableBuilder builder, int module) { - builder.addDoubleProperty( - "ModuleStates/" + module + "/Drive/Volts", - () -> drivetrain - .getModules()[module] - .getDriveMotor() - .getMotorVoltage() - .getValue() - .in(Units.Volts), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Rotation/Volts", - () -> drivetrain - .getModules()[module] - .getSteerMotor() - .getMotorVoltage() - .getValue() - .in(Units.Volts), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Drive/Stator Current", - () -> drivetrain - .getModules()[module] - .getDriveMotor() - .getStatorCurrent() - .getValue() - .in(Units.Amps), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Drive/Temperature Celsius", - () -> drivetrain - .getModules()[module] - .getDriveMotor() - .getDeviceTemp() - .getValue() - .in(Units.Celsius), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Rotation/Stator Current", - () -> drivetrain - .getModules()[module] - .getSteerMotor() - .getStatorCurrent() - .getValue() - .in(Units.Amps), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Drive/Supply Current", - () -> drivetrain - .getModules()[module] - .getDriveMotor() - .getSupplyCurrent() - .getValue() - .in(Units.Amps), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Rotation/Supply Current", - () -> drivetrain - .getModules()[module] - .getSteerMotor() - .getSupplyCurrent() - .getValue() - .in(Units.Amps), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Rotation/Temperature Celsius", - () -> drivetrain - .getModules()[module] - .getSteerMotor() - .getDeviceTemp() - .getValue() - .in(Units.Celsius), - null); + TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Drive", + drivetrain.getModules()[module].getDriveMotor(), builder); + + TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Angle", + drivetrain.getModules()[module].getSteerMotor(), builder); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java new file mode 100644 index 0000000..1f32ab6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -0,0 +1,55 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Time; +import frc.robot.lib.control.ControlConstants.PIDFConstants; +import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; +import frc.robot.subsystems.drive.ctre.CtreDriveConstants; + +public final class DriveConstants { + public static final double EPSILON_TRANSLATION = 0.02; // 2 cm + public static final double EPSILON_ROTATION = Units.Degrees.of(2.0).in(Units.Radians); // 2.0 deg in rads + + // Maximums + public static final double MAX_SPEED = Units.MetersPerSecond.of(2.5).in(Units.MetersPerSecond); + public static final double MAX_ACCEL = Units.MetersPerSecondPerSecond.of(5.0).in(Units.MetersPerSecondPerSecond); + public static final double MAX_ROTATION_SPEED = + Units.RotationsPerSecond.of(3.0).in(Units.RadiansPerSecond); + public static final double MAX_ROTATION_ACCEL = + Units.RotationsPerSecondPerSecond.of(5.0).in(Units.RadiansPerSecondPerSecond); + + // Swerve dimensions + public static final double TRACK_WIDTH = Units.Inches.of(24).in(Units.Meters); + public static final double WHEEL_BASE = Units.Inches.of(24).in(Units.Meters); + public static final double WHEEL_DIAMETER = 2 * CtreDriveConstants.kWheelRadius.in(Units.Meters); + public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI; + + // Stability constants + public static final double MAX_VELOCITY_STABLE = 10; // degrees per second + public static final double MAX_PITCH_STABLE = 5; // degrees + public static final LinearVelocity MAX_SPEED_SCORING_TRANSLATION = + Units.Centimeters.of(15.0).per(Units.Seconds); + public static final AngularVelocity MAX_ROTATION_SPEED_SCORING = + Units.Degrees.of(7.0).per(Units.Seconds); // oh god + public static final Time POSE_RESET_PREVENTION_TIME = Units.Seconds.of(0.15); + + // Trajectory and snap constants + public static final PIDFConstants TRANSLATION_CONSTANTS = + new PIDFConstants(4.5, 0.0, 0.1, 1.0); + public static final ProfiledPIDFConstants PROFILED_TRANSLATION_CONSTANTS = + new ProfiledPIDFConstants(1.0, 0.0, 0.1, 1.0, + new TrapezoidProfile.Constraints( + MAX_SPEED, + MAX_ACCEL)); + public static final PIDFConstants ROTATION_CONSTANTS = + new PIDFConstants(4.0, 0.0, 0.1, 1.0); + public static final ProfiledPIDFConstants PROFILED_ROTATION_CONSTANTS = + new ProfiledPIDFConstants(4.0, 0.0, 0.0, 1.0, + new TrapezoidProfile.Constraints( + MAX_ROTATION_SPEED, + MAX_ROTATION_ACCEL)); + public static final double ACCELERATION_CONSTANT = 0.1; +} diff --git a/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java index 5688318..57d57e1 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java @@ -16,7 +16,7 @@ public class ExtendedTrajectoryCommand extends TrajectoryCommand { public ExtendedTrajectoryCommand(Drive drive, RedTrajectory trajectory, Pair... triggers) { super(trajectory); addRequirements(drive); - setName("Extended Trajectory Command"); + setName(trajectory.name + ": Extended Trajectory Command"); this.triggers = new ArrayList<>(List.of(triggers)); for (Pair trigger : triggers) { new Trigger(() -> trajectory.progress > trigger.getFirst()).onTrue(trigger.getSecond()); diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index 4dca0f1..9b8789c 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -1,48 +1,56 @@ package frc.robot.subsystems.drive.commands; +import static frc.robot.subsystems.drive.DriveConstants.*; + import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; +import frc.robot.lib.control.ControlConstants.PIDFConstants; +import frc.robot.lib.control.PIDVController; import frc.robot.lib.util.Util; -import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.subsystems.drive.Drive; +/** + * A command that moves the drivetrain to a pose. + */ public class PIDToPoseCommand extends Command { public final Drive drive; private final SwerveRequest.ApplyFieldSpeeds request = new SwerveRequest.ApplyFieldSpeeds(); - private final ProfiledPIDVController translationController; - private final ProfiledPIDVController thetaController; + private final PIDVController translationController; + private final PIDVController thetaController; private final Pose2d target; private Pose2d currentPose; private ChassisSpeeds currentSpeeds; + private ChassisSpeeds targetSpeeds = new ChassisSpeeds(); + public PIDToPoseCommand(Pose2d target) { this( Drive.getInstance(), target, - Constants.Auto.PROFILED_TRANSLATION_CONSTANTS, - Constants.Auto.ROTATION_CONSTANTS); + TRANSLATION_CONSTANTS, + ROTATION_CONSTANTS); } - public PIDToPoseCommand(Drive drive, Pose2d target, ProfiledPIDFConstants translationConstants, ProfiledPIDFConstants rotationConstants) { + public PIDToPoseCommand(Drive drive, Pose2d target, PIDFConstants translationConstants, PIDFConstants rotationConstants) { this.drive = drive; this.target = target; - translationController = new ProfiledPIDVController(translationConstants); - thetaController = new ProfiledPIDVController(rotationConstants); + translationController = new PIDVController(translationConstants); + thetaController = new PIDVController(rotationConstants); thetaController.enableContinuousInput(-Math.PI, Math.PI); addRequirements(drive); - setName("PID to " + target.toString()); + setName("(" + target.getX() + ", " + target.getY() + ", " + + target.getRotation().getDegrees() + " deg)" + " :PID to pose"); } @Override @@ -55,7 +63,8 @@ public void execute() { setRobotState( drive.getPose(), drive.getFieldSpeeds()); // updates the request - request.withSpeeds(calculateSpeeds()); + targetSpeeds = calculateSpeeds(); + request.withSpeeds(targetSpeeds); } public void setRobotState(Pose2d pose, ChassisSpeeds speeds) { @@ -65,46 +74,81 @@ public void setRobotState(Pose2d pose, ChassisSpeeds speeds) { public ChassisSpeeds calculateSpeeds() { if (target == null || currentPose == null || currentSpeeds == null) { + // Safety return new ChassisSpeeds(); } + // Calculate difference var delta = target.getTranslation().minus(currentPose.getTranslation()); - translationController.setTarget(delta.getNorm()); - - translationController.setInput( - 0.0, - Util.chassisSpeedsMagnitude( - currentSpeeds)); + // Magnitude target + double vMagnitude = MathUtil.clamp( + translationController.setTarget(delta.getNorm()) + .setMeasurement( + 0.0, // We are exactly where we are + Util.chassisSpeedsMagnitude( + currentSpeeds)) // How fast we are going + .getOutput(), + -MAX_SPEED, MAX_SPEED); - double vMagnitude = translationController.getOutput(); - SmartDashboard.putNumber("Debug/PIDToPoseCommand/vmag", vMagnitude); + // The angle we are at relative to the target var deltaRotation = delta.getAngle(); - thetaController.setTarget(target.getRotation().getRadians()); - thetaController.setInput( - currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond); + double rotation = MathUtil.clamp( + thetaController.setTarget(target.getRotation().getRadians()) // Theta target + .setMeasurement( + currentPose.getRotation().getRadians(), + currentSpeeds.omegaRadiansPerSecond) // We are where we are and we are as fast as how fast we are going + .getOutput(), + -MAX_ROTATION_SPEED, MAX_ROTATION_SPEED); - double rotation = thetaController.getOutput(); + SmartDashboard.putNumber("Debug/PIDToPose/vx", vMagnitude * deltaRotation.getCos()); + SmartDashboard.putNumber("Debug/PIDToPose/vy", vMagnitude * deltaRotation.getSin()); + SmartDashboard.putNumber("Debug/PIDToPose/vrotation", rotation); return new ChassisSpeeds( - vMagnitude * deltaRotation.getCos(), + vMagnitude * deltaRotation.getCos(), // convert from polar to rectangular vMagnitude * deltaRotation.getSin(), rotation); } @Override public boolean isFinished() { - return translationController.error < 0.03 - && MathUtil.isNear(thetaController.error / Math.PI * 180, 0, 3.0); + return translationController.getError() < EPSILON_TRANSLATION + && MathUtil.isNear(thetaController.getError(), 0, EPSILON_ROTATION); // Within tolerance } @Override public void end(boolean interrupted) { + // Swaps out the drive request to a default robot oriented request drive.setSwerveRequest(new SwerveRequest.ApplyRobotSpeeds()); } + public static double estimateTimeToPose( + Pose2d currentPose, + ChassisSpeeds currentSpeeds, + Pose2d targetPose + ) { + double translationError = + currentPose.getTranslation().getDistance(targetPose.getTranslation()); + + double currentV = Util.chassisSpeedsMagnitude(currentSpeeds); + + return Util.trapezoidProfileTimeToTarget( + translationError, + currentV, + 0, + MAX_SPEED, + MAX_ACCEL); + } + + /** Helper for retrieving the target of this PID to Pose command */ public Pose2d getTarget() { return target; } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index e4acfa2..af5fe0d 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.drive.commands; +import static frc.robot.subsystems.drive.DriveConstants.*; + import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.MathUtil; @@ -8,7 +10,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; import frc.robot.lib.control.ControlConstants.PIDFConstants; import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; import frc.robot.lib.control.PIDVController; @@ -42,9 +43,9 @@ public TrajectoryCommand(RedTrajectory trajectory) { this( Drive.getInstance(), trajectory, - Constants.Auto.TRANSLATION_CONSTANTS, - Constants.Auto.ROTATION_CONSTANTS, - Constants.Auto.ACCELERATION_CONSTANT); + TRANSLATION_CONSTANTS, + PROFILED_ROTATION_CONSTANTS, + ACCELERATION_CONSTANT); } /** @@ -68,12 +69,12 @@ public TrajectoryCommand(Drive drive, RedTrajectory trajectory, PIDFConstants tr .addStructPublisher("Debug/TrajectoryCommand", Pose3d.struct, () -> new Pose3d( targetState.pose)); - setName("Trajectory " + trajectory.name); + setName(trajectory.name + " :Trajectory"); } @Override public void initialize() { - timer.start(); + timer.start(); // actually starts the timer drive.setSwerveRequest(request); } @@ -92,69 +93,77 @@ public void setRobotState(Pose2d pose, ChassisSpeeds speeds) { public ChassisSpeeds calculateSpeeds() { if (trajectory == null || currentPose == null || currentSpeeds == null || trajectory.isDone()) { - return new ChassisSpeeds(); + return new ChassisSpeeds(); // Safety } + // Advances along the trajectory targetState = trajectory.advanceTo(timer.get()); + // gets the feedforwards for translation double vxFF = targetState.speeds.vxMetersPerSecond; double vyFF = targetState.speeds.vyMetersPerSecond; double xAccelFF = MathUtil.applyDeadband( targetState.accels.ax, - Constants.Drive.MAX_ACCEL * 0.5); + MAX_ACCEL * 0.5); double yAccelFF = MathUtil.applyDeadband( targetState.accels.ay, - Constants.Drive.MAX_ACCEL * 0.5); - + MAX_ACCEL * 0.5); double angularAccel = MathUtil.applyDeadband( targetState.accels.alpha, - Constants.Drive.MAX_ROTATION_ACCEL * 0.5); + MAX_ROTATION_ACCEL * 0.5); // what did i even want to accomplish from this + + // acceleration feedforwards xAccelFF += -angularAccel * targetState.pose.getRotation().getSin(); yAccelFF += angularAccel * targetState.pose.getRotation().getCos(); - xController.setTarget(targetState.pose.getX()); - yController.setTarget(targetState.pose.getY()); - - xController.setFeedforward(vxFF); - yController.setFeedforward(vyFF); + double vx = xController.setTarget(targetState.pose.getX())// Target setting + .setFeedforward(vxFF) // Feedforward setting + .setMeasurement(currentPose.getX(), currentSpeeds.vxMetersPerSecond) // Measurement setting + .getOutput(); // Output getting - xController.setInput(currentPose.getX(), currentSpeeds.vxMetersPerSecond); - yController.setInput(currentPose.getY(), currentSpeeds.vyMetersPerSecond); - double vx = xController.getOutput(); - double vy = yController.getOutput(); + // same thing but for vy and rotation instead + double vy = yController.setTarget(targetState.pose.getY()) + .setFeedforward(vyFF) + .setMeasurement(currentPose.getY(), currentSpeeds.vyMetersPerSecond) + .getOutput(); - thetaController.setTarget(targetState.pose.getRotation().getRadians()); - thetaController.setFeedforward(targetState.speeds.omegaRadiansPerSecond); - thetaController.setInput( - currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond); - - double rotation = thetaController.getOutput(); + double rotation = thetaController.setTarget(targetState.pose.getRotation().getRadians()) + .setFeedforward(targetState.speeds.omegaRadiansPerSecond) + .setMeasurement( + currentPose.getRotation().getRadians(), + currentSpeeds.omegaRadiansPerSecond) + .getOutput(); return new ChassisSpeeds( vx + xAccelFF * accelConstant, vy + yAccelFF * accelConstant, - rotation); + rotation); // Finally } @Override public boolean isFinished() { if (trajectory == null) { - return true; + return true; // safety } + if (trajectory.isDone()) { - System.out.println("Done with trajectory, error: " + Math.hypot(xController.error, yController.error)); - return true; + System.out.println("Done with trajectory, error: " + + Math.hypot(xController.getError(), yController.getError())); + return true; // We are done guys } + return false; } @Override public void end(boolean interrupted) { + // swap out request at end drive.setSwerveRequest(new SwerveRequest.FieldCentric()); } + /** Helper for getting trajectory from this object */ public RedTrajectory getTrajectory() { return trajectory; } diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 986f8ef..57a3e5b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -23,7 +23,7 @@ // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class CtreDriveConstants { - // Both sets of gains need to be tuned to your individual robot. + // TODO: Both sets of gains need to be tuned to your individual robot. // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput @@ -84,11 +84,11 @@ public class CtreDriveConstants { public static final CANBus kCANBus = new CANBus("CV", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot + // TODO: This needs to be tuned to your individual robot public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot + // TODO: This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.5714285714285716; public static final double kDriveGearRatio = 6.122448979591837; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java index 288408e..62c2617 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.vision; -import frc.robot.lib.localization.FieldLayout; import frc.robot.subsystems.drive.Drive; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; @@ -9,11 +8,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.Limelight.VisionDeviceConstants; +import frc.robot.lib.field.FieldLayout; import java.util.ArrayList; import java.util.Arrays; import java.util.List; - import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonUtils; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java index 5164c17..f59a5bb 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java @@ -19,6 +19,7 @@ public static VisionDeviceManager getInstance() { } // private VisionDevice leftCamera; + @SuppressWarnings("unused") private VisionDevice rightCamera; private VisionDevice frontrCamera; private VisionDevice frontlCamera; From 8b62b6d8e0765939bc9c58b472bbf37e57951197 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 6 Dec 2025 21:13:09 -0800 Subject: [PATCH 15/36] Trajectory update soon --- simgui-ds.json | 8 ++++---- .../drive/commands/TrajectoryCommand.java | 15 +++++++-------- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 23c0a81..0bea79e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -7,14 +7,14 @@ "keyboardJoysticks": [ { "axisConfig": [ - { - "decKey": 65, - "incKey": 68 - }, { "decKey": 83, "incKey": 87 }, + { + "decKey": 65, + "incKey": 68 + }, { "decayRate": 0.0, "keyRate": 0.009999999776482582 diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index af5fe0d..82cda63 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -11,15 +11,14 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.lib.control.ControlConstants.PIDFConstants; -import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; import frc.robot.lib.control.PIDVController; -import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.lib.trajectory.RedTrajectory; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.Drive; /** * Command that follows a trajectory + * TODO: fix the profiled pid controller */ public class TrajectoryCommand extends Command { public final Drive drive; @@ -29,7 +28,7 @@ public class TrajectoryCommand extends Command { private final PIDVController xController; private final PIDVController yController; - private final ProfiledPIDVController thetaController; + private final PIDVController thetaController; private double accelConstant; private final RedTrajectory trajectory; @@ -44,22 +43,22 @@ public TrajectoryCommand(RedTrajectory trajectory) { Drive.getInstance(), trajectory, TRANSLATION_CONSTANTS, - PROFILED_ROTATION_CONSTANTS, + ROTATION_CONSTANTS, ACCELERATION_CONSTANT); } /** - * A drive controller that works with 2 {@link PIDVController}s for translation and one {@link ProfiledPIDVController} for rotation. + * A drive controller that works with 2 {@link PIDVController}s for translation and one {@link PIDVController} for rotation. * @param translationConstants The {@link PIDFConstants} for the translation of the robot. - * @param rotationConstants The {@link ProfiledPIDFConstants} for the rotation of the robot. + * @param rotationConstants The {@link PIDFConstants} for the rotation of the robot. * @param accelConstant The acceleration feedforwards (useful for traversing sharp turns on a trajectory). */ - public TrajectoryCommand(Drive drive, RedTrajectory trajectory, PIDFConstants translationConstants, ProfiledPIDFConstants rotationConstants, double accelConstant) { + public TrajectoryCommand(Drive drive, RedTrajectory trajectory, PIDFConstants translationConstants, PIDFConstants rotationConstants, double accelConstant) { this.drive = drive; this.trajectory = trajectory; xController = new PIDVController(translationConstants); yController = new PIDVController(translationConstants); - thetaController = new ProfiledPIDVController(rotationConstants); + thetaController = new PIDVController(rotationConstants); thetaController.enableContinuousInput(-Math.PI, Math.PI); this.accelConstant = accelConstant; From 7364323c7933ecf7f9e709224e8fd08b7622d86e Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 6 Dec 2025 21:25:04 -0800 Subject: [PATCH 16/36] i think later though --- .../java/frc/robot/subsystems/drive/DriveConstants.java | 8 ++++---- .../subsystems/drive/commands/TrajectoryCommand.java | 1 - 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 1f32ab6..c5d5321 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -14,12 +14,12 @@ public final class DriveConstants { public static final double EPSILON_ROTATION = Units.Degrees.of(2.0).in(Units.Radians); // 2.0 deg in rads // Maximums - public static final double MAX_SPEED = Units.MetersPerSecond.of(2.5).in(Units.MetersPerSecond); - public static final double MAX_ACCEL = Units.MetersPerSecondPerSecond.of(5.0).in(Units.MetersPerSecondPerSecond); + public static final double MAX_SPEED = Units.MetersPerSecond.of(3.5).in(Units.MetersPerSecond); + public static final double MAX_ACCEL = Units.MetersPerSecondPerSecond.of(6.0).in(Units.MetersPerSecondPerSecond); public static final double MAX_ROTATION_SPEED = - Units.RotationsPerSecond.of(3.0).in(Units.RadiansPerSecond); + Units.RotationsPerSecond.of(2.0).in(Units.RadiansPerSecond); public static final double MAX_ROTATION_ACCEL = - Units.RotationsPerSecondPerSecond.of(5.0).in(Units.RadiansPerSecondPerSecond); + Units.RotationsPerSecondPerSecond.of(4.0).in(Units.RadiansPerSecondPerSecond); // Swerve dimensions public static final double TRACK_WIDTH = Units.Inches.of(24).in(Units.Meters); diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index 82cda63..e1b00f8 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -121,7 +121,6 @@ public ChassisSpeeds calculateSpeeds() { .setMeasurement(currentPose.getX(), currentSpeeds.vxMetersPerSecond) // Measurement setting .getOutput(); // Output getting - // same thing but for vy and rotation instead double vy = yController.setTarget(targetState.pose.getY()) .setFeedforward(vyFF) From dd0973a8b5521f7f78890ed0c8c8a4f8069f8c79 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 6 Dec 2025 21:33:15 -0800 Subject: [PATCH 17/36] minor change --- src/main/java/frc/robot/subsystems/drive/Drive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 71e1dcd..0526668 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -146,8 +146,8 @@ public Command autoAlign(boolean left) { } else { pose = getPose().nearest(FieldLayout.ALIGN_POSES_RIGHT); } - return new PIDToPoseCommand(pose).withName("Auto Align to " + pose); - }); + return new PIDToPoseCommand(pose); + }).withName("Auto Align"); } /** Adds a vision update */ From 7cb20890b00bd302f11d55e7645863051962a922 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Sun, 14 Dec 2025 12:17:03 -0800 Subject: [PATCH 18/36] FEAT: SysID Rotation routine and logs --- src/main/java/frc/robot/ControlsMapping.java | 14 ++++----- .../subsystems/drive/ctre/CtreDrive.java | 30 ++++++++++++++----- 2 files changed, 30 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 67da8bd..1139456 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -17,14 +17,14 @@ public class ControlsMapping { public static void mapTeleopCommand() { Drive.getInstance().setDefaultCommand((Drive.getInstance().teleopCommand())); - // // run sysID functions - // Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); + // // // run sysID functions + // // Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); - controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); - controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); - controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); - controller.b().whileTrue(new PIDToPoseCommand( - new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120.0)))); + // controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); + // controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); + // controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); + // controller.b().whileTrue(new PIDToPoseCommand( + // new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120.0)))); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java index 183b5ff..98b5004 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java @@ -111,17 +111,33 @@ public static enum SysIdRoutineType { Volts.of(Math.PI), null, // Use default timeout (10 s) // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) + //state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) + null ), new SysIdRoutine.Mechanism( output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); + System.out.println("sysid/rotation routine called."); + m_lastAppliedVolts = output.in(Volts); + setControl(m_rotationCharacterization.withRotationalRate(m_lastAppliedVolts)); }, - null, - this + log -> { + var s = getStateCopy(); + log.motor("yaw") + .voltage(Volts.of(m_lastAppliedVolts)) + .angularPosition(Radians.of(s.Pose.getRotation().getRadians())) // get rotation position + .angularVelocity(RadiansPerSecond.of(getKinematics() + .toChassisSpeeds(s.ModuleStates).omegaRadiansPerSecond)); + }, + this + + // output -> { + // /* output is actually radians per second, but SysId only supports "volts" */ + // setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); + // /* also log the requested output for SysId */ + // SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); + // }, + // null, + // this ) ); From 4ab705e297bce1511c7658e436f51007025e75d0 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Mon, 15 Dec 2025 00:50:11 -0800 Subject: [PATCH 19/36] feat: enable VisualVM to debug RAM issues --- build.gradle | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/build.gradle b/build.gradle index 3a947b7..1d8df5e 100644 --- a/build.gradle +++ b/build.gradle @@ -27,6 +27,13 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + // Enable VisualVM connection + // jvmArgs.add("-Dcom.sun.management.jmxremote=true") + // jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + // jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + // jvmArgs.add("-Djava.rmi.server.hostname=10.14.58.2") // Replace TE.AM with team number } // Static files artifact From 35fff997f1da68d231994e5a3387a1d70f4ac9fb Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Mon, 15 Dec 2025 00:50:55 -0800 Subject: [PATCH 20/36] feat: add steer motor offsets of the 2nd bot --- .../subsystems/drive/ctre/CtreDriveConstants.java | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 2e9d6f6..249c59d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -22,7 +22,10 @@ // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html -public class CtreDriveConstants { +public class CtreDriveConstants { + // which set of robot constants we are deploy + private static final boolean kIs2ndBot = false; //true for the 2nd bot we built for 2026; false for 2025 bot + // mechanical and geometric parameters of drive train public static final double kDriveGearRatio = 6.122448979591837; public static final double kSteerGearRatio = 21.428571428571427; @@ -154,7 +157,7 @@ public class CtreDriveConstants { private static final int kFrontLeftDriveMotorId = 8; private static final int kFrontLeftSteerMotorId = 10; private static final int kFrontLeftEncoderId = 7; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.355224609375); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(kIs2ndBot? -0.462890625: 0.355224609375); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -165,7 +168,7 @@ public class CtreDriveConstants { private static final int kFrontRightDriveMotorId = 9; private static final int kFrontRightSteerMotorId = 11; private static final int kFrontRightEncoderId = 6; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4296875); + private static final Angle kFrontRightEncoderOffset = Rotations.of(kIs2ndBot? 0.025390625: -0.4296875); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -176,7 +179,7 @@ public class CtreDriveConstants { private static final int kBackLeftDriveMotorId = 3; private static final int kBackLeftSteerMotorId = 5; private static final int kBackLeftEncoderId = 14; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.326416015625); + private static final Angle kBackLeftEncoderOffset = Rotations.of(kIs2ndBot? 0.1435546875: 0.326416015625); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -187,7 +190,7 @@ public class CtreDriveConstants { private static final int kBackRightDriveMotorId = 4; private static final int kBackRightSteerMotorId = 2; private static final int kBackRightEncoderId = 1; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.0869140625); + private static final Angle kBackRightEncoderOffset = Rotations.of(kIs2ndBot? 0.183837890625: 0.0869140625); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; From 76bd01fe8eefcab5046298d468454377c25ad105 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Mon, 15 Dec 2025 00:53:15 -0800 Subject: [PATCH 21/36] feat: add radialDeadband to fix yaw impulse when driver releases stick --- src/main/java/frc/robot/lib/util/Util.java | 13 +++++++++++++ src/main/java/frc/robot/subsystems/drive/Drive.java | 8 ++++++-- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/lib/util/Util.java b/src/main/java/frc/robot/lib/util/Util.java index 7f802b4..36d0ea3 100644 --- a/src/main/java/frc/robot/lib/util/Util.java +++ b/src/main/java/frc/robot/lib/util/Util.java @@ -130,6 +130,19 @@ public static double applyJoystickDeadband(double stickValue, double stickDeadba return Math.signum(deadbandedValue) * ((Math.abs(deadbandedValue) - stickDeadband) / (1.0 - stickDeadband)); //joystick max is always 1.0 } + // dc 12.15.25, apply radial deadband instead of axis-based ones + public static double[] applyRadialDeadband(double x, double y, double deadband) { + double mag = Math.hypot(x, y); + if (mag <= deadband) return new double[] {0.0, 0.0}; + + // Rescale so output reaches 1.0 when mag==1.0 + double scaledMag = (mag - deadband) / (1.0 - deadband); + double ux = x / mag; + double uy = y / mag; + return new double[] {ux * scaledMag, uy * scaledMag}; + } + + public static Rotation2d robotToFieldRelative(Rotation2d rot, boolean is_red_alliance) { if (is_red_alliance) { return rot.rotateBy(Rotation2d.fromDegrees(180.0)); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 17c9703..f49d3a7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -121,8 +121,12 @@ public Command teleopCommand() { double xDesiredRaw = -Robot.controller.getLeftY(); double yDesiredRaw = -Robot.controller.getLeftX(); double rotDesiredRaw = -Robot.controller.getRightX(); - double xFancy = Util.applyJoystickDeadband(xDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); - double yFancy = Util.applyJoystickDeadband(yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + + double[] xy = Util.applyRadialDeadband(xDesiredRaw, yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + double xFancy = xy[0]; + double yFancy = xy[1]; + // double xFancy = Util.applyJoystickDeadband(xDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + // double yFancy = Util.applyJoystickDeadband(yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); double rotFancy = Util.applyJoystickDeadband(rotDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); SmartDashboard.putNumber("Sticks/vX", xDesiredRaw); From 8e71e1052292cf672dd706f5a518b058364658fb Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Mon, 15 Dec 2025 01:05:49 -0800 Subject: [PATCH 22/36] fix kCoupleRatio value for steer/drive coupling --- .../frc/robot/subsystems/drive/ctre/CtreDriveConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 249c59d..551bfa3 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -110,7 +110,7 @@ public class CtreDriveConstants { // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = kDriveGearRatio /3.; //(drive gear ratio)/(bevel gear ratio) 3.5714285714285716; + private static final double kCoupleRatio = kDriveGearRatio /(150./7.); //(drive gear ratio)/(steering gear ratio) 3.5714285714285716; private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; From 657d15826c11a717be086c1c0b31fd6e7cfcd512 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Thu, 18 Dec 2025 21:50:53 -0800 Subject: [PATCH 23/36] Pid to pose borke again --- src/main/java/frc/robot/ControlsMapping.java | 4 +- .../lib/field/AprilTagLayoutGenerated.java | 45 +++ .../lib/field/AprilTagLayoutGenerator.html | 215 ++++++++++++++ .../java/frc/robot/lib/field/FieldLayout.java | 35 ++- .../frc/robot/subsystems/drive/Drive.java | 278 ++++++++++-------- .../drive/commands/AutopilotCommand.java | 80 +++++ .../drive/commands/PIDToPoseCommand.java | 16 +- 7 files changed, 540 insertions(+), 133 deletions(-) create mode 100644 src/main/java/frc/robot/lib/field/AprilTagLayoutGenerated.java create mode 100644 src/main/java/frc/robot/lib/field/AprilTagLayoutGenerator.html create mode 100644 src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index b59c59f..caa4807 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -10,8 +10,8 @@ public class ControlsMapping { public static void mapTeleopCommand() { controller.back().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); - controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); - controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); + controller.leftBumper().whileTrue(Drive.getInstance().autopilotAlign(true)); + controller.rightBumper().whileTrue(Drive.getInstance().autopilotAlign(false)); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/lib/field/AprilTagLayoutGenerated.java b/src/main/java/frc/robot/lib/field/AprilTagLayoutGenerated.java new file mode 100644 index 0000000..f4f0834 --- /dev/null +++ b/src/main/java/frc/robot/lib/field/AprilTagLayoutGenerated.java @@ -0,0 +1,45 @@ +package frc.robot.lib.field; + +import java.util.ArrayList; +import java.util.List; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; + +public class AprilTagLayoutGenerated { + public static final List APRILTAG_FIELD_POSES = new ArrayList<>(); + static { + APRILTAG_FIELD_POSES.add(new AprilTag(1, new Pose3d(new Translation3d(16.697198000000D, 0.655320000000D, 1.485900000000D), new Rotation3d(new Quaternion(0.453990499740D, 0.000000000000D, 0.000000000000D, 0.891006524188D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(2, new Pose3d(new Translation3d(16.697198000000D, 7.396480000000D, 1.485900000000D), new Rotation3d(new Quaternion(-0.453990499740D, 0.000000000000D, 0.000000000000D, 0.891006524188D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(3, new Pose3d(new Translation3d(11.560810000000D, 8.055610000000D, 1.301750000000D), new Rotation3d(new Quaternion(-0.707106781187D, 0.000000000000D, 0.000000000000D, 0.707106781187D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(4, new Pose3d(new Translation3d(9.276080000000D, 6.137656000000D, 1.867916000000D), new Rotation3d(new Quaternion(0.965925826289D, 0.000000000000D, 0.258819045103D, 0.000000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(5, new Pose3d(new Translation3d(9.276080000000D, 1.914906000000D, 1.867916000000D), new Rotation3d(new Quaternion(0.965925826289D, 0.000000000000D, 0.258819045103D, 0.000000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(6, new Pose3d(new Translation3d(13.474446000000D, 3.306318000000D, 0.308102000000D), new Rotation3d(new Quaternion(-0.866025403784D, 0.000000000000D, 0.000000000000D, 0.500000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(7, new Pose3d(new Translation3d(13.890498000000D, 4.025900000000D, 0.308102000000D), new Rotation3d(new Quaternion(1.000000000000D, 0.000000000000D, 0.000000000000D, 0.000000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(8, new Pose3d(new Translation3d(13.474446000000D, 4.745482000000D, 0.308102000000D), new Rotation3d(new Quaternion(0.866025403784D, 0.000000000000D, 0.000000000000D, 0.500000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(9, new Pose3d(new Translation3d(12.643358000000D, 4.745482000000D, 0.308102000000D), new Rotation3d(new Quaternion(0.500000000000D, 0.000000000000D, 0.000000000000D, 0.866025403784D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(10, new Pose3d(new Translation3d(12.227306000000D, 4.025900000000D, 0.308102000000D), new Rotation3d(new Quaternion(0.000000000000D, 0.000000000000D, 0.000000000000D, 1.000000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(11, new Pose3d(new Translation3d(12.643358000000D, 3.306318000000D, 0.308102000000D), new Rotation3d(new Quaternion(-0.500000000000D, 0.000000000000D, 0.000000000000D, 0.866025403784D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(12, new Pose3d(new Translation3d(0.851154000000D, 0.655320000000D, 1.485900000000D), new Rotation3d(new Quaternion(0.891006524188D, 0.000000000000D, 0.000000000000D, 0.453990499740D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(13, new Pose3d(new Translation3d(0.851154000000D, 7.396480000000D, 1.485900000000D), new Rotation3d(new Quaternion(-0.891006524188D, 0.000000000000D, 0.000000000000D, 0.453990499740D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(14, new Pose3d(new Translation3d(8.272272000000D, 6.137656000000D, 1.867916000000D), new Rotation3d(new Quaternion(0.000000000000D, -0.258819045103D, 0.000000000000D, 0.965925826289D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(15, new Pose3d(new Translation3d(8.272272000000D, 1.914906000000D, 1.867916000000D), new Rotation3d(new Quaternion(0.000000000000D, -0.258819045103D, 0.000000000000D, 0.965925826289D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(16, new Pose3d(new Translation3d(5.987542000000D, -0.003810000000D, 1.301750000000D), new Rotation3d(new Quaternion(0.707106781187D, 0.000000000000D, 0.000000000000D, 0.707106781187D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(17, new Pose3d(new Translation3d(4.073906000000D, 3.306318000000D, 0.308102000000D), new Rotation3d(new Quaternion(-0.500000000000D, 0.000000000000D, 0.000000000000D, 0.866025403784D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(18, new Pose3d(new Translation3d(3.657600000000D, 4.025900000000D, 0.308102000000D), new Rotation3d(new Quaternion(0.000000000000D, 0.000000000000D, 0.000000000000D, 1.000000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(19, new Pose3d(new Translation3d(4.073906000000D, 4.745482000000D, 0.308102000000D), new Rotation3d(new Quaternion(0.500000000000D, 0.000000000000D, 0.000000000000D, 0.866025403784D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(20, new Pose3d(new Translation3d(4.904740000000D, 4.745482000000D, 0.308102000000D), new Rotation3d(new Quaternion(0.866025403784D, 0.000000000000D, 0.000000000000D, 0.500000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(21, new Pose3d(new Translation3d(5.321046000000D, 4.025900000000D, 0.308102000000D), new Rotation3d(new Quaternion(1.000000000000D, 0.000000000000D, 0.000000000000D, 0.000000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(22, new Pose3d(new Translation3d(4.904740000000D, 3.306318000000D, 0.308102000000D), new Rotation3d(new Quaternion(-0.866025403784D, 0.000000000000D, 0.000000000000D, 0.500000000000D))))); + }; + public static final double FIELD_LENGTH_METERS = 17.548D; + public static final double FIELD_WIDTH_METERS = 8.052D; + public static AprilTagFieldLayout getLayout() { + return new AprilTagFieldLayout(APRILTAG_FIELD_POSES, FIELD_LENGTH_METERS, FIELD_WIDTH_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/field/AprilTagLayoutGenerator.html b/src/main/java/frc/robot/lib/field/AprilTagLayoutGenerator.html new file mode 100644 index 0000000..8499ecf --- /dev/null +++ b/src/main/java/frc/robot/lib/field/AprilTagLayoutGenerator.html @@ -0,0 +1,215 @@ + + + + + + AprilTag Code Generator + + + + +
+

AprilTag Code Generator

+ + + + + + +
+ + +
+
// Generated code will appear here...
+
+ + + + + \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/field/FieldLayout.java b/src/main/java/frc/robot/lib/field/FieldLayout.java index c16c84f..dd8752b 100644 --- a/src/main/java/frc/robot/lib/field/FieldLayout.java +++ b/src/main/java/frc/robot/lib/field/FieldLayout.java @@ -2,8 +2,13 @@ import java.io.IOException; import java.util.ArrayList; +import java.util.Collections; +import java.util.Comparator; +import java.util.HashMap; import java.util.List; +import com.therekrab.autopilot.APTarget; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; @@ -39,6 +44,9 @@ public class FieldLayout { public static final double APRITAG_WIDTH = Units.inchesToMeters(6.50); public static final AprilTagFieldLayout APRILTAG_MAP; + public static final HashMap ENTRY_ANGLES_RIGHT = new HashMap<>(); + public static final HashMap ENTRY_ANGLES_LEFT = new HashMap<>(); + public static final List ALIGN_POSES_RIGHT = new ArrayList<>(); public static final List ALIGN_POSES_LEFT = new ArrayList<>(); @@ -50,19 +58,38 @@ public class FieldLayout { int[] reefIds = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; Transform2d leftReefTransform = new Transform2d( DriveConstants.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), -Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); - for (int i : reefIds) { - ALIGN_POSES_LEFT.add(APRILTAG_MAP.getTagPose(i).get().toPose2d().transformBy(leftReefTransform)); - } Transform2d rightReefTransform = new Transform2d( DriveConstants.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); for (int i : reefIds) { - ALIGN_POSES_RIGHT.add(APRILTAG_MAP.getTagPose(i).get().toPose2d().transformBy(rightReefTransform)); + var tagPose = APRILTAG_MAP.getTagPose(i).get().toPose2d(); + var right = tagPose.transformBy(rightReefTransform); + var left = tagPose.transformBy(leftReefTransform); + ALIGN_POSES_RIGHT.add(right); + ALIGN_POSES_LEFT.add(left); + ENTRY_ANGLES_RIGHT.put(right, tagPose.getRotation().plus(Rotation2d.k180deg)); + ENTRY_ANGLES_LEFT.put(left, tagPose.getRotation().plus(Rotation2d.k180deg)); } } catch (IOException e) { throw new RuntimeException(e); } } + public static Pose2d getNearestPose(Pose2d pose, boolean left) { + return pose.nearest(left ? ALIGN_POSES_LEFT : ALIGN_POSES_RIGHT); + } + + public static APTarget getNearestTarget(Pose2d pose, boolean left) { + var set = left ? ENTRY_ANGLES_LEFT : ENTRY_ANGLES_RIGHT; + var out = Collections.min( + set.keySet(), + Comparator.comparing( + (Pose2d other) -> pose.getTranslation().getDistance(other.getTranslation()) + ).thenComparing( + (Pose2d other) -> + Math.abs(pose.getRotation().minus(other.getRotation()).getRadians()))); + return new APTarget(out).withEntryAngle(set.get(out)); + } + public static Pose2d handleAllianceFlip(Pose2d blue_pose, boolean is_red_alliance) { if (is_red_alliance) { blue_pose = new Pose2d( diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 0526668..5bb245b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -4,6 +4,8 @@ import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +import com.therekrab.autopilot.APTarget; +import com.therekrab.autopilot.Autopilot; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; @@ -25,27 +27,30 @@ import frc.robot.lib.util.Util; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.ctre.CtreDriveConstants; +import frc.robot.subsystems.drive.commands.AutopilotCommand; import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.ctre.CtreDrive; import frc.robot.subsystems.drive.ctre.CtreDriveTelemetry; public class Drive extends SubsystemBase { private static Drive driveInstance; - public static Drive getInstance() { - if (driveInstance == null) { - driveInstance = new Drive(); - } - return driveInstance; - } + public static Drive getInstance() { + if (driveInstance == null) { + driveInstance = new Drive(); + } + return driveInstance; + } private SwerveDriveState lastReadState; - public static final SwerveRequest.FieldCentric teleopRequest = new SwerveRequest.FieldCentric(); + public static final SwerveRequest.FieldCentric teleopRequest = new SwerveRequest.FieldCentric(); public SwerveRequest driveRequest = teleopRequest; - private final CtreDrive drivetrain = CtreDriveConstants.createDrivetrain(); + private final CtreDrive drivetrain = CtreDriveConstants.createDrivetrain(); private final CtreDriveTelemetry telemetry = new CtreDriveTelemetry(MAX_SPEED); - @SuppressWarnings("unused") - private Time lastPoseResetTime = BaseUnits.TimeUnit.of(0.0); // Citrus what are you doing + @SuppressWarnings("unused") + private Time lastPoseResetTime = BaseUnits.TimeUnit.of(0.0); // Citrus what are you doing + + Autopilot ap; private Drive() { lastReadState = drivetrain.getState(); @@ -54,11 +59,34 @@ private Drive() { })); drivetrain.getOdometryThread().setThreadPriority(31); - TelemetryManager.getInstance().addStructPublisher("Mechanisms/Drive", Pose3d.struct, () -> new Pose3d(getPose())); - TelemetryManager.getInstance().addSendable(this); + TelemetryManager.getInstance().addStructPublisher("Mechanisms/Drive", Pose3d.struct, () -> new Pose3d(getPose())); + TelemetryManager.getInstance().addStructPublisher("Drive/TargetSpeeds", ChassisSpeeds.struct, + () -> { + try { + if (driveRequest instanceof SwerveRequest.ApplyFieldSpeeds) { + return ChassisSpeeds.fromFieldRelativeSpeeds( + ((SwerveRequest.ApplyFieldSpeeds) driveRequest).Speeds, + lastReadState.Pose.getRotation()); + } else if (driveRequest instanceof SwerveRequest.ApplyRobotSpeeds) { + return ((SwerveRequest.ApplyRobotSpeeds) driveRequest).Speeds; + } else if (driveRequest instanceof SwerveRequest.FieldCentric) { + var req = ((SwerveRequest.FieldCentric) driveRequest); + return ChassisSpeeds.fromFieldRelativeSpeeds( + req.VelocityX, + req.VelocityY, + req.RotationalRate, + lastReadState.Pose.getRotation()); + } else if (driveRequest instanceof SwerveRequest.RobotCentric) { + var req = ((SwerveRequest.RobotCentric) driveRequest); + return new ChassisSpeeds(req.VelocityX, req.VelocityY, req.RotationalRate); + } + } finally {} + return lastReadState.Speeds; + }); + TelemetryManager.getInstance().addSendable(this); } - /** @return the ctre generated drivetrain */ + /** @return the ctre generated drivetrain */ public CtreDrive getCtreDrive() { return drivetrain; } @@ -71,161 +99,172 @@ public void periodic() { public void outputTelemetry() { telemetry.telemeterize(lastReadState); - FieldLayout.field.setRobotPose(getPose()); + FieldLayout.field.setRobotPose(getPose()); } - /** - * @return the current state - */ + /** + * @return the current state + */ public SwerveDriveState getState() { return drivetrain.getState(); } - /** - * @return the last read pose - */ + /** + * @return the last read pose + */ public Pose2d getPose() { return lastReadState.Pose; } - /** - * @return the chassis speeds, field relative - */ - public ChassisSpeeds getFieldSpeeds() { - return ChassisSpeeds.fromRobotRelativeSpeeds(lastReadState.Speeds, lastReadState.Pose.getRotation()); - } + /** + * @return the chassis speeds, field relative + */ + public ChassisSpeeds getFieldSpeeds() { + return ChassisSpeeds.fromRobotRelativeSpeeds(lastReadState.Speeds, lastReadState.Pose.getRotation()); + } - /** - * Switches the swerve request - *

Please do not the new swerve request every 20 ms

- */ - public void setSwerveRequest(SwerveRequest request) { + /** + * Switches the swerve request + *

Please do not the new swerve request every 20 ms

+ */ + public void setSwerveRequest(SwerveRequest request) { driveRequest = request; } - /** - * @return the current swerve request - */ - public SwerveRequest getSwerveRequest() { + /** + * @return the current swerve request + */ + public SwerveRequest getSwerveRequest() { return driveRequest; } - /** Open loop during teleop mode */ - public Command teleopCommand() { - return runOnce(() -> { - teleopRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0); - setSwerveRequest(teleopRequest); - }).andThen(run(() -> { - double xDesiredRaw = -Robot.controller.getLeftY(); - double yDesiredRaw = -Robot.controller.getLeftX(); - double rotDesiredRaw = -Robot.controller.getRightX(); - double xFancy = Util.applyJoystickDeadband(xDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); - double yFancy = Util.applyJoystickDeadband(yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); - double rotFancy = Util.applyJoystickDeadband(rotDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); - - SmartDashboard.putNumber("Sticks/vX", xDesiredRaw); - SmartDashboard.putNumber("Sticks/vY", yDesiredRaw); - SmartDashboard.putNumber("Sticks/vW", rotDesiredRaw); - - teleopRequest - .withVelocityX(xFancy * MAX_SPEED) - .withVelocityY(yFancy * MAX_SPEED) - .withRotationalRate(rotFancy * MAX_ROTATION_SPEED); - }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Teleop"); - } - - /** - * Auto aligns to the nearest reef face - * @param left chooses the left or right face - */ - public Command autoAlign(boolean left) { - return defer(() -> { - Pose2d pose; - if (left) { - pose = getPose().nearest(FieldLayout.ALIGN_POSES_LEFT); - } else { - pose = getPose().nearest(FieldLayout.ALIGN_POSES_RIGHT); - } - return new PIDToPoseCommand(pose); - }).withName("Auto Align"); - } - - /** Adds a vision update */ + /** Open loop during teleop mode */ + public Command teleopCommand() { + return runOnce(() -> { + teleopRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0); + setSwerveRequest(teleopRequest); + }).andThen(run(() -> { + double xDesiredRaw = -Robot.controller.getLeftY(); + double yDesiredRaw = -Robot.controller.getLeftX(); + double rotDesiredRaw = -Robot.controller.getRightX(); + double xFancy = Util.applyJoystickDeadband(xDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + double yFancy = Util.applyJoystickDeadband(yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + double rotFancy = Util.applyJoystickDeadband(rotDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + + SmartDashboard.putNumber("Sticks/vX", xDesiredRaw); + SmartDashboard.putNumber("Sticks/vY", yDesiredRaw); + SmartDashboard.putNumber("Sticks/vW", rotDesiredRaw); + + teleopRequest + .withVelocityX(xFancy * MAX_SPEED) + .withVelocityY(yFancy * MAX_SPEED) + .withRotationalRate(rotFancy * MAX_ROTATION_SPEED); + }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Teleop"); + } + + /** + * Auto aligns to the nearest reef face + * @param left chooses the left or right face + */ + public Command autoAlign(boolean left) { + return defer(() -> { + Pose2d pose; + if (left) { + pose = getPose().nearest(FieldLayout.ALIGN_POSES_LEFT); + } else { + pose = getPose().nearest(FieldLayout.ALIGN_POSES_RIGHT); + } + return new PIDToPoseCommand(pose); + }).withName("Auto Align"); + } + + /** + * Auto aligns to the nearest reef face + * @param left chooses the left or right face + */ + public Command autopilotAlign(boolean left) { + return defer(() -> { + APTarget pose = FieldLayout.getNearestTarget(getPose(), left); + return new AutopilotCommand(pose); + }).withName("Autopilot Align"); + } + + /** Adds a vision update */ public void addVisionUpdate(Pose2d pose, Time timestamp) { getCtreDrive().addVisionMeasurement(pose, timestamp.in(Units.Seconds)); } - /** Adds a vision update with standard deviations */ + /** Adds a vision update with standard deviations */ public void addVisionUpdate(Pose2d pose, Time timestamp, Matrix stdDevs) { getCtreDrive().addVisionMeasurement(pose, timestamp.in(Units.Seconds), stdDevs); } - /** Resets pose estimator to a pose */ + /** Resets pose estimator to a pose */ public void resetPose(Pose2d pose) { getCtreDrive().resetPose(pose); lastPoseResetTime = - Units.Seconds.of(Utils.getCurrentTimeSeconds()).plus(POSE_RESET_PREVENTION_TIME); + Units.Seconds.of(Utils.getCurrentTimeSeconds()).plus(POSE_RESET_PREVENTION_TIME); } - /** A command that resets the pose */ + /** A command that resets the pose */ public Command resetPoseCommand(Pose2d pose) { return Commands.runOnce(() -> resetPose(pose)); } - /** Whether the pitch is stable */ + /** Whether the pitch is stable */ public boolean isPitchStable() { return drivetrain.getPigeon2().getAngularVelocityYDevice().getValue().abs(Units.DegreesPerSecond) - < MAX_VELOCITY_STABLE - && drivetrain.getPigeon2().getPitch().getValue().abs(BaseUnits.AngleUnit) - < MAX_PITCH_STABLE; + < MAX_VELOCITY_STABLE + && drivetrain.getPigeon2().getPitch().getValue().abs(BaseUnits.AngleUnit) + < MAX_PITCH_STABLE; } - /** Whether the roll is stable */ + /** Whether the roll is stable */ public boolean isRollStable() { return drivetrain.getPigeon2().getAngularVelocityXDevice().getValue().abs(Units.DegreesPerSecond) - < MAX_VELOCITY_STABLE - && drivetrain.getPigeon2().getRoll().getValue().abs(BaseUnits.AngleUnit) - < MAX_PITCH_STABLE; + < MAX_VELOCITY_STABLE + && drivetrain.getPigeon2().getRoll().getValue().abs(BaseUnits.AngleUnit) + < MAX_PITCH_STABLE; } - /** Whether the robot is stable */ + /** Whether the robot is stable */ public boolean isStable() { ChassisSpeeds speeds = getState().Speeds; return isPitchStable() - && isRollStable() - && Units.MetersPerSecond.of(Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond)) - .lte(MAX_SPEED_SCORING_TRANSLATION) - && Units.RadiansPerSecond.of(speeds.omegaRadiansPerSecond).lte(MAX_ROTATION_SPEED_SCORING); + && isRollStable() + && Units.MetersPerSecond.of(Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond)) + .lte(MAX_SPEED_SCORING_TRANSLATION) + && Units.RadiansPerSecond.of(speeds.omegaRadiansPerSecond).lte(MAX_ROTATION_SPEED_SCORING); } @Override public void initSendable(SendableBuilder builder) { - super.initSendable(builder); + super.initSendable(builder); builder.addDoubleProperty( - "Pitch Velocity Degrees Per Second", - () -> drivetrain - .getPigeon2() - .getAngularVelocityYDevice() - .getValue() - .in(Units.DegreesPerSecond), - null); + "Pitch Velocity Degrees Per Second", + () -> drivetrain + .getPigeon2() + .getAngularVelocityYDevice() + .getValue() + .in(Units.DegreesPerSecond), + null); builder.addDoubleProperty( - "Pitch Degrees", - () -> drivetrain.getPigeon2().getPitch().getValue().in(Units.Degrees), - null); + "Pitch Degrees", + () -> drivetrain.getPigeon2().getPitch().getValue().in(Units.Degrees), + null); builder.addDoubleProperty( - "Roll Velocity Degrees Per Second", - () -> drivetrain - .getPigeon2() - .getAngularVelocityXDevice() - .getValue() - .in(Units.DegreesPerSecond), - null); + "Roll Velocity Degrees Per Second", + () -> drivetrain + .getPigeon2() + .getAngularVelocityXDevice() + .getValue() + .in(Units.DegreesPerSecond), + null); builder.addDoubleProperty( - "Roll Degrees", - () -> drivetrain.getPigeon2().getRoll().getValue().in(Units.Degrees), - null); + "Roll Degrees", + () -> drivetrain.getPigeon2().getRoll().getValue().in(Units.Degrees), + null); addModuleToBuilder(builder, 0); addModuleToBuilder(builder, 1); @@ -233,12 +272,11 @@ public void initSendable(SendableBuilder builder) { addModuleToBuilder(builder, 3); } - /** Telemeterizes a module */ + /** Telemeterizes a module */ private void addModuleToBuilder(SendableBuilder builder, int module) { - TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Drive", - drivetrain.getModules()[module].getDriveMotor(), builder); - - TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Angle", - drivetrain.getModules()[module].getSteerMotor(), builder); + TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Drive", + drivetrain.getModules()[module].getDriveMotor(), builder); + TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Angle", + drivetrain.getModules()[module].getSteerMotor(), builder); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java new file mode 100644 index 0000000..31d5b96 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java @@ -0,0 +1,80 @@ +package frc.robot.subsystems.drive.commands; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; +import com.therekrab.autopilot.APConstraints; +import com.therekrab.autopilot.APProfile; +import com.therekrab.autopilot.APTarget; +import com.therekrab.autopilot.Autopilot; +import com.therekrab.autopilot.Autopilot.APResult; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.Drive; + +public class AutopilotCommand extends Command { + private final APTarget target; + private final Drive drive; + private static final APConstraints constraints = new APConstraints() + .withVelocity(MAX_SPEED) + .withAcceleration(MAX_ACCEL) + .withJerk(3.0); + + private static final APProfile profile = new APProfile(constraints) + .withErrorXY(Units.Centimeters.of(2)) + .withErrorTheta(Units.Degrees.of(0.5)) + .withBeelineRadius(Units.Centimeters.of(8)); + + public static final Autopilot autoPilot = new Autopilot(profile); + + private final SwerveRequest.FieldCentricFacingAngle request = new SwerveRequest.FieldCentricFacingAngle() + .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance) + .withDriveRequestType(DriveRequestType.Velocity) + .withHeadingPID( + ROTATION_CONSTANTS.kP, + ROTATION_CONSTANTS.kI, + ROTATION_CONSTANTS.kD); + + public AutopilotCommand(APTarget target) { + this(target, Drive.getInstance()); + } + + public AutopilotCommand(APTarget target, Drive drive) { + this.target = target; + this.drive = drive; + addRequirements(drive); + } + + @Override + public void initialize() { + drive.setSwerveRequest(request); + } + + @Override + public void execute() { + ChassisSpeeds robotRelativeSpeeds = drive.getState().Speeds; + Pose2d pose = drive.getPose(); + + APResult out = autoPilot.calculate(pose, robotRelativeSpeeds, target); + + request + .withVelocityX(out.vx()) + .withVelocityY(out.vy()) + .withTargetDirection(out.targetAngle()); + } + + @Override + public boolean isFinished() { + return autoPilot.atTarget(drive.getPose(), target); + } + + @Override + public void end(boolean interrupted) { + drive.setSwerveRequest(new SwerveRequest.RobotCentric()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index 9b8789c..4327be3 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -11,7 +11,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.lib.control.ControlConstants.PIDFConstants; +import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; import frc.robot.lib.control.PIDVController; +import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.lib.util.Util; import frc.robot.subsystems.drive.Drive; @@ -25,7 +27,7 @@ public class PIDToPoseCommand extends Command { new SwerveRequest.ApplyFieldSpeeds(); private final PIDVController translationController; - private final PIDVController thetaController; + private final ProfiledPIDVController thetaController; private final Pose2d target; private Pose2d currentPose; @@ -38,14 +40,14 @@ public PIDToPoseCommand(Pose2d target) { Drive.getInstance(), target, TRANSLATION_CONSTANTS, - ROTATION_CONSTANTS); + PROFILED_ROTATION_CONSTANTS); } - public PIDToPoseCommand(Drive drive, Pose2d target, PIDFConstants translationConstants, PIDFConstants rotationConstants) { + public PIDToPoseCommand(Drive drive, Pose2d target, PIDFConstants translationConstants, ProfiledPIDFConstants rotationConstants) { this.drive = drive; this.target = target; translationController = new PIDVController(translationConstants); - thetaController = new PIDVController(rotationConstants); + thetaController = new ProfiledPIDVController(rotationConstants); thetaController.enableContinuousInput(-Math.PI, Math.PI); addRequirements(drive); @@ -83,10 +85,10 @@ public ChassisSpeeds calculateSpeeds() { // Magnitude target double vMagnitude = MathUtil.clamp( - translationController.setTarget(delta.getNorm()) + translationController.setTarget(0.0) .setMeasurement( - 0.0, // We are exactly where we are - Util.chassisSpeedsMagnitude( + delta.getNorm(), // We are exactly where we are + -Util.chassisSpeedsMagnitude( currentSpeeds)) // How fast we are going .getOutput(), -MAX_SPEED, MAX_SPEED); From 19c3d7b93131a0758a6dd45a198f9972a2274ae2 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Fri, 9 Jan 2026 21:09:13 -0800 Subject: [PATCH 24/36] Fixed PIDToPose --- build.gradle | 12 ++++----- src/main/java/frc/robot/Robot.java | 1 - .../lib/control/ProfiledPIDVController.java | 26 ++++++++++--------- .../drive/commands/PIDToPoseCommand.java | 9 +++++-- 4 files changed, 27 insertions(+), 21 deletions(-) diff --git a/build.gradle b/build.gradle index 1d8df5e..c842242 100644 --- a/build.gradle +++ b/build.gradle @@ -28,12 +28,12 @@ deploy { frcJava(getArtifactTypeClass('FRCJavaArtifact')) { // Enable VisualVM connection - // jvmArgs.add("-Dcom.sun.management.jmxremote=true") - // jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") - // jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") - // jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") - // jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") - // jvmArgs.add("-Djava.rmi.server.hostname=10.14.58.2") // Replace TE.AM with team number + jvmArgs.add("-Dcom.sun.management.jmxremote=true") + jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + jvmArgs.add("-Djava.rmi.server.hostname=10.14.58.2") // Replace TE.AM with team number } // Static files artifact diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3c6281b..0b73ec1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -143,7 +143,6 @@ public void testPeriodic() { /** This function is called once when the robot is first started up. */ @Override public void simulationInit() { - DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); } /** This function is called periodically whilst in simulation. */ diff --git a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java index 9de1797..2e76a9c 100644 --- a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java +++ b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; import frc.robot.lib.control.ControlConstants.ProfiledPIDVConstants; @@ -21,6 +20,11 @@ public ProfiledPIDVController(ProfiledPIDVConstants constants) { profile = new TrapezoidProfile(constants.constraints); } + public ProfiledPIDVController setInitialSetpoint(double position, double speed) { + setpoint = new TrapezoidProfile.State(position, speed); + return this; + } + /** * Makes the controller continuous, which means that values repeat. */ @@ -51,23 +55,21 @@ public ProfiledPIDVController setMeasurement(double position, double speed) { } public double getOutput() { - if (controller.isContinuous) { - double errorBound = (controller.maxRange - controller.minRange) / 2.0; + // if (controller.isContinuous) { + // double errorBound = (controller.maxRange - controller.minRange) / 2.0; - double goalDelta = - MathUtil.inputModulus(goal.position - controller.positionMeasurement, -errorBound, errorBound); - double setpointDelta = - MathUtil.inputModulus(setpoint.position - controller.positionMeasurement, -errorBound, errorBound); + // double goalDelta = + // MathUtil.inputModulus(goal.position - controller.positionMeasurement, -errorBound, errorBound); + // double setpointDelta = + // MathUtil.inputModulus(setpoint.position - controller.positionMeasurement, -errorBound, errorBound); - goal.position = goalDelta + controller.positionMeasurement; - setpoint.position = setpointDelta + controller.positionMeasurement; - } + // goal.position = goalDelta + controller.positionMeasurement; + // setpoint.position = setpointDelta + controller.positionMeasurement; + // } // Advance profile by one timestep setpoint = profile.calculate(Constants.DT, setpoint, goal); - SmartDashboard.putNumber("Debug/Profile", setpoint.velocity); - // Use profiled position & velocity as setpoints return controller.setTarget(setpoint.position, setpoint.velocity).getOutput(); } diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index c63da29..f662bf3 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -55,6 +55,10 @@ public PIDToPoseCommand(Drive drive, Pose2d target, PIDVConstants translationCon @Override public void initialize() { + var state = drive.getState(); + thetaController.setInitialSetpoint( + state.Pose.getRotation().getRadians(), + state.Speeds.omegaRadiansPerSecond); drive.setSwerveRequest(request); } @@ -94,12 +98,13 @@ public ChassisSpeeds calculateSpeeds() { // The angle we are at relative to the target var deltaRotation = delta.getAngle(); - double rotation = + double rotation = MathUtil.clamp( thetaController.setTarget(target.getRotation().getRadians()) // Theta target .setMeasurement( currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond) // We are where we are and we are as fast as how fast we are going - .getOutput(); + .getOutput(), + -MAX_ROTATION_SPEED, MAX_ROTATION_SPEED); SmartDashboard.putNumber("Debug/PIDToPose/vx", vMagnitude * deltaRotation.getCos()); SmartDashboard.putNumber("Debug/PIDToPose/vy", vMagnitude * deltaRotation.getSin()); From 7e53dc40674d6f7c63f7a7fb1005ecfdcd4f9a57 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Fri, 9 Jan 2026 21:37:41 -0800 Subject: [PATCH 25/36] last few bugs trust --- src/main/java/frc/robot/ControlsMapping.java | 4 +-- .../subsystems/drive/DriveConstants.java | 16 +++++----- .../drive/commands/AutopilotCommand.java | 5 ++-- .../drive/commands/PIDToPoseCommand.java | 29 ++++++++++++------- .../drive/commands/TrajectoryCommand.java | 15 ++++++---- 5 files changed, 40 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index ef544ff..d8942eb 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -21,8 +21,8 @@ public static void mapTeleopCommand() { controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); - controller.b().whileTrue(new PIDToPoseCommand( - new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120.0)))); + controller.x().whileTrue(Drive.getInstance().autopilotAlign(true)); + controller.y().whileTrue(Drive.getInstance().autopilotAlign(false)); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 047505f..1792291 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -9,12 +9,12 @@ import frc.robot.subsystems.drive.ctre.CtreDriveConstants; public final class DriveConstants { - public static final double EPSILON_TRANSLATION = 0.015; // cm - public static final double EPSILON_ROTATION = Units.Degrees.of(1).in(Units.Radians); + public static final double EPSILON_TRANSLATION = 0.02; // cm + public static final double EPSILON_ROTATION = Units.Degrees.of(2).in(Units.Radians); // Maximums - public static final double MAX_SPEED = Units.MetersPerSecond.of(3.5).in(Units.MetersPerSecond); - public static final double MAX_ACCEL = Units.MetersPerSecondPerSecond.of(6.0).in(Units.MetersPerSecondPerSecond); + public static final double MAX_SPEED = Units.MetersPerSecond.of(4.5).in(Units.MetersPerSecond); + public static final double MAX_ACCEL = Units.MetersPerSecondPerSecond.of(9.0).in(Units.MetersPerSecondPerSecond); public static final double MAX_ROTATION_SPEED = Units.RotationsPerSecond.of(2.0).in(Units.RadiansPerSecond); public static final double MAX_ROTATION_ACCEL = @@ -37,16 +37,16 @@ public final class DriveConstants { // Trajectory and snap constants public static final PIDVConstants TRANSLATION_CONSTANTS = - new PIDVConstants(5, 0.001, 0.1); + new PIDVConstants(5, 0.0, 0.1); public static final ProfiledPIDVConstants PROFILED_TRANSLATION_CONSTANTS = - new ProfiledPIDVConstants(5.0, 0.001, 0.1, + new ProfiledPIDVConstants(5.0, 0.0, 0.1, new TrapezoidProfile.Constraints( MAX_SPEED, MAX_ACCEL)); public static final PIDVConstants ROTATION_CONSTANTS = - new PIDVConstants(4.0, 0.001, 0.1); + new PIDVConstants(5.0, 0.0, 0.1); public static final ProfiledPIDVConstants PROFILED_ROTATION_CONSTANTS = - new ProfiledPIDVConstants(5.0, 0.001, 0.1, + new ProfiledPIDVConstants(5.0, 0.0, 0.1, new TrapezoidProfile.Constraints( MAX_ROTATION_SPEED, MAX_ROTATION_ACCEL)); diff --git a/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java index 31d5b96..6a38cc5 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java @@ -16,6 +16,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveConstants; public class AutopilotCommand extends Command { private final APTarget target; @@ -26,8 +27,8 @@ public class AutopilotCommand extends Command { .withJerk(3.0); private static final APProfile profile = new APProfile(constraints) - .withErrorXY(Units.Centimeters.of(2)) - .withErrorTheta(Units.Degrees.of(0.5)) + .withErrorXY(Units.Meters.of(DriveConstants.EPSILON_TRANSLATION)) + .withErrorTheta(Units.Radians.of(DriveConstants.EPSILON_ROTATION)) .withBeelineRadius(Units.Centimeters.of(8)); public static final Autopilot autoPilot = new Autopilot(profile); diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index f662bf3..99fccb6 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -5,6 +5,8 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.util.sendable.SendableBuilder; @@ -24,7 +26,7 @@ public class PIDToPoseCommand extends Command { private final SwerveRequest.ApplyFieldSpeeds request = new SwerveRequest.ApplyFieldSpeeds(); - private final PIDVController translationController; + private final ProfiledPIDVController translationController; private final ProfiledPIDVController thetaController; private final Pose2d target; @@ -33,21 +35,25 @@ public class PIDToPoseCommand extends Command { private ChassisSpeeds targetSpeeds = new ChassisSpeeds(); + private final Debouncer finishDebouncer; + public PIDToPoseCommand(Pose2d target) { this( Drive.getInstance(), target, - TRANSLATION_CONSTANTS, + PROFILED_TRANSLATION_CONSTANTS, PROFILED_ROTATION_CONSTANTS); } - public PIDToPoseCommand(Drive drive, Pose2d target, PIDVConstants translationConstants, ProfiledPIDVConstants rotationConstants) { + public PIDToPoseCommand(Drive drive, Pose2d target, ProfiledPIDVConstants translationConstants, ProfiledPIDVConstants rotationConstants) { this.drive = drive; this.target = target; - translationController = new PIDVController(translationConstants); + translationController = new ProfiledPIDVController(translationConstants); thetaController = new ProfiledPIDVController(rotationConstants); thetaController.enableContinuousInput(-Math.PI, Math.PI); + finishDebouncer = new Debouncer(0.040, DebounceType.kRising); + addRequirements(drive); setName("(" + target.getX() + ", " + target.getY() + ", " + target.getRotation().getDegrees() + " deg)" + " :PID to pose"); @@ -86,11 +92,11 @@ public ChassisSpeeds calculateSpeeds() { var delta = target.getTranslation().minus(currentPose.getTranslation()); // Magnitude target - double vMagnitude = MathUtil.clamp( - translationController.setTarget(delta.getNorm()) + double vMagnitude = -MathUtil.clamp( + translationController.setTarget(0) .setMeasurement( - 0, // We are exactly where we are - Util.chassisSpeedsMagnitude( + delta.getNorm(), // We are exactly where we are + -Util.chassisSpeedsMagnitude( currentSpeeds)) // How fast we are going .getOutput(), -MAX_SPEED, MAX_SPEED); @@ -118,8 +124,9 @@ public ChassisSpeeds calculateSpeeds() { @Override public boolean isFinished() { - return Math.abs(translationController.getError()) <= EPSILON_TRANSLATION - && MathUtil.isNear(thetaController.getError(), 0, EPSILON_ROTATION); // Within tolerance + return finishDebouncer.calculate( + Math.abs(translationController.getError()) <= EPSILON_TRANSLATION + && MathUtil.isNear(thetaController.getError(), 0, EPSILON_ROTATION)); // Within tolerance } @Override @@ -128,7 +135,7 @@ public void end(boolean interrupted) { drive.setSwerveRequest(new SwerveRequest.ApplyRobotSpeeds()); System.out.printf( "Done with auto-align, error: %.5f m, interrupted: %b\n", - translationController.getError(), interrupted); + -translationController.getError(), interrupted); } public static double estimateTimeToPose( diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index e1d3a65..6c32594 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -120,17 +120,20 @@ public ChassisSpeeds calculateSpeeds() { xAccelFF += -angularAccel * targetState.pose.getRotation().getSin(); yAccelFF += angularAccel * targetState.pose.getRotation().getCos(); - double vx = xController.setTarget(targetState.pose.getX())// Target setting + double vx = xController.setTarget(targetState.pose.getX(), targetState.speeds.vxMetersPerSecond) // Target setting .setMeasurement(currentPose.getX(), currentSpeeds.vxMetersPerSecond) // Measurement setting .getOutput(); // Output getting // same thing but for vy and rotation instead - double vy = yController.setTarget(targetState.pose.getY()) + double vy = yController.setTarget(targetState.pose.getY(), targetState.speeds.vyMetersPerSecond) .setMeasurement(currentPose.getY(), currentSpeeds.vyMetersPerSecond) .getOutput(); - double rotation = thetaController.setTarget(targetState.pose.getRotation().getRadians()) - .setMeasurement( + double rotation = thetaController + .setTarget( + targetState.pose.getRotation().getRadians(), + targetState.speeds.omegaRadiansPerSecond + ).setMeasurement( currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond) .getOutput(); @@ -149,10 +152,10 @@ public boolean isFinished() { if (trajectory.isDone()) { System.out.printf( - "Done with trajectory, error: %.5f m, int error translation: %.5f m, rot: %.5f rad\n", + "Done with trajectory, error: %.5f m, int error translation: %.5f m*s, rot: %.5f deg*s\n", Math.hypot(xController.getError(), yController.getError()), tracker.getTranslationRmsError(), - tracker.getRotationRmsError()); + tracker.getRotationRmsError() / Math.PI * 180); return true; // We are done guys } From e4c60a856f5be484a7cb37526acf6819a1ab980e Mon Sep 17 00:00:00 2001 From: "akhil.space" <145620839+aerospikeengineer@users.noreply.github.com> Date: Wed, 14 Jan 2026 16:17:25 -0800 Subject: [PATCH 26/36] Basic Shooter Code Barebones code --- .../frc/robot/subsystems/shooter/Shooter.java | 115 ++++++++++++++++++ .../subsystems/shooter/ShooterConstants.java | 46 +++++++ vendordeps/libgrapplefrc2025.json | 71 +++++++++++ 3 files changed, 232 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/shooter/Shooter.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java create mode 100644 vendordeps/libgrapplefrc2025.json diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java new file mode 100644 index 0000000..0712b04 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -0,0 +1,115 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import au.grapplerobotics.LaserCan; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.TelemetryManager; + +public class Shooter extends SubsystemBase { + private static Shooter ShooterInstance; + public static Shooter getInstance() { + if (ShooterInstance == null) { + ShooterInstance = new Shooter(); + } + return ShooterInstance; + } + + private final TalonFX leftMotor; + private final TalonFX rightMotor; + + private final LaserCan shooterLaser; + + private final Debouncer shooterDebouncer; + + private boolean inRangeShooter; + + private double lastReadSpeed; + private ControlRequest request = new NeutralOut(); + + private Shooter() { + super(); + + leftMotor = new TalonFX(ShooterConstants.Motors.LEFT.id); + rightMotor = new TalonFX(ShooterConstants.Motors.RIGHT.id); + + shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); + + leftMotor.getConfigurator().apply(ShooterConstants.getConfig()); + rightMotor.getConfigurator().apply(ShooterConstants.getConfig()); + leftMotor.setNeutralMode(NeutralModeValue.Brake); + rightMotor.setNeutralMode(NeutralModeValue.Brake); + rightMotor.setControl( + new Follower(leftMotor.getDeviceID(), true)); + + shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); + + inRangeShooter = false; + + TelemetryManager.getInstance().addSendable(this); + } + + @Override + public void periodic() { + // Read inputs + lastReadSpeed = leftMotor.getVelocity().getValueAsDouble(); + inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); + leftMotor.setControl(request); + } + + /** Replaces the request */ + private void setRequest(ControlRequest request) { + this.request = request; + } + + /** Stops the shooter */ + public Command stop() { + return runOnce(() -> setRequest(new NeutralOut())).withName("Stopped"); + } + + /** Gets the intake laser measurement */ + private double getMeasurementShooter() { + var measurement = shooterLaser.getMeasurement(); + if (measurement != null) { + return measurement.distance_mm; + } else { + return Double.POSITIVE_INFINITY; + } + } + + /** Gets whether the intake laser detects a coral */ + private boolean getShooterLaser() { + return getMeasurementShooter() < 100; + } + + public Command shoot() { + return runOnce(() -> setRequest( + new VelocityVoltage(ShooterConstants.SHOOT_SPEED)) + ).andThen( + Commands.waitUntil(() -> !inRangeShooter), + Commands.waitSeconds(0.15), + stop() + ).withName("Shooting"); + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + builder.addDoubleProperty( + "/Speed", + () -> lastReadSpeed, + null); + TelemetryManager.makeSendableTalonFX("/Left", leftMotor, builder); + TelemetryManager.makeSendableTalonFX("/Right", rightMotor, builder); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java new file mode 100644 index 0000000..9c4c69b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.VoltageConfigs; + +public final class ShooterConstants { + public static final double SHOOT_SPEED = 25; // rotations per second + + /** Motor ids */ + public static enum Motors { + LEFT(12), + RIGHT(13); + public final int id; + private Motors(int id) { + this.id = id; + } + } + + /** Laser ids */ + public static enum Lasers { + //BACK(30), + FRONT(31); + public final int id; + private Lasers(int id) { + this.id = id; + } + } + + /** Config for shooter motors */ + public static TalonFXConfiguration getConfig() { + return new TalonFXConfiguration() + .withSlot0(new Slot0Configs() + .withKV(0.0) + .withKP(0.3) + .withKI(0.0) + .withKD(0.0)) + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(30) + .withSupplyCurrentLimit(30)) + .withVoltage(new VoltageConfigs() + .withPeakForwardVoltage(12.0) + .withPeakReverseVoltage(-12.0)); + } +} diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2025.json new file mode 100644 index 0000000..508fd7b --- /dev/null +++ b/vendordeps/libgrapplefrc2025.json @@ -0,0 +1,71 @@ +{ + "fileName": "libgrapplefrc2025.json", + "name": "libgrapplefrc", + "version": "2025.0.8", + "frcYear": "2025", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ + "https://storage.googleapis.com/grapple-frc-maven" + ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2025.0.8" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.0.8", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2025.0.8", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.0.8", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From 77b51a7745ef5484d67249644469d01e14a2cab6 Mon Sep 17 00:00:00 2001 From: "akhil.space" <145620839+aerospikeengineer@users.noreply.github.com> Date: Sat, 17 Jan 2026 11:13:15 -0800 Subject: [PATCH 27/36] Addiitonal Shooter Changes --- .../frc/robot/subsystems/shooter/Shooter.java | 30 +++++++++---------- .../subsystems/shooter/ShooterConstants.java | 4 +-- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 0712b04..0f620cb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -25,14 +25,14 @@ public static Shooter getInstance() { return ShooterInstance; } - private final TalonFX leftMotor; - private final TalonFX rightMotor; + private final TalonFX topMotor; + private final TalonFX bottomMotor; private final LaserCan shooterLaser; private final Debouncer shooterDebouncer; - private boolean inRangeShooter; + private boolean inRangeShooter; private double lastReadSpeed; private ControlRequest request = new NeutralOut(); @@ -40,17 +40,17 @@ public static Shooter getInstance() { private Shooter() { super(); - leftMotor = new TalonFX(ShooterConstants.Motors.LEFT.id); - rightMotor = new TalonFX(ShooterConstants.Motors.RIGHT.id); + topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); + bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); - leftMotor.getConfigurator().apply(ShooterConstants.getConfig()); - rightMotor.getConfigurator().apply(ShooterConstants.getConfig()); - leftMotor.setNeutralMode(NeutralModeValue.Brake); - rightMotor.setNeutralMode(NeutralModeValue.Brake); - rightMotor.setControl( - new Follower(leftMotor.getDeviceID(), true)); + topMotor.getConfigurator().apply(ShooterConstants.getConfig()); + bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); + topMotor.setNeutralMode(NeutralModeValue.Brake); + bottomMotor.setNeutralMode(NeutralModeValue.Brake); + bottomMotor.setControl( + new Follower(topMotor.getDeviceID(), true)); shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); @@ -62,9 +62,9 @@ private Shooter() { @Override public void periodic() { // Read inputs - lastReadSpeed = leftMotor.getVelocity().getValueAsDouble(); + lastReadSpeed = topMotor.getVelocity().getValueAsDouble(); inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); - leftMotor.setControl(request); + bottomMotor.setControl(request); } /** Replaces the request */ @@ -109,7 +109,7 @@ public void initSendable(SendableBuilder builder) { "/Speed", () -> lastReadSpeed, null); - TelemetryManager.makeSendableTalonFX("/Left", leftMotor, builder); - TelemetryManager.makeSendableTalonFX("/Right", rightMotor, builder); + TelemetryManager.makeSendableTalonFX("/Top", topMotor, builder); + TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 9c4c69b..76bdae3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -10,8 +10,8 @@ public final class ShooterConstants { /** Motor ids */ public static enum Motors { - LEFT(12), - RIGHT(13); + TOP(12), + BOTTOM(13); public final int id; private Motors(int id) { this.id = id; From 96a29e30a95e88d4000fa4f6b718d9d75d4a926e Mon Sep 17 00:00:00 2001 From: "akhil.space" <145620839+aerospikeengineer@users.noreply.github.com> Date: Sat, 17 Jan 2026 14:57:38 -0800 Subject: [PATCH 28/36] sping up --- .../frc/robot/subsystems/shooter/Shooter.java | 43 +++++++++++++++++-- .../subsystems/shooter/ShooterConstants.java | 5 ++- 2 files changed, 44 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 0f620cb..01ca276 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -87,19 +87,56 @@ private double getMeasurementShooter() { } } - /** Gets whether the intake laser detects a coral */ + public Command spinUp() + { + ShooterConstants.SHOOT_SPEED_BOTTOM = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; + ShooterConstants.SHOOT_SPEED_TOP = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; + } + + public Command spinDown() + { + ShooterConstants.SHOOT_SPEED_BOTTOM = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; + ShooterConstants.SHOOT_SPEED_TOP = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; + } + + /** Gets whether the shooter laser detects the "fuel" (balls) */ private boolean getShooterLaser() { return getMeasurementShooter() < 100; } - public Command shoot() { + public Command topShoot() { + return runOnce(() -> setRequest( + new VelocityVoltage(ShooterConstants.SHOOT_SPEED_TOP)) + ).andThen( + Commands.waitUntil(() -> !inRangeShooter), + Commands.waitSeconds(0.15), + stop() + ).withName("Top Shooting"); + } + + public Command bottomShoot() { return runOnce(() -> setRequest( - new VelocityVoltage(ShooterConstants.SHOOT_SPEED)) + new VelocityVoltage(ShooterConstants.SHOOT_SPEED_BOTTOM)) + ).andThen( + Commands.waitUntil(() -> !inRangeShooter), + Commands.waitSeconds(0.15), + stop() + ).withName("Bottom Shooting"); + } + + public Command shoot() + { + + topShoot(); + bottomShoot(); + + return runOnce(() -> setRequest(request) ).andThen( Commands.waitUntil(() -> !inRangeShooter), Commands.waitSeconds(0.15), stop() ).withName("Shooting"); + } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 76bdae3..ad84a91 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -6,7 +6,10 @@ import com.ctre.phoenix6.configs.VoltageConfigs; public final class ShooterConstants { - public static final double SHOOT_SPEED = 25; // rotations per second + public static double SHOOT_SPEED_TOP = 25; // rotations per second + public static double SHOOT_SPEED_BOTTOM = 25; // rotations per second + + public static final double SHOOT_SPEED_INCREMENT = 5; // rotation per second /** Motor ids */ public static enum Motors { From db2419b6213e29e1ec550c9b5bd39a2aed8ca198 Mon Sep 17 00:00:00 2001 From: "akhil.space" <145620839+aerospikeengineer@users.noreply.github.com> Date: Sat, 17 Jan 2026 16:01:28 -0800 Subject: [PATCH 29/36] Separate Shooters for Each Motor Hi. My name is Shooter. TalonFX: --- .../subsystems/shooter/BottomShooter.java | 124 ++++++++++++++++++ .../shooter/{Shooter.java => TopShooter.java} | 60 +++------ 2 files changed, 140 insertions(+), 44 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/BottomShooter.java rename src/main/java/frc/robot/subsystems/shooter/{Shooter.java => TopShooter.java} (70%) diff --git a/src/main/java/frc/robot/subsystems/shooter/BottomShooter.java b/src/main/java/frc/robot/subsystems/shooter/BottomShooter.java new file mode 100644 index 0000000..4ea3e3b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/BottomShooter.java @@ -0,0 +1,124 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import au.grapplerobotics.LaserCan; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.TelemetryManager; + +public class BottomShooter extends SubsystemBase { + private static BottomShooter ShooterInstance; + public static BottomShooter getInstance() { + if (ShooterInstance == null) { + ShooterInstance = new BottomShooter(); + } + return ShooterInstance; + } + + private final TalonFX bottomMotor; + + private final LaserCan shooterLaser; + + private final Debouncer shooterDebouncer; + + private boolean inRangeShooter; + + private double lastReadSpeed; + private ControlRequest request = new NeutralOut(); + + private BottomShooter() { + super(); + + bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); + + shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); + + bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); + bottomMotor.setNeutralMode(NeutralModeValue.Brake); + + shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); + + inRangeShooter = false; + + TelemetryManager.getInstance().addSendable(this); + } + + @Override + public void periodic() { + // Read inputs + lastReadSpeed = bottomMotor.getVelocity().getValueAsDouble(); + inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); + } + + /** Replaces the request */ + private void setRequest(ControlRequest request) { + this.request = request; + } + + /** Stops the shooter */ + public Command stop() { + return runOnce(() -> setRequest(new NeutralOut())).withName("Stopped"); + } + + /** Gets the intake laser measurement */ + private double getMeasurementShooter() { + var measurement = shooterLaser.getMeasurement(); + if (measurement != null) { + return measurement.distance_mm; + } else { + return Double.POSITIVE_INFINITY; + } + } + + public Command spinUp() + { + ShooterConstants.SHOOT_SPEED_BOTTOM = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; + return runOnce(() -> setRequest(request) + ).andThen( + stop() + ).withName("Increase Bottom Motor Speed"); + } + + public Command spinDown() + { + ShooterConstants.SHOOT_SPEED_BOTTOM = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; + return runOnce(() -> setRequest(request) + ).andThen( + stop() + ).withName("Lower Bottom Motor Speed"); + } + + /** Gets whether the shooter laser detects the "fuel" (balls) */ + private boolean getShooterLaser() { + return getMeasurementShooter() < 100; + } + + public Command BottomShoot() { + return runOnce(() -> setRequest( + new VelocityVoltage(ShooterConstants.SHOOT_SPEED_BOTTOM)) + ).andThen( + Commands.waitUntil(() -> !inRangeShooter), + Commands.waitSeconds(0.15), + stop() + ).withName("Bottom Shooting"); + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + builder.addDoubleProperty( + "/Speed", + () -> lastReadSpeed, + null); + TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/TopShooter.java similarity index 70% rename from src/main/java/frc/robot/subsystems/shooter/Shooter.java rename to src/main/java/frc/robot/subsystems/shooter/TopShooter.java index 01ca276..5d42a39 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/TopShooter.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.shooter; import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -16,17 +15,16 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.TelemetryManager; -public class Shooter extends SubsystemBase { - private static Shooter ShooterInstance; - public static Shooter getInstance() { +public class TopShooter extends SubsystemBase { + private static TopShooter ShooterInstance; + public static TopShooter getInstance() { if (ShooterInstance == null) { - ShooterInstance = new Shooter(); + ShooterInstance = new TopShooter(); } return ShooterInstance; } private final TalonFX topMotor; - private final TalonFX bottomMotor; private final LaserCan shooterLaser; @@ -37,20 +35,15 @@ public static Shooter getInstance() { private double lastReadSpeed; private ControlRequest request = new NeutralOut(); - private Shooter() { + private TopShooter() { super(); topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); - bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); topMotor.getConfigurator().apply(ShooterConstants.getConfig()); - bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); topMotor.setNeutralMode(NeutralModeValue.Brake); - bottomMotor.setNeutralMode(NeutralModeValue.Brake); - bottomMotor.setControl( - new Follower(topMotor.getDeviceID(), true)); shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); @@ -64,7 +57,6 @@ public void periodic() { // Read inputs lastReadSpeed = topMotor.getVelocity().getValueAsDouble(); inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); - bottomMotor.setControl(request); } /** Replaces the request */ @@ -89,17 +81,23 @@ private double getMeasurementShooter() { public Command spinUp() { - ShooterConstants.SHOOT_SPEED_BOTTOM = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; - ShooterConstants.SHOOT_SPEED_TOP = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; + ShooterConstants.SHOOT_SPEED_TOP = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; + return runOnce(() -> setRequest(request) + ).andThen( + stop() + ).withName("Increase Top Motor Speed"); } public Command spinDown() { - ShooterConstants.SHOOT_SPEED_BOTTOM = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; - ShooterConstants.SHOOT_SPEED_TOP = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; + ShooterConstants.SHOOT_SPEED_TOP = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; + return runOnce(() -> setRequest(request) + ).andThen( + stop() + ).withName("Lower Top Motor Speed"); } - /** Gets whether the shooter laser detects the "fuel" (balls) */ + /* Gets whether the shooter laser detects the "fuel" (balls) */ private boolean getShooterLaser() { return getMeasurementShooter() < 100; } @@ -114,31 +112,6 @@ public Command topShoot() { ).withName("Top Shooting"); } - public Command bottomShoot() { - return runOnce(() -> setRequest( - new VelocityVoltage(ShooterConstants.SHOOT_SPEED_BOTTOM)) - ).andThen( - Commands.waitUntil(() -> !inRangeShooter), - Commands.waitSeconds(0.15), - stop() - ).withName("Bottom Shooting"); - } - - public Command shoot() - { - - topShoot(); - bottomShoot(); - - return runOnce(() -> setRequest(request) - ).andThen( - Commands.waitUntil(() -> !inRangeShooter), - Commands.waitSeconds(0.15), - stop() - ).withName("Shooting"); - - } - @Override public void initSendable(SendableBuilder builder) { super.initSendable(builder); @@ -147,6 +120,5 @@ public void initSendable(SendableBuilder builder) { () -> lastReadSpeed, null); TelemetryManager.makeSendableTalonFX("/Top", topMotor, builder); - TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); } } \ No newline at end of file From 266fd99429779904141928b264b5d1ff5d54dd53 Mon Sep 17 00:00:00 2001 From: "akhil.space" <145620839+aerospikeengineer@users.noreply.github.com> Date: Tue, 20 Jan 2026 16:22:09 -0800 Subject: [PATCH 30/36] Combined Shooter into one file --- .../subsystems/shooter/BottomShooter.java | 124 ------------------ .../shooter/{TopShooter.java => Shooter.java} | 67 ++++++---- .../subsystems/shooter/ShooterConstants.java | 3 - 3 files changed, 38 insertions(+), 156 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/shooter/BottomShooter.java rename src/main/java/frc/robot/subsystems/shooter/{TopShooter.java => Shooter.java} (69%) diff --git a/src/main/java/frc/robot/subsystems/shooter/BottomShooter.java b/src/main/java/frc/robot/subsystems/shooter/BottomShooter.java deleted file mode 100644 index 4ea3e3b..0000000 --- a/src/main/java/frc/robot/subsystems/shooter/BottomShooter.java +++ /dev/null @@ -1,124 +0,0 @@ -package frc.robot.subsystems.shooter; - -import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import au.grapplerobotics.LaserCan; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.TelemetryManager; - -public class BottomShooter extends SubsystemBase { - private static BottomShooter ShooterInstance; - public static BottomShooter getInstance() { - if (ShooterInstance == null) { - ShooterInstance = new BottomShooter(); - } - return ShooterInstance; - } - - private final TalonFX bottomMotor; - - private final LaserCan shooterLaser; - - private final Debouncer shooterDebouncer; - - private boolean inRangeShooter; - - private double lastReadSpeed; - private ControlRequest request = new NeutralOut(); - - private BottomShooter() { - super(); - - bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); - - shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); - - bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); - bottomMotor.setNeutralMode(NeutralModeValue.Brake); - - shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); - - inRangeShooter = false; - - TelemetryManager.getInstance().addSendable(this); - } - - @Override - public void periodic() { - // Read inputs - lastReadSpeed = bottomMotor.getVelocity().getValueAsDouble(); - inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); - } - - /** Replaces the request */ - private void setRequest(ControlRequest request) { - this.request = request; - } - - /** Stops the shooter */ - public Command stop() { - return runOnce(() -> setRequest(new NeutralOut())).withName("Stopped"); - } - - /** Gets the intake laser measurement */ - private double getMeasurementShooter() { - var measurement = shooterLaser.getMeasurement(); - if (measurement != null) { - return measurement.distance_mm; - } else { - return Double.POSITIVE_INFINITY; - } - } - - public Command spinUp() - { - ShooterConstants.SHOOT_SPEED_BOTTOM = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; - return runOnce(() -> setRequest(request) - ).andThen( - stop() - ).withName("Increase Bottom Motor Speed"); - } - - public Command spinDown() - { - ShooterConstants.SHOOT_SPEED_BOTTOM = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; - return runOnce(() -> setRequest(request) - ).andThen( - stop() - ).withName("Lower Bottom Motor Speed"); - } - - /** Gets whether the shooter laser detects the "fuel" (balls) */ - private boolean getShooterLaser() { - return getMeasurementShooter() < 100; - } - - public Command BottomShoot() { - return runOnce(() -> setRequest( - new VelocityVoltage(ShooterConstants.SHOOT_SPEED_BOTTOM)) - ).andThen( - Commands.waitUntil(() -> !inRangeShooter), - Commands.waitSeconds(0.15), - stop() - ).withName("Bottom Shooting"); - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addDoubleProperty( - "/Speed", - () -> lastReadSpeed, - null); - TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/TopShooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java similarity index 69% rename from src/main/java/frc/robot/subsystems/shooter/TopShooter.java rename to src/main/java/frc/robot/subsystems/shooter/Shooter.java index 5d42a39..1db77a4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TopShooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -15,33 +15,41 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.TelemetryManager; -public class TopShooter extends SubsystemBase { - private static TopShooter ShooterInstance; - public static TopShooter getInstance() { +public class Shooter extends SubsystemBase { + private static Shooter ShooterInstance; + public static Shooter getInstance() { if (ShooterInstance == null) { - ShooterInstance = new TopShooter(); + ShooterInstance = new Shooter(); } return ShooterInstance; } private final TalonFX topMotor; + private final TalonFX bottomMotor; + private final LaserCan shooterLaser; private final Debouncer shooterDebouncer; private boolean inRangeShooter; - private double lastReadSpeed; + private double lastReadSpeedTop; + private double lastReadSpeedBottom; private ControlRequest request = new NeutralOut(); - private TopShooter() { + private Shooter() { super(); - topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); + bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); + bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); + bottomMotor.setNeutralMode(NeutralModeValue.Brake); + + topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); + topMotor.getConfigurator().apply(ShooterConstants.getConfig()); topMotor.setNeutralMode(NeutralModeValue.Brake); @@ -55,7 +63,8 @@ private TopShooter() { @Override public void periodic() { // Read inputs - lastReadSpeed = topMotor.getVelocity().getValueAsDouble(); + lastReadSpeedTop = topMotor.getVelocity().getValueAsDouble(); + lastReadSpeedBottom = bottomMotor.getVelocity().getValueAsDouble(); inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); } @@ -79,32 +88,14 @@ private double getMeasurementShooter() { } } - public Command spinUp() - { - ShooterConstants.SHOOT_SPEED_TOP = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; - return runOnce(() -> setRequest(request) - ).andThen( - stop() - ).withName("Increase Top Motor Speed"); - } - - public Command spinDown() - { - ShooterConstants.SHOOT_SPEED_TOP = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; - return runOnce(() -> setRequest(request) - ).andThen( - stop() - ).withName("Lower Top Motor Speed"); - } - /* Gets whether the shooter laser detects the "fuel" (balls) */ private boolean getShooterLaser() { return getMeasurementShooter() < 100; } - public Command topShoot() { + public Command topShoot(int targetSpeed) { return runOnce(() -> setRequest( - new VelocityVoltage(ShooterConstants.SHOOT_SPEED_TOP)) + new VelocityVoltage(targetSpeed)) ).andThen( Commands.waitUntil(() -> !inRangeShooter), Commands.waitSeconds(0.15), @@ -112,13 +103,31 @@ public Command topShoot() { ).withName("Top Shooting"); } + public Command BottomShoot(int targetSpeed) { + return runOnce(() -> setRequest( + new VelocityVoltage(targetSpeed)) + ).andThen( + Commands.waitUntil(() -> !inRangeShooter), + Commands.waitSeconds(0.15), + stop() + ).withName("Bottom Shooting"); + } + @Override public void initSendable(SendableBuilder builder) { super.initSendable(builder); + builder.addDoubleProperty( "/Speed", - () -> lastReadSpeed, + () -> lastReadSpeedTop, null); TelemetryManager.makeSendableTalonFX("/Top", topMotor, builder); + + builder.addDoubleProperty( + "/Speed", + () -> lastReadSpeedBottom, + null); + TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index ad84a91..cad087f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -6,9 +6,6 @@ import com.ctre.phoenix6.configs.VoltageConfigs; public final class ShooterConstants { - public static double SHOOT_SPEED_TOP = 25; // rotations per second - public static double SHOOT_SPEED_BOTTOM = 25; // rotations per second - public static final double SHOOT_SPEED_INCREMENT = 5; // rotation per second /** Motor ids */ From 84376c524eac3552379a68133a6a2a4f6936a0ca Mon Sep 17 00:00:00 2001 From: "akhil.space" <145620839+aerospikeengineer@users.noreply.github.com> Date: Tue, 20 Jan 2026 16:29:47 -0800 Subject: [PATCH 31/36] Commented out Laser logic We don't need laser right now --- .../frc/robot/subsystems/shooter/Shooter.java | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 1db77a4..4353746 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -6,7 +6,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import au.grapplerobotics.LaserCan; +//import au.grapplerobotics.LaserCan; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.util.sendable.SendableBuilder; @@ -28,7 +28,7 @@ public static Shooter getInstance() { private final TalonFX bottomMotor; - private final LaserCan shooterLaser; + //private final LaserCan shooterLaser; private final Debouncer shooterDebouncer; @@ -43,7 +43,7 @@ private Shooter() { bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); - shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); + //shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); bottomMotor.setNeutralMode(NeutralModeValue.Brake); @@ -55,7 +55,7 @@ private Shooter() { shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); - inRangeShooter = false; + //inRangeShooter = false; TelemetryManager.getInstance().addSendable(this); } @@ -65,7 +65,7 @@ public void periodic() { // Read inputs lastReadSpeedTop = topMotor.getVelocity().getValueAsDouble(); lastReadSpeedBottom = bottomMotor.getVelocity().getValueAsDouble(); - inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); + //inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); } /** Replaces the request */ @@ -78,7 +78,8 @@ public Command stop() { return runOnce(() -> setRequest(new NeutralOut())).withName("Stopped"); } - /** Gets the intake laser measurement */ + /** Gets the shooter laser measurement (Not needed as of now) */ + /* private double getMeasurementShooter() { var measurement = shooterLaser.getMeasurement(); if (measurement != null) { @@ -87,11 +88,13 @@ private double getMeasurementShooter() { return Double.POSITIVE_INFINITY; } } + */ - /* Gets whether the shooter laser detects the "fuel" (balls) */ + /* Gets whether the shooter laser detects the "fuel" (balls) not needed right now private boolean getShooterLaser() { return getMeasurementShooter() < 100; } + */ public Command topShoot(int targetSpeed) { return runOnce(() -> setRequest( From 5df56d9d2227b387d23ddc5fd1ebc10180eae9b6 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Wed, 28 Jan 2026 17:41:48 -0800 Subject: [PATCH 32/36] akhil ran out of memory so i took over his shooter --- .../frc/robot/subsystems/shooter/Shooter.java | 78 ++++++------------- 1 file changed, 23 insertions(+), 55 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 4353746..c82bb52 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -//import au.grapplerobotics.LaserCan; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.util.sendable.SendableBuilder; @@ -25,38 +24,22 @@ public static Shooter getInstance() { } private final TalonFX topMotor; - private final TalonFX bottomMotor; - - //private final LaserCan shooterLaser; - - private final Debouncer shooterDebouncer; - - private boolean inRangeShooter; - private double lastReadSpeedTop; private double lastReadSpeedBottom; - private ControlRequest request = new NeutralOut(); + private ControlRequest topRequest = new NeutralOut(); + private ControlRequest bottomRequest = new NeutralOut(); private Shooter() { super(); bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); - - //shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); - bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); bottomMotor.setNeutralMode(NeutralModeValue.Brake); topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); - topMotor.getConfigurator().apply(ShooterConstants.getConfig()); topMotor.setNeutralMode(NeutralModeValue.Brake); - - shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); - - //inRangeShooter = false; - TelemetryManager.getInstance().addSendable(this); } @@ -65,54 +48,39 @@ public void periodic() { // Read inputs lastReadSpeedTop = topMotor.getVelocity().getValueAsDouble(); lastReadSpeedBottom = bottomMotor.getVelocity().getValueAsDouble(); - //inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); + topMotor.setControl(topRequest); + bottomMotor.setControl(bottomRequest); } /** Replaces the request */ - private void setRequest(ControlRequest request) { - this.request = request; + private void setTopRequest(ControlRequest request) { + this.topRequest = request; + } + + /** Replaces the request */ + private void setBottomRequest(ControlRequest request) { + this.bottomRequest = request; } /** Stops the shooter */ public Command stop() { - return runOnce(() -> setRequest(new NeutralOut())).withName("Stopped"); - } - - /** Gets the shooter laser measurement (Not needed as of now) */ - /* - private double getMeasurementShooter() { - var measurement = shooterLaser.getMeasurement(); - if (measurement != null) { - return measurement.distance_mm; - } else { - return Double.POSITIVE_INFINITY; - } - } - */ - - /* Gets whether the shooter laser detects the "fuel" (balls) not needed right now - private boolean getShooterLaser() { - return getMeasurementShooter() < 100; + return runOnce( + () -> { + setTopRequest(new NeutralOut()); + setBottomRequest(new NeutralOut()); + } + ).withName("Stopped"); } - */ - public Command topShoot(int targetSpeed) { - return runOnce(() -> setRequest( + public Command topShoot(double targetSpeed) { + return runOnce(() -> setTopRequest( new VelocityVoltage(targetSpeed)) - ).andThen( - Commands.waitUntil(() -> !inRangeShooter), - Commands.waitSeconds(0.15), - stop() ).withName("Top Shooting"); } - public Command BottomShoot(int targetSpeed) { - return runOnce(() -> setRequest( + public Command bottomShoot(double targetSpeed) { + return runOnce(() -> setBottomRequest( new VelocityVoltage(targetSpeed)) - ).andThen( - Commands.waitUntil(() -> !inRangeShooter), - Commands.waitSeconds(0.15), - stop() ).withName("Bottom Shooting"); } @@ -121,13 +89,13 @@ public void initSendable(SendableBuilder builder) { super.initSendable(builder); builder.addDoubleProperty( - "/Speed", + "/TopSpeed", () -> lastReadSpeedTop, null); TelemetryManager.makeSendableTalonFX("/Top", topMotor, builder); builder.addDoubleProperty( - "/Speed", + "/BottomSpeed", () -> lastReadSpeedBottom, null); TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); From fc777a50b075503a81b99c893f93d9f56aa388aa Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 31 Jan 2026 09:28:56 -0800 Subject: [PATCH 33/36] added sim Hopefully this works here we go --- src/main/java/frc/robot/Robot.java | 2 + .../frc/robot/subsystems/shooter/Shooter.java | 46 +++++++++++++++++-- .../subsystems/shooter/ShooterConstants.java | 14 +----- 3 files changed, 47 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0b73ec1..0dc0a42 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -21,6 +21,7 @@ import frc.robot.Constants.Controllers; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.*; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.vision.VisionDeviceManager; /** @@ -46,6 +47,7 @@ public Robot() { VisionDeviceManager.getInstance(); } Drive.getInstance(); + Shooter.getInstance(); TelemetryManager.getInstance(); FollowPathCommand.warmupCommand().schedule(); autoChooser = new AutoSelector(); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index c82bb52..b233d4d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -6,12 +6,15 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; import frc.robot.subsystems.TelemetryManager; public class Shooter extends SubsystemBase { @@ -30,6 +33,9 @@ public static Shooter getInstance() { private ControlRequest topRequest = new NeutralOut(); private ControlRequest bottomRequest = new NeutralOut(); + private FlywheelSim topSim; + private FlywheelSim bottomSim; + private Shooter() { super(); @@ -40,6 +46,21 @@ private Shooter() { topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); topMotor.getConfigurator().apply(ShooterConstants.getConfig()); topMotor.setNeutralMode(NeutralModeValue.Brake); + + if (Robot.isSimulation()) { + topSim = new FlywheelSim( + LinearSystemId.createFlywheelSystem( + DCMotor.getKrakenX60(1), + 0.000489000861, + 1 + ), DCMotor.getKrakenX60(1), 0.0); + bottomSim = new FlywheelSim( + LinearSystemId.createFlywheelSystem( + DCMotor.getKrakenX60(1), + 0.000489000861, + 1 + ), DCMotor.getKrakenX60(1), 0.0); + } TelemetryManager.getInstance().addSendable(this); } @@ -52,6 +73,25 @@ public void periodic() { bottomMotor.setControl(bottomRequest); } + @Override + public void simulationPeriodic() { + topMotor.getSimState() + .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0); + topMotor.getSimState().setSupplyVoltage(12.0); + topSim.setInput(topMotor.getMotorVoltage().getValueAsDouble()); + topSim.update(0.020); + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(topSim.getCurrentDrawAmps())); + + bottomMotor.getSimState() + .setRotorVelocity(bottomSim.getAngularVelocityRPM() / 60.0); + bottomMotor.getSimState().setSupplyVoltage(12.0); + bottomSim.setInput(bottomMotor.getMotorVoltage().getValueAsDouble()); + bottomSim.update(0.020); + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(bottomSim.getCurrentDrawAmps())); + } + /** Replaces the request */ private void setTopRequest(ControlRequest request) { this.topRequest = request; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index cad087f..61fad82 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -6,7 +6,7 @@ import com.ctre.phoenix6.configs.VoltageConfigs; public final class ShooterConstants { - public static final double SHOOT_SPEED_INCREMENT = 5; // rotation per second + public static final double GEAR_RATIO = 1; /** Motor ids */ public static enum Motors { @@ -17,17 +17,7 @@ private Motors(int id) { this.id = id; } } - - /** Laser ids */ - public static enum Lasers { - //BACK(30), - FRONT(31); - public final int id; - private Lasers(int id) { - this.id = id; - } - } - + /** Config for shooter motors */ public static TalonFXConfiguration getConfig() { return new TalonFXConfiguration() From 6938988817d6e27c38b649047e51e22fe42df3dc Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 31 Jan 2026 10:44:23 -0800 Subject: [PATCH 34/36] Shooter simulation working --- .vscode/launch.json | 24 +------ simgui-ds.json | 7 ++- src/main/java/frc/robot/ControlsMapping.java | 3 + .../lib/control/ProfiledPIDVController.java | 18 +++--- .../frc/robot/subsystems/shooter/Shooter.java | 54 +++++++++------- .../subsystems/shooter/ShooterConstants.java | 13 ++-- ...enix6-26.1.0.json => Phoenix6-26.1.1.json} | 62 +++++++++---------- ...plefrc2025.json => libgrapplefrc2026.json} | 16 ++--- 8 files changed, 96 insertions(+), 101 deletions(-) rename vendordeps/{Phoenix6-26.1.0.json => Phoenix6-26.1.1.json} (92%) rename vendordeps/{libgrapplefrc2025.json => libgrapplefrc2026.json} (87%) diff --git a/.vscode/launch.json b/.vscode/launch.json index 396fe88..e6ca564 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -18,26 +18,4 @@ "desktop": false, } ] -} - -{ - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 - "version": "0.2.0", - "configurations": [ - - { - "type": "wpilib", - "name": "WPILib Desktop Debug", - "request": "launch", - "desktop": true, - }, - { - "type": "wpilib", - "name": "WPILib roboRIO Debug", - "request": "launch", - "desktop": false, - } - ] -} +} \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json index 0bea79e..1486df5 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -16,10 +16,15 @@ "incKey": 68 }, { + "decKey": 50, "decayRate": 0.0, + "incKey": 49, "keyRate": 0.009999999776482582 }, - {}, + { + "decKey": 52, + "incKey": 51 + }, { "decKey": 74, "incKey": 76 diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index e40c703..045e581 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; +import frc.robot.subsystems.shooter.Shooter; import edu.wpi.first.wpilibj2.command.Commands; public class ControlsMapping { @@ -21,6 +22,8 @@ public static void mapTeleopCommand() { controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); controller.x().whileTrue(Drive.getInstance().autopilotAlign(true)); controller.y().whileTrue(Drive.getInstance().autopilotAlign(false)); + + controller.rightTrigger().whileTrue(Shooter.getInstance().shoot()); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java index 2e76a9c..a256ae9 100644 --- a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java +++ b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java @@ -55,17 +55,17 @@ public ProfiledPIDVController setMeasurement(double position, double speed) { } public double getOutput() { - // if (controller.isContinuous) { - // double errorBound = (controller.maxRange - controller.minRange) / 2.0; + if (controller.isContinuous) { + double errorBound = (controller.maxRange - controller.minRange) / 2.0; - // double goalDelta = - // MathUtil.inputModulus(goal.position - controller.positionMeasurement, -errorBound, errorBound); - // double setpointDelta = - // MathUtil.inputModulus(setpoint.position - controller.positionMeasurement, -errorBound, errorBound); + double goalDelta = + MathUtil.inputModulus(goal.position - controller.positionMeasurement, -errorBound, errorBound); + double setpointDelta = + MathUtil.inputModulus(setpoint.position - controller.positionMeasurement, -errorBound, errorBound); - // goal.position = goalDelta + controller.positionMeasurement; - // setpoint.position = setpointDelta + controller.positionMeasurement; - // } + goal.position = goalDelta + controller.positionMeasurement; + setpoint.position = setpointDelta + controller.positionMeasurement; + } // Advance profile by one timestep setpoint = profile.calculate(Constants.DT, setpoint, goal); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b233d4d..692c6c3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.shooter; +import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -9,10 +10,12 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.subsystems.TelemetryManager; @@ -41,11 +44,11 @@ private Shooter() { bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); - bottomMotor.setNeutralMode(NeutralModeValue.Brake); + bottomMotor.setNeutralMode(NeutralModeValue.Coast); topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); topMotor.getConfigurator().apply(ShooterConstants.getConfig()); - topMotor.setNeutralMode(NeutralModeValue.Brake); + topMotor.setNeutralMode(NeutralModeValue.Coast); if (Robot.isSimulation()) { topSim = new FlywheelSim( @@ -75,21 +78,26 @@ public void periodic() { @Override public void simulationPeriodic() { + topMotor.getSimState().setSupplyVoltage(12); + bottomMotor.getSimState().setSupplyVoltage(12); + + topSim.setInput(topMotor.getSimState().getMotorVoltage()); + topSim.update(0.020); topMotor.getSimState() .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0); - topMotor.getSimState().setSupplyVoltage(12.0); - topSim.setInput(topMotor.getMotorVoltage().getValueAsDouble()); - topSim.update(0.020); + topMotor.getSimState().addRotorPosition(topSim.getAngularVelocityRPM() / 60.0 * 0.020); RoboRioSim.setVInVoltage( BatterySim.calculateDefaultBatteryLoadedVoltage(topSim.getCurrentDrawAmps())); - bottomMotor.getSimState() - .setRotorVelocity(bottomSim.getAngularVelocityRPM() / 60.0); - bottomMotor.getSimState().setSupplyVoltage(12.0); - bottomSim.setInput(bottomMotor.getMotorVoltage().getValueAsDouble()); + bottomSim.setInput(bottomMotor.getSimState().getMotorVoltage()); bottomSim.update(0.020); RoboRioSim.setVInVoltage( BatterySim.calculateDefaultBatteryLoadedVoltage(bottomSim.getCurrentDrawAmps())); + bottomMotor.getSimState() + .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0); + bottomMotor.getSimState().addRotorPosition(topSim.getAngularVelocityRPM() / 60.0 * 0.020); + + setDefaultCommand(stop()); } /** Replaces the request */ @@ -106,22 +114,21 @@ private void setBottomRequest(ControlRequest request) { public Command stop() { return runOnce( () -> { - setTopRequest(new NeutralOut()); - setBottomRequest(new NeutralOut()); + setTopRequest(new CoastOut()); + setBottomRequest(new CoastOut()); } ).withName("Stopped"); } - public Command topShoot(double targetSpeed) { - return runOnce(() -> setTopRequest( - new VelocityVoltage(targetSpeed)) - ).withName("Top Shooting"); + public Command shoot(double topSpeed, double bottomSpeed) { + return runOnce(() -> { + setTopRequest(new VelocityVoltage(topSpeed)); + setBottomRequest(new VelocityVoltage(bottomSpeed)); + }).andThen(Commands.repeatingSequence(Commands.none())).withName("Shooting"); } - public Command bottomShoot(double targetSpeed) { - return runOnce(() -> setBottomRequest( - new VelocityVoltage(targetSpeed)) - ).withName("Bottom Shooting"); + public Command shoot() { + return shoot(3000 / 60.0, 3000 / 60.0); } @Override @@ -129,16 +136,15 @@ public void initSendable(SendableBuilder builder) { super.initSendable(builder); builder.addDoubleProperty( - "/TopSpeed", + "TopSpeed", () -> lastReadSpeedTop, null); - TelemetryManager.makeSendableTalonFX("/Top", topMotor, builder); + TelemetryManager.makeSendableTalonFX("Top", topMotor, builder); builder.addDoubleProperty( - "/BottomSpeed", + "BottomSpeed", () -> lastReadSpeedBottom, null); - TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); - + TelemetryManager.makeSendableTalonFX("Bottom", bottomMotor, builder); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 61fad82..657827e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -23,12 +23,15 @@ public static TalonFXConfiguration getConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) - .withKP(0.3) - .withKI(0.0) - .withKD(0.0)) + .withKP(0.5) + .withKI(0.001) + .withKD(0.0) + .withKA(0.1) + .withKS(0.1) + .withKV(0.1)) // placeholder values .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(30) - .withSupplyCurrentLimit(30)) + .withStatorCurrentLimit(60) + .withSupplyCurrentLimit(60)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) .withPeakReverseVoltage(-12.0)); diff --git a/vendordeps/Phoenix6-26.1.0.json b/vendordeps/Phoenix6-26.1.1.json similarity index 92% rename from vendordeps/Phoenix6-26.1.0.json rename to vendordeps/Phoenix6-26.1.1.json index dc5dc62..7a0eca0 100644 --- a/vendordeps/Phoenix6-26.1.0.json +++ b/vendordeps/Phoenix6-26.1.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.1.0.json", + "fileName": "Phoenix6-26.1.1.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", + "version": "26.1.1", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.1.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2026.json similarity index 87% rename from vendordeps/libgrapplefrc2025.json rename to vendordeps/libgrapplefrc2026.json index 508fd7b..2f620b5 100644 --- a/vendordeps/libgrapplefrc2025.json +++ b/vendordeps/libgrapplefrc2026.json @@ -1,25 +1,25 @@ { - "fileName": "libgrapplefrc2025.json", + "fileName": "libgrapplefrc2026.json", "name": "libgrapplefrc", - "version": "2025.0.8", - "frcYear": "2025", + "version": "2026.0.0", + "frcYear": "2026", "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", "mavenUrls": [ "https://storage.googleapis.com/grapple-frc-maven" ], - "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2026.json", "javaDependencies": [ { "groupId": "au.grapplerobotics", "artifactId": "libgrapplefrcjava", - "version": "2025.0.8" + "version": "2026.0.0" } ], "jniDependencies": [ { "groupId": "au.grapplerobotics", "artifactId": "libgrapplefrcdriver", - "version": "2025.0.8", + "version": "2026.0.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "au.grapplerobotics", "artifactId": "libgrapplefrccpp", - "version": "2025.0.8", + "version": "2026.0.0", "libName": "grapplefrc", "headerClassifier": "headers", "sharedLibrary": true, @@ -53,7 +53,7 @@ { "groupId": "au.grapplerobotics", "artifactId": "libgrapplefrcdriver", - "version": "2025.0.8", + "version": "2026.0.0", "libName": "grapplefrcdriver", "headerClassifier": "headers", "sharedLibrary": true, From 780135fc1597e69b0327b90676ed3573cf482434 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Mon, 2 Feb 2026 17:00:33 -0800 Subject: [PATCH 35/36] hi it has been tested --- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 4 ++-- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 692c6c3..414d0cc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -65,6 +65,7 @@ private Shooter() { ), DCMotor.getKrakenX60(1), 0.0); } TelemetryManager.getInstance().addSendable(this); + setDefaultCommand(stop()); } @Override @@ -97,7 +98,6 @@ public void simulationPeriodic() { .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0); bottomMotor.getSimState().addRotorPosition(topSim.getAngularVelocityRPM() / 60.0 * 0.020); - setDefaultCommand(stop()); } /** Replaces the request */ @@ -128,7 +128,7 @@ public Command shoot(double topSpeed, double bottomSpeed) { } public Command shoot() { - return shoot(3000 / 60.0, 3000 / 60.0); + return shoot(512, -512); } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 657827e..32d9c8f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -23,7 +23,7 @@ public static TalonFXConfiguration getConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) - .withKP(0.5) + .withKP(0.3 ) .withKI(0.001) .withKD(0.0) .withKA(0.1) From 0f57de3b3f2e2f878b24f817b6f9563d32dffa1c Mon Sep 17 00:00:00 2001 From: shakability Date: Wed, 11 Feb 2026 17:44:47 -0800 Subject: [PATCH 36/36] there's two now --- .../frc/robot/subsystems/shooter/Shooter.java | 36 ++++++++++++++----- .../subsystems/shooter/ShooterConstants.java | 6 ++-- 2 files changed, 32 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 414d0cc..26aa81b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -21,14 +21,22 @@ import frc.robot.subsystems.TelemetryManager; public class Shooter extends SubsystemBase { - private static Shooter ShooterInstance; - public static Shooter getInstance() { - if (ShooterInstance == null) { - ShooterInstance = new Shooter(); + private static Shooter shooterLeftInstance; + private static Shooter shooterRightInstance; + public static Shooter getLeftInstance() { + if (shooterLeftInstance == null) { + shooterLeftInstance = new Shooter(true); } - return ShooterInstance; + return shooterLeftInstance; } + + public static Shooter getRightInstance() { + if (shooterRightInstance == null) { + shooterRightInstance = new Shooter(false); + } + return shooterRightInstance; + } private final TalonFX topMotor; private final TalonFX bottomMotor; private double lastReadSpeedTop; @@ -39,14 +47,26 @@ public static Shooter getInstance() { private FlywheelSim topSim; private FlywheelSim bottomSim; - private Shooter() { + private Shooter(boolean left) { super(); - bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); + int bottomID; + int topID; + + if (left) { + bottomID = ShooterConstants.Motors.BOTTOMLEFT.id; + topID = ShooterConstants.Motors.TOPLEFT.id; + } + else { + bottomID = ShooterConstants.Motors.BOTTOMRIGHT.id; + topID = ShooterConstants.Motors.TOPRIGHT.id; + } + + bottomMotor = new TalonFX(bottomID); bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); bottomMotor.setNeutralMode(NeutralModeValue.Coast); - topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); + topMotor = new TalonFX(topID); topMotor.getConfigurator().apply(ShooterConstants.getConfig()); topMotor.setNeutralMode(NeutralModeValue.Coast); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 32d9c8f..9bca810 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -10,8 +10,10 @@ public final class ShooterConstants { /** Motor ids */ public static enum Motors { - TOP(12), - BOTTOM(13); + TOPLEFT(12), + BOTTOMLEFT(13), + TOPRIGHT(12), + BOTTOMRIGHT(13); public final int id; private Motors(int id) { this.id = id;