idk?

TitaniumTigers4829 · Sep 13, 2024 · a23f0df · a23f0df
1 parent 65c0f08
commit a23f0df
Show file tree

Hide file tree

Showing 5 changed files with 539 additions and 560 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -10,7 +10,6 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc.robot.subsystems.vision.VisionSubsystem;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -21,7 +20,8 @@
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
   private RobotContainer m_robotContainer;
-  private VisionSubsystem visionSubsystem;
+
+  // private VisionSubsystem visionSubsystem;
 
   /**
    * This function is run when the robot is first started up and should be used for any
@@ -32,7 +32,7 @@ public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
-    visionSubsystem = new VisionSubsystem();
+    // visionSubsystem = new VisionSubsystem();
     //    uncomment the line below if there is a USB camera plugged into the RoboRIO
     //    CameraServer.startAutomaticCapture();
   }
@@ -56,7 +56,7 @@ public void robotPeriodic() {
   /** This function is called once each time the robot enters Disabled mode. */
   @Override
   public void disabledInit() {
-    visionSubsystem.endAllThreads();
+    // visionSubsystem.endAllThreads();
   }
 
   @Override

diff --git a/src/main/java/frc/robot/extras/utils/EqualsUtil.java b/src/main/java/frc/robot/extras/utils/EqualsUtil.java
@@ -9,9 +9,7 @@
 
 import edu.wpi.first.math.geometry.Twist2d;
 
