TwoWheel + PinpointIMU Localizer
This commit is contained in:
BaronClaps
@ -0,0 +1,300 @@
|
||||
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
|
||||
|
||||
|
||||
/**
|
||||
* This is the TwoWheelLocalizer class. This class extends the Localizer superclass and is a
|
||||
* localizer that uses the two wheel odometry with IMU set up. The diagram below, which is modified from
|
||||
* Road Runner, shows a typical set up.
|
||||
*
|
||||
* The view is from the top of the robot looking downwards.
|
||||
*
|
||||
* left on robot is the y positive direction
|
||||
*
|
||||
* forward on robot is the x positive direction
|
||||
*
|
||||
* /--------------\
|
||||
* | ____ |
|
||||
* | ---- |
|
||||
* | || || |
|
||||
* | || || | ----> left (y positive)
|
||||
* | |
|
||||
* | |
|
||||
* \--------------/
|
||||
* |
|
||||
* |
|
||||
* V
|
||||
* forward (x positive)
|
||||
*
|
||||
* @author Anyi Lin - 10158 Scott's Bots
|
||||
* @author Havish Sripada - 12808 RevAmped Robotics
|
||||
* @author Baron Henderson - 20077 The Indubitables
|
||||
* @version 1.0, 11/28/2024
|
||||
*/
|
||||
@Config
|
||||
public class TwoWheelPinpointIMULocalizer extends Localizer {
|
||||
private HardwareMap hardwareMap;
|
||||
private GoBildaPinpointDriver pinpoint;
|
||||
private Pose startPose;
|
||||
private Pose displacementPose;
|
||||
private Pose currentVelocity;
|
||||
private Matrix prevRotationMatrix;
|
||||
private NanoTimer timer;
|
||||
private long deltaTimeNano;
|
||||
private Encoder forwardEncoder;
|
||||
private Encoder strafeEncoder;
|
||||
private Pose forwardEncoderPose;
|
||||
private Pose strafeEncoderPose;
|
||||
private double previousHeading;
|
||||
private double deltaRadians;
|
||||
private double totalHeading;
|
||||
public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
|
||||
public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
||||
|
||||
/**
|
||||
* This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
|
||||
* facing 0 heading.
|
||||
*
|
||||
* @param map the HardwareMap
|
||||
*/
|
||||
public TwoWheelPinpointIMULocalizer(HardwareMap map) {
|
||||
this(map, new Pose());
|
||||
}
|
||||
|
||||
/**
|
||||
* This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose
|
||||
* specifying the starting pose of the localizer.
|
||||
*
|
||||
* @param map the HardwareMap
|
||||
* @param setStartPose the Pose to start from
|
||||
*/
|
||||
public TwoWheelPinpointIMULocalizer(HardwareMap map, Pose setStartPose) {
|
||||
// TODO: replace these with your encoder positions
|
||||
forwardEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0);
|
||||
strafeEncoderPose = new Pose(-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90));
|
||||
|
||||
hardwareMap = map;
|
||||
|
||||
pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint");
|
||||
pinpoint.resetPosAndIMU();
|
||||
|
||||
// TODO: replace these with your encoder ports
|
||||
forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
|
||||
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
|
||||
|
||||
// TODO: reverse any encoders necessary
|
||||
forwardEncoder.setDirection(Encoder.REVERSE);
|
||||
strafeEncoder.setDirection(Encoder.FORWARD);
|
||||
|
||||
setStartPose(setStartPose);
|
||||
timer = new NanoTimer();
|
||||
deltaTimeNano = 1;
|
||||
displacementPose = new Pose();
|
||||
currentVelocity = new Pose();
|
||||
deltaRadians = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* This returns the current pose estimate.
|
||||
*
|
||||
* @return returns the current pose estimate as a Pose
|
||||
*/
|
||||
@Override
|
||||
public Pose getPose() {
|
||||
return MathFunctions.addPoses(startPose, displacementPose);
|
||||
}
|
||||
|
||||
/**
|
||||
* This returns the current velocity estimate.
|
||||
*
|
||||
* @return returns the current velocity estimate as a Pose
|
||||
*/
|
||||
@Override
|
||||
public Pose getVelocity() {
|
||||
return currentVelocity.copy();
|
||||
}
|
||||
|
||||
/**
|
||||
* This returns the current velocity estimate.
|
||||
*
|
||||
* @return returns the current velocity estimate as a Vector
|
||||
*/
|
||||
@Override
|
||||
public Vector getVelocityVector() {
|
||||
return currentVelocity.getVector();
|
||||
}
|
||||
|
||||
/**
|
||||
* This sets the start pose. Changing the start pose should move the robot as if all its
|
||||
* previous movements were displacing it from its new start pose.
|
||||
*
|
||||
* @param setStart the new start pose
|
||||
*/
|
||||
@Override
|
||||
public void setStartPose(Pose setStart) {
|
||||
startPose = setStart;
|
||||
}
|
||||
|
||||
/**
|
||||
* This sets the Matrix that contains the previous pose's heading rotation.
|
||||
*
|
||||
* @param heading the rotation of the Matrix
|
||||
*/
|
||||
public void setPrevRotationMatrix(double heading) {
|
||||
prevRotationMatrix = new Matrix(3, 3);
|
||||
prevRotationMatrix.set(0, 0, Math.cos(heading));
|
||||
prevRotationMatrix.set(0, 1, -Math.sin(heading));
|
||||
prevRotationMatrix.set(1, 0, Math.sin(heading));
|
||||
prevRotationMatrix.set(1, 1, Math.cos(heading));
|
||||
prevRotationMatrix.set(2, 2, 1.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* This sets the current pose estimate. Changing this should just change the robot's current
|
||||
* pose estimate, not anything to do with the start pose.
|
||||
*
|
||||
* @param setPose the new current pose estimate
|
||||
*/
|
||||
@Override
|
||||
public void setPose(Pose setPose) {
|
||||
displacementPose = MathFunctions.subtractPoses(setPose, startPose);
|
||||
resetEncoders();
|
||||
}
|
||||
|
||||
/**
|
||||
* This updates the elapsed time timer that keeps track of time between updates, as well as the
|
||||
* change position of the Encoders and the IMU readings. Then, the robot's global change in
|
||||
* position is calculated using the pose exponential method.
|
||||
*/
|
||||
@Override
|
||||
public void update() {
|
||||
deltaTimeNano = timer.getElapsedTime();
|
||||
timer.resetTimer();
|
||||
|
||||
updateEncoders();
|
||||
Matrix robotDeltas = getRobotDeltas();
|
||||
Matrix globalDeltas;
|
||||
setPrevRotationMatrix(getPose().getHeading());
|
||||
|
||||
Matrix transformation = new Matrix(3, 3);
|
||||
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
|
||||
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
|
||||
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
|
||||
transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
|
||||
transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
|
||||
transformation.set(2, 2, 1.0);
|
||||
} else {
|
||||
transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
|
||||
transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
|
||||
transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
|
||||
transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
|
||||
transformation.set(2, 2, 1.0);
|
||||
}
|
||||
|
||||
globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
|
||||
|
||||
displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
|
||||
currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9)));
|
||||
|
||||
totalHeading += globalDeltas.get(2, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* This updates the Encoders as well as the IMU.
|
||||
*/
|
||||
public void updateEncoders() {
|
||||
forwardEncoder.update();
|
||||
strafeEncoder.update();
|
||||
|
||||
pinpoint.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING);
|
||||
double currentHeading = startPose.getHeading() + MathFunctions.normalizeAngle(pinpoint.getHeading());
|
||||
deltaRadians = MathFunctions.getTurnDirection(previousHeading, currentHeading) * MathFunctions.getSmallestAngleDifference(currentHeading, previousHeading);
|
||||
previousHeading = currentHeading;
|
||||
}
|
||||
|
||||
/**
|
||||
* This resets the Encoders.
|
||||
*/
|
||||
public void resetEncoders() {
|
||||
forwardEncoder.reset();
|
||||
strafeEncoder.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* This calculates the change in position from the perspective of the robot using information
|
||||
* from the Encoders and IMU.
|
||||
*
|
||||
* @return returns a Matrix containing the robot relative movement.
|
||||
*/
|
||||
public Matrix getRobotDeltas() {
|
||||
Matrix returnMatrix = new Matrix(3, 1);
|
||||
// x/forward movement
|
||||
returnMatrix.set(0, 0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians));
|
||||
//y/strafe movement
|
||||
returnMatrix.set(1, 0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians));
|
||||
// theta/turning
|
||||
returnMatrix.set(2, 0, deltaRadians);
|
||||
return returnMatrix;
|
||||
}
|
||||
|
||||
/**
|
||||
* This returns how far the robot has turned in radians, in a number not clamped between 0 and
|
||||
* 2 * pi radians. This is used for some tuning things and nothing actually within the following.
|
||||
*
|
||||
* @return returns how far the robot has turned in total, in radians.
|
||||
*/
|
||||
public double getTotalHeading() {
|
||||
return totalHeading;
|
||||
}
|
||||
|
||||
/**
|
||||
* This returns the multiplier applied to forward movement measurement to convert from encoder
|
||||
* ticks to inches. This is found empirically through a tuner.
|
||||
*
|
||||
* @return returns the forward ticks to inches multiplier
|
||||
*/
|
||||
public double getForwardMultiplier() {
|
||||
return FORWARD_TICKS_TO_INCHES;
|
||||
}
|
||||
|
||||
/**
|
||||
* This returns the multiplier applied to lateral/strafe movement measurement to convert from
|
||||
* encoder ticks to inches. This is found empirically through a tuner.
|
||||
*
|
||||
* @return returns the lateral/strafe ticks to inches multiplier
|
||||
*/
|
||||
public double getLateralMultiplier() {
|
||||
return STRAFE_TICKS_TO_INCHES;
|
||||
}
|
||||
|
||||
/**
|
||||
* This returns the multiplier applied to turning movement measurement to convert from encoder
|
||||
* ticks to radians. This is found empirically through a tuner.
|
||||
*
|
||||
* @return returns the turning ticks to radians multiplier
|
||||
*/
|
||||
public double getTurningMultiplier() {
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* This resets the IMU.
|
||||
*/
|
||||
|
||||
public void resetIMU() {
|
||||
pinpoint.resetPosAndIMU();
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user