update to 9.2, use official driver

wheelerrobotics · Sep 8, 2024 · 76027d0 · 76027d0
1 parent 6bbaf26
commit 76027d0
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 15 deletions.
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
@@ -35,8 +35,8 @@ repositories {
 dependencies {
     implementation project(':FtcRobotController')
 
-    implementation "com.github.jdhs-ftc:road-runner-ftc-otos:22dd547404"
-    implementation "com.acmerobotics.roadrunner:core:1.0.0-beta8"
-    implementation "com.acmerobotics.roadrunner:actions:1.0.0-beta8"
+    implementation "com.acmerobotics.roadrunner:core:1.0.0"
+    implementation "com.acmerobotics.roadrunner:actions:1.0.0"
     implementation "com.acmerobotics.dashboard:dashboard:0.4.15"
+    implementation "com.github.jdhs-ftc:road-runner-ftc-otos:4b219ff0c0"
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
@@ -217,10 +217,10 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
 
         // TODO: make sure your config has motors with these names (or change them)
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
-        leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
-        rightBack = hardwareMap.get(DcMotorEx.class, "rightBack");
-        rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
+        leftFront = hardwareMap.get(DcMotorEx.class, "left_front");
+        leftBack = hardwareMap.get(DcMotorEx.class, "left_back");
+        rightBack = hardwareMap.get(DcMotorEx.class, "right_back");
+        rightFront = hardwareMap.get(DcMotorEx.class, "right_front");
 
         leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SparkFunOTOSDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SparkFunOTOSDrive.java
@@ -9,8 +9,10 @@
 import com.acmerobotics.roadrunner.PoseVelocity2d;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.ftc.FlightRecorder;
-import com.acmerobotics.roadrunner.ftc.SparkFunOTOS;
+import com.qualcomm.hardware.sparkfun.SparkFunOTOS;
 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.teamcode.messages.PoseMessage;
 
 /**
@@ -65,8 +67,8 @@ public SparkFunOTOSDrive(HardwareMap hardwareMap, Pose2d pose) {
         super(hardwareMap, pose);
         otos = hardwareMap.get(SparkFunOTOS.class,"sensor_otos");
 
-        otos.setLinearUnit(SparkFunOTOS.LinearUnit.INCHES);
-        otos.setAngularUnit(SparkFunOTOS.AngularUnit.RADIANS);
+        otos.setLinearUnit(DistanceUnit.INCH);
+        otos.setAngularUnit(AngleUnit.RADIANS);
 
         otos.setOffset(PARAMS.offset);
         System.out.println("OTOS calibration beginning!");
@@ -102,9 +104,13 @@ public PoseVelocity2d updatePoseEstimate() {
             // the only alternative is to add getter and setters but that breaks compat
             otos.setPosition(RRPoseToOTOSPose(pose));
         }
-        SparkFunOTOS.Pose2D[] posVelList = otos.getPosVelCorrected();
-        SparkFunOTOS.Pose2D otosPose = posVelList[0];
-        SparkFunOTOS.Pose2D otosVel = posVelList[1];
+        // passed by reference
+        // reading acc is slightly worse (1ms) for loop times but oh well, this is what the driver supports
+        // might have to make a custom driver eventually
+        SparkFunOTOS.Pose2D otosPose = new SparkFunOTOS.Pose2D();
+        SparkFunOTOS.Pose2D otosVel = new SparkFunOTOS.Pose2D();
+        SparkFunOTOS.Pose2D otosAcc = new SparkFunOTOS.Pose2D();
+        otos.getPosVelAcc(otosPose,otosVel,otosAcc);
         pose = OTOSPoseToRRPose(otosPose);
         lastOtosPose = pose;
 
@@ -123,4 +129,6 @@ public PoseVelocity2d updatePoseEstimate() {
 
         return new PoseVelocity2d(new Vector2d(otosVel.x, otosVel.y),otosVel.h);
     }
+
+
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java
@@ -60,8 +60,8 @@ public static void register(OpModeManager manager) {
 
                 List<Encoder> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
                 List<Encoder> parEncs = new ArrayList<>(), perpEncs = new ArrayList<>();
-                parEncs.add(new OtosEncoder(od.otos,false, od.leftBack));
-                perpEncs.add(new OtosEncoder(od.otos,true, od.leftBack));
+                parEncs.add(new OtosEncoder(od.otos,false,false, od.leftBack));
+                perpEncs.add(new OtosEncoder(od.otos,true,false, od.leftBack));
 
                 return new DriveView(
                         DriveType.MECANUM,