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

Tuned localizer for Competition Robot

Browse Source
This commit is contained in:
Carlos
2024-09-17 17:16:31 -07:00
parent 2594ff79ca
commit 4ed1c8c615
3 changed files with 13 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java
View File

@ -69,7 +69,7 @@ public class PoseUpdater {
*/
public PoseUpdater(HardwareMap hardwareMap) {
// TODO: replace the second argument with your preferred localizer
this(hardwareMap, new ThreeWheelLocalizer(hardwareMap));
this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
}
/**

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java
View File

@ -65,8 +65,8 @@ public class ThreeWheelIMULocalizer extends Localizer {
private double previousIMUOrientation;
private double deltaRadians;
private double totalHeading;
public static double FORWARD_TICKS_TO_INCHES = 0.002957;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
public static double STRAFE_TICKS_TO_INCHES = -0.003127403096038503;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
public static double FORWARD_TICKS_TO_INCHES = 0.0038;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
public static double STRAFE_TICKS_TO_INCHES = -0.0033;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
public static double TURN_TICKS_TO_RADIANS = 0.002995;//8192 * 1.37795 * 2 * Math.PI * 0.5;
public static boolean useIMU = true;
@ -93,12 +93,12 @@ public class ThreeWheelIMULocalizer extends Localizer {
imu = hardwareMap.get(IMU.class, IMU);
// TODO: replace this with your IMU's orientation
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.LEFT, RevHubOrientationOnRobot.UsbFacingDirection.UP)));
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.DOWN, RevHubOrientationOnRobot.UsbFacingDirection.LEFT)));
// TODO: replace these with your encoder positions
leftEncoderPose = new Pose(-3, 5.7, 0);
rightEncoderPose = new Pose(-3, -5.7, 0);
strafeEncoderPose = new Pose(6.9, 1, Math.toRadians(90));
leftEncoderPose = new Pose(-7.625, 6.19375, 0);
rightEncoderPose = new Pose(-7.625, -6.19375, 0);
strafeEncoderPose = new Pose(7, 1, Math.toRadians(90));
// TODO: replace these with your encoder ports
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
@ -106,9 +106,9 @@ public class ThreeWheelIMULocalizer extends Localizer {
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
// TODO: reverse any encoders necessary
leftEncoder.setDirection(Encoder.REVERSE);
rightEncoder.setDirection(Encoder.FORWARD);
strafeEncoder.setDirection(Encoder.FORWARD);
//leftEncoder.setDirection(Encoder.REVERSE);
//rightEncoder.setDirection(Encoder.FORWARD);
//strafeEncoder.setDirection(Encoder.FORWARD);
setStartPose(setStartPose);
timer = new NanoTimer();

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java
View File

@ -92,9 +92,9 @@ public class ThreeWheelLocalizer extends Localizer {
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
// TODO: reverse any encoders necessary
leftEncoder.setDirection(Encoder.REVERSE);
rightEncoder.setDirection(Encoder.REVERSE);
strafeEncoder.setDirection(Encoder.FORWARD);
//leftEncoder.setDirection(Encoder.REVERSE);
// rightEncoder.setDirection(Encoder.REVERSE);
//strafeEncoder.setDirection(Encoder.FORWARD);
setStartPose(setStartPose);
timer = new NanoTimer();