From c31e40d3323311bf4d7cdd38c4be652e7416718a Mon Sep 17 00:00:00 2001 From: Spencer Date: Fri, 4 Feb 2022 18:29:02 -0600 Subject: [PATCH] Fixed up tele op and robot object --- .../MecanumRobot_Spencer/MecanumRobot.java | 227 +----------------- .../TeleOpStateMecanumRobot.java | 59 ++--- 2 files changed, 36 insertions(+), 250 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/MecanumRobot.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/MecanumRobot.java index fb79056..109db06 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/MecanumRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/MecanumRobot.java @@ -1,82 +1,15 @@ package org.timecrafters.FreightFrenzy.MecanumRobot_Spencer; -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.hardware.rev.RevTouchSensor; -import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; 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.FreightFrenzy.Competition.Common.RobotLocation; -import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; - -import java.util.ArrayList; -import java.util.List; - public class MecanumRobot { - 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; - private Orientation orientation; - - // Sensors - public BNO055IMU imu; - public RevTouchSensor whiteMag; - public RevTouchSensor orangeMag; // Drivetrain - public static final double WHEEL_CIRCUMFERENCE = Math.PI * 8; - public static final int COUNTS_PER_REVOLUTION = 1000; public DcMotor driveFrontLeft, driveFrontRight, driveBackLeft, driveBackRight; @@ -84,121 +17,14 @@ public class MecanumRobot { public MecanumRobot(CyberarmEngine engine) { this.engine = engine; - initMagnetSwitches(); - initConfiguration(); - initBlinkin(); - initIMU(); initDrivetrain(); - initVuforia(); - initTensorflow(); } - - private void initMagnetSwitches() { - whiteMag = engine.hardwareMap.get(RevTouchSensor.class, "whiteMag"); - orangeMag = engine.hardwareMap.get(RevTouchSensor.class, "orangeMag"); - } - - public double heading() { - return orientation.firstAngle; - } - - public double roll() { - return orientation.secondAngle; - } - - public double pitch() { - return orientation.thirdAngle; - } - - public void updateRobotOrientation(){ - orientation = imu.getAngularOrientation(); - } - - 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); - } - - public double inchesToTicks(double inches) { - return inches * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE); - } - - private void initConfiguration() { - this.configuration = new TimeCraftersConfiguration(); - } - - private void initBlinkin() { -// engine.hardwareMap.get(RevBlinkinLedDriver.class, "leds"); - } - - private void initIMU() { - BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); - - parameters.mode = BNO055IMU.SensorMode.IMU; - parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; - parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; - parameters.loggingEnabled = false; - - this.imu = engine.hardwareMap.get(BNO055IMU.class, "imu"); - imu.initialize(parameters); - - imu.startAccelerationIntegration(new Position(), new Velocity(), 10); - } - private void initDrivetrain() { - driveFrontRight = engine.hardwareMap.dcMotor.get("driveWarehouseRight"); - driveFrontLeft = engine.hardwareMap.dcMotor.get("driveWarehouseLeft"); - driveBackRight = engine.hardwareMap.dcMotor.get("driveGoalRight"); - driveBackLeft = engine.hardwareMap.dcMotor.get("driveGoalLeft"); + + driveFrontRight = engine.hardwareMap.dcMotor.get("driveFrontRight"); + driveFrontLeft = engine.hardwareMap.dcMotor.get("driveFrontLeft"); + driveBackRight = engine.hardwareMap.dcMotor.get("driveBackRight"); + driveBackLeft = engine.hardwareMap.dcMotor.get("driveBackLeft"); driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD); driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); @@ -206,47 +32,6 @@ public class MecanumRobot { driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD); driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE); } - - - 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/MecanumRobot_Spencer/TeleOpStateMecanumRobot.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/TeleOpStateMecanumRobot.java index 97a60d9..8f6d726 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/TeleOpStateMecanumRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/TeleOpStateMecanumRobot.java @@ -18,12 +18,12 @@ public class TeleOpStateMecanumRobot extends CyberarmState { if (engine.gamepad1.left_trigger > 0.5){ // TankDrive - // Left joystick foward; Left side foward; + // Left joystick forward; Left side forward; robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 0.5); robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 0.5); - // Right joystick foward; Right side foward; + // Right joystick forward; Right side forward; robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 0.5); robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 0.5); @@ -40,19 +40,20 @@ public class TeleOpStateMecanumRobot extends CyberarmState { robot.driveBackLeft.setPower(0.5); robot.driveFrontRight.setPower(0.5); robot.driveBackRight.setPower(-0.5); - } else { - robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 0.5); - robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 0.5); - robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 0.5); - robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 0.5); + } +// else { +// robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 0.5); +// robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 0.5); +// robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 0.5); +// robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 0.5); - } - } + + // } // Normal Speed - else if (engine.gamepad1.right_stick_button){ + if (engine.gamepad1.right_stick_button) { - // left stick foward; right side foward + // left stick forward; right side foward robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1); robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1); @@ -60,7 +61,7 @@ public class TeleOpStateMecanumRobot extends CyberarmState { robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1); robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1); - + } // Strafing left and right if (engine.gamepad1.left_bumper) { @@ -94,23 +95,23 @@ public class TeleOpStateMecanumRobot extends CyberarmState { // Strafing left and right - if (engine.gamepad1.left_bumper) { - robot.driveFrontLeft.setPower(1); - robot.driveBackLeft.setPower(-1); - robot.driveFrontRight.setPower(-1); - robot.driveBackRight.setPower(1); - } else if (engine.gamepad1.right_bumper) { - robot.driveFrontLeft.setPower(-1); - robot.driveBackLeft.setPower(1); - robot.driveFrontRight.setPower(1); - robot.driveBackRight.setPower(-1); - } else { - robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1); - robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1); - robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1); - robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1); - - } +// if (engine.gamepad1.left_bumper) { +// robot.driveFrontLeft.setPower(1); +// robot.driveBackLeft.setPower(-1); +// robot.driveFrontRight.setPower(-1); +// robot.driveBackRight.setPower(1); +// } else if (engine.gamepad1.right_bumper) { +// robot.driveFrontLeft.setPower(-1); +// robot.driveBackLeft.setPower(1); +// robot.driveFrontRight.setPower(1); +// robot.driveBackRight.setPower(-1); +// } else { +// robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1); +// robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1); +// robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1); +// robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1); +// +// } } } }