SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

uncommented Road Runner adapter files

Browse Source
This commit is contained in:
brotherhobo
2024-05-09 23:12:32 -04:00
parent 2468dbb58a
commit ccc74c8120
3 changed files with 335 additions and 335 deletions
Download Patch File Download Diff File Expand all files Collapse all files

186
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java
View File

@ -1,93 +1,93 @@
//package org.firstinspires.ftc.teamcode.pedroPathing.localization; package org.firstinspires.ftc.teamcode.pedroPathing.localization;
//
//import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
//
//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
//
//import java.util.ArrayList; import java.util.ArrayList;
//import java.util.List; import java.util.List;
//
///** /**
// * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and * 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 * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing
// * localizer system. * localizer system.
// * *
// * @author Anyi Lin - 10158 Scott's Bots * @author Anyi Lin - 10158 Scott's Bots
// * @version 1.0, 5/9/2024 * @version 1.0, 5/9/2024
// */ */
//public class RRToPedroThreeWheelLocalizer extends Localizer { public class RRToPedroThreeWheelLocalizer extends Localizer {
// private RoadRunnerThreeWheelLocalizer localizer; private RoadRunnerThreeWheelLocalizer localizer;
// private double totalHeading; private double totalHeading;
// private Pose startPose; private Pose startPose;
// private Pose previousPose; private Pose previousPose;
//
// public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) {
// List<Integer> lastTrackingEncPositions = new ArrayList<>(); List<Integer> lastTrackingEncPositions = new ArrayList<>();
// List<Integer> lastTrackingEncVels = new ArrayList<>(); List<Integer> lastTrackingEncVels = new ArrayList<>();
//
// localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels);
//
// startPose = new Pose(); startPose = new Pose();
// previousPose = new Pose(); previousPose = new Pose();
// } }
// @Override @Override
// public Pose getPose() { public Pose getPose() {
// Pose2d pose = localizer.getPoseEstimate(); Pose2d pose = localizer.getPoseEstimate();
// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); return new Pose(pose.getX(), pose.getY(), pose.getHeading());
// } }
//
// @Override @Override
// public Pose getVelocity() { public Pose getVelocity() {
// Pose2d pose = localizer.getPoseVelocity(); Pose2d pose = localizer.getPoseVelocity();
// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); return new Pose(pose.getX(), pose.getY(), pose.getHeading());
// } }
//
// @Override @Override
// public Vector getVelocityVector() { public Vector getVelocityVector() {
// Pose2d pose = localizer.getPoseVelocity(); Pose2d pose = localizer.getPoseVelocity();
// Vector returnVector = new Vector(); Vector returnVector = new Vector();
// returnVector.setOrthogonalComponents(pose.getX(), pose.getY()); returnVector.setOrthogonalComponents(pose.getX(), pose.getY());
// return returnVector; return returnVector;
// } }
//
// @Override @Override
// public void setStartPose(Pose setStart) { public void setStartPose(Pose setStart) {
// Pose oldStart = startPose; Pose oldStart = startPose;
// startPose = setStart; startPose = setStart;
// Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart); Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart);
// localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading()));
// } }
//
// @Override @Override
// public void setPose(Pose setPose) { public void setPose(Pose setPose) {
// localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading()));
// } }
//
// @Override @Override
// public void update() { public void update() {
// totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading());
// previousPose = getPose(); previousPose = getPose();
// } }
//
// @Override @Override
// public double getTotalHeading() { public double getTotalHeading() {
// return totalHeading; return totalHeading;
// } }
//
// @Override @Override
// public double getForwardMultiplier() { public double getForwardMultiplier() {
// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER;
// } }
//
// @Override @Override
// public double getLateralMultiplier() { public double getLateralMultiplier() {
// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER;
// } }
//
// @Override @Override
// public double getTurningMultiplier() { public double getTurningMultiplier() {
// return (getForwardMultiplier() + getLateralMultiplier()) / 2; return (getForwardMultiplier() + getLateralMultiplier()) / 2;
// } }
//} }

250
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java
View File

