Merge pull request #55 from Manchester-Central/FieldPoseUpdates

Field pose updates

Author: NinjaBuilderPro

src/main/java/com/chaos131/poses/FieldPose.java

@@ -4,8 +4,12 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+import static edu.wpi.first.units.Units.Meters;
+
 import java.util.ArrayList;
 import java.util.HashMap;
 import java.util.Map;
@@ -82,8 +86,20 @@ public String getName() {
    * @param robotpose to calculate from
    * @return the distance along the floor. Does not account for vertical component.
    */
-  public double getDistanceFromLocation(Pose2d robotpose) {
-    return getCurrentAlliancePose().getTranslation().getDistance(robotpose.getTranslation());
+  public Distance getDistanceFromLocation(Pose2d robotpose) {
+    return getDistanceFromLocations(getCurrentAlliancePose(), robotpose);
+  }
+
+  /**
+   * Calculates the distance from a specific spot on the field. Typically this is the robot's
+   * position.
+   *
+   * @param pose1
+   * @param pose2 
+   * @return the distance along the floor. Does not account for vertical component. Returns absolute values.
+   */
+  public static Distance getDistanceFromLocations(Pose2d pose1, Pose2d pose2) {
+    return Meters.of(pose2.getTranslation().getDistance(pose1.getTranslation()));
   }
 
   /**

src/main/java/com/chaos131/poses/FieldPose2026.java

@@ -4,11 +4,11 @@
 
 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.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import java.io.IOException;
 
@@ -45,10 +45,14 @@ public class FieldPose2026 extends PivotedFieldPose {
   public static FieldPose2026 ClimbRightBar;
   public static FieldPose2026 ClimbFarRightBar;
 
-  private FieldPose2026(Alliance defaultAlliance, String name, Pose3d defaultPose) {
+  public FieldPose2026(Alliance defaultAlliance, String name, Pose3d defaultPose) {
     super(Midpoint, defaultAlliance, name, defaultPose);
   }
 
+  public FieldPose2026(Alliance defaultAlliance, String name, Pose2d defaultPose) {
+    super(Midpoint, defaultAlliance, name, new Pose3d(defaultPose));
+  }
+
   /**
    * Required function before using any field points. Necessary because Java's static function
    * initializers will fail to load any of it. Values will be null. Run this at runtime to load the
@@ -59,11 +63,6 @@ private FieldPose2026(Alliance defaultAlliance, String name, Pose3d defaultPose)
    * @throws IOException
    */
   public static void initializeFieldPoses() throws IOException {
-    var alliance = DriverStation.getAlliance();
-    if (alliance.isEmpty()) {
-      throw new IOException(
-          "Failed to load file because Alliance is unknown. Try waiting next time.");
-    }
     AprilTagLayout = new AprilTagFieldLayout(AprilTagField.m_resourceFile);
     Midpoint =
         new Translation2d(

src/main/java/com/chaos131/util/ChaosTalonFx.java

@@ -14,13 +14,16 @@
 import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.sim.CANcoderSimState;
 import com.ctre.phoenix6.sim.ChassisReference;
 import com.ctre.phoenix6.sim.TalonFXSimState;
 import com.ctre.phoenix6.sim.TalonFXSimState.MotorType;
 import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Mass;
+import edu.wpi.first.units.measure.Velocity;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 
@@ -31,6 +34,7 @@ public class ChaosTalonFx extends TalonFX {
   private boolean m_isMainSimMotor;
   private ChaosCanCoder m_attachedCanCoder;
   private final PositionVoltage m_positionVoltage = new PositionVoltage(0);
+  private final VelocityVoltage m_velocityVoltage = new VelocityVoltage(0);
   private final MotionMagicVoltage m_positionMotionMagicVoltage = new MotionMagicVoltage(0);
   private final DynamicMotionMagicVoltage m_positionDynamicMotionMagicVoltage =
       new DynamicMotionMagicVoltage(0, 0, 0);
@@ -148,6 +152,11 @@ public void tuneMotionMagic(PIDFValue pidValue, double kg, double ks, double kv,
     applyConfig();
   }
 
+  public void moveAtVelocity(AngularVelocity velocity) {
+    m_velocityVoltage.Slot = 0;
+    setControl(m_velocityVoltage.withVelocity(velocity));
+  }
+
   /** Tells the motor controller to move to the target position. */
   public void moveToPosition(Angle position) {
     m_positionVoltage.Slot = 0;