From 0caeb3c72aec6a6cd2b4b237b056507c98502a6a Mon Sep 17 00:00:00 2001 From: j5155 <54331556+j5155@users.noreply.github.com> Date: Sat, 28 Sep 2024 14:42:19 -0800 Subject: [PATCH] Bump roadrunner-ftc and fix misc issues --- TeamCode/build.gradle | 2 +- .../ftc/teamcode/PinpointDrive.java | 12 +++--- .../SensorGoBildaPinpointExample.java | 7 ++-- .../teamcode/tuning/ManualFeedbackTuner.java | 38 +++++++++++-------- 4 files changed, 35 insertions(+), 24 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 1cba582e72c3..b80292b6ff0a 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -38,5 +38,5 @@ dependencies { implementation "com.acmerobotics.roadrunner:core:1.0.0" implementation "com.acmerobotics.roadrunner:actions:1.0.0" implementation "com.acmerobotics.dashboard:dashboard:0.4.16" - implementation "com.github.jdhs-ftc:road-runner-ftc-otos:447094304b" + implementation "com.github.jdhs-ftc:road-runner-ftc-otos:60262dd686" } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java index 9a83c6208327..7e05e2f33abd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java @@ -10,6 +10,7 @@ import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.FlightRecorder; import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriver; +import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriverRR; import com.qualcomm.hardware.sparkfun.SparkFunOTOS; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; @@ -47,7 +48,7 @@ the tracking point the Y (strafe) odometry pod is: forward of the center is a po To get this value from inPerTick, first convert the value to millimeters (multiply by 25.4) and then take its inverse (one over the value) */ - public double encoderResolution = GoBildaPinpointDriver.goBILDA_4_BAR_POD; + public double encoderResolution = GoBildaPinpointDriverRR.goBILDA_4_BAR_POD; /* Set the direction that each of the two odometry pods count. The X (forward) pod should @@ -59,12 +60,12 @@ increase when you move the robot forward. And the Y (strafe) pod should increase } public static Params PARAMS = new Params(); - public GoBildaPinpointDriver pinpoint; + public GoBildaPinpointDriverRR pinpoint; private Pose2d lastPinpointPose = pose; public PinpointDrive(HardwareMap hardwareMap, Pose2d pose) { super(hardwareMap, pose); - pinpoint = hardwareMap.get(GoBildaPinpointDriver.class,"pinpoint"); + pinpoint = hardwareMap.get(GoBildaPinpointDriverRR.class,"pinpoint"); // RR localizer note: don't love this conversion (change driver?) pinpoint.setOffsets(DistanceUnit.MM.fromInches(PARAMS.xOffset), DistanceUnit.MM.fromInches(PARAMS.yOffset)); @@ -72,6 +73,8 @@ public PinpointDrive(HardwareMap hardwareMap, Pose2d pose) { pinpoint.setEncoderResolution(PARAMS.encoderResolution); + pinpoint.setEncoderDirections(PARAMS.xDirection, PARAMS.yDirection); + /* Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary The IMU will automatically calibrate when first powered on, but recalibrating before running @@ -97,8 +100,7 @@ public PoseVelocity2d updatePoseEstimate() { // Potential alternate solution: timestamp the pose set and backtrack it based on speed? pinpoint.setPosition(pose); } - pinpoint.updatePoseAndVelocity(); // RR LOCALIZER NOTE: this is not ideal for loop times. - // Driver needs update to be optimized + pinpoint.update(); pose = pinpoint.getPositionRR(); lastPinpointPose = pose; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java index 75851aa830c9..9c999139c32d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java @@ -25,6 +25,7 @@ import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriver; +import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriverRR; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -63,7 +64,7 @@ //@Disabled public class SensorGoBildaPinpointExample extends LinearOpMode { - GoBildaPinpointDriver odo; // Declare OpMode member for the Odometry Computer + GoBildaPinpointDriverRR odo; // Declare OpMode member for the Odometry Computer double oldTime = 0; @@ -74,7 +75,7 @@ public void runOpMode() { // Initialize the hardware variables. Note that the strings used here must correspond // to the names assigned during the robot configuration step on the DS or RC devices. - odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo"); + odo = hardwareMap.get(GoBildaPinpointDriverRR.class,"odo"); /* Set the odometry pod positions relative to the point that the odometry computer tracks around. @@ -134,7 +135,7 @@ increase when you move the robot forward. And the Y (strafe) pod should increase Request a bulk update from the Pinpoint odometry computer. This checks almost all outputs from the device in a single I2C read. */ - odo.bulkUpdate(); + odo.update(); if (gamepad1.a){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java index ce1fc45228a5..10a4d5295bf8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java @@ -4,18 +4,26 @@ import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.MecanumDrive; -import org.firstinspires.ftc.teamcode.SparkFunOTOSDrive; -import org.firstinspires.ftc.teamcode.TankDrive; -import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer; -import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer; +import org.firstinspires.ftc.teamcode.*; public final class ManualFeedbackTuner extends LinearOpMode { public static double DISTANCE = 64; @Override public void runOpMode() throws InterruptedException { - if (TuningOpModes.DRIVE_CLASS.equals(SparkFunOTOSDrive.class)) { + if (TuningOpModes.DRIVE_CLASS.equals(PinpointDrive.class)) { + PinpointDrive drive = new PinpointDrive(hardwareMap, new Pose2d(0, 0, 0)); + + waitForStart(); + + while (opModeIsActive()) { + Actions.runBlocking( + drive.actionBuilder(new Pose2d(0, 0, 0)) + .lineToX(DISTANCE) + .lineToX(0) + .build()); + } + } else if (TuningOpModes.DRIVE_CLASS.equals(SparkFunOTOSDrive.class)) { SparkFunOTOSDrive drive = new SparkFunOTOSDrive(hardwareMap, new Pose2d(0, 0, 0)); waitForStart(); @@ -29,7 +37,7 @@ public void runOpMode() throws InterruptedException { } } else if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); - + if (drive.localizer instanceof TwoDeadWheelLocalizer) { if (TwoDeadWheelLocalizer.PARAMS.perpXTicks == 0 && TwoDeadWheelLocalizer.PARAMS.parYTicks == 0) { throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them."); @@ -43,10 +51,10 @@ public void runOpMode() throws InterruptedException { while (opModeIsActive()) { Actions.runBlocking( - drive.actionBuilder(new Pose2d(0, 0, 0)) - .lineToX(DISTANCE) - .lineToX(0) - .build()); + drive.actionBuilder(new Pose2d(0, 0, 0)) + .lineToX(DISTANCE) + .lineToX(0) + .build()); } } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) { TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); @@ -64,10 +72,10 @@ public void runOpMode() throws InterruptedException { while (opModeIsActive()) { Actions.runBlocking( - drive.actionBuilder(new Pose2d(0, 0, 0)) - .lineToX(DISTANCE) - .lineToX(0) - .build()); + drive.actionBuilder(new Pose2d(0, 0, 0)) + .lineToX(DISTANCE) + .lineToX(0) + .build()); } } else { throw new RuntimeException();