diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentificationWebcam.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentificationWebcam.java index 866261f223..bff93d0e42 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentificationWebcam.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentificationWebcam.java @@ -91,7 +91,7 @@ public class ConceptVuMarkIdentificationWebcam extends LinearOpMode { /* * Retrieve the camera we are to use. */ - webcamName = hardwareMap.get(WebcamName.class, "Webcam 1"); + webcamName = hardwareMap.get(WebcamName.class, "Webcam"); /* * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone); @@ -115,7 +115,7 @@ public class ConceptVuMarkIdentificationWebcam extends LinearOpMode { * Once you've obtained a license key, copy the string from the Vuforia web site * and paste it in to your code on the next line, between the double quotes. */ - parameters.vuforiaLicenseKey = " -- YOUR NEW VUFORIA KEY GOES HERE --- "; + parameters.vuforiaLicenseKey = "AZ7AcNj/////AAABmdKSAUs6jUbsp930wtIqmTIRxjvnBoJicMSGMSRBRbhQXiAWgOguLnfnmd4VY/dCrYjng3gsv1I5Ay+eqb/l35mf6A2IuMtk67j2418iymTvTLGolY0+WhbIGrOwoF8GEjnqqn0fGDSZQGJbrpIKvNvDyj87SiV7cqDvtpyKzU8IqmlPzxSq8S5+opn6ai7Rl3w6P/uDSIThMYNi9Eb/BelfNf/OjToOO5JXqx4muxjz8wJbGSIC++GYHjoPPn+oPSEvHEKaLMIy43JsTMC1lilc8XK2JBnUjQf+b7rmphbz8kMwI6Kr6rv/ubUkvwIor2lBKw7SPeGIawSWBoVdVncDgyUylbWBmh5RHIx1g2nX"; /** diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavigationWebcam.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavigationWebcam.java index 1b619f3dea..d70c8c05b1 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavigationWebcam.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavigationWebcam.java @@ -147,7 +147,7 @@ public class ConceptVuforiaNavigationWebcam extends LinearOpMode { * Once you've obtained a license key, copy the string from the Vuforia web site * and paste it in to your code on the next line, between the double quotes. */ - parameters.vuforiaLicenseKey = " -- YOUR NEW VUFORIA KEY GOES HERE --- "; + parameters.vuforiaLicenseKey = "Abhg0uD/////AAABmY26d2nlY0Hmptt0cQ/Ros4OL3zWSe0yB6zr8WuTqEvE/K9IgpgHArL+IxI5hb23LlUKkh5sx6XPbxX+Z6yg/jAIlHKYPrLqVgM8efHoio7dx5E8EfTyNmNaa83pY4beZ4MySDc3lB7CXjoScgP1+pVEujlo1nGp3nx61E3oVegAymgyZdfAtIjJgs8/OmeXKDXxOaYZcSN/ww99IkGFwSIA02mX8pdIUXr0DtCVKLsqq5v4sNoRsc4udSDr+Y+3eyDuMGCQDlHoiN5yPHyPiIBG1Urko8/Pnq1fl9VmBVg6l/pRJ2H6KfyNmFnt2ceoFIAFopWuV8Tc8VaSKC5n95PjYda6pOpbH8jQjMr/sEns"; /** * We also indicate which camera on the RC we wish to use. diff --git a/README.md b/README.md index 65740d8c79..5df3f0c399 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,13 @@ -## NOTICE +# FTC-Skystone-2019-2020-Programming -This repository contains the public FTC SDK for the SKYSTONE (2019-2020) competition season. +Welcome! -Formerly this software project was hosted [here](https://github.com/ftctechnh/ftc_app). Teams who are competing in the SKYSTONE Challenge should use this [new SKYSTONE repository](https://github.com/FIRST-Tech-Challenge/SKYSTONE) instead of the older (and no longer updated) ftc_app repository. +This module, TeamCode, is the place where our team has wrote the code for our robot controller app for the FTC Rover Ruckus season. + +Programmers: Read the issue in the Issues tab. This is where you can assign different team members to do different things. + +Software Team: Ian Fernandes, Melissa Fusco, and Bella Gonzalez -## Welcome! -This GitHub repository contains the source code that is used to build an Android app to control a *FIRST* Tech Challenge competition robot. To use this SDK, download/clone the entire project to your local computer. ## Getting Started If you are new to robotics or new to the *FIRST* Tech Challenge, then you should consider reviewing the [FTC Blocks Tutorial](https://github.com/FIRST-Tech-Challenge/SKYSTONE/wiki/Blocks-Tutorial) to get familiar with how to use the control system: diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java new file mode 100644 index 0000000000..212af5ebc0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java @@ -0,0 +1,177 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + + +/** + Tinoco's starting working code, I named the robot Cleopatra. + */ + +@TeleOp(name="Basic: Linear OpMode for 1019-2020 Season", group="Linear Opmode") +//@Disabled +public class Cleopatra12820TeleOp extends LinearOpMode { + + + // public static final double INCREMENT=0.01; // amount to slew servo each CYCLE_MS cycle + public static final int CYCLE_MS = 50; // period of each cycle + public static final double MID_SERVO = 0.5; + + // Declare OpMode members. + private ElapsedTime runtime = new ElapsedTime(); + double position = MID_SERVO;// Start at halfway position + + DcMotor backMotorLeft = null; + DcMotor backMotorRight = null; + DcMotor frontMotorLeft = null; + DcMotor frontMotorRight = null; + DcMotor armElbow = null; + DcMotor armWrist = null; + Servo claw = null; + Servo gripperLeft=null; + Servo gripperRight=null; + + + @Override + public void runOpMode() { + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + + + /* Initialize the hardware variables. Note that the strings used here as parameters + to 'get' must correspond to the names assigned during the robot configuration + step (using the FTC Robot Controller app on the phone).*/ + + + + backMotorLeft = hardwareMap.get(DcMotor.class, "backMotorLeft"); + backMotorRight = hardwareMap.get(DcMotor.class, "backMotorRight"); + frontMotorLeft = hardwareMap.get(DcMotor.class, "frontMotorLeft"); + frontMotorRight = hardwareMap.get(DcMotor.class, "frontMotorRight"); + armElbow = hardwareMap.get(DcMotor.class, "armElbow"); + armWrist = hardwareMap.get(DcMotor.class, "armWrist"); + + claw = hardwareMap.get(Servo.class, "claw"); + gripperLeft = hardwareMap.get(Servo.class, "gripperLeft"); + gripperRight = hardwareMap.get(Servo.class, "gripperRight"); + + + + + // Most robots need the motor on one side to be reversed to drive forward + // Reverse the motor that runs backwards when connected directly to the battery + armElbow.setDirection(DcMotor.Direction.FORWARD); + armWrist.setDirection(DcMotor.Direction.REVERSE); + + backMotorLeft.setDirection(DcMotor.Direction.FORWARD); + backMotorRight.setDirection(DcMotor.Direction.REVERSE); + frontMotorLeft.setDirection(DcMotor.Direction.REVERSE); //I CHANGED THIS ONE ALSO + frontMotorRight.setDirection(DcMotor.Direction.FORWARD); //THIS IS THE ONE I CHANGED + + claw.setDirection(Servo.Direction.FORWARD); + gripperRight.setDirection(Servo.Direction.FORWARD); + gripperLeft.setDirection(Servo.Direction.REVERSE); + + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + runtime.reset(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // Setup a variable for each drive wheel to save power level for telemetry + + double Speed = -gamepad1.left_stick_y; + double Turn = gamepad1.left_stick_x; + double Strafe = gamepad1.right_stick_x; + + + //telemetry.addData("Right Servo position", intakeServoRight.getPosition()); + //telemetry.update(); + +/* + Front Left= +Speed + Turn - Strafe Front Right= +Speed - Turn + Strafe + Back Left = +Speed + Turn + Strafe Back Right = +Speed - Turn - Strafe +*/ + double numFl = Range.clip((+Speed + Turn - Strafe), -1, +1); + double numBl = Range.clip((+Speed + Turn + Strafe), -1, 1); + double numFr = Range.clip((+Speed - Turn + Strafe), -1, +1); + double numBr = Range.clip((+Speed - Turn - Strafe), -1, 1); + double armElbowPower = Range.clip(0.5*gamepad2.left_stick_y, -1, 1); + double armWristPower = Range.clip(2 *gamepad2.right_stick_y, -1, 1); + + armWrist.setPower(armWristPower); + armElbow.setPower(armElbowPower); + frontMotorLeft.setPower(numFl); + if (backMotorLeft != null) + backMotorLeft.setPower(numBl); + + frontMotorRight.setPower(numFr); + if (backMotorRight != null) + backMotorRight.setPower(numBr); + + + + //Use gamepad2 left & right Bumpers to open and close the claw + if (gamepad2.right_bumper) { + claw.setPosition(-1); + } else if (gamepad2.left_bumper) { + claw.setPosition(1); + } + + if(gamepad2.x){ + gripperRight.setPosition(-1); + gripperLeft.setPosition(-1); + }else if(gamepad2.y){ + gripperRight.setPosition(1); + gripperLeft.setPosition(1); + + } + sleep(CYCLE_MS); + idle(); + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + //telemetry.addData("Motors", "left (%.2f), right (%.2f)", intakeLeftPower, intakeRightPower); + telemetry.update(); + } + } + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java new file mode 100644 index 0000000000..af934bf4cd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java @@ -0,0 +1,151 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +/** + * This is NOT an opmode. + * + * This class can be used to define all the specific hardware for a single robot. + * In this case that robot is a Pushbot. + * See PushbotTeleopTank_Iterative and others classes starting with "Pushbot" for usage examples. + * + * This hardware class assumes the following device names have been configured on the robot: + * Note: All names are lower case and some have single spaces between words. + * + * Motor channel: Left drive motor: "left_drive" + * Motor channel: Right drive motor: "right_drive" + * Motor channel: Manipulator drive motor: "left_arm" + * Servo channel: Servo to open left claw: "left_hand" + * Servo channel: Servo to open right claw: "right_hand" + */ +public class CleopatraHardware +{ + /* Public OpMode members. */ + public DcMotor intakeMotorLeft=null; + public DcMotor intakeMotorRight=null; + public DcMotor backMotorLeft=null; + public DcMotor backMotorRight=null; + public DcMotor frontMotorLeft=null; + public DcMotor frontMotorRight=null; + public DcMotor armElbow=null; + public DcMotor armWrist=null; + public Servo intakeServoRight=null; + public Servo intakeServoLeft=null; + public Servo claw=null; + public Servo rotator=null; + + public static final double MID_SERVO = 0.5 ; + public static final double ARM_UP_POWER = 0.45 ; + public static final double ARM_DOWN_POWER = -0.45 ; + public static final double ELBOW_UP_POWER = 0.45 ; + public static final double ELBOW_DOWN_POWER = -0.45 ; + + /* local OpMode members. */ + HardwareMap hwMap = null; + private ElapsedTime period = new ElapsedTime(); + + /* Constructor */ + public CleopatraHardware(){ + + } + + /* Initialize standard Hardware interfaces */ + public void init(HardwareMap ahwMap) { + // Save reference to Hardware map + hwMap = ahwMap; + + // Define and Initialize Motors + intakeMotorLeft = hwMap.get(DcMotor.class, "intakeMotorLeft"); + intakeMotorRight = hwMap.get(DcMotor.class, "intakeMotorRight"); + backMotorLeft = hwMap.get(DcMotor.class, "backMotorLeft"); + backMotorRight = hwMap.get(DcMotor.class, "backMotorRight"); + frontMotorLeft = hwMap.get(DcMotor.class, "frontMotorLeft"); + frontMotorRight = hwMap.get(DcMotor.class, "frontMotorRight"); + armElbow = hwMap.get(DcMotor.class,"armElbow"); + armWrist = hwMap.get(DcMotor.class, "armWrist"); + intakeServoRight = hwMap.get(Servo.class, "intakeServoRight"); + intakeServoLeft = hwMap.get(Servo.class, "intakeServoLeft"); + claw = hwMap.get(Servo.class, "claw"); + rotator=hwMap.get(Servo.class, "rotator"); + + + armElbow.setDirection(DcMotor.Direction.FORWARD); + armWrist.setDirection(DcMotor.Direction.REVERSE); + intakeMotorLeft.setDirection(DcMotor.Direction.REVERSE); + intakeMotorRight.setDirection(DcMotor.Direction.FORWARD); + backMotorLeft.setDirection(DcMotor.Direction.FORWARD); + backMotorRight.setDirection(DcMotor.Direction.REVERSE); + frontMotorLeft.setDirection(DcMotor.Direction.FORWARD); + frontMotorRight.setDirection(DcMotor.Direction.REVERSE); + claw.setDirection(Servo.Direction.FORWARD); + intakeServoRight.setDirection(Servo.Direction.FORWARD); + intakeServoLeft.setDirection(Servo.Direction.REVERSE); + rotator.setDirection(Servo.Direction.FORWARD); + + // Set all motors to zero power + + armElbow.setPower(0); + armWrist.setPower(0); + intakeMotorLeft.setPower(0); + intakeMotorRight.setPower(0); + backMotorLeft.setPower(0); + backMotorRight.setPower(0); + frontMotorLeft.setPower(0); + frontMotorRight.setPower(0); + + + // Set all motors to run without encoders. + // May want to use RUN_USING_ENCODERS if encoders are installed. + armElbow.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + armWrist.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + intakeMotorLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + intakeMotorRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + backMotorLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + backMotorRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontMotorLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontMotorRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + // Define and initialize ALL installed servos. + claw = hwMap.get(Servo.class, "claw"); + intakeServoLeft = hwMap.get(Servo.class, "intakeServoLeft"); + intakeServoRight= hwMap.get(Servo.class, "intakeServoRight"); + rotator=hwMap.get(Servo.class, "rotator"); + claw.setPosition(MID_SERVO); + intakeServoLeft.setPosition(MID_SERVO); + intakeServoRight.setPosition(MID_SERVO); + rotator.setPosition(MID_SERVO); + } + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java new file mode 100644 index 0000000000..7cefb003b6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java @@ -0,0 +1,189 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.robotcontroller.external.samples.HardwarePushbot; + +/** + * This OpMode uses the common Cleopatra hardware class to define the devices on the robot. + * All device access is managed through the CleopatraHardware class. + * The code is structured as a LinearOpMode + * + + */ + +@TeleOp(name="Hardware Cleopatra Tele_Op", group="Tele_Op") +//@Disabled +public class HCleopatra12820TeleOp extends LinearOpMode { + + /* Declare OpMode members. */ + CleopatraHardware robot = new CleopatraHardware(); // It uses Cleopatra's hardware + double clawOffset; // Servo mid position + final double CLAW_SPEED; // sets rate to move claw servo + final double INCREMENT; // amount to slew servo each CYCLE_MS cycle + static final int CYCLE_MS; // period of each cycle + + + static { + CYCLE_MS = 50; + + } + + public HCleopatra12820TeleOp() { + INCREMENT = 0.01; + CLAW_SPEED = 0.02; + clawOffset = 0; + } + // Define class members + Servo servo; + double position = robot.MID_SERVO;// Start at halfway position + //boolean rampUp = true; + + @Override + public void runOpMode() { + + + + /* The init() method of the hardware class does all the work here + */ + robot.init(hardwareMap); + + // Send telemetry message to signify robot waiting; + telemetry.addData("Say", "Hello Driver"); // + telemetry.update(); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + double intakeLeftPower; + double intakeRightPower; + double Speed = -gamepad1.left_stick_y; + double Turn = gamepad1.left_stick_x; + double Strafe = gamepad1.right_stick_x; + double MAX_SPEED = 1.0; + + /* + Front Left= +Speed + Turn - Strafe Front Right= +Speed - Turn + Strafe + Back Left = +Speed + Turn + Strafe Back Right = +Speed - Turn - Strafe +*/ + double numFl = Range.clip((+Speed + Turn - Strafe), -1, +1); + double numBl = Range.clip((+Speed + Turn + Strafe), -1, 1); + double numFr = Range.clip((+Speed - Turn + Strafe), -1, +1); + double numBr = Range.clip((+Speed - Turn - Strafe), -1, 1); + //double armElbowPower = Range.clip(0.5*gamepad2.left_stick_y, -1, 1); + + robot.frontMotorLeft.setPower(numFl -MAX_SPEED +MAX_SPEED); + if (robot.backMotorLeft!= null) { + robot.backMotorLeft.setPower(numBl -MAX_SPEED +MAX_SPEED); + } + robot.frontMotorRight.setPower(numFr -MAX_SPEED +MAX_SPEED); + if (robot.backMotorRight != null) { + robot.backMotorRight.setPower(numBr -MAX_SPEED +MAX_SPEED); + } + //Rotator. + if(gamepad1.a) + position += INCREMENT ; + if(gamepad1.b) + position-=INCREMENT; + robot.rotator.setPosition(position); + sleep(CYCLE_MS); + idle(); + + //Arm wrist + if(gamepad2.a) + robot.armWrist.setPower(robot.ARM_UP_POWER); + + else if (gamepad2.b) + robot.armWrist.setPower(robot.ARM_DOWN_POWER); + + else + robot.armWrist.setPower(0.0); + + //Arm Elbow + if(gamepad2.dpad_up) + robot.armElbow.setPower(robot.ELBOW_UP_POWER); + + else if (gamepad2.dpad_down) + robot.armElbow.setPower(robot.ELBOW_DOWN_POWER); + else + robot.armElbow.setPower(0.0); + + //Intake + if(gamepad2.left_trigger>0 ){ + intakeLeftPower = Range.clip(gamepad2.left_trigger, -1.0, 1.0) ; + intakeRightPower = Range.clip(gamepad2.left_trigger, -1.0, 1.0) ;} + else{ + intakeLeftPower = Range.clip(-gamepad2.right_trigger, -1.0, 1.0) ; + intakeRightPower = Range.clip(-gamepad2.right_trigger, -1.0, 1.0) ;} + robot.intakeMotorLeft.setPower(intakeLeftPower); + robot.intakeMotorRight.setPower(intakeRightPower); + + //Intake Servos + if (gamepad2.x){ + //intakeServoRight.setPosition(srvPower); + robot.intakeServoRight.setPosition(1); + robot.intakeServoLeft.setPosition(1); + } + if(gamepad2.y){ + robot.intakeServoRight.setPosition(0); + robot.intakeServoLeft.setPosition(0); + + } + //Use gamepad2 left & right Bumpers to open and close the claw + if(gamepad2.right_bumper) + clawOffset += CLAW_SPEED; + + else if(gamepad2.left_bumper) + robot.claw.setPosition(0); + clawOffset -= CLAW_SPEED; + + clawOffset = Range.clip(clawOffset, -0.5, 0.5); + robot.claw.setPosition(robot.MID_SERVO + clawOffset); + + // Send telemetry message to signify robot running; + telemetry.addData("claw", "Offset = %.2f", clawOffset); + telemetry.addData("left", "%.2f", robot.intakeMotorLeft); + telemetry.addData("right", "%.2f", robot.intakeMotorRight); + telemetry.update(); + + // Pace this loop so jaw action is reasonable speed. + sleep(50); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigation12820.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigation12820.java new file mode 100644 index 0000000000..61cab9c0dd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigation12820.java @@ -0,0 +1,362 @@ +/* Copyright (c) 2019 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.ClassFactory; +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.matrices.VectorF; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +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 java.util.ArrayList; +import java.util.List; + +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.AxesOrder.YZX; +import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC; +import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection.BACK; + +/** + * This 2019-2020 OpMode illustrates the basics of using the Vuforia localizer to determine + * positioning and orientation of robot on the SKYSTONE FTC field. + * The code is structured as a LinearOpMode + * + * When images are located, Vuforia is able to determine the position and orientation of the + * image relative to the camera. This sample code then combines that information with a + * knowledge of where the target images are on the field, to determine the location of the camera. + * + * From the Audience perspective, the Red Alliance station is on the right and the + * Blue Alliance Station is on the left. + + * Eight perimeter targets are distributed evenly around the four perimeter walls + * Four Bridge targets are located on the bridge uprights. + * Refer to the Field Setup manual for more specific location details + * + * A final calculation then uses the location of the camera on the robot to determine the + * robot's location and orientation on the field. + * + * @see VuforiaLocalizer + * @see VuforiaTrackableDefaultListener + * see skystone/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list. + * + * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as + * is explained below. + */ + + +@TeleOp(name="SKYSTONE Vuforia Nav", group ="Concept") +@Disabled +public class VuforiaSkyStoneNavigation12820 extends LinearOpMode { + + // IMPORTANT: For Phone Camera, set 1) the camera source and 2) the orientation, based on how your phone is mounted: + // 1) Camera Source. Valid choices are: BACK (behind screen) or FRONT (selfie side) + // 2) Phone Orientation. Choices are: PHONE_IS_PORTRAIT = true (portrait) or PHONE_IS_PORTRAIT = false (landscape) + // + // NOTE: If you are running on a CONTROL HUB, with only one USB WebCam, you must select CAMERA_CHOICE = BACK; and PHONE_IS_PORTRAIT = false; + // + private static final VuforiaLocalizer.CameraDirection CAMERA_CHOICE = BACK; + private static final boolean PHONE_IS_PORTRAIT = false ; + + /* + * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which + * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function. + * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer + * web site at https://developer.vuforia.com/license-manager. + * + * Vuforia license keys are always 380 characters long, and look as if they contain mostly + * random data. As an example, here is a example of a fragment of a valid key: + * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ... + * Once you've obtained a license key, copy the string from the Vuforia web site + * and paste it in to your code on the next line, between the double quotes. + */ + private static final String VUFORIA_KEY = + " AZ7AcNj/////AAABmdKSAUs6jUbsp930wtIqmTIRxjvnBoJicMSGMSRBRbhQXiAWgOguLnfnmd4VY/dCrYjng3gsv1I5Ay+eqb/l35mf6A2IuMtk67j2418iymTvTLGolY0+WhbIGrOwoF8GEjnqqn0fGDSZQGJbrpIKvNvDyj87SiV7cqDvtpyKzU8IqmlPzxSq8S5+opn6ai7Rl3w6P/uDSIThMYNi9Eb/BelfNf/OjToOO5JXqx4muxjz8wJbGSIC++GYHjoPPn+oPSEvHEKaLMIy43JsTMC1lilc8XK2JBnUjQf+b7rmphbz8kMwI6Kr6rv/ubUkvwIor2lBKw7SPeGIawSWBoVdVncDgyUylbWBmh5RHIx1g2nX "; + + // Since ImageTarget trackables use mm to specifiy their dimensions, we must use mm for all the physical dimension. + // We will define some constants and conversions here + 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 + + // Constant for Stone Target + private static final float stoneZ = 2.00f * mmPerInch; + + // Constants for the center support targets + private static final float bridgeZ = 6.42f * mmPerInch; + private static final float bridgeY = 23 * mmPerInch; + private static final float bridgeX = 5.18f * mmPerInch; + private static final float bridgeRotY = 59; // Units are degrees + private static final float bridgeRotZ = 180; + + // Constants for perimeter targets + private static final float halfField = 72 * mmPerInch; + private static final float quadField = 36 * mmPerInch; + + // Class Members + private OpenGLMatrix lastLocation = null; + private VuforiaLocalizer vuforia = null; + private boolean targetVisible = false; + private float phoneXRotate = 0; + private float phoneYRotate = 0; + private float phoneZRotate = 0; + + @Override public void runOpMode() { + /* + * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine. + * We can pass Vuforia the handle to a camera preview resource (on the RC phone); + * If no camera monitor is desired, use the parameter-less constructor instead (commented out below). + */ + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); + + // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + parameters.vuforiaLicenseKey = VUFORIA_KEY; + parameters.cameraDirection = CAMERA_CHOICE; + + // Instantiate the Vuforia engine + vuforia = ClassFactory.getInstance().createVuforia(parameters); + + // Load the data sets for the trackable objects. These particular data + // sets are stored in the 'assets' part of our application. + VuforiaTrackables targetsSkyStone = this.vuforia.loadTrackablesFromAsset("Skystone"); + + VuforiaTrackable stoneTarget = targetsSkyStone.get(0); + stoneTarget.setName("Stone Target"); + VuforiaTrackable blueRearBridge = targetsSkyStone.get(1); + blueRearBridge.setName("Blue Rear Bridge"); + VuforiaTrackable redRearBridge = targetsSkyStone.get(2); + redRearBridge.setName("Red Rear Bridge"); + VuforiaTrackable redFrontBridge = targetsSkyStone.get(3); + redFrontBridge.setName("Red Front Bridge"); + VuforiaTrackable blueFrontBridge = targetsSkyStone.get(4); + blueFrontBridge.setName("Blue Front Bridge"); + VuforiaTrackable red1 = targetsSkyStone.get(5); + red1.setName("Red Perimeter 1"); + VuforiaTrackable red2 = targetsSkyStone.get(6); + red2.setName("Red Perimeter 2"); + VuforiaTrackable front1 = targetsSkyStone.get(7); + front1.setName("Front Perimeter 1"); + VuforiaTrackable front2 = targetsSkyStone.get(8); + front2.setName("Front Perimeter 2"); + VuforiaTrackable blue1 = targetsSkyStone.get(9); + blue1.setName("Blue Perimeter 1"); + VuforiaTrackable blue2 = targetsSkyStone.get(10); + blue2.setName("Blue Perimeter 2"); + VuforiaTrackable rear1 = targetsSkyStone.get(11); + rear1.setName("Rear Perimeter 1"); + VuforiaTrackable rear2 = targetsSkyStone.get(12); + rear2.setName("Rear Perimeter 2"); + + // For convenience, gather together all the trackable objects in one easily-iterable collection */ + List allTrackables = new ArrayList(); + allTrackables.addAll(targetsSkyStone); + + /** + * In order for localization to work, we need to tell the system where each target is on the field, and + * where the phone resides on the robot. These specifications are in the form of transformation matrices. + * Transformation matrices are a central, important concept in the math here involved in localization. + * See Transformation Matrix + * for detailed information. Commonly, you'll encounter transformation matrices as instances + * of the {@link OpenGLMatrix} class. + * + * If you are standing in the Red Alliance Station looking towards the center of the field, + * - The X axis runs from your left to the right. (positive from the center to the right) + * - The Y axis runs from the Red Alliance Station towards the other side of the field + * where the Blue Alliance Station is. (Positive is from the center, towards the BlueAlliance station) + * - The Z axis runs from the floor, upwards towards the ceiling. (Positive is above the floor) + * + * Before being transformed, each target image is conceptually located at the origin of the field's + * coordinate system (the center of the field), facing up. + */ + + // Set the position of the Stone Target. Since it's not fixed in position, assume it's at the field origin. + // Rotated it to to face forward, and raised it to sit on the ground correctly. + // This can be used for generic target-centric approach algorithms + stoneTarget.setLocation(OpenGLMatrix + .translation(0, 0, stoneZ) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90))); + + //Set the position of the bridge support targets with relation to origin (center of field) + blueFrontBridge.setLocation(OpenGLMatrix + .translation(-bridgeX, bridgeY, bridgeZ) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 0, bridgeRotY, bridgeRotZ))); + + blueRearBridge.setLocation(OpenGLMatrix + .translation(-bridgeX, bridgeY, bridgeZ) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 0, -bridgeRotY, bridgeRotZ))); + + redFrontBridge.setLocation(OpenGLMatrix + .translation(-bridgeX, -bridgeY, bridgeZ) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 0, -bridgeRotY, 0))); + + redRearBridge.setLocation(OpenGLMatrix + .translation(bridgeX, -bridgeY, bridgeZ) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 0, bridgeRotY, 0))); + + //Set the position of the perimeter targets with relation to origin (center of field) + red1.setLocation(OpenGLMatrix + .translation(quadField, -halfField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 180))); + + red2.setLocation(OpenGLMatrix + .translation(-quadField, -halfField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 180))); + + front1.setLocation(OpenGLMatrix + .translation(-halfField, -quadField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0 , 90))); + + front2.setLocation(OpenGLMatrix + .translation(-halfField, quadField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 90))); + + blue1.setLocation(OpenGLMatrix + .translation(-quadField, halfField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 0))); + + blue2.setLocation(OpenGLMatrix + .translation(quadField, halfField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 0))); + + rear1.setLocation(OpenGLMatrix + .translation(halfField, quadField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0 , -90))); + + rear2.setLocation(OpenGLMatrix + .translation(halfField, -quadField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90))); + + // + // Create a transformation matrix describing where the phone is on the robot. + // + // NOTE !!!! It's very important that you turn OFF your phone's Auto-Screen-Rotation option. + // Lock it into Portrait for these numbers to work. + // + // Info: The coordinate frame for the robot looks the same as the field. + // The robot's "forward" direction is facing out along X axis, with the LEFT side facing out along the Y axis. + // Z is UP on the robot. This equates to a bearing angle of Zero degrees. + // + // The phone starts out lying flat, with the screen facing Up and with the physical top of the phone + // pointing to the LEFT side of the Robot. + // The two examples below assume that the camera is facing forward out the front of the robot. + + // We need to rotate the camera around it's long axis to bring the correct camera forward. + if (CAMERA_CHOICE == BACK) { + phoneYRotate = -90; + } else { + phoneYRotate = 90; + } + + // Rotate the phone vertical about the X axis if it's in portrait mode + if (PHONE_IS_PORTRAIT) { + phoneXRotate = 90 ; + } + + // Next, translate the camera lens to where it is on the robot. + // In this example, it is centered (left to right), but forward of the middle of the robot, and above ground level. + final float CAMERA_FORWARD_DISPLACEMENT = 4.0f * mmPerInch; // eg: Camera is 4 Inches in front of robot center + final float CAMERA_VERTICAL_DISPLACEMENT = 8.0f * mmPerInch; // eg: Camera is 8 Inches above ground + final float CAMERA_LEFT_DISPLACEMENT = 0; // eg: Camera is ON the robot's center line + + OpenGLMatrix robotFromCamera = OpenGLMatrix + .translation(CAMERA_FORWARD_DISPLACEMENT, CAMERA_LEFT_DISPLACEMENT, CAMERA_VERTICAL_DISPLACEMENT) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, YZX, DEGREES, phoneYRotate, phoneZRotate, phoneXRotate)); + + /** Let all the trackable listeners know where the phone is. */ + for (VuforiaTrackable trackable : allTrackables) { + ((VuforiaTrackableDefaultListener) trackable.getListener()).setPhoneInformation(robotFromCamera, parameters.cameraDirection); + } + + // WARNING: + // In this sample, we do not wait for PLAY to be pressed. Target Tracking is started immediately when INIT is pressed. + // This sequence is used to enable the new remote DS Camera Preview feature to be used with this sample. + // CONSEQUENTLY do not put any driving commands in this loop. + // To restore the normal opmode structure, just un-comment the following line: + + // waitForStart(); + + // Note: To use the remote camera preview: + // AFTER you hit Init on the Driver Station, use the "options menu" to select "Camera Stream" + // Tap the preview window to receive a fresh image. + + targetsSkyStone.activate(); + while (!isStopRequested()) { + + // check all the trackable targets to see which one (if any) is visible. + targetVisible = false; + for (VuforiaTrackable trackable : allTrackables) { + if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) { + telemetry.addData("Visible Target", trackable.getName()); + targetVisible = true; + + // getUpdatedRobotLocation() will return null if no new information is available since + // the last time that call was made, or if the trackable is not currently visible. + OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation(); + if (robotLocationTransform != null) { + lastLocation = robotLocationTransform; + } + break; + } + } + + // Provide feedback as to where the robot is located (if we know). + if (targetVisible) { + // express position (translation) of robot in inches. + VectorF translation = lastLocation.getTranslation(); + telemetry.addData("Pos (in)", "{X, Y, Z} = %.1f, %.1f, %.1f", + translation.get(0) / mmPerInch, translation.get(1) / mmPerInch, translation.get(2) / mmPerInch); + + // express the rotation of the robot in degrees. + Orientation rotation = Orientation.getOrientation(lastLocation, EXTRINSIC, XYZ, DEGREES); + telemetry.addData("Rot (deg)", "{Roll, Pitch, Heading} = %.0f, %.0f, %.0f", rotation.firstAngle, rotation.secondAngle, rotation.thirdAngle); + } + else { + telemetry.addData("Visible Target", "none"); + } + telemetry.update(); + } + + // Disable Tracking when we are done; + targetsSkyStone.deactivate(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java new file mode 100644 index 0000000000..87ba9f8084 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java @@ -0,0 +1,371 @@ +/* Copyright (c) 2019 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +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.matrices.VectorF; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +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 java.util.ArrayList; +import java.util.List; + +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.AxesOrder.YZX; +import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC; +import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection.BACK; + +/** + * This 2019-2020 OpMode illustrates the basics of using the Vuforia localizer to determine + * positioning and orientation of robot on the SKYSTONE FTC field. + * The code is structured as a LinearOpMode + * + * When images are located, Vuforia is able to determine the position and orientation of the + * image relative to the camera. This sample code then combines that information with a + * knowledge of where the target images are on the field, to determine the location of the camera. + * + * From the Audience perspective, the Red Alliance station is on the right and the + * Blue Alliance Station is on the left. + + * Eight perimeter targets are distributed evenly around the four perimeter walls + * Four Bridge targets are located on the bridge uprights. + * Refer to the Field Setup manual for more specific location details + * + * A final calculation then uses the location of the camera on the robot to determine the + * robot's location and orientation on the field. + * + * @see VuforiaLocalizer + * @see VuforiaTrackableDefaultListener + * see skystone/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list. + * + * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as + * is explained below. + */ + +@TeleOp(name="Vuforia Navtri1", group ="Concept") +// @Disabled +public class VuforiaSkyStoneNavigationWebcam12820 extends LinearOpMode { + + // IMPORTANT: If you are using a USB WebCam, you must select CAMERA_CHOICE = BACK; and PHONE_IS_PORTRAIT = false; + private static final VuforiaLocalizer.CameraDirection CAMERA_CHOICE = BACK; + private static final boolean PHONE_IS_PORTRAIT = false ; + + /* + * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which + * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function. + * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer + * web site at https://developer.vuforia.com/license-manager. + * + * Vuforia license keys are always 380 characters long, and look as if they contain mostly + * random data. As an example, here is a example of a fragment of a valid key: + * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ... + * Once you've obtained a license key, copy the string from the Vuforia web site + * and paste it in to your code on the next line, between the double quotes. + */ + private static final String VUFORIA_KEY = + " AZ7AcNj/////AAABmdKSAUs6jUbsp930wtIqmTIRxjvnBoJicMSGMSRBRbhQXiAWgOguLnfnmd4VY/dCrYjng3gsv1I5Ay+eqb/l35mf6A2IuMtk67j2418iymTvTLGolY0+WhbIGrOwoF8GEjnqqn0fGDSZQGJbrpIKvNvDyj87SiV7cqDvtpyKzU8IqmlPzxSq8S5+opn6ai7Rl3w6P/uDSIThMYNi9Eb/BelfNf/OjToOO5JXqx4muxjz8wJbGSIC++GYHjoPPn+oPSEvHEKaLMIy43JsTMC1lilc8XK2JBnUjQf+b7rmphbz8kMwI6Kr6rv/ubUkvwIor2lBKw7SPeGIawSWBoVdVncDgyUylbWBmh5RHIx1g2nX"; + + // Since ImageTarget trackables use mm to specifiy their dimensions, we must use mm for all the physical dimension. + // We will define some constants and conversions here + 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 + + // Constant for Stone Target + private static final float stoneZ = 2.00f * mmPerInch; + + // Constants for the center support targets + private static final float bridgeZ = 6.42f * mmPerInch; + private static final float bridgeY = 23 * mmPerInch; + private static final float bridgeX = 5.18f * mmPerInch; + private static final float bridgeRotY = 59; // Units are degrees + private static final float bridgeRotZ = 180; + + // Constants for perimeter targets + private static final float halfField = 72 * mmPerInch; + private static final float quadField = 36 * mmPerInch; + + // Class Members + private OpenGLMatrix lastLocation = null; + private VuforiaLocalizer vuforia = null; + + /** + * This is the webcam we are to use. As with other hardware devices such as motors and + * servos, this device is identified using the robot configuration tool in the FTC application. + */ + WebcamName webcamName = null; + + private boolean targetVisible = false; + private float phoneXRotate = 0; + private float phoneYRotate = 0; + private float phoneZRotate = 0; + + @Override public void runOpMode() { + /* + * Retrieve the camera we are to use. + */ + webcamName = hardwareMap.get(WebcamName.class, "Webcam"); + + /* + * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine. + * We can pass Vuforia the handle to a camera preview resource (on the RC phone); + * If no camera monitor is desired, use the parameter-less constructor instead (commented out below). + */ + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); + + // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + parameters.vuforiaLicenseKey = VUFORIA_KEY; + + /** + * We also indicate which camera on the RC we wish to use. + */ + parameters.cameraName = webcamName; + + // Instantiate the Vuforia engine + vuforia = ClassFactory.getInstance().createVuforia(parameters); + + // Load the data sets for the trackable objects. These particular data + // sets are stored in the 'assets' part of our application. + VuforiaTrackables targetsSkyStone = this.vuforia.loadTrackablesFromAsset("Skystone"); + + VuforiaTrackable stoneTarget = targetsSkyStone.get(0); + stoneTarget.setName("Stone Target"); + VuforiaTrackable blueRearBridge = targetsSkyStone.get(1); + blueRearBridge.setName("Blue Rear Bridge"); + VuforiaTrackable redRearBridge = targetsSkyStone.get(2); + redRearBridge.setName("Red Rear Bridge"); + VuforiaTrackable redFrontBridge = targetsSkyStone.get(3); + redFrontBridge.setName("Red Front Bridge"); + VuforiaTrackable blueFrontBridge = targetsSkyStone.get(4); + blueFrontBridge.setName("Blue Front Bridge"); + VuforiaTrackable red1 = targetsSkyStone.get(5); + red1.setName("Red Perimeter 1"); + VuforiaTrackable red2 = targetsSkyStone.get(6); + red2.setName("Red Perimeter 2"); + VuforiaTrackable front1 = targetsSkyStone.get(7); + front1.setName("Front Perimeter 1"); + VuforiaTrackable front2 = targetsSkyStone.get(8); + front2.setName("Front Perimeter 2"); + VuforiaTrackable blue1 = targetsSkyStone.get(9); + blue1.setName("Blue Perimeter 1"); + VuforiaTrackable blue2 = targetsSkyStone.get(10); + blue2.setName("Blue Perimeter 2"); + VuforiaTrackable rear1 = targetsSkyStone.get(11); + rear1.setName("Rear Perimeter 1"); + VuforiaTrackable rear2 = targetsSkyStone.get(12); + rear2.setName("Rear Perimeter 2"); + + // For convenience, gather together all the trackable objects in one easily-iterable collection */ + List allTrackables = new ArrayList<>(); + allTrackables.addAll(targetsSkyStone); + + /** + * In order for localization to work, we need to tell the system where each target is on the field, and + * where the phone resides on the robot. These specifications are in the form of transformation matrices. + * Transformation matrices are a central, important concept in the math here involved in localization. + * See Transformation Matrix + * for detailed information. Commonly, you'll encounter transformation matrices as instances + * of the {@link OpenGLMatrix} class. + * + * If you are standing in the Red Alliance Station looking towards the center of the field, + * - The X axis runs from your left to the right. (positive from the center to the right) + * - The Y axis runs from the Red Alliance Station towards the other side of the field + * where the Blue Alliance Station is. (Positive is from the center, towards the BlueAlliance station) + * - The Z axis runs from the floor, upwards towards the ceiling. (Positive is above the floor) + * + * Before being transformed, each target image is conceptually located at the origin of the field's + * coordinate system (the center of the field), facing up. + */ + + // Set the position of the Stone Target. Since it's not fixed in position, assume it's at the field origin. + // Rotated it to to face forward, and raised it to sit on the ground correctly. + // This can be used for generic target-centric approach algorithms + stoneTarget.setLocation(OpenGLMatrix + .translation(0, 0, stoneZ) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90))); + + //Set the position of the bridge support targets with relation to origin (center of field) + blueFrontBridge.setLocation(OpenGLMatrix + .translation(-bridgeX, bridgeY, bridgeZ) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 0, bridgeRotY, bridgeRotZ))); + + blueRearBridge.setLocation(OpenGLMatrix + .translation(-bridgeX, bridgeY, bridgeZ) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 0, -bridgeRotY, bridgeRotZ))); + + redFrontBridge.setLocation(OpenGLMatrix + .translation(-bridgeX, -bridgeY, bridgeZ) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 0, -bridgeRotY, 0))); + + redRearBridge.setLocation(OpenGLMatrix + .translation(bridgeX, -bridgeY, bridgeZ) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 0, bridgeRotY, 0))); + + //Set the position of the perimeter targets with relation to origin (center of field) + red1.setLocation(OpenGLMatrix + .translation(quadField, -halfField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 180))); + + red2.setLocation(OpenGLMatrix + .translation(-quadField, -halfField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 180))); + + front1.setLocation(OpenGLMatrix + .translation(-halfField, -quadField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0 , 90))); + + front2.setLocation(OpenGLMatrix + .translation(-halfField, quadField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 90))); + + blue1.setLocation(OpenGLMatrix + .translation(-quadField, halfField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 0))); + + blue2.setLocation(OpenGLMatrix + .translation(quadField, halfField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 0))); + + rear1.setLocation(OpenGLMatrix + .translation(halfField, quadField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0 , -90))); + + rear2.setLocation(OpenGLMatrix + .translation(halfField, -quadField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90))); + + // + // Create a transformation matrix describing where the phone is on the robot. + // + // NOTE !!!! It's very important that you turn OFF your phone's Auto-Screen-Rotation option. + // Lock it into Portrait for these numbers to work. + // + // Info: The coordinate frame for the robot looks the same as the field. + // The robot's "forward" direction is facing out along X axis, with the LEFT side facing out along the Y axis. + // Z is UP on the robot. This equates to a bearing angle of Zero degrees. + // + // The phone starts out lying flat, with the screen facing Up and with the physical top of the phone + // pointing to the LEFT side of the Robot. + // The two examples below assume that the camera is facing forward out the front of the robot. + + // We need to rotate the camera around it's long axis to bring the correct camera forward. + if (CAMERA_CHOICE == BACK) { + phoneYRotate = -90; + } else { + phoneYRotate = 90; + } + + // Rotate the phone vertical about the X axis if it's in portrait mode + if (PHONE_IS_PORTRAIT) { + phoneXRotate = 90 ; + } + + // Next, translate the camera lens to where it is on the robot. + // In this example, it is centered (left to right), but forward of the middle of the robot, and above ground level. + final float CAMERA_FORWARD_DISPLACEMENT = 4.0f * mmPerInch; // eg: Camera is 4 Inches in front of robot-center + final float CAMERA_VERTICAL_DISPLACEMENT = 8.0f * mmPerInch; // eg: Camera is 8 Inches above ground + final float CAMERA_LEFT_DISPLACEMENT = 0; // eg: Camera is ON the robot's center line + + OpenGLMatrix robotFromCamera = OpenGLMatrix + .translation(CAMERA_FORWARD_DISPLACEMENT, CAMERA_LEFT_DISPLACEMENT, CAMERA_VERTICAL_DISPLACEMENT) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, YZX, DEGREES, phoneYRotate, phoneZRotate, phoneXRotate)); + + /** Let all the trackable listeners know where the phone is. */ + for (VuforiaTrackable trackable : allTrackables) { + ((VuforiaTrackableDefaultListener) trackable.getListener()).setPhoneInformation(robotFromCamera, parameters.cameraDirection); + } + + // WARNING: + // In this sample, we do not wait for PLAY to be pressed. Target Tracking is started immediately when INIT is pressed. + // This sequence is used to enable the new remote DS Camera Preview feature to be used with this sample. + // CONSEQUENTLY do not put any driving commands in this loop. + // To restore the normal opmode structure, just un-comment the following line: + + // waitForStart(); + + // Note: To use the remote camera preview: + // AFTER you hit Init on the Driver Station, use the "options menu" to select "Camera Stream" + // Tap the preview window to receive a fresh image. + + targetsSkyStone.activate(); + while (!isStopRequested()) { + + // check all the trackable targets to see which one (if any) is visible. + targetVisible = false; + for (VuforiaTrackable trackable : allTrackables) { + if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) { + telemetry.addData("Visible Target", trackable.getName()); + targetVisible = true; + + // getUpdatedRobotLocation() will return null if no new information is available since + // the last time that call was made, or if the trackable is not currently visible. + OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation(); + if (robotLocationTransform != null) { + lastLocation = robotLocationTransform; + } + break; + } + } + + // Provide feedback as to where the robot is located (if we know). + if (targetVisible) { + // express position (translation) of robot in inches. + VectorF translation = lastLocation.getTranslation(); + telemetry.addData("Pos (in)", "{X, Y, Z} = %.1f, %.1f, %.1f", + translation.get(0) / mmPerInch, translation.get(1) / mmPerInch, translation.get(2) / mmPerInch); + + // express the rotation of the robot in degrees. + Orientation rotation = Orientation.getOrientation(lastLocation, EXTRINSIC, XYZ, DEGREES); + telemetry.addData("Rot (deg)", "{Roll, Pitch, Heading} = %.0f, %.0f, %.0f", rotation.firstAngle, rotation.secondAngle, rotation.thirdAngle); + } + else { + telemetry.addData("Visible Target", "none"); + } + telemetry.update(); + } + + // Disable Tracking when we are done; + targetsSkyStone.deactivate(); + } +} diff --git a/build.gradle b/build.gradle index bb3b818fc2..50960db6a9 100644 --- a/build.gradle +++ b/build.gradle @@ -9,7 +9,7 @@ buildscript { jcenter() } dependencies { - classpath 'com.android.tools.build:gradle:3.4.1' + classpath 'com.android.tools.build:gradle:3.5.0' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index caf54fa280..94f5073ebd 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,6 @@ +#Tue Sep 03 20:47:46 CDT 2019 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-5.1.1-all.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-5.4.1-all.zip