diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java index af027a2..cce8ce8 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java @@ -1,5 +1,9 @@ package org.timecrafters.FreightFrenzy.Competition.Common; +import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES; +import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ; +import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC; + import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.rev.RevBlinkinLedDriver; import com.qualcomm.robotcore.hardware.DcMotor; @@ -7,14 +11,59 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; 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.matrices.OpenGLMatrix; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.Velocity; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; +import java.util.ArrayList; +import java.util.List; + public class Robot { + private static final float mmPerInch = 25.4f; + private static final float mmTargetHeight = 6 * mmPerInch; // the height of the center of the target image above the floor + private static final float halfField = 72 * mmPerInch; + private static final float halfTile = 12 * mmPerInch; + private static final float oneAndHalfTile = 36 * mmPerInch; + + private static final String TENSORFLOW_MODEL_ASSET = "FreightFrenzy_BCDM.tflite"; + private static final String[] TENSORFLOW_MODEL_LABELS = { + "Ball", + "Cube", + "Duck", + "Marker" + }; + private static final String VUFORIA_KEY = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah"; + private static final float VUFORIA_CAMERA_FORWARD_DISPLACEMENT = 0.0f * mmPerInch; // Camera lens forward distance from robot center + private static final float VUFORIA_CAMERA_VERTICAL_DISPLACEMENT = 0.0f * mmPerInch; // Camera lens height above ground + private static final float VUFORIA_CAMERA_LEFT_DISPLACEMENT = 0.0f * mmPerInch; // Camera lens left distance from robot center + private final CyberarmEngine engine; public TimeCraftersConfiguration configuration; // Shiny public RevBlinkinLedDriver leds; + public TFObjectDetector tensorflow; + private List tensorflowRecognitions = new ArrayList<>(); + public VuforiaLocalizer vuforia; + private VuforiaTrackables vuforiaTargets; + private List vuforiaTrackables; + private OpenGLMatrix vuforiaCameraLocationOnRobot = OpenGLMatrix + .translation(VUFORIA_CAMERA_FORWARD_DISPLACEMENT, VUFORIA_CAMERA_LEFT_DISPLACEMENT, VUFORIA_CAMERA_VERTICAL_DISPLACEMENT) + .multiplied(Orientation.getRotationMatrix(AxesReference.EXTRINSIC, AxesOrder.XZY, AngleUnit.DEGREES, 90, 90, 0)); + private RobotLocation vuforiaLastLocation; // Sensors public BNO055IMU imu; @@ -43,20 +92,72 @@ public class Robot { initCollector(); initDepositor(); initCarousel(); + initVuforia(); + initTensorflow(); } - public double rotation() { + public double heading() { return imu.getAngularOrientation().firstAngle; } - public double pitch() { + public double roll() { return imu.getAngularOrientation().secondAngle; } - public double roll() { + public double pitch() { return imu.getAngularOrientation().thirdAngle; } + public void activateTensorflow() { + tensorflow.activate(); + } + + public List tensorflowDetections() { + List updateRecognitions = tensorflow.getUpdatedRecognitions(); + + if (updateRecognitions != null) { + tensorflowRecognitions = updateRecognitions; + } + + return tensorflowRecognitions; + } + + public void deactivateTensorflow() { + tensorflow.deactivate(); + } + + public void activateVuforia() { + vuforiaTargets.activate(); + } + + public RobotLocation vuforiaLocation() { + for (VuforiaTrackable trackable : vuforiaTrackables) { + if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) { + OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation(); + if (robotLocationTransform != null) { + vuforiaLastLocation = new RobotLocation(robotLocationTransform); + } + + // Recycle old position since there is no new data + return vuforiaLastLocation; + } + } + + // Return null if there is no visible trackable + return null; + } + + private void vuforiaIdentifyTarget(int targetIndex, String targetName, float dx, float dy, float dz, float rx, float ry, float rz) { + VuforiaTrackable aTarget = vuforiaTargets.get(targetIndex); + aTarget.setName(targetName); + aTarget.setLocation(OpenGLMatrix.translation(dx, dy, dz) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, rx, ry, rz))); + } + + public void deactivateVuforia() { + vuforiaTargets.deactivate(); + } + public double ticksToInches(int ticks) { return ticks * (WHEEL_CIRCUMFERENCE / COUNTS_PER_REVOLUTION); } @@ -83,6 +184,8 @@ public class Robot { this.imu = engine.hardwareMap.get(BNO055IMU.class, "imu"); imu.initialize(parameters); + + imu.startAccelerationIntegration(new Position(), new Velocity(), 10); } private void initDrivetrain() { @@ -106,4 +209,44 @@ public class Robot { private void initCarousel() { } + + private void initVuforia() { + 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"); + parameters.useExtendedTracking = false; + + vuforia = ClassFactory.getInstance().createVuforia(parameters); + + vuforiaTargets = vuforia.loadTrackablesFromAsset("FreightFrenzy"); + + vuforiaTrackables = new ArrayList<>(vuforiaTargets); + + // Name and locate each trackable object + vuforiaIdentifyTarget(0, "Blue Storage", -halfField, oneAndHalfTile, mmTargetHeight, 90, 0, 90); + vuforiaIdentifyTarget(1, "Blue Alliance Wall", halfTile, halfField, mmTargetHeight, 90, 0, 0); + vuforiaIdentifyTarget(2, "Red Storage", -halfField, -oneAndHalfTile, mmTargetHeight, 90, 0, 90); + vuforiaIdentifyTarget(3, "Red Alliance Wall", halfTile, -halfField, mmTargetHeight, 90, 0, 180); + + for (VuforiaTrackable trackable : vuforiaTrackables) { + ((VuforiaTrackableDefaultListener) trackable.getListener()) + .setCameraLocationOnRobot(parameters.cameraName, vuforiaCameraLocationOnRobot); + } + } + + private void initTensorflow() { + int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier( + "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName()); + + TFObjectDetector.Parameters parameters = new TFObjectDetector.Parameters(tfodMonitorViewId); + parameters.minResultConfidence = 0.8f; + parameters.isModelTensorFlow2 = true; + parameters.inputSize = 320; + + tensorflow = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia); + tensorflow.loadModelFromAsset(TENSORFLOW_MODEL_ASSET, TENSORFLOW_MODEL_LABELS); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/RobotLocation.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/RobotLocation.java new file mode 100644 index 0000000..a9f5b60 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/RobotLocation.java @@ -0,0 +1,25 @@ +package org.timecrafters.FreightFrenzy.Competition.Common; + +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +import org.firstinspires.ftc.robotcore.external.matrices.VectorF; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; + +public class RobotLocation { + public double x, y, z, roll, pitch, heading; + public RobotLocation(OpenGLMatrix matrix) { + VectorF translation = matrix.getTranslation(); + Orientation rotation = Orientation.getOrientation( + matrix, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES); + + x = translation.get(0); + y = translation.get(1); + z = translation.get(2); + + roll = rotation.firstAngle; + pitch = rotation.secondAngle; + heading = rotation.thirdAngle; + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/TensorFlowTestEngine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/TensorFlowTestEngine.java new file mode 100644 index 0000000..2043934 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/TensorFlowTestEngine.java @@ -0,0 +1,17 @@ +package org.timecrafters.FreightFrenzy.HardwareTesting.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; +import org.timecrafters.FreightFrenzy.HardwareTesting.States.TensorFlowTestState; + +@TeleOp(name = "TensorFlow Test", group = "testing") +public class TensorFlowTestEngine extends CyberarmEngine { + @Override + public void setup() { + Robot robot = new Robot(this); + + addState(new TensorFlowTestState(robot)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/VuforiaTestEngine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/VuforiaTestEngine.java new file mode 100644 index 0000000..fa9e29d --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/VuforiaTestEngine.java @@ -0,0 +1,17 @@ +package org.timecrafters.FreightFrenzy.HardwareTesting.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; +import org.timecrafters.FreightFrenzy.HardwareTesting.States.VuforiaTestState; + +@TeleOp(name = "Vuforia Test", group = "testing") +public class VuforiaTestEngine extends CyberarmEngine { + @Override + public void setup() { + Robot robot = new Robot(this); + + addState(new VuforiaTestState(robot)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/TensorFlowTestState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/TensorFlowTestState.java new file mode 100644 index 0000000..1314e34 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/TensorFlowTestState.java @@ -0,0 +1,38 @@ +package org.timecrafters.FreightFrenzy.HardwareTesting.States; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; + +public class TensorFlowTestState extends CyberarmState { + private Robot robot; + + public TensorFlowTestState(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + robot.activateTensorflow(); + } + + @Override + public void exec() { + + } + + @Override + public void stop() { + robot.deactivateTensorflow(); + } + + @Override + public void telemetry() { + for (Recognition recognition : robot.tensorflowDetections()) { + engine.telemetry.addData("Label", recognition.getLabel()); + engine.telemetry.addData("Left", recognition.getLeft()); + engine.telemetry.addData("Top", recognition.getTop()); + engine.telemetry.addData("Confidence", recognition.getConfidence()); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/VuforiaTestState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/VuforiaTestState.java new file mode 100644 index 0000000..99c3dbe --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/VuforiaTestState.java @@ -0,0 +1,42 @@ +package org.timecrafters.FreightFrenzy.HardwareTesting.States; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; +import org.timecrafters.FreightFrenzy.Competition.Common.RobotLocation; + +public class VuforiaTestState extends CyberarmState { + private Robot robot; + + public VuforiaTestState(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + robot.activateVuforia(); + } + + @Override + public void exec() { + + } + + @Override + public void telemetry() { + RobotLocation robotLocation = robot.vuforiaLocation(); + + engine.telemetry.addLine("Robot Vuforia Location"); + + if (robotLocation != null) { + engine.telemetry.addData("Orientation", "heading: %.2f, roll: %.2f, pitch: %.2f", robotLocation.heading, robotLocation.roll, robotLocation.pitch); + engine.telemetry.addData("Location", "X: %.2f, Y: %.2f, Z: %.2f", robotLocation.x, robotLocation.y, robotLocation.z); + } else { + engine.telemetry.addLine("No Data"); + } + } + + @Override + public void stop() { + robot.deactivateVuforia(); + } +}