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..41d336c --- /dev/null +++ b/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); + } +} 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..d092166 --- /dev/null +++ b/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() {} +} diff --git a/TeamCode/src/main/java/codebase/geometry/FieldPosition.java b/TeamCode/src/main/java/codebase/geometry/FieldPosition.java index 046fab3..91d4fae 100644 --- a/TeamCode/src/main/java/codebase/geometry/FieldPosition.java +++ b/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; } } \ 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..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; @@ -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(); 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..4a3b8c8 --- /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 { + + FieldPosition getCurrentPosition(); + + void init(FieldPosition initialPosition); +}