Merge pull request #19 from Logan-Nash/master
Commented out PinpointLocalizer.java
This commit is contained in:
Logan Nash
GitHub
@ -1,211 +1,211 @@
|
|||||||
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
|
//package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
|
||||||
|
//
|
||||||
|
//
|
||||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
//import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||||
|
//
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
//import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
//import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
|
//import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver;
|
//import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver;
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
|
//import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
//import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
||||||
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;
|
||||||
|
//
|
||||||
/**
|
///**
|
||||||
* This is the Pinpoint class. This class extends the Localizer superclass and is a
|
// * 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
|
// * 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.
|
// * 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.
|
// * The view is from the top of the robot looking downwards.
|
||||||
*
|
// *
|
||||||
* left on robot is the y positive direction
|
// * left on robot is the y positive direction
|
||||||
*
|
// *
|
||||||
* forward on robot is the x positive direction
|
// * forward on robot is the x positive direction
|
||||||
*
|
// *
|
||||||
* forward (x positive)
|
// * forward (x positive)
|
||||||
* △
|
// * △
|
||||||
* |
|
// * |
|
||||||
* |
|
// * |
|
||||||
* /--------------\
|
// * /--------------\
|
||||||
* | |
|
// * | |
|
||||||
* | |
|
// * | |
|
||||||
* | || |
|
// * | || |
|
||||||
* left (y positive) <--- | || |
|
// * left (y positive) <--- | || |
|
||||||
* | ____ |
|
// * | ____ |
|
||||||
* | ---- |
|
// * | ---- |
|
||||||
* \--------------/
|
// * \--------------/
|
||||||
* With the pinpoint your readings will be used in mm
|
// * With the pinpoint your readings will be used in mm
|
||||||
* to use inches ensure to divide your mm value by 25.4
|
// * to use inches ensure to divide your mm value by 25.4
|
||||||
* @author Logan Nash
|
// * @author Logan Nash
|
||||||
* @author Havish Sripada 12808 - RevAmped Robotics
|
// * @author Havish Sripada 12808 - RevAmped Robotics
|
||||||
* @author Ethan Doak - Gobilda
|
// * @author Ethan Doak - Gobilda
|
||||||
* @version 1.0, 10/2/2024
|
// * @version 1.0, 10/2/2024
|
||||||
*/
|
// */
|
||||||
public class PinpointLocalizer extends Localizer {
|
//public class PinpointLocalizer extends Localizer {
|
||||||
private HardwareMap hardwareMap;
|
// private HardwareMap hardwareMap;
|
||||||
private GoBildaPinpointDriver odo;
|
// private GoBildaPinpointDriver odo;
|
||||||
private double previousHeading;
|
// private double previousHeading;
|
||||||
private double totalHeading;
|
// private double totalHeading;
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This creates a new PinpointLocalizer from a HardwareMap, with a starting Pose at (0,0)
|
// * This creates a new PinpointLocalizer from a HardwareMap, with a starting Pose at (0,0)
|
||||||
* facing 0 heading.
|
// * facing 0 heading.
|
||||||
*
|
// *
|
||||||
* @param map the HardwareMap
|
// * @param map the HardwareMap
|
||||||
*/
|
// */
|
||||||
public PinpointLocalizer(HardwareMap map){ this(map, new Pose());}
|
// public PinpointLocalizer(HardwareMap map){ this(map, new Pose());}
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This creates a new PinpointLocalizer from a HardwareMap and a Pose, with the Pose
|
// * This creates a new PinpointLocalizer from a HardwareMap and a Pose, with the Pose
|
||||||
* specifying the starting pose of the localizer.
|
// * specifying the starting pose of the localizer.
|
||||||
*
|
// *
|
||||||
* @param map the HardwareMap
|
// * @param map the HardwareMap
|
||||||
* @param setStartPose the Pose to start from
|
// * @param setStartPose the Pose to start from
|
||||||
*/
|
// */
|
||||||
public PinpointLocalizer(HardwareMap map, Pose setStartPose){
|
// public PinpointLocalizer(HardwareMap map, Pose setStartPose){
|
||||||
hardwareMap = map;
|
// hardwareMap = map;
|
||||||
// TODO: replace this with your Pinpoint port
|
// // TODO: replace this with your Pinpoint port
|
||||||
odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo");
|
// odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo");
|
||||||
|
//
|
||||||
//This uses mm, to use inches divide these numbers by 25.4
|
// //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
|
// 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
|
// //TODO: If you find that the gobilda Yaw Scaling is incorrect you can edit this here
|
||||||
// odo.setYawScalar(1.0);
|
// // odo.setYawScalar(1.0);
|
||||||
//TODO: Set your encoder resolution here, I have the Gobilda Odometry products already included.
|
// //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
|
// //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(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
|
||||||
//odo.setEncoderResolution(13.26291192);
|
// //odo.setEncoderResolution(13.26291192);
|
||||||
//TODO: Set encoder directions
|
// //TODO: Set encoder directions
|
||||||
odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD);
|
// odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD);
|
||||||
|
//
|
||||||
odo.resetPosAndIMU();
|
// odo.resetPosAndIMU();
|
||||||
|
//
|
||||||
setStartPose(setStartPose);
|
// setStartPose(setStartPose);
|
||||||
totalHeading = 0;
|
// totalHeading = 0;
|
||||||
previousHeading = setStartPose.getHeading();
|
// previousHeading = setStartPose.getHeading();
|
||||||
|
//
|
||||||
resetPinpoint();
|
// resetPinpoint();
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This returns the current pose estimate.
|
// * This returns the current pose estimate.
|
||||||
*
|
// *
|
||||||
* @return returns the current pose estimate as a Pose
|
// * @return returns the current pose estimate as a Pose
|
||||||
*/
|
// */
|
||||||
@Override
|
// @Override
|
||||||
public Pose getPose() {
|
// public Pose getPose() {
|
||||||
Pose2D rawPose = odo.getPosition();
|
// Pose2D rawPose = odo.getPosition();
|
||||||
return new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS));
|
// return new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS));
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This returns the current velocity estimate.
|
// * This returns the current velocity estimate.
|
||||||
*
|
// *
|
||||||
* @return returns the current velocity estimate as a Pose
|
// * @return returns the current velocity estimate as a Pose
|
||||||
*/
|
// */
|
||||||
@Override
|
// @Override
|
||||||
public Pose getVelocity() {
|
// public Pose getVelocity() {
|
||||||
Pose2D pose = odo.getVelocity();
|
// Pose2D pose = odo.getVelocity();
|
||||||
return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS));
|
// return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS));
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This returns the current velocity estimate.
|
// * This returns the current velocity estimate.
|
||||||
*
|
// *
|
||||||
* @return returns the current velocity estimate as a Vector
|
// * @return returns the current velocity estimate as a Vector
|
||||||
*/
|
// */
|
||||||
@Override
|
// @Override
|
||||||
public Vector getVelocityVector() {
|
// public Vector getVelocityVector() {
|
||||||
Pose2D pose = odo.getVelocity();
|
// Pose2D pose = odo.getVelocity();
|
||||||
Vector returnVector = new Vector();
|
// Vector returnVector = new Vector();
|
||||||
returnVector.setOrthogonalComponents(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH));
|
// returnVector.setOrthogonalComponents(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH));
|
||||||
return returnVector;
|
// return returnVector;
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This sets the start pose. Since nobody should be using this after the robot has begun moving,
|
// * This sets the start pose. Since nobody should be using this after the robot has begun moving,
|
||||||
* and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose).
|
// * and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose).
|
||||||
*
|
// *
|
||||||
* @param setStart the new start pose
|
// * @param setStart the new start pose
|
||||||
*/
|
// */
|
||||||
@Override
|
// @Override
|
||||||
public void setStartPose(Pose setStart) {
|
// public void setStartPose(Pose setStart) {
|
||||||
odo.setPosition(new Pose2D(DistanceUnit.INCH, setStart.getX(), setStart.getY(), AngleUnit.RADIANS, setStart.getHeading()));
|
// odo.setPosition(new Pose2D(DistanceUnit.INCH, setStart.getX(), setStart.getY(), AngleUnit.RADIANS, setStart.getHeading()));
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This sets the current pose estimate. Changing this should just change the robot's current
|
// * 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.
|
// * pose estimate, not anything to do with the start pose.
|
||||||
*
|
// *
|
||||||
* @param setPose the new current pose estimate
|
// * @param setPose the new current pose estimate
|
||||||
*/
|
// */
|
||||||
@Override
|
// @Override
|
||||||
public void setPose(Pose setPose) {
|
// public void setPose(Pose setPose) {
|
||||||
resetPinpoint();
|
// resetPinpoint();
|
||||||
odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading()));
|
// odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading()));
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This updates the total heading of the robot. The Pinpoint handles all other updates itself.
|
// * This updates the total heading of the robot. The Pinpoint handles all other updates itself.
|
||||||
*/
|
// */
|
||||||
@Override
|
// @Override
|
||||||
public void update() {
|
// public void update() {
|
||||||
odo.update();
|
// odo.update();
|
||||||
totalHeading += MathFunctions.getSmallestAngleDifference(MathFunctions.normalizeAngle(odo.getHeading()), previousHeading);
|
// totalHeading += MathFunctions.getSmallestAngleDifference(MathFunctions.normalizeAngle(odo.getHeading()), previousHeading);
|
||||||
previousHeading = MathFunctions.normalizeAngle(odo.getHeading());
|
// previousHeading = MathFunctions.normalizeAngle(odo.getHeading());
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This returns how far the robot has turned in radians, in a number not clamped between 0 and
|
// * 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.
|
// * 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.
|
// * @return returns how far the robot has turned in total, in radians.
|
||||||
*/
|
// */
|
||||||
@Override
|
// @Override
|
||||||
public double getTotalHeading() {
|
// public double getTotalHeading() {
|
||||||
return totalHeading;
|
// return totalHeading;
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This returns the Y encoder value as none of the odometry tuners are required for this localizer
|
// * This returns the Y encoder value as none of the odometry tuners are required for this localizer
|
||||||
* @return returns the Y encoder value
|
// * @return returns the Y encoder value
|
||||||
*/
|
// */
|
||||||
@Override
|
// @Override
|
||||||
public double getForwardMultiplier() {
|
// public double getForwardMultiplier() {
|
||||||
return odo.getEncoderY();
|
// return odo.getEncoderY();
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This returns the X encoder value as none of the odometry tuners are required for this localizer
|
// * This returns the X encoder value as none of the odometry tuners are required for this localizer
|
||||||
* @return returns the X encoder value
|
// * @return returns the X encoder value
|
||||||
*/
|
// */
|
||||||
@Override
|
// @Override
|
||||||
public double getLateralMultiplier() {
|
// public double getLateralMultiplier() {
|
||||||
return odo.getEncoderX();
|
// return odo.getEncoderX();
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This returns either the factory tuned yaw scalar or the yaw scalar tuned by yourself.
|
// * This returns either the factory tuned yaw scalar or the yaw scalar tuned by yourself.
|
||||||
* @return returns the yaw scalar
|
// * @return returns the yaw scalar
|
||||||
*/
|
// */
|
||||||
@Override
|
// @Override
|
||||||
public double getTurningMultiplier() {
|
// public double getTurningMultiplier() {
|
||||||
return odo.getYawScalar();
|
// return odo.getYawScalar();
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This resets the IMU.
|
// * This resets the IMU.
|
||||||
*/
|
// */
|
||||||
@Override
|
// @Override
|
||||||
public void resetIMU() {
|
// public void resetIMU() {
|
||||||
odo.recalibrateIMU();
|
// odo.recalibrateIMU();
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
/**
|
// /**
|
||||||
* This resets the OTOS.
|
// * This resets the OTOS.
|
||||||
*/
|
// */
|
||||||
public void resetPinpoint(){
|
// public void resetPinpoint(){
|
||||||
odo.resetPosAndIMU();
|
// odo.resetPosAndIMU();
|
||||||
}
|
// }
|
||||||
}
|
//}
|
||||||
|
|||||||
Reference in New Issue
Block a user