Tested MechanumDrive. Added speed scaling. Minor bugfixes and checks.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/season/programs/samples/AutonomousUsingConfig/AutonomousConfigSelectorOpModeSample.java

@@ -10,7 +10,7 @@
 @Disabled
 public class AutonomousConfigSelectorOpModeSample extends BaseAutonomous {
 
-    //Robot to be used int the program
+    //Robot to be used in the program
     private AutonomousConfigSelectorBotSample robot;
 
     @Override

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/season/programs/samples/BasicSample/BasicSampleBot.java

@@ -4,6 +4,7 @@
 
 import org.firstinspires.ftc.teamcode.system.source.BaseRobot.Robot;
 import org.firstinspires.ftc.teamcode.system.subsystems.MechanumDrive;
+import org.firstinspires.ftc.teamcode.util.misc.Button;
 
 public class BasicSampleBot extends Robot {
 
@@ -18,8 +19,7 @@ public BasicSampleBot(OpMode opMode) {
         super(opMode);
         //sets the drive subSystem to tank drive. This one uses default params
         //drive = new TankDrive(this, new TankDrive.Params("MotorConfigLeft", "MotorConfigRight"));
-        drive = new MechanumDrive(this,new MechanumDrive.Params("forwardLeftMotor","forwardRightMotor","backLeftMotor","backRightMotor"));
-
+        drive = new MechanumDrive(this,new MechanumDrive.Params("forwardLeftMotor","forwardRightMotor","backLeftMotor","backRightMotor").setDriveType(MechanumDrive.DriveType.STANDARD_TTA).setTurnPIDCoeffs(0.1,0,0).setTTAStick(new Button(1, Button.VectorInputs.left_stick)));
          /*
         This is an example of how to setup TankDrive without using default params. Use .set(setting to set) to change a setting from default. Otherwise it will stay default.
         Remember to import button if uncomment this (try alt+enter)

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/season/programs/samples/GUISample/GUISampleProgram.java

@@ -31,5 +31,4 @@ protected void onUpdate() {
         robot.displayMenu1.addData("Look at my amazing data",7);
         robot.displayMenu2.addLine("This quote is plagiarised");
     }
-}
-
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/system/menus/ConfigMenu.java

@@ -368,6 +368,12 @@ else if(name.equals(ConfigCursor.BACK_BUTTON)) {
                         String[] data = parseOptionLine(lines.get(cursor.y));
 
                         List<ConfigParam> subsystemParams = config.get(selectedSubsystemName);
+
+                        if(subsystemParams == null) {
+                            Log.w("Config Menu Warning", "Selected Subsystem wasn't found! Setting config for that subsystem to an empty list.");
+                            subsystemParams = new ArrayList<>();
+                        }
+
                         ConfigParam currentParam = new ConfigParam("", new String[]{}, "");
 
                         for (ConfigParam param : subsystemParams) {
@@ -398,6 +404,12 @@ else if(name.equals(ConfigCursor.REVERSE_SELECT) && !lines.get(cursor.y).postSel
                     String[] data = parseOptionLine(lines.get(cursor.y));
 
                     List<ConfigParam> subsystemParams = config.get(selectedSubsystemName);
+
+                    if(subsystemParams == null) {
+                        Log.w("Config Menu Warning", "Selected Subsystem wasn't found! Setting config for that subsystem to an empty list.");
+                        subsystemParams = new ArrayList<>();
+                    }
+
                     ConfigParam currentParam = new ConfigParam("", new String[]{}, "");
 
                     for (ConfigParam param : subsystemParams) {
@@ -427,6 +439,12 @@ else if(name.equals(ConfigCursor.SWITCH_GAMEPAD) && !lines.get(cursor.y).postSel
                         currentOptionValue = unparsedLine.substring(unparsedLine.indexOf('|') + 1, unparsedLine.indexOf('|') + tempIdx).trim();
                         currentGamepadOptionValue = unparsedLine.substring(unparsedLine.indexOf('|') + tempIdx + 3).trim();
                         List<ConfigParam> subsystemParams = config.get(selectedSubsystemName);
+
+                        if(subsystemParams == null) {
+                            Log.w("Config Menu Warning", "Selected Subsystem wasn't found! Setting config for that subsystem to an empty list.");
+                            subsystemParams = new ArrayList<>();
+                        }
+
                         ConfigParam currentParam = new ConfigParam("", new String[]{}, "");
 
                         for (ConfigParam param : subsystemParams) {
@@ -590,7 +608,14 @@ private void setRootDirLines() {
     private void setConfigureSubsystemLines() {
         List<GuiLine> newLines = new ArrayList<>();
 
-        for(ConfigParam param : config.get(selectedSubsystemName)) {
+        List<ConfigParam> subsystemParams = config.get(selectedSubsystemName);
+
+        if(subsystemParams == null) {
+            Log.w("Config Menu Warning", "Selected Subsystem wasn't found! Setting config for that subsystem to an empty list.");
+            subsystemParams = new ArrayList<>();
+        }
+
+        for(ConfigParam param : subsystemParams) {
             newLines.add(new GuiLine("#",param.usesGamepad ? param.name+ " | " + param.currentOption + " | " + param.currentGamepadOption : param.name+ " | " + param.currentOption));
         }
         newLines.add(new GuiLine("#","Done"));
@@ -671,7 +696,14 @@ private void genDefaultConfigMap() {
         if(configState == ConfigurationState.AUTONOMOUS) {
             for (String subsystem : Robot.autonomousConfig.keySet()) {
                 List<ConfigParam> params = new ArrayList<>();
-                for (ConfigParam param : Robot.autonomousConfig.get(subsystem)) {
+                List<ConfigParam> autoConfig = Robot.autonomousConfig.get(subsystem);
+
+                if(autoConfig == null) {
+                    Log.w("Config Menu Warning", "Autonomous global config doesn't exist! Setting it to an empty list.");
+                    autoConfig = new ArrayList<>();
+                }
+
+                for (ConfigParam param : autoConfig) {
                     params.add(param.clone());
                 }
                 config.put(subsystem, params);
@@ -680,7 +712,14 @@ private void genDefaultConfigMap() {
         else  {
             for (String subsystem : Robot.teleopConfig.keySet()) {
                 List<ConfigParam> params = new ArrayList<>();
-                for (ConfigParam param : Robot.teleopConfig.get(subsystem)) {
+                List<ConfigParam> teleopConfig = Robot.teleopConfig.get(subsystem);
+
+                if(teleopConfig == null) {
+                    Log.w("Config Menu Warning", "Teleop global config doesn't exist! Setting it to an empty list.");
+                    teleopConfig = new ArrayList<>();
+                }
+
+                for (ConfigParam param : teleopConfig) {
                     params.add(param.clone());
                 }
                 config.put(subsystem, params);
@@ -702,7 +741,7 @@ private void updateConfigMapSubsystem(List<GuiLine> newConfig, String subsystemN
             config.get(subsystemName).get(i).name = data[0];
             config.get(subsystemName).get(i).currentOption = data[1];
 
-            if(config.get(subsystemName).get(i).usesGamepad) {
+            if (config.get(subsystemName).get(i).usesGamepad) {
                 config.get(subsystemName).get(i).currentGamepadOption = data[2];
             }
         }
@@ -725,7 +764,14 @@ private void exportConfigFile(String filepath) {
             Robot.autonomousConfig = new HashMap<>();
             for (String subsystem : config.keySet()) {
                 List<ConfigParam> params = new ArrayList<>();
-                for (ConfigParam param : config.get(subsystem)) {
+                List<ConfigParam> subsystemParams = config.get(subsystem);
+
+                if(subsystemParams == null) {
+                    Log.w("Config Menu Warning", "Autonomous config for subsystem " + subsystem + " does not exist! Setting the config to an empty list.");
+                    subsystemParams = new ArrayList<>();
+                }
+
+                for (ConfigParam param : subsystemParams) {
                     params.add(param.clone());
                 }
                 Robot.autonomousConfig.put(subsystem, params);
@@ -735,7 +781,14 @@ private void exportConfigFile(String filepath) {
             Robot.teleopConfig = new HashMap<>();
             for (String subsystem : config.keySet()) {
                 List<ConfigParam> params = new ArrayList<>();
-                for (ConfigParam param : config.get(subsystem)) {
+                List<ConfigParam> subsystemParams = config.get(subsystem);
+
+                if(subsystemParams == null) {
+                    Log.w("Config Menu Warning", "Teleop config for subsystem " + subsystem + " does not exist! Setting the config to an empty list.");
+                    subsystemParams = new ArrayList<>();
+                }
+
+                for (ConfigParam param : subsystemParams) {
                     params.add(param.clone());
                 }
                 Robot.teleopConfig.put(subsystem, params);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/system/source/BaseRobot/BaseAutonomous.java

@@ -32,20 +32,20 @@ public abstract class BaseAutonomous extends LinearOpMode {
      * Method that runs when the robot is initialized. It is not an abstract method so that it does not have to be implemented if it
      * is unneeded.
      */
-    protected void onInit() {};
+    protected void onInit() {}
 
     /**
      * Method that runs in a loop after the robot is initialized. It is not an abstract method so that it does not have to be implemented if it
      * is unneeded.
      */
-    protected void onInitLoop() {};
+    protected void onInitLoop() {}
 
 
     /**
      * Method that runs when the robot is stopped. It is not an abstract method so that it does not have to be implemented if it
      * is unneeded.
      */
-    protected void onStop(){}
+    protected void onStop() {}
 
     /**
      * An abstract method that contains the code for the robot to run.
@@ -104,7 +104,9 @@ protected final Robot getRobot() {
      */
     protected final void waitFor(long millis) {
         long stopTime = System.currentTimeMillis() + millis;
-        while (opModeIsActive() && System.currentTimeMillis() < stopTime);
+        while (opModeIsActive() && System.currentTimeMillis() < stopTime) {
+            sleep(1);
+        }
     }
 
     /**
@@ -117,6 +119,8 @@ protected final void waitFor(long millis) {
      * @param <X> - The second parameter's object type.
      */
     protected final <T,X> void waitFor(BiFunction<T,X,Boolean> condition, T param1, X param2) {
-        while (opModeIsActive() && !condition.apply(param1,param2));
+        while (opModeIsActive() && !condition.apply(param1,param2)) {
+            sleep(1);
+        }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/system/source/BaseRobot/BaseTeleop.java

@@ -11,6 +11,8 @@
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
+import org.firstinspires.ftc.teamcode.util.functional_interfaces.BiFunction;
+
 /**
  * An abstract class used to more easily create teleop programs
  */
@@ -97,4 +99,31 @@ public final void runOpMode() {
     protected final Robot getRobot(){
         return robot;
     }
+
+    /**
+     * Waits for a specified number of milliseconds.
+     *
+     * @param millis - The number of milliseconds to wait.
+     */
+    protected final void waitFor(long millis) {
+        long stopTime = System.currentTimeMillis() + millis;
+        while (opModeIsActive() && System.currentTimeMillis() < stopTime) {
+            sleep(1);
+        }
+    }
+
+    /**
+     * Waits for a boolean function with two inputs to return true. param1 and 2 must be updated from separate thread.
+     *
+     * @param condition - An arbitrary function taking two inputs and outputting a boolean.
+     * @param param1 - The function's first parameter.
+     * @param param2 - The function's second parameter.
+     * @param <T> - The first parameter's object type.
+     * @param <X> - The second parameter's object type.
+     */
+    protected final <T,X> void waitFor(BiFunction<T,X,Boolean> condition, T param1, X param2) {
+        while (opModeIsActive() && !condition.apply(param1,param2)) {
+            sleep(1);
+        }
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/system/source/BaseRobot/Robot.java

@@ -142,7 +142,7 @@ protected final void putSubSystem(String name, SubSystem subSystem)
      *
      * @param cycleButton - The button used to cycle through multiple menus in GUI.
      */
-    public final void startGui(Button cycleButton) {
+    protected final void startGui(Button cycleButton) {
         if(!useGui) {
             gui = new GUI(this, cycleButton);
             useGui = true;
@@ -170,6 +170,11 @@ public final void init()
         this.gamepad1 = opMode.gamepad1;
         this.gamepad2 = opMode.gamepad2;
 
+        File root = new File(Environment.getExternalStorageDirectory().getPath()+"/System64");
+        if(!root.exists()) {
+            Log.i("File Creation",root.mkdir() ? "Directory created!" : "File error, couldn't create directory");
+        }
+
         if(useConfig) {
 
             //create overall robot folder
@@ -185,6 +190,7 @@ public final void init()
                 Log.i("File Creation",autoDir.mkdir() ? "Directory created!" : "File error, couldn't create directory");
                 writeFile(autoDir.getPath() + "/robot_info.txt", "");
             }
+
             //create teleop directory in robot folder
             File teleopDir = new File(robotConfigDirectory.getPath() + "/teleop");
             if(!teleopDir.exists()) {
@@ -469,17 +475,51 @@ public final boolean isTeleop() {
         return opMode instanceof BaseTeleop;
     }
 
+    /**
+     * Gets if the program the robot is running is an autonomous program.
+     *
+     * @return Whether the program being run is an autonomous program.
+     */
     public final boolean isAutonomous() {
         return opMode instanceof BaseAutonomous;
     }
 
-    public final boolean isRunning() {
+    /**
+     * Gets whether the robot's current program is running.
+     *
+     * @return Whether the robot's current program is running.
+     */
+    public final boolean opModeIsActive() {
         if(!isTeleop() && !isAutonomous()) {
             throw new DumpsterFireException("Program is not an instance of BaseAutonomous or BaseTeleop, cannot tell if its running. A lot of other things are probably broken too if you're seeing this.");
         }
         return ((LinearOpMode) opMode).opModeIsActive();
     }
 
+    /**
+     * Gets whether the robot's current program has requested to be stopped.
+     *
+     * @return Whether the robot's current program has requested to be stopped.
+     */
+    public final boolean isStopRequested() {
+        if(!isTeleop() && !isAutonomous()) {
+            throw new DumpsterFireException("Program is not an instance of BaseAutonomous or BaseTeleop, cannot tell if its running. A lot of other things are probably broken too if you're seeing this.");
+        }
+        return ((LinearOpMode) opMode).isStopRequested();
+    }
+
+    /**
+     * Gets whether the robot's current program has been started.
+     *
+     * @return Whether the robot's current program has been started.
+     */
+    public final boolean isStarted() {
+        if(!isTeleop() && !isAutonomous()) {
+            throw new DumpsterFireException("Program is not an instance of BaseAutonomous or BaseTeleop, cannot tell if its running. A lot of other things are probably broken too if you're seeing this.");
+        }
+        return ((LinearOpMode) opMode).isStarted();
+    }
+
     /**
      * Writes data to a specified filepath. Creates the file if it doesn't exist, overwrites it if it does.
      *