added drive base mechanum fixes from 16750 to Ptechnodactyl

Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java

@@ -47,5 +47,6 @@ public static class GlobalSettings {
         public static double STRAIGHTEN_SCALE_FACTOR = 0.25;
         public static double STRAIGHTEN_RANGE = .15; // Fraction of 45 degrees...
         public static double TRIGGER_THRESHOLD = 0.7;
+        public static double STRAIGHTEN_DEAD_ZONE = 0.08;
     }
 }

Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/DrivingCommands.java

@@ -0,0 +1,39 @@
+package org.firstinspires.ftc.ptechnodactyl.commands;
+
+import com.technototes.library.command.Command;
+import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem;
+
+public class DrivingCommands {
+
+    public static Command NormalDriving(DrivebaseSubsystem ds) {
+        return Command.create(ds::setNormalMode);
+    }
+
+    public static Command SnailDriving(DrivebaseSubsystem ds) {
+        return Command.create(ds::setSnailMode);
+    }
+
+    public static Command TurboDriving(DrivebaseSubsystem ds) {
+        return Command.create(ds::setTurboMode);
+    }
+
+    public static Command NormalSpeed(DrivebaseSubsystem ds) {
+        return Command.create(ds::auto);
+    }
+
+    public static Command SlowSpeed(DrivebaseSubsystem ds) {
+        return Command.create(ds::slow);
+    }
+
+    public static Command FastSpeed(DrivebaseSubsystem ds) {
+        return Command.create(ds::fast);
+    }
+
+    public static Command ResetGyro(DrivebaseSubsystem ds) {
+        return Command.create(ds::setExternalHeading, 0.0);
+    }
+
+    public static Command RecordHeading(DrivebaseSubsystem drive) {
+        return Command.create(drive::saveHeading);
+    }
+}

Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java

@@ -5,68 +5,98 @@
 import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.technototes.library.command.Command;
 import com.technototes.library.control.Stick;
+import com.technototes.library.logger.Loggable;
 import com.technototes.library.util.MathUtils;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 import org.firstinspires.ftc.ptechnodactyl.Setup;
 import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem;
 
