TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-15 20:12:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Initial implementation of camera, changed access of CyberarmEngine#setupFromConfig to public from protected, refactored autonomous engines to only use the 2 logical sides instead of the 4 for states, a hardware fault during autonomous will cause the opmode to abort, misc. changes.

Browse Source
This commit is contained in:
cyberarm
2023-01-26 10:16:31 -06:00
parent 578ab45327
commit 1b50fd015b
10 changed files with 187 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java
View File

@@ -415,7 +415,7 @@ public abstract class CyberarmEngine extends OpMode {
* @param objectClass Class to cast object to
* @param groupName Group name
*/
protected void setupFromConfig(TimeCraftersConfiguration configuration, String packageName, Object object, Class<?> objectClass, String groupName) {
public void setupFromConfig(TimeCraftersConfiguration configuration, String packageName, Object object, Class<?> objectClass, String groupName) {
CyberarmState lastState = null;
String lastActionName = null;
String[] lastActionNameSplit = new String[0];

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

@@ -17,9 +17,13 @@ import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.ServoImplEx;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
@@ -43,6 +47,7 @@ public class Robot {
public boolean wristManuallyControlled = false;
public boolean automaticAntiTipActive = false;
public boolean hardwareFault = false;
public String hardwareFaultMessage = "";
private Status status = Status.OKAY, lastStatus = Status.OKAY;
private final CopyOnWriteArrayList<Status> reportedStatuses = new CopyOnWriteArrayList<>();
@@ -82,6 +87,11 @@ public class Robot {
private WristPosition wristTargetPosition, wristCurrentPosition;
private double wristPositionChangeTime, wristPositionChangeRequestTime;
private static final String VUFORIA_KEY =
"Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah";
private final VuforiaLocalizer vuforia;
private final TFObjectDetector tfod;
private boolean LEDStatusToggle = false;
private double lastLEDStatusAnimationTime = 0;
@@ -198,11 +208,38 @@ public class Robot {
expansionHub.setPattern(ledPatternOkay());
}
// Webcam
vuforia = initVuforia();
tfod = initTfod();
// INITIALIZE AFTER EVERYTHING ELSE to prevent use before set crashes
this.fieldLocalizer.setRobot(this);
this.fieldLocalizer.standardSetup();
}
private VuforiaLocalizer initVuforia() {
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
parameters.vuforiaLicenseKey = VUFORIA_KEY;
parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
return ClassFactory.getInstance().createVuforia(parameters);
}
private TFObjectDetector initTfod() {
int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
"tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
tfodParameters.minResultConfidence = 0.75f;
tfodParameters.isModelTensorFlow2 = true;
tfodParameters.inputSize = 300;
TFObjectDetector tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
tfod.loadModelFromAsset("PowerPlay.tflite", "1 Bolt", "2 Bulb", "3 Panel");
return tfod;
}
public void standardTelemetry() {
engine.telemetry.addLine();
@@ -210,6 +247,7 @@ public class Robot {
engine.telemetry.addLine("DATA");
engine.telemetry.addData(" Robot Status", status);
engine.telemetry.addData(" Hardware Fault", hardwareFault);
engine.telemetry.addData(" Hardware Fault Message", hardwareFaultMessage);
engine.telemetry.addLine();
// Motor Powers
@@ -392,8 +430,11 @@ public class Robot {
wrist.setPower(0);
}
if (getVoltage() < 9.75) {
double voltage = getVoltage();
if (voltage < 9.75) {
reportStatus(Status.DANGER);
hardwareFaultMessage = "Battery voltage to low! (" + voltage + " volts)";
hardwareFault = true;
}
@@ -528,6 +569,14 @@ public class Robot {
public double getDiameter() { return diameter; }
public double getVoltage() {
return engine.hardwareMap.voltageSensor.iterator().next().getVoltage();
}
public TFObjectDetector getTfod() { return tfod; }
public VuforiaLocalizer getVuforia() { return vuforia; }
public TimeCraftersConfiguration getConfiguration() { return configuration; }
public FieldLocalizer getFieldLocalizer() { return fieldLocalizer; }
@@ -667,10 +716,6 @@ public class Robot {
arm.setPower(0.75);
}
public double getVoltage() {
return engine.hardwareMap.voltageSensor.iterator().next().getVoltage();
}
public double facing() {
double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);

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

@@ -1,11 +1,14 @@
package org.timecrafters.minibots.cyberarm.chiron.engines;
import android.util.Log;
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;
public class AutonomousEngine extends CyberarmEngine {
private static final String TAG = "CHIRON|AutonomousEngine";
protected Robot robot;
protected FieldLocalizer fieldLocalizer;
protected TimeCraftersConfiguration configuration;
@@ -49,6 +52,12 @@ public class AutonomousEngine extends CyberarmEngine {
super.loop();
robot.standardTelemetry();
if (robot.hardwareFault) {
Log.e(TAG, "Hardware fault detected! Aborting run. Message: " + robot.hardwareFaultMessage);
stop();
}
}
@Override

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

@@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
public class AutonomousBlueLeftSideEngine extends AutonomousEngine {
@Override
public void setup() {
actionsGroupName = "AutonomousBlueLeftSide";
actionsGroupName = "AutonomousLeftSide";
tuningGroupName = "Autonomous";
tuningActionName = "Tuning_Blue_LeftSide";

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

@@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
public class AutonomousBlueRightSideEngine extends AutonomousEngine {
@Override
public void setup() {
actionsGroupName = "AutonomousBlueRightSide";
actionsGroupName = "AutonomousRightSide";
tuningGroupName = "Autonomous";
tuningActionName = "Tuning_Blue_RightSide";

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

@@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
public class AutonomousRedLeftSideEngine extends AutonomousEngine {
@Override
public void setup() {
actionsGroupName = "AutonomousRedLeftSide";
actionsGroupName = "AutonomousLeftSide";
tuningGroupName = "Autonomous";
tuningActionName = "Tuning_Red_LeftSide";

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

@@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
public class AutonomousRedRightSideEngine extends AutonomousEngine {
@Override
public void setup() {
actionsGroupName = "AutonomousRedRightSide";
actionsGroupName = "AutonomousRightSide";
tuningGroupName = "Autonomous";
tuningActionName = "Tuning_Red_RightSide";

34
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java
View File

@@ -7,15 +7,45 @@ public class SelectParkingPosition extends CyberarmState {
private final Robot robot;
private final String groupName, actionName;
private final double timeInMS;
private final boolean stateDisabled;
public SelectParkingPosition(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
}
@Override
public void start() {
int pos = engine.blackboardGetInt("parking_position");
engine.setupFromConfig(
robot.getConfiguration(),
"org.timecrafters.minibots.cyberarm.chiron.states.autonomous",
robot,
Robot.class,
"" + groupName + "_Parking_" + pos);
}
@Override
public void exec() {
// FIXME: NO OP
setHasFinished(true);
if (stateDisabled) {
setHasFinished(true);
return;
}
if (runTime() >= timeInMS) {
stop();
setHasFinished(true);
return;
}
}
}

87
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java
View File

@@ -1,26 +1,109 @@
package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import org.timecrafters.minibots.cyberarm.chiron.Robot;
import java.util.List;
public class SignalProcessor extends CyberarmState {
private final Robot robot;
private final String groupName, actionName;
private final double timeInMS, minConfidence;
private final int fallbackPosition;
private final boolean stateDisabled;
public SignalProcessor(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
minConfidence = robot.getConfiguration().variable(groupName, actionName, "minConfidence").value();
fallbackPosition = robot.getConfiguration().variable(groupName, actionName, "fallbackPosition").value();
stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
}
@Override
public void start() {
engine.blackboardSet("parking_position", System.currentTimeMillis() % 3);
engine.blackboardSet("parking_position", fallbackPosition);
robot.getTfod().activate();
}
@Override
public void exec() {
// FIXME: NO OP
if (stateDisabled) {
stop();
return;
}
if (runTime() >= timeInMS) {
stop();
return;
}
if (robot.getTfod() != null) {
// getUpdatedRecognitions() will return null if no new information is available since
// the last time that call was made.
List<Recognition> updatedRecognitions = robot.getTfod().getUpdatedRecognitions();
if (updatedRecognitions != null) {
for (Recognition recognition : updatedRecognitions) {
switch (recognition.getLabel()) {
case "1 Bolt":
engine.blackboardSet("parking_position", 1);
break;
case "2 Bulb":
engine.blackboardSet("parking_position", 2);
break;
case "3 Panel":
engine.blackboardSet("parking_position", 3);
break;
}
}
}
}
}
@Override
public void telemetry() {
if (robot.getTfod() != null) {
// getUpdatedRecognitions() will return null if no new information is available since
// the last time that call was made.
List<Recognition> updatedRecognitions = robot.getTfod().getUpdatedRecognitions();
if (updatedRecognitions != null) {
engine.telemetry.addData("# Objects Detected", updatedRecognitions.size());
// step through the list of recognitions and display image position/size information for each one
// Note: "Image number" refers to the randomized image orientation/number
for (Recognition recognition : updatedRecognitions) {
double col = (recognition.getLeft() + recognition.getRight()) / 2 ;
double row = (recognition.getTop() + recognition.getBottom()) / 2 ;
double width = Math.abs(recognition.getRight() - recognition.getLeft()) ;
double height = Math.abs(recognition.getTop() - recognition.getBottom()) ;
engine.telemetry.addData(""," ");
engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100 );
engine.telemetry.addData("- Position (Row/Col)","%.0f / %.0f", row, col);
engine.telemetry.addData("- Size (Width/Height)","%.0f / %.0f", width, height);
}
}
}
}
@Override
public void stop() {
setHasFinished(true);
robot.getTfod().deactivate();
}
}

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

@@ -128,6 +128,12 @@ public class DrivetrainDriverControl extends CyberarmState {
// DEBUG: Toggle hardware fault
if (button.equals("guide")) {
robot.hardwareFault = !robot.hardwareFault;
if (robot.hardwareFault) {
robot.hardwareFaultMessage = "Manually triggered.";
} else {
robot.hardwareFaultMessage = "";
}
}
if (button.equals("back")) {