Added experimental Hub led control, stubbed automous engines, added Robot#reportStatus for Robot to select 'worst' status

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java

@@ -1,9 +1,9 @@
 package org.timecrafters.minibots.cyberarm.chiron;
 
-import static org.timecrafters.minibots.cyberarm.chiron.Robot.Status;
-
 import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.Blinker;
 import com.qualcomm.robotcore.hardware.ColorSensor;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -21,18 +21,23 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Act
 import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
 import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;
 
+import java.util.ArrayList;
+import java.util.concurrent.TimeUnit;
+
 public class Robot {
     public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
     public final ServoImplEx gripper, wrist;
     public final IMU imu;
     public final ColorSensor indicatorA, indicatorB;
+    public LynxModule expansionHub;
 
     public final double imuAngleOffset;
     public boolean wristManuallyControlled = false;
     public boolean automaticAntiTipActive = false;
     public boolean hardwareFault = false;
 
-    public Status status = Status.OKAY;
+    private Status status = Status.OKAY, lastStatus = Status.OKAY;
+    private ArrayList<Status> reportedStatuses = new ArrayList<>();
 
     public enum ArmPosition {
         COLLECT,
@@ -152,6 +157,20 @@ public class Robot {
 
         imu.initialize(parameters);
 
+        // BulkRead from Hubs
+        for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+
+            if (!hub.isParent() && expansionHub == null) {
+                expansionHub = hub;
+            }
+        }
+
+        // Set LED pattern
+        if (expansionHub != null) {
+            expansionHub.setPattern(ledPatternOkay());
+        }
+
         // INITIALIZE AFTER EVERYTHING ELSE to prevent use before set crashes
         this.fieldLocalizer.setRobot(this);
         this.fieldLocalizer.standardSetup();
@@ -162,6 +181,7 @@ public class Robot {
 
         // STATUS
         engine.telemetry.addLine("DATA");
+        engine.telemetry.addData("      Robot Status", status);
         engine.telemetry.addData("      Hardware Fault", hardwareFault);
         engine.telemetry.addLine();
 
@@ -280,10 +300,70 @@ public class Robot {
         engine.telemetry.addLine();
     }
 
-    public void update() {
+    private ArrayList<Blinker.Step> ledPatternOkay() {
-        status = Status.OKAY;
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
 
-        // TODO: Handle status priority; That is, store reported statuses and select the "worst" one as status
+        steps.add(new Blinker.Step(0x00aa00, 500, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x00aa44, 500, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternMonitoring() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0xaaaaaa, 500, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x000000, 250, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternWarning() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0xffff00, 500, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0xff8800, 500, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternDanger() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0xff0000, 250, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x000000, 100, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    public void reportStatus(Status status) {
+        reportedStatuses.add(status);
+    }
+
+    public void update() {
+        for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
+            hub.clearBulkCache();
+        }
+
+        status = Status.OKAY;
+        for (Status s : reportedStatuses) {
+            if (s.ordinal() > status.ordinal()) {
+                status = s;
+            }
+        }
+
+        reportedStatuses.clear();
+
+        if (status != lastStatus) {
+            lastStatus = status;
+
+            if (expansionHub != null) {
+                if (lastStatus == Status.OKAY) { expansionHub.setPattern(ledPatternOkay()); }
+                if (lastStatus == Status.MONITORING) { expansionHub.setPattern(ledPatternMonitoring()); }
+                if (lastStatus == Status.WARNING) { expansionHub.setPattern(ledPatternWarning()); }
+                if (lastStatus == Status.DANGER) { expansionHub.setPattern(ledPatternDanger()); }
+            }
+        }
 
         automaticLEDStatus();
     }
@@ -322,6 +402,8 @@ public class Robot {
         }
     }
 
+    public Status getStatus() { return status; }
+
     public double getRadius() { return radius; }
 
     public double getDiameter() { return diameter; }
@@ -334,11 +416,11 @@ public class Robot {
     public int unitToTicks(DistanceUnit unit, double distance) {
         double fI = (gearRatio * ticksPerRevolution) / (wheelRadius * 2 * Math.PI * (gearRatio * ticksPerRevolution) / (gearRatio * ticksPerRevolution));
 
-        double inches = unit.toInches(unit.fromUnit(unit, distance)); // NOTE: UNTESTED
+        double inches = unit.toInches(unit.fromUnit(unit, distance));
 
         double ticks = fI * inches;
 
-        return (int)ticks; // NOTE: UNTESTED
+        return (int)ticks;
     }
 
     // For: Drive Wheels
@@ -346,7 +428,7 @@ public class Robot {
         // Convert to inches, then to unit.
         double inches = wheelRadius * 2 * Math.PI * ticks / (gearRatio * ticksPerRevolution);
 
-        return unit.fromUnit(DistanceUnit.INCH, inches); // NOTE: UNTESTED
+        return unit.fromUnit(DistanceUnit.INCH, inches);
     }
 
     // For: Arm

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java

@@ -1,25 +1,27 @@
 package org.timecrafters.minibots.cyberarm.chiron.engines;
 
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 import org.timecrafters.minibots.cyberarm.chiron.Robot;
 import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;
 
-@Autonomous(name = "CHIRON | RED Left Side", group = "CHIRON", preselectTeleOp = "CHIRON | TeleOp")
+public class AutonomousEngine extends CyberarmEngine {
-public class AutonomousRedLeftSideEngine extends CyberarmEngine {
+    protected Robot robot;
-    private Robot robot;
+    protected FieldLocalizer fieldLocalizer;
-    private FieldLocalizer fieldLocalizer;
+    protected TimeCraftersConfiguration configuration;
-    private TimeCraftersConfiguration configuration;
+
+    protected String configurationName = "CHIRON";
+    protected String actionsGroupName;
+    protected String tuningGroupName;
+    protected String tuningActionName;
 
     @Override
     public void setup() {
-        configuration = new TimeCraftersConfiguration("CHIRON");
+        configuration = new TimeCraftersConfiguration(configurationName);
 
         fieldLocalizer = new FieldLocalizer(
-                configuration.variable("Autonomous", "Tuning_Red_LeftSide", "starting_position_x_in_inches").value(),
+                configuration.variable(tuningGroupName, tuningActionName, "starting_position_x_in_inches").value(),
-                configuration.variable("Autonomous", "Tuning_Red_LeftSide", "starting_position_y_in_inches").value()
+                configuration.variable(tuningGroupName, tuningActionName, "starting_position_y_in_inches").value()
         );
 
         robot = new Robot(
@@ -37,11 +39,13 @@ public class AutonomousRedLeftSideEngine extends CyberarmEngine {
                 "org.timecrafters.minibots.cyberarm.chiron.states.autonomous",
                 robot,
                 Robot.class,
-                "AutonomousRedLeftSide");
+                actionsGroupName);
     }
 
     @Override
     public void loop() {
+        robot.update();
+
         super.loop();
 
         robot.standardTelemetry();

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java

@@ -0,0 +1,17 @@
+package org.timecrafters.minibots.cyberarm.chiron.engines.autonomous_engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
+
+//@Autonomous(name = "CHIRON | BLUE Left Side", group = "CHIRON", preselectTeleOp = "CHIRON | TeleOp")
+public class AutonomousBlueLeftSideEngine extends AutonomousEngine {
+    @Override
+    public void setup() {
+        actionsGroupName = "AutonomousBlueLeftSide";
+        tuningGroupName = "Autonomous";
+        tuningActionName = "Tuning_Blue_LeftSide";
+
+        super.setup();
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java

@@ -0,0 +1,17 @@
+package org.timecrafters.minibots.cyberarm.chiron.engines.autonomous_engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
+
+//@Autonomous(name = "CHIRON | BLUE Right Side", group = "CHIRON", preselectTeleOp = "CHIRON | TeleOp")
+public class AutonomousBlueRightSideEngine extends AutonomousEngine {
+    @Override
+    public void setup() {
+        actionsGroupName = "AutonomousBlueRightSide";
+        tuningGroupName = "Autonomous";
+        tuningActionName = "Tuning_Blue_RightSide";
+
+        super.setup();
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java

@@ -0,0 +1,17 @@
+package org.timecrafters.minibots.cyberarm.chiron.engines.autonomous_engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
+
+@Autonomous(name = "CHIRON | RED Left Side", group = "CHIRON", preselectTeleOp = "CHIRON | TeleOp")
+public class AutonomousRedLeftSideEngine extends AutonomousEngine {
+    @Override
+    public void setup() {
+       actionsGroupName = "AutonomousRedLeftSide";
+        tuningGroupName = "Autonomous";
+        tuningActionName = "Tuning_Red_LeftSide";
+
+        super.setup();
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java

@@ -0,0 +1,17 @@
+package org.timecrafters.minibots.cyberarm.chiron.engines.autonomous_engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
+
+//@Autonomous(name = "CHIRON | RED Right Side", group = "CHIRON", preselectTeleOp = "CHIRON | TeleOp")
+public class AutonomousRedRightSideEngine extends AutonomousEngine {
+    @Override
+    public void setup() {
+        actionsGroupName = "AutonomousRedRightSide";
+        tuningGroupName = "Autonomous";
+        tuningActionName = "Tuning_Red_RightSide";
+
+        super.setup();
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java

@@ -46,7 +46,7 @@ public class ArmDriverControl extends CyberarmState {
             return;
         }
 
-        robot.status = Robot.Status.WARNING;
+        robot.reportStatus(Robot.Status.WARNING);
 
         double stepInterval = robot.tuningConfig("arm_manual_step_interval").value();
         int stepSize = robot.tuningConfig("arm_manual_step_size").value();
@@ -112,7 +112,7 @@ public class ArmDriverControl extends CyberarmState {
 
     private void automaticHardwareMonitor() {
         if (robot.hardwareFault) {
-            robot.status = Robot.Status.DANGER;
+            robot.reportStatus(Robot.Status.DANGER);
 
             stopArm();
         } else {
@@ -126,7 +126,7 @@ public class ArmDriverControl extends CyberarmState {
             return;
         }
 
-        robot.status = Robot.Status.WARNING;
+        robot.reportStatus(Robot.Status.WARNING);
 
         switch (position) {
             case COLLECT:

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java

@@ -26,8 +26,6 @@ public class DrivetrainDriverControl extends CyberarmState {
 
     @Override
     public void exec() {
-        robot.update();
-
         move();
 
         automatics();
@@ -113,7 +111,7 @@ public class DrivetrainDriverControl extends CyberarmState {
         // robot.hardwareFault = true;
 
         if (robot.hardwareFault) {
-            robot.status = Robot.Status.DANGER;
+            robot.reportStatus(Robot.Status.DANGER);
 
             stopDrive();
         }