-/**
- * Checks if value a is very close to b.
- */
+/** Checks if value a is very close to b. */
 public class EqualsUtil {
   public static boolean epsilonEquals(double a, double b, double epsilon) {
     return (a - epsilon <= b) && (a + epsilon >= b);

diff --git a/src/main/java/frc/robot/extras/utils/GeomUtil.java b/src/main/java/frc/robot/extras/utils/GeomUtil.java
@@ -2,8 +2,6 @@
 
 package frc.robot.extras.utils;
 
-import frc.robot.Constants.FieldConstants;
-
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -14,154 +12,147 @@
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.geometry.Twist3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import frc.robot.Constants.FieldConstants;
 
-/**
- * Geometry utilities for working with translations, rotations, transforms, and
- * poses.
- */
+/** Geometry utilities for working with translations, rotations, transforms, and poses. */
 public class GeomUtil {
-    public static final Translation2d TRANSLATION2D_ZERO = new Translation2d();
-    public static final Translation2d TRANSLATION2D_CENTER = new Translation2d(
-        FieldConstants.FIELD_LENGTH_METERS / 2.0,
-        FieldConstants.FIELD_WIDTH_METERS / 2.0
-    );
-    public static final Rotation2d ROTATION2D_ZERO = new Rotation2d();
-    public static final Rotation2d ROTATION2D_PI = Rotation2d.fromDegrees(180.0);
-    public static final Pose2d POSE2D_ZERO = new Pose2d();
-    public static final Pose2d POSE2D_CENTER = new Pose2d(TRANSLATION2D_CENTER, ROTATION2D_ZERO);
-    public static final Transform2d TRANSFORM2D_ZERO = new Transform2d();
-    public static final Translation3d TRANSLATION3D_ZERO = new Translation3d();
-    public static final Rotation2d ROTATION3D_ZERO = new Rotation2d();
-    public static final Transform3d TRANSFORM3D_ZERO = new Transform3d();
-    public static final Pose3d POSE3D_ZERO = new Pose3d();
-    public static final Twist2d TWIST2D_ZERO = new Twist2d();
-    public static final Twist3d TWIST3D_ZERO = new Twist3d();
-
-    /**
-     * Creates a pure translating transform
-     *
-     * @param translation The translation to create the transform with
-     * @return The resulting transform
-     */
-    public static Transform2d toTransform2d(Translation2d translation) {
-        return new Transform2d(translation, new Rotation2d());
-    }
-
-    /**
-     * Creates a pure rotating transform
-     *
-     * @param rotation The rotation to create the transform with
-     * @return The resulting transform
-     */
-    public static Transform2d toTransform2d(Rotation2d rotation) {
-        return new Transform2d(new Translation2d(), rotation);
-    }
-
-    /**
-     * Converts a Pose2d to a Transform2d to be used in a kinematic chain
-     *
-     * @param pose The pose that will represent the transform
-     * @return The resulting transform
-     */
-    public static Transform2d toTransform2d(Pose2d pose) {
-        return new Transform2d(pose.getTranslation(), pose.getRotation());
-    }
-
-    public static Pose2d inverse(Pose2d pose) {
-        Rotation2d rotationInverse = pose.getRotation().unaryMinus();
-        return new Pose2d(
-                pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse);
-    }
-
-    /**
-     * Converts a Transform2d to a Pose2d to be used as a position or as the start
-     * of a kinematic
-     * chain
-     *
-     * @param transform The transform that will represent the pose
-     * @return The resulting pose
-     */
-    public static Pose2d toPose2d(Transform2d transform) {
-        return new Pose2d(transform.getTranslation(), transform.getRotation());
-    }
-
-    /**
-     * Creates a pure translated pose
-     *
-     * @param translation The translation to create the pose with
-     * @return The resulting pose
-     */
-    public static Pose2d toPose2d(Translation2d translation) {
-        return new Pose2d(translation, new Rotation2d());
-    }
-
-    /**
-     * Creates a pure rotated pose
-     *
-     * @param rotation The rotation to create the pose with
-     * @return The resulting pose
-     */
-    public static Pose2d toPose2d(Rotation2d rotation) {
-        return new Pose2d(new Translation2d(), rotation);
-    }
-
-    /**
-     * Multiplies a twist by a scaling factor
-     *
-     * @param twist  The twist to multiply
-     * @param factor The scaling factor for the twist components
-     * @return The new twist
-     */
-    public static Twist2d multiply(Twist2d twist, double factor) {
-        return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor);
-    }
-
-    /**
-     * Converts a Pose3d to a Transform3d to be used in a kinematic chain
-     *
-     * @param pose The pose that will represent the transform
-     * @return The resulting transform
-     */
-    public static Transform3d toTransform3d(Pose3d pose) {
-        return new Transform3d(pose.getTranslation(), pose.getRotation());
-    }
-
-    /**
-     * Converts a Transform3d to a Pose3d to be used as a position or as the start
-     * of a kinematic
-     * chain
-     *
-     * @param transform The transform that will represent the pose
-     * @return The resulting pose
-     */
-    public static Pose3d toPose3d(Transform3d transform) {
-        return new Pose3d(transform.getTranslation(), transform.getRotation());
-    }
-
-    /**
-     * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z).
-     * chain
-     *
-     * @param speeds The original translation
-     * @return The resulting translation
-     */
-    public static Twist2d toTwist2d(ChassisSpeeds speeds) {
-        return new Twist2d(
-                speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond);
-    }
-
-    /**
-     * Gets the angle between two points
-     * 
-     * @param currentTrans The current translation
-     * @param pose The pose to get the angle to
-     * @param angleOffet An offset to add to the angle
-     * @return The angle between the two points
-     */
-    public static Rotation2d rotationRelativeToPose(Translation2d currentTrans, Translation2d pose) {
-        double angleBetween = Math.atan2(
-                pose.getY() - currentTrans.getY(),
-                pose.getX() - currentTrans.getX());
-        return Rotation2d.fromRadians(angleBetween);
-    }
-}
+  public static final Translation2d TRANSLATION2D_ZERO = new Translation2d();
+  public static final Translation2d TRANSLATION2D_CENTER =
+      new Translation2d(
+          FieldConstants.FIELD_LENGTH_METERS / 2.0, FieldConstants.FIELD_WIDTH_METERS / 2.0);
+  public static final Rotation2d ROTATION2D_ZERO = new Rotation2d();
+  public static final Rotation2d ROTATION2D_PI = Rotation2d.fromDegrees(180.0);
+  public static final Pose2d POSE2D_ZERO = new Pose2d();
+  public static final Pose2d POSE2D_CENTER = new Pose2d(TRANSLATION2D_CENTER, ROTATION2D_ZERO);
+  public static final Transform2d TRANSFORM2D_ZERO = new Transform2d();
+  public static final Translation3d TRANSLATION3D_ZERO = new Translation3d();
+  public static final Rotation2d ROTATION3D_ZERO = new Rotation2d();
+  public static final Transform3d TRANSFORM3D_ZERO = new Transform3d();
+  public static final Pose3d POSE3D_ZERO = new Pose3d();
+  public static final Twist2d TWIST2D_ZERO = new Twist2d();
+  public static final Twist3d TWIST3D_ZERO = new Twist3d();
+
+  /**
+   * Creates a pure translating transform
+   *
+   * @param translation The translation to create the transform with
+   * @return The resulting transform
+   */
+  public static Transform2d toTransform2d(Translation2d translation) {
+    return new Transform2d(translation, new Rotation2d());
+  }
+
+  /**
+   * Creates a pure rotating transform
+   *
+   * @param rotation The rotation to create the transform with
+   * @return The resulting transform
+   */
+  public static Transform2d toTransform2d(Rotation2d rotation) {
+    return new Transform2d(new Translation2d(), rotation);
+  }
+
+  /**
+   * Converts a Pose2d to a Transform2d to be used in a kinematic chain
+   *
+   * @param pose The pose that will represent the transform
+   * @return The resulting transform
+   */
+  public static Transform2d toTransform2d(Pose2d pose) {
+    return new Transform2d(pose.getTranslation(), pose.getRotation());
+  }
+
+  public static Pose2d inverse(Pose2d pose) {
+    Rotation2d rotationInverse = pose.getRotation().unaryMinus();
+    return new Pose2d(
+        pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse);
+  }
+
+  /**
+   * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic
+   * chain
+   *
+   * @param transform The transform that will represent the pose
+   * @return The resulting pose
+   */
+  public static Pose2d toPose2d(Transform2d transform) {
+    return new Pose2d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Creates a pure translated pose
+   *
+   * @param translation The translation to create the pose with
+   * @return The resulting pose
+   */
+  public static Pose2d toPose2d(Translation2d translation) {
+    return new Pose2d(translation, new Rotation2d());
+  }
+
+  /**
+   * Creates a pure rotated pose
+   *
+   * @param rotation The rotation to create the pose with
+   * @return The resulting pose
+   */
+  public static Pose2d toPose2d(Rotation2d rotation) {
+    return new Pose2d(new Translation2d(), rotation);
+  }
+
+  /**
+   * Multiplies a twist by a scaling factor
+   *
+   * @param twist The twist to multiply
+   * @param factor The scaling factor for the twist components
+   * @return The new twist
+   */
+  public static Twist2d multiply(Twist2d twist, double factor) {
+    return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor);
+  }
+
+  /**
+   * Converts a Pose3d to a Transform3d to be used in a kinematic chain
+   *
+   * @param pose The pose that will represent the transform
+   * @return The resulting transform
+   */
+  public static Transform3d toTransform3d(Pose3d pose) {
+    return new Transform3d(pose.getTranslation(), pose.getRotation());
+  }
+
+  /**
+   * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic
+   * chain
+   *
+   * @param transform The transform that will represent the pose
+   * @return The resulting pose
+   */
+  public static Pose3d toPose3d(Transform3d transform) {
+    return new Pose3d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain
+   *
+   * @param speeds The original translation
+   * @return The resulting translation
+   */
+  public static Twist2d toTwist2d(ChassisSpeeds speeds) {
+    return new Twist2d(
+        speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond);
+  }
+
+  /**
+   * Gets the angle between two points
+   *
+   * @param currentTrans The current translation
+   * @param pose The pose to get the angle to
+   * @param angleOffet An offset to add to the angle
+   * @return The angle between the two points
+   */
+  public static Rotation2d rotationRelativeToPose(Translation2d currentTrans, Translation2d pose) {
+    double angleBetween =
+        Math.atan2(pose.getY() - currentTrans.getY(), pose.getX() - currentTrans.getX());
+    return Rotation2d.fromRadians(angleBetween);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/DriveSubsystem.java
@@ -18,11 +18,10 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.DriveConstants;
-import frc.robot.Constants.HardwareConstants;
 import frc.robot.Constants.TrajectoryConstants;
 import frc.robot.Constants.VisionConstants;
 import frc.robot.extras.SmarterDashboardRegistry;
-import frc.robot.subsystems.swerve.SwerveSetpointGenerator;
+import frc.robot.subsystems.swerve.SwerveSetpointGenerator.SwerveSetpoint;
 import java.util.Optional;
 
 public class DriveSubsystem extends SubsystemBase {
@@ -117,7 +116,7 @@ public DriveSubsystem() {
 
     alliance = DriverStation.getAlliance();
 
-    setpointGenerator = new SwerveSetpointGenerator();
+    swerveSetpointGenerator = new SwerveSetpointGenerator();
 
     // Configure AutoBuilder
     AutoBuilder.configureHolonomic(
@@ -154,10 +153,10 @@ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fi
             ? ChassisSpeeds.fromFieldRelativeSpeeds(
                 xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d())
             : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed);
-    currentSetpoint =
-        setpointGenerator.generateSetpoint(desiredSpeeds);
+    SwerveSetpoint currentSetpoint = swerveSetpointGenerator.generateSetpoint(desiredSpeeds);
 
-    setModuleStates(DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(currentSetpoint.chassisSpeeds()));
+    setModuleStates(
+        DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(currentSetpoint.chassisSpeeds()));
   }
 
   public void drive(ChassisSpeeds speeds) {