diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java new file mode 100644 index 0000000..4aa7702 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the Localizer class. It is an abstract superclass of all localizers used in Pedro Pathing, + * so it contains abstract methods that will have a concrete implementation in the subclasses. Any + * method that all localizers will need will be in this class. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +public abstract class Localizer { + + public abstract Pose getPose(); + + public abstract Pose getVelocity(); + + public abstract Vector getVelocityVector(); + + public abstract void setStartPose(Pose setStart); + + public abstract void setPose(Pose setPose); + + public abstract void update(); + + public abstract double getTotalHeading(); + + public abstract double getForwardMultiplier(); + + public abstract double getLateralMultiplier(); + + public abstract double getTurningMultiplier(); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java new file mode 100644 index 0000000..667196c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java @@ -0,0 +1,109 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/** + * This is the Pose class. It defines poses in 2D space, like the Pose2D class in Road Runner except + * in the Pedro Pathing code so I don't have to import the Road Runner library. A Pose consists of + * two coordinates defining a position and a third value for the heading, so basically just defining + * any position and orientation the robot can be at, unless your robot can fly for whatever reason. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +public class Pose { + private double x; + private double y; + private double heading; + + public Pose(double setX, double setY, double setHeading) { + setX(setX); + setY(setY); + setHeading(setHeading); + } + + public Pose(double setX, double setY) { + this(setX, setY, 0); + } + + public Pose() { + this(0,0,0); + } + + public void setX(double set) { + x = set; + } + + public void setY(double set) { + y = set; + } + + public void setHeading(double set) { + heading = MathFunctions.normalizeAngle(set); + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + + public double getHeading() { + return heading; + } + + public Vector getVector() { + Vector returnVector = new Vector(); + returnVector.setOrthogonalComponents(x, y); + return returnVector; + } + + public Vector getHeadingVector() { + return new Vector(1, heading); + } + + public void add(Pose pose) { + setX(x + pose.getX()); + setY(y + pose.getY()); + setHeading(heading + pose.getHeading()); + } + + public void subtract(Pose pose) { + setX(x - pose.getX()); + setY(y - pose.getY()); + setHeading(heading - pose.getHeading()); + } + + public void scalarMultiply(double scalar) { + setX(x * scalar); + setY(y * scalar); + setHeading(heading * scalar); + } + + public void scalarDivide(double scalar) { + setX(x / scalar); + setY(y / scalar); + setHeading(heading / scalar); + } + + public void flipSigns() { + setX(-x); + setY(-y); + setHeading(-heading); + } + + public boolean roughlyEquals(Pose pose, double accuracy) { + return MathFunctions.roughlyEquals(x, pose.getX(), accuracy) && MathFunctions.roughlyEquals(y, pose.getY(), accuracy) && MathFunctions.roughlyEquals(MathFunctions.getSmallestAngleDifference(heading, pose.getHeading()), 0, accuracy); + } + + public boolean roughlyEquals(Pose pose) { + return roughlyEquals(pose, 0.0001); + } + + public Pose copy() { + return new Pose(getX(), getY(), getHeading()); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java new file mode 100644 index 0000000..4df27fb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java @@ -0,0 +1,93 @@ +//package org.firstinspires.ftc.teamcode.pedroPathing.localization; +// +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.qualcomm.robotcore.hardware.HardwareMap; +// +//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +// +//import java.util.ArrayList; +//import java.util.List; +// +///** +// * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and +// * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing +// * localizer system. +// * +// * @author Anyi Lin - 10158 Scott's Bots +// * @version 1.0, 5/9/2024 +// */ +//public class RRToPedroThreeWheelLocalizer extends Localizer { +// private RoadRunnerThreeWheelLocalizer localizer; +// private double totalHeading; +// private Pose startPose; +// private Pose previousPose; +// +// public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { +// List lastTrackingEncPositions = new ArrayList<>(); +// List lastTrackingEncVels = new ArrayList<>(); +// +// localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); +// +// startPose = new Pose(); +// previousPose = new Pose(); +// } +// @Override +// public Pose getPose() { +// Pose2d pose = localizer.getPoseEstimate(); +// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); +// } +// +// @Override +// public Pose getVelocity() { +// Pose2d pose = localizer.getPoseVelocity(); +// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); +// } +// +// @Override +// public Vector getVelocityVector() { +// Pose2d pose = localizer.getPoseVelocity(); +// Vector returnVector = new Vector(); +// returnVector.setOrthogonalComponents(pose.getX(), pose.getY()); +// return returnVector; +// } +// +// @Override +// public void setStartPose(Pose setStart) { +// Pose oldStart = startPose; +// startPose = setStart; +// Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart); +// localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); +// } +// +// @Override +// public void setPose(Pose setPose) { +// localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); +// } +// +// @Override +// public void update() { +// totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); +// previousPose = getPose(); +// } +// +// @Override +// public double getTotalHeading() { +// return totalHeading; +// } +// +// @Override +// public double getForwardMultiplier() { +// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; +// } +// +// @Override +// public double getLateralMultiplier() { +// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; +// } +// +// @Override +// public double getTurningMultiplier() { +// return (getForwardMultiplier() + getLateralMultiplier()) / 2; +// } +//}