22 move to action

TeamCode/src/main/java/codebase/actions/MoveToAction.java

@@ -0,0 +1,82 @@
+package codebase.actions;
+
+import com.qualcomm.robotcore.hardware.PIDCoefficients;
+
+import codebase.geometry.Angles;
+import codebase.geometry.FieldPosition;
+import codebase.geometry.MovementVector;
+import codebase.movement.mecanum.MecanumDriver;
+import codebase.pathing.Localizer;
+
+public class MoveToAction implements Action {
+
+    private static final PIDCoefficients MOVEMENT_PID_COEFFICIENTS = new PIDCoefficients(1, 0, 0);
+    private static final PIDCoefficients DIRECTION_PID_COEFFICIENTS = new PIDCoefficients(1, 0, 0);
+    private final MecanumDriver driver;
+    private final Localizer localizer;
+
+    private final FieldPosition destination;
+
+    /**
+     * The speed to move horizontally/vertically or some combination of the two in inches/sec
+     */
+    private final double movementSpeed;
+
+    /**
+     * The max rotational speed of the robot in radians/sec
+     */
+    private final double rotationalSpeed;
+
+    private final double maxDistanceError;
+    private final double maxRotationalError;
+
+    private final PIDController xPID;
+    private final PIDController yPID;
+    private final PIDController directionPID;
+
+    public MoveToAction(MecanumDriver driver, Localizer localizer, FieldPosition destination, double movementSpeed, double rotationalSpeed, double maxDistanceError, double maxRotationalError) {
+        this.driver = driver;
+        this.localizer = localizer;
+        this.destination = destination;
+        this.movementSpeed = movementSpeed;
+        this.rotationalSpeed = rotationalSpeed;
+        this.maxDistanceError = maxDistanceError;
+        this.maxRotationalError = maxRotationalError;
+
+        this.xPID = new PIDController(MOVEMENT_PID_COEFFICIENTS.p, MOVEMENT_PID_COEFFICIENTS.i, MOVEMENT_PID_COEFFICIENTS.d, () -> localizer.getCurrentPosition().x, () -> destination.x);
+        this.yPID = new PIDController(MOVEMENT_PID_COEFFICIENTS.p, MOVEMENT_PID_COEFFICIENTS.i, MOVEMENT_PID_COEFFICIENTS.d, () -> localizer.getCurrentPosition().y, () -> destination.y);
+        this.directionPID = new PIDController(
+                DIRECTION_PID_COEFFICIENTS.p, DIRECTION_PID_COEFFICIENTS.i, DIRECTION_PID_COEFFICIENTS.d,
+                () -> localizer.getCurrentPosition().direction,
+                () -> destination.direction,
+                () -> Angles.angleDifference(localizer.getCurrentPosition().direction, destination.direction)
+        );
+    }
+
+    @Override
+    public void init() {
+
+    }
+
+    @Override
+    public void loop() {
+        double powerX = xPID.getPower();
+        double powerY = yPID.getPower();
+        double powerRotational = directionPID.getPower();
+
+        MovementVector vector = new MovementVector(
+                movementSpeed * powerY,
+                movementSpeed * powerX,
+                rotationalSpeed * powerRotational);
+
+        this.driver.setAbsoluteVelocity(localizer.getCurrentPosition(), vector);
+    }
+
+    @Override
+    public boolean isComplete() {
+        double distanceError = Math.sqrt(Math.pow(localizer.getCurrentPosition().x - destination.x, 2) + Math.pow(localizer.getCurrentPosition().y - destination.y, 2));
+        double rotationalError = Angles.angleDifference(localizer.getCurrentPosition().direction, destination.direction);
+
+        return (distanceError <= maxDistanceError) && (rotationalError <= maxRotationalError);
+    }
+}

TeamCode/src/main/java/codebase/geometry/Angles.java

@@ -0,0 +1,21 @@
+package codebase.geometry;
+
+public final class Angles {
+    public static double angleDifference(double fromAngle, double toAngle) {
+        double diff = toAngle - fromAngle;
+
+        // Normalize to [-2PI, 2PI]
+        diff = diff % (Math.PI * 2);
+
+        // Convert to [-PI, PI] range
+        if (diff > Math.PI) {
+            diff -= (Math.PI * 2);
+        } else if (diff <= -Math.PI) {
+            diff += (Math.PI * 2);
+        }
+
+        return diff;
+    }
+
+    private Angles() {}
+}

TeamCode/src/main/java/codebase/geometry/FieldPosition.java

@@ -10,22 +10,10 @@ public class FieldPosition extends Point {
     /**
      * The direction of the field position defined as radians counterclockwise from directly to the right of the field
      */
-    private double direction;
+    public double direction;
 
-    public FieldPosition(double x, double y, double direction, AngleUnit angleUnit) {
+    public FieldPosition(double x, double y, double direction) {
         super(x, y);
-        setDirection(direction, angleUnit);
-    }
-
-    public void setDirection(double direction, AngleUnit angleUnit) {
-        this.direction = angleUnit.toRadians(direction);
-    }
-
-    public double getDirection(AngleUnit angleUnit) {
-        return angleUnit.fromRadians(direction);
-    }
-
-    public Pose2D toPose2D() {
-        return new Pose2D(DistanceUnit.INCH, this.x, this.y, AngleUnit.RADIANS, this.getDirection(AngleUnit.RADIANS));
+        this.direction = direction;
     }
 }

TeamCode/src/main/java/codebase/movement/mecanum/MecanumDriver.java

@@ -1,7 +1,5 @@
 package codebase.movement.mecanum;
 
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-
 import codebase.geometry.FieldPosition;
 import codebase.geometry.MovementVector;
 import codebase.hardware.Motor;
@@ -127,7 +125,7 @@ public void setRelativeVelocity(MovementVector velocity) {
      * @param powerInput MovementVector containing absolute power inputs (-1 to 1).
      */
     public void setAbsolutePower(FieldPosition position, MovementVector powerInput) {
-        double direction = position.getDirection(AngleUnit.RADIANS);
+        double direction = position.direction;
 
         double relativeVerticalPower = Math.cos(direction) * powerInput.getHorizontalVelocity() + Math.sin(direction) * powerInput.getVerticalVelocity();
         double relativeHorizontalPower = Math.sin(direction) * powerInput.getHorizontalVelocity() - Math.cos(direction) * powerInput.getVerticalVelocity();
@@ -148,7 +146,7 @@ public void setAbsolutePower(FieldPosition position, MovementVector powerInput)
      * @param velocity MovementVector containing absolute velocity inputs (inches/second or radians/second).
      */
     public void setAbsoluteVelocity(FieldPosition position, MovementVector velocity) {
-        double direction = position.getDirection(AngleUnit.RADIANS);
+        double direction = position.direction;
 
         double relativeVerticalVelocity = Math.cos(direction) * velocity.getHorizontalVelocity() + Math.sin(direction) * velocity.getVerticalVelocity();
         double relativeHorizontalVelocity = Math.sin(direction) * velocity.getHorizontalVelocity() - Math.cos(direction) * velocity.getVerticalVelocity();

TeamCode/src/main/java/codebase/pathing/Localizer.java

@@ -0,0 +1,11 @@
+package codebase.pathing;
+
+import codebase.Loop;
+import codebase.geometry.FieldPosition;
+
+public interface Localizer extends Loop {
+
+    FieldPosition getCurrentPosition();
+
+    void init(FieldPosition initialPosition);
+}