From dbadf8386196b87c3a531de387b7c93d74bf2cb6 Mon Sep 17 00:00:00 2001 From: TSTEM TRILOBYTES PROGRAMMING TEAM Date: Tue, 3 Sep 2019 20:15:04 -0500 Subject: [PATCH 01/24] Update README.md --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 65740d8c799..b1b62011e1a 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,11 @@ -## 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. + +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: From cb812dd2386aebcc83ecc0027032efd870924209 Mon Sep 17 00:00:00 2001 From: TSTEM TRILOBYTES PROGRAMMING TEAM Date: Tue, 3 Sep 2019 20:37:52 -0500 Subject: [PATCH 02/24] Update README.md Added message for programming team --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index b1b62011e1a..acb13a25c27 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,7 @@ Welcome! 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 From f1501b3873ce0f56a218f69684df852bd63b1619 Mon Sep 17 00:00:00 2001 From: Ian Fernandes <34047201+fernandesi2244@users.noreply.github.com> Date: Tue, 3 Sep 2019 20:38:38 -0500 Subject: [PATCH 03/24] Update README.md Fixed spacing --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index acb13a25c27..5df3f0c3997 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,7 @@ Welcome! 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 From 22ce18c10a9316e21792c87b95cc4f1f4e85adb5 Mon Sep 17 00:00:00 2001 From: fernandesi2244 Date: Tue, 3 Sep 2019 20:58:53 -0500 Subject: [PATCH 04/24] Add test Operational Mode to TeamCode folder. Added basic OP mode to test pushing to Github. The OP mode can be edited to be run on a basic FTC chassis so that programmers can get used to deploying code. --- .../ftc/teamcode/testOpMode_TeleOp.java | 141 ++++++++++++++++++ build.gradle | 2 +- gradle/wrapper/gradle-wrapper.properties | 3 +- 3 files changed, 144 insertions(+), 2 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java new file mode 100644 index 00000000000..480948dd02a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java @@ -0,0 +1,141 @@ +/* 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.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="test Opmode") +@Disabled +public class testOpMode_TeleOp extends OpMode +{ + // Declare OpMode members. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftDrive = null; + private DcMotor rightDrive = 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). + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // 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 + leftDrive.setDirection(DcMotor.Direction.FORWARD); + rightDrive.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; + double turn = gamepad1.right_stick_x; + 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 + leftDrive.setPower(leftPower); + rightDrive.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/build.gradle b/build.gradle index bb3b818fc2b..50960db6a9d 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 caf54fa2801..94f5073ebda 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 From 7e379404e85394f29e44d2227cbb52031e422db0 Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Wed, 16 Oct 2019 21:21:51 -0500 Subject: [PATCH 05/24] test one --- .../java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java index 480948dd02a..8acd86d8d7a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java @@ -60,6 +60,7 @@ public class testOpMode_TeleOp extends OpMode private ElapsedTime runtime = new ElapsedTime(); private DcMotor leftDrive = null; private DcMotor rightDrive = null; + private DcMotor frontLeftMotor=null; /* * Code to run ONCE when the driver hits INIT @@ -74,6 +75,7 @@ public void init() { leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + // 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 leftDrive.setDirection(DcMotor.Direction.FORWARD); From 2da99049cbaacb92a221f51721b8ba3ce0e3f34e Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Wed, 16 Oct 2019 21:28:07 -0500 Subject: [PATCH 06/24] messing around. --- .../org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java index 8acd86d8d7a..6f3064b522c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java @@ -62,9 +62,7 @@ public class testOpMode_TeleOp extends OpMode private DcMotor rightDrive = null; private DcMotor frontLeftMotor=null; - /* - * Code to run ONCE when the driver hits INIT - */ + @Override public void init() { telemetry.addData("Status", "Initialized"); From 63dc8aa3c57ec26aa91d6ff0db17331599f88a46 Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Sat, 19 Oct 2019 14:35:22 -0500 Subject: [PATCH 07/24] Cleopatra. --- .../ftc/teamcode/Cleopatra12820TeleOp.java | 168 ++++++++++++++++++ .../ftc/teamcode/testOpMode_TeleOp.java | 141 --------------- 2 files changed, 168 insertions(+), 141 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java 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 00000000000..2a5a0369f6c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java @@ -0,0 +1,168 @@ +/* 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.DcMotor; +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 { + + // Declare OpMode members. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor intakeMotorLeft=null; + private DcMotor intakeMotorRight=null; + private DcMotor backMotorLeft=null; + private DcMotor backMotorRight=null; + private DcMotor frontMotorLeft=null; + private DcMotor frontMotorRight=null; + private DcMotor armElbow=null; + private DcMotor armWrist=null; + private Servo intakeServoRight=null; + private Servo intakeServoLeft=null; + private Servo claw=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). + intakeMotorLeft = hardwareMap.get(DcMotor.class, "intakeMotorLeft"); + intakeMotorRight = hardwareMap.get(DcMotor.class, "intakeMotorRight"); + 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"); + intakeServoRight = hardwareMap.get(Servo.class, "intakeServoRight"); + intakeServoLeft = hardwareMap.get(Servo.class, "intakeServoLeft"); + claw = hardwareMap.get(Servo.class, "claw"); + + // 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); + 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); + intakeServoLeft.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 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); + double armWristPower = Range.clip(gamepad2.right_stick_y, -1, 1); + //double srvPower=Range.clip(gamepad2.right_stick_x, 0,1); + armWrist.setPower(armWristPower); + armElbow.setPower(armElbowPower); + + 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) ;} + frontMotorLeft.setPower(numFl -MAX_SPEED +MAX_SPEED); + if (backMotorLeft!= null) { + backMotorLeft.setPower(numBl -MAX_SPEED +MAX_SPEED); + } + frontMotorRight.setPower(numFr -MAX_SPEED +MAX_SPEED); + if (backMotorRight != null) { + backMotorRight.setPower(numBr -MAX_SPEED +MAX_SPEED); + } + intakeMotorLeft.setPower(intakeLeftPower); + intakeMotorRight.setPower(intakeRightPower); + + if (gamepad2.x){ + //intakeServoRight.setPosition(srvPower); + intakeServoRight.setPosition(1); + intakeServoLeft.setPosition(1); + } + if(gamepad2.y){ + intakeServoRight.setPosition(0); + intakeServoLeft.setPosition(0); + + } + if(gamepad2.a){ + + claw.setPosition(1); + } + if(gamepad2.b) { + claw.setPosition(0); + } + + // 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/testOpMode_TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java deleted file mode 100644 index 6f3064b522c..00000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testOpMode_TeleOp.java +++ /dev/null @@ -1,141 +0,0 @@ -/* 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.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="test Opmode") -@Disabled -public class testOpMode_TeleOp extends OpMode -{ - // Declare OpMode members. - private ElapsedTime runtime = new ElapsedTime(); - private DcMotor leftDrive = null; - private DcMotor rightDrive = null; - private DcMotor frontLeftMotor=null; - - - @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). - leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); - rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); - - - // 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 - leftDrive.setDirection(DcMotor.Direction.FORWARD); - rightDrive.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; - double turn = gamepad1.right_stick_x; - 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 - leftDrive.setPower(leftPower); - rightDrive.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() { - } - -} From de417fbd014018bcffa81765414cb131fed6f0ac Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Sun, 20 Oct 2019 13:29:43 -0500 Subject: [PATCH 08/24] Cleopatra hardware definition. --- .../ftc/teamcode/CleopatraHardware.java | 149 ++++++++++++++++++ 1 file changed, 149 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java 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 00000000000..df42615ab23 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java @@ -0,0 +1,149 @@ +/* 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. */ + private DcMotor intakeMotorLeft=null; + private DcMotor intakeMotorRight=null; + private DcMotor backMotorLeft=null; + private DcMotor backMotorRight=null; + private DcMotor frontMotorLeft=null; + private DcMotor frontMotorRight=null; + private DcMotor armElbow=null; + private DcMotor armWrist=null; + private Servo intakeServoRight=null; + private Servo intakeServoLeft=null; + private Servo claw=null; + private 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 ; + + /* 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); + } + } + From 63cfc812e3d90ceb8ebb1941ed179c36a8fcdbaa Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Sun, 20 Oct 2019 14:07:24 -0500 Subject: [PATCH 09/24] Cleopatra Hardware update made it public. --- .../ftc/teamcode/CleopatraHardware.java | 24 +-- .../ftc/teamcode/HCleopatra12820TeleOp.java | 143 ++++++++++++++++++ 2 files changed, 155 insertions(+), 12 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java index df42615ab23..96311657061 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java @@ -53,18 +53,18 @@ public class CleopatraHardware { /* Public OpMode members. */ - private DcMotor intakeMotorLeft=null; - private DcMotor intakeMotorRight=null; - private DcMotor backMotorLeft=null; - private DcMotor backMotorRight=null; - private DcMotor frontMotorLeft=null; - private DcMotor frontMotorRight=null; - private DcMotor armElbow=null; - private DcMotor armWrist=null; - private Servo intakeServoRight=null; - private Servo intakeServoLeft=null; - private Servo claw=null; - private Servo rotator=null; + 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 ; 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 00000000000..97bbe46a654 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java @@ -0,0 +1,143 @@ +/* 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.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 = 0; // Servo mid position + final double CLAW_SPEED = 0.02 ; // sets rate to move servo + + @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); + double armWristPower = Range.clip(gamepad2.right_stick_y, -1, 1); + //double srvPower=Range.clip(gamepad2.right_stick_x, 0,1); + robot.armWrist.setPower(armWristPower); + robot.armElbow.setPower(armElbowPower); + + + 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.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); + } + robot.intakeMotorLeft.setPower(intakeLeftPower); + robot.intakeMotorRight.setPower(intakeRightPower); + + 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); + + } + if(gamepad2.a){ + + robot.claw.setPosition(1); + } + if(gamepad2.b) { + robot.claw.setPosition(0); + } + + + // 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); + } + } +} From 48800b40210d8ed0a83fa9071bd178e57bd02e7c Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Sun, 20 Oct 2019 14:07:24 -0500 Subject: [PATCH 10/24] Cleopatra Hardware update made it public. --- .../ftc/teamcode/CleopatraHardware.java | 24 +-- .../ftc/teamcode/HCleopatra12820TeleOp.java | 143 ++++++++++++++++++ 2 files changed, 155 insertions(+), 12 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java index df42615ab23..96311657061 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java @@ -53,18 +53,18 @@ public class CleopatraHardware { /* Public OpMode members. */ - private DcMotor intakeMotorLeft=null; - private DcMotor intakeMotorRight=null; - private DcMotor backMotorLeft=null; - private DcMotor backMotorRight=null; - private DcMotor frontMotorLeft=null; - private DcMotor frontMotorRight=null; - private DcMotor armElbow=null; - private DcMotor armWrist=null; - private Servo intakeServoRight=null; - private Servo intakeServoLeft=null; - private Servo claw=null; - private Servo rotator=null; + 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 ; 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 00000000000..97bbe46a654 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java @@ -0,0 +1,143 @@ +/* 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.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 = 0; // Servo mid position + final double CLAW_SPEED = 0.02 ; // sets rate to move servo + + @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); + double armWristPower = Range.clip(gamepad2.right_stick_y, -1, 1); + //double srvPower=Range.clip(gamepad2.right_stick_x, 0,1); + robot.armWrist.setPower(armWristPower); + robot.armElbow.setPower(armElbowPower); + + + 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.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); + } + robot.intakeMotorLeft.setPower(intakeLeftPower); + robot.intakeMotorRight.setPower(intakeRightPower); + + 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); + + } + if(gamepad2.a){ + + robot.claw.setPosition(1); + } + if(gamepad2.b) { + robot.claw.setPosition(0); + } + + + // 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); + } + } +} From f9e26ab7c4d7d8511ff1ae4cecf599d41cd42cce Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Sun, 20 Oct 2019 18:04:34 -0500 Subject: [PATCH 11/24] Cleopatra Hardware update made it public. --- .../ftc/teamcode/HCleopatra12820TeleOp.java | 28 +++++++++++++------ 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java index 97bbe46a654..b0707932644 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java @@ -88,9 +88,17 @@ public void runOpMode() { 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(gamepad2.right_stick_y, -1, 1); - //double srvPower=Range.clip(gamepad2.right_stick_x, 0,1); - robot.armWrist.setPower(armWristPower); + //double armWristPower = Range.clip(gamepad2.right_stick_y, -1, 1); + if(gamepad2.right_stick_y>0) + + robot.armWrist.setPower(robot.ARM_UP_POWER); + + else if (gamepad2.right_stick_x>0) + robot.armWrist.setPower(robot.ARM_DOWN_POWER); + else + robot.armWrist.setPower(0.0); + + //Continue here with armElbow! robot.armElbow.setPower(armElbowPower); @@ -121,14 +129,16 @@ public void runOpMode() { robot.intakeServoLeft.setPosition(0); } - if(gamepad2.a){ + //Use gamepad2 left & right Bumpers to open and close the claw + if(gamepad2.right_bumper) + clawOffset += CLAW_SPEED; - robot.claw.setPosition(1); - } - if(gamepad2.b) { + else if(gamepad2.left_bumper) robot.claw.setPosition(0); - } - + clawOffset -= CLAW_SPEED; + // Move both servos to new position. Assume servos are mirror image of each other. + 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); From 1fbcf519c63917dc8086bab994cb684617c2bc45 Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Sun, 20 Oct 2019 19:04:18 -0500 Subject: [PATCH 12/24] Cleopatra Hardware update made it public. --- .../firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java index b0707932644..128cd86d44b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java @@ -88,12 +88,12 @@ public void runOpMode() { 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(gamepad2.right_stick_y, -1, 1); - if(gamepad2.right_stick_y>0) + + if(gamepad2.a) robot.armWrist.setPower(robot.ARM_UP_POWER); - else if (gamepad2.right_stick_x>0) + else if (gamepad2.b) robot.armWrist.setPower(robot.ARM_DOWN_POWER); else robot.armWrist.setPower(0.0); From 7695836f3c06c347c57198a282ab1f5431e754d6 Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Sun, 20 Oct 2019 23:24:02 -0500 Subject: [PATCH 13/24] CleopatraHardware/HCleopatra12820TeleOp update --- .../ftc/teamcode/CleopatraHardware.java | 2 + .../ftc/teamcode/HCleopatra12820TeleOp.java | 68 +++++++++++++++---- 2 files changed, 55 insertions(+), 15 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java index 96311657061..af934bf4cd7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleopatraHardware.java @@ -69,6 +69,8 @@ public class CleopatraHardware 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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java index 128cd86d44b..75173d68e64 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java @@ -32,6 +32,7 @@ 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; @@ -50,8 +51,28 @@ public class HCleopatra12820TeleOp extends LinearOpMode { /* Declare OpMode members. */ CleopatraHardware robot = new CleopatraHardware(); // It uses Cleopatra's hardware - double clawOffset = 0; // Servo mid position - final double CLAW_SPEED = 0.02 ; // sets rate to move servo + 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 final double MAX_POS; // Maximum rotational position + //static final double MIN_POS; // Minimum rotational position + + static { + CYCLE_MS = 50; + //MAX_POS = 1.0; + //MIN_POS = 0.0; + } + + 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() { @@ -87,38 +108,55 @@ public void runOpMode() { 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 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); - //Continue here with armElbow! - robot.armElbow.setPower(armElbowPower); + //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.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); - } robot.intakeMotorLeft.setPower(intakeLeftPower); robot.intakeMotorRight.setPower(intakeRightPower); + //Intake Servos if (gamepad2.x){ //intakeServoRight.setPosition(srvPower); robot.intakeServoRight.setPosition(1); @@ -136,7 +174,7 @@ else if (gamepad2.b) else if(gamepad2.left_bumper) robot.claw.setPosition(0); clawOffset -= CLAW_SPEED; - // Move both servos to new position. Assume servos are mirror image of each other. + clawOffset = Range.clip(clawOffset, -0.5, 0.5); robot.claw.setPosition(robot.MID_SERVO + clawOffset); From e7d1da1b998ce0080777b7d18ce958cab4f8d717 Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Mon, 21 Oct 2019 15:05:15 -0500 Subject: [PATCH 14/24] CleopatraHardware/HCleopatra12820TeleOp update --- .../org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java index 75173d68e64..76f38f14083 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java @@ -46,7 +46,7 @@ */ @TeleOp(name="Hardware Cleopatra Tele_Op", group="Tele_Op") -@Disabled +//Disabled public class HCleopatra12820TeleOp extends LinearOpMode { /* Declare OpMode members. */ From 3d9dee4e32ea86d94b60c894c483a89fa17fb2fe Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Mon, 21 Oct 2019 20:55:12 -0500 Subject: [PATCH 15/24] CleopatraHardware/HCleopatra12820TeleOp update --- .../ftc/teamcode/Cleopatra12820TeleOp.java | 41 +++++++++++-------- 1 file changed, 23 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java index 2a5a0369f6c..a3f21aac86b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java @@ -29,7 +29,6 @@ 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.DcMotor; @@ -48,8 +47,8 @@ public class Cleopatra12820TeleOp extends LinearOpMode { // Declare OpMode members. private ElapsedTime runtime = new ElapsedTime(); - private DcMotor intakeMotorLeft=null; private DcMotor intakeMotorRight=null; + private DcMotor intakeMotorLeft=null; private DcMotor backMotorLeft=null; private DcMotor backMotorRight=null; private DcMotor frontMotorLeft=null; @@ -59,6 +58,8 @@ public class Cleopatra12820TeleOp extends LinearOpMode { private Servo intakeServoRight=null; private Servo intakeServoLeft=null; private Servo claw=null; + private Servo rotator=null; + @Override @@ -69,7 +70,7 @@ public void runOpMode() { // 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). - intakeMotorLeft = hardwareMap.get(DcMotor.class, "intakeMotorLeft"); + intakeMotorLeft = hardwareMap.get(DcMotor.class, "intakeMotorLeft"); intakeMotorRight = hardwareMap.get(DcMotor.class, "intakeMotorRight"); backMotorLeft = hardwareMap.get(DcMotor.class, "backMotorLeft"); backMotorRight = hardwareMap.get(DcMotor.class, "backMotorRight"); @@ -80,6 +81,7 @@ public void runOpMode() { intakeServoRight = hardwareMap.get(Servo.class, "intakeServoRight"); intakeServoLeft = hardwareMap.get(Servo.class, "intakeServoLeft"); claw = hardwareMap.get(Servo.class, "claw"); + rotator=hardwareMap.get(Servo.class, "rotator"); // 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 @@ -91,9 +93,10 @@ public void runOpMode() { backMotorRight.setDirection(DcMotor.Direction.REVERSE); frontMotorLeft.setDirection(DcMotor.Direction.FORWARD); frontMotorRight.setDirection(DcMotor.Direction.REVERSE); - claw.setDirection(Servo.Direction.FORWARD); intakeServoLeft.setDirection(Servo.Direction.REVERSE); - + intakeServoRight.setDirection(Servo.Direction.FORWARD); + claw.setDirection(Servo.Direction.FORWARD); + rotator.setDirection(Servo.Direction.FORWARD); // Wait for the game to start (driver presses PLAY) waitForStart(); runtime.reset(); @@ -127,38 +130,40 @@ public void runOpMode() { 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{ + else { intakeLeftPower = Range.clip(-gamepad2.right_trigger, -1.0, 1.0) ; intakeRightPower = Range.clip(-gamepad2.right_trigger, -1.0, 1.0) ;} frontMotorLeft.setPower(numFl -MAX_SPEED +MAX_SPEED); - if (backMotorLeft!= null) { + if (backMotorLeft!= null) backMotorLeft.setPower(numBl -MAX_SPEED +MAX_SPEED); - } + frontMotorRight.setPower(numFr -MAX_SPEED +MAX_SPEED); - if (backMotorRight != null) { + if (backMotorRight != null) backMotorRight.setPower(numBr -MAX_SPEED +MAX_SPEED); - } + intakeMotorLeft.setPower(intakeLeftPower); intakeMotorRight.setPower(intakeRightPower); - if (gamepad2.x){ + if (gamepad2.x) //intakeServoRight.setPosition(srvPower); intakeServoRight.setPosition(1); intakeServoLeft.setPosition(1); - } - if(gamepad2.y){ + + if(gamepad2.y) intakeServoRight.setPosition(0); intakeServoLeft.setPosition(0); - } - if(gamepad2.a){ + if(gamepad2.a) claw.setPosition(1); - } - if(gamepad2.b) { + + if(gamepad2.b) claw.setPosition(0); - } + if (gamepad2.dpad_up) + rotator.setPosition(1); + if(gamepad2.dpad_down) + rotator.setPosition(0); // Show the elapsed game time and wheel power. telemetry.addData("Status", "Run Time: " + runtime.toString()); telemetry.addData("Motors", "left (%.2f), right (%.2f)", intakeLeftPower, intakeRightPower); From 5d5c396686a8a66f6a1aac982498007781acc780 Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Mon, 21 Oct 2019 21:13:20 -0500 Subject: [PATCH 16/24] CleopatraHardware/HCleopatra12820TeleOp update --- .../ftc/teamcode/Cleopatra12820TeleOp.java | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java index a3f21aac86b..66507dfa30a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java @@ -47,18 +47,18 @@ public class Cleopatra12820TeleOp extends LinearOpMode { // Declare OpMode members. private ElapsedTime runtime = new ElapsedTime(); - private DcMotor intakeMotorRight=null; - private DcMotor intakeMotorLeft=null; - private DcMotor backMotorLeft=null; - private DcMotor backMotorRight=null; - private DcMotor frontMotorLeft=null; - private DcMotor frontMotorRight=null; - private DcMotor armElbow=null; - private DcMotor armWrist=null; - private Servo intakeServoRight=null; - private Servo intakeServoLeft=null; - private Servo claw=null; - private Servo rotator=null; + DcMotor intakeMotorRight=null; + DcMotor intakeMotorLeft=null; + DcMotor backMotorLeft=null; + DcMotor backMotorRight=null; + DcMotor frontMotorLeft=null; + DcMotor frontMotorRight=null; + DcMotor armElbow=null; + DcMotor armWrist=null; + Servo intakeServoRight=null; + Servo intakeServoLeft=null; + Servo claw=null; + Servo rotator=null; From cd95cdf0d4814f828dcc5ecfcfa9cc0b9d802029 Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Tue, 22 Oct 2019 15:39:22 -0500 Subject: [PATCH 17/24] CleopatraHardware/HCleopatra12820TeleOp update --- .../ftc/teamcode/Cleopatra12820TeleOp.java | 65 ++++++++++++++++--- .../ftc/teamcode/HCleopatra12820TeleOp.java | 6 +- 2 files changed, 57 insertions(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java index 66507dfa30a..5668fbd5ce8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java @@ -44,9 +44,18 @@ @TeleOp(name="Basic: Linear OpMode for 1019-2020 Season", group="Linear Opmode") //@Disabled public class Cleopatra12820TeleOp extends LinearOpMode { - + double clawOffset=0; // Servo mid position + public static final double CLAW_SPEED=0.02; // sets rate to move claw servo + 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 ; + 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 ; // Declare OpMode members. private ElapsedTime runtime = new ElapsedTime(); + double position = MID_SERVO;// Start at halfway position DcMotor intakeMotorRight=null; DcMotor intakeMotorLeft=null; DcMotor backMotorLeft=null; @@ -121,11 +130,36 @@ public void runOpMode() { 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 armElbowPower = Range.clip(0.5*gamepad2.left_stick_y, -1, 1); double armWristPower = Range.clip(gamepad2.right_stick_y, -1, 1); - //double srvPower=Range.clip(gamepad2.right_stick_x, 0,1); + double srvPower=Range.clip(gamepad2.right_stick_x, 0,1); armWrist.setPower(armWristPower); - armElbow.setPower(armElbowPower); + armElbow.setPower(armElbowPower);*/ + //Arm wrist + if(gamepad2.a) + armWrist.setPower(ARM_UP_POWER); + + else if (gamepad2.b) + armWrist.setPower(ARM_DOWN_POWER); + + else + armWrist.setPower(0.0); + + //Arm Elbow + if(gamepad2.dpad_up) + armElbow.setPower(ELBOW_UP_POWER); + + else if (gamepad2.dpad_down) + armElbow.setPower(ELBOW_DOWN_POWER); + else + armElbow.setPower(0.0); + + + + intakeServoRight.setPosition(0); + intakeServoLeft.setPosition(0); + claw.setPosition(0); + rotator.setPosition(0); if(gamepad2.left_trigger>0 ){ intakeLeftPower = Range.clip(gamepad2.left_trigger, -1.0, 1.0) ; @@ -144,6 +178,7 @@ public void runOpMode() { intakeMotorLeft.setPower(intakeLeftPower); intakeMotorRight.setPower(intakeRightPower); + //Intake Servos if (gamepad2.x) //intakeServoRight.setPosition(srvPower); intakeServoRight.setPosition(1); @@ -154,16 +189,26 @@ public void runOpMode() { intakeServoLeft.setPosition(0); - if(gamepad2.a) - claw.setPosition(1); + //Use gamepad2 left & right Bumpers to open and close the claw + if(gamepad2.right_bumper) + clawOffset += CLAW_SPEED; - if(gamepad2.b) + else if(gamepad2.left_bumper) claw.setPosition(0); + clawOffset -= CLAW_SPEED; + + clawOffset = Range.clip(clawOffset, -0.5, 0.5); + claw.setPosition(MID_SERVO + clawOffset); - if (gamepad2.dpad_up) - rotator.setPosition(1); + //Rotator. + if(gamepad2.dpad_up) + position += INCREMENT ; if(gamepad2.dpad_down) - rotator.setPosition(0); + position-=INCREMENT; + rotator.setPosition(position); + 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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java index 76f38f14083..a18a62d3d1b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java @@ -55,13 +55,11 @@ public class HCleopatra12820TeleOp extends LinearOpMode { 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 final double MAX_POS; // Maximum rotational position - //static final double MIN_POS; // Minimum rotational position + static { CYCLE_MS = 50; - //MAX_POS = 1.0; - //MIN_POS = 0.0; + } public HCleopatra12820TeleOp() { From a47c832779c9f2db7a6ce77b1c2bcd3a15e23b8d Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Tue, 22 Oct 2019 21:24:20 -0500 Subject: [PATCH 18/24] added methods to setup intake servos. --- .../ftc/teamcode/Cleopatra12820TeleOp.java | 106 +++++++++--------- .../ftc/teamcode/HCleopatra12820TeleOp.java | 4 +- 2 files changed, 53 insertions(+), 57 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java index 5668fbd5ce8..8b3a1acbc3f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java @@ -46,13 +46,13 @@ public class Cleopatra12820TeleOp extends LinearOpMode { double clawOffset=0; // Servo mid position public static final double CLAW_SPEED=0.02; // sets rate to move claw servo - public static final double INCREMENT=0.01; // amount to slew servo each CYCLE_MS cycle + // 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 ; - public static final double ARM_UP_POWER = 0.45 ; + /* 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 ; + public static final double ELBOW_DOWN_POWER = -0.45 ;*/ // Declare OpMode members. private ElapsedTime runtime = new ElapsedTime(); double position = MID_SERVO;// Start at halfway position @@ -70,15 +70,27 @@ public class Cleopatra12820TeleOp extends LinearOpMode { Servo rotator=null; + public void setIntakeServoLeft(Servo intakeServoLeft) { + intakeServoLeft.setPosition(MID_SERVO); + } + public void setIntakeServoRightt(Servo intakeServoRight) { + intakeServoRight.setPosition(MID_SERVO); + } @Override - public void runOpMode() { + public void runOpMode(){ + setIntakeServoLeft(intakeServoLeft); + setIntakeServoRightt(intakeServoRight); 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). + + + /* 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).*/ + + intakeMotorLeft = hardwareMap.get(DcMotor.class, "intakeMotorLeft"); intakeMotorRight = hardwareMap.get(DcMotor.class, "intakeMotorRight"); backMotorLeft = hardwareMap.get(DcMotor.class, "backMotorLeft"); @@ -106,6 +118,8 @@ public void runOpMode() { intakeServoRight.setDirection(Servo.Direction.FORWARD); claw.setDirection(Servo.Direction.FORWARD); rotator.setDirection(Servo.Direction.FORWARD); + intakeServoRight.setPosition(0); + intakeServoLeft.setPosition(0); // Wait for the game to start (driver presses PLAY) waitForStart(); runtime.reset(); @@ -119,8 +133,10 @@ public void runOpMode() { double Speed = -gamepad1.left_stick_y; double Turn = gamepad1.left_stick_x; double Strafe = gamepad1.right_stick_x; - double MAX_SPEED = 1.0; + //double MAX_SPEED = 1.0; + telemetry.addData("Right Servo position", intakeServoRight.getPosition()); + telemetry.update(); /* Front Left= +Speed + Turn - Strafe Front Right= +Speed - Turn + Strafe @@ -130,88 +146,68 @@ public void runOpMode() { 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 armElbowPower = Range.clip(0.5*gamepad2.left_stick_y, -1, 1); double armWristPower = Range.clip(gamepad2.right_stick_y, -1, 1); - double srvPower=Range.clip(gamepad2.right_stick_x, 0,1); - armWrist.setPower(armWristPower); - armElbow.setPower(armElbowPower);*/ - //Arm wrist - if(gamepad2.a) - armWrist.setPower(ARM_UP_POWER); - - else if (gamepad2.b) - armWrist.setPower(ARM_DOWN_POWER); - - else - armWrist.setPower(0.0); - - //Arm Elbow - if(gamepad2.dpad_up) - armElbow.setPower(ELBOW_UP_POWER); - else if (gamepad2.dpad_down) - armElbow.setPower(ELBOW_DOWN_POWER); - else - armElbow.setPower(0.0); + armWrist.setPower(armWristPower); + armElbow.setPower(armElbowPower); - intakeServoRight.setPosition(0); - intakeServoLeft.setPosition(0); - claw.setPosition(0); - rotator.setPosition(0); + /*Sets the servo positions to their "home" position when the Teleop stage begins + intakeServoRight.setPosition(0.5); + intakeServoLeft.setPosition(0.5); + claw.setPosition(0.5); + rotator.setPosition(0.5);*/ if(gamepad2.left_trigger>0 ){ - intakeLeftPower = Range.clip(gamepad2.left_trigger, -1.0, 1.0) ; + intakeLeftPower = Range.clip(gamepad2.left_trigger, -1.0, 1.0) ; //swaps between the two programed "extremes" on the servo 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) ;} - frontMotorLeft.setPower(numFl -MAX_SPEED +MAX_SPEED); + frontMotorLeft.setPower(numFl); if (backMotorLeft!= null) - backMotorLeft.setPower(numBl -MAX_SPEED +MAX_SPEED); + backMotorLeft.setPower(numBl); - frontMotorRight.setPower(numFr -MAX_SPEED +MAX_SPEED); + frontMotorRight.setPower(numFr); if (backMotorRight != null) - backMotorRight.setPower(numBr -MAX_SPEED +MAX_SPEED); + backMotorRight.setPower(numBr); intakeMotorLeft.setPower(intakeLeftPower); intakeMotorRight.setPower(intakeRightPower); //Intake Servos - if (gamepad2.x) + if (gamepad2.x){ //intakeServoRight.setPosition(srvPower); intakeServoRight.setPosition(1); - intakeServoLeft.setPosition(1); + intakeServoLeft.setPosition(1);} - if(gamepad2.y) + //intakeServoRight.getPosition() + if(gamepad2.y){ intakeServoRight.setPosition(0); - intakeServoLeft.setPosition(0); + 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) - claw.setPosition(0); - clawOffset -= CLAW_SPEED; - + if(gamepad2.right_bumper){ + clawOffset += CLAW_SPEED;} + else if(gamepad2.left_bumper){ + clawOffset -= CLAW_SPEED;} clawOffset = Range.clip(clawOffset, -0.5, 0.5); claw.setPosition(MID_SERVO + clawOffset); //Rotator. - if(gamepad2.dpad_up) - position += INCREMENT ; - if(gamepad2.dpad_down) - position-=INCREMENT; - rotator.setPosition(position); + if(gamepad1.dpad_left){ + rotator.setPosition(1); } + if(gamepad2.dpad_right) { + rotator.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.addData("Motors", "left (%.2f), right (%.2f)", intakeLeftPower, intakeRightPower); telemetry.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java index a18a62d3d1b..7cefb003b61 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HCleopatra12820TeleOp.java @@ -46,7 +46,7 @@ */ @TeleOp(name="Hardware Cleopatra Tele_Op", group="Tele_Op") -//Disabled +//@Disabled public class HCleopatra12820TeleOp extends LinearOpMode { /* Declare OpMode members. */ @@ -68,7 +68,7 @@ public HCleopatra12820TeleOp() { clawOffset = 0; } // Define class members - //Servo servo; + Servo servo; double position = robot.MID_SERVO;// Start at halfway position //boolean rampUp = true; From c9dc7bbfe58f3795090f9100baa4cbfda7629801 Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Wed, 23 Oct 2019 13:17:50 -0500 Subject: [PATCH 19/24] Fixed methods to setup intake servos. --- .../ftc/teamcode/Cleopatra12820TeleOp.java | 146 +++++++++--------- 1 file changed, 77 insertions(+), 69 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java index 8b3a1acbc3f..87853c3f4d9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java @@ -44,43 +44,45 @@ @TeleOp(name="Basic: Linear OpMode for 1019-2020 Season", group="Linear Opmode") //@Disabled public class Cleopatra12820TeleOp extends LinearOpMode { - double clawOffset=0; // Servo mid position - public static final double CLAW_SPEED=0.02; // sets rate to move claw servo - // 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 ; - /* 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 ;*/ + double clawOffset = 0; // Servo mid position + double servoOffset = 0; + public static final double CLAW_SPEED = 0.02; // sets rate to move claw servo + public static final double SERV_SPEED = 0.02; // sets rate to move claw servo + // 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; + /* 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 ;*/ // Declare OpMode members. private ElapsedTime runtime = new ElapsedTime(); - double position = MID_SERVO;// Start at halfway position - DcMotor intakeMotorRight=null; - DcMotor intakeMotorLeft=null; - DcMotor backMotorLeft=null; - DcMotor backMotorRight=null; - DcMotor frontMotorLeft=null; - DcMotor frontMotorRight=null; - DcMotor armElbow=null; - DcMotor armWrist=null; - Servo intakeServoRight=null; - Servo intakeServoLeft=null; - Servo claw=null; - Servo rotator=null; + double position = MID_SERVO;// Start at halfway position + DcMotor intakeMotorRight = null; + DcMotor intakeMotorLeft = null; + DcMotor backMotorLeft = null; + DcMotor backMotorRight = null; + DcMotor frontMotorLeft = null; + DcMotor frontMotorRight = null; + DcMotor armElbow = null; + DcMotor armWrist = null; + Servo intakeServoRight = null; + Servo intakeServoLeft = null; + Servo claw = null; + Servo rotator = null; public void setIntakeServoLeft(Servo intakeServoLeft) { intakeServoLeft.setPosition(MID_SERVO); } + public void setIntakeServoRightt(Servo intakeServoRight) { intakeServoRight.setPosition(MID_SERVO); } @Override - public void runOpMode(){ - setIntakeServoLeft(intakeServoLeft); - setIntakeServoRightt(intakeServoRight); + public void runOpMode() { + telemetry.addData("Status", "Initialized"); telemetry.update(); @@ -92,17 +94,17 @@ public void runOpMode(){ intakeMotorLeft = hardwareMap.get(DcMotor.class, "intakeMotorLeft"); - intakeMotorRight = hardwareMap.get(DcMotor.class, "intakeMotorRight"); + intakeMotorRight = hardwareMap.get(DcMotor.class, "intakeMotorRight"); 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"); + armElbow = hardwareMap.get(DcMotor.class, "armElbow"); armWrist = hardwareMap.get(DcMotor.class, "armWrist"); intakeServoRight = hardwareMap.get(Servo.class, "intakeServoRight"); intakeServoLeft = hardwareMap.get(Servo.class, "intakeServoLeft"); claw = hardwareMap.get(Servo.class, "claw"); - rotator=hardwareMap.get(Servo.class, "rotator"); + rotator = hardwareMap.get(Servo.class, "rotator"); // 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 @@ -123,7 +125,8 @@ public void runOpMode(){ // Wait for the game to start (driver presses PLAY) waitForStart(); runtime.reset(); - + setIntakeServoLeft(intakeServoLeft); + setIntakeServoRightt(intakeServoRight); // run until the end of the match (driver presses STOP) while (opModeIsActive()) { @@ -146,7 +149,7 @@ public void runOpMode(){ 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 armElbowPower = Range.clip(0.5 * gamepad2.left_stick_y, -1, 1); double armWristPower = Range.clip(gamepad2.right_stick_y, -1, 1); armWrist.setPower(armWristPower); @@ -160,14 +163,15 @@ public void runOpMode(){ claw.setPosition(0.5); rotator.setPosition(0.5);*/ - if(gamepad2.left_trigger>0 ){ - intakeLeftPower = Range.clip(gamepad2.left_trigger, -1.0, 1.0) ; //swaps between the two programed "extremes" on the servo - 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) ;} + if (gamepad2.left_trigger > 0) { + intakeLeftPower = Range.clip(gamepad2.left_trigger, -1.0, 1.0); //swaps between the two programed "extremes" on the servo + 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); + } frontMotorLeft.setPower(numFl); - if (backMotorLeft!= null) + if (backMotorLeft != null) backMotorLeft.setPower(numBl); frontMotorRight.setPower(numFr); @@ -178,37 +182,41 @@ public void runOpMode(){ intakeMotorRight.setPower(intakeRightPower); //Intake Servos - if (gamepad2.x){ - //intakeServoRight.setPosition(srvPower); - intakeServoRight.setPosition(1); - intakeServoLeft.setPosition(1);} - - //intakeServoRight.getPosition() - if(gamepad2.y){ - intakeServoRight.setPosition(0); - 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){ - clawOffset -= CLAW_SPEED;} - clawOffset = Range.clip(clawOffset, -0.5, 0.5); - claw.setPosition(MID_SERVO + clawOffset); - - //Rotator. - if(gamepad1.dpad_left){ - rotator.setPosition(1); } - if(gamepad2.dpad_right) { - rotator.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(); + if (gamepad2.x) { + servoOffset += SERV_SPEED; + servoOffset = Range.clip(servoOffset, -0.5, 0.5); + intakeServoRight.setPosition(MID_SERVO + servoOffset);} + intakeServoLeft.setPosition(MID_SERVO + servoOffset); + if (gamepad2.y) { + servoOffset -= SERV_SPEED; + servoOffset = Range.clip(servoOffset, -0.5, 0.5); + intakeServoRight.setPosition(MID_SERVO + servoOffset); + intakeServoLeft.setPosition(MID_SERVO + servoOffset); + } + + //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); + } + + + //Rotator. + if (gamepad2.a) { + rotator.setPosition(1); + } + if (gamepad2.b) { + rotator.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(); + } } } -} + From c89db0b90c5cabb3775fbb1ccac2ae4f5e71c48f Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Wed, 23 Oct 2019 17:12:01 -0500 Subject: [PATCH 20/24] Changed direction front right wheel. --- .../org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java index 87853c3f4d9..882608881c6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java @@ -32,6 +32,7 @@ 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; @@ -115,7 +116,7 @@ public void runOpMode() { backMotorLeft.setDirection(DcMotor.Direction.FORWARD); backMotorRight.setDirection(DcMotor.Direction.REVERSE); frontMotorLeft.setDirection(DcMotor.Direction.FORWARD); - frontMotorRight.setDirection(DcMotor.Direction.REVERSE); + frontMotorRight.setDirection(DcMotor.Direction.FORWARD); //THIS IS THE ONE I CHANGED intakeServoLeft.setDirection(Servo.Direction.REVERSE); intakeServoRight.setDirection(Servo.Direction.FORWARD); claw.setDirection(Servo.Direction.FORWARD); From 458c250b27c24d2d5b6c92ec53b2a0f01b1545df Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Tue, 29 Oct 2019 21:02:51 -0500 Subject: [PATCH 21/24] Starting SkyStone Navigation webcam. --- .../ftc/teamcode/Cleopatra12820TeleOp.java | 28 +- .../VuforiaSkyStoneNavigationWebcam12820.java | 371 ++++++++++++++++++ 2 files changed, 385 insertions(+), 14 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java index 882608881c6..de8c8c62e14 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java @@ -67,8 +67,8 @@ public class Cleopatra12820TeleOp extends LinearOpMode { DcMotor frontMotorRight = null; DcMotor armElbow = null; DcMotor armWrist = null; - Servo intakeServoRight = null; - Servo intakeServoLeft = null; + //Servo intakeServoRight = null; + //Servo intakeServoLeft = null; Servo claw = null; Servo rotator = null; @@ -102,8 +102,8 @@ public void runOpMode() { frontMotorRight = hardwareMap.get(DcMotor.class, "frontMotorRight"); armElbow = hardwareMap.get(DcMotor.class, "armElbow"); armWrist = hardwareMap.get(DcMotor.class, "armWrist"); - intakeServoRight = hardwareMap.get(Servo.class, "intakeServoRight"); - intakeServoLeft = hardwareMap.get(Servo.class, "intakeServoLeft"); + //intakeServoRight = hardwareMap.get(Servo.class, "intakeServoRight"); + //intakeServoLeft = hardwareMap.get(Servo.class, "intakeServoLeft"); claw = hardwareMap.get(Servo.class, "claw"); rotator = hardwareMap.get(Servo.class, "rotator"); @@ -117,17 +117,17 @@ public void runOpMode() { backMotorRight.setDirection(DcMotor.Direction.REVERSE); frontMotorLeft.setDirection(DcMotor.Direction.FORWARD); frontMotorRight.setDirection(DcMotor.Direction.FORWARD); //THIS IS THE ONE I CHANGED - intakeServoLeft.setDirection(Servo.Direction.REVERSE); - intakeServoRight.setDirection(Servo.Direction.FORWARD); + //intakeServoLeft.setDirection(Servo.Direction.REVERSE); + //intakeServoRight.setDirection(Servo.Direction.FORWARD); claw.setDirection(Servo.Direction.FORWARD); rotator.setDirection(Servo.Direction.FORWARD); - intakeServoRight.setPosition(0); - intakeServoLeft.setPosition(0); + //intakeServoRight.setPosition(0); + //intakeServoLeft.setPosition(0); // Wait for the game to start (driver presses PLAY) waitForStart(); runtime.reset(); - setIntakeServoLeft(intakeServoLeft); - setIntakeServoRightt(intakeServoRight); + //setIntakeServoLeft(intakeServoLeft); + //setIntakeServoRightt(intakeServoRight); // run until the end of the match (driver presses STOP) while (opModeIsActive()) { @@ -139,8 +139,8 @@ public void runOpMode() { double Strafe = gamepad1.right_stick_x; //double MAX_SPEED = 1.0; - telemetry.addData("Right Servo position", intakeServoRight.getPosition()); - telemetry.update(); + //telemetry.addData("Right Servo position", intakeServoRight.getPosition()); + //telemetry.update(); /* Front Left= +Speed + Turn - Strafe Front Right= +Speed - Turn + Strafe @@ -182,7 +182,7 @@ public void runOpMode() { intakeMotorLeft.setPower(intakeLeftPower); intakeMotorRight.setPower(intakeRightPower); - //Intake Servos + /*Intake Servos if (gamepad2.x) { servoOffset += SERV_SPEED; servoOffset = Range.clip(servoOffset, -0.5, 0.5); @@ -193,7 +193,7 @@ public void runOpMode() { servoOffset = Range.clip(servoOffset, -0.5, 0.5); intakeServoRight.setPosition(MID_SERVO + servoOffset); intakeServoLeft.setPosition(MID_SERVO + servoOffset); - } + }*/ //Use gamepad2 left & right Bumpers to open and close the claw if (gamepad2.right_bumper) { 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 00000000000..22342696e7d --- /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="SKYSTONE Vuforia Nav Webcam", 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 = + " --- YOUR NEW VUFORIA KEY GOES HERE --- "; + + // 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 1"); + + /* + * 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(); + } +} From ba670a628f1a07383fc698a68d8c71e219decbee Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Tue, 29 Oct 2019 21:49:47 -0500 Subject: [PATCH 22/24] Starting SkyStone Navigation no webcam. --- .../ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java index 22342696e7d..4249cc28180 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java @@ -103,7 +103,7 @@ public class VuforiaSkyStoneNavigationWebcam12820 extends LinearOpMode { * and paste it in to your code on the next line, between the double quotes. */ private static final String VUFORIA_KEY = - " --- YOUR NEW VUFORIA KEY GOES HERE --- "; + " 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 From f133e6c5d264a56f65eb5f9ae2b5d7381b277095 Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 Date: Thu, 7 Nov 2019 18:02:18 -0600 Subject: [PATCH 23/24] SkyStone Navigation with webcam. --- .../external/samples/ConceptVuforiaNavigationWebcam.java | 2 +- .../firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java | 8 ++++---- .../teamcode/VuforiaSkyStoneNavigationWebcam12820.java | 8 ++++---- 3 files changed, 9 insertions(+), 9 deletions(-) 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 1b619f3dea5..d70c8c05b18 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/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java index de8c8c62e14..0662b4ee5ed 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java @@ -52,10 +52,10 @@ 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; - /* public static final double ARM_UP_POWER = 0.45 ; + 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 ;*/ + public static final double ELBOW_DOWN_POWER = -0.45 ; // Declare OpMode members. private ElapsedTime runtime = new ElapsedTime(); double position = MID_SERVO;// Start at halfway position @@ -150,8 +150,8 @@ public void runOpMode() { 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(gamepad2.right_stick_y, -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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java index 4249cc28180..d216834e313 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java @@ -82,8 +82,8 @@ * is explained below. */ -@TeleOp(name="SKYSTONE Vuforia Nav Webcam", group ="Concept") -@Disabled +@TeleOp(name="Vuforia Nav", 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; @@ -103,7 +103,7 @@ public class VuforiaSkyStoneNavigationWebcam12820 extends LinearOpMode { * 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 "; + " Abhg0uD/////AAABmY26d2nlY0Hmptt0cQ/Ros4OL3zWSe0yB6zr8WuTqEvE/K9IgpgHArL+IxI5hb23LlUKkh5sx6XPbxX+Z6yg/jAIlHKYPrLqVgM8efHoio7dx5E8EfTyNmNaa83pY4beZ4MySDc3lB7CXjoScgP1+pVEujlo1nGp3nx61E3oVegAymgyZdfAtIjJgs8/OmeXKDXxOaYZcSN/ww99IkGFwSIA02mX8pdIUXr0DtCVKLsqq5v4sNoRsc4udSDr+Y+3eyDuMGCQDlHoiN5yPHyPiIBG1Urko8/Pnq1fl9VmBVg6l/pRJ2H6KfyNmFnt2ceoFIAFopWuV8Tc8VaSKC5n95PjYda6pOpbH8jQjMr/sEns"; // 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 @@ -143,7 +143,7 @@ public class VuforiaSkyStoneNavigationWebcam12820 extends LinearOpMode { /* * Retrieve the camera we are to use. */ - webcamName = hardwareMap.get(WebcamName.class, "Webcam 1"); + webcamName = hardwareMap.get(WebcamName.class, "Webcam"); /* * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine. From fe26a621ed9568520dfa668eae5f15dc81e5b807 Mon Sep 17 00:00:00 2001 From: sgutierrez8c54 <39575591+sgutierrez8c54@users.noreply.github.com> Date: Sun, 24 May 2020 20:37:16 -0500 Subject: [PATCH 24/24] After retrieval. --- .../ConceptVuMarkIdentificationWebcam.java | 4 +- .../ftc/teamcode/Cleopatra12820TeleOp.java | 106 ++--- .../VuforiaSkyStoneNavigation12820.java | 362 ++++++++++++++++++ .../VuforiaSkyStoneNavigationWebcam12820.java | 6 +- 4 files changed, 397 insertions(+), 81 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigation12820.java 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 866261f223c..bff93d0e429 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/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java index 0662b4ee5ed..212af5ebc09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cleopatra12820TeleOp.java @@ -45,41 +45,26 @@ @TeleOp(name="Basic: Linear OpMode for 1019-2020 Season", group="Linear Opmode") //@Disabled public class Cleopatra12820TeleOp extends LinearOpMode { - double clawOffset = 0; // Servo mid position - double servoOffset = 0; - public static final double CLAW_SPEED = 0.02; // sets rate to move claw servo - public static final double SERV_SPEED = 0.02; // sets rate to move claw servo + + // 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; - 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 ; + // Declare OpMode members. private ElapsedTime runtime = new ElapsedTime(); double position = MID_SERVO;// Start at halfway position - DcMotor intakeMotorRight = null; - DcMotor intakeMotorLeft = null; + DcMotor backMotorLeft = null; DcMotor backMotorRight = null; DcMotor frontMotorLeft = null; DcMotor frontMotorRight = null; DcMotor armElbow = null; DcMotor armWrist = null; - //Servo intakeServoRight = null; - //Servo intakeServoLeft = null; Servo claw = null; - Servo rotator = null; - - - public void setIntakeServoLeft(Servo intakeServoLeft) { - intakeServoLeft.setPosition(MID_SERVO); - } + Servo gripperLeft=null; + Servo gripperRight=null; - public void setIntakeServoRightt(Servo intakeServoRight) { - intakeServoRight.setPosition(MID_SERVO); - } @Override public void runOpMode() { @@ -94,50 +79,49 @@ public void runOpMode() { step (using the FTC Robot Controller app on the phone).*/ - intakeMotorLeft = hardwareMap.get(DcMotor.class, "intakeMotorLeft"); - intakeMotorRight = hardwareMap.get(DcMotor.class, "intakeMotorRight"); + 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"); - //intakeServoRight = hardwareMap.get(Servo.class, "intakeServoRight"); - //intakeServoLeft = hardwareMap.get(Servo.class, "intakeServoLeft"); + claw = hardwareMap.get(Servo.class, "claw"); - rotator = hardwareMap.get(Servo.class, "rotator"); + 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); - 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); + frontMotorLeft.setDirection(DcMotor.Direction.REVERSE); //I CHANGED THIS ONE ALSO frontMotorRight.setDirection(DcMotor.Direction.FORWARD); //THIS IS THE ONE I CHANGED - //intakeServoLeft.setDirection(Servo.Direction.REVERSE); - //intakeServoRight.setDirection(Servo.Direction.FORWARD); + claw.setDirection(Servo.Direction.FORWARD); - rotator.setDirection(Servo.Direction.FORWARD); - //intakeServoRight.setPosition(0); - //intakeServoLeft.setPosition(0); + gripperRight.setDirection(Servo.Direction.FORWARD); + gripperLeft.setDirection(Servo.Direction.REVERSE); + + // Wait for the game to start (driver presses PLAY) waitForStart(); runtime.reset(); - //setIntakeServoLeft(intakeServoLeft); - //setIntakeServoRightt(intakeServoRight); + // 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 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; + //telemetry.addData("Right Servo position", intakeServoRight.getPosition()); //telemetry.update(); @@ -155,22 +139,6 @@ public void runOpMode() { armWrist.setPower(armWristPower); armElbow.setPower(armElbowPower); - - - - /*Sets the servo positions to their "home" position when the Teleop stage begins - intakeServoRight.setPosition(0.5); - intakeServoLeft.setPosition(0.5); - claw.setPosition(0.5); - rotator.setPosition(0.5);*/ - - if (gamepad2.left_trigger > 0) { - intakeLeftPower = Range.clip(gamepad2.left_trigger, -1.0, 1.0); //swaps between the two programed "extremes" on the servo - 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); - } frontMotorLeft.setPower(numFl); if (backMotorLeft != null) backMotorLeft.setPower(numBl); @@ -179,21 +147,7 @@ public void runOpMode() { if (backMotorRight != null) backMotorRight.setPower(numBr); - intakeMotorLeft.setPower(intakeLeftPower); - intakeMotorRight.setPower(intakeRightPower); - - /*Intake Servos - if (gamepad2.x) { - servoOffset += SERV_SPEED; - servoOffset = Range.clip(servoOffset, -0.5, 0.5); - intakeServoRight.setPosition(MID_SERVO + servoOffset);} - intakeServoLeft.setPosition(MID_SERVO + servoOffset); - if (gamepad2.y) { - servoOffset -= SERV_SPEED; - servoOffset = Range.clip(servoOffset, -0.5, 0.5); - intakeServoRight.setPosition(MID_SERVO + servoOffset); - intakeServoLeft.setPosition(MID_SERVO + servoOffset); - }*/ + //Use gamepad2 left & right Bumpers to open and close the claw if (gamepad2.right_bumper) { @@ -202,13 +156,13 @@ public void runOpMode() { claw.setPosition(1); } + if(gamepad2.x){ + gripperRight.setPosition(-1); + gripperLeft.setPosition(-1); + }else if(gamepad2.y){ + gripperRight.setPosition(1); + gripperLeft.setPosition(1); - //Rotator. - if (gamepad2.a) { - rotator.setPosition(1); - } - if (gamepad2.b) { - rotator.setPosition(-1); } sleep(CYCLE_MS); idle(); 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 00000000000..61cab9c0dda --- /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 index d216834e313..87ba9f80840 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaSkyStoneNavigationWebcam12820.java @@ -82,7 +82,7 @@ * is explained below. */ -@TeleOp(name="Vuforia Nav", group ="Concept") +@TeleOp(name="Vuforia Navtri1", group ="Concept") // @Disabled public class VuforiaSkyStoneNavigationWebcam12820 extends LinearOpMode { @@ -103,7 +103,7 @@ public class VuforiaSkyStoneNavigationWebcam12820 extends LinearOpMode { * and paste it in to your code on the next line, between the double quotes. */ private static final String VUFORIA_KEY = - " Abhg0uD/////AAABmY26d2nlY0Hmptt0cQ/Ros4OL3zWSe0yB6zr8WuTqEvE/K9IgpgHArL+IxI5hb23LlUKkh5sx6XPbxX+Z6yg/jAIlHKYPrLqVgM8efHoio7dx5E8EfTyNmNaa83pY4beZ4MySDc3lB7CXjoScgP1+pVEujlo1nGp3nx61E3oVegAymgyZdfAtIjJgs8/OmeXKDXxOaYZcSN/ww99IkGFwSIA02mX8pdIUXr0DtCVKLsqq5v4sNoRsc4udSDr+Y+3eyDuMGCQDlHoiN5yPHyPiIBG1Urko8/Pnq1fl9VmBVg6l/pRJ2H6KfyNmFnt2ceoFIAFopWuV8Tc8VaSKC5n95PjYda6pOpbH8jQjMr/sEns"; + " 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 @@ -197,7 +197,7 @@ public class VuforiaSkyStoneNavigationWebcam12820 extends LinearOpMode { rear2.setName("Rear Perimeter 2"); // For convenience, gather together all the trackable objects in one easily-iterable collection */ - List allTrackables = new ArrayList(); + List allTrackables = new ArrayList<>(); allTrackables.addAll(targetsSkyStone); /**