LearnBot setup for X-Drive on ColinBot (#5)

* Getting the little Omni-based X-Drive bot working in LearnBot * Cleaned up LearnBot a fair bit, tested it out with an X-Drive drive base
technototes · Sep 19, 2024 · 9607109 · 9607109
1 parent b088e29
commit 9607109
Show file tree

Hide file tree

Showing 21 changed files with 70 additions and 82 deletions.
diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/ClawAndWristBot.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/ClawAndWristBot.java
@@ -4,7 +4,7 @@
 import com.technototes.library.hardware.servo.Servo;
 import org.firstinspires.ftc.learnbot.subsystems.ClawAndWristSubsystem;
 
-@Config
+//@Config
 public class ClawAndWristBot {
 
     public String CLAW1 = "claw1";

diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/Hardware.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/Hardware.java
@@ -67,8 +67,8 @@ public Hardware(HardwareMap hwmap) {
         }
         this.imu = new IMU(
             Setup.HardwareNames.IMU,
-            RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
-            RevHubOrientationOnRobot.UsbFacingDirection.UP
+            RevHubOrientationOnRobot.LogoFacingDirection.UP,
+            RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
         );
     }
 

diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/Robot.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/Robot.java
@@ -26,8 +26,7 @@ public Robot(Hardware hw) {
                 hw.frMotor,
                 hw.rlMotor,
                 hw.rrMotor,
-                hw.imu,
-                hw.distanceSensor
+                hw.imu
             );
         }
         if (Setup.Connected.TESTSUBSYSTEM) {

diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/Setup.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/Setup.java
@@ -7,7 +7,7 @@ public class Setup {
     @Config
     public static class Connected {
 
-        public static boolean DRIVEBASE = false;
+        public static boolean DRIVEBASE = true;
         public static boolean TESTSUBSYSTEM = false;
         public static boolean MOTOR = false;
         public static boolean SERVO = false;
@@ -32,4 +32,13 @@ public static class HardwareNames {
         public static String COLOR = "c";
         public static String CAMERA = "camera";
     }
+
+    @Config
+    public static class GlobalSettings {
+
+        public static double STICK_DEAD_ZONE = 0.1;
+        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;
+    }
 }
diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/commands/AnalogMotorControlCmd.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/commands/AnalogMotorControlCmd.java
@@ -5,13 +5,11 @@
 import com.technototes.library.logger.Log;
 import com.technototes.library.logger.Loggable;
 import java.util.function.DoubleSupplier;
+import org.firstinspires.ftc.learnbot.Setup;
 import org.firstinspires.ftc.learnbot.subsystems.MotorTestSubsystem;
 
-@Config
 public class AnalogMotorControlCmd implements Command, Loggable {
 
-    public static double STICK_DEAD_ZONE = 0.1;
-
     @Log(name = "")
     public String instruction = "RStick ^/v";
 
@@ -30,11 +28,13 @@ public AnalogMotorControlCmd(MotorTestSubsystem motorTest, DoubleSupplier axis)
     @Override
     public void execute() {
         double stickPos = ds.getAsDouble();
-        if (Math.abs(stickPos) < STICK_DEAD_ZONE) {
+        if (Math.abs(stickPos) < Setup.GlobalSettings.STICK_DEAD_ZONE) {
             stickPos = 0;
         } else {
             // Scale stickPos from deadzone to 1 and square it to make it a little easier to control
-            double scaled = (Math.abs(stickPos) - STICK_DEAD_ZONE) / (1 - STICK_DEAD_ZONE);
+            double scaled =
+                (Math.abs(stickPos) - Setup.GlobalSettings.STICK_DEAD_ZONE) /
+                (1 - Setup.GlobalSettings.STICK_DEAD_ZONE);
             // This gets the sign from stickPos, and puts it on scaled ^ 3
             stickPos = Math.copySign(scaled * scaled * scaled, stickPos);
         }

diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/commands/JoystickDriveCommand.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/commands/JoystickDriveCommand.java
@@ -1,20 +1,17 @@
 package org.firstinspires.ftc.learnbot.commands;
 
-import static org.firstinspires.ftc.learnbot.commands.AnalogMotorControlCmd.STICK_DEAD_ZONE;
-
 import com.acmerobotics.roadrunner.drive.DriveSignal;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 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.learnbot.Setup;
 import org.firstinspires.ftc.learnbot.subsystems.DrivebaseSubsystem;
 
-public class JoystickDriveCommand implements Command, Loggable {
+public class JoystickDriveCommand implements Command {
 
     public DrivebaseSubsystem subsystem;
     public DoubleSupplier x, y, r;
@@ -26,15 +23,15 @@ public JoystickDriveCommand(
         DrivebaseSubsystem sub,
         Stick xyStick,
         Stick rotStick,
-        DoubleSupplier strtDrive
+        DoubleSupplier straightDrive
     ) {
         addRequirements(sub);
         subsystem = sub;
         x = xyStick.getXSupplier();
         y = xyStick.getYSupplier();
         r = rotStick.getXSupplier();
         targetHeadingRads = -sub.getExternalHeading();
-        driveStraighten = strtDrive;
+        driveStraighten = straightDrive;
     }
 
     // Use this constructor if you don't want auto-straightening
@@ -48,27 +45,27 @@ private double getRotation(double headingInRads) {
         // Check to see if we're trying to straighten the robot
         if (
             driveStraighten == null ||
-            driveStraighten.getAsDouble() < DrivebaseSubsystem.DriveConstants.TRIGGER_THRESHOLD
+            driveStraighten.getAsDouble() < Setup.GlobalSettings.TRIGGER_THRESHOLD
         ) {
             // No straighten override: return the stick value
             // (with some adjustment...)
             return -Math.pow(r.getAsDouble(), 3) * subsystem.speed;
         } else {
             // 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.);
             // Dead zone of 5 degreesLiftHighJunctionCommand(liftSubsystem)
-            if (Math.abs(normalized) < STICK_DEAD_ZONE) {
+            if (Math.abs(normalized) < Setup.GlobalSettings.STRAIGHTEN_RANGE) {
                 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 Math.cbrt(normalized) * 0.3;
+            return normalized * Setup.GlobalSettings.STRAIGHTEN_SCALE_FACTOR; // Math.cbrt(normalized) * Setup.GlobalSettings.STRAIGHTEN_SCALE_FACTOR;
         }
     }
 

diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/controllers/DriverController.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/controllers/DriverController.java
@@ -6,6 +6,7 @@
 import com.technototes.library.control.CommandGamepad;
 import com.technototes.library.control.Stick;
 import org.firstinspires.ftc.learnbot.Robot;
+import org.firstinspires.ftc.learnbot.Setup;
 import org.firstinspires.ftc.learnbot.commands.EZCmd;
 import org.firstinspires.ftc.learnbot.commands.JoystickDriveCommand;
 
@@ -24,10 +25,9 @@ public DriverController(CommandGamepad g, Robot r) {
         gamepad = g;
 
         AssignNamedControllerButton();
-        //        if (Setup.Connected.DRIVEBASE) {
-        bindDriveControls();
-        //        }
-
+        if (Setup.Connected.DRIVEBASE) {
+            bindDriveControls();
+        }
     }
 
     private void AssignNamedControllerButton() {

diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/auto/Basic.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/auto/Basic.java
@@ -21,8 +21,8 @@
  * Wing position: 🪶
  * Backstage pos: 🎦
  */
-@Config
-@Autonomous(name = "Basic Auto")
+//@Config
+@Autonomous(name = "Basic Auto", preselectTeleOp = "PlainDrive")
 @SuppressWarnings("unused")
 public class Basic extends CommandOpMode implements Loggable {
 

diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/tele/AnalogMotorTest.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/tele/AnalogMotorTest.java
@@ -2,6 +2,7 @@
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.technototes.library.logger.Loggable;
 import com.technototes.library.structure.CommandOpMode;
@@ -10,6 +11,7 @@
 import org.firstinspires.ftc.learnbot.Setup;
 import org.firstinspires.ftc.learnbot.controllers.MotorController;
 
+@Disabled
 @TeleOp(name = "AnalogMotorTest")
 @SuppressWarnings("unused")
 public class AnalogMotorTest extends CommandOpMode implements Loggable {

diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/tele/ClawAndWrist.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/tele/ClawAndWrist.java
@@ -2,12 +2,14 @@
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.technototes.library.logger.Loggable;
 import com.technototes.library.structure.CommandOpMode;
 import org.firstinspires.ftc.learnbot.ClawAndWristBot;
 import org.firstinspires.ftc.learnbot.controllers.ClawAndWristController;
 
+@Disabled
 @TeleOp(name = "ClawAndWrist")
 @SuppressWarnings("unused")
 public class ClawAndWrist extends CommandOpMode implements Loggable {

diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/tele/PlainDrive.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/opmodes/tele/PlainDrive.java
@@ -3,10 +3,12 @@
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.technototes.library.command.CommandScheduler;
 import com.technototes.library.logger.Loggable;
 import com.technototes.library.structure.CommandOpMode;
 import org.firstinspires.ftc.learnbot.Hardware;
 import org.firstinspires.ftc.learnbot.Robot;
+import org.firstinspires.ftc.learnbot.commands.DriveCommand;
 import org.firstinspires.ftc.learnbot.controllers.DriverController;
 
 @TeleOp(name = "PlainDrive")

diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/subsystems/ClawAndWristSubsystem.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/subsystems/ClawAndWristSubsystem.java
@@ -7,7 +7,7 @@
 import com.technototes.library.logger.Loggable;
 import com.technototes.library.subsystem.Subsystem;
 
-@Config
+//@Config
 public class ClawAndWristSubsystem implements Subsystem, Loggable {
 
     public static double CLAW_STEP = 0.1;