diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java index b96d034..6dbc79e 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java @@ -5,6 +5,8 @@ import android.util.Log; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.Gamepad; +import java.util.ArrayList; +import java.util.HashMap; import java.util.concurrent.CopyOnWriteArrayList; /** @@ -17,7 +19,8 @@ public abstract class CyberarmEngine extends OpMode { public static CyberarmEngine instance; //Array To Hold States final private CopyOnWriteArrayList cyberarmStates = new CopyOnWriteArrayList<>(); - private int activeStateIndex = 0; + public HashMap blackboard = new HashMap<>(); + private int activeStateIndex = 0; private boolean isRunning; private final static String TAG = "PROGRAM.ENGINE"; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java index 50f5bce..b64a4f9 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java @@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.Autonomous.States.CollectorDistanceState; import org.timecrafters.Autonomous.States.CollectorState; +import org.timecrafters.Autonomous.States.ConeIdentification; import org.timecrafters.Autonomous.States.DriverState; import org.timecrafters.Autonomous.States.BottomArm; import org.timecrafters.Autonomous.States.RotationState; @@ -20,7 +21,7 @@ public class TestAutonomousEngine extends CyberarmEngine { @Override public void setup() { robot = new PrototypeBot1(this); - + addState(new ConeIdentification(robot, "TestAutonomous", "00-0")); //drive to high pole addState(new DriverState(robot, "TestAutonomous", "01-0")); //turn towards high pole @@ -67,15 +68,22 @@ public class TestAutonomousEngine extends CyberarmEngine { addState(new TopArm(robot, "TestAutonomous", "22-0")); // get rid of cone addState(new CollectorState(robot, "TestAutonomous", "23-0")); + // lift arm up to clear + addState(new TopArm(robot, "TestAutonomous", "24-0")); + // drive back + addState(new DriverState(robot, "TestAutonomous", "25-0")); + // bring bottom arm down + addState(new BottomArm(robot, "TestAutonomous", "26-0")); + // bring top arm down + addState(new TopArm(robot, "TestAutonomous", "27-0")); + // rotate towards stack of cones + addState(new RotationState(robot, "TestAutonomous", "28-0")); + } + @Override + public void loop() { + super.loop(); - - - - - - - - + telemetry.addData("BlackBoard Input", blackboard.get("parkPlace")); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java index 0865724..6c74a5c 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java @@ -11,6 +11,7 @@ public class BottomArm extends CyberarmState { long time; long lastStepTime = 0; boolean up; + boolean directPosition; public BottomArm(PrototypeBot1 robot, String groupName, String actionName) { this.robot = robot; @@ -18,6 +19,7 @@ public class BottomArm extends CyberarmState { this.LowerRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value(); this.time = robot.configuration.variable(groupName, actionName, "time").value(); this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value(); + this.directPosition = robot.configuration.variable(groupName, actionName, "directPosition").value(); this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; @@ -35,22 +37,31 @@ public class BottomArm extends CyberarmState { return; } - if (System.currentTimeMillis() - lastStepTime >= time) { - lastStepTime = System.currentTimeMillis(); + if (directPosition) { + robot.LowRiserLeft.setPosition(LowerRiserLeftPos); + robot.LowRiserRight.setPosition(LowerRiserLeftPos); - if (robot.LowRiserLeft.getPosition() < LowerRiserLeftPos && up) { - - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance); - - } else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) { - - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance); - - } else { + if (runTime() >= time){ setHasFinished(true); } + } else { + if (System.currentTimeMillis() - lastStepTime >= time) { + lastStepTime = System.currentTimeMillis(); + + if (robot.LowRiserLeft.getPosition() < LowerRiserLeftPos && up) { + + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance); + + } else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) { + + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance); + + } else { + setHasFinished(true); + } + } } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java index e15daac..9fc3355 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -14,6 +14,10 @@ public class CollectorDistanceState extends CyberarmState { private int RampDownDistance; private double drivePower; private double targetDrivePower; + private double lastMeasuredTime; + private double currentDistance; + private double LastDistanceRead; + public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) { this.robot = robot; @@ -38,6 +42,8 @@ public class CollectorDistanceState extends CyberarmState { engine.telemetry.addData("traveledDistance", traveledDistance); engine.telemetry.addData("RampUpDistance", RampUpDistance); engine.telemetry.addData("RampDownDistance", RampDownDistance); + engine.telemetry.addData("Current Distance", currentDistance); + engine.telemetry.addData("last Distance", LastDistanceRead); } @@ -57,12 +63,37 @@ public class CollectorDistanceState extends CyberarmState { robot.collectorLeft.setPower(1); robot.collectorRight.setPower(1); + + lastMeasuredTime = System.currentTimeMillis(); + LastDistanceRead = robot.collectorDistance.getDistance(DistanceUnit.MM); + + + } @Override public void exec() { + + if (System.currentTimeMillis() - lastMeasuredTime > 150) { + // checking to see if time is greater than 150 milliseconds + lastMeasuredTime = System.currentTimeMillis(); + + currentDistance = robot.collectorDistance.getDistance(DistanceUnit.MM); + + if (LastDistanceRead - currentDistance >= 0.0 || currentDistance > 500) { + // I am moving forward + // and im close too my target. + LastDistanceRead = currentDistance; + + } else { + // I have stopped + targetDrivePower = 0; +// setHasFinished(true); + } + } + if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) { double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition()); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java new file mode 100644 index 0000000..4255cb7 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java @@ -0,0 +1,98 @@ +package org.timecrafters.Autonomous.States; + +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.timecrafters.testing.states.PrototypeBot1; + +import java.util.List; + +public class ConeIdentification extends CyberarmState { + PrototypeBot1 robot; + private int time; + private float minimumConfidence; + private int ParkPlace; + + public ConeIdentification(PrototypeBot1 robot, String groupName, String actionName) { + this.robot = robot; + minimumConfidence = robot.configuration.variable(groupName, actionName, "Minimum Confidence").value(); + time = robot.configuration.variable(groupName, actionName, "time").value(); + } + + @Override + public void init() { + engine.blackboard.put("parkPlace", "1"); + robot.tfod.activate(); + } + + @Override + public void telemetry() { + if (robot.tfod != null) { + // getUpdatedRecognitions() will return null if no new information is available since + // the last time that call was made. + List updatedRecognitions = robot.tfod.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("Label", recognition.getLabel()); + 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); + + if (recognition.getLabel().equals("2 Bulb")) { + engine.telemetry.addData("2 Bulb", engine.blackboard.put("parkPlace", "2")); + } else if (recognition.getLabel().equals("3 Panel")) { + engine.telemetry.addData("3 Panel",engine.blackboard.put("parkPlace", "3")); + } else { + engine.telemetry.addData("1 Bolt", engine.blackboard.put("parkPlace", "1")); + } + } + } + } + } + + @Override + public void exec() { + + if (robot.tfod != null) { + // getUpdatedRecognitions() will return null if no new information is available since + // the last time that call was made. + List updatedRecognitions = robot.tfod.getUpdatedRecognitions(); + if (updatedRecognitions != null) { + // 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 + float bestConfidence = 0; + + for (Recognition recognition : updatedRecognitions) { + if (recognition.getConfidence() >= minimumConfidence && recognition.getConfidence() > bestConfidence) { + bestConfidence = recognition.getConfidence(); + + if (recognition.getLabel().equals("2 Bulb")) { + engine.blackboard.put("parkPlace", "2"); + } else if (recognition.getLabel().equals("3 Panel")) { + engine.blackboard.put("parkPlace", "3"); + + } else { + engine.blackboard.put("parkPlace", "1"); + } + } + } + } + } + + if (runTime() >= time){ + setHasFinished(true); + } + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java new file mode 100644 index 0000000..97c4ae4 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java @@ -0,0 +1,39 @@ +package org.timecrafters.Autonomous.States; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.testing.states.PrototypeBot1; + +public class PathDecision extends CyberarmState { + PrototypeBot1 robot; + private String groupName; + + + public PathDecision (PrototypeBot1 robot, String groupName, String actionName){ + this.robot = robot; + this.groupName = groupName; + + } + + @Override + public void exec() { + String placement = engine.blackboard.get("parkPlace"); + + if (placement != null) { + if (placement.equals("2")) { + engine.insertState(this, new DriverState(robot, groupName, "29-1")); + } else if (placement.equals("3")) { + engine.insertState(this, new DriverState(robot, groupName, "29-2")); + } + } + else { + engine.insertState(this, new DriverState(robot, groupName, "29-0")); + } + } + + } + + + + + + diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java index 13f1eda..24724ff 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java @@ -29,11 +29,11 @@ public class PrototypeBot1 { private static final String VUFORIA_KEY = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah"; - private VuforiaLocalizer vuforia; + public VuforiaLocalizer vuforia; - private TFObjectDetector tfod; + public TFObjectDetector tfod; - public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight; + public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight, CameraServo; private final CyberarmEngine engine; public Rev2mDistanceSensor collectorDistance; @@ -80,6 +80,9 @@ public class PrototypeBot1 { // servo configuration + //Camera Servo + CameraServo = engine.hardwareMap.servo.get("Camera Servo"); + // Collector collectorLeft = engine.hardwareMap.crservo.get("Collector Left"); collectorRight = engine.hardwareMap.crservo.get("Collector Right"); @@ -122,17 +125,21 @@ public class PrototypeBot1 { HighRiserLeft.setPosition(0.45); HighRiserRight.setPosition(0.45); + CameraServo.setPosition(0); + } private void initVuforia(){ /* * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine. */ - VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); parameters.vuforiaLicenseKey = VUFORIA_KEY; parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1"); + // Instantiate the Vuforia engine vuforia = ClassFactory.getInstance().createVuforia(parameters); }