TimeCrafters/FreightFrenzy
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FreightFrenzy.git synced 2025-12-13 05:02:34 +00:00
Code Issues Projects Releases Wiki Activity

Added Tensorflow and Vuforia to Robot class, added testing engines

Browse Source
This commit is contained in:
Cyberarm
2021-11-04 12:05:23 -05:00
parent af064f1641
commit 4064e07463
6 changed files with 285 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

149
TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java
View File

@@ -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<Recognition> tensorflowRecognitions = new ArrayList<>();
public VuforiaLocalizer vuforia;
private VuforiaTrackables vuforiaTargets;
private List<VuforiaTrackable> 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<Recognition> tensorflowDetections() {
List<Recognition> 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);
}
}

25
TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/RobotLocation.java Normal file
View File

@@ -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;
}
}

17
TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/TensorFlowTestEngine.java Normal file
View File

@@ -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));
}
}

17
TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/VuforiaTestEngine.java Normal file
View File

@@ -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));
}
}

38
TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/TensorFlowTestState.java Normal file
View File

@@ -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());
}
}
}

42
TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/VuforiaTestState.java Normal file
View File

@@ -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();
}
}