diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java index 8e0f72c..cb69483 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java +++ b/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 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() { - status = Status.OKAY; + private ArrayList ledPatternOkay() { + final ArrayList 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 ledPatternMonitoring() { + final ArrayList 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 ledPatternWarning() { + final ArrayList 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 ledPatternDanger() { + final ArrayList 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 diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java similarity index 56% rename from TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java rename to TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java index 3b3f658..8581217 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java +++ b/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 AutonomousRedLeftSideEngine extends CyberarmEngine { - private Robot robot; - private FieldLocalizer fieldLocalizer; - private TimeCraftersConfiguration configuration; +public class AutonomousEngine extends CyberarmEngine { + protected Robot robot; + protected FieldLocalizer fieldLocalizer; + protected 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("Autonomous", "Tuning_Red_LeftSide", "starting_position_y_in_inches").value() + configuration.variable(tuningGroupName, tuningActionName, "starting_position_x_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(); diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java new file mode 100644 index 0000000..9d4e684 --- /dev/null +++ b/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(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java new file mode 100644 index 0000000..0f55457 --- /dev/null +++ b/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(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java new file mode 100644 index 0000000..847a85c --- /dev/null +++ b/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(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java new file mode 100644 index 0000000..7da0668 --- /dev/null +++ b/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(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java index 7c2ea54..19838e5 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java +++ b/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: diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java index 6e55fc7..cd0ccf6 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java +++ b/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(); }