@ -1,125 +1,125 @@
//package org.firstinspires.ftc.teamcode.pedroPathing.localization; package org.firstinspires.ftc.teamcode.pedroPathing.localization;
//
//import com.acmerobotics.roadrunner.util.NanoClock; import com.acmerobotics.roadrunner.util.NanoClock;
//import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorEx;
//import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DcMotorSimple;
//
///** /**
// * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding
// * slot's motor direction * slot's motor direction
// */ */
//public class RoadRunnerEncoder { public class RoadRunnerEncoder {
// private final static int CPS_STEP = 0x10000; private final static int CPS_STEP = 0x10000;
//
// private static double inverseOverflow(double input, double estimate) { private static double inverseOverflow(double input, double estimate) {
// // convert to uint16 // convert to uint16
// int real = (int) input & 0xffff; int real = (int) input & 0xffff;
// // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
// // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
// real += ((real % 20) / 4) * CPS_STEP; real += ((real % 20) / 4) * CPS_STEP;
// // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
// real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
// return real; return real;
// } }
//
// public enum Direction { public enum Direction {
// FORWARD(1), FORWARD(1),
// REVERSE(-1); REVERSE(-1);
//
// private int multiplier; private int multiplier;
//
// Direction(int multiplier) { Direction(int multiplier) {
// this.multiplier = multiplier; this.multiplier = multiplier;
// } }
//
// public int getMultiplier() { public int getMultiplier() {
// return multiplier; return multiplier;
// } }
// } }
//
// private DcMotorEx motor; private DcMotorEx motor;
// private NanoClock clock; private NanoClock clock;
//
// private Direction direction; private Direction direction;
//
// private int lastPosition; private int lastPosition;
// private int velocityEstimateIdx; private int velocityEstimateIdx;
// private double[] velocityEstimates; private double[] velocityEstimates;
// private double lastUpdateTime; private double lastUpdateTime;
//
// public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) { public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) {
// this.motor = motor; this.motor = motor;
// this.clock = clock; this.clock = clock;
//
// this.direction = Direction.FORWARD; this.direction = Direction.FORWARD;
//
// this.lastPosition = 0; this.lastPosition = 0;
// this.velocityEstimates = new double[3]; this.velocityEstimates = new double[3];
// this.lastUpdateTime = clock.seconds(); this.lastUpdateTime = clock.seconds();
// } }
//
// public RoadRunnerEncoder(DcMotorEx motor) { public RoadRunnerEncoder(DcMotorEx motor) {
// this(motor, NanoClock.system()); this(motor, NanoClock.system());
// } }
//
// public Direction getDirection() { public Direction getDirection() {
// return direction; return direction;
// } }
//
// private int getMultiplier() { private int getMultiplier() {
// return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
// } }
//
// /** /**
// * Allows you to set the direction of the counts and velocity without modifying the motor's direction state * Allows you to set the direction of the counts and velocity without modifying the motor's direction state
// * @param direction either reverse or forward depending on if encoder counts should be negated * @param direction either reverse or forward depending on if encoder counts should be negated
// */ */
// public void setDirection(Direction direction) { public void setDirection(Direction direction) {
// this.direction = direction; this.direction = direction;
// } }
//
// /** /**
// * Gets the position from the underlying motor and adjusts for the set direction. * Gets the position from the underlying motor and adjusts for the set direction.
// * Additionally, this method updates the velocity estimates used for compensated velocity * Additionally, this method updates the velocity estimates used for compensated velocity
// * *
// * @return encoder position * @return encoder position
// */ */
// public int getCurrentPosition() { public int getCurrentPosition() {
// int multiplier = getMultiplier(); int multiplier = getMultiplier();
// int currentPosition = motor.getCurrentPosition() * multiplier; int currentPosition = motor.getCurrentPosition() * multiplier;
// if (currentPosition != lastPosition) { if (currentPosition != lastPosition) {
// double currentTime = clock.seconds(); double currentTime = clock.seconds();
// double dt = currentTime - lastUpdateTime; double dt = currentTime - lastUpdateTime;
// velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
// velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
// lastPosition = currentPosition; lastPosition = currentPosition;
// lastUpdateTime = currentTime; lastUpdateTime = currentTime;
// } }
// return currentPosition; return currentPosition;
// } }
//
// /** /**
// * Gets the velocity directly from the underlying motor and compensates for the direction * Gets the velocity directly from the underlying motor and compensates for the direction
// * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore)
// * *
// * @return raw velocity * @return raw velocity
// */ */
// public double getRawVelocity() { public double getRawVelocity() {
// int multiplier = getMultiplier(); int multiplier = getMultiplier();
// return motor.getVelocity() * multiplier; return motor.getVelocity() * multiplier;
// } }
//
// /** /**
// * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity
// * that are lost in overflow due to velocity being transmitted as 16 bits. * that are lost in overflow due to velocity being transmitted as 16 bits.
// * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
// * *
// * @return corrected velocity * @return corrected velocity
// */ */
// public double getCorrectedVelocity() { public double getCorrectedVelocity() {
// double median = velocityEstimates[0] > velocityEstimates[1] double median = velocityEstimates[0] > velocityEstimates[1]
// ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
// : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
// return inverseOverflow(getRawVelocity(), median); return inverseOverflow(getRawVelocity(), median);
// } }
//} }

234
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java
View File

