Brown protection go!

simgui-ds.json

@@ -125,4 +125,4 @@
       "guid": "Keyboard1"
     }
   ]
-}
+}

src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java

@@ -378,21 +378,6 @@ public boolean isNear(Pose2d target, Distance translationTolerance, Angle angula
         && angularError <= angularTolerance.in(Radians);
   }
 
-  /**
-   * Checks if the robot is within a translational tolerance of a target pose.
-   *
-   * @param target The target pose to check against
-   * @param translationTolerance Maximum allowed translation error from target position
-   * @return {@code true} if within the translational tolerance, {@code false} otherwise
-   */
-  public boolean isNear(Pose2d target, Distance translationTolerance) {
-    Pose2d current = getPosition2d();
-
-    double linearError = current.getTranslation().getDistance(target.getTranslation());
-
-    return linearError <= translationTolerance.in(Meters);
-  }
-
   /**
    * Gets the current field-relative velocity of the robot as a {@link ChassisSpeeds} object.
    *

src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java

@@ -1,10 +1,8 @@
 package com.team6962.lib.swerve.commands;
 
 import static edu.wpi.first.units.Units.Hertz;
-import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
-import static edu.wpi.first.units.Units.Seconds;
 
 import com.team6962.lib.control.MotionProfile;
 import com.team6962.lib.control.ProfiledController;
@@ -13,15 +11,11 @@
 import com.team6962.lib.math.AngleMath;
 import com.team6962.lib.math.TranslationalVelocity;
 import com.team6962.lib.swerve.CommandSwerveDrive;
-import dev.doglog.DogLog;
-import edu.wpi.first.math.Vector;
 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.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj2.command.Command;
 
@@ -187,13 +181,6 @@ public void initialize() {
 
     // Generate initial motion profiles
     createMotionProfiles();
-
-    DogLog.log(
-        "Drivetrain/DriveToState/FinalTarget",
-        new Pose2d(target.translation, new Rotation2d(target.angle)));
-    DogLog.log("Drivetrain/DriveToState/FinalVelocityX", target.translationalVelocity.x);
-    DogLog.log("Drivetrain/DriveToState/FinalVelocityY", target.translationalVelocity.y);
-    DogLog.log("Drivetrain/DriveToState/FinalAngularVelocity", target.angularVelocity);
   }
 
   /** Creates motion profiles for translation and rotation controllers. */
@@ -240,38 +227,16 @@ public void execute() {
       createMotionProfiles();
     }
 
-    // Current target pose and speeds for logging
+    // Current target pose for logging
     Pose2d currentTarget = new Pose2d();
