diff --git a/.gitignore b/.gitignore index b85aa2fac629..0b1b7348c5d0 100644 --- a/.gitignore +++ b/.gitignore @@ -26,6 +26,7 @@ build/ # Local configuration file (sdk path, etc) local.properties +my.properties # Proguard folder generated by Eclipse proguard/ @@ -46,6 +47,7 @@ captures/ # For Mac users .DS_Store + # Keystore files # Uncomment the following lines if you do not want to check your keystore files in. #*.jks @@ -78,4 +80,5 @@ lint/intermediates/ lint/generated/ lint/outputs/ lint/tmp/ -# lint/reports/ \ No newline at end of file +# lint/reports/ +/roadrunner_settings.xml diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 000000000000..26d33521af10 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/.kotlin/.gitignore b/.kotlin/.gitignore new file mode 100644 index 000000000000..38bfd77abbd2 --- /dev/null +++ b/.kotlin/.gitignore @@ -0,0 +1 @@ +sessions/ \ No newline at end of file diff --git a/.run/Team417.run.xml b/.run/Team417.run.xml new file mode 100644 index 000000000000..5d22ed586c6d --- /dev/null +++ b/.run/Team417.run.xml @@ -0,0 +1,68 @@ + + + + + \ No newline at end of file diff --git a/.run/Team6220.run.xml b/.run/Team6220.run.xml new file mode 100644 index 000000000000..f21561ebdbbf --- /dev/null +++ b/.run/Team6220.run.xml @@ -0,0 +1,68 @@ + + + + + \ No newline at end of file diff --git a/.run/WilyWorks417.run.xml b/.run/WilyWorks417.run.xml new file mode 100644 index 000000000000..f89c396ee156 --- /dev/null +++ b/.run/WilyWorks417.run.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/.run/WilyWorksMentor.run.xml b/.run/WilyWorksMentor.run.xml new file mode 100644 index 000000000000..bdb48ba2bc3f --- /dev/null +++ b/.run/WilyWorksMentor.run.xml @@ -0,0 +1,12 @@ + + + + + + + \ No newline at end of file diff --git a/FtcRobotController/assets/PowerPlay_BCDM.tflite b/FtcRobotController/assets/PowerPlay_BCDM.tflite new file mode 100644 index 000000000000..88787fd5f746 Binary files /dev/null and b/FtcRobotController/assets/PowerPlay_BCDM.tflite differ diff --git a/README.md b/README.md index 355d349291f8..c3925f392e7d 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ This repository contains the public FTC SDK for the DECODE (2025-2026) competition season. ## 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. +This GitHub repository contains th e 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. ## Requirements To use this Android Studio project, you will need Android Studio Ladybug (2024.2) or later. diff --git a/Team6220/build.gradle b/Team6220/build.gradle new file mode 100644 index 000000000000..c7861fbf694c --- /dev/null +++ b/Team6220/build.gradle @@ -0,0 +1,53 @@ +// +// build.gradle in TeamCode +// +// Most of the definitions for building your module reside in a common, shared +// file 'build.common.gradle'. Being factored in this way makes it easier to +// integrate updates to the FTC into your code. If you really need to customize +// the build definitions, you can place those customizations in this file, but +// please think carefully as to whether such customizations are really necessary +// before doing so. + + +// Custom definitions may go here +buildscript { + repositories { + mavenCentral() + maven { + url = 'https://www.matthewo.tech/maven/' + } + } + dependencies { + classpath 'org.team11260:fast-load-plugin:0.1.2' + } +} + +// Include common definitions from above. +apply from: '../build.common.gradle' +apply from: '../build.dependencies.gradle' +apply plugin: 'org.team11260.fast-load-plugin' + +android { + namespace = 'org.firstinspires.ftc.teamcode' + + packagingOptions { + jniLibs.useLegacyPackaging true + } +} + +repositories { + maven { url = 'https://www.matthewo.tech/maven/' } + maven { url 'https://jitpack.io' } +} + +dependencies { + implementation project(':FtcRobotController') + implementation project(path: ':WilyWorks') + + implementation "com.acmerobotics.roadrunner:ftc:0.1.13" + implementation "com.acmerobotics.roadrunner:core:1.0.0" + implementation "com.acmerobotics.roadrunner:actions:1.0.0" + implementation "com.acmerobotics.dashboard:dashboard:0.4.16" + implementation 'org.team11260:fast-load:0.1.2' + implementation 'com.github.SwerveRobotics:FtcDrivers:0.0.2' +} \ No newline at end of file diff --git a/TeamCode/lib/OpModeAnnotationProcessor.jar b/Team6220/lib/OpModeAnnotationProcessor.jar similarity index 100% rename from TeamCode/lib/OpModeAnnotationProcessor.jar rename to Team6220/lib/OpModeAnnotationProcessor.jar diff --git a/TeamCode/src/main/AndroidManifest.xml b/Team6220/src/main/AndroidManifest.xml similarity index 100% rename from TeamCode/src/main/AndroidManifest.xml rename to Team6220/src/main/AndroidManifest.xml diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/AutonomousEnums.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/AutonomousEnums.java new file mode 100644 index 000000000000..d1c235503e07 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/AutonomousEnums.java @@ -0,0 +1,57 @@ +package org.firstinspires.ftc.team6220; + +import androidx.annotation.NonNull; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; + + +public class AutonomousEnums { + + public enum AutoStartPosition { + LEFT(DRIFTConstants.LEFT_STARTING_POSE), + MIDDLE(DRIFTConstants.MIDDLE_STARTING_POSE), + RIGHT(DRIFTConstants.RIGHT_STARTING_POSE); + + public final Pose2d startingPose; + + AutoStartPosition(Pose2d startingPose) { + this.startingPose = startingPose; + } + } + + public enum AutoType { + SCORING, + PARK; + } + + public enum ParkPosition { + OBSERVATION(DRIFTConstants.OBSERVATION_PARK_POSITION), + SUBMERSIBLE(DRIFTConstants.SUBMERSIBLE_PARK_POSITION); + + public final Vector2d parkingPosition; + + @NonNull + @Override + public String toString() { + // haven't implemented this yet, so ternary operator override go brrrt + return this.equals(ParkPosition.SUBMERSIBLE) ? "DISABLED" : super.toString(); + } + + ParkPosition(Vector2d parkingPosition) { + this.parkingPosition = parkingPosition; + } + } + + public enum SpikeMarkSide { + LEFT, + RIGHT; + + @NonNull + @Override + public String toString() { + // haven't implemented this yet, so ternary operator override go brrrt + return this.equals(SpikeMarkSide.RIGHT) ? "DISABLED" : super.toString(); + } + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/BaseOpMode.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/BaseOpMode.java new file mode 100644 index 000000000000..7db15c3dddec --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/BaseOpMode.java @@ -0,0 +1,399 @@ +package org.firstinspires.ftc.team6220; + +import static java.lang.System.nanoTime; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Servo; +import com.wilyworks.common.WilyWorks; + +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; +import org.firstinspires.ftc.team6220.roadrunner.RobotAction; + +import java.util.ArrayList; + +/** + * This class contains all of the base logic that is shared between all of the TeleOp and + * Autonomous logic. All TeleOp and Autonomous classes should derive from this class. + */ +abstract public class BaseOpMode extends LinearOpMode { + + class SlideAction extends RobotAction { + final int EPSILON = 3; // Ticks + + int targetMotorPosition; + double targetMotorPower; + + public SlideAction(int targetMotorPosition, double targetMotorPower) { + this.targetMotorPosition = targetMotorPosition; + this.targetMotorPower = targetMotorPower; + } + + @Override + public boolean run(double elapsedTime) { + setSlideMotorPosition(targetMotorPosition); + setSlideMotorPower(targetMotorPower); + int currentSlideError = Math.abs(getSlideMotorPosition() - targetMotorPosition); + + // Return 'true' to call again when not at target position yet: + return (currentSlideError > EPSILON); + } + } + + // Arm positions, in ticks: + final int ARM_HOME = 0; + final int ARM_COLLECT = 2000; + final int ARM_SCORE_BASKET = 1500; + + final double INTAKE_COLLECT = 0.5; + final double INTAKE_DEPOSIT = -1.0; + final double INTAKE_OFF = 0.0; + + // putting more hardware initialization stuff into baseopmode go brrr + protected DcMotorEx armBaseMotor; + protected CRServo intakeCRServo; + protected Servo armElbowServo; + protected DcMotorEx slidesMotor; + protected Servo dumperServo; + + // load simulators + protected SlideAndDumperSimulator slideAndDumperSim = new SlideAndDumperSimulator(); + + protected void initializeHardware() { + // yonk stuff from hardwaremap + armBaseMotor = hardwareMap.get(DcMotorEx.class, DRIFTConstants.ARM_BASE_MOTOR_HARDWARE_IDENTIFIER); + slidesMotor = hardwareMap.get(DcMotorEx.class,DRIFTConstants.SLIDES_MOTOR_HARDWARE_IDENTIFIER); + intakeCRServo = hardwareMap.get(CRServo.class, DRIFTConstants.INTAKE_SERVO_HARDWARE_IDENTIFIER); + dumperServo = hardwareMap.get(Servo.class,DRIFTConstants.DUMPER_SERVO_HARDWARE_IDENTIFIER); + armElbowServo = hardwareMap.get(Servo.class,DRIFTConstants.ARM_ELBOW_SERVO_HARDWARE_IDENTIFIER); + + // initializing motors yay :D + armBaseMotor.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE); + slidesMotor.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE); + + armBaseMotor.setCurrentAlert(5, CurrentUnit.AMPS); + slidesMotor.setCurrentAlert(5, CurrentUnit.AMPS); + + armBaseMotor.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); + slidesMotor.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); + armBaseMotor.setTargetPosition(0); + slidesMotor.setTargetPosition(0); + armBaseMotor.setMode(DcMotorEx.RunMode.RUN_TO_POSITION); + slidesMotor.setMode(DcMotorEx.RunMode.RUN_TO_POSITION); + + armBaseMotor.setPower(1.0); + slidesMotor.setPower(1.0); + + //armBaseMotor.setVelocity(Constants.ARM_BASE_MOTOR_VELOCITY); + //slidesMotor.setVelocity(Constants.SLIDES_MOTOR_VELOCITY); + } + + // Helper function for settings the arm position, in ticks: + void setSlideMotorPosition(int targetMotorPosition) { + if ((targetMotorPosition < ARM_HOME) || (targetMotorPosition > ARM_COLLECT)) { + throw new IllegalArgumentException("Invalid setArmPosition() request."); + } + slidesMotor.setTargetPosition(targetMotorPosition); + slideAndDumperSim.setSlidePosition(targetMotorPosition); + } + // Helper function for querying the arm position, in ticks. Uses the simulator when running + // under Wily Works. + int getSlideMotorPosition() { + if (WilyWorks.isSimulating) + return slideAndDumperSim.getSlidePosition(); + else + return slidesMotor.getCurrentPosition(); + } + // Helper function to set the power on the intake. + void setSlideMotorPower(double power) { // Positive is intake, negative is out-take, zero is stop + if ((power > INTAKE_COLLECT) || (power < INTAKE_DEPOSIT)) { + throw new IllegalArgumentException("Invalid setIntakePower() request."); + } + slidesMotor.setPower(power); + slideAndDumperSim.setDumperPower(power); + } + // Class for simulating the arm and drawing it to the field view of FTC Dashboard. + static class SlideAndDumperSimulator { + final double VELOCITY = 3000; // The arm rotates at about 2000 ticks per second + final double SLIDE_BASE_HEIGHT = 13.77; + double currentSlidePosition = 0; // Current arm motor position, in ticks + double targetSlidePosition = 0; // Target arm motor position, in ticks + double dumperPower = 0; // Current intake servo power, positive is intake, negative is deposit + double previousTime; // Seconds + + // Return the time, in seconds: + double time() { + return nanoTime() * 1e-9; + } + + // These simulate the 'real' helper functions: + void setSlidePosition(int targetInTicks) { + this.targetSlidePosition = targetInTicks; + previousTime = time(); + } + int getSlidePosition() { + return (int) currentSlidePosition; + } + void setDumperPower(double power) { + dumperPower = power; + } + + // This method updates our simulation and draws the arm's state on FTC dashboard. + void update(Canvas canvas, Pose2d pose) { + // Figure out the change in time from the last update call, in seconds: + double currentTime = time(); + double dt = currentTime - previousTime; + previousTime = currentTime; + + // Advance the arm's position according to our simple simulation: + double remainingTicks = targetSlidePosition - currentSlidePosition; + double magnitudeToAdvance = Math.min(VELOCITY * dt, Math.abs(remainingTicks)); + currentSlidePosition += Math.copySign(magnitudeToAdvance, remainingTicks); + + // Draw a side view of the arm in the middle of the FTC Dashboard field, on top of + // the submersible floor. We use the same coordinates for this as when we're + // calculating the coordinates for the robot, so here the submersible base where + // we'll be drawing goes from (-12, 22) to (12, -22), in inches: + canvas.setFill("#808080"); // Set the fill color to grey + canvas.fillRect(-12, 22, 24, -44); // Erase the submersible base + + canvas.setStroke("#000000"); // Draw the robot base in black + canvas.strokeLine(0, 0, 18, 0); // The line representing the robot base + + canvas.setFill("#000000"); // Draw the wheels in black + canvas.fillCircle(3, 0, 2); // Wheel #1 + canvas.fillCircle(15, 0, 2); // Wheel #2 + + double slideHeight = SLIDE_BASE_HEIGHT; + // Draw a line representing the arm. It starts at the origin and has an angle that + // is calculated as a fraction of the end angles: + // Add more lines to change slide height + canvas.strokeLine(18, 0, 18, slideHeight); + } + } + + static class ArmElbowAndIntakeSimulator { + + final double ARM_MOTOR_VELOCITY = 3000; + + double intakePower; + int currentArmMotorPosition; + int targetArmMotorPosition; + double armElbowPosition; + + double previousTime; + + // Return the time, in seconds: + double time() { + return nanoTime() * 1e-9; + } + + void update(Canvas canvas, Pose2d pose) { + double currentTime = time(); + double dt = currentTime - previousTime; + previousTime = currentTime; + + } + + public double getIntakePower() { + return intakePower; + } + + public void setIntakePower(double intakePower) { + this.intakePower = intakePower; + } + + public double getArmElbowPosition() { + return armElbowPosition; + } + + public void setArmElbowPosition(double armElbowPosition) { + this.armElbowPosition = armElbowPosition; + } + + public int getCurrentArmMotorPosition() { + return currentArmMotorPosition; + } + + public void setTargetArmMotorPosition(int targetArmMotorPosition) { + this.targetArmMotorPosition = targetArmMotorPosition; + this.previousTime = time(); + } + } + + public class ArmMoveAction extends RobotAction { + ArmActionState armActionState; + + public ArmMoveAction(ArmActionState armActionState) { + this.armActionState = armActionState; + } + + @Override + public boolean run(double elapsedTime) { + // only call servo once +// if (elapsedTime == 0) { +// armElbowServo.setPosition(armActionState.armElbowServoPosition); +// } + + // update motor many times because it goofy + armBaseMotor.setTargetPosition(armActionState.armBaseMotorTargetPositionTicks); + + telemetry.addData("armBaseMotorIsBusy", armBaseMotor.isBusy()); + + // if it hasnt passed target position, run this again :) + return armBaseMotor.isBusy(); + } + } + + public enum ArmActionState { + PICKUP(DRIFTConstants.ARM_BASE_MOTOR_POSITION_GROUND, DRIFTConstants.ARM_ELBOW_SERVO_POSITION_GROUND), + OVER_BAR(DRIFTConstants.ARM_BASE_MOTOR_POSITION_OVER_BAR, DRIFTConstants.ARM_ELBOW_SERVO_POSITION_OVER_BAR), + INIT(DRIFTConstants.ARM_BASE_MOTOR_POSITION_INIT, DRIFTConstants.ARM_ELBOW_SERVO_POSITION_OVER_BAR), + OUT(DRIFTConstants.ARM_BASE_MOTOR_POSITION_OUT, DRIFTConstants.ARM_ELBOW_SERVO_POSITION_TRANSFER), + TRANSFER(DRIFTConstants.ARM_BASE_MOTOR_POSITION_TRANSFER, DRIFTConstants.ARM_ELBOW_SERVO_POSITION_TRANSFER); + + final int armBaseMotorTargetPositionTicks; + final double armElbowServoPosition; + + ArmActionState(int armBaseMotorTargetPositionTicks, double armElbowServoPosition) { + this.armBaseMotorTargetPositionTicks = armBaseMotorTargetPositionTicks; + this.armElbowServoPosition = armElbowServoPosition; + } + } + + public class SlideMoveAction extends RobotAction { + + // set to 0 as a placeholder + private double timeoutTime = 0; + final SlideActionState actionState; + + public SlideMoveAction(SlideActionState actionState) { + this.actionState = actionState; + } + + @Override + public boolean run(double elapsedTime) { + + final double TIMEOUT_DELAY_SECONDS = 3; + + if (timeoutTime == 0) { + timeoutTime = elapsedTime + TIMEOUT_DELAY_SECONDS; + } + + // update motor many times because it goofy + slidesMotor.setTargetPosition(actionState.slidesMotorTargetPositionTicks); + + telemetry.addData("slidesMotorIsBusy", slidesMotor.isBusy()); + + // if it hasnt passed target position, run this again :) + return slidesMotor.isBusy() && elapsedTime < timeoutTime; + } + } + + public enum SlideActionState { + GROUND(DRIFTConstants.SLIDES_MOTOR_GROUND_POSITION), + LOW_BASKET(DRIFTConstants.SLIDES_MOTOR_LOW_BASKET_POSITION), + HIGH_BASKET(DRIFTConstants.SLIDES_MOTOR_HIGH_BASKET_POSITION), + OVER_HIGH_CHAMBER(DRIFTConstants.SLIDES_MOTOR_OVER_HIGH_CHAMBER_POSITION), + HIGH_CHAMBER(DRIFTConstants.SLIDES_MOTOR_HIGH_CHAMBER_POSITION); + + final int slidesMotorTargetPositionTicks; + + SlideActionState(int slidesMotorTargetPositionTicks) { + this.slidesMotorTargetPositionTicks = slidesMotorTargetPositionTicks; + } + } + + public class DumperMoveAction extends RobotAction { + DumperActionState dumperActionState; + + public DumperMoveAction(DumperActionState dumperActionState) { + this.dumperActionState = dumperActionState; + } + + public boolean run(double elapsedTime) { + dumperServo.setPosition(dumperActionState.dumperServoPosition); + + return false; + } + } + + public enum DumperActionState { + DUMP(DRIFTConstants.DUMPER_SERVO_POSITION_DUMP), + TRANSFER(DRIFTConstants.DUMPER_SERVO_POSITION_TRANSFER), + INIT(DRIFTConstants.DUMPER_SERVO_POSITION_INIT); + + final double dumperServoPosition; + + DumperActionState(double dumperServoPosition) { + this.dumperServoPosition = dumperServoPosition; + } + } + + public enum ArmElbowServoState { + GROUND(DRIFTConstants.ARM_ELBOW_SERVO_POSITION_GROUND), + TRANSFER(DRIFTConstants.ARM_ELBOW_SERVO_POSITION_TRANSFER), + OVER_BAR(DRIFTConstants.ARM_ELBOW_SERVO_POSITION_OVER_BAR); + + final double armElbowServoPosition; + + ArmElbowServoState(double armElbowServoPosition) { + this.armElbowServoPosition = armElbowServoPosition; + } + } + + public class ArmElbowMoveAction extends RobotAction { + ArmElbowServoState armElbowServoState; + + public ArmElbowMoveAction(ArmElbowServoState armElbowServoState) { + this.armElbowServoState = armElbowServoState; + } + + public boolean run(double elapsedTime) { + armElbowServo.setPosition(armElbowServoState.armElbowServoPosition); + + return false; + } + } + + public class IntakeMoveAction extends RobotAction { + final double intakeServoPower; + + public IntakeMoveAction(double intakeServoPower) { + this.intakeServoPower = intakeServoPower; + } + + public boolean run(double elapsedTime) { + intakeCRServo.setPower(intakeServoPower); + + return false; + } + } + + // this is super not cooked rn, please use + public class CompoundRobotAction extends RobotAction { + + ArrayList compoundActionList; + + public CompoundRobotAction(ArrayList compoundActionList) { + this.compoundActionList = compoundActionList; + } + + @Override + public boolean run(double elapsedTime) { + // grab the first action in the list + RobotAction currentAction = compoundActionList.get(0); + + // if the current action returned false, omit it from the list so that it moves to the next action in the queue + if (!currentAction.run(elapsedTime)) { + compoundActionList.remove(0); + } + + // if the list isn't empty, there's more actions to run + return !compoundActionList.isEmpty(); + } + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/ColorSensorTest.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/ColorSensorTest.java new file mode 100644 index 000000000000..f257d84feb78 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/ColorSensorTest.java @@ -0,0 +1,222 @@ +/* Copyright (c) 2017-2020 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.team6220; + +import android.app.Activity; +import android.graphics.Color; +import android.view.View; + +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.DistanceSensor; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import com.qualcomm.robotcore.hardware.SwitchableLight; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* + * This OpMode shows how to use a color sensor in a generic + * way, regardless of which particular make or model of color sensor is used. The OpMode + * assumes that the color sensor is configured with a name of "sensor_color". + * + * There will be some variation in the values measured depending on the specific sensor you are using. + * + * You can increase the gain (a multiplier to make the sensor report higher values) by holding down + * the A button on the gamepad, and decrease the gain by holding down the B button on the gamepad. + * + * If the color sensor has a light which is controllable from software, you can use the X button on + * the gamepad to toggle the light on and off. The REV sensors don't support this, but instead have + * a physical switch on them to turn the light on and off, beginning with REV Color Sensor V2. + * + * If the color sensor also supports short-range distance measurements (usually via an infrared + * proximity sensor), the reported distance will be written to telemetry. As of September 2020, + * the only color sensors that support this are the ones from REV Robotics. These infrared proximity + * sensor measurements are only useful at very small distances, and are sensitive to ambient light + * and surface reflectivity. You should use a different sensor if you need precise distance measurements. + * + * 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 + */ +@TeleOp(name = "6220 Color Sensor Test", group = "Test") +public class ColorSensorTest extends LinearOpMode { + + /** The colorSensor field will contain a reference to our color sensor hardware object */ + NormalizedColorSensor colorSensor; + + /** The relativeLayout field is used to aid in providing interesting visual feedback + * in this sample application; you probably *don't* need this when you use a color sensor on your + * robot. Note that you won't see anything change on the Driver Station, only on the Robot Controller. */ + View relativeLayout; + + /* + * The runOpMode() method is the root of this OpMode, as it is in all LinearOpModes. + * Our implementation here, though is a bit unusual: we've decided to put all the actual work + * in the runSample() method rather than directly in runOpMode() itself. The reason we do that is + * that in this sample we're changing the background color of the robot controller screen as the + * OpMode runs, and we want to be able to *guarantee* that we restore it to something reasonable + * and palatable when the OpMode ends. The simplest way to do that is to use a try...finally + * block around the main, core logic, and an easy way to make that all clear was to separate + * the former from the latter in separate methods. + */ + @Override public void runOpMode() { + + // Get a reference to the RelativeLayout so we can later change the background + // color of the Robot Controller app to match the hue detected by the RGB sensor. + int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName()); + relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId); + + try { + runSample(); // actually execute the sample + } finally { + // On the way out, *guarantee* that the background is reasonable. It doesn't actually start off + // as pure white, but it's too much work to dig out what actually was used, and this is good + // enough to at least make the screen reasonable again. + // Set the panel back to the default color + relativeLayout.post(new Runnable() { + public void run() { + relativeLayout.setBackgroundColor(Color.WHITE); + } + }); + } + } + + protected void runSample() { + // You can give the sensor a gain value, will be multiplied by the sensor's raw value before the + // normalized color values are calculated. Color sensors (especially the REV Color Sensor V3) + // can give very low values (depending on the lighting conditions), which only use a small part + // of the 0-1 range that is available for the red, green, and blue values. In brighter conditions, + // you should use a smaller gain than in dark conditions. If your gain is too high, all of the + // colors will report at or near 1, and you won't be able to determine what color you are + // actually looking at. For this reason, it's better to err on the side of a lower gain + // (but always greater than or equal to 1). + float gain = 2; + + // Once per loop, we will update this hsvValues array. The first element (0) will contain the + // hue, the second element (1) will contain the saturation, and the third element (2) will + // contain the value. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html + // for an explanation of HSV color. + final float[] hsvValues = new float[3]; + + // xButtonPreviouslyPressed and xButtonCurrentlyPressed keep track of the previous and current + // state of the X button on the gamepad + boolean xButtonPreviouslyPressed = false; + boolean xButtonCurrentlyPressed = false; + + // Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over + // ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while + // the values you get from ColorSensor are dependent on the specific sensor you're using. + colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color"); + + // If possible, turn the light on in the beginning (it might already be on anyway, + // we just make sure it is if we can). + if (colorSensor instanceof SwitchableLight) { + ((SwitchableLight)colorSensor).enableLight(true); + } + + // Wait for the start button to be pressed. + waitForStart(); + + // Loop until we are asked to stop + while (opModeIsActive()) { + // Explain basic gain information via telemetry + telemetry.addLine("Hold the A button on gamepad 1 to increase gain, or B to decrease it.\n"); + telemetry.addLine("Higher gain values mean that the sensor will report larger numbers for Red, Green, and Blue, and Value\n"); + + // Update the gain value if either of the A or B gamepad buttons is being held + if (gamepad1.a) { + // Only increase the gain by a small amount, since this loop will occur multiple times per second. + gain += 0.005; + } else if (gamepad1.b && gain > 1) { // A gain of less than 1 will make the values smaller, which is not helpful. + gain -= 0.005; + } + + // Show the gain value via telemetry + telemetry.addData("Gain", gain); + + // Tell the sensor our desired gain value (normally you would do this during initialization, + // not during the loop) + colorSensor.setGain(gain); + + // Check the status of the X button on the gamepad + xButtonCurrentlyPressed = gamepad1.x; + + // If the button state is different than what it was, then act + if (xButtonCurrentlyPressed != xButtonPreviouslyPressed) { + // If the button is (now) down, then toggle the light + if (xButtonCurrentlyPressed) { + if (colorSensor instanceof SwitchableLight) { + SwitchableLight light = (SwitchableLight)colorSensor; + light.enableLight(!light.isLightOn()); + } + } + } + xButtonPreviouslyPressed = xButtonCurrentlyPressed; + + // Get the normalized colors from the sensor + NormalizedRGBA colors = colorSensor.getNormalizedColors(); + + /* Use telemetry to display feedback on the driver station. We show the red, green, and blue + * normalized values from the sensor (in the range of 0 to 1), as well as the equivalent + * HSV (hue, saturation and value) values. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html + * for an explanation of HSV color. */ + + // Update the hsvValues array by passing it to Color.colorToHSV() + Color.colorToHSV(colors.toColor(), hsvValues); + + telemetry.addLine() + .addData("Red", "%.3f", colors.red) + .addData("Green", "%.3f", colors.green) + .addData("Blue", "%.3f", colors.blue); + telemetry.addLine() + .addData("Hue", "%.3f", hsvValues[0]) + .addData("Saturation", "%.3f", hsvValues[1]) + .addData("Value", "%.3f", hsvValues[2]); + telemetry.addData("Alpha", "%.3f", colors.alpha); + + /* If this color sensor also has a distance sensor, display the measured distance. + * Note that the reported distance is only useful at very close range, and is impacted by + * ambient light and surface reflectivity. */ + if (colorSensor instanceof DistanceSensor) { + telemetry.addData("Distance (cm)", "%.3f", ((DistanceSensor) colorSensor).getDistance(DistanceUnit.CM)); + } + + telemetry.update(); + + // Change the Robot Controller's background color to match the color detected by the color sensor. + relativeLayout.post(new Runnable() { + public void run() { + relativeLayout.setBackgroundColor(Color.HSVToColor(hsvValues)); + } + }); + } + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/CompetitionAuto.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/CompetitionAuto.java new file mode 100644 index 000000000000..31bc03b3b121 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/CompetitionAuto.java @@ -0,0 +1,307 @@ +package org.firstinspires.ftc.team6220; + +//import android.transition.Slide; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.team6220.javatextmenu.MenuFinishedButton; +import org.firstinspires.ftc.team6220.javatextmenu.MenuInput; +import org.firstinspires.ftc.team6220.javatextmenu.TextMenu; + +import org.firstinspires.ftc.team6220.roadrunner.MecanumDrive; +import org.firstinspires.ftc.team6220.roadrunner.RobotAction; + +import java.util.ArrayList; +import java.util.Objects; + +@Autonomous(name="CompetitionAuto", group="Competition", preselectTeleOp="CompetitionTeleOp") +public class CompetitionAuto extends BaseOpMode { + + // defaults so it doesnt explode if you skip the text menu + private AutonomousEnums.AutoStartPosition autoStartPosition = AutonomousEnums.AutoStartPosition.LEFT; + private AutonomousEnums.AutoType autoType = AutonomousEnums.AutoType.SCORING; + private AutonomousEnums.ParkPosition parkPosition = AutonomousEnums.ParkPosition.OBSERVATION; + private AutonomousEnums.SpikeMarkSide spikeMarkSide = AutonomousEnums.SpikeMarkSide.LEFT; + + @Override + public void runOpMode() { + + // initialize hardware :> + initializeHardware(); + + // lock dumper servo to init position + dumperServo.setPosition(DRIFTConstants.DUMPER_SERVO_POSITION_INIT); + + // TextMenu implementation yoinked from valsei's GitHub + TextMenu startingConditionMenu = new TextMenu(); + MenuInput input = new MenuInput(MenuInput.InputType.CONTROLLER); + + startingConditionMenu.add("Select Starting Conditions") + .add("Start Position: ") + .add("start_position", AutonomousEnums.AutoStartPosition.class) + .add("Auto Type: ") + .add("auto_type", AutonomousEnums.AutoType.class) + .add("confirm_selection", new MenuFinishedButton()); + + // update the starting condition menu until it's done + textMenuUpdateUntilComplete(startingConditionMenu, input); + + // get values from textmenu + autoStartPosition = startingConditionMenu.getResult(AutonomousEnums.AutoStartPosition.class, "start_position"); + autoType = startingConditionMenu.getResult(AutonomousEnums.AutoType.class, "auto_type"); + + // initialize mecanumdrive + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, autoStartPosition.startingPose); + + TextMenu scoringSelectionMenu = new TextMenu(); + + // declare new textmenu + scoringSelectionMenu.add("Scoring Settings: ") + .addTextConditional("SpikeMark Side: ", autoType.equals(AutonomousEnums.AutoType.SCORING)) + .addEnumConditional("spikemark_side", AutonomousEnums.SpikeMarkSide.class, autoType.equals(AutonomousEnums.AutoType.SCORING)) + .add("Park Position: ") + .add("park_position", AutonomousEnums.ParkPosition.class) + .add("confirm_selection", new MenuFinishedButton()); + + // update the scoring selection menu until it's done + textMenuUpdateUntilComplete(scoringSelectionMenu, input); + + // only assign values if they are enabled for selected autotype + if (autoType.equals(AutonomousEnums.AutoType.SCORING)) { + spikeMarkSide = scoringSelectionMenu.getResult(AutonomousEnums.SpikeMarkSide.class, "spikemark_side"); + } + parkPosition = scoringSelectionMenu.getResult(AutonomousEnums.ParkPosition.class, "park_position"); + + // Build the trajectory *before* the start button is pressed because Road Runner + // can take multiple seconds for this operation. We wouldn't want to have to wait + // as soon as the Start button is pressed! + // Scoring trajectories + Action trajectoryAction = computeAutoPath(drive); + + // Get a preview of the trajectory's path: + Canvas previewCanvas = new Canvas(); + trajectoryAction.preview(previewCanvas); + + // Show the preview on FTC Dashboard now. + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + MecanumDrive.sendTelemetryPacket(packet); + + // Wait for Start to be pressed on the Driver Hub! + waitForStart(); + + dumperServo.setPosition(DRIFTConstants.DUMPER_SERVO_POSITION_TRANSFER / 2); + dumperServo.setPosition(DRIFTConstants.DUMPER_SERVO_POSITION_TRANSFER); + + // commented out so nothing's borked on accident :) + // drive.runParallel(elbowAction.setTargetPosition(DRIFTConstants.ARM_ELBOW_SERVO_PRESET_POSITION_OVER_BARRIER)); + + boolean more = true; + while (opModeIsActive() && more) { + telemetry.addLine("Running Auto!"); + + // 'packet' is the object used to send data to FTC Dashboard: + packet = MecanumDrive.getTelemetryPacket(); + + // Update and draw the arm simulator: + slideAndDumperSim.update(packet.fieldOverlay(), drive.pose); + // Draw the preview and then run the next step of the trajectory on top: + + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + more = trajectoryAction.run(packet); + + // Only send the packet if there's more to do in order to keep the very last + // drawing up on the field once the robot is done: + if (more) + MecanumDrive.sendTelemetryPacket(packet); + telemetry.update(); + } + } + + private void textMenuUpdateUntilComplete(TextMenu textMenu, MenuInput input) { + while (!textMenu.isCompleted() && !isStopRequested() && !this.isStarted()) { + for (String line : textMenu.toListOfStrings()) { + telemetry.addLine(line); + } + telemetry.update(); + + input.update( + gamepad1.left_stick_x, gamepad1.left_stick_y, + gamepad1.dpad_left, gamepad1.dpad_right, + gamepad1.dpad_down, gamepad1.dpad_up, + gamepad1.a + ); + textMenu.updateWithInput(input); + sleep(17); + } + } + + private Action computeAutoPath(MecanumDrive drive) { + + + TrajectoryActionBuilder actionBuilder = drive.actionBuilder(autoStartPosition.startingPose); + /* + slidesMotor.setVelocity(1500); + actionBuilder = actionBuilder + .stopAndAdd(new SlideMoveAction(SlideActionState.OVER_HIGH_CHAMBER)) + .setTangent((3*Math.PI)/2) + .splineToLinearHeading(new Pose2d(0, 33.5, Math.toRadians(90)), Math.toRadians(270)) + .stopAndAdd(new SlideMoveAction(SlideActionState.HIGH_CHAMBER)) + .waitSeconds(1) + .splineToLinearHeading(new Pose2d(0, 36, Math.toRadians(90)), Math.toRadians(270)) + .stopAndAdd(new SlideMoveAction(SlideActionState.OVER_HIGH_CHAMBER)) + .splineToLinearHeading(new Pose2d(0, 40, Math.toRadians(90)), Math.toRadians(270)) + + .stopAndAdd(new DumperMoveAction(DumperActionState.TRANSFER)) + .afterDisp(0, new SlideMoveAction(SlideActionState.GROUND)) + + .splineToLinearHeading(new Pose2d(0, 62, Math.toRadians(90)), Math.toRadians(270)) + + // spline to prepare to collect first sample + .setTangent(Math.toRadians(180)) + .splineToLinearHeading(new Pose2d(40, 24, Math.toRadians(270)), 3 * Math.PI / 2) + .splineToLinearHeading(new Pose2d(47, 9, Math.toRadians(270)), Math.toRadians(340)) + .endTrajectory() + .setTangent(Math.toRadians(90)) + + // spline to deposit first sample in scoring area + .splineToLinearHeading(new Pose2d(55, 60, Math.toRadians(220)), Math.toRadians(40)) + .endTrajectory() + .setTangent(Math.toRadians(180)) + + // spline to prepare to collect second sample + .splineToLinearHeading(new Pose2d(57, 10, Math.toRadians(270)), Math.toRadians(330)) + .endTrajectory() + .setTangent(Math.toRadians(90)) + + // spline to deposit second sample in scoring area + .splineToLinearHeading(new Pose2d(61, 55, Math.toRadians(220)), Math.toRadians(40)) + .endTrajectory() + .setTangent(Math.toRadians(-80)) + + // spline to prepare to collect third sample + .splineToLinearHeading(new Pose2d(70, 12, Math.toRadians(270)), Math.toRadians(40)) + .endTrajectory() + + // strafe to deposit third sample + .strafeTo(new Vector2d(70, 52)) + + // prepare to move to park position and move dumper and arm to init position for teleop + .splineToLinearHeading(new Pose2d(62, 50, Math.PI), 0) //change this in backup code too if it works! + .endTrajectory() + .setTangent(Math.toRadians(200)) + + .endTrajectory(); + */ + + + // apply scoring portion of auto + if (Objects.requireNonNull(autoType) == AutonomousEnums.AutoType.SCORING) { + switch (spikeMarkSide) { + case LEFT: { + + // declarations for the compound arm actions + ArrayList GRAB = new ArrayList<>(); + // GRAB.add(); + ArrayList TRANSFER = new ArrayList<>(); + ArrayList IDLE = new ArrayList<>(); + + actionBuilder = actionBuilder + // Go to high basket and raise slides + .afterDisp(0, new SlideMoveAction(SlideActionState.HIGH_BASKET)) + .splineToLinearHeading(new Pose2d(59, 57, Math.toRadians(225)), 0) + + // Lift slides and dump + .stopAndAdd(new DumperMoveAction(DumperActionState.DUMP)) + + // Reset dumper and lower slides + .waitSeconds(2) + + .stopAndAdd(new DumperMoveAction(DumperActionState.TRANSFER)) + .afterDisp(0, new SlideMoveAction(SlideActionState.GROUND)) + + // spline to prepare to collect first sample + .setTangent(Math.toRadians(180)) + .splineToLinearHeading(new Pose2d(40, 24, Math.toRadians(270)), 3 * Math.PI / 2) + .splineToLinearHeading(new Pose2d(45, 9, Math.toRadians(270)), Math.toRadians(340)) + .endTrajectory() + .setTangent(Math.toRadians(90)) + + // spline to deposit first sample in scoring area + .splineToLinearHeading(new Pose2d(55, 60, Math.toRadians(220)), Math.toRadians(40)) + .endTrajectory() + .setTangent(Math.toRadians(180)) + + // spline to prepare to collect second sample + .splineToLinearHeading(new Pose2d(57, 10, Math.toRadians(270)), Math.toRadians(330)) + .endTrajectory() + .setTangent(Math.toRadians(90)) + + // spline to deposit second sample in scoring area + .splineToLinearHeading(new Pose2d(61, 55, Math.toRadians(220)), Math.toRadians(40)) + .endTrajectory() + .setTangent(Math.toRadians(-80)) + + // spline to prepare to collect third sample + .splineToLinearHeading(new Pose2d(70, 12, Math.toRadians(270)), Math.toRadians(40)) + .endTrajectory() + + // strafe to deposit third sample + .strafeTo(new Vector2d(70, 52)) + + // prepare to move to park position and move dumper and arm to init position for teleop + .splineToLinearHeading(new Pose2d(62, 50, Math.PI), 0) //change this in backup code too if it works! + .endTrajectory() + .setTangent(Math.toRadians(200)); + + + } + case RIGHT: { + // to be implemented + } + } + } + + + + // apply parking portion of auto + switch (parkPosition) { + case OBSERVATION: { + switch(autoType) { + case PARK: { + actionBuilder = actionBuilder.strafeTo(parkPosition.parkingPosition) + .endTrajectory(); + } + case SCORING: { + actionBuilder = actionBuilder.splineTo(parkPosition.parkingPosition, Math.toRadians(170)) + .endTrajectory(); + } + } + } + + case SUBMERSIBLE: { + + switch (autoType) { + case PARK: { + // code for submersible park auto goes here + } + case SCORING: { + // code for submersible scoring-park auto goes here + } + } + } + } + + + return actionBuilder.endTrajectory().build(); + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/CompetitionSimTeleOp.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/CompetitionSimTeleOp.java new file mode 100644 index 000000000000..94728cb5c5a3 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/CompetitionSimTeleOp.java @@ -0,0 +1,150 @@ +package org.firstinspires.ftc.team6220; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.PoseVelocity2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; +import org.firstinspires.ftc.team6220.roadrunner.Drawing; +import org.firstinspires.ftc.team6220.roadrunner.MecanumDrive; +import org.firstinspires.ftc.team6220.roadrunner.RobotAction; + +/** + * This class exposes the competition version of TeleOp. As a general rule, add code to the + * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. + */ +@TeleOp(name="CompSimTeleOp", group="Competition") +public class CompetitionSimTeleOp extends BaseOpMode { + + // Action for controlling the arm in Auto trajectories. + + private DcMotorEx armBaseMotor = null; + private DcMotorEx slidesMotor = null; + private CRServo intakeCRServo = null; + private Servo dumperServo = null; + private Servo armElbowServo = null; + private NormalizedColorSensor colorSensor = null; + private AllianceColor allianceColor; + + @Override + public void runOpMode() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + allianceColor = AllianceColor.RED; + + //colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color"); + + //colorSensor.setGain(Constants.COLOR_SENSOR_GAIN); + + armBaseMotor = hardwareMap.get(DcMotorEx.class, DRIFTConstants.ARM_BASE_MOTOR_HARDWARE_IDENTIFIER); + //slidesMotor = hardwareMap.get(DcMotorEx.class,DRIFTConstants.SLIDES_MOTOR_HARDWARE_IDENTIFIER); + intakeCRServo = hardwareMap.get(CRServo.class, DRIFTConstants.INTAKE_SERVO_HARDWARE_IDENTIFIER); + //dumperServo = hardwareMap.get(Servo.class,DRIFTConstants.DUMPER_SERVO_HARDWARE_IDENTIFIER); + armElbowServo = hardwareMap.get(Servo.class,DRIFTConstants.ARM_ELBOW_SERVO_HARDWARE_IDENTIFIER); + + armBaseMotor.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE); + //slidesMotor.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE); + + armBaseMotor.setCurrentAlert(5, CurrentUnit.AMPS); + //slidesMotor.setCurrentAlert(5, CurrentUnit.AMPS); + + armBaseMotor.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); + //slidesMotor.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); + armBaseMotor.setTargetPosition(0); + //slidesMotor.setTargetPosition(0); + armBaseMotor.setMode(DcMotorEx.RunMode.RUN_TO_POSITION); + //slidesMotor.setMode(DcMotorEx.RunMode.RUN_TO_POSITION); + + //armBaseMotor.setVelocity(Constants.ARM_BASE_MOTOR_VELOCITY); + //slidesMotor.setVelocity(Constants.SLIDES_MOTOR_VELOCITY); + + Pose2d beginPose = new Pose2d(0, 0, 0); + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); + ControlManager controls = new ControlManager(gamepad1, gamepad2); + // Wait for Start to be pressed on the Driver Hub! + waitForStart(); + + while (opModeIsActive()) { + telemetry.addLine("Running TeleOp!"); + //telemetry.addLine("red: " + colorSensor.getNormalizedColors().red); + //telemetry.addLine("blue: " + colorSensor.getNormalizedColors().blue); + + /*if (isHeldSampleInvalid(allianceColor, colorSensor)) { + telemetry.addLine("WEEWOO WEEWOO! EJECT IMMEDIATELY"); + }*/ + + telemetry.update(); + + controls.update(getRuntime()); + + armBaseMotor.setPower(1.0); + armBaseMotor.setTargetPosition(controls.getArmBaseMotorPosition()); + telemetry.addLine("Target Position: " + controls.getArmBaseMotorPosition()); + //slidesMotor.setTargetPosition(controls.getSlidesMotorPosition()); + intakeCRServo.setPower(controls.getIntakeServoPower()); + //dumperServo.setPosition(controls.getDumperServoPosition()); + armElbowServo.setPosition(controls.getArmElbowServoPosition()); + + // jank ass speed modifier + float speedModifier = 1 - (gamepad1.right_trigger * 0.5f); + + // Set the drive motor powers according to the gamepad input: + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y * speedModifier, + -gamepad1.left_stick_x * speedModifier + ), + -gamepad1.right_stick_x * speedModifier + )); + + // Update the current pose: + drive.updatePoseEstimate(); + + // 'packet' is the object used to send data to FTC Dashboard: + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + + // Do the work now for all active Road Runner actions, if any: + drive.doActionsWork(packet); + + // Draw the robot and field: + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), drive.pose); + MecanumDrive.sendTelemetryPacket(packet); + } + } + + private enum AllianceColor { + RED, + BLUE + } + + private boolean isHeldSampleInvalid(AllianceColor allianceColor, NormalizedColorSensor colorSensor) { + NormalizedRGBA colorSensorValues = colorSensor.getNormalizedColors(); + + // code monster that determines if the opposing alliance color is greater, and checks if it's over the threshold + switch(allianceColor) { + case RED: { + if (colorSensorValues.blue > colorSensorValues.red + && colorSensorValues.blue > DRIFTConstants.BLUE_COLOR_SENSOR_RANGES[0]) { + return true; + } + } + case BLUE: { + if (colorSensorValues.red > colorSensorValues.blue + && colorSensorValues.red > DRIFTConstants.RED_COLOR_SENSOR_RANGES[0]) { + return true; + } + } + } + return false; + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/CompetitionTeleOp.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/CompetitionTeleOp.java new file mode 100644 index 000000000000..e42267ce2b3c --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/CompetitionTeleOp.java @@ -0,0 +1,137 @@ +package org.firstinspires.ftc.team6220; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.PoseVelocity2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; + +import org.firstinspires.ftc.team6220.roadrunner.Drawing; +import org.firstinspires.ftc.team6220.roadrunner.MecanumDrive; + +/** + * This class exposes the competition version of TeleOp. As a general rule, add code to the + * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. + */ +@TeleOp(name="TeleOp", group="Competition") +public class CompetitionTeleOp extends BaseOpMode { + + private NormalizedColorSensor colorSensor = null; + private AllianceColor allianceColor; + + @Override + public void runOpMode() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + allianceColor = AllianceColor.RED; + + //colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color"); + + //colorSensor.setGain(Constants.COLOR_SENSOR_GAIN); + + + initializeHardware(); + //armElbowServo.setPosition(0); + + Pose2d beginPose = new Pose2d(0, 0, 0); + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); + ControlManager controls = new ControlManager(gamepad1, gamepad2); + // Wait for Start to be pressed on the Driver Hub! + waitForStart(); + + while (opModeIsActive()) { + telemetry.addLine("Running TeleOp!"); + //telemetry.addLine("red: " + colorSensor.getNormalizedColors().red); + //telemetry.addLine("blue: " + colorSensor.getNormalizedColors().blue); + + /*if (isHeldSampleInvalid(allianceColor, colorSensor)) { + telemetry.addLine("WEEWOO WEEWOO! EJECT IMMEDIATELY"); + }*/ + + + controls.update(getRuntime()); + + /* + telemetry.addLine("Slides Motor Target Position: " + controls.getSlidesMotorPosition()); + telemetry.addLine("Slides Motor Current Position: " + slidesMotor.getCurrentPosition()); + telemetry.addLine("GP2 Right Stick Y: " + -gamepad2.right_stick_y); + */ + + armBaseMotor.setTargetPosition(controls.getArmBaseMotorPosition()); + slidesMotor.setTargetPosition(controls.getSlidesMotorPosition()); + intakeCRServo.setPower(controls.getIntakeServoPower()); + dumperServo.setPosition(controls.getDumperServoPosition()); + armElbowServo.setPosition(controls.getArmElbowServoPosition()); + + // Backup control in case slides stop working mid-game + if (controls.shouldResetSlideEncoder()) { + slidesMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + slidesMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + // speed modifier + float speedModifier = 1 - (gamepad1.right_trigger * 0.5f); + + // if transferring, halve drive speed + if (controls.isTransferring()) { + speedModifier *= 0.5F; + } + + // Set the drive motor powers according to the gamepad input: + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y * speedModifier, + -gamepad1.left_stick_x * speedModifier + ), + -gamepad1.right_stick_x * speedModifier + )); + + // Update the current pose: + drive.updatePoseEstimate(); + + // update telemetry + telemetry.update(); + // 'packet' is the object used to send data to FTC Dashboard: + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + + // Do the work now for all active Road Runner actions, if any: + drive.doActionsWork(packet); + + // Draw the robot and field: + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), drive.pose); + MecanumDrive.sendTelemetryPacket(packet); + } + } + + private enum AllianceColor { + RED, + BLUE + } + + private boolean isHeldSampleInvalid(AllianceColor allianceColor, NormalizedColorSensor colorSensor) { + NormalizedRGBA colorSensorValues = colorSensor.getNormalizedColors(); + + // code monster that determines if the opposing alliance color is greater, and checks if it's over the threshold + switch(allianceColor) { + case RED: { + if (colorSensorValues.blue > colorSensorValues.red + && colorSensorValues.blue > DRIFTConstants.BLUE_COLOR_SENSOR_RANGES[0]) { + return true; + } + } + case BLUE: { + if (colorSensorValues.red > colorSensorValues.blue + && colorSensorValues.red > DRIFTConstants.RED_COLOR_SENSOR_RANGES[0]) { + return true; + } + } + } + return false; + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/ControlManager.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/ControlManager.java new file mode 100644 index 000000000000..0ed92538522f --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/ControlManager.java @@ -0,0 +1,227 @@ +package org.firstinspires.ftc.team6220; + +import com.qualcomm.robotcore.hardware.Gamepad; + +import java.util.ArrayList; +import java.util.function.Predicate; + +public class ControlManager { + + private Gamepad gamepad1 = null; + private Gamepad gamepad2 = null; + + private int armBaseMotorPosition = DRIFTConstants.ARM_BASE_MOTOR_POSITION_OUT; + private int slidesMotorPosition; + private double intakeServoPower; + private double dumperServoPosition = DRIFTConstants.DUMPER_SERVO_POSITION_INIT; + private double armElbowServoPosition = 0; + private boolean shouldResetSlideEncoder; + + private double currentRunTime; + + boolean oldLeftBumperToggleState = false; + + private ProtectedButton slideEncoderResetToggle; + + private ArrayList delayedActions = new ArrayList<>(); + + public ControlManager(Gamepad gamepad1, Gamepad gamepad2) { + this.gamepad1 = gamepad1; + this.gamepad2 = gamepad2; + + this.slideEncoderResetToggle = new ProtectedButton(); + } + + public void update(double currentRunTime) { + // update current run time + this.currentRunTime = currentRunTime; + + // Raise arm elbow to go over submersible bar + if(gamepad2.a && !gamepad2.b) { + armElbowServoPosition = DRIFTConstants.ARM_ELBOW_SERVO_POSITION_OVER_BAR; + clearList(armElbowServoPosition); + } + + // Lower arm elbow to pick up samples + if(gamepad2.b && !gamepad2.a) { + armElbowServoPosition = DRIFTConstants.ARM_ELBOW_SERVO_POSITION_GROUND; + clearList(armElbowServoPosition); + } + + // Transfer sequence for intake to dumper + if(gamepad2.x && !gamepad2.y) { + armElbowServoPosition = DRIFTConstants.ARM_ELBOW_SERVO_POSITION_TRANSFER; + armBaseMotorPosition = DRIFTConstants.ARM_BASE_MOTOR_POSITION_TRANSFER; + dumperServoPosition = DRIFTConstants.DUMPER_SERVO_POSITION_HORIZONTAL; + clearList(armElbowServoPosition); + clearList(armBaseMotorPosition); + clearList(dumperServoPosition); + } + + // Raises the arm up to its initial position (against the hubs) + if (gamepad2.y && !gamepad2.x) { + armElbowServoPosition = DRIFTConstants.ARM_ELBOW_SERVO_POSITION_OVER_BAR; + clearList(armElbowServoPosition); + } + + // Powers the intake + intakeServoPower = -gamepad2.left_stick_y; + // slows it down if its in transfer position + if (dumperServoPosition == DRIFTConstants.DUMPER_SERVO_POSITION_TRANSFER) { + intakeServoPower *= 0.4; + } + + // Lowers the arm to go over the submersible + if (gamepad2.right_bumper && !gamepad2.left_bumper) { + armBaseMotorPosition = DRIFTConstants.ARM_BASE_MOTOR_POSITION_OVER_BAR; + clearList(armBaseMotorPosition); + } + + // Lowers the slides to the ground position + if (gamepad2.dpad_down && !gamepad2.dpad_up) { + slidesMotorPosition = DRIFTConstants.SLIDES_MOTOR_GROUND_POSITION; + // move dumper out of the way of the buckets + dumperServoPosition = DRIFTConstants.DUMPER_SERVO_POSITION_TRANSFER; + // move intake out of the way of the dumper + armElbowServoPosition = DRIFTConstants.ARM_ELBOW_SERVO_POSITION_OVER_BAR; + clearList(slidesMotorPosition); + clearList(dumperServoPosition); + clearList(armElbowServoPosition); + } + + // Raises the slides to reach the high basket + if (gamepad2.dpad_up && !gamepad2.dpad_down) { + slidesMotorPosition = DRIFTConstants.SLIDES_MOTOR_HIGH_BASKET_POSITION; + dumperServoPosition = DRIFTConstants.DUMPER_SERVO_POSITION_HORIZONTAL; + armElbowServoPosition = DRIFTConstants.ARM_ELBOW_SERVO_POSITION_OVER_BAR; + clearList(slidesMotorPosition); + clearList(dumperServoPosition); + clearList(armElbowServoPosition); + } + if (gamepad2.dpad_right && !gamepad2.dpad_left) { + slidesMotorPosition = DRIFTConstants.SLIDES_MOTOR_LOW_BASKET_POSITION; + dumperServoPosition = DRIFTConstants.DUMPER_SERVO_POSITION_HORIZONTAL; + clearList(slidesMotorPosition); + clearList(dumperServoPosition); + } + + // Resets slide encoder if left stick button is pressed + //shouldResetSlideEncoder = slideEncoderResetToggle.getToggleState(gamepad2.left_stick_button); + + + // hacky crap code + // lets you toggle dumper with one single button (left bumper) + if (oldLeftBumperToggleState != gamepad2.left_bumper) { + double interimDumperServoPosition = dumperServoPosition; + dumperServoPosition = gamepad2.left_bumper ? DRIFTConstants.DUMPER_SERVO_POSITION_DUMP : DRIFTConstants.DUMPER_SERVO_POSITION_HORIZONTAL; + if (interimDumperServoPosition != dumperServoPosition) { + clearList(dumperServoPosition); + } + } + oldLeftBumperToggleState = gamepad2.left_bumper; + + + // deadzone go brrt + if (Math.abs(gamepad2.right_stick_y) > 0.1) slidesMotorPosition += (int) (10 * -gamepad2.right_stick_y); + + if (gamepad2.right_stick_y != 0) { + clearList(slidesMotorPosition); + } + +/* + armElbowServoPosition = Math.abs(gamepad1.left_stick_y); + intakeServoPower = -gamepad1.right_stick_y; + slidesMotorPosition += (int) (10 * -gamepad2.left_stick_y); + if (gamepad2.right_trigger >= 0) { + armElbowServoPosition = DRIFTConstants.ARM_ELBOW_SERVO_POSITION_TRANSFER; + } + */ + // iterate through all the delayed actions, and run them if possible + for (DelayedAction delayedAction : delayedActions) { + delayedAction.runActionIfPossible(); + } + } + + public boolean isTransferring() { + return armElbowServoPosition == DRIFTConstants.ARM_ELBOW_SERVO_POSITION_TRANSFER + && slidesMotorPosition == DRIFTConstants.SLIDES_MOTOR_GROUND_POSITION + //&& dumperServoPosition == DRIFTConstants.DUMPER_SERVO_POSITION_TRANSFER + // check if it's running but account for stick drift error (OMG 6220 DRIFT SO ON BRAND YOO) + && Math.abs(intakeServoPower) > 0.05; + } + + public int getArmBaseMotorPosition() { + return armBaseMotorPosition; + } + + public int getSlidesMotorPosition() { + return slidesMotorPosition; + } + + public double getIntakeServoPower() { + return intakeServoPower; + } + + public double getDumperServoPosition() { + return dumperServoPosition; + } + + public double getArmElbowServoPosition() { + return armElbowServoPosition; + } + + public boolean shouldResetSlideEncoder() { + return shouldResetSlideEncoder; + } + + + private class ProtectedButton { + private boolean wasPressed; + + public ProtectedButton() { + this.wasPressed = false; + } + + public boolean getToggleState(boolean protectedCondition) { + // check that it isnt being held down + boolean protectedOutput = protectedCondition && !wasPressed; + // update button to being pressed + this.wasPressed = protectedCondition; + return protectedOutput; + } + } + + private class DelayedAction { + private final Predicate completionCondition; + private final Runnable runnableAction; + private final double targetRunTime; + private final Object trackedObject; + + public DelayedAction(Object trackedObject, Runnable runnableAction, double delaySeconds, Predicate completionCondition) { + clearList(trackedObject); + this.completionCondition = completionCondition; + this.runnableAction = runnableAction; + this.targetRunTime = currentRunTime + delaySeconds; + this.trackedObject = trackedObject; + } + + private Object getTrackedObject() { + return trackedObject; + } + + private void runActionIfPossible() { + if (completionCondition.test(true) && currentRunTime >= targetRunTime) { + runnableAction.run(); + delayedActions.remove(this); + } + } + } + + private void addDelayedAction(Object trackedObject, Runnable runnableAction, double delaySeconds, Predicate completionCondition) { + delayedActions.add(new DelayedAction(trackedObject, runnableAction, delaySeconds, completionCondition)); + } + + private void clearList(Object trackedObject) { + // delayedActions.removeIf(action -> action.getTrackedObject().equals(trackedObject)); + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/DRIFTConstants.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/DRIFTConstants.java new file mode 100644 index 000000000000..5b52ebbf8dcf --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/DRIFTConstants.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.team6220; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; + +@Config +public abstract class DRIFTConstants { + public static double ARM_ELBOW_SERVO_POSITION_OVER_BAR = 0.5; + public static double ARM_ELBOW_SERVO_POSITION_GROUND = 0.7; + public static double ARM_ELBOW_SERVO_POSITION_TRANSFER = 0.46; + public static int ARM_BASE_MOTOR_POSITION_GROUND = -840; + public static int ARM_BASE_MOTOR_POSITION_INIT = 0; + public static int ARM_BASE_MOTOR_POSITION_OUT = -400; + public static int ARM_BASE_MOTOR_POSITION_TRANSFER = -270; + public static int ARM_BASE_MOTOR_POSITION_OVER_BAR = -1450; + + public static int SLIDES_MOTOR_GROUND_POSITION = 0; + //public static int SLIDES_MOTOR_LOW_BASKET_POSITION = 3700; + public static int SLIDES_MOTOR_LOW_BASKET_POSITION = 2000; + public static int SLIDES_MOTOR_HIGH_BASKET_POSITION = 3850; + public static int SLIDES_MOTOR_OVER_HIGH_CHAMBER_POSITION = 2500; + public static int SLIDES_MOTOR_HIGH_CHAMBER_POSITION = 1500; + + public static double DUMPER_SERVO_POSITION_DUMP = 0.9; + public static double DUMPER_SERVO_POSITION_TRANSFER = 0.40; + public static double DUMPER_SERVO_POSITION_HORIZONTAL = 0.5; + public static double DUMPER_SERVO_POSITION_INIT = 0.2; + public static final float[] RED_COLOR_SENSOR_RANGES = {0.025f, 1f}; + public static final float[] BLUE_COLOR_SENSOR_RANGES = {0.025f, 1f}; + public static final float COLOR_SENSOR_GAIN = 3.8f; + //public static final Pose2d BASKET_SCORING_POSE = new Pose2d() + public static final double ROBOT_LENGTH_IN = 16.50; + public static final Pose2d MIDDLE_STARTING_POSE = new Pose2d(0, 72-(ROBOT_LENGTH_IN/2), (3 * Math.PI)/2); + public static final Pose2d LEFT_STARTING_POSE = new Pose2d(24, 72-(ROBOT_LENGTH_IN/2), (3 * Math.PI)/2); + public static final Pose2d RIGHT_STARTING_POSE = new Pose2d(-20, 72-(ROBOT_LENGTH_IN/2), (3 * Math.PI)/2); + public static final Pose2d MIDDLE_STARTING_POSE_SPECIMEN = new Pose2d(0, 72-(ROBOT_LENGTH_IN/2), Math.PI/2); + public static final Pose2d LEFT_STARTING_POSE_SPECIMEN = new Pose2d(24, 72-(ROBOT_LENGTH_IN/2), Math.PI/2); + public static final Pose2d RIGHT_STARTING_POSE_SPECIMEN = new Pose2d(-20, 72-(ROBOT_LENGTH_IN/2), Math.PI/2); + public static final Vector2d SUBMERSIBLE_PARK_POSITION = new Vector2d(20, 12); + public static final Vector2d OBSERVATION_PARK_POSITION = new Vector2d(-60, 60); + + // hardware identifiers + public static final String ARM_ELBOW_SERVO_HARDWARE_IDENTIFIER = "armElbowServo"; + public static final String SLIDES_MOTOR_HARDWARE_IDENTIFIER = "slidesMotor"; + public static final String ARM_BASE_MOTOR_HARDWARE_IDENTIFIER = "armBaseMotor"; + public static final String INTAKE_SERVO_HARDWARE_IDENTIFIER = "intakeServo"; + public static final String DUMPER_SERVO_HARDWARE_IDENTIFIER = "dumperServo"; + public static final String OTOS_HARDWARE_IDENTIFIER = "otos"; + public static final String PINPOINT_ODOMETRY_HARDWARE_IDENTIFIER = "pinpoint"; + public static final String LEFT_FRONT_MOTOR_HARDWARE_IDENTIFIER = "leftFront"; + public static final String LEFT_BACK_MOTOR_HARDWARE_IDENTIFIER = "leftBack"; + public static final String RIGHT_BACK_MOTOR_HARDWARE_IDENTIFIER = "rightBack"; + public static final String RIGHT_FRONT_MOTOR_HARDWARE_IDENTIFIER = "rightFront"; +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/ResetADB.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/ResetADB.java new file mode 100644 index 000000000000..b355fef906ab --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/ResetADB.java @@ -0,0 +1,23 @@ +package org.firstinspires.ftc.team6220; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import java.io.IOException; + +@TeleOp(name = "Reset ADB", group = "Utility") +public class ResetADB extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + telemetry.addLine("Press ▶ to reset the ADB connection."); + telemetry.update(); + waitForStart(); + + try { + // Restart the ADB daemon: + Runtime.getRuntime().exec("setprop ctl.restart adbd"); + } catch (IOException e) { + throw new RuntimeException(e); + } + } +} + diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/WilyConfig.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/WilyConfig.java new file mode 100644 index 000000000000..ababe7578344 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/WilyConfig.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.team6220; + +import com.wilyworks.common.Wily; +import com.wilyworks.common.WilyWorks; + +/** + * This customizes the configuration of the Wily Works simulator to match your robot. + * + * @noinspection unused + */ +@Wily +public class WilyConfig extends WilyWorks.Config { + WilyConfig() { + // Impersonate the DevBot when running the simulator: + deviceName = "6220-D-RC"; + + // Use these dimensions for the robot: + robotWidth = 16; + robotLength = 16.5; + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/HoverableMenuElement.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/HoverableMenuElement.java new file mode 100644 index 000000000000..14c40bf23053 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/HoverableMenuElement.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.team6220.javatextmenu; + +/** + * An interface for interactable elements that can be shoved into the menu. + */ +public interface HoverableMenuElement extends MenuElement { + + // start and stop hovering between elements + public void showHover(boolean showHover); + + // allow it to take input + public void updateWithInput(MenuInput input); + + // for checking if the entire menu is complete + public boolean isCompleted(); + + // get the result of the element once the menu is complete + public T result(); +} \ No newline at end of file diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuElement.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuElement.java new file mode 100644 index 000000000000..f759065eba14 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuElement.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.team6220.javatextmenu; + +/** + * A general interface for anything that can be shoved into the menu. + */ +public interface MenuElement { + + // for rendering each element in the whole menu + public String getAsString(); +} \ No newline at end of file diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuFinishedButton.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuFinishedButton.java new file mode 100644 index 000000000000..ba5c1abc0f8a --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuFinishedButton.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.team6220.javatextmenu; + +/** + * A type of menu element that can stop the menu from registering as completed before the + * user is sure they have selected the right options. Once pressed, it will count as completed. + * Not meant to be read from; Menu will not be able to finish if there is more than one. + */ +public class MenuFinishedButton implements HoverableMenuElement { + + private boolean isHovered = false, isPressed = false; + + /** + * creates a new finish menu button. + */ + public MenuFinishedButton() {} + + // MenuElement interface required methods + + public String getAsString() { + return (this.isHovered ? "➤" : " ") + "[[ Finish Menu ]]"; + } + + // HoverableMenuElement interface required methods + + public void showHover(boolean showHover) { + this.isHovered = showHover; + this.isPressed = false; // so it can only be pressed last + } + + public void updateWithInput(MenuInput input) { + // once pressed stay pressed (while being hovered) + this.isPressed = this.isPressed || input.getSelect(); + } + + public boolean isCompleted() { + return this.isPressed; + } + + public Void result() { + // this element doesn't have anything to return + // but is required to because of the HoverableMenuElement interface + return null; + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuHeader.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuHeader.java new file mode 100644 index 000000000000..099737bd9e05 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuHeader.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.team6220.javatextmenu; + +/** + * A type of menu element that displays text. + */ +public class MenuHeader implements MenuElement { + + public String text; + + public MenuHeader(String text) { + this.text = text; + } + + // MenuElement interface required methods + + public String getAsString() { + return this.text; + } +} \ No newline at end of file diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuInput.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuInput.java new file mode 100644 index 000000000000..9ef5c0e02418 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuInput.java @@ -0,0 +1,206 @@ +package org.firstinspires.ftc.team6220.javatextmenu; + +/** + * A class for bridging input methods to a text menu. + *