@ -1,117 +1,117 @@
//package org.firstinspires.ftc.teamcode.pedroPathing.localization; package org.firstinspires.ftc.teamcode.pedroPathing.localization;
//
//import androidx.annotation.NonNull; import androidx.annotation.NonNull;
//
//import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
//import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorEx;
//import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
//
//import java.util.Arrays; import java.util.Arrays;
//import java.util.List; import java.util.List;
//
///* /*
// * Sample tracking wheel localizer implementation assuming the standard configuration: * Sample tracking wheel localizer implementation assuming the standard configuration:
// * *
// * left on robot is y pos * left on robot is y pos
// * *
// * front of robot is x pos * front of robot is x pos
// * *
// * /--------------\ * /--------------\
// * | ____ | * | ____ |
// * | ---- | * | ---- |
// * | || || | * | || || |
// * | || || | * | || || |
// * | | * | |
// * | | * | |
// * \--------------/ * \--------------/
// * *
// */ */
//
///** /**
// * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will
// * be replaced with a custom localizer. * be replaced with a custom localizer.
// */ */
//@Config @Config
//public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer {
// public static double TICKS_PER_REV = 8192; public static double TICKS_PER_REV = 8192;
// public static double WHEEL_RADIUS = 1.37795; // in public static double WHEEL_RADIUS = 1.37795; // in
// public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
//
// public static double X_MULTIPLIER = 0.5008239963; public static double X_MULTIPLIER = 0.5008239963;
// public static double Y_MULTIPLIER = 0.5018874659; public static double Y_MULTIPLIER = 0.5018874659;
//
// public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23; public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23;
//
// private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder; private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder;
//
// private List<Integer> lastEncPositions, lastEncVels; private List<Integer> lastEncPositions, lastEncVels;
//
// public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels) { public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels) {
// super(Arrays.asList( super(Arrays.asList(
// new Pose2d(leftX, leftY, 0), // left new Pose2d(leftX, leftY, 0), // left
// new Pose2d(rightX, rightY, 0), // right new Pose2d(rightX, rightY, 0), // right
// new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe
// )); ));
//
// lastEncPositions = lastTrackingEncPositions; lastEncPositions = lastTrackingEncPositions;
// lastEncVels = lastTrackingEncVels; lastEncVels = lastTrackingEncVels;
//
// // TODO: redo the configs here // TODO: redo the configs here
// leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear")); leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
// rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")); rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront"));
// strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
//
// // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
// leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
// rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
// strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD); strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD);
// } }
//
// public void resetHeading(double heading) { public void resetHeading(double heading) {
// setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading));
// } }
//
// public static double encoderTicksToInches(double ticks) { public static double encoderTicksToInches(double ticks) {
// return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
// } }
//
// @NonNull @NonNull
// @Override @Override
// public List<Double> getWheelPositions() { public List<Double> getWheelPositions() {
// int leftPos = leftEncoder.getCurrentPosition(); int leftPos = leftEncoder.getCurrentPosition();
// int rightPos = rightEncoder.getCurrentPosition(); int rightPos = rightEncoder.getCurrentPosition();
// int frontPos = strafeEncoder.getCurrentPosition(); int frontPos = strafeEncoder.getCurrentPosition();
//
// lastEncPositions.clear(); lastEncPositions.clear();
// lastEncPositions.add(leftPos); lastEncPositions.add(leftPos);
// lastEncPositions.add(rightPos); lastEncPositions.add(rightPos);
// lastEncPositions.add(frontPos); lastEncPositions.add(frontPos);
//
// return Arrays.asList( return Arrays.asList(
// encoderTicksToInches(leftPos) * X_MULTIPLIER, encoderTicksToInches(leftPos) * X_MULTIPLIER,
// encoderTicksToInches(rightPos) * X_MULTIPLIER, encoderTicksToInches(rightPos) * X_MULTIPLIER,
// encoderTicksToInches(frontPos) * Y_MULTIPLIER encoderTicksToInches(frontPos) * Y_MULTIPLIER
// ); );
// } }
//
// @NonNull @NonNull
// @Override @Override
// public List<Double> getWheelVelocities() { public List<Double> getWheelVelocities() {
// int leftVel = (int) leftEncoder.getCorrectedVelocity(); int leftVel = (int) leftEncoder.getCorrectedVelocity();
// int rightVel = (int) rightEncoder.getCorrectedVelocity(); int rightVel = (int) rightEncoder.getCorrectedVelocity();
// int frontVel = (int) strafeEncoder.getCorrectedVelocity(); int frontVel = (int) strafeEncoder.getCorrectedVelocity();
//
// lastEncVels.clear(); lastEncVels.clear();
// lastEncVels.add(leftVel); lastEncVels.add(leftVel);
// lastEncVels.add(rightVel); lastEncVels.add(rightVel);
// lastEncVels.add(frontVel); lastEncVels.add(frontVel);
//
// return Arrays.asList( return Arrays.asList(
// encoderTicksToInches(leftVel) * X_MULTIPLIER, encoderTicksToInches(leftVel) * X_MULTIPLIER,
// encoderTicksToInches(rightVel) * X_MULTIPLIER, encoderTicksToInches(rightVel) * X_MULTIPLIER,
// encoderTicksToInches(frontVel) * Y_MULTIPLIER encoderTicksToInches(frontVel) * Y_MULTIPLIER
// ); );
// } }
//} }