From aafa5cf65d8f20ea8a48230ceefe26c939226338 Mon Sep 17 00:00:00 2001 From: Spencer Date: Sun, 30 Jan 2022 11:44:45 -0600 Subject: [PATCH] I added a new folder/ directory for My Mecanum Wheel Robot, and created a engine for it, and new robot object... followed by a tele op state and this can be uploaded to the robot when finished being built and all the wires are attached and expansion hub is attached --- .idea/misc.xml | 2 +- .../TeleOp/States/TeleOpState.java | 9 +- .../MecanumRobot_Spencer/MecanumRobot.java | 252 ++++++++++++++++++ .../Mecanum_Drive_Engine.java | 13 + .../TeleOpStateMecanumRobot.java | 116 ++++++++ 5 files changed, 386 insertions(+), 6 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/MecanumRobot.java create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/Mecanum_Drive_Engine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/TeleOpStateMecanumRobot.java diff --git a/.idea/misc.xml b/.idea/misc.xml index 5c9f89f..54d5acd 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,7 +1,7 @@ - + diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java index d1621da..e63d863 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java @@ -28,7 +28,6 @@ public class TeleOpState extends CyberarmState { @Override public void start() { super.start(); - // FIXME: Don't reset when doing autonomous stuff robot.whiteArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.whiteArmBobbin.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.orangeArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); @@ -74,11 +73,11 @@ public class TeleOpState extends CyberarmState { if (engine.gamepad1.dpad_up || engine.gamepad1.dpad_down) { if (engine.gamepad1.dpad_up) { - robot.orangeArmRiser.setPower(0.8); + robot.orangeArmRiser.setPower(1); } if (engine.gamepad1.dpad_down) { - robot.orangeArmRiser.setPower(-0.5); + robot.orangeArmRiser.setPower(-1); } } // nothing pressed nothing move... @@ -130,11 +129,11 @@ public class TeleOpState extends CyberarmState { if (engine.gamepad2.dpad_down || engine.gamepad2.dpad_up) { if (engine.gamepad2.dpad_up) { - robot.whiteArmRiser.setPower(8); + robot.whiteArmRiser.setPower(1); } if (engine.gamepad2.dpad_down) { - robot.whiteArmRiser.setPower(-0.5); + robot.whiteArmRiser.setPower(-1); } } // nothing pressed nothing move... 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 new file mode 100644 index 0000000..fb79056 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/MecanumRobot.java @@ -0,0 +1,252 @@ +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; + + + + 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"); + + driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD); + driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + + 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/Mecanum_Drive_Engine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/Mecanum_Drive_Engine.java new file mode 100644 index 0000000..391a399 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/Mecanum_Drive_Engine.java @@ -0,0 +1,13 @@ +package org.timecrafters.FreightFrenzy.MecanumRobot_Spencer; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; + +@TeleOp(name = "TeleOp Mecanum Robot") + +public class Mecanum_Drive_Engine extends CyberarmEngine { + @Override + public void setup() { addState(new TeleOpStateMecanumRobot(new MecanumRobot(this)));} + + } 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 new file mode 100644 index 0000000..97a60d9 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/TeleOpStateMecanumRobot.java @@ -0,0 +1,116 @@ +package org.timecrafters.FreightFrenzy.MecanumRobot_Spencer; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.FreightFrenzy.MecanumRobot_Spencer.MecanumRobot; + +public class TeleOpStateMecanumRobot extends CyberarmState { + + MecanumRobot robot; + + public TeleOpStateMecanumRobot(MecanumRobot mecanumRobot) { + + } + + @Override + public void exec() { + + // Slow Mode + + if (engine.gamepad1.left_trigger > 0.5){ + // TankDrive + // Left joystick foward; Left side foward; + + 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; + robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 0.5); + robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 0.5); + + + // Strafing left and right + + if (engine.gamepad1.left_bumper) { + robot.driveFrontLeft.setPower(0.5); + robot.driveBackLeft.setPower(-0.5); + robot.driveFrontRight.setPower(-0.5); + robot.driveBackRight.setPower(0.5); + } else if (engine.gamepad1.right_bumper) { + robot.driveFrontLeft.setPower(-0.5); + 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); + + } + } + // Normal Speed + + else if (engine.gamepad1.right_stick_button){ + + // left stick foward; right side foward + robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1); + robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1); + + // right stick foward; right side foward; + 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) { + 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); + + } + } + + else { + // left stick foward; right side foward + robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1); + robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1); + + // right stick foward; right side foward; + 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) { + 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); + + } + } + } +}