+ * This is made with the intention of supporting controllers! + * That means that when passing input, one should set the input + * type to {@code InputType.CONTROLLER} in the constructor. The user can + * then pass raw input into the {@code .update(...)} method and it + * will be accordingly processed. + */ +public class MenuInput { + + // determines the type of input processing + // raw is good for standalone console debugging via keyboard + private final InputType inputType; + public enum InputType { + CONTROLLER, + RAW, + } + + // a constant for stick deadzone + public static final double INPUT_DEADZONE = 0.5; + // constants for spacing out sustained input (in seconds) + private static final double STICK_TAP_COOLDOWN = 0.3; + private static final double STICK_HOLD_COOLDOWN = 0.1; + + // processed values + private int x, y; + // input timer to stop input spamming + private double stickTimer; // in seconds + // switches on when past tap cooldown, then using hold cooldown + private boolean isHoldingStick = false; + // time passed between update calls to update timers with + private double deltaTime = 0.0; + private Long lastTime = null; + + // processed value + private boolean select; + // so it only registers once per held press + private boolean hasAlreadySelected = false; + + /** + * creates a new input processing object. + * @param inputType determines the type of input processing + */ + public MenuInput(InputType inputType) { + this.inputType = inputType; + } + /** + * creates a new input processing object. + * defaults to raw input type. + */ + public MenuInput() { + this(InputType.RAW); + } + + /** + * updates the input values. considers current input type. + * converts to and sums x,y inputs. + * @param x current raw x stick input + * @param y current raw y stick input + * @param xLeft dPad left pressed + * @param xRight dPad right pressed + * @param yDown dPad down pressed + * @param yUp dPad up pressed + * @param select current raw select input + */ + public MenuInput update(double x, double y, boolean xLeft, boolean xRight, boolean yDown, boolean yUp, boolean select) { + double xVal = clamp((xLeft ? -1 : 0) + (xRight ? 1 : 0) + x, -1, 1); + double yVal = clamp((yDown ? -1 : 0) + (yUp ? 1 : 0) + y, -1, 1); + return this.update(xVal, yVal, select); + } + /** + * updates the input values. considers current input type. + * converts to x,y inputs. + * @param xLeft dPad left pressed + * @param xRight dPad right pressed + * @param yDown dPad down pressed + * @param yUp dPad up pressed + * @param select current raw select input + */ + public MenuInput update(boolean xLeft, boolean xRight, boolean yDown, boolean yUp, boolean select) { + double xVal = (xLeft ? -1 : 0) + (xRight ? 1 : 0); + double yVal = (yDown ? -1 : 0) + (yUp ? 1 : 0); + return this.update(xVal, yVal, select); + } + /** + * updates the input values. considers current input type. + * @param x current raw x stick input + * @param y current raw y stick input + * @param select current raw select input + */ + public MenuInput update(double x, double y, boolean select) { + + // reset all + this.select = false; + this.x = 0; + this.y = 0; + + switch (this.inputType) { + + case RAW: + + // directly apply the values + this.select = select; + this.x = (int)x; + this.y = (int)y; + + break; + + case CONTROLLER: + + updateDeltaTime(); + + // only register one select per sustained input: + // initial select + if (!this.hasAlreadySelected && select) { + this.select = true; + this.hasAlreadySelected = true; + // if select button isn't held anymore reset it + } else if (this.hasAlreadySelected && !select) { + this.hasAlreadySelected = false; + } + + // get new x,y input; consider deadzone + if (Math.hypot(x, y) > INPUT_DEADZONE) { + // snap input vector to axis of greatest magnitude component + if (Math.abs(x) >= Math.abs(y)) { + this.x = (int)Math.signum(x); + } else { + this.y = (int)Math.signum(y); + } + } else { + // stopped holding stick, reset timer and held status + this.isHoldingStick = false; + this.stickTimer = 0; + } + + // slightly cursed stick input spacing implementation: + // (sustained input -> repeated but spaced out input after an initial pause) + + // if it's the initial stick input: + if (!this.isHoldingStick && this.stickTimer <= 0) { + if (Math.hypot(x, y) > INPUT_DEADZONE) { + // only starting adding deltatime if the stick is held + this.stickTimer += this.deltaTime; + } + // allows the x and y values to pass through + } else { + this.stickTimer += this.deltaTime; + + // if it's held for long enough to pass the tap cooldown + if (!this.isHoldingStick && this.stickTimer >= STICK_TAP_COOLDOWN) { + this.stickTimer = this.stickTimer % STICK_TAP_COOLDOWN; + this.isHoldingStick = true; // changes to holding cooldown mode + // allows the x and y values to pass through + + // if it's past the tap cooldown and is also past the hold cooldown + } else if (this.isHoldingStick && this.stickTimer >= STICK_HOLD_COOLDOWN) { + this.stickTimer = this.stickTimer % STICK_HOLD_COOLDOWN; + // allows the x and y values to pass through + + } else { + // otherwise block the x and y values + this.x = 0; + this.y = 0; + } + } + + break; + } + return this; + } + + /** + * checks if there are any active inputs being used + * @return if inputs are active + */ + public boolean isActive() { + return Math.abs(this.x) > 0 || Math.abs(this.y) > 0 || this.select; + } + + // updates the deltatime in seconds + private void updateDeltaTime() { + if (this.lastTime != null) { + this.deltaTime = (double)(System.nanoTime() - this.lastTime) / 1_000_000_000.0; + } + this.lastTime = System.nanoTime(); + } + + public int getX() { + return this.x; + } + public int getY() { + return this.y; + } + public boolean getSelect() { + return this.select; + } + + // clamps value between a minimum and maximum value + private static double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } +} \ No newline at end of file diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuSelection.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuSelection.java new file mode 100644 index 000000000000..36327fb8dece --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuSelection.java @@ -0,0 +1,79 @@ +package org.firstinspires.ftc.team6220.javatextmenu; + +import java.util.EnumSet; + +/** + * A type of menu element for choosing enum states. + */ +public class MenuSelection> implements HoverableMenuElement { + + public int selectedIndex = 0; + public int hoverIndex = -1; + private Object[] options; + private Class enumClass; + + // border formats for hovered and selected options + private static final String[] borders = {" x "," ➤x "," [x]","➤[x]"}; + + /** + * creates a new enum selector using an enum type + * param requires that the class type is of an enum + * @param enumClass the class of the enum (do myEnum.class) + */ + public MenuSelection(Class enumClass) { + this.enumClass = enumClass; + // get a list of all of the enum states from the enum group + Object[] options = EnumSet.allOf(enumClass).toArray(); + if (options == null || options.length < 1) { + throw new IllegalArgumentException( + "Enum must have at least one option" + ); + } + this.options = options; + } + + // MenuElement interface required methods + + // render the selection and hover into a string to display + public String getAsString() { + String asString = ""; + for (int i = 0; i < options.length; i++) { + // find border format index to use by adding value of hover/select + // 0 is nothing, 1 is hover, 2 is selected, 3 is both + int borderValue = (i == hoverIndex ? 1 : 0) + + (i == selectedIndex ? 2 : 0); + asString += borders[borderValue].replace("x", options[i].toString()); + } + return asString; + } + + // HoverableMenuElement interface required methods + + public void showHover(boolean showHover) { + this.hoverIndex = showHover ? Math.min(0, this.hoverIndex) : -1; + } + + public void updateWithInput(MenuInput input) { + this.hoverIndex = clamp(this.hoverIndex + input.getX(), 0, this.options.length-1); + if (input.getSelect()) { + this.selectedIndex = this.hoverIndex; + } + } + + public boolean isCompleted() { + return selectedIndex != -1; + } + + public E result() { + try { + return Enum.valueOf(this.enumClass, this.options[this.selectedIndex].toString()); + } catch (Exception e) { + return null; + } + } + + // clamps value between a minimum and maximum value + private static int clamp(int value, int min, int max) { + return Math.max(min, Math.min(max, value)); + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuSlider.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuSlider.java new file mode 100644 index 000000000000..a29518b584d1 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuSlider.java @@ -0,0 +1,76 @@ +package org.firstinspires.ftc.team6220.javatextmenu; + +/** + * A type of menu element for choosing a value inside a range. + */ +public class MenuSlider implements HoverableMenuElement { + + private boolean isHovered; + private double value, min, max, scale; + + /** + * creates a new slider. + * @param min the minimum value allowed + * @param max the maximum value allowed + * @param scale scales the units per character when displayed + * @param defaultValue the starting value, instead of using minimum + */ + public MenuSlider(double min, double max, double scale, double defaultValue) { + this.min = min; + this.max = max; + this.scale = scale; + this.value = defaultValue; + } + /** + * creates a new slider. + * @param min the minimum value allowed + * @param max the maximum value allowed + * @param scale scales the units per character when displayed + */ + public MenuSlider(double min, double max, double scale) { + this(min, max, scale, min); + } + /** + * creates a new slider. + * @param min the minimum value allowed + * @param max the maximum value allowed + */ + public MenuSlider(double min, double max) { + this(min, max, 1.0, min); + } + + // MenuElement interface required methods + + public String getAsString() { + String asString = isHovered ? "➤[" : " ["; + double sliderLength = (this.max - this.min) * this.scale; + for (int i = 0; i < Math.round(sliderLength); i++) { + asString += i < (this.value - this.min) * this.scale ? "/" : "-"; + } + return asString + "] " + (Math.round(this.value * 10.0) / 10.0); + } + + // HoverableMenuElement interface required methods + + public void showHover(boolean showHover) { + this.isHovered = showHover; + } + + public void updateWithInput(MenuInput input) { + double nextValue = this.value + input.getX() / this.scale; + this.value = clamp(nextValue, this.min, this.max); + } + + public boolean isCompleted() { + return true; + } + + public Double result() { + return this.value; + } + + // clamps value between a minimum and maximum value + private static double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuSwitch.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuSwitch.java new file mode 100644 index 000000000000..6f308b852929 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/MenuSwitch.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.team6220.javatextmenu; + +/** + * A type of menu element for choosing booleans, switching between on and off. + */ +public class MenuSwitch implements HoverableMenuElement { + + private boolean isHovered = false, switchState; + private String[] optionNames = {"False", "True"}; + + /** + * creates a new boolean switch. + * @param defaultState the starting state of the switch + * @param falseName name of the false option + * @param trueName name of the true option + */ + public MenuSwitch(boolean defaultState, String falseName, String trueName) { + this(defaultState); + this.optionNames = new String[] {falseName, trueName}; + } + /** + * creates a new boolean switch. + * @param defaultState the starting state of the switch + */ + public MenuSwitch(boolean defaultState) { + this.switchState = defaultState; + } + /** + * creates a new boolean switch. + */ + public MenuSwitch() { + this(false); + } + + // MenuElement interface required methods + + public String getAsString() { + return (this.isHovered ? "➤" : " ") + + (this.switchState ? " "+optionNames[0]+" | ["+optionNames[1]+"]" : + "["+optionNames[0]+"] | "+optionNames[1]+" "); + } + + // HoverableMenuElement interface required methods + + public void showHover(boolean showHover) { + this.isHovered = showHover; + } + + public void updateWithInput(MenuInput input) { + if (input.getSelect()) { + this.switchState = !this.switchState; + } + } + + public boolean isCompleted() { + return true; + } + + public Boolean result() { + return this.switchState; + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/TextMenu.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/TextMenu.java new file mode 100644 index 000000000000..69b4991b01b2 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/javatextmenu/TextMenu.java @@ -0,0 +1,291 @@ +package org.firstinspires.ftc.team6220.javatextmenu; + + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.LinkedHashMap; +import java.util.NoSuchElementException; + +/** + * A highly advanced, very cool and definitely + * professionally coded text-based menu predominantly + * aimed at choosing between enums via a controller. + *

+ * By valsei!! [https://github.com/valsei/java-text-menu] + *

+ * Some examples and documentation can be found on the readme in github. + */ +public class TextMenu { + + // entire list of elements, including both hoverable and not, used in printing all in order + private ArrayList elements = new ArrayList<>(); + + // map type used in preventing element name duplicates, linked for indexing order + private LinkedHashMap> hoverableElements = new LinkedHashMap<>(); + + // map that stores each rendered element to prevent rerendering every loop cycle + private HashMap elementRenderCache = new HashMap<>(); + + // index of currently hovered element + private int hoverRow = 0; + + // scroll position + private int scrollPos = -1; + // number of rows that will be displayed at a time + private int viewHeight = 0; + // number of rows to pad the scroll view + private int viewMargin = 0; + + /** + * creates an empty TextMenu. use {@code .add()} to add elements. + */ + public TextMenu() {} + /** + * creates an empty TextMenu. use {@code .add()} to add elements + * @param viewHeight number of rows to show; show all with 0 + */ + public TextMenu(int viewHeight, int viewMargin) { + if (viewHeight < 0 || viewMargin < 0) { + throw new IllegalArgumentException("Menu view params must be greater than or equal to 0!"); + } + this.viewHeight = viewHeight; + this.viewMargin = viewMargin; + } + + // this is used to make my code gooder (enabling / disabling options based on prev choices) + + public > TextMenu addEnumConditional(String name, Class enumClass, boolean isEnabled) { + if (isEnabled) { + this.add(name, new MenuSelection(enumClass)); + } + return this; + } + + /** + * adds a HoverableMenuElement to the end of the menu. + * @param name a unique internal name for the element; used in retrieving result later + * @param element any HoverableMenuElement implementing object (ex. MenuSelection) + * @return returns itself so you can chain {@code .add()} methods + */ + public TextMenu add(String name, HoverableMenuElement element) { + this.elements.add(element); + if (this.hoverableElements.containsKey(name)) { + throw new IllegalArgumentException( + "The menu already contains an element with name: " + name + ); + } + this.hoverableElements.put(name, element); + // show starting hover + this.updateWithInput(new MenuInput().update(0, 1, false)); + return this; + } + /** + * adds a MenuElement to the end of the menu. + * note that HoverableMenuElements require a name parameter. + * @param element any MenuElement implementing object (ex. MenuHeader) + * @return returns itself so you can chain {@code .add()} methods + */ + public TextMenu add(MenuElement element) { + if (element instanceof HoverableMenuElement) { + throw new IllegalArgumentException( + "This is a HoverableMenuElement, so it must have an identifier name! ex. menu.add(name, element)" + ); + } + this.elements.add(element); + return this; + } + /** + * adds an enum selector section to the end of the menu. + * @param requires that the class type is of an enum + * @param name a unique internal name for the element; used in retrieving result later + * @param enumClass the class of the enum (do {@code myEnum.class}) + * @return returns itself so you can chain {@code .add()} methods + */ + public > TextMenu add(String name, Class enumClass) { + return this.add(name, new MenuSelection(enumClass)); + } + /** + * adds a text section to the end of the menu. + * @param text any string of text + * @return returns itself so you can chain {@code .add()} methods + */ + public TextMenu add(String text) { + return this.add(new MenuHeader(text)); + } + + public TextMenu addTextConditional(String text, boolean isDisplayed) { + if (isDisplayed) { + return this.add(text); + } + return this; + } + /** + * adds an empty line for spacing to the end of the menu. + * @return returns itself so you can chain {@code .add()} methods + */ + public TextMenu add() { + return this.add(""); + } + + /** + * passes input into the menu for navigation and selecting. + * @param input uses a MenuInput object as an inbetween + */ + public void updateWithInput(MenuInput input) { + if (!this.hoverableElements.isEmpty()) { + // update hover row from y input + if (input.getY() != 0) { + // stop hovering previous line + getMapValueAt(this.hoverRow).showHover(false); + updateRenderCacheAtHover(); + // move down to next row + this.hoverRow = clamp(this.hoverRow - input.getY(), 0, this.hoverableElements.size() - 1); + // start hovering new row + getMapValueAt(this.hoverRow).showHover(true); + // render cache will be updated in the if block below since input is active + updateScrollView(); + } + if (input.isActive()) { + // pass input into the hovered element + getMapValueAt(this.hoverRow).updateWithInput(input); + updateRenderCacheAtHover(); + } + } + } + + // updates the render cache for the element currently being hovered over + private void updateRenderCacheAtHover() { + this.elementRenderCache.put(getMapValueAt(this.hoverRow), getMapValueAt(this.hoverRow).getAsString()); + } + + // returns element inside the hoverableElement map at an index + private HoverableMenuElement getMapValueAt(int index) { + return this.hoverableElements.get(this.hoverableElements.keySet().toArray()[index]); + } + + /** + * renders the menu in its current state into a list of strings. + * should then be printed/logged using external methods. + * @return list of strings representing the menu elements + */ + public ArrayList toListOfStrings() { + + if (this.scrollPos < 0) { + updateScrollView(); + } + + ArrayList list = new ArrayList<>(); + + //for (MenuElement element : this.elements) { + for (int i = 0; i < this.elements.size(); i++) { + // only show the lines within the scrolled view + if ((i >= this.scrollPos && i < this.scrollPos + this.viewHeight) || this.viewHeight <= 0) { + MenuElement element = this.elements.get(i); + + // retrieve as string from element render cache + String asString = null; + if (this.elementRenderCache.containsKey(element)) { + asString = this.elementRenderCache.get(element); + } else { + asString = element.getAsString(); + this.elementRenderCache.put(element, asString); + } + + if (element instanceof MenuFinishedButton) { + asString += " (" + countIncompleted() + " incomplete)"; + } + + list.add(asString); + // indicate there's more rows outside of scroll view + } else if (i == this.scrollPos - 1) { + list.add("˄˄˄˄˄"); + } else if (i == this.scrollPos + this.viewHeight) { + list.add("˅˅˅˅˅"); + } + } + return list; + } + + // calculates the starting index of the scrolled view + private void updateScrollView() { + // if viewHeight is 0 (show all), lock scroll position to the top + if (this.viewHeight <= 0) { + this.scrollPos = 0; + } else { + // get the actual index that is being hovered over; snap to start/end + int elementIndex = 0; + if (this.hoverRow == 0) { + elementIndex = 0; + } else if (this.hoverRow == this.hoverableElements.size() - 1) { + elementIndex = this.elements.size() - 1; + } else { + elementIndex = this.elements.indexOf(getMapValueAt(this.hoverRow)); + } + + // yeahhhh i'm not going to explain this one + // logic drafted here: https://www.desmos.com/calculator/zazudhzflp + this.scrollPos = clamp( + (int)(clamp( + this.scrollPos + this.viewHeight / 2.0, + elementIndex - this.viewHeight / 2.0 + this.viewMargin + 1, + elementIndex + this.viewHeight / 2.0 - this.viewMargin + ) - this.viewHeight / 2.0), + 0, + this.elements.size() - this.viewHeight + ); + } + } + + /** + * checks the result of a hoverable element using its name. + * @param the type to return as + * @param clazz the class of the type to return as + * @param name the unique internal name of the desired element + */ + public T getResult(Class clazz, String name) { + if (!this.hoverableElements.containsKey(name)) { + throw new NoSuchElementException("Could not find a menu element with the name: " + name); + } + try { + return clazz.cast(this.hoverableElements.get(name).result()); + } catch (ClassCastException e) { + throw new IllegalArgumentException("Class " + clazz + " is not the correct class for element " + name); + } + } + + /** + * checks if all the applicable menu elements have been filled out. + * recommended to include a MenuFinishedButton element. + * @return boolean of if the menu is completed + */ + public boolean isCompleted() { + for (HoverableMenuElement sel : this.hoverableElements.values()) { + if (!sel.isCompleted()) { + return false; + } + } + return true; + } + + /** + * checks completion status and counts the incomplete, excludes MenuFinishedButton + * @return number of incomplete elements + */ + public int countIncompleted() { + int incomplete = 0; + for (HoverableMenuElement sel : this.hoverableElements.values()) { + if (!(sel instanceof MenuFinishedButton || sel.isCompleted())) { + incomplete++; + } + } + return incomplete; + } + + // clamps value between a minimum and maximum value + private static int clamp(int value, int min, int max) { + return Math.max(min, Math.min(max, value)); + } + private static double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } +} \ No newline at end of file diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ParkAutoLeft.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ParkAutoLeft.java new file mode 100644 index 000000000000..9f7232430b34 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ParkAutoLeft.java @@ -0,0 +1,66 @@ +package org.firstinspires.ftc.team6220.old; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + +import org.firstinspires.ftc.team6220.BaseOpMode; +import org.firstinspires.ftc.team6220.DRIFTConstants; +import org.firstinspires.ftc.team6220.roadrunner.MecanumDrive; + +/** + * This class exposes the competition version of Autonomous. As a general rule, add code to the + * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. + */ +@Disabled +@Autonomous(name="ParkAutoLeft", group="Competition", preselectTeleOp="CompetitionTeleOp") +public class ParkAutoLeft extends BaseOpMode { + + @Override + public void runOpMode() { + + Pose2d leftPose = DRIFTConstants.LEFT_STARTING_POSE; + + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, leftPose); + + // Create left parking trajectory + Action leftParkingTrajectory = drive.actionBuilder(leftPose) + .splineTo(new Vector2d(-60, 60), (Math.PI)) + .build(); + Action trajectoryAction = leftParkingTrajectory; + + // Get a preview of the trajectory's path: + Canvas previewCanvas = new Canvas(); + trajectoryAction.preview(previewCanvas); + + // Show the preview on FTC Dashboard now. + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + MecanumDrive.sendTelemetryPacket(packet); + + // Wait for Start to be pressed on the Driver Hub! + waitForStart(); + + boolean more = true; + while (opModeIsActive() && more) { + telemetry.addLine("Running Auto!"); + + // 'packet' is the object used to send data to FTC Dashboard: + packet = MecanumDrive.getTelemetryPacket(); + + // Draw the preview and then run the next step of the trajectory on top: + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + more = trajectoryAction.run(packet); + + // Only send the packet if there's more to do in order to keep the very last + // drawing up on the field once the robot is done: + if (more) + MecanumDrive.sendTelemetryPacket(packet); + telemetry.update(); + } + } +} \ No newline at end of file diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ParkAutoMiddle.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ParkAutoMiddle.java new file mode 100644 index 000000000000..fa3f34063b66 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ParkAutoMiddle.java @@ -0,0 +1,67 @@ +package org.firstinspires.ftc.team6220.old; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + +import org.firstinspires.ftc.team6220.BaseOpMode; +import org.firstinspires.ftc.team6220.DRIFTConstants; +import org.firstinspires.ftc.team6220.roadrunner.MecanumDrive; + +/** + * This class exposes the competition version of Autonomous. As a general rule, add code to the + * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. + */ + +@Disabled +@Autonomous(name="ParkAutoMiddle", group="Competition", preselectTeleOp="CompetitionTeleOp") +public class ParkAutoMiddle extends BaseOpMode { + + @Override + public void runOpMode() { + + Pose2d middlePose = DRIFTConstants.MIDDLE_STARTING_POSE; + + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, middlePose); + + // Create middle parking trajectory + Action middleParkingTrajectory = drive.actionBuilder(middlePose) + .splineTo(new Vector2d(-60, 60), (Math.PI)) + .build(); + Action trajectoryAction = middleParkingTrajectory; + + // Get a preview of the trajectory's path: + Canvas previewCanvas = new Canvas(); + trajectoryAction.preview(previewCanvas); + + // Show the preview on FTC Dashboard now. + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + MecanumDrive.sendTelemetryPacket(packet); + + // Wait for Start to be pressed on the Driver Hub! + waitForStart(); + + boolean more = true; + while (opModeIsActive() && more) { + telemetry.addLine("Running Auto!"); + + // 'packet' is the object used to send data to FTC Dashboard: + packet = MecanumDrive.getTelemetryPacket(); + + // Draw the preview and then run the next step of the trajectory on top: + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + more = trajectoryAction.run(packet); + + // Only send the packet if there's more to do in order to keep the very last + // drawing up on the field once the robot is done: + if (more) + MecanumDrive.sendTelemetryPacket(packet); + telemetry.update(); + } + } +} \ No newline at end of file diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ParkAutoRight.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ParkAutoRight.java new file mode 100644 index 000000000000..f30fd9c70605 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ParkAutoRight.java @@ -0,0 +1,67 @@ +package org.firstinspires.ftc.team6220.old; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + +import org.firstinspires.ftc.team6220.BaseOpMode; +import org.firstinspires.ftc.team6220.DRIFTConstants; +import org.firstinspires.ftc.team6220.roadrunner.MecanumDrive; + +/** + * This class exposes the competition version of Autonomous. As a general rule, add code to the + * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. + */ + +@Disabled +@Autonomous(name="ParkAutoRight", group="Competition", preselectTeleOp="CompetitionTeleOp") +public class ParkAutoRight extends BaseOpMode { + + @Override + public void runOpMode() { + + Pose2d rightPose = DRIFTConstants.RIGHT_STARTING_POSE; + + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, rightPose); + + // Create right parking trajectory + Action rightParkingTrajectory = drive.actionBuilder(rightPose) + .splineTo(new Vector2d(-60, 60), (Math.PI)) + .build(); + Action trajectoryAction = rightParkingTrajectory; + + // Get a preview of the trajectory's path: + Canvas previewCanvas = new Canvas(); + trajectoryAction.preview(previewCanvas); + + // Show the preview on FTC Dashboard now. + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + MecanumDrive.sendTelemetryPacket(packet); + + // Wait for Start to be pressed on the Driver Hub! + waitForStart(); + + boolean more = true; + while (opModeIsActive() && more) { + telemetry.addLine("Running Auto!"); + + // 'packet' is the object used to send data to FTC Dashboard: + packet = MecanumDrive.getTelemetryPacket(); + + // Draw the preview and then run the next step of the trajectory on top: + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + more = trajectoryAction.run(packet); + + // Only send the packet if there's more to do in order to keep the very last + // drawing up on the field once the robot is done: + if (more) + MecanumDrive.sendTelemetryPacket(packet); + telemetry.update(); + } + } +} \ No newline at end of file diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoLeft.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoLeft.java new file mode 100644 index 000000000000..2cc7b17b9e34 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoLeft.java @@ -0,0 +1,97 @@ +package org.firstinspires.ftc.team6220.old; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + +import org.firstinspires.ftc.team6220.BaseOpMode; +import org.firstinspires.ftc.team6220.DRIFTConstants; +import org.firstinspires.ftc.team6220.javatextmenu.MenuInput; +import org.firstinspires.ftc.team6220.javatextmenu.TextMenu; +import org.firstinspires.ftc.team6220.roadrunner.MecanumDrive; + +/** + * This class exposes the competition version of Autonomous. As a general rule, add code to the + * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. + */ +@Disabled +@Autonomous(name="ScoringAutoLeft", group="Competition", preselectTeleOp="CompetitionTeleOp") +public class ScoringAutoLeft extends BaseOpMode { + + + @Override + public void runOpMode() { + + Pose2d leftPose = DRIFTConstants.LEFT_STARTING_POSE; + + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, leftPose); + + Action leftScoringTrajectory = drive.actionBuilder(leftPose) + .splineTo(new Vector2d(48, 36), (3*Math.PI)/2) + .endTrajectory() + .splineTo(new Vector2d(48, 50), (5*Math.PI)/4) //scored a sample + .splineTo(new Vector2d(25, 10),(3*Math.PI)/2) + .splineTo(new Vector2d(28, 10),(1*Math.PI)/1) + .build(); + + Action trajectoryAction = leftScoringTrajectory; + + // Get a preview of the trajectory's path: + Canvas previewCanvas = new Canvas(); + trajectoryAction.preview(previewCanvas); + + // Show the preview on FTC Dashboard now. + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + MecanumDrive.sendTelemetryPacket(packet); + + // Wait for Start to be pressed on the Driver Hub! + waitForStart(); + + boolean more = true; + while (opModeIsActive() && more) { + telemetry.addLine("Running Auto!"); + + // 'packet' is the object used to send data to FTC Dashboard: + packet = MecanumDrive.getTelemetryPacket(); + + // Draw the preview and then run the next step of the trajectory on top: + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + more = trajectoryAction.run(packet); + + // Only send the packet if there's more to do in order to keep the very last + // drawing up on the field once the robot is done: + if (more) + MecanumDrive.sendTelemetryPacket(packet); + telemetry.update(); + } + } + + private void textMenuUpdateUntilComplete(TextMenu textMenu, MenuInput input) { + while (!textMenu.isCompleted() && !isStopRequested()) { + for (String line : textMenu.toListOfStrings()) { + telemetry.addLine(line); + } + telemetry.update(); + + input.update( + gamepad1.left_stick_x, gamepad1.left_stick_y, + gamepad1.dpad_left, gamepad1.dpad_right, + gamepad1.dpad_down, gamepad1.dpad_up, + gamepad1.a + ); + textMenu.updateWithInput(input); + sleep(17); + } + } + + private void runIfNotNull (Class enumClass, Runnable runnable) { + if (enumClass != null) { + + } + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoLeftPushbot.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoLeftPushbot.java new file mode 100644 index 000000000000..e82017ac8ccd --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoLeftPushbot.java @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.team6220.old; + +import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + +import org.firstinspires.ftc.team6220.DRIFTConstants; + +@Disabled +@Autonomous(name = "ScoringAutoLeftPushbot", group = "Competition", preselectTeleOp = "CompetitionTeleOp") +public class ScoringAutoLeftPushbot extends ScoringAutoMiddlePushbot{ + + @Override + protected Pose2d getInitializationPose() { + return DRIFTConstants.LEFT_STARTING_POSE; + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoMiddle.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoMiddle.java new file mode 100644 index 000000000000..830fb846d230 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoMiddle.java @@ -0,0 +1,99 @@ +package org.firstinspires.ftc.team6220.old; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + +import org.firstinspires.ftc.team6220.BaseOpMode; +import org.firstinspires.ftc.team6220.DRIFTConstants; +import org.firstinspires.ftc.team6220.javatextmenu.MenuInput; +import org.firstinspires.ftc.team6220.javatextmenu.TextMenu; +import org.firstinspires.ftc.team6220.roadrunner.MecanumDrive; + +/** + * This class exposes the competition version of Autonomous. As a general rule, add code to the + * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. + */ + +@Disabled +@Autonomous(name="ScoringAutoMiddle", group="Competition", preselectTeleOp="CompetitionTeleOp") +public class ScoringAutoMiddle extends BaseOpMode { + + + @Override + public void runOpMode() { + + Pose2d middlePose = DRIFTConstants.MIDDLE_STARTING_POSE; + + + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, middlePose); + + Action middleScoringTrajectory = drive.actionBuilder(middlePose) + .splineTo(new Vector2d(48, 36), (3*Math.PI)/2) + .endTrajectory() + .splineTo(new Vector2d(48, 50), (5*Math.PI)/4) //scored a sample + .splineTo(new Vector2d(25, 10),(3*Math.PI)/2) + .splineTo(new Vector2d(28, 10),(1*Math.PI)/1) + .build(); + + Action trajectoryAction = middleScoringTrajectory; + + // Get a preview of the trajectory's path: + Canvas previewCanvas = new Canvas(); + trajectoryAction.preview(previewCanvas); + + // Show the preview on FTC Dashboard now. + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + MecanumDrive.sendTelemetryPacket(packet); + + // Wait for Start to be pressed on the Driver Hub! + waitForStart(); + + boolean more = true; + while (opModeIsActive() && more) { + telemetry.addLine("Running Auto!"); + + // 'packet' is the object used to send data to FTC Dashboard: + packet = MecanumDrive.getTelemetryPacket(); + + // Draw the preview and then run the next step of the trajectory on top: + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + more = trajectoryAction.run(packet); + + // Only send the packet if there's more to do in order to keep the very last + // drawing up on the field once the robot is done: + if (more) + MecanumDrive.sendTelemetryPacket(packet); + telemetry.update(); + } + } + + private void textMenuUpdateUntilComplete(TextMenu textMenu, MenuInput input) { + while (!textMenu.isCompleted() && !isStopRequested()) { + for (String line : textMenu.toListOfStrings()) { + telemetry.addLine(line); + } + telemetry.update(); + + input.update( + gamepad1.left_stick_x, gamepad1.left_stick_y, + gamepad1.dpad_left, gamepad1.dpad_right, + gamepad1.dpad_down, gamepad1.dpad_up, + gamepad1.a + ); + textMenu.updateWithInput(input); + sleep(17); + } + } + + private void runIfNotNull (Class enumClass, Runnable runnable) { + if (enumClass != null) { + + } + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoMiddlePushbot.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoMiddlePushbot.java new file mode 100644 index 000000000000..3ad41b5f09a8 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoMiddlePushbot.java @@ -0,0 +1,127 @@ +package org.firstinspires.ftc.team6220.old; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + +import org.firstinspires.ftc.team6220.BaseOpMode; +import org.firstinspires.ftc.team6220.DRIFTConstants; +import org.firstinspires.ftc.team6220.javatextmenu.MenuInput; +import org.firstinspires.ftc.team6220.javatextmenu.TextMenu; +import org.firstinspires.ftc.team6220.roadrunner.MecanumDrive; + +/** + * This class exposes the competition version of Autonomous. As a general rule, add code to the + * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. + */ + +@Disabled +@Autonomous(name="ScoringAutoMiddlePushbot", group="Competition", preselectTeleOp="CompetitionTeleOp") +public class ScoringAutoMiddlePushbot extends BaseOpMode { + + + @Override + public void runOpMode() { + + // overridden in subclasses, middle by default + Pose2d startingPose = getInitializationPose(); + + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startingPose); + + Action middleScoringTrajectory = drive.actionBuilder(startingPose) + // strafe to scoring area + .strafeTo(new Vector2d(55, 60)) + .endTrajectory() + .setTangent(Math.toRadians(-180)) + // spline to prepare to collect first sample + .splineToLinearHeading(new Pose2d(45, 10, Math.toRadians(-90)), Math.toRadians(-40)) + .endTrajectory() + .setTangent(Math.toRadians(90)) + // spline to deposit first sample in scoring area + .splineToLinearHeading(new Pose2d(55, 55, Math.toRadians(-140)), Math.toRadians(40)) + .endTrajectory() + .setTangent(Math.toRadians(-180)) + // spline to prepare to collect second sample + .splineToLinearHeading(new Pose2d(55, 10, Math.toRadians(-90)), Math.toRadians(-40)) + .endTrajectory() + .setTangent(Math.toRadians(90)) + // spline to deposit second sample in scoring area + .splineToLinearHeading(new Pose2d(55, 61, Math.toRadians(-140)), Math.toRadians(40)) + .endTrajectory() + .setTangent(Math.toRadians(-180)) + // spline to prepare to collect third sample + .splineToLinearHeading(new Pose2d(62, 10, Math.toRadians(-90)), Math.toRadians(40)) + .endTrajectory() + // strafe to deposit third sample + .strafeTo(new Vector2d(62, 55)) + // prepare to move to park position + .strafeTo(new Vector2d(62, 50)) + .waitSeconds(3) + // drive to park position + .splineTo(new Vector2d(-55,60), Math.toRadians(180)) + .build(); + Action trajectoryAction = middleScoringTrajectory; + + // Get a preview of the trajectory's path: + Canvas previewCanvas = new Canvas(); + trajectoryAction.preview(previewCanvas); + + // Show the preview on FTC Dashboard now. + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + MecanumDrive.sendTelemetryPacket(packet); + + // Wait for Start to be pressed on the Driver Hub! + waitForStart(); + + boolean more = true; + while (opModeIsActive() && more) { + telemetry.addLine("Running Auto!"); + + // 'packet' is the object used to send data to FTC Dashboard: + packet = MecanumDrive.getTelemetryPacket(); + + // Draw the preview and then run the next step of the trajectory on top: + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + more = trajectoryAction.run(packet); + + // Only send the packet if there's more to do in order to keep the very last + // drawing up on the field once the robot is done: + if (more) + MecanumDrive.sendTelemetryPacket(packet); + telemetry.update(); + } + } + + private void textMenuUpdateUntilComplete(TextMenu textMenu, MenuInput input) { + while (!textMenu.isCompleted() && !isStopRequested()) { + for (String line : textMenu.toListOfStrings()) { + telemetry.addLine(line); + } + telemetry.update(); + + input.update( + gamepad1.left_stick_x, gamepad1.left_stick_y, + gamepad1.dpad_left, gamepad1.dpad_right, + gamepad1.dpad_down, gamepad1.dpad_up, + gamepad1.a + ); + textMenu.updateWithInput(input); + sleep(17); + } + } + + private void runIfNotNull (Class enumClass, Runnable runnable) { + if (enumClass != null) { + + } + } + + protected Pose2d getInitializationPose() { + return DRIFTConstants.MIDDLE_STARTING_POSE; + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoRight.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoRight.java new file mode 100644 index 000000000000..03d6207e27a6 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoRight.java @@ -0,0 +1,99 @@ +package org.firstinspires.ftc.team6220.old; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + +import org.firstinspires.ftc.team6220.BaseOpMode; +import org.firstinspires.ftc.team6220.DRIFTConstants; +import org.firstinspires.ftc.team6220.javatextmenu.MenuInput; +import org.firstinspires.ftc.team6220.javatextmenu.TextMenu; +import org.firstinspires.ftc.team6220.roadrunner.MecanumDrive; + +/** + * This class exposes the competition version of Autonomous. As a general rule, add code to the + * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. + */ + +@Disabled +@Autonomous(name="ScoringAutoRight", group="Competition", preselectTeleOp="CompetitionTeleOp") +public class ScoringAutoRight extends BaseOpMode { + + + @Override + public void runOpMode() { + + Pose2d rightPose = DRIFTConstants.RIGHT_STARTING_POSE; + + + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, rightPose); + + Action rightScoringTrajectory = drive.actionBuilder(rightPose) + .splineTo(new Vector2d(48, 36), (3*Math.PI)/2) + .endTrajectory() + .splineTo(new Vector2d(48, 50), (5*Math.PI)/4) //scored a sample + .splineTo(new Vector2d(25, 10),(3*Math.PI)/2) + .splineTo(new Vector2d(28, 10),(1*Math.PI)/1) + .build(); + + Action trajectoryAction = rightScoringTrajectory; + + // Get a preview of the trajectory's path: + Canvas previewCanvas = new Canvas(); + trajectoryAction.preview(previewCanvas); + + // Show the preview on FTC Dashboard now. + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + MecanumDrive.sendTelemetryPacket(packet); + + // Wait for Start to be pressed on the Driver Hub! + waitForStart(); + + boolean more = true; + while (opModeIsActive() && more) { + telemetry.addLine("Running Auto!"); + + // 'packet' is the object used to send data to FTC Dashboard: + packet = MecanumDrive.getTelemetryPacket(); + + // Draw the preview and then run the next step of the trajectory on top: + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + more = trajectoryAction.run(packet); + + // Only send the packet if there's more to do in order to keep the very last + // drawing up on the field once the robot is done: + if (more) + MecanumDrive.sendTelemetryPacket(packet); + telemetry.update(); + } + } + + private void textMenuUpdateUntilComplete(TextMenu textMenu, MenuInput input) { + while (!textMenu.isCompleted() && !isStopRequested()) { + for (String line : textMenu.toListOfStrings()) { + telemetry.addLine(line); + } + telemetry.update(); + + input.update( + gamepad1.left_stick_x, gamepad1.left_stick_y, + gamepad1.dpad_left, gamepad1.dpad_right, + gamepad1.dpad_down, gamepad1.dpad_up, + gamepad1.a + ); + textMenu.updateWithInput(input); + sleep(17); + } + } + + private void runIfNotNull (Class enumClass, Runnable runnable) { + if (enumClass != null) { + + } + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoRightPushbot.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoRightPushbot.java new file mode 100644 index 000000000000..36106b646dc1 --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/old/ScoringAutoRightPushbot.java @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.team6220.old; + +import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + +import org.firstinspires.ftc.team6220.DRIFTConstants; + +@Disabled +@Autonomous(name = "ScoringAutoRightPushbot", group = "Competition", preselectTeleOp = "CompetitionTeleOp") +public class ScoringAutoRightPushbot extends ScoringAutoMiddlePushbot{ + + @Override + protected Pose2d getInitializationPose() { + return DRIFTConstants.RIGHT_STARTING_POSE; + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/roadrunner/ArmAction.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/roadrunner/ArmAction.java new file mode 100644 index 000000000000..a01abe4dd21a --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/roadrunner/ArmAction.java @@ -0,0 +1,61 @@ +/*// Action for controlling the arm in Auto trajectories. +class ArmAction extends RobotAction { + final double EPSILON = 3; // Ticks + + int targetArmPosition; + double targetIntakePower; + + public ArmAction(int armPosition, double intakePower) { + this.targetArmPosition = armPosition; + this.targetIntakePower = intakePower; + } + + @Override + public boolean run(double elapsedTime) { + setArmPosition(targetArmPosition); + setIntakePower(targetIntakePower); + double armDistance = Math.abs(getArmPosition() - targetArmPosition); + + // Return 'true' to call again when not at target position yet: + return (armDistance > EPSILON); + } + + // Arm positions, in ticks: + final int ARM_HOME = 0; + final int ARM_COLLECT = 2000; + final int ARM_SCORE_BASKET = 1500; + + final double INTAKE_COLLECT = 0.5; + final double INTAKE_DEPOSIT = -1.0; + final double INTAKE_OFF = 0.0; + + // Mechanism state: + DcMotorEx armMotor; + CRServo intakeCRServo; + ArmSimulator armSimulator = new ArmSimulator(); + + // Helper function for settings the arm position, in ticks: + void setArmPosition(int targetInTicks) { + if ((targetInTicks < ARM_HOME) || (targetInTicks > ARM_COLLECT)) { + throw new IllegalArgumentException("Invalid setArmPosition() request."); + } + armMotor.setTargetPosition(targetInTicks); + armSimulator.setArmPosition(targetInTicks); + } + // Helper function for querying the arm position, in ticks. Uses the simulator when running + // under Wily Works. + int getArmPosition() { + if (WilyWorks.isSimulating) + return armSimulator.getArmPosition(); + else + return armMotor.getCurrentPosition(); + } + // Helper function to set the power on the intake. + void setIntakePower(double power) { // Positive is intake, negative is out-take, zero is stop + if ((power > INTAKE_COLLECT) || (power < INTAKE_DEPOSIT)) { + throw new IllegalArgumentException("Invalid setIntakePower() request."); + } + intakeCRServo.setPower(power); + armSimulator.setIntakePower(power); + } +}*/ \ No newline at end of file diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/roadrunner/Drawing.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/roadrunner/Drawing.java new file mode 100644 index 000000000000..7f805b9edf6a --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/roadrunner/Drawing.java @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.team6220.roadrunner; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; + +public final class Drawing { + private Drawing() {} + + + public static void drawRobot(Canvas c, Pose2d t) { + final double ROBOT_RADIUS = 9; + + c.setStrokeWidth(1); + c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS); + + Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS); + Vector2d p1 = t.position.plus(halfv); + Vector2d p2 = p1.plus(halfv); + c.strokeLine(p1.x, p1.y, p2.x, p2.y); + } +} diff --git a/Team6220/src/main/java/org/firstinspires/ftc/team6220/roadrunner/Localizer.java b/Team6220/src/main/java/org/firstinspires/ftc/team6220/roadrunner/Localizer.java new file mode 100644 index 000000000000..d848a4bf7d6e --- /dev/null +++ b/Team6220/src/main/java/org/firstinspires/ftc/team6220/roadrunner/Localizer.java @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.team6220.roadrunner; + +import com.acmerobotics.roadrunner.Time; +import com.acmerobotics.roadrunner.Twist2dDual; + +public interface Localizer { + Twist2dDual

+ * Example: + *

{@code
+ *  public abstract void setTextColor(@ColorInt int color);
+ * }
+ */ +@Documented +@Retention(CLASS) +@Target({PARAMETER, METHOD, LOCAL_VARIABLE, FIELD}) +public @interface ColorInt { +} diff --git a/WilyCore/src/main/java/androidx/annotation/NonNull.java b/WilyCore/src/main/java/androidx/annotation/NonNull.java new file mode 100644 index 000000000000..c33ec8f31564 --- /dev/null +++ b/WilyCore/src/main/java/androidx/annotation/NonNull.java @@ -0,0 +1,6 @@ +package androidx.annotation; + +import java.lang.annotation.Documented; + +public @interface NonNull { +} diff --git a/WilyCore/src/main/java/androidx/annotation/Nullable.java b/WilyCore/src/main/java/androidx/annotation/Nullable.java new file mode 100644 index 000000000000..192b5e0db78e --- /dev/null +++ b/WilyCore/src/main/java/androidx/annotation/Nullable.java @@ -0,0 +1,4 @@ +package androidx.annotation; + +public @interface Nullable { +} diff --git a/WilyCore/src/main/java/androidx/annotation/RecentlyNonNull.java b/WilyCore/src/main/java/androidx/annotation/RecentlyNonNull.java new file mode 100644 index 000000000000..e7d239dd85fb --- /dev/null +++ b/WilyCore/src/main/java/androidx/annotation/RecentlyNonNull.java @@ -0,0 +1,4 @@ +package androidx.annotation; + +public @interface RecentlyNonNull { +} diff --git a/WilyCore/src/main/java/androidx/annotation/RecentlyNullable.java b/WilyCore/src/main/java/androidx/annotation/RecentlyNullable.java new file mode 100644 index 000000000000..841ad284c2f5 --- /dev/null +++ b/WilyCore/src/main/java/androidx/annotation/RecentlyNullable.java @@ -0,0 +1,4 @@ +package androidx.annotation; + +public @interface RecentlyNullable { +} diff --git a/WilyCore/src/main/java/androidx/annotation/StringRes.java b/WilyCore/src/main/java/androidx/annotation/StringRes.java new file mode 100644 index 000000000000..8f15e2c15fea --- /dev/null +++ b/WilyCore/src/main/java/androidx/annotation/StringRes.java @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2014 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package androidx.annotation; + +import static java.lang.annotation.ElementType.FIELD; +import static java.lang.annotation.ElementType.LOCAL_VARIABLE; +import static java.lang.annotation.ElementType.METHOD; +import static java.lang.annotation.ElementType.PARAMETER; +import static java.lang.annotation.RetentionPolicy.CLASS; + +import java.lang.annotation.Documented; +import java.lang.annotation.Retention; +import java.lang.annotation.Target; + +/** + * Denotes that an integer parameter, field or method return value is expected + * to be a String resource reference (e.g. {@code android.R.string.ok}). + */ +@Documented +@Retention(CLASS) +@Target({METHOD, PARAMETER, FIELD, LOCAL_VARIABLE}) +public @interface StringRes { +} diff --git a/WilyCore/src/main/java/com/acmerobotics/TimeTurn.kt b/WilyCore/src/main/java/com/acmerobotics/TimeTurn.kt new file mode 100644 index 000000000000..5107f2118632 --- /dev/null +++ b/WilyCore/src/main/java/com/acmerobotics/TimeTurn.kt @@ -0,0 +1,58 @@ +package com.acmerobotics.roadrunner + +import kotlin.math.abs + +data class TurnConstraints( + @JvmField + val maxAngVel: Double, + @JvmField + val minAngAccel: Double, + @JvmField + val maxAngAccel: Double, +) + +/** + * A time-indexed turn beginning at [beginPose] and rotating CCW [angle] radians subject to [constraints]. + * + * Note: Regardless of whether the turn is to the left or to the right, the max accel limit is applied when speeding up + * and the min accel limit is applied when speeding down. + */ +class TimeTurn( + @JvmField + val beginPose: Pose2d, + @JvmField + val angle: Double, + @JvmField + val constraints: TurnConstraints, +) { + @JvmField + val profile = TimeProfile( + constantProfile( + abs(angle), + 0.0, + constraints.maxAngVel, + constraints.minAngAccel, + constraints.maxAngAccel + ).baseProfile + ) + + @JvmField + val duration = profile.duration + + @JvmField + val reversed = angle < 0 + + operator fun get(t: Double): Pose2dDual