diff --git a/README.md b/README.md index 832a2d396e8..a7c0a47ced3 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,38 @@ +## General Info for 2018 Code: + +``` +DRIVETRAIN CONFIGURATION + front + C D + left right + B A + back + + Works with the hub:port configuration +``` + +Competition Autonomous: Cratersample2 +* drops and turns left to unhook +* linear OpMode + +Competition TeleOp: FourwdTeleop2 +* gamepad1 for drivetrain, gamepad2 for the other 4 motors +* uses tuner to adjust drivetrain multiplier and senscurve exponents +* iterative opmode + +Tuner: +* use this to adjust values of something while program is running. + + +look in individual classes for more info. + + + + + + +## ABOUT THE APP +======= ## NOTICE This repository contains v5.0 of the FTC SDK. No further versions will pushed to https://github.com/ftctechnh/ftc_app. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpModeSample_Iterative.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpModeSample_Iterative.java new file mode 100755 index 00000000000..013fdd7f23c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpModeSample_Iterative.java @@ -0,0 +1,137 @@ +/* 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.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +/** + * This file contains an example of an iterative (Non-Linear) "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When an selection is made from the menu, the corresponding OpMode + * class is instantiated on the Robot Controller and executed. + * + * This particular OpMode just executes a basic Tank Drive Teleop for a two wheeled robot + * It includes all the skeletal structure that all iterative OpModes contain. + * + * Use Android Studios 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 + */ + +@TeleOp(name="Basic: Iterative OpMode", group="Iterative Opmode") +public class BasicOpModeSample_Iterative extends OpMode +{ + // Declare OpMode members. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor aDrive = null; + private DcMotor bDrive = null; + + /* + * Code to run ONCE when the driver hits INIT + */ + @Override + public void init() { + telemetry.addData("Status", "Initialized"); + + // 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). + aDrive = hardwareMap.get(DcMotor.class, "aDrive"); + bDrive = hardwareMap.get(DcMotor.class, "bDrive"); + + // 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 + aDrive.setDirection(DcMotor.Direction.FORWARD); + bDrive.setDirection(DcMotor.Direction.REVERSE); + + // Tell the driver that initialization is complete. + telemetry.addData("Status", "Initialized"); + } + + /* + * Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY + */ + @Override + public void init_loop() { + } + + /* + * Code to run ONCE when the driver hits PLAY + */ + @Override + public void start() { + runtime.reset(); + } + + /* + * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP + */ + @Override + public void loop() { + // Setup a variable for each drive wheel to save power level for telemetry + double leftPower; + double rightPower; + + // Choose to drive using either Tank Mode, or POV Mode + // Comment out the method that's not used. The default below is POV. + + // POV Mode uses left stick to go forward, and right stick to turn. + // - This uses basic math to combine motions and is easier to drive straight. + double drive = -gamepad1.left_stick_y*0.5; + double turn = gamepad1.right_stick_x*0.2; + leftPower = Range.clip(drive + turn, -1.0, 1.0) ; + rightPower = Range.clip(drive - turn, -1.0, 1.0) ; + + // Tank Mode uses one stick to control each wheel. + // - This requires no math, but it is hard to drive forward slowly and keep straight. + // leftPower = -gamepad1.left_stick_y ; + // rightPower = -gamepad1.right_stick_y ; + + // Send calculated power to wheels + aDrive.setPower(leftPower); + bDrive.setPower(rightPower); + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); + } + + /* + * Code to run ONCE after the driver hits STOP + */ + @Override + public void stop() { + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ButtonPress.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ButtonPress.java new file mode 100644 index 00000000000..a43030a6742 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ButtonPress.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode; + +/** + * Manages toggling an action. + * + * Call checkToggleStatus once every loop to determine whether a full button press has + * occurred or not. + */ +final class ButtonPress +{ + /** + * Holds all the states that a toggle can be in. When pressing a button, there are 3 states: + * 1. Not begun + * 2. In progress + * 3. Complete + * + * If you're checking a button press using a conventional on/off check and using it to + * flip a boolean, then you'll flip once for every time the button is held and the + * loop iterates. + */ + enum Status + { + NOT_BEGUN , + IN_PROGRESS , + COMPLETE + } + + + private Status _status = Status.NOT_BEGUN; // Current status of the toggle + + + /** + * Monitors and adjusts the toggle value based on previous toggle values and the + * state of the boolean passed in. + */ + final public Status status(boolean buttonStatus) + { + // If the button is being held + if(buttonStatus && _status == Status.NOT_BEGUN) + _status = Status.IN_PROGRESS; + + // If the button is not being pressed and the toggle was in progress + else if(!buttonStatus && _status == Status.IN_PROGRESS) + _status = Status.COMPLETE; + + // If the toggle is finished + else if(_status == Status.COMPLETE) + _status = Status.NOT_BEGUN; + + return _status; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calculate.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calculate.java new file mode 100644 index 00000000000..8a234e282f1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Calculate.java @@ -0,0 +1,106 @@ +package org.firstinspires.ftc.teamcode; + + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +public class Calculate { + +//DRIVE + public static double[] FOD(double x, double y, double gyroAngle, boolean inputSquare, boolean outputSquare) { + //input the components of the vector (in circle or square) in which you want to robot to go and the heading of the robot + //outputs the relative x and y to the global coordinate plane (circle or square) + + double localU; + double localV; + + //if not a circle coordinate, convert to circle + if (inputSquare) { + double[] coord = squareToCircle(x, y); + localU = coord[0]; + localV = coord[1]; + } else { + localU = x; + localV = y; + } + + //does rotation in circled coordinate plane because rotation is a circle + //uses the rotate point around origin formula + double rotAngle = -Math.toRadians(gyroAngle); + double globalX = 0.0001 * (Math.round((localU * Math.cos(rotAngle) - localV * Math.sin(rotAngle)) * 10000)); + double globalY = 0.0001 * (Math.round((localV * Math.cos(rotAngle) + localU * Math.sin(rotAngle)) * 10000)); + + //output as requested + if (outputSquare) { + return circleToSquare(globalX, globalY); + } else { + return new double[]{globalX, globalY}; + } + } + + +//COORDINATE PLANE + + public static double sensCurve(double joystickVal, double exponent){ + return Math.copySign( Math.pow( Math.abs( joystickVal), exponent), joystickVal); + } + + public static double[] polarToCartesian(double magnitude, double angle, boolean angleInRadians) { + //converts a power and angle to x and y coordinates + //output: x, y + double radians; + if (angleInRadians) { + radians = angle; + } else { + radians = Math.toRadians(angle); + } + return new double[] {magnitude * Math.cos(radians), magnitude * Math.sin(radians)}; + } + + public static double[] cartesianToPolar(double x, double y) { + //converts x and y coordinates to power and angle + //output: magnitude, direction + double radians = Math.atan2(y,x); //get diretion (but it outputs in radians) + double degrees = Math.toDegrees(radians); //convert to degrees + double magnitude = Math.sqrt(x*x + y*y); //pythagorean theorem + + return new double[] {magnitude, degrees}; + } + + public static double[] circleToSquare(double u, double v) { + //when you want to "stretch" points of a circular coordinate plane onto a square one, ex (0.707, 0.707) maps to (1, 1) + //visit http://squircular.blogspot.com/2015/09/mapping-circle-to-square.html for more info (this is the inverse) + + double u2 = u * u; + double v2 = v * v; + double twosqrt2 = 2.0 * Math.sqrt(2.0); + double subtermx = 2.0 + u2 - v2; + double subtermy = 2.0 - u2 + v2; + double termx1 = subtermx + u * twosqrt2; + double termx2 = subtermx - u * twosqrt2; + double termy1 = subtermy + v * twosqrt2; + double termy2 = subtermy - v * twosqrt2; + double x = (0.5 * Math.sqrt(termx1) - 0.5 * Math.sqrt(termx2)); + double y = (0.5 * Math.sqrt(termy1) - 0.5 * Math.sqrt(termy2)); + return new double[]{x, y}; + } + + public static double[] squareToCircle(double x, double y) { + //when you want to "squeeze" points of a square coordinate plane onto a circle one, ex (1, 1) maps to (0.707, 0.707) + //visit http://squircular.blogspot.com/2015/09/mapping-circle-to-square.html for more info + + double u = x * Math.sqrt(1 - y * y / 2); + double v = y * Math.sqrt(1 - x * x / 2); + return new double[]{u, v}; + } + + +//ANGLES + public static double normalizeAngle(double degrees) { + //Takes any degree angle and converts it to between -179 and +180 degrees + return AngleUnit.DEGREES.normalize(degrees); + } + + + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTensorFlowObjectDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTensorFlowObjectDetection.java new file mode 100644 index 00000000000..368d6d3aeab --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTensorFlowObjectDetection.java @@ -0,0 +1,231 @@ +/* Copyright (c) 2018 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 android.media.AudioFormat; +import android.media.AudioManager; +import android.media.AudioTrack; +import android.media.ToneGenerator; + +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.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; + +import java.util.List; + +/** + * This 2018-2019 OpMode illustrates the basics of using the TensorFlow Object Detection API to + * determine the position of the gold and silver minerals. + * + * 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 = "Concept: TensorFlow Object Detection", group = "Concept") +public class ConceptTensorFlowObjectDetection extends LinearOpMode { + private static final String TFOD_MODEL_ASSET = "RoverRuckus.tflite"; + private static final String LABEL_GOLD_MINERAL = "Gold Mineral"; + private static final String LABEL_SILVER_MINERAL = "Silver Mineral"; + + /* + * 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 = "AZCMreL/////AAABmdWWWC3/vE83hRmhRnWGAul7owT5y1Mn4hOtumsnaMVr67TGWwxY7HgprjL5Ng7ryllGy3WuOPA2BLCwD3s1zxukV6N4Eo5sgzqntZSibOI2BPj2xm7fdQklrFDF+XqkxD9xpKRTDuZk39tRPXREvU0RIrC0gl1AdvmLNh4UN48GZRHtjgTz1cNTHq9HMO5q0bJ1FF4bQHyilcOeK6YoYSaLE+H9cpSarCjIP7H77Z7acyfzE0sjWxfGcONmQoREkhVC3VeOPh3HkRSMMnxxBj+uaDCLQkOA4Ejilr7YGceRz8HY9yy02vJs+oYCfCZOz97vIjF4rOZW171bH7CwMRhqNIWs0SS2oBydH1nVk/yR"; + + /** + * {@link #vuforia} is the variable we will use to store our instance of the Vuforia + * localization engine. + */ + private VuforiaLocalizer vuforia; + + /** + * {@link #tfod} is the variable we will use to store our instance of the Tensor Flow Object + * Detection engine. + */ + private TFObjectDetector tfod; + + @Override + public void runOpMode() { + // The TFObjectDetector uses the camera frames from the VuforiaLocalizer, so we create that + // first. + initVuforia(); + + if (ClassFactory.getInstance().canCreateTFObjectDetector()) { + initTfod(); + } else { + telemetry.addData("Sorry!", "This device is not compatible with TFOD"); + } + + /** Wait for the game to begin */ + telemetry.addData(">", "Press Play to start tracking"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + /** Activate Tensor Flow Object Detection. */ + if (tfod != null) { + tfod.activate(); + } + + ToneGenerator tone = new ToneGenerator(AudioManager.STREAM_NOTIFICATION, 100); + tone.startTone(ToneGenerator.TONE_CDMA_KEYPAD_VOLUME_KEY_LITE); + + + while (opModeIsActive()) { + if (tfod != null) { + // getUpdatedRecognitions() will return null if no new information is available since + // the last time that call was made. + List updatedRecognitions = tfod.getUpdatedRecognitions(); + + if (updatedRecognitions != null) { + telemetry.addData("# Object Detected", updatedRecognitions.size()); + int goldMineralX = -1; + int silverMineral1X = -1; + int silverMineral2X = -1; + int getRight = -1; + int getTop = -1; + + for (Recognition recognition : updatedRecognitions) { + + getRight = (int) recognition.getRight(); + getTop = (int) recognition.getTop(); + + if (recognition.getLabel().equals(LABEL_GOLD_MINERAL)) { + goldMineralX = (int) recognition.getLeft(); + } else if (silverMineral1X == -1) { + silverMineral1X = (int) recognition.getLeft(); + } else { + silverMineral2X = (int) recognition.getLeft(); + } + } + + telemetry.addData("getLeft",goldMineralX); + telemetry.addData("getRight",getRight); + telemetry.addData("getTop",getTop); + if(goldMineralX != -1){ + playSound(goldMineralX*4,getTop*2,10); + } + + if (goldMineralX != -1 && silverMineral1X != -1 && silverMineral2X != -1) { + if (goldMineralX < silverMineral1X && goldMineralX < silverMineral2X) { + telemetry.addData("Gold Mineral Position", "Left"); + } else if (goldMineralX > silverMineral1X && goldMineralX > silverMineral2X) { + telemetry.addData("Gold Mineral Position", "Right"); + } else { + telemetry.addData("Gold Mineral Position", "Center"); + } + } + + telemetry.update(); + //end if object detected + } + } + } + } + + if (tfod != null) { + tfod.shutdown(); + } + } + + + + private void playSound(double Lfreq, double Rfreq, int durationMs) + { + int count = (int)(44100.0 * 2.0 * (durationMs / 1000.0)) & ~1; + short[] samples = new short[count]; + for(int i = 0; i < count; i += 2){ + short Lsample = (short)(Math.sin(2 * Math.PI * i / (44100.0 / Lfreq)) * 0x7FFF); + short Rsample = (short)(Math.sin(2 * Math.PI * i / (44100.0 / Rfreq)) * 0x7FFF); + samples[i + 0] = Lsample; + samples[i + 1] = Rsample; + } + AudioTrack track = new AudioTrack(AudioManager.STREAM_MUSIC, 44100, + AudioFormat.CHANNEL_OUT_STEREO, AudioFormat.ENCODING_PCM_16BIT, + count * (Short.SIZE / 8), AudioTrack.MODE_STATIC); + track.write(samples, 0, count); + + try { + track.play(); + }catch (Exception err) { + telemetry.addData("CaughtError",err.getMessage()); + } + track.play(); + track.release(); + } + + + + /** + * Initialize the Vuforia localization engine. + */ + private void initVuforia() { + /* + * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine. + */ + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + parameters.vuforiaLicenseKey = VUFORIA_KEY; + parameters.cameraDirection = CameraDirection.BACK; + + // Instantiate the Vuforia engine + vuforia = ClassFactory.getInstance().createVuforia(parameters); + + // Loading trackables is not necessary for the Tensor Flow Object Detection engine. + } + + /** + * Initialize the Tensor Flow Object Detection engine. + */ + private void initTfod() { + int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId); + tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia); + tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptVuforiaNavRoverRuckus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptVuforiaNavRoverRuckus.java new file mode 100755 index 00000000000..a0dbe7b99ee --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptVuforiaNavRoverRuckus.java @@ -0,0 +1,340 @@ +/* Copyright (c) 2018 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 android.media.AudioFormat; +import android.media.AudioManager; +import android.media.AudioTrack; + +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.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; +import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection.FRONT; + + +/** + * This 2018-2019 OpMode illustrates the basics of using the Vuforia localizer to determine + * positioning and orientation of robot on the FTC field. + * The code is structured as a LinearOpMode + * + * Vuforia uses the phone's camera to inspect it's surroundings, and attempt to locate target images. + * + * When images are located, Vuforia is able to determine the position and orientation of the + * image relative to the camera. This sample code than combines that information with a + * knowledge of where the target images are on the field, to determine the location of the camera. + * + * This example assumes a "square" field configuration where the red and blue alliance stations + * are on opposite walls of each other. + * + * From the Audience perspective, the Red Alliance station is on the right and the + * Blue Alliance Station is on the left. + + * The four vision targets are located in the center of each of the perimeter walls with + * the images facing inwards towards the robots: + * - BlueRover is the Mars Rover image target on the wall closest to the blue alliance + * - RedFootprint is the Lunar Footprint target on the wall closest to the red alliance + * - FrontCraters is the Lunar Craters image target on the wall closest to the audience + * - BackSpace is the Deep Space image target on the wall farthest from the audience + * + * 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 ftc_app/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="Concept: Vuforia Rover Nav", group ="Concept") +public class ConceptVuforiaNavRoverRuckus extends LinearOpMode { + + /* + * 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 = "AZCMreL/////AAABmdWWWC3/vE83hRmhRnWGAul7owT5y1Mn4hOtumsnaMVr67TGWwxY7HgprjL5Ng7ryllGy3WuOPA2BLCwD3s1zxukV6N4Eo5sgzqntZSibOI2BPj2xm7fdQklrFDF+XqkxD9xpKRTDuZk39tRPXREvU0RIrC0gl1AdvmLNh4UN48GZRHtjgTz1cNTHq9HMO5q0bJ1FF4bQHyilcOeK6YoYSaLE+H9cpSarCjIP7H77Z7acyfzE0sjWxfGcONmQoREkhVC3VeOPh3HkRSMMnxxBj+uaDCLQkOA4Ejilr7YGceRz8HY9yy02vJs+oYCfCZOz97vIjF4rOZW171bH7CwMRhqNIWs0SS2oBydH1nVk/yR\n"; + + // 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 mmFTCFieldWidth = (12*6) * mmPerInch; // the width of the FTC field (from the center point to the outer panels) + private static final float mmTargetHeight = (6) * mmPerInch; // the height of the center of the target image above the floor + + // Select which camera you want use. The FRONT camera is the one on the same side as the screen. + // Valid choices are: BACK or FRONT + private static final VuforiaLocalizer.CameraDirection CAMERA_CHOICE = BACK; + + private OpenGLMatrix lastLocation = null; + private boolean targetVisible = false; + + /** + * {@link #vuforia} is the variable we will use to store our instance of the Vuforia + * localization engine. + */ + VuforiaLocalizer vuforia; + + @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 parameterless 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 that for the trackable objects. These particular data + // sets are stored in the 'assets' part of our application. + VuforiaTrackables targetsRoverRuckus = this.vuforia.loadTrackablesFromAsset("RoverRuckus"); + VuforiaTrackable blueRover = targetsRoverRuckus.get(0); + blueRover.setName("Blue-Rover"); + VuforiaTrackable redFootprint = targetsRoverRuckus.get(1); + redFootprint.setName("Red-Footprint"); + VuforiaTrackable frontCraters = targetsRoverRuckus.get(2); + frontCraters.setName("Front-Craters"); + VuforiaTrackable backSpace = targetsRoverRuckus.get(3); + backSpace.setName("Back-Space"); + + // For convenience, gather together all the trackable objects in one easily-iterable collection */ + List allTrackables = new ArrayList(); + allTrackables.addAll(targetsRoverRuckus); + + /** + * 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) + * + * This Rover Ruckus sample places a specific target in the middle of each perimeter wall. + * + * 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. + */ + + /** + * To place the BlueRover target in the middle of the blue perimeter wall: + * - First we rotate it 90 around the field's X axis to flip it upright. + * - Then, we translate it along the Y axis to the blue perimeter wall. + */ + OpenGLMatrix blueRoverLocationOnField = OpenGLMatrix + .translation(0, mmFTCFieldWidth, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 0)); + blueRover.setLocation(blueRoverLocationOnField); + + /** + * To place the RedFootprint target in the middle of the red perimeter wall: + * - First we rotate it 90 around the field's X axis to flip it upright. + * - Second, we rotate it 180 around the field's Z axis so the image is flat against the red perimeter wall + * and facing inwards to the center of the field. + * - Then, we translate it along the negative Y axis to the red perimeter wall. + */ + OpenGLMatrix redFootprintLocationOnField = OpenGLMatrix + .translation(0, -mmFTCFieldWidth, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 180)); + redFootprint.setLocation(redFootprintLocationOnField); + + /** + * To place the FrontCraters target in the middle of the front perimeter wall: + * - First we rotate it 90 around the field's X axis to flip it upright. + * - Second, we rotate it 90 around the field's Z axis so the image is flat against the front wall + * and facing inwards to the center of the field. + * - Then, we translate it along the negative X axis to the front perimeter wall. + */ + OpenGLMatrix frontCratersLocationOnField = OpenGLMatrix + .translation(-mmFTCFieldWidth, 0, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0 , 90)); + frontCraters.setLocation(frontCratersLocationOnField); + + /** + * To place the BackSpace target in the middle of the back perimeter wall: + * - First we rotate it 90 around the field's X axis to flip it upright. + * - Second, we rotate it -90 around the field's Z axis so the image is flat against the back wall + * and facing inwards to the center of the field. + * - Then, we translate it along the X axis to the back perimeter wall. + */ + OpenGLMatrix backSpaceLocationOnField = OpenGLMatrix + .translation(mmFTCFieldWidth, 0, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90)); + backSpace.setLocation(backSpaceLocationOnField); + + /** + * Create a transformation matrix describing where the phone is on the robot. + * + * 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. It's very important when you test this code that the top of the + * camera is pointing to the left side of the robot. The rotation angles don't work if you flip the phone. + * + * If using the rear (High Res) camera: + * We need to rotate the camera around it's long axis to bring the rear camera forward. + * This requires a negative 90 degree rotation on the Y axis + * + * If using the Front (Low Res) camera + * We need to rotate the camera around it's long axis to bring the FRONT camera forward. + * This requires a Positive 90 degree rotation on the Y axis + * + * Next, translate the camera lens to where it is on the robot. + * In this example, it is centered (left to right), but 110 mm forward of the middle of the robot, and 200 mm above ground level. + */ + + final int CAMERA_FORWARD_DISPLACEMENT = 110; // eg: Camera is 110 mm in front of robot center + final int CAMERA_VERTICAL_DISPLACEMENT = 200; // eg: Camera is 200 mm above ground + final int CAMERA_LEFT_DISPLACEMENT = 0; // eg: Camera is ON the robot's center line + + OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix + .translation(CAMERA_FORWARD_DISPLACEMENT, CAMERA_LEFT_DISPLACEMENT, CAMERA_VERTICAL_DISPLACEMENT) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, YZX, DEGREES, + CAMERA_CHOICE == FRONT ? 90 : -90, 0, 0)); + + /** Let all the trackable listeners know where the phone is. */ + for (VuforiaTrackable trackable : allTrackables) + { + ((VuforiaTrackableDefaultListener)trackable.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); + } + + /** Wait for the game to begin */ + telemetry.addData(">", "Press Play to start tracking"); + telemetry.update(); + waitForStart(); + + /** Start tracking the data sets we care about. */ + targetsRoverRuckus.activate(); + while (opModeIsActive()) { + + // check all the trackable target 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(); + + playSound(translation.get(1),translation.get(2),100); + + 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(); + } + } + + private void playSound(double Lfreq, double Rfreq, int durationMs) + { + int count = (int)(44100.0 * 2.0 * (durationMs / 1000.0)) & ~1; + short[] samples = new short[count]; + for(int i = 0; i < count; i += 2){ + short Lsample = (short)(Math.sin(2 * Math.PI * i / (44100.0 / Lfreq)) * 0x7FFF); + short Rsample = (short)(Math.sin(2 * Math.PI * i / (44100.0 / Rfreq)) * 0x7FFF); + samples[i + 0] = Lsample; + samples[i + 1] = Rsample; + } + AudioTrack track = new AudioTrack(AudioManager.STREAM_MUSIC, 44100, + AudioFormat.CHANNEL_OUT_STEREO, AudioFormat.ENCODING_PCM_16BIT, + count * (Short.SIZE / 8), AudioTrack.MODE_STATIC); + track.write(samples, 0, count); + + try { + track.play(); + }catch (Exception err) { + telemetry.addData("CaughtError",err.getMessage()); + } + track.play(); + track.release(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptVuforiaNavigation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptVuforiaNavigation.java new file mode 100755 index 00000000000..6dbfc12cb3a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptVuforiaNavigation.java @@ -0,0 +1,336 @@ +/* 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.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.matrices.MatrixF; +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.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; + +/** + * This 2016-2017 OpMode illustrates the basics of using the Vuforia localizer to determine + * positioning and orientation of robot on the FTC field. + * The code is structured as a LinearOpMode + * + * Vuforia uses the phone's camera to inspect it's surroundings, and attempt to locate target images. + * + * When images are located, Vuforia is able to determine the position and orientation of the + * image relative to the camera. This sample code than combines that information with a + * knowledge of where the target images are on the field, to determine the location of the camera. + * + * This example assumes a "diamond" field configuration where the red and blue alliance stations + * are adjacent on the corner of the field furthest from the audience. + * From the Audience perspective, the Red driver station is on the right. + * The two vision target are located on the two walls closest to the audience, facing in. + * The Stones are on the RED side of the field, and the Chips are on the Blue side. + * + * 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 ftc_app/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="Concept: Vuforia Navigation", group ="Concept") +@Disabled +public class ConceptVuforiaNavigation extends LinearOpMode { + + public static final String TAG = "Vuforia Navigation Sample"; + + OpenGLMatrix lastLocation = null; + + /** + * {@link #vuforia} is the variable we will use to store our instance of the Vuforia + * localization engine. + */ + VuforiaLocalizer vuforia; + + @Override public void runOpMode() { + /* + * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone); + * If no camera monitor is desired, use the parameterless constructor instead (commented out below). + */ + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); + + // OR... Do Not Activate the Camera Monitor View, to save power + // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + /* + * 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. + */ + parameters.vuforiaLicenseKey = "AZCMreL/////AAABmdWWWC3/vE83hRmhRnWGAul7owT5y1Mn4hOtumsnaMVr67TGWwxY7HgprjL5Ng7ryllGy3WuOPA2BLCwD3s1zxukV6N4Eo5sgzqntZSibOI2BPj2xm7fdQklrFDF+XqkxD9xpKRTDuZk39tRPXREvU0RIrC0gl1AdvmLNh4UN48GZRHtjgTz1cNTHq9HMO5q0bJ1FF4bQHyilcOeK6YoYSaLE+H9cpSarCjIP7H77Z7acyfzE0sjWxfGcONmQoREkhVC3VeOPh3HkRSMMnxxBj+uaDCLQkOA4Ejilr7YGceRz8HY9yy02vJs+oYCfCZOz97vIjF4rOZW171bH7CwMRhqNIWs0SS2oBydH1nVk/yR"; + + /* + * We also indicate which camera on the RC that we wish to use. + * Here we chose the back (HiRes) camera (for greater range), but + * for a competition robot, the front camera might be more convenient. + */ + parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; + + /** + * Instantiate the Vuforia engine + */ + vuforia = ClassFactory.getInstance().createVuforia(parameters); + + /** + * Load the data sets that for the trackable objects we wish to track. These particular data + * sets are stored in the 'assets' part of our application (you'll see them in the Android + * Studio 'Project' view over there on the left of the screen). You can make your own datasets + * with the Vuforia Target Manager: https://developer.vuforia.com/target-manager. PDFs for the + * example "StonesAndChips", datasets can be found in in this project in the + * documentation directory. + */ + VuforiaTrackables stonesAndChips = this.vuforia.loadTrackablesFromAsset("StonesAndChips"); + VuforiaTrackable redTarget = stonesAndChips.get(0); + redTarget.setName("RedTarget"); // Stones + + VuforiaTrackable blueTarget = stonesAndChips.get(1); + blueTarget.setName("BlueTarget"); // Chips + + /** For convenience, gather together all the trackable objects in one easily-iterable collection */ + List allTrackables = new ArrayList(); + allTrackables.addAll(stonesAndChips); + + /** + * We use units of mm here because that's the recommended units of measurement for the + * size values specified in the XML for the ImageTarget trackables in data sets. E.g.: + * + * You don't *have to* use mm here, but the units here and the units used in the XML + * target configuration files *must* correspond for the math to work out correctly. + */ + float mmPerInch = 25.4f; + float mmBotWidth = 18 * mmPerInch; // ... or whatever is right for your robot + float mmFTCFieldWidth = (12*12 - 2) * mmPerInch; // the FTC field is ~11'10" center-to-center of the glass panels + + /** + * In order for localization to work, we need to tell the system where each target we + * wish to use for navigation resides on the field, and we need to specify where on the robot + * the phone resides. 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. + * + * For the most part, you don't need to understand the details of the math of how transformation + * matrices work inside (as fascinating as that is, truly). Just remember these key points: + *
    + * + *
  1. You can put two transformations together to produce a third that combines the effect of + * both of them. If, for example, you have a rotation transform R and a translation transform T, + * then the combined transformation matrix RT which does the rotation first and then the translation + * is given by {@code RT = T.multiplied(R)}. That is, the transforms are multiplied in the + * reverse of the chronological order in which they applied.
    2. + * + *
  2. A common way to create useful transforms is to use methods in the {@link OpenGLMatrix} + * class and the Orientation class. See, for example, {@link OpenGLMatrix#translation(float, + * float, float)}, {@link OpenGLMatrix#rotation(AngleUnit, float, float, float, float)}, and + * {@link Orientation#getRotationMatrix(AxesReference, AxesOrder, AngleUnit, float, float, float)}. + * Related methods in {@link OpenGLMatrix}, such as {@link OpenGLMatrix#rotated(AngleUnit, + * float, float, float, float)}, are syntactic shorthands for creating a new transform and + * then immediately multiplying the receiver by it, which can be convenient at times.
    3. + * + *
  3. If you want to break open the black box of a transformation matrix to understand + * what it's doing inside, use {@link MatrixF#getTranslation()} to fetch how much the + * transform will move you in x, y, and z, and use {@link Orientation#getOrientation(MatrixF, + * AxesReference, AxesOrder, AngleUnit)} to determine the rotational motion that the transform + * will impart. See {@link #format(OpenGLMatrix)} below for an example.
    4. + * + *
+ * + * This example places the "stones" image on the perimeter wall to the Left + * of the Red Driver station wall. Similar to the Red Beacon Location on the Res-Q + * + * This example places the "chips" image on the perimeter wall to the Right + * of the Blue Driver station. Similar to the Blue Beacon Location on the Res-Q + * + * See the doc folder of this project for a description of the field Axis conventions. + * + * Initially the target is conceptually lying at the origin of the field's coordinate system + * (the center of the field), facing up. + * + * In this configuration, the target's coordinate system aligns with that of the field. + * + * In a real situation we'd also account for the vertical (Z) offset of the target, + * but for simplicity, we ignore that here; for a real robot, you'll want to fix that. + * + * To place the Stones Target on the Red Audience wall: + * - First we rotate it 90 around the field's X axis to flip it upright + * - Then we rotate it 90 around the field's Z access to face it away from the audience. + * - Finally, we translate it back along the X axis towards the red audience wall. + */ + OpenGLMatrix redTargetLocationOnField = OpenGLMatrix + /* Then we translate the target off to the RED WALL. Our translation here + is a negative translation in X.*/ + .translation(-mmFTCFieldWidth/2, 0, 0) + .multiplied(Orientation.getRotationMatrix( + /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ + AxesReference.EXTRINSIC, AxesOrder.XZX, + AngleUnit.DEGREES, 90, 90, 0)); + redTarget.setLocation(redTargetLocationOnField); + RobotLog.ii(TAG, "Red Target=%s", format(redTargetLocationOnField)); + + /* + * To place the Stones Target on the Blue Audience wall: + * - First we rotate it 90 around the field's X axis to flip it upright + * - Finally, we translate it along the Y axis towards the blue audience wall. + */ + OpenGLMatrix blueTargetLocationOnField = OpenGLMatrix + /* Then we translate the target off to the Blue Audience wall. + Our translation here is a positive translation in Y.*/ + .translation(0, mmFTCFieldWidth/2, 0) + .multiplied(Orientation.getRotationMatrix( + /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ + AxesReference.EXTRINSIC, AxesOrder.XZX, + AngleUnit.DEGREES, 90, 0, 0)); + blueTarget.setLocation(blueTargetLocationOnField); + RobotLog.ii(TAG, "Blue Target=%s", format(blueTargetLocationOnField)); + + /** + * Create a transformation matrix describing where the phone is on the robot. Here, we + * put the phone on the right hand side of the robot with the screen facing in (see our + * choice of BACK camera above) and in landscape mode. Starting from alignment between the + * robot's and phone's axes, this is a rotation of -90deg along the Y axis. + * + * When determining whether a rotation is positive or negative, consider yourself as looking + * down the (positive) axis of rotation from the positive towards the origin. Positive rotations + * are then CCW, and negative rotations CW. An example: consider looking down the positive Z + * axis towards the origin. A positive rotation about Z (ie: a rotation parallel to the the X-Y + * plane) is then CCW, as one would normally expect from the usual classic 2D geometry. + */ + OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix + .translation(mmBotWidth/2,0,0) + .multiplied(Orientation.getRotationMatrix( + AxesReference.EXTRINSIC, AxesOrder.YZY, + AngleUnit.DEGREES, -90, 0, 0)); + RobotLog.ii(TAG, "phone=%s", format(phoneLocationOnRobot)); + + /** + * Let the trackable listeners we care about know where the phone is. We know that each + * listener is a {@link VuforiaTrackableDefaultListener} and can so safely cast because + * we have not ourselves installed a listener of a different type. + */ + ((VuforiaTrackableDefaultListener)redTarget.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); + ((VuforiaTrackableDefaultListener)blueTarget.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); + + /** + * A brief tutorial: here's how all the math is going to work: + * + * C = phoneLocationOnRobot maps phone coords -> robot coords + * P = tracker.getPose() maps image target coords -> phone coords + * L = redTargetLocationOnField maps image target coords -> field coords + * + * So + * + * C.inverted() maps robot coords -> phone coords + * P.inverted() maps phone coords -> imageTarget coords + * + * Putting that all together, + * + * L x P.inverted() x C.inverted() maps robot coords to field coords. + * + * @see VuforiaTrackableDefaultListener#getRobotLocation() + */ + + /** Wait for the game to begin */ + telemetry.addData(">", "Press Play to start tracking"); + telemetry.update(); + waitForStart(); + + /** Start tracking the data sets we care about. */ + stonesAndChips.activate(); + + while (opModeIsActive()) { + + for (VuforiaTrackable trackable : allTrackables) { + /** + * 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. + * getRobotLocation() will return null if the trackable is not currently visible. + */ + telemetry.addData(trackable.getName(), ((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible() ? "Visible" : "Not Visible"); // + + OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation(); + if (robotLocationTransform != null) { + lastLocation = robotLocationTransform; + } + } + /** + * Provide feedback as to where the robot was last located (if we know). + */ + if (lastLocation != null) { + // RobotLog.vv(TAG, "robot=%s", format(lastLocation)); + telemetry.addData("Pos", format(lastLocation)); + } else { + telemetry.addData("Pos", "Unknown"); + } + telemetry.update(); + } + } + + /** + * A simple utility that extracts positioning information from a transformation matrix + * and formats it in a form palatable to a human being. + */ + String format(OpenGLMatrix transformationMatrix) { + return transformationMatrix.formatAsTransform(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CraterSample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CraterSample.java new file mode 100644 index 00000000000..60d5eb63074 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CraterSample.java @@ -0,0 +1,208 @@ +package org.firstinspires.ftc.teamcode; + + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; + +import java.util.List; + + +/** DRIVETRAIN CONFIGURATION + * front + * C D + * left right + * B A + * back + */ + + + +@Autonomous(name = "CraterSample", group = "Auto") +public class CraterSample extends LinearOpMode { + + private static final String TFOD_MODEL_ASSET = "RoverRuckus.tflite"; + private static final String LABEL_GOLD_MINERAL = "Gold Mineral"; + private static final String LABEL_SILVER_MINERAL = "Silver Mineral"; + private static final String VUFORIA_KEY = "AZCMreL/////AAABmdWWWC3/vE83hRmhRnWGAul7owT5y1Mn4hOtumsnaMVr67TGWwxY7HgprjL5Ng7ryllGy3WuOPA2BLCwD3s1zxukV6N4Eo5sgzqntZSibOI2BPj2xm7fdQklrFDF+XqkxD9xpKRTDuZk39tRPXREvU0RIrC0gl1AdvmLNh4UN48GZRHtjgTz1cNTHq9HMO5q0bJ1FF4bQHyilcOeK6YoYSaLE+H9cpSarCjIP7H77Z7acyfzE0sjWxfGcONmQoREkhVC3VeOPh3HkRSMMnxxBj+uaDCLQkOA4Ejilr7YGceRz8HY9yy02vJs+oYCfCZOz97vIjF4rOZW171bH7CwMRhqNIWs0SS2oBydH1nVk/yR"; + private VuforiaLocalizer vuforia; + private TFObjectDetector tfod; + + private int goldMineralX = -1; + + private DcMotor aDrive = null; + private DcMotor bDrive = null; + private DcMotor cDrive = null; + private DcMotor dDrive = null; + private DcMotor aMech = null; + private DcMotor bMech = null; + private DcMotor cMech = null; + private DcMotor dMech = null; + + private double forward = 0; + private double turn = 0; + + + private String[] titles = new String[] {"armpower", "armtime", "armdist", "turn0power", "turn0time", "forward0power", "forward0time", "turn1power", "turn1time"} ; + private double[] values = new double[] { 0.3, 2, 1000, 0.3, 3, 0.3, 5, 0.3, 7 }; + private Tuner tuner; + + private ElapsedTime runtime = new ElapsedTime(); + + @Override + public void runOpMode() { + /////////////////////////////////////////////////////////// INIT + initVuforia(); + initTfod(); + + aDrive = hardwareMap.get(DcMotor.class, "1-0"); + aDrive.setDirection(DcMotorSimple.Direction.FORWARD); + bDrive = hardwareMap.get(DcMotor.class, "1-1"); + bDrive.setDirection(DcMotorSimple.Direction.REVERSE); + cDrive = hardwareMap.get(DcMotor.class, "1-2"); + cDrive.setDirection(DcMotorSimple.Direction.FORWARD); + dDrive = hardwareMap.get(DcMotor.class, "1-3"); + dDrive.setDirection(DcMotorSimple.Direction.REVERSE); + + aMech = hardwareMap.get(DcMotor.class, "2-0"); + bMech = hardwareMap.get(DcMotor.class, "2-1"); + cMech = hardwareMap.get(DcMotor.class, "2-2"); + dMech = hardwareMap.get(DcMotor.class, "2-3"); + + tuner = new Tuner(titles, values, gamepad1, telemetry); + + telemetry.update(); + /////////////////////////////////////////////////////////// END INIT + + while (!opModeIsActive()){ + tuner.tune(); + } + + + waitForStart(); + runtime.reset(); + + while (opModeIsActive()){ + + if (tfod != null) { + List updatedRecognitions = tfod.getUpdatedRecognitions(); + if (updatedRecognitions != null) { + //if object detected + for (Recognition recognition : updatedRecognitions) { + if (recognition.getLabel().equals(LABEL_GOLD_MINERAL)) { + goldMineralX = (int) recognition.getLeft(); + } + } + telemetry.addData("goldMineralX",goldMineralX); + telemetry.update(); + //end if object detected + } + } + + ///////////////////////////////////////////////////////////////// ACTIONS + + if (timedAction(0, tuner.get("armtime"))) { //arm + cMech.setPower(tuner.get("armpower")); + } + + if (timedAction(tuner.get("armtime"), tuner.get("turn0time"))) { //turn to release hook + forward = 0; + turn = tuner.get("turn0power"); + cMech.setPower(0); + } + + if (timedAction(tuner.get("turn0time"), tuner.get("forward0time"))) { //forward til near mineral + forward = tuner.get("forward1time"); + turn = 0; + } + + if (timedAction(tuner.get("forward0time"), tuner.get("turn0time"))) { //turn parallel to minerals + forward = 0; + turn = tuner.get("turn0power"); + } + + if (timedAction(6, 12) && goldMineralX == -1) { //forward til see gold + forward = 0.1; + turn = 0; + } + + if (timedAction(12, 13)){ //turn to face gold + forward = 0; + turn = 0.1; + } + + if (timedAction(13, 15)){ //forward to push gold using brush + forward = 0.4; + turn = 0; + bMech.setPower(0.9); + } + + if (timedAction(15, 20)){ //stop + forward = 0; + turn = 0; + bMech.setPower(0); + } + + ///////////////////////////////////////////////////////////////// END ACTIONS + + + aDrive.setPower(forward + turn); + bDrive.setPower(forward - turn); + cDrive.setPower(forward - turn); + dDrive.setPower(forward + turn); + + + telemetry.addData("arm encoder", cMech.getCurrentPosition()); + telemetry.addData("forward",forward); + telemetry.addData("turn",turn); + telemetry.addData("runtime", runtime.seconds()); + telemetry.update(); + } + + + + + } + + + + + private void initVuforia() { + /* + * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine. + */ + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + parameters.vuforiaLicenseKey = VUFORIA_KEY; + parameters.cameraDirection = CameraDirection.BACK; + + // Instantiate the Vuforia engine + vuforia = ClassFactory.getInstance().createVuforia(parameters); + + // Loading trackables is not necessary for the Tensor Flow Object Detection engine. + } + private void initTfod() { + int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId); + tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia); + tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL); + } + + private Boolean timedAction(double start, double end){ + return (runtime.seconds() > start && runtime.seconds() < end); + } + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CraterSample2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CraterSample2.java new file mode 100644 index 00000000000..e9e178b87d5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CraterSample2.java @@ -0,0 +1,138 @@ +package org.firstinspires.ftc.teamcode; + + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; + +import java.util.List; + + +/** DRIVETRAIN CONFIGURATION + * front + * C D + * left right + * B A + * back + */ + + + +@Autonomous(name = "CraterSample2", group = "Auto") +public class CraterSample2 extends LinearOpMode { + + private DcMotor aDrive = null; + private DcMotor bDrive = null; + private DcMotor cDrive = null; + private DcMotor dDrive = null; + private DcMotor aMech = null; + private DcMotor bMech = null; + private DcMotor cMech = null; + private DcMotor dMech = null; + + private double forward = 0; + private double turn = 0; + + + private String[] titles = new String[] {"armpower", "armtime", "armdist", "turn0power", "turn0time", "forward0power", "forward0time", "turn1power", "turn1time"} ; + private double[] values = new double[] { 0.3, 2, 1000, 0.3, 3, 0.3, 5, 0.3, 7 }; + private Tuner tuner; + + private ElapsedTime runtime = new ElapsedTime(); + + @Override + public void runOpMode() { + /////////////////////////////////////////////////////////// INIT + + + aDrive = hardwareMap.get(DcMotor.class, "1-0"); + aDrive.setDirection(DcMotorSimple.Direction.FORWARD); + bDrive = hardwareMap.get(DcMotor.class, "1-1"); + bDrive.setDirection(DcMotorSimple.Direction.REVERSE); + cDrive = hardwareMap.get(DcMotor.class, "1-2"); + cDrive.setDirection(DcMotorSimple.Direction.FORWARD); + dDrive = hardwareMap.get(DcMotor.class, "1-3"); + dDrive.setDirection(DcMotorSimple.Direction.REVERSE); + + aMech = hardwareMap.get(DcMotor.class, "2-0"); + bMech = hardwareMap.get(DcMotor.class, "2-1"); + cMech = hardwareMap.get(DcMotor.class, "2-2"); + dMech = hardwareMap.get(DcMotor.class, "2-3"); + + tuner = new Tuner(titles, values, gamepad1, telemetry); + + telemetry.update(); + /////////////////////////////////////////////////////////// END INIT + + while (!opModeIsActive()){ + tuner.tune(); + } + + telemetry.addData("armpos",cMech.getCurrentPosition()); + telemetry.update(); + + + waitForStart(); + runtime.reset(); + + telemetry.addData("armpos",cMech.getCurrentPosition()); + telemetry.update(); + + int target = cMech.getCurrentPosition() + 21000; + while(cMech.getCurrentPosition() < target){ + cMech.setPower(0.5); + telemetry.addData("armpos",cMech.getCurrentPosition()); + telemetry.update(); + } + cMech.setPower(0); + + runtime.reset(); + while(runtime.seconds()<0.5){ + turn = 0.5; + forward=0; + drive(forward,turn); + } + pause(); + + + + + + + } + + + + + private Boolean timedAction(double start, double end){ + return (runtime.seconds() > start && runtime.seconds() < end); + } + + private void drive(double forward, double turn){ + aDrive.setPower(forward + turn); + bDrive.setPower(forward - turn); + cDrive.setPower(forward - turn); + dDrive.setPower(forward + turn); + } + + private void pause(){ + aDrive.setPower(0); + bDrive.setPower(0); + cDrive.setPower(0); + dDrive.setPower(0); + aMech.setPower(0); + bMech.setPower(0); + cMech.setPower(0); + dMech.setPower(0); + } + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FourwdTeleOp2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FourwdTeleOp2.java new file mode 100644 index 00000000000..e88e550595f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FourwdTeleOp2.java @@ -0,0 +1,127 @@ +package org.firstinspires.ftc.teamcode; + + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.Range; + + +/** DRIVETRAIN CONFIGURATION + * front + * C D + * left right + * B A + * back + */ + + + +@TeleOp(name = "FourwdTeleop2", group = "Joystick Opmode") +public class FourwdTeleOp2 extends OpMode { + + private DcMotor aDrive = null; + private DcMotor bDrive = null; + private DcMotor cDrive = null; + private DcMotor dDrive = null; + private DcMotor aMech = null; + private DcMotor bMech = null; + private DcMotor cMech = null; + private DcMotor dMech = null; + + private double forward = 0; + private double turn = 0; + private double multiplier = 1; + private double ForwardExponent = 2; + private double TurnExponent = 2; + + //FOR TUNER: make sure to match these names when getting values + //These are the default values + private String[] titles = new String[] {"multiplier", "ForwardExponent", "TurnExponent"}; + private double[] values = new double[] { 1, 2, 2}; + private Tuner tuner; + + @Override + public void init() { + telemetry.addData("Status", "Initializing"); + + aDrive = hardwareMap.get(DcMotor.class, "1-0"); + aDrive.setDirection(DcMotorSimple.Direction.FORWARD); + bDrive = hardwareMap.get(DcMotor.class, "1-1"); + bDrive.setDirection(DcMotorSimple.Direction.REVERSE); + cDrive = hardwareMap.get(DcMotor.class, "1-2"); + cDrive.setDirection(DcMotorSimple.Direction.FORWARD); + dDrive = hardwareMap.get(DcMotor.class, "1-3"); + dDrive.setDirection(DcMotorSimple.Direction.REVERSE); + + aMech = hardwareMap.get(DcMotor.class, "2-0"); + bMech = hardwareMap.get(DcMotor.class, "2-1"); + cMech = hardwareMap.get(DcMotor.class, "2-2"); + dMech = hardwareMap.get(DcMotor.class, "2-3"); + + tuner = new Tuner(titles, values, gamepad1, telemetry); + + telemetry.addData("Status", "Initialized"); + } + + @Override + public void loop() { + + tuner.tune(); + multiplier = tuner.get("multiplier"); + ForwardExponent = tuner.get("ForwardExponent"); + TurnExponent = tuner.get("TurnExponent"); + + + if (gamepad1.dpad_up){ + forward = 0.9; + turn = 0; + }else if (gamepad1.dpad_down){ + forward = -0.9; + turn = 0;} + else if (gamepad1.dpad_right){ + forward = 0; + turn = 0.9; + }else if (gamepad1.dpad_left){ + forward = 0; + turn = -0.9; + }else{ + forward = Calculate.sensCurve(-gamepad1.left_stick_y, ForwardExponent); + turn = Calculate.sensCurve(-gamepad1.right_stick_x, TurnExponent); + } + + multiplier = tuner.get("multiplier"); + aDrive.setPower(Range.clip((forward + turn)*multiplier,-1,1)); + bDrive.setPower(Range.clip((forward - turn)*multiplier,-1,1)); + cDrive.setPower(Range.clip((forward - turn)*multiplier,-1,1)); + dDrive.setPower(Range.clip((forward + turn)*multiplier,-1,1)); + + + if(gamepad2.dpad_up){ aMech.setPower(0.9); } + else if(gamepad2.dpad_down){ aMech.setPower(-0.9); } + else{ aMech.setPower(0); } + + if(gamepad2.dpad_right){ bMech.setPower(0.9); } + else if(gamepad2.dpad_left){ bMech.setPower(-0.9); } + else{ bMech.setPower(0); } + + + if(gamepad2.y){ cMech.setPower(0.9); } + else if(gamepad2.a){ cMech.setPower(-0.9); } + else{ cMech.setPower(0); } + + if(gamepad2.b){ dMech.setPower(0.9); } + else if(gamepad2.x){ dMech.setPower(-0.9); } + else{ dMech.setPower(0); } + + telemetry.addData("forward",forward); + telemetry.addData("turn",turn); + telemetry.addData("armPos",cMech.getCurrentPosition()); + telemetry.update(); + + } + + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FourwdTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FourwdTeleop.java new file mode 100644 index 00000000000..70c451cc173 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FourwdTeleop.java @@ -0,0 +1,127 @@ +package org.firstinspires.ftc.teamcode; + + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.Range; + + +/** DRIVETRAIN CONFIGURATION + * front + * C D + * left right + * B A + * back + */ + + + +@TeleOp(name = "FourwdTeleop", group = "Joystick Opmode") +public class FourwdTeleop extends OpMode { + + private DcMotor aDrive = null; + private DcMotor bDrive = null; + private DcMotor cDrive = null; + private DcMotor dDrive = null; + private DcMotor aMech = null; + private DcMotor bMech = null; + private DcMotor cMech = null; + private DcMotor dMech = null; + + private double forward = 0; + private double turn = 0; + + //make sure to match these names when getting values + private String[] titles = new String[] {"forward", "title1", "title2", "title3", "title4", "title5", "title6", "title7", "title8"}; + private double[] values = new double[] { 0.4, 0, 0, 0, 0, 0, 0, 0, 0 }; + +// Tuner tuner = new Tuner(titles, values, gamepad1, telemetry); + + @Override + public void init() { + telemetry.addData("Status", "Initializing"); + + aDrive = hardwareMap.get(DcMotor.class, "1-0"); + aDrive.setDirection(DcMotorSimple.Direction.FORWARD); + bDrive = hardwareMap.get(DcMotor.class, "1-1"); + bDrive.setDirection(DcMotorSimple.Direction.REVERSE); + cDrive = hardwareMap.get(DcMotor.class, "1-2"); + cDrive.setDirection(DcMotorSimple.Direction.FORWARD); + dDrive = hardwareMap.get(DcMotor.class, "1-3"); + dDrive.setDirection(DcMotorSimple.Direction.REVERSE); + + aMech = hardwareMap.get(DcMotor.class, "2-0"); + bMech = hardwareMap.get(DcMotor.class, "2-1"); + cMech = hardwareMap.get(DcMotor.class, "2-2"); + dMech = hardwareMap.get(DcMotor.class, "2-3"); + + telemetry.addData("Status", "Initialized"); + } + + @Override + public void init_loop() { + } + + @Override + public void start() { + } + + @Override + public void loop() { + + // tuner.tune(); + // telemetry.addData("get(forward)", tuner.get("forward")); + + + if (gamepad1.dpad_up){ +// forward = tuner.get("forward"); + forward = 0.9; + turn = 0; + }else if (gamepad1.dpad_down){ + forward = -0.9; + turn = 0;} + else if (gamepad1.dpad_right){ + forward = 0; + turn = 0.9; + }else if (gamepad1.dpad_left){ + forward = 0; + turn = -0.9; + }else{ + forward = Calculate.sensCurve(-gamepad1.left_stick_y, 2); + turn = Calculate.sensCurve(-gamepad1.right_stick_x, 2); + } + + aDrive.setPower(forward+turn); + bDrive.setPower(forward-turn); + cDrive.setPower(forward-turn); + dDrive.setPower(forward+turn); + + + if(gamepad2.dpad_up){ aMech.setPower(0.9); } + else if(gamepad2.dpad_down){ aMech.setPower(-0.9); } + else{ aMech.setPower(0); } + + if(gamepad2.dpad_right){ bMech.setPower(0.9); } + else if(gamepad2.dpad_left){ bMech.setPower(-0.9); } + else{ bMech.setPower(0); } + + + if(gamepad2.y){ cMech.setPower(0.9); } + else if(gamepad2.a){ cMech.setPower(-0.9); } + else{ cMech.setPower(0); } + + if(gamepad2.b){ dMech.setPower(0.9); } + else if(gamepad2.x){ dMech.setPower(-0.9); } + else{ dMech.setPower(0); } + + telemetry.addData("forward",forward); + telemetry.addData("turn",turn); + telemetry.update(); + + } + + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuner.java new file mode 100644 index 00000000000..cef599e076f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuner.java @@ -0,0 +1,79 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.hardware.Gamepad; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class Tuner{ + + + private Gamepad gamepad; + private Telemetry telemetry; + + private int index = 0; + + private String[] titles; + private double[] values; + + private ButtonPress leftBumper = new ButtonPress(); + private ButtonPress rightBumper = new ButtonPress(); + + + Tuner(String[] titles, double[] values, Gamepad gamepad, Telemetry telemetry){ + this.titles = titles; + this.values = values; + this.gamepad = gamepad; + this.telemetry = telemetry; + } + + public void tune(){ //should run continuously + + if(leftBumper.status(gamepad.left_bumper) == ButtonPress.Status.COMPLETE){ + if(index == 0){ + index = titles.length-1; + }else{ + index--; + } + } + + if (rightBumper.status(gamepad.right_bumper) == ButtonPress.Status.COMPLETE){ + if(index > titles.length-2){ + index = 0; + }else{ + index++; + } + } + + if (gamepad.left_trigger > 0.05){ + values[index] -= gamepad.left_trigger*0.01; + } + if (gamepad.right_trigger > 0.05){ + values[index] += gamepad.right_trigger*0.01; + } + + telemetry.addData(titles[index], values[index]); + telemetry.addData("index", index); + + } + + public double get(String title){ + int keyIndex = find(titles, title); + if(keyIndex == -1){ + telemetry.addData("ERROR", "DIDN'T FIND "+title); + return 0; + }else{ + return values[keyIndex]; + + } + } + + + // Function to find the index of an String in a String array + public static int find(String[] array, String target){ + for (int i = 0; i < array.length; i++) + if (array[i] == target) + return i; + return -1; + } + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TunerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TunerTest.java new file mode 100644 index 00000000000..d9cda69eb94 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TunerTest.java @@ -0,0 +1,65 @@ +package org.firstinspires.ftc.teamcode; + + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + + + +@TeleOp(name = "TunerTest", group = "Joystick Opmode") +public class TunerTest extends OpMode { + + private double forward = 0; + private double turn = 0; + private double tuneValue = 0; + + //make sure to match these names when getting values + private String[] titles = new String[] {"tuneValue", "DriveExponent", "TurnExponent"}; + private double[] values = new double[] { 0, 2, 2}; + + private Tuner tuner; + + @Override + public void init() { + + + tuner = new Tuner(titles, values, gamepad1, telemetry); + + } + + @Override + public void loop() { + + tuner.tune(); + + + if (gamepad1.dpad_up){ + forward = 0.9; + turn = 0; + }else if (gamepad1.dpad_down){ + forward = -0.9; + turn = 0;} + else if (gamepad1.dpad_right){ + forward = 0; + turn = 0.9; + }else if (gamepad1.dpad_left){ + forward = 0; + turn = -0.9; + }else{ + forward = Calculate.sensCurve(-gamepad1.left_stick_y, tuner.get("DriveExponent")); + turn = Calculate.sensCurve(-gamepad1.right_stick_x, tuner.get("TurnExponent")); + } + + + + telemetry.addData("forward",forward); + telemetry.addData("turn",turn); + telemetry.update(); + + } + + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/spinMotors.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/spinMotors.java new file mode 100644 index 00000000000..6b03f1d7d82 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/spinMotors.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.teamcode; + + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + +@TeleOp(name = "spinMotors", group = "Joystick Opmode") +public class spinMotors extends OpMode { + + private DcMotor aDrive = null; + private DcMotor bDrive = null; + private DcMotor cDrive = null; + private DcMotor dDrive = null; + + @Override + public void init() { + telemetry.addData("Status", "Initializing"); + aDrive = hardwareMap.get(DcMotor.class, "aDrive"); + bDrive = hardwareMap.get(DcMotor.class, "bDrive"); + cDrive = hardwareMap.get(DcMotor.class, "cDrive"); + dDrive = hardwareMap.get(DcMotor.class, "dDrive"); + telemetry.addData("Status", "Initialized"); + } + + @Override + public void init_loop() { + } + + + @Override + public void start() { + } + + @Override + public void loop() { + + telemetry.addData("cDrive", cDrive.getCurrentPosition()); + + + if (gamepad1.y){ + aDrive.setPower(0.9); + bDrive.setPower(-0.9); + }else if (gamepad1.a){ + aDrive.setPower(-0.9); + bDrive.setPower(0.9); + } + aDrive.setPower(gamepad1.left_stick_y); + bDrive.setPower(-gamepad1.right_stick_y); + + + if (gamepad1.dpad_up) { + cDrive.setPower(0.5); + }else if (gamepad1.dpad_down){ + cDrive.setPower(-0.5); + }else{ + cDrive.setPower(0); + } + + } + +}