From 2ccda2ea22c81eec1d81df9db1f8f08020d6b3c9 Mon Sep 17 00:00:00 2001 From: liambridgers Date: Wed, 6 Aug 2025 15:40:43 -0400 Subject: [PATCH 1/4] add Angles for angle helpers such as angleDifference, make direction public in FieldPosition for easy access and standardize to radians to match our conventions, add Localizer interface, update MecanumDriver for standardization to radians in FieldPosition, add MoveToAction --- .../java/codebase/actions/MoveToAction.java | 88 +++++++++++++++++++ .../main/java/codebase/geometry/Angles.java | 22 +++++ .../java/codebase/geometry/FieldPosition.java | 16 +--- .../movement/mecanum/MecanumDriver.java | 4 +- .../main/java/codebase/pathing/Localizer.java | 11 +++ 5 files changed, 127 insertions(+), 14 deletions(-) create mode 100644 TeamCode/src/main/java/codebase/actions/MoveToAction.java create mode 100644 TeamCode/src/main/java/codebase/geometry/Angles.java create mode 100644 TeamCode/src/main/java/codebase/pathing/Localizer.java diff --git a/TeamCode/src/main/java/codebase/actions/MoveToAction.java b/TeamCode/src/main/java/codebase/actions/MoveToAction.java new file mode 100644 index 0000000..ed7f6cc --- /dev/null +++ b/TeamCode/src/main/java/codebase/actions/MoveToAction.java @@ -0,0 +1,88 @@ +package codebase.actions; + +import codebase.controllers.PIDController; +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 double MOVEMENT_Kp = 1; + private static double MOVEMENT_Ki = 0; + private static double MOVEMENT_Kd = 0; + + private static double DIRECTION_Kp = 1; + private static double DIRECTION_Ki = 0; + private static double Direction_Kd = 0; + + private final MecanumDriver driver; + private final Localizer localizer; + + private final FieldPosition destination; + + /** + * The max magnitude of the robot velocity 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_Kp, MOVEMENT_Ki, MOVEMENT_Kd, () -> localizer.getCurrentPosition().x, () -> destination.x); + this.yPID = new PIDController(MOVEMENT_Kp, MOVEMENT_Ki, MOVEMENT_Kd, () -> localizer.getCurrentPosition().y, () -> destination.y); + this.directionPID = new PIDController( + DIRECTION_Kp, DIRECTION_Ki, Direction_Kd, + () -> 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); + } +} diff --git a/TeamCode/src/main/java/codebase/geometry/Angles.java b/TeamCode/src/main/java/codebase/geometry/Angles.java new file mode 100644 index 0000000..c2fe9e4 --- /dev/null +++ b/TeamCode/src/main/java/codebase/geometry/Angles.java @@ -0,0 +1,22 @@ +package codebase.geometry; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +public class Angles { + public static double angleDifference(double fromAngle, double toAngle) { + // Calculate initial difference (toAngle - fromAngle) + 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; + } +} diff --git a/TeamCode/src/main/java/codebase/geometry/FieldPosition.java b/TeamCode/src/main/java/codebase/geometry/FieldPosition.java index 046fab3..bd72fee 100644 --- a/TeamCode/src/main/java/codebase/geometry/FieldPosition.java +++ b/TeamCode/src/main/java/codebase/geometry/FieldPosition.java @@ -10,22 +10,14 @@ 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); + this.direction = direction; } public Pose2D toPose2D() { - return new Pose2D(DistanceUnit.INCH, this.x, this.y, AngleUnit.RADIANS, this.getDirection(AngleUnit.RADIANS)); + return new Pose2D(DistanceUnit.INCH, this.x, this.y, AngleUnit.RADIANS, this.direction); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/codebase/movement/mecanum/MecanumDriver.java b/TeamCode/src/main/java/codebase/movement/mecanum/MecanumDriver.java index 99313b3..99741f1 100644 --- a/TeamCode/src/main/java/codebase/movement/mecanum/MecanumDriver.java +++ b/TeamCode/src/main/java/codebase/movement/mecanum/MecanumDriver.java @@ -127,7 +127,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 +148,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(); diff --git a/TeamCode/src/main/java/codebase/pathing/Localizer.java b/TeamCode/src/main/java/codebase/pathing/Localizer.java new file mode 100644 index 0000000..2b30f99 --- /dev/null +++ b/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 { + + public FieldPosition getCurrentPosition(); + + public FieldPosition init(FieldPosition initialPosition); +} From 368e7b3e3b09efdaf015bcc3e33df3871a4af157 Mon Sep 17 00:00:00 2001 From: liambridgers Date: Wed, 6 Aug 2025 15:41:50 -0400 Subject: [PATCH 2/4] remove unused imports, remove redundant public modifier in Localizer --- TeamCode/src/main/java/codebase/geometry/Angles.java | 2 -- .../main/java/codebase/movement/mecanum/MecanumDriver.java | 2 -- TeamCode/src/main/java/codebase/pathing/Localizer.java | 4 ++-- 3 files changed, 2 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/codebase/geometry/Angles.java b/TeamCode/src/main/java/codebase/geometry/Angles.java index c2fe9e4..9cd4950 100644 --- a/TeamCode/src/main/java/codebase/geometry/Angles.java +++ b/TeamCode/src/main/java/codebase/geometry/Angles.java @@ -1,7 +1,5 @@ package codebase.geometry; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; - public class Angles { public static double angleDifference(double fromAngle, double toAngle) { // Calculate initial difference (toAngle - fromAngle) diff --git a/TeamCode/src/main/java/codebase/movement/mecanum/MecanumDriver.java b/TeamCode/src/main/java/codebase/movement/mecanum/MecanumDriver.java index 99741f1..5b6d9b7 100644 --- a/TeamCode/src/main/java/codebase/movement/mecanum/MecanumDriver.java +++ b/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; diff --git a/TeamCode/src/main/java/codebase/pathing/Localizer.java b/TeamCode/src/main/java/codebase/pathing/Localizer.java index 2b30f99..85c5e8e 100644 --- a/TeamCode/src/main/java/codebase/pathing/Localizer.java +++ b/TeamCode/src/main/java/codebase/pathing/Localizer.java @@ -5,7 +5,7 @@ public interface Localizer extends Loop { - public FieldPosition getCurrentPosition(); + FieldPosition getCurrentPosition(); - public FieldPosition init(FieldPosition initialPosition); + FieldPosition init(FieldPosition initialPosition); } From 3d11f8aa98712cc0fec39a7784843624a14d76e6 Mon Sep 17 00:00:00 2001 From: liam Date: Thu, 7 Aug 2025 00:29:42 -0400 Subject: [PATCH 3/4] make Angles class final and uninstantiatable, remove unused toPose2D method of FieldPosition, fix return type of init method in Localizer, turn PID coefficients into a class (PIDCoefficients) and adjust comments --- .../java/codebase/actions/MoveToAction.java | 20 +++++++------------ .../codebase/controllers/PIDCoefficients.java | 13 ++++++++++++ .../main/java/codebase/geometry/Angles.java | 5 +++-- .../java/codebase/geometry/FieldPosition.java | 4 ---- .../main/java/codebase/pathing/Localizer.java | 2 +- 5 files changed, 24 insertions(+), 20 deletions(-) create mode 100644 TeamCode/src/main/java/codebase/controllers/PIDCoefficients.java diff --git a/TeamCode/src/main/java/codebase/actions/MoveToAction.java b/TeamCode/src/main/java/codebase/actions/MoveToAction.java index ed7f6cc..cbd9515 100644 --- a/TeamCode/src/main/java/codebase/actions/MoveToAction.java +++ b/TeamCode/src/main/java/codebase/actions/MoveToAction.java @@ -1,5 +1,6 @@ package codebase.actions; +import codebase.controllers.PIDCoefficients; import codebase.controllers.PIDController; import codebase.geometry.Angles; import codebase.geometry.FieldPosition; @@ -9,21 +10,15 @@ public class MoveToAction implements Action { - private static double MOVEMENT_Kp = 1; - private static double MOVEMENT_Ki = 0; - private static double MOVEMENT_Kd = 0; - - private static double DIRECTION_Kp = 1; - private static double DIRECTION_Ki = 0; - private static double Direction_Kd = 0; - + 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 max magnitude of the robot velocity in inches/sec + * The speed to move horizontally/vertically or some combination of the two in inches/sec */ private final double movementSpeed; @@ -48,10 +43,10 @@ public MoveToAction(MecanumDriver driver, Localizer localizer, FieldPosition des this.maxDistanceError = maxDistanceError; this.maxRotationalError = maxRotationalError; - this.xPID = new PIDController(MOVEMENT_Kp, MOVEMENT_Ki, MOVEMENT_Kd, () -> localizer.getCurrentPosition().x, () -> destination.x); - this.yPID = new PIDController(MOVEMENT_Kp, MOVEMENT_Ki, MOVEMENT_Kd, () -> localizer.getCurrentPosition().y, () -> destination.y); + this.xPID = new PIDController(MOVEMENT_PID_COEFFICIENTS.Kp, MOVEMENT_PID_COEFFICIENTS.Ki, MOVEMENT_PID_COEFFICIENTS.Kd, () -> localizer.getCurrentPosition().x, () -> destination.x); + this.yPID = new PIDController(MOVEMENT_PID_COEFFICIENTS.Kp, MOVEMENT_PID_COEFFICIENTS.Ki, MOVEMENT_PID_COEFFICIENTS.Kd, () -> localizer.getCurrentPosition().y, () -> destination.y); this.directionPID = new PIDController( - DIRECTION_Kp, DIRECTION_Ki, Direction_Kd, + DIRECTION_PID_COEFFICIENTS.Kp, DIRECTION_PID_COEFFICIENTS.Ki, DIRECTION_PID_COEFFICIENTS.Kd, () -> localizer.getCurrentPosition().direction, () -> destination.direction, () -> Angles.angleDifference(localizer.getCurrentPosition().direction, destination.direction) @@ -63,7 +58,6 @@ public void init() { } - @Override public void loop() { double powerX = xPID.getPower(); diff --git a/TeamCode/src/main/java/codebase/controllers/PIDCoefficients.java b/TeamCode/src/main/java/codebase/controllers/PIDCoefficients.java new file mode 100644 index 0000000..eec3655 --- /dev/null +++ b/TeamCode/src/main/java/codebase/controllers/PIDCoefficients.java @@ -0,0 +1,13 @@ +package codebase.controllers; + +public class PIDCoefficients { + public double Kp; + public double Ki; + public double Kd; + + public PIDCoefficients(double Kp, double Ki, double Kd) { + this.Kp = Kp; + this.Ki = Ki; + this.Kd = Kd; + } +} diff --git a/TeamCode/src/main/java/codebase/geometry/Angles.java b/TeamCode/src/main/java/codebase/geometry/Angles.java index 9cd4950..d092166 100644 --- a/TeamCode/src/main/java/codebase/geometry/Angles.java +++ b/TeamCode/src/main/java/codebase/geometry/Angles.java @@ -1,8 +1,7 @@ package codebase.geometry; -public class Angles { +public final class Angles { public static double angleDifference(double fromAngle, double toAngle) { - // Calculate initial difference (toAngle - fromAngle) double diff = toAngle - fromAngle; // Normalize to [-2PI, 2PI] @@ -17,4 +16,6 @@ public static double angleDifference(double fromAngle, double toAngle) { return diff; } + + private Angles() {} } diff --git a/TeamCode/src/main/java/codebase/geometry/FieldPosition.java b/TeamCode/src/main/java/codebase/geometry/FieldPosition.java index bd72fee..91d4fae 100644 --- a/TeamCode/src/main/java/codebase/geometry/FieldPosition.java +++ b/TeamCode/src/main/java/codebase/geometry/FieldPosition.java @@ -16,8 +16,4 @@ public FieldPosition(double x, double y, double direction) { super(x, y); this.direction = direction; } - - public Pose2D toPose2D() { - return new Pose2D(DistanceUnit.INCH, this.x, this.y, AngleUnit.RADIANS, this.direction); - } } \ No newline at end of file diff --git a/TeamCode/src/main/java/codebase/pathing/Localizer.java b/TeamCode/src/main/java/codebase/pathing/Localizer.java index 85c5e8e..4a3b8c8 100644 --- a/TeamCode/src/main/java/codebase/pathing/Localizer.java +++ b/TeamCode/src/main/java/codebase/pathing/Localizer.java @@ -7,5 +7,5 @@ public interface Localizer extends Loop { FieldPosition getCurrentPosition(); - FieldPosition init(FieldPosition initialPosition); + void init(FieldPosition initialPosition); } From 1fdc23cd6adeabf230454b0028725604e1141142 Mon Sep 17 00:00:00 2001 From: liam Date: Thu, 7 Aug 2025 00:38:18 -0400 Subject: [PATCH 4/4] remove PIDCoefficients as one is already supplied by robotcore, change to use of this in MoveToAction --- .../main/java/codebase/actions/MoveToAction.java | 10 +++++----- .../java/codebase/controllers/PIDCoefficients.java | 13 ------------- 2 files changed, 5 insertions(+), 18 deletions(-) delete mode 100644 TeamCode/src/main/java/codebase/controllers/PIDCoefficients.java diff --git a/TeamCode/src/main/java/codebase/actions/MoveToAction.java b/TeamCode/src/main/java/codebase/actions/MoveToAction.java index cbd9515..41d336c 100644 --- a/TeamCode/src/main/java/codebase/actions/MoveToAction.java +++ b/TeamCode/src/main/java/codebase/actions/MoveToAction.java @@ -1,7 +1,7 @@ package codebase.actions; -import codebase.controllers.PIDCoefficients; -import codebase.controllers.PIDController; +import com.qualcomm.robotcore.hardware.PIDCoefficients; + import codebase.geometry.Angles; import codebase.geometry.FieldPosition; import codebase.geometry.MovementVector; @@ -43,10 +43,10 @@ public MoveToAction(MecanumDriver driver, Localizer localizer, FieldPosition des this.maxDistanceError = maxDistanceError; this.maxRotationalError = maxRotationalError; - this.xPID = new PIDController(MOVEMENT_PID_COEFFICIENTS.Kp, MOVEMENT_PID_COEFFICIENTS.Ki, MOVEMENT_PID_COEFFICIENTS.Kd, () -> localizer.getCurrentPosition().x, () -> destination.x); - this.yPID = new PIDController(MOVEMENT_PID_COEFFICIENTS.Kp, MOVEMENT_PID_COEFFICIENTS.Ki, MOVEMENT_PID_COEFFICIENTS.Kd, () -> localizer.getCurrentPosition().y, () -> destination.y); + 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.Kp, DIRECTION_PID_COEFFICIENTS.Ki, DIRECTION_PID_COEFFICIENTS.Kd, + DIRECTION_PID_COEFFICIENTS.p, DIRECTION_PID_COEFFICIENTS.i, DIRECTION_PID_COEFFICIENTS.d, () -> localizer.getCurrentPosition().direction, () -> destination.direction, () -> Angles.angleDifference(localizer.getCurrentPosition().direction, destination.direction) diff --git a/TeamCode/src/main/java/codebase/controllers/PIDCoefficients.java b/TeamCode/src/main/java/codebase/controllers/PIDCoefficients.java deleted file mode 100644 index eec3655..0000000 --- a/TeamCode/src/main/java/codebase/controllers/PIDCoefficients.java +++ /dev/null @@ -1,13 +0,0 @@ -package codebase.controllers; - -public class PIDCoefficients { - public double Kp; - public double Ki; - public double Kd; - - public PIDCoefficients(double Kp, double Ki, double Kd) { - this.Kp = Kp; - this.Ki = Ki; - this.Kd = Kd; - } -}