-public class JoystickDriveCommand implements Command {
+public class JoystickDriveCommand implements Command, Loggable {
 
     public DrivebaseSubsystem subsystem;
     public DoubleSupplier x, y, r;
     public BooleanSupplier watchTrigger;
     public double targetHeadingRads;
     public DoubleSupplier driveStraighten;
+    public DoubleSupplier drive45;
+    public boolean driverDriving;
+    public boolean operatorDriving;
 
     public JoystickDriveCommand(
         DrivebaseSubsystem sub,
         Stick xyStick,
         Stick rotStick,
-        DoubleSupplier straightDrive
+        DoubleSupplier strtDrive,
+        DoubleSupplier angleDrive
     ) {
         addRequirements(sub);
         subsystem = sub;
         x = xyStick.getXSupplier();
         y = xyStick.getYSupplier();
         r = rotStick.getXSupplier();
         targetHeadingRads = -sub.getExternalHeading();
-        driveStraighten = straightDrive;
+        driveStraighten = strtDrive;
+        drive45 = angleDrive;
+        driverDriving = true;
+        operatorDriving = false;
     }
 
     // Use this constructor if you don't want auto-straightening
     public JoystickDriveCommand(DrivebaseSubsystem sub, Stick xyStick, Stick rotStick) {
-        this(sub, xyStick, rotStick, null);
+        this(sub, xyStick, rotStick, null, null);
     }
 
     // This will make the bot snap to an angle, if the 'straighten' button is pressed
     // Otherwise, it just reads the rotation value from the rotation stick
     private double getRotation(double headingInRads) {
         // Check to see if we're trying to straighten the robot
-        if (
-            driveStraighten == null ||
-            driveStraighten.getAsDouble() < Setup.GlobalSettings.TRIGGER_THRESHOLD
-        ) {
+        double normalized = 0.0;
+        boolean straightTrigger;
+        boolean fortyfiveTrigger;
+        straightTrigger = isTriggered(driveStraighten);
+        fortyfiveTrigger = isTriggered(drive45);
+        if (!straightTrigger && !fortyfiveTrigger) {
             // No straighten override: return the stick value
             // (with some adjustment...)
             return -Math.pow(r.getAsDouble(), 3) * subsystem.speed;
-        } else {
+        }
+        if (straightTrigger) {
             // headingInRads is [0-2pi]
             double heading = -Math.toDegrees(headingInRads);
+            // Snap to the closest 90 or 270 degree angle (for going through the depot)
             double close = MathUtils.closestTo(heading, 0, 90, 180, 270, 360);
             double offBy = close - heading;
             // Normalize the error to -1 to 1
-            double normalized = Math.max(Math.min(offBy / 45, 1.), -1.);
+            normalized = Math.max(Math.min(offBy / 45, 1.), -1.);
             // Dead zone of 5 degreesLiftHighJunctionCommand(liftSubsystem)
-            if (Math.abs(normalized) < Setup.GlobalSettings.STRAIGHTEN_RANGE) {
+            if (Math.abs(normalized) < Setup.GlobalSettings.STRAIGHTEN_DEAD_ZONE) {
                 return 0.0;
             }
-            // Fix the range to be from (abs) dead_zone => 1 to scal from 0 to 1
-            // Scale it by the cube root, the scale that down by 30%
-            // .9 (about 40 degrees off) provides .96 power => .288
-            // .1 (about 5 degrees off) provides .46 power => .14
-            return normalized * Setup.GlobalSettings.STRAIGHTEN_SCALE_FACTOR; // Math.cbrt(normalized) * Setup.GlobalSettings.STRAIGHTEN_SCALE_FACTOR;
+        } else {
+            // headingInRads is [0-2pi]
+            double heading45 = -Math.toDegrees(headingInRads);
+            // Snap to the closest 90 or 270 degree angle (for going through the depot)
+            double close45 = MathUtils.closestTo(heading45, 45, 135, 225, 315);
+            double offBy45 = close45 - heading45;
+            // Normalize the error to -1 to 1
+            normalized = Math.max(Math.min(offBy45 / 45, 1.), -1.);
+            // Dead zone of 5 degreesLiftHighJunctionCommand(liftSubsystem)
+            if (Math.abs(normalized) < Setup.GlobalSettings.STRAIGHTEN_DEAD_ZONE) {
+                return 0.0;
+            }
+        }
+        // Scale it by the cube root, the scale that down by 30%
+        // .9 (about 40 degrees off) provides .96 power => .288
+        // .1 (about 5 degrees off) provides .46 power => .14
+        return Math.cbrt(normalized) * 0.3;
+    }
+
+    public static boolean isTriggered(DoubleSupplier ds) {
+        if (ds == null || ds.getAsDouble() < Setup.GlobalSettings.TRIGGER_THRESHOLD) {
+            return false;
         }
+        return true;
     }
 
     @Override

Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java

@@ -7,7 +7,7 @@
 import com.technototes.library.control.Stick;
 import org.firstinspires.ftc.ptechnodactyl.Robot;
 import org.firstinspires.ftc.ptechnodactyl.Setup;
-import org.firstinspires.ftc.ptechnodactyl.commands.EZCmd;
+import org.firstinspires.ftc.ptechnodactyl.commands.DrivingCommands;
 import org.firstinspires.ftc.ptechnodactyl.commands.JoystickDriveCommand;
 
 public class DriverController {
@@ -19,10 +19,12 @@ public class DriverController {
     public CommandButton resetGyroButton, turboButton, snailButton;
     public CommandButton override;
     public CommandAxis driveStraighten;
+    public CommandAxis drive45;
 
     public DriverController(CommandGamepad g, Robot r) {
         this.robot = r;
         gamepad = g;
+        override = g.leftTrigger.getAsButton(0.5);
 
         AssignNamedControllerButton();
         if (Setup.Connected.DRIVEBASE) {
@@ -35,6 +37,7 @@ private void AssignNamedControllerButton() {
         driveLeftStick = gamepad.leftStick;
         driveRightStick = gamepad.rightStick;
         driveStraighten = gamepad.rightTrigger;
+        drive45 = gamepad.leftTrigger;
         turboButton = gamepad.leftBumper;
         snailButton = gamepad.rightBumper;
     }
@@ -45,14 +48,15 @@ public void bindDriveControls() {
                 robot.drivebaseSubsystem,
                 driveLeftStick,
                 driveRightStick,
-                driveStraighten
+                driveStraighten,
+                drive45
             )
         );
-        turboButton.whenPressed(EZCmd.Drive.TurboMode(robot.drivebaseSubsystem));
-        turboButton.whenReleased(EZCmd.Drive.NormalMode(robot.drivebaseSubsystem));
-        snailButton.whenPressed(EZCmd.Drive.SnailMode(robot.drivebaseSubsystem));
-        snailButton.whenReleased(EZCmd.Drive.NormalMode(robot.drivebaseSubsystem));
 
-        resetGyroButton.whenPressed(EZCmd.Drive.ResetGyro(robot.drivebaseSubsystem));
+        turboButton.whenPressed(DrivingCommands.TurboDriving(robot.drivebaseSubsystem));
+        turboButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem));
+        snailButton.whenPressed(DrivingCommands.SnailDriving(robot.drivebaseSubsystem));
+        snailButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem));
+        resetGyroButton.whenPressed(DrivingCommands.ResetGyro(robot.drivebaseSubsystem));
     }
 }

Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/HeadingHelper.java

@@ -0,0 +1,75 @@
+package org.firstinspires.ftc.ptechnodactyl.helpers;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import org.firstinspires.ftc.robotcontroller.internal.FtcRobotControllerActivity;
+
+@Config
+public class HeadingHelper {
+
+    public static double DEFAULT_START_HEADING = 180;
+    public static double DEFAULT_START_X = 53;
+    public static double DEFAULT_START_Y = 63;
+    public static int EXPIRATION_TIME = 20;
+
+    public double headingUpdateTime;
+    public double lastHeading;
+    public double lastXPosition;
+    public double lastYPosition;
+
+    public HeadingHelper(double lastX, double lastY, double heading) {
+        headingUpdateTime = System.currentTimeMillis() / 1000.0;
+        lastXPosition = lastX;
+        lastYPosition = lastY;
+        lastHeading = heading;
+    }
+
+    public static void saveHeading(double x, double y, double h) {
+        FtcRobotControllerActivity.SaveBetweenRuns = new HeadingHelper(x, y, h);
+    }
+
+    public static void savePose(Pose2d p) {
+        saveHeading(p.getX(), p.getY(), p.getHeading());
+    }
+
+    public static void clearSavedInfo() {
+        FtcRobotControllerActivity.SaveBetweenRuns = null;
+    }
+
+    public static boolean validHeading() {
+        HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns;
+        if (hh == null) {
+            return false;
+        }
+        double now = System.currentTimeMillis() / 1000.0;
+        return now < hh.headingUpdateTime + EXPIRATION_TIME;
+    }
+
+    public static Pose2d getSavedPose() {
+        return new Pose2d(getSavedX(), getSavedY(), getSavedHeading());
+    }
+
+    public static double getSavedHeading() {
+        HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns;
+        if (hh != null) {
+            return hh.lastHeading;
+        }
+        return DEFAULT_START_HEADING;
+    }
+
+    public static double getSavedX() {
+        HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns;
+        if (hh != null) {
+            return hh.lastXPosition;
+        }
+        return DEFAULT_START_X;
+    }
+
+    public static double getSavedY() {
+        HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns;
+        if (hh != null) {
+            return hh.lastYPosition;
+        }
+        return DEFAULT_START_Y;
+    }
+}

Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java

@@ -13,6 +13,7 @@
 import com.technototes.path.subsystem.PathingMecanumDrivebaseSubsystem;
 import java.util.function.Supplier;
 import org.firstinspires.ftc.ptechnodactyl.Setup;
+import org.firstinspires.ftc.ptechnodactyl.helpers.HeadingHelper;
 
 public class DrivebaseSubsystem
     extends PathingMecanumDrivebaseSubsystem
@@ -188,6 +189,10 @@ public void setSnailMode() {
         Turbo = false;
     }
 
+    public void saveHeading() {
+        HeadingHelper.saveHeading(get().getX(), get().getY(), gyro.getHeading());
+    }
+
     public void setTurboMode() {
         Turbo = true;
         Snail = false;