diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java index 6e5e0150..39a497ac 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java +++ b/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; } } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/DrivingCommands.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/DrivingCommands.java new file mode 100644 index 00000000..2e11e466 --- /dev/null +++ b/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); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java index 16babd83..1602b2c3 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java @@ -5,25 +5,30 @@ 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; @@ -31,42 +36,67 @@ public JoystickDriveCommand( 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 diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java index c5095805..0bf7390a 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java +++ b/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)); } } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/HeadingHelper.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/HeadingHelper.java new file mode 100644 index 00000000..af82fa17 --- /dev/null +++ b/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; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java index e13fab12..5ec27188 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java +++ b/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;