add PinpointDrive to TuningOpModes

wheelerrobotics · Nov 18, 2024 · 7208a1e · 7208a1e
1 parent 7d6ccd9
commit 7208a1e
Showing 1 changed file with 39 additions and 20 deletions.
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
@@ -4,37 +4,21 @@
 import com.acmerobotics.dashboard.config.reflection.ReflectionConfig;
 import com.acmerobotics.roadrunner.MotorFeedforward;
 import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.ftc.AngularRampLogger;
-import com.acmerobotics.roadrunner.ftc.DeadWheelDirectionDebugger;
-import com.acmerobotics.roadrunner.ftc.DriveType;
-import com.acmerobotics.roadrunner.ftc.DriveView;
-import com.acmerobotics.roadrunner.ftc.DriveViewFactory;
-import com.acmerobotics.roadrunner.ftc.Encoder;
-import com.acmerobotics.roadrunner.ftc.ForwardPushTest;
-import com.acmerobotics.roadrunner.ftc.ForwardRampLogger;
-import com.acmerobotics.roadrunner.ftc.LateralPushTest;
-import com.acmerobotics.roadrunner.ftc.LateralRampLogger;
-import com.acmerobotics.roadrunner.ftc.ManualFeedforwardTuner;
-import com.acmerobotics.roadrunner.ftc.MecanumMotorDirectionDebugger;
-import com.acmerobotics.roadrunner.ftc.OtosEncoder;
+import com.acmerobotics.roadrunner.ftc.*;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
 import com.qualcomm.robotcore.eventloop.opmode.OpModeRegistrar;
 
 import org.firstinspires.ftc.robotcore.internal.opmode.OpModeMeta;
-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.*;
 
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
 
 public final class TuningOpModes {
-    public static final Class<?> DRIVE_CLASS = SparkFunOTOSDrive.class;
+    public static final Class<?> DRIVE_CLASS = SparkFunOTOSDrive.class; // TODO: change to your drive class i.e. PinpointDrive if using pinpoint
 
     public static final String GROUP = "quickstart";
     public static final boolean DISABLED = false;
@@ -54,7 +38,42 @@ public static void register(OpModeManager manager) {
         if (DISABLED) return;
 
         DriveViewFactory dvf;
-        if (DRIVE_CLASS.equals(SparkFunOTOSDrive.class)) {
+        if (DRIVE_CLASS.equals(PinpointDrive.class)) {
+                dvf = hardwareMap -> {
+                    PinpointDrive pd = new PinpointDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+                    List<Encoder> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
+                    List<Encoder> parEncs = new ArrayList<>(), perpEncs = new ArrayList<>();
+                    parEncs.add(new PinpointEncoder(pd.pinpoint,false, PinpointDrive.PARAMS.xDirection == GoBildaPinpointDriver.EncoderDirection.FORWARD, pd.leftBack));
+                    perpEncs.add(new PinpointEncoder(pd.pinpoint,true,PinpointDrive.PARAMS.yDirection == GoBildaPinpointDriver.EncoderDirection.FORWARD, pd.leftBack));
+
+                    return new DriveView(
+                            DriveType.MECANUM,
+                            MecanumDrive.PARAMS.inPerTick,
+                            MecanumDrive.PARAMS.maxWheelVel,
+                            MecanumDrive.PARAMS.minProfileAccel,
+                            MecanumDrive.PARAMS.maxProfileAccel,
+                            hardwareMap.getAll(LynxModule.class),
+                            Arrays.asList(
+                                    pd.leftFront,
+                                    pd.leftBack
+                            ),
+                            Arrays.asList(
+                                    pd.rightFront,
+                                    pd.rightBack
+                            ),
+                            leftEncs,
+                            rightEncs,
+                            parEncs,
+                            perpEncs,
+                            pd.lazyImu,
+                            pd.voltageSensor,
+                            () -> new MotorFeedforward(MecanumDrive.PARAMS.kS,
+                                    MecanumDrive.PARAMS.kV / MecanumDrive.PARAMS.inPerTick,
+                                    MecanumDrive.PARAMS.kA / MecanumDrive.PARAMS.inPerTick)
+                    );
+                };
+        } else if (DRIVE_CLASS.equals(SparkFunOTOSDrive.class)) {
             dvf = hardwareMap -> {
                 SparkFunOTOSDrive od = new SparkFunOTOSDrive(hardwareMap, new Pose2d(0, 0, 0));