-    ChassisSpeeds currentSpeeds = new ChassisSpeeds();
 
     // Calculate and apply velocity commands for translation and rotation
     if (translationController != null) {
-      TranslationalVelocity currentTranslationalVelocity =
+      TranslationalVelocity outputTranslationalVelocity =
           translationController.calculate(
               swerveDrive.getPosition2d().getTranslation(), swerveDrive.getTranslationalVelocity());
 
-      currentSpeeds =
-          new ChassisSpeeds(
-              currentTranslationalVelocity.x.in(MetersPerSecond),
-              currentTranslationalVelocity.y.in(MetersPerSecond),
-              currentSpeeds.omegaRadiansPerSecond);
-
-      TranslationalVelocity profileTranslationalVelocity = translationController.sample(0);
-
-      TranslationalVelocity nextTranslationalVelocity = translationController.sample(0.02);
-
-      Vector<N2> acceleration =
-          nextTranslationalVelocity
-              .toVector()
-              .minus(profileTranslationalVelocity.toVector())
-              .div(0.02);
-
-      currentTranslationalVelocity =
-          currentTranslationalVelocity.plus(
-              new TranslationalVelocity(acceleration)
-                  .times(swerveDrive.getConstants().Driving.AutoLinearAccelerationScalar));
-
-      swerveDrive.applyVelocityMotion(currentTranslationalVelocity);
+      swerveDrive.applyVelocityMotion(outputTranslationalVelocity);
 
       // Add translation to current target pose
       currentTarget =
@@ -280,32 +245,14 @@ public void execute() {
     }
 
     if (headingController != null) {
-      MotionProfile.State angularState =
-          new MotionProfile.State(
-              swerveDrive.getYaw().in(Radians), swerveDrive.getYawVelocity().in(RadiansPerSecond));
+      AngularVelocity outputAngularVelocity =
+          RadiansPerSecond.of(
+              headingController.calculate(
+                  new MotionProfile.State(
+                      swerveDrive.getYaw().in(Radians),
+                      swerveDrive.getYawVelocity().in(RadiansPerSecond))));
 
-      AngularVelocity currentAngularVelocity =
-          RadiansPerSecond.of(headingController.calculate(angularState));
-
-      currentSpeeds =
-          new ChassisSpeeds(
-              currentSpeeds.vxMetersPerSecond,
-              currentSpeeds.vyMetersPerSecond,
-              currentAngularVelocity.in(RadiansPerSecond));
-
-      AngularVelocity profileAngularVelocity = RadiansPerSecond.of(headingController.sample(0));
-
-      AngularVelocity nextAngularVelocity = RadiansPerSecond.of(headingController.sample(0.02));
-
-      AngularAcceleration angularAcceleration =
-          nextAngularVelocity.minus(profileAngularVelocity).div(Seconds.of(0.02));
-
-      currentAngularVelocity =
-          currentAngularVelocity.plus(
-              angularAcceleration.times(
-                  Seconds.of(swerveDrive.getConstants().Driving.AutoAngularAccelerationScalar)));
-
-      swerveDrive.applyVelocityMotion(currentAngularVelocity);
+      swerveDrive.applyVelocityMotion(outputAngularVelocity);
 
       // Add rotation to current target pose
       currentTarget =
@@ -316,20 +263,6 @@ public void execute() {
 
     // Log current target pose
     swerveDrive.getFieldLogger().getField().getObject("Current Target").setPose(currentTarget);
-
-    DogLog.log("Drivetrain/DriveToState/ProfilePose", currentTarget);
-    DogLog.log(
-        "Drivetrain/DriveToState/ProfileVelocityX",
-        currentSpeeds.vxMetersPerSecond,
-        MetersPerSecond);
-    DogLog.log(
-        "Drivetrain/DriveToState/ProfileVelocityY",
-        currentSpeeds.vyMetersPerSecond,
-        MetersPerSecond);
-    DogLog.log(
-        "Drivetrain/DriveToState/ProfileAngularVelocity",
-        currentSpeeds.omegaRadiansPerSecond,
-        RadiansPerSecond);
   }
 
   @Override

src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java

@@ -1,11 +1,7 @@
 package com.team6962.lib.swerve.motion;
 
-import static edu.wpi.first.units.Units.Amps;
 import static edu.wpi.first.units.Units.Hertz;
 import static edu.wpi.first.units.Units.MetersPerSecond;
-import static edu.wpi.first.units.Units.NewtonMeters;
-import static edu.wpi.first.units.Units.Newtons;
-import static edu.wpi.first.units.Units.Ohms;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.Rotations;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
@@ -25,12 +21,7 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.Current;
-import edu.wpi.first.units.measure.Force;
 import edu.wpi.first.units.measure.LinearVelocity;
-import edu.wpi.first.units.measure.Resistance;
-import edu.wpi.first.units.measure.Torque;
-import edu.wpi.first.units.measure.Voltage;
 
 /**
  * A swerve motion that drives the robot at a specified field-relative velocity.
@@ -64,35 +55,18 @@ public class VelocityMotion implements SwerveMotion {
   /** Whether this motion has a non-zero rotation component. */
   private final boolean hasRotation;
 
-  /** The feedforward forces for each swerve module, which can be null if not used. */
-  private final Force[] forceFeedforwards;
-
   /**
    * Creates a new VelocityMotion with the specified field-relative velocity.
    *
    * @param velocity The target field-relative chassis speeds
-   * @param forceFeedforwards The feedforward forces for each swerve module, which can be null if
-   *     not used
    * @param swerveDrive The swerve drive to control
    */
-  public VelocityMotion(
-      ChassisSpeeds velocity, Force[] forceFeedforwards, MotionSwerveDrive swerveDrive) {
+  public VelocityMotion(ChassisSpeeds velocity, MotionSwerveDrive swerveDrive) {
     this.velocity = velocity;
     this.hasTranslation =
         Math.abs(velocity.vxMetersPerSecond) > 0.01 || Math.abs(velocity.vyMetersPerSecond) > 0.01;
     this.hasRotation = Math.abs(velocity.omegaRadiansPerSecond) > 0.01;
     this.swerveDrive = swerveDrive;
-    this.forceFeedforwards = forceFeedforwards;
-  }
-
-  /**
-   * Creates a new VelocityMotion with the specified field-relative velocity.
-   *
-   * @param velocity The target field-relative chassis speeds
-   * @param swerveDrive The swerve drive to control
-   */
-  public VelocityMotion(ChassisSpeeds velocity, MotionSwerveDrive swerveDrive) {
-    this(velocity, null, swerveDrive);
   }
 
   /**
@@ -114,14 +88,6 @@ public SwerveMotion fuseWith(SwerveMotion other) {
             "Cannot fuse two VelocityMotions with overlapping translation or rotation components.");
       }
 
-      if (!otherVelocityMotion.hasRotation && !otherVelocityMotion.hasTranslation) {
-        return this;
-      }
-
-      if (!hasRotation && !hasTranslation) {
-        return otherVelocityMotion;
-      }
-
       return new VelocityMotion(
           SwerveKinematicsUtil.addChassisSpeeds(velocity, otherVelocityMotion.velocity),
           swerveDrive);
@@ -187,17 +153,6 @@ public void update(double deltaTimeSeconds) {
       LinearVelocity driveVelocity = MetersPerSecond.of(state.speedMetersPerSecond);
       Angle steerAngle = state.angle.getMeasure();
 
-      Force ffForce = forceFeedforwards != null ? forceFeedforwards[i] : Newtons.of(0);
-      Torque ffWheelTorque = swerveDrive.getConstants().getWheelRadius(i).times(ffForce);
-      Torque ffMotorTorque = ffWheelTorque.div(swerveDrive.getConstants().DriveMotor.GearReduction);
-      double ffMotorTorqueNm = ffMotorTorque.in(NewtonMeters);
-      double motorKT = swerveDrive.getConstants().DriveMotor.SimulatedMotor.KtNMPerAmp;
-      double ffCurrentAmps = ffMotorTorqueNm / motorKT;
-      Current ffCurrent = Amps.of(ffCurrentAmps);
-      Resistance windingResistance =
-          Ohms.of(swerveDrive.getConstants().DriveMotor.SimulatedMotor.rOhms);
-      Voltage ffVoltage = ffCurrent.times(windingResistance);
-
       module.setControl(
           new VelocityControlRequest(
                   WheelMath.toAngular(driveVelocity, swerveDrive.getConstants().getWheelRadius(i))
@@ -207,7 +162,6 @@ public void update(double deltaTimeSeconds) {
               .withSlot(driveConstants.VelocitySlot)
               .withUpdateFreqHz(updateFrequencyHz)
               .withUseTimesync(useTimesync)
-              .withAdditionalFeedforward(ffVoltage)
               .toControlRequest(),
           new PositionControlRequest(steerAngle.in(Rotations))
               .withMotionProfileType(steerConstants.PositionControlMotionProfile)

src/main/java/com/team6962/lib/swerve/pathplanner/PathPlanner.java

@@ -68,7 +68,7 @@ public PathPlanner(CommandSwerveDrive drivetrain) {
             }
           }
 
-          drivetrain.applyMotion(new VelocityMotion(appliedSpeeds, forces, drivetrain));
+          drivetrain.applyMotion(new VelocityMotion(appliedSpeeds, drivetrain));
         },
         new PPHolonomicDriveController(
             new PIDConstants(