Use Pinpoint IMU for tuning

wheelerrobotics · Dec 1, 2024 · f48e8d4 · f48e8d4
1 parent 63c073c
commit f48e8d4
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 2 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
@@ -110,7 +110,7 @@ kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
 
     public final VoltageSensor voltageSensor;
 
-    public final LazyImu lazyImu;
+    public LazyImu lazyImu;
 
     public final Localizer localizer;
     public Pose2d pose;

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java
@@ -2,11 +2,15 @@
 
 
 
+import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.zyxOrientation;
+
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.PoseVelocity2d;
 import com.acmerobotics.roadrunner.ftc.FlightRecorder;
 import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriver;
 import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriverRR;
+import com.acmerobotics.roadrunner.ftc.LazyImu;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
@@ -22,6 +26,10 @@
  */
 public class PinpointDrive extends MecanumDrive {
     public static class Params {
+        /*
+        Set this to the name that your Pinpoint is configured as in your hardware config.
+         */
+        public String pinpointDeviceName = "pinpoint";
         /*
         Set the odometry pod positions relative to the point that the odometry computer tracks around.
         The X pod offset refers to how far sideways from the tracking point the
@@ -54,6 +62,15 @@ increase when you move the robot forward. And the Y (strafe) pod should increase
          */
         public GoBildaPinpointDriver.EncoderDirection xDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD;
         public GoBildaPinpointDriver.EncoderDirection yDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD;
+
+        /*
+        Use the pinpoint IMU for tuning
+        If true, overrides any IMU setting in MecanumDrive and uses exclusively Pinpoint for tuning
+        You can also use the pinpoint directly in MecanumDrive if this doesn't work for some reason;
+         replace "imu" with "pinpoint" or whatever your pinpoint is called in config.
+         Note: Pinpoint IMU is always used for base localization
+         */
+        public boolean usePinpointIMUForTuning = true;
     }
 
     public static Params PARAMS = new Params();
@@ -63,7 +80,11 @@ increase when you move the robot forward. And the Y (strafe) pod should increase
     public PinpointDrive(HardwareMap hardwareMap, Pose2d pose) {
         super(hardwareMap, pose);
         FlightRecorder.write("PINPOINT_PARAMS",PARAMS);
-        pinpoint = hardwareMap.get(GoBildaPinpointDriverRR.class,"pinpoint");
+        pinpoint = hardwareMap.get(GoBildaPinpointDriverRR.class,PARAMS.pinpointDeviceName);
+
+        if (PARAMS.usePinpointIMUForTuning) {
+            lazyImu = new LazyImu(hardwareMap, PARAMS.pinpointDeviceName, new RevHubOrientationOnRobot(zyxOrientation(0, 0, 0)));
+        }
 
         // RR localizer note: don't love this conversion (change driver?)
         pinpoint.setOffsets(DistanceUnit.MM.fromInches(PARAMS.xOffset), DistanceUnit.MM.fromInches(PARAMS.yOffset));