uncommented Road Runner adapter files
This commit is contained in:
brotherhobo
@ -1,93 +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<Integer> lastTrackingEncPositions = new ArrayList<>();
|
||||
// List<Integer> 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;
|
||||
// }
|
||||
//}
|
||||
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<Integer> lastTrackingEncPositions = new ArrayList<>();
|
||||
List<Integer> 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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,125 +1,125 @@
|
||||
//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
|
||||
//
|
||||
//import com.acmerobotics.roadrunner.util.NanoClock;
|
||||
//import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
//import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
//
|
||||
///**
|
||||
// * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding
|
||||
// * slot's motor direction
|
||||
// */
|
||||
//public class RoadRunnerEncoder {
|
||||
// private final static int CPS_STEP = 0x10000;
|
||||
//
|
||||
// private static double inverseOverflow(double input, double estimate) {
|
||||
// // convert to uint16
|
||||
// int real = (int) input & 0xffff;
|
||||
// // 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
|
||||
// real += ((real % 20) / 4) * CPS_STEP;
|
||||
// // 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;
|
||||
// return real;
|
||||
// }
|
||||
//
|
||||
// public enum Direction {
|
||||
// FORWARD(1),
|
||||
// REVERSE(-1);
|
||||
//
|
||||
// private int multiplier;
|
||||
//
|
||||
// Direction(int multiplier) {
|
||||
// this.multiplier = multiplier;
|
||||
// }
|
||||
//
|
||||
// public int getMultiplier() {
|
||||
// return multiplier;
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// private DcMotorEx motor;
|
||||
// private NanoClock clock;
|
||||
//
|
||||
// private Direction direction;
|
||||
//
|
||||
// private int lastPosition;
|
||||
// private int velocityEstimateIdx;
|
||||
// private double[] velocityEstimates;
|
||||
// private double lastUpdateTime;
|
||||
//
|
||||
// public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) {
|
||||
// this.motor = motor;
|
||||
// this.clock = clock;
|
||||
//
|
||||
// this.direction = Direction.FORWARD;
|
||||
//
|
||||
// this.lastPosition = 0;
|
||||
// this.velocityEstimates = new double[3];
|
||||
// this.lastUpdateTime = clock.seconds();
|
||||
// }
|
||||
//
|
||||
// public RoadRunnerEncoder(DcMotorEx motor) {
|
||||
// this(motor, NanoClock.system());
|
||||
// }
|
||||
//
|
||||
// public Direction getDirection() {
|
||||
// return direction;
|
||||
// }
|
||||
//
|
||||
// private int getMultiplier() {
|
||||
// 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
|
||||
// * @param direction either reverse or forward depending on if encoder counts should be negated
|
||||
// */
|
||||
// public void setDirection(Direction direction) {
|
||||
// this.direction = 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
|
||||
// *
|
||||
// * @return encoder position
|
||||
// */
|
||||
// public int getCurrentPosition() {
|
||||
// int multiplier = getMultiplier();
|
||||
// int currentPosition = motor.getCurrentPosition() * multiplier;
|
||||
// if (currentPosition != lastPosition) {
|
||||
// double currentTime = clock.seconds();
|
||||
// double dt = currentTime - lastUpdateTime;
|
||||
// velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
|
||||
// velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
|
||||
// lastPosition = currentPosition;
|
||||
// lastUpdateTime = currentTime;
|
||||
// }
|
||||
// return currentPosition;
|
||||
// }
|
||||
//
|
||||
// /**
|
||||
// * 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)
|
||||
// *
|
||||
// * @return raw velocity
|
||||
// */
|
||||
// public double getRawVelocity() {
|
||||
// int multiplier = getMultiplier();
|
||||
// return motor.getVelocity() * multiplier;
|
||||
// }
|
||||
//
|
||||
// /**
|
||||
// * 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.
|
||||
// * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
|
||||
// *
|
||||
// * @return corrected velocity
|
||||
// */
|
||||
// public double getCorrectedVelocity() {
|
||||
// double median = velocityEstimates[0] > velocityEstimates[1]
|
||||
// ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
|
||||
// : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
|
||||
// return inverseOverflow(getRawVelocity(), median);
|
||||
// }
|
||||
//}
|
||||
package org.firstinspires.ftc.teamcode.pedroPathing.localization;
|
||||
|
||||
import com.acmerobotics.roadrunner.util.NanoClock;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
|
||||
/**
|
||||
* Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding
|
||||
* slot's motor direction
|
||||
*/
|
||||
public class RoadRunnerEncoder {
|
||||
private final static int CPS_STEP = 0x10000;
|
||||
|
||||
private static double inverseOverflow(double input, double estimate) {
|
||||
// convert to uint16
|
||||
int real = (int) input & 0xffff;
|
||||
// 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
|
||||
real += ((real % 20) / 4) * CPS_STEP;
|
||||
// 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;
|
||||
return real;
|
||||
}
|
||||
|
||||
public enum Direction {
|
||||
FORWARD(1),
|
||||
REVERSE(-1);
|
||||
|
||||
private int multiplier;
|
||||
|
||||
Direction(int multiplier) {
|
||||
this.multiplier = multiplier;
|
||||
}
|
||||
|
||||
public int getMultiplier() {
|
||||
return multiplier;
|
||||
}
|
||||
}
|
||||
|
||||
private DcMotorEx motor;
|
||||
private NanoClock clock;
|
||||
|
||||
private Direction direction;
|
||||
|
||||
private int lastPosition;
|
||||
private int velocityEstimateIdx;
|
||||
private double[] velocityEstimates;
|
||||
private double lastUpdateTime;
|
||||
|
||||
public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) {
|
||||
this.motor = motor;
|
||||
this.clock = clock;
|
||||
|
||||
this.direction = Direction.FORWARD;
|
||||
|
||||
this.lastPosition = 0;
|
||||
this.velocityEstimates = new double[3];
|
||||
this.lastUpdateTime = clock.seconds();
|
||||
}
|
||||
|
||||
public RoadRunnerEncoder(DcMotorEx motor) {
|
||||
this(motor, NanoClock.system());
|
||||
}
|
||||
|
||||
public Direction getDirection() {
|
||||
return direction;
|
||||
}
|
||||
|
||||
private int getMultiplier() {
|
||||
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
|
||||
* @param direction either reverse or forward depending on if encoder counts should be negated
|
||||
*/
|
||||
public void setDirection(Direction direction) {
|
||||
this.direction = 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
|
||||
*
|
||||
* @return encoder position
|
||||
*/
|
||||
public int getCurrentPosition() {
|
||||
int multiplier = getMultiplier();
|
||||
int currentPosition = motor.getCurrentPosition() * multiplier;
|
||||
if (currentPosition != lastPosition) {
|
||||
double currentTime = clock.seconds();
|
||||
double dt = currentTime - lastUpdateTime;
|
||||
velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
|
||||
velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
|
||||
lastPosition = currentPosition;
|
||||
lastUpdateTime = currentTime;
|
||||
}
|
||||
return currentPosition;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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)
|
||||
*
|
||||
* @return raw velocity
|
||||
*/
|
||||
public double getRawVelocity() {
|
||||
int multiplier = getMultiplier();
|
||||
return motor.getVelocity() * multiplier;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
|
||||
*
|
||||
* @return corrected velocity
|
||||
*/
|
||||
public double getCorrectedVelocity() {
|
||||
double median = velocityEstimates[0] > velocityEstimates[1]
|
||||
? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
|
||||
: Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
|
||||
return inverseOverflow(getRawVelocity(), median);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,117 +1,117 @@
|
||||
//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
|
||||
//
|
||||
//import androidx.annotation.NonNull;
|
||||
//
|
||||
//import com.acmerobotics.dashboard.config.Config;
|
||||
//import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
//import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
|
||||
//import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
//import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
//
|
||||
//import java.util.Arrays;
|
||||
//import java.util.List;
|
||||
//
|
||||
///*
|
||||
// * Sample tracking wheel localizer implementation assuming the standard configuration:
|
||||
// *
|
||||
// * left on robot is y pos
|
||||
// *
|
||||
// * front of robot is x pos
|
||||
// *
|
||||
// * /--------------\
|
||||
// * | ____ |
|
||||
// * | ---- |
|
||||
// * | || || |
|
||||
// * | || || |
|
||||
// * | |
|
||||
// * | |
|
||||
// * \--------------/
|
||||
// *
|
||||
// */
|
||||
//
|
||||
///**
|
||||
// * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will
|
||||
// * be replaced with a custom localizer.
|
||||
// */
|
||||
//@Config
|
||||
//public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer {
|
||||
// public static double TICKS_PER_REV = 8192;
|
||||
// public static double WHEEL_RADIUS = 1.37795; // in
|
||||
// public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
|
||||
//
|
||||
// public static double X_MULTIPLIER = 0.5008239963;
|
||||
// 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;
|
||||
//
|
||||
// private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder;
|
||||
//
|
||||
// private List<Integer> lastEncPositions, lastEncVels;
|
||||
//
|
||||
// public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels) {
|
||||
// super(Arrays.asList(
|
||||
// new Pose2d(leftX, leftY, 0), // left
|
||||
// new Pose2d(rightX, rightY, 0), // right
|
||||
// new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe
|
||||
// ));
|
||||
//
|
||||
// lastEncPositions = lastTrackingEncPositions;
|
||||
// lastEncVels = lastTrackingEncVels;
|
||||
//
|
||||
// // TODO: redo the configs here
|
||||
// leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
|
||||
// rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront"));
|
||||
// strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
|
||||
//
|
||||
// // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
|
||||
// leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
|
||||
// rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
|
||||
// strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD);
|
||||
// }
|
||||
//
|
||||
// public void resetHeading(double heading) {
|
||||
// setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading));
|
||||
// }
|
||||
//
|
||||
// public static double encoderTicksToInches(double ticks) {
|
||||
// return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
|
||||
// }
|
||||
//
|
||||
// @NonNull
|
||||
// @Override
|
||||
// public List<Double> getWheelPositions() {
|
||||
// int leftPos = leftEncoder.getCurrentPosition();
|
||||
// int rightPos = rightEncoder.getCurrentPosition();
|
||||
// int frontPos = strafeEncoder.getCurrentPosition();
|
||||
//
|
||||
// lastEncPositions.clear();
|
||||
// lastEncPositions.add(leftPos);
|
||||
// lastEncPositions.add(rightPos);
|
||||
// lastEncPositions.add(frontPos);
|
||||
//
|
||||
// return Arrays.asList(
|
||||
// encoderTicksToInches(leftPos) * X_MULTIPLIER,
|
||||
// encoderTicksToInches(rightPos) * X_MULTIPLIER,
|
||||
// encoderTicksToInches(frontPos) * Y_MULTIPLIER
|
||||
// );
|
||||
// }
|
||||
//
|
||||
// @NonNull
|
||||
// @Override
|
||||
// public List<Double> getWheelVelocities() {
|
||||
// int leftVel = (int) leftEncoder.getCorrectedVelocity();
|
||||
// int rightVel = (int) rightEncoder.getCorrectedVelocity();
|
||||
// int frontVel = (int) strafeEncoder.getCorrectedVelocity();
|
||||
//
|
||||
// lastEncVels.clear();
|
||||
// lastEncVels.add(leftVel);
|
||||
// lastEncVels.add(rightVel);
|
||||
// lastEncVels.add(frontVel);
|
||||
//
|
||||
// return Arrays.asList(
|
||||
// encoderTicksToInches(leftVel) * X_MULTIPLIER,
|
||||
// encoderTicksToInches(rightVel) * X_MULTIPLIER,
|
||||
// encoderTicksToInches(frontVel) * Y_MULTIPLIER
|
||||
// );
|
||||
// }
|
||||
//}
|
||||
package org.firstinspires.ftc.teamcode.pedroPathing.localization;
|
||||
|
||||
import androidx.annotation.NonNull;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
/*
|
||||
* Sample tracking wheel localizer implementation assuming the standard configuration:
|
||||
*
|
||||
* left on robot is y pos
|
||||
*
|
||||
* front of robot is x pos
|
||||
*
|
||||
* /--------------\
|
||||
* | ____ |
|
||||
* | ---- |
|
||||
* | || || |
|
||||
* | || || |
|
||||
* | |
|
||||
* | |
|
||||
* \--------------/
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will
|
||||
* be replaced with a custom localizer.
|
||||
*/
|
||||
@Config
|
||||
public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer {
|
||||
public static double TICKS_PER_REV = 8192;
|
||||
public static double WHEEL_RADIUS = 1.37795; // in
|
||||
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
|
||||
|
||||
public static double X_MULTIPLIER = 0.5008239963;
|
||||
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;
|
||||
|
||||
private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder;
|
||||
|
||||
private List<Integer> lastEncPositions, lastEncVels;
|
||||
|
||||
public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels) {
|
||||
super(Arrays.asList(
|
||||
new Pose2d(leftX, leftY, 0), // left
|
||||
new Pose2d(rightX, rightY, 0), // right
|
||||
new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe
|
||||
));
|
||||
|
||||
lastEncPositions = lastTrackingEncPositions;
|
||||
lastEncVels = lastTrackingEncVels;
|
||||
|
||||
// TODO: redo the configs here
|
||||
leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
|
||||
rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront"));
|
||||
strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
|
||||
|
||||
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
|
||||
leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
|
||||
rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
|
||||
strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD);
|
||||
}
|
||||
|
||||
public void resetHeading(double heading) {
|
||||
setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading));
|
||||
}
|
||||
|
||||
public static double encoderTicksToInches(double ticks) {
|
||||
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
|
||||
}
|
||||
|
||||
@NonNull
|
||||
@Override
|
||||
public List<Double> getWheelPositions() {
|
||||
int leftPos = leftEncoder.getCurrentPosition();
|
||||
int rightPos = rightEncoder.getCurrentPosition();
|
||||
int frontPos = strafeEncoder.getCurrentPosition();
|
||||
|
||||
lastEncPositions.clear();
|
||||
lastEncPositions.add(leftPos);
|
||||
lastEncPositions.add(rightPos);
|
||||
lastEncPositions.add(frontPos);
|
||||
|
||||
return Arrays.asList(
|
||||
encoderTicksToInches(leftPos) * X_MULTIPLIER,
|
||||
encoderTicksToInches(rightPos) * X_MULTIPLIER,
|
||||
encoderTicksToInches(frontPos) * Y_MULTIPLIER
|
||||
);
|
||||
}
|
||||
|
||||
@NonNull
|
||||
@Override
|
||||
public List<Double> getWheelVelocities() {
|
||||
int leftVel = (int) leftEncoder.getCorrectedVelocity();
|
||||
int rightVel = (int) rightEncoder.getCorrectedVelocity();
|
||||
int frontVel = (int) strafeEncoder.getCorrectedVelocity();
|
||||
|
||||
lastEncVels.clear();
|
||||
lastEncVels.add(leftVel);
|
||||
lastEncVels.add(rightVel);
|
||||
lastEncVels.add(frontVel);
|
||||
|
||||
return Arrays.asList(
|
||||
encoderTicksToInches(leftVel) * X_MULTIPLIER,
|
||||
encoderTicksToInches(rightVel) * X_MULTIPLIER,
|
||||
encoderTicksToInches(frontVel) * Y_MULTIPLIER
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user