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

Pinpoint is fixed Huzzah!!!

Browse Source 
Update PinpointLocalizer.java
This commit is contained in:
Logan Nash
2024-12-22 23:59:33 -05:00
committed by GitHub
parent 61f0b8afe9 11bad4df76
commit 06431bb98f
1 changed files with 244 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files

463
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java
View File

@ -1,220 +1,245 @@
//package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
//
//
//import com.qualcomm.robotcore.hardware.HardwareMap;
//
//import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
//import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
//import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
//import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver;
//import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
//import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
//
///**
// * This is the Pinpoint class. This class extends the Localizer superclass and is a
// * localizer that uses the two wheel odometry set up with the IMU to have more accurate heading
// * readings. 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
// *
// * forward (x positive)
// * △
// * |
// * |
// * /--------------\
// * | |
// * | |
// * | || |
// * left (y positive) <--- | || |
// * | ____ |
// * | ---- |
// * \--------------/
// * With the pinpoint your readings will be used in mm
// * to use inches ensure to divide your mm value by 25.4
// * @author Logan Nash
// * @author Havish Sripada 12808 - RevAmped Robotics
// * @author Ethan Doak - Gobilda
// * @version 1.0, 10/2/2024
// */
//public class PinpointLocalizer extends Localizer {
// private HardwareMap hardwareMap;
// private GoBildaPinpointDriver odo;
// private double previousHeading;
// private double totalHeading;
//
// /**
// * This creates a new PinpointLocalizer from a HardwareMap, with a starting Pose at (0,0)
// * facing 0 heading.
// *
// * @param map the HardwareMap
// */
// public PinpointLocalizer(HardwareMap map){ this(map, new Pose());}
//
// /**
// * This creates a new PinpointLocalizer 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 PinpointLocalizer(HardwareMap map, Pose setStartPose){
// hardwareMap = map;
// // TODO: replace this with your Pinpoint port
// odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo");
//
// //This uses mm, to use inches divide these numbers by 25.4
// odo.setOffsets(-84.0, -168.0); //these are tuned for 3110-0002-0001 Product Insight #1
// //TODO: If you find that the gobilda Yaw Scaling is incorrect you can edit this here
// // odo.setYawScalar(1.0);
// //TODO: Set your encoder resolution here, I have the Gobilda Odometry products already included.
// //TODO: If you would like to use your own odometry pods input the ticks per mm in the commented part below
// odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
// //odo.setEncoderResolution(13.26291192);
// //TODO: Set encoder directions
// odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD);
//
// resetPinpoint();;
// setStartPose(setStartPose);
// totalHeading = 0; import com.qualcomm.robotcore.hardware.HardwareMap;
// previousHeading = setStartPose.getHeading();
// } import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
// import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
// /** import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
// * This returns the current pose estimate. import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver;
// * import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
// * @return returns the current pose estimate as a Pose import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
// */ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
// @Override import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
// public Pose getPose() { import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
// Pose2D rawPose = odo.getPosition(); import org.opencv.core.Mat;
// return new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS));
// } /**
// * This is the Pinpoint class. This class extends the Localizer superclass and is a
// /** * localizer that uses the two wheel odometry set up with the IMU to have more accurate heading
// * This returns the current velocity estimate. * readings. The diagram below, which is modified from Road Runner, shows a typical set up.
// * *
// * @return returns the current velocity estimate as a Pose * The view is from the top of the robot looking downwards.
// */ *
// @Override * left on robot is the y positive direction
// public Pose getVelocity() { *
// Pose2D pose = odo.getVelocity(); * forward on robot is the x positive direction
// return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), odo.getHeadingVelocity()); *
// } * /--------------\
// * | ____ |
// /** * | ---- |
// * This returns the current velocity estimate. * | || |
// * * | || | ----> left (y positive)
// * @return returns the current velocity estimate as a Vector * | |
// */ * | |
// @Override * \--------------/
// public Vector getVelocityVector() { * |
// Pose2D pose = odo.getVelocity(); * |
// Vector returnVector = new Vector(); * V
// returnVector.setOrthogonalComponents(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH)); * forward (x positive)
// return returnVector; * With the pinpoint your readings will be used in mm
// } * to use inches ensure to divide your mm value by 25.4
// * @author Logan Nash
// /** * @author Havish Sripada 12808 - RevAmped Robotics
// * This sets the start pose. Since nobody should be using this after the robot has begun moving, * @author Ethan Doak - Gobilda
// * and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose). * @version 1.0, 10/2/2024
// * */
// * @param setStart the new start pose public class PinpointLocalizer extends Localizer {
// */ private HardwareMap hardwareMap;
// @Override private GoBildaPinpointDriver odo;
// public void setStartPose(Pose setStart) { private double previousHeading;
// odo.setPosition(new Pose2D(DistanceUnit.INCH, setStart.getX(), setStart.getY(), AngleUnit.RADIANS, setStart.getHeading())); private double totalHeading;
// } private Pose startPose;
// private long deltaTimeNano;
// /** private NanoTimer timer;
// * This sets the current pose estimate. Changing this should just change the robot's current private Pose currentVelocity;
// * pose estimate, not anything to do with the start pose. private Pose previousPinpointPose;
// *
// * @param setPose the new current pose estimate /**
// */ * This creates a new PinpointLocalizer from a HardwareMap, with a starting Pose at (0,0)
// @Override * facing 0 heading.
// public void setPose(Pose setPose) { *
// odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading())); * @param map the HardwareMap
// } */
// public PinpointLocalizer(HardwareMap map){ this(map, new Pose());}
// /**
// * This updates the total heading of the robot. The Pinpoint handles all other updates itself. /**
// */ * This creates a new PinpointLocalizer from a HardwareMap and a Pose, with the Pose
// @Override * specifying the starting pose of the localizer.
// public void update() { *
// odo.update(); * @param map the HardwareMap
// totalHeading += MathFunctions.getSmallestAngleDifference(MathFunctions.normalizeAngle(odo.getHeading()), previousHeading); * @param setStartPose the Pose to start from
// previousHeading = MathFunctions.normalizeAngle(odo.getHeading()); */
// } public PinpointLocalizer(HardwareMap map, Pose setStartPose){
// hardwareMap = map;
// /**
// * This returns how far the robot has turned in radians, in a number not clamped between 0 and odo = hardwareMap.get(GoBildaPinpointDriver.class,"pinpoint");
// * 2 * pi radians. This is used for some tuning things and nothing actually within the following.
// * //The default units are inches, but you can swap the units if you wish.
// * @return returns how far the robot has turned in total, in radians. //If you have already tuned the TwoWheelLocalizer, you can simply use the forwardEncoderPose's y value and strafeEncoderPose's x values.
// */ setOffsets(-2.815, 0.125, DistanceUnit.INCH); //these are tuned for 3110-0002-0001 Product Insight #1
// @Override
// public double getTotalHeading() { //TODO: Tune urself if needed
// return totalHeading; // odo.setYawScalar(1.0);
// }
// odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
// /** //odo.setEncoderResolution(13.26291192);
// * This returns the Y encoder value as none of the odometry tuners are required for this localizer
// * @return returns the Y encoder value odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.REVERSED, GoBildaPinpointDriver.EncoderDirection.FORWARD);
// */
// @Override resetPinpoint();
// public double getForwardMultiplier() {
// return odo.getEncoderY(); setStartPose(setStartPose);
// } totalHeading = 0;
// timer = new NanoTimer();
// /** previousPinpointPose = new Pose();
// * This returns the X encoder value as none of the odometry tuners are required for this localizer currentVelocity = new Pose();
// * @return returns the X encoder value deltaTimeNano = 1;
// */ previousHeading = setStartPose.getHeading();
// @Override }
// public double getLateralMultiplier() {
// return odo.getEncoderX(); /**
// } * This returns the current pose estimate.
// *
// /** * @return returns the current pose estimate as a Pose
// * This returns either the factory tuned yaw scalar or the yaw scalar tuned by yourself. */
// * @return returns the yaw scalar @Override
// */ public Pose getPose() {
// @Override return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(previousPinpointPose, startPose.getHeading(), false));
// public double getTurningMultiplier() { }
// return odo.getYawScalar();
// } /**
// * This returns the current velocity estimate.
// /** *
// * This resets the IMU. Note: This does not change the estimated heading orientation. * @return returns the current velocity estimate as a Pose
// */ */
// @Override @Override
// public void resetIMU() throws InterruptedException { public Pose getVelocity() {
// odo.recalibrateIMU(); return currentVelocity.copy();
// }
// try {
// Thread.sleep(300); /**
// } catch (InterruptedException e) { * This returns the current velocity estimate.
// throw new RuntimeException(e); *
// } * @return returns the current velocity estimate as a Vector
// } */
// @Override
// /** public Vector getVelocityVector() {
// * This resets the pinpoint. return currentVelocity.getVector();
// */ }
// private void resetPinpoint() {
// odo.resetPosAndIMU(); /**
// * This sets the start pose. Since nobody should be using this after the robot has begun moving,
// try { * and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose).
// Thread.sleep(300); *
// } catch (InterruptedException e) { * @param setStart the new start pose
// throw new RuntimeException(e); */
// } @Override
// } public void setStartPose(Pose setStart) {
//} this.startPose = setStart;
}
/**
* 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) {
Pose setNewPose = MathFunctions.subtractPoses(setPose, startPose);
odo.setPosition(new Pose2D(DistanceUnit.INCH, setNewPose.getX(), setNewPose.getY(), AngleUnit.RADIANS, setNewPose.getHeading()));
}
/**
* This updates the total heading of the robot. The Pinpoint handles all other updates itself.
*/
@Override
public void update() {
deltaTimeNano = timer.getElapsedTime();
timer.resetTimer();
odo.update();
Pose2D pinpointPose = odo.getPosition();
Pose currentPinpointPose = new Pose(pinpointPose.getX(DistanceUnit.INCH), pinpointPose.getY(DistanceUnit.INCH), pinpointPose.getHeading(AngleUnit.RADIANS));
totalHeading += MathFunctions.getSmallestAngleDifference(currentPinpointPose.getHeading(), previousHeading);
previousHeading = currentPinpointPose.getHeading();
Pose deltaPose = MathFunctions.subtractPoses(currentPinpointPose, previousPinpointPose);
currentVelocity = new Pose(deltaPose.getX() / (deltaTimeNano / Math.pow(10.0, 9)), deltaPose.getY() / (deltaTimeNano / Math.pow(10.0, 9)), deltaPose.getHeading() / (deltaTimeNano / Math.pow(10.0, 9)));
previousPinpointPose = currentPinpointPose;
}
/**
* 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.
*/
@Override
public double getTotalHeading() {
return totalHeading;
}
/**
* This returns the Y encoder value as none of the odometry tuners are required for this localizer
* @return returns the Y encoder value
*/
@Override
public double getForwardMultiplier() {
return odo.getEncoderY();
}
/**
* This returns the X encoder value as none of the odometry tuners are required for this localizer
* @return returns the X encoder value
*/
@Override
public double getLateralMultiplier() {
return odo.getEncoderX();
}
/**
* This returns either the factory tuned yaw scalar or the yaw scalar tuned by yourself.
* @return returns the yaw scalar
*/
@Override
public double getTurningMultiplier() {
return odo.getYawScalar();
}
/**
* This sets the offsets and converts inches to millimeters
* @param xOffset How far to the side from the center of the robot is the x-pod? Use positive values if it's to the left and negative if it's to the right.
* @param yOffset How far forward from the center of the robot is the y-pod? Use positive values if it's forward and negative if it's to the back.
* @param unit The units that the measurements are given in
*/
private void setOffsets(double xOffset, double yOffset, DistanceUnit unit) {
odo.setOffsets(unit.toMm(xOffset), unit.toMm(yOffset));
}
/**
* This resets the IMU. Does not change heading estimation.
*/
@Override
public void resetIMU() throws InterruptedException {
odo.recalibrateIMU();
try {
Thread.sleep(300);
} catch (InterruptedException e) {
throw new RuntimeException(e);
}
}
/**
* This resets the pinpoint.
*/
private void resetPinpoint() {
odo.resetPosAndIMU();
try {
Thread.sleep(300);
} catch (InterruptedException e) {
throw new RuntimeException(e);
}
}
}