From 311e7eb03975c90a9f06c883b8d1c469f19df351 Mon Sep 17 00:00:00 2001 From: Isaac Hanson Date: Sun, 28 Sep 2025 20:38:04 -0400 Subject: [PATCH 01/16] Initial drive teleop --- .../ftc/teamcode/MecanumDrive.java | 6 ++- .../ftc/teamcode/SpiritTeleop.java | 51 +++++++++++++++++++ 2 files changed, 55 insertions(+), 2 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index b919194ab23..b21b30ea7e2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -58,9 +58,9 @@ public static class Params { // TODO: fill in these values based on // see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = - RevHubOrientationOnRobot.LogoFacingDirection.UP; + RevHubOrientationOnRobot.LogoFacingDirection.RIGHT; public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = - RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + RevHubOrientationOnRobot.UsbFacingDirection.UP; // drive model parameters public double inPerTick = 1; @@ -237,6 +237,8 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { // TODO: reverse motor directions if needed // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftBack.setDirection(DcMotorSimple.Direction.REVERSE); // TODO: make sure your config has an IMU with this name (can be BNO or BHI) // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java new file mode 100644 index 00000000000..9c9462d33ca --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.teamcode; + +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.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.tuning.TuningOpModes; + +@TeleOp(name="Spirit", group="Teleop") +public class SpiritTeleop extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + waitForStart(); + + while (opModeIsActive()) { + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x + ), + -gamepad1.right_stick_x + )); + + drive.updatePoseEstimate(); + + Pose2d pose = drive.localizer.getPose(); + telemetry.addData("x", pose.position.x); + telemetry.addData("y", pose.position.y); + telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); + telemetry.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), pose); + FtcDashboard.getInstance().sendTelemetryPacket(packet); + } + } else { + throw new RuntimeException(); + } + } +} From 6dff7d950a75c940cd5b56dd66b308df030f8b9f Mon Sep 17 00:00:00 2001 From: Brenda Date: Mon, 3 Nov 2025 19:26:54 -0500 Subject: [PATCH 02/16] Drive and intake working at this point --- .../firstinspires/ftc/teamcode/SpiritTeleop.java | 9 +++++++-- .../firstinspires/ftc/teamcode/robot/Intake.java | 16 ++++++++++++++++ build.gradle | 2 +- gradle/wrapper/gradle-wrapper.properties | 2 +- 4 files changed, 25 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Intake.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java index 9c9462d33ca..9985fe17f88 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java @@ -9,9 +9,10 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.robot.Intake; import org.firstinspires.ftc.teamcode.tuning.TuningOpModes; -@TeleOp(name="Spirit", group="Teleop") +@TeleOp(name = "Spirit", group = "Teleop") public class SpiritTeleop extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { @@ -19,10 +20,14 @@ public void runOpMode() throws InterruptedException { if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + Intake intake = new Intake(hardwareMap);//instantiate a new intake motor waitForStart(); while (opModeIsActive()) { + double netPower = gamepad2.right_trigger - gamepad2.left_trigger;//if the player pulls both triggers at the same time, power is netted + intake.setPower(netPower);//sets the power on the intake moter based on the values from the triggers + drive.setDrivePowers(new PoseVelocity2d( new Vector2d( -gamepad1.left_stick_y, @@ -44,7 +49,7 @@ public void runOpMode() throws InterruptedException { Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); } - } else { + } else { throw new RuntimeException(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Intake.java new file mode 100644 index 00000000000..4c30de886f8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Intake.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.teamcode.robot; + +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class Intake { + private final DcMotorEx intakeMotor; + + public Intake(HardwareMap hardwareMap) { + intakeMotor = hardwareMap.get(DcMotorEx.class, "intakeMotor"); + } + + public void setPower(double power) { + intakeMotor.setPower(power); + } +} diff --git a/build.gradle b/build.gradle index e70f2095b7a..6dd7fd3483c 100644 --- a/build.gradle +++ b/build.gradle @@ -11,7 +11,7 @@ buildscript { } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:8.7.0' + classpath 'com.android.tools.build:gradle:8.13.0' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 19cfad969ba..2733ed5dc3c 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.9-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.13-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists From 5106f7b627923053af72d5b78240c371f8ac1604 Mon Sep 17 00:00:00 2001 From: Brenda Date: Mon, 3 Nov 2025 20:40:28 -0500 Subject: [PATCH 03/16] Adding sample files for pinpoint and limelight --- .../teamcode/SensorGoBildaPinpointSpirit.java | 116 +++++++++++++ .../ftc/teamcode/SensorLimelight3ASpirit.java | 158 ++++++++++++++++++ 2 files changed, 274 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointSpirit.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorLimelight3ASpirit.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointSpirit.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointSpirit.java new file mode 100644 index 00000000000..d2ab1557cc0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointSpirit.java @@ -0,0 +1,116 @@ +/* +* Copyright (c) 2025 Alan Smith +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: + +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. + +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. +*/ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; + +/* + * This OpMode illustrates how to use the GoBildaPinpoint + * + * The OpMode assumes that the sensor is configured with a name of "pinpoint". + * + * 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 + * + * See the sensor's product page: https://www.gobilda.com/pinpoint-odometry-computer-imu-sensor-fusion-for-2-wheel-odometry/ + */ +@TeleOp(name = "Sensor: GoBilda Pinpoint", group = "Sensor") +//@Disabled +public class SensorGoBildaPinpointSpirit extends OpMode { + // Create an instance of the sensor + GoBildaPinpointDriver pinpoint; + + @Override + public void init() { + // Get a reference to the sensor + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); + + // Configure the sensor + configurePinpoint(); + + // Set the location of the robot - this should be the place you are starting the robot from + pinpoint.setPosition(new Pose2D(DistanceUnit.INCH, 0, 0, AngleUnit.DEGREES, 0)); + } + + @Override + public void loop() { + telemetry.addLine("Push your robot around to see it track"); + telemetry.addLine("Press A to reset the position"); + if(gamepad1.a){ + // You could use readings from April Tags here to give a new known position to the pinpoint + pinpoint.setPosition(new Pose2D(DistanceUnit.INCH, 0, 0, AngleUnit.DEGREES, 0)); + } + pinpoint.update(); + Pose2D pose2D = pinpoint.getPosition(); + + telemetry.addData("X coordinate (IN)", pose2D.getX(DistanceUnit.INCH)); + telemetry.addData("Y coordinate (IN)", pose2D.getY(DistanceUnit.INCH)); + telemetry.addData("Heading angle (DEGREES)", pose2D.getHeading(AngleUnit.DEGREES)); + } + + public void configurePinpoint(){ + /* + * Set the odometry pod positions relative to the point that you want the position to be measured from. + * + * The X pod offset refers to how far sideways from the tracking point the X (forward) odometry pod is. + * Left of the center is a positive number, right of center is a negative number. + * + * The Y pod offset refers to how far forwards from the tracking point the Y (strafe) odometry pod is. + * Forward of center is a positive number, backwards is a negative number. + */ + pinpoint.setOffsets(-84.0, -168.0, DistanceUnit.MM); //these are tuned for 3110-0002-0001 Product Insight #1 + + /* + * Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either + * the goBILDA_SWINGARM_POD, or the goBILDA_4_BAR_POD. + * If you're using another kind of odometry pod, uncomment setEncoderResolution and input the + * number of ticks per unit of your odometry pod. For example: + * pinpoint.setEncoderResolution(13.26291192, DistanceUnit.MM); + */ + pinpoint.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD); + + /* + * Set the direction that each of the two odometry pods count. The X (forward) pod should + * increase when you move the robot forward. And the Y (strafe) pod should increase when + * you move the robot to the left. + */ + pinpoint.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, + GoBildaPinpointDriver.EncoderDirection.FORWARD); + + /* + * Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary + * The IMU will automatically calibrate when first powered on, but recalibrating before running + * the robot is a good idea to ensure that the calibration is "good". + * resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU. + * This is recommended before you run your autonomous, as a bad initial calibration can cause + * an incorrect starting value for x, y, and heading. + */ + pinpoint.resetPosAndIMU(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorLimelight3ASpirit.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorLimelight3ASpirit.java new file mode 100644 index 00000000000..a81a942c8b7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorLimelight3ASpirit.java @@ -0,0 +1,158 @@ +/* +Copyright (c) 2024 Limelight Vision + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; + +/* + * This OpMode illustrates how to use the Limelight3A Vision Sensor. + * + * @see Limelight + * + * Notes on configuration: + * + * The device presents itself, when plugged into a USB port on a Control Hub as an ethernet + * interface. A DHCP server running on the Limelight automatically assigns the Control Hub an + * ip address for the new ethernet interface. + * + * Since the Limelight is plugged into a USB port, it will be listed on the top level configuration + * activity along with the Control Hub Portal and other USB devices such as webcams. Typically + * serial numbers are displayed below the device's names. In the case of the Limelight device, the + * Control Hub's assigned ip address for that ethernet interface is used as the "serial number". + * + * Tapping the Limelight's name, transitions to a new screen where the user can rename the Limelight + * and specify the Limelight's ip address. Users should take care not to confuse the ip address of + * the Limelight itself, which can be configured through the Limelight settings page via a web browser, + * and the ip address the Limelight device assigned the Control Hub and which is displayed in small text + * below the name of the Limelight on the top level configuration screen. + */ +@TeleOp(name = "Sensor: Limelight3A", group = "Sensor") +//@Disabled +public class SensorLimelight3ASpirit extends LinearOpMode { + + private Limelight3A limelight; + + @Override + public void runOpMode() throws InterruptedException + { + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + telemetry.setMsTransmissionInterval(11); + + limelight.pipelineSwitch(0); + + /* + * Starts polling for data. If you neglect to call start(), getLatestResult() will return null. + */ + limelight.start(); + + telemetry.addData(">", "Robot Ready. Press Play."); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) { + LLStatus status = limelight.getStatus(); + telemetry.addData("Name", "%s", + status.getName()); + telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d", + status.getTemp(), status.getCpu(),(int)status.getFps()); + telemetry.addData("Pipeline", "Index: %d, Type: %s", + status.getPipelineIndex(), status.getPipelineType()); + + LLResult result = limelight.getLatestResult(); + if (result.isValid()) { + // Access general information + Pose3D botpose = result.getBotpose(); + double captureLatency = result.getCaptureLatency(); + double targetingLatency = result.getTargetingLatency(); + double parseLatency = result.getParseLatency(); + telemetry.addData("LL Latency", captureLatency + targetingLatency); + telemetry.addData("Parse Latency", parseLatency); + telemetry.addData("PythonOutput", java.util.Arrays.toString(result.getPythonOutput())); + + telemetry.addData("tx", result.getTx());//angle in degrees on x axis from target, by default it's the center of the April tag + telemetry.addData("txnc", result.getTxNC());//not using this one + telemetry.addData("ty", result.getTy());//will need this if we can tilt the shooter + telemetry.addData("tync", result.getTyNC());//not using but tells you the number of the april tag it's looking at + + telemetry.addData("Botpose", botpose.toString()); + + // Access barcode results + List barcodeResults = result.getBarcodeResults(); + for (LLResultTypes.BarcodeResult br : barcodeResults) { + telemetry.addData("Barcode", "Data: %s", br.getData()); + } + + // Access classifier results + List classifierResults = result.getClassifierResults(); + for (LLResultTypes.ClassifierResult cr : classifierResults) { + telemetry.addData("Classifier", "Class: %s, Confidence: %.2f", cr.getClassName(), cr.getConfidence()); + } + + // Access detector results + List detectorResults = result.getDetectorResults(); + for (LLResultTypes.DetectorResult dr : detectorResults) { + telemetry.addData("Detector", "Class: %s, Area: %.2f", dr.getClassName(), dr.getTargetArea()); + } + + // Access fiducial results + List fiducialResults = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fr : fiducialResults) { + telemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f", fr.getFiducialId(), fr.getFamily(), fr.getTargetXDegrees(), fr.getTargetYDegrees()); + //21-23 are diferent oblisks (patterns) so 21 is gpp, 22 is pgp, 23 is ppg. 20 is blue and 25 is red (we think) + } + + // Access color results + List colorResults = result.getColorResults(); + for (LLResultTypes.ColorResult cr : colorResults) { + telemetry.addData("Color", "X: %.2f, Y: %.2f", cr.getTargetXDegrees(), cr.getTargetYDegrees()); + } + } else { + telemetry.addData("Limelight", "No data available"); + } + + telemetry.update(); + } + limelight.stop(); + } +} From 14669bbc3624897d0eb8d4cfba9ddd68ea5e3db3 Mon Sep 17 00:00:00 2001 From: Brenda Date: Fri, 14 Nov 2025 20:58:50 -0500 Subject: [PATCH 04/16] Adding sample files for pinpoint and limelight --- .../ftc/teamcode/SpiritTeleop.java | 81 ++++++++++++------- .../ftc/teamcode/robot/Carousel.java | 30 +++++++ .../teamcode/{ => robot}/MecanumDrive.java | 4 +- .../ftc/teamcode/robot/Shooter.java | 26 ++++++ .../ftc/teamcode/tuning/LocalizationTest.java | 2 +- .../teamcode/tuning/ManualFeedbackTuner.java | 2 +- .../ftc/teamcode/tuning/SplineTest.java | 2 +- .../ftc/teamcode/tuning/TuningOpModes.java | 2 +- 8 files changed, 117 insertions(+), 32 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => robot}/MecanumDrive.java (99%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java index 9985fe17f88..0c6cd250fb8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java @@ -9,48 +9,75 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.robot.Carousel; import org.firstinspires.ftc.teamcode.robot.Intake; +import org.firstinspires.ftc.teamcode.robot.MecanumDrive; +import org.firstinspires.ftc.teamcode.robot.Shooter; import org.firstinspires.ftc.teamcode.tuning.TuningOpModes; @TeleOp(name = "Spirit", group = "Teleop") public class SpiritTeleop extends LinearOpMode { @Override - public void runOpMode() throws InterruptedException { + public void runOpMode() { telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor + Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter + Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel waitForStart(); while (opModeIsActive()) { - double netPower = gamepad2.right_trigger - gamepad2.left_trigger;//if the player pulls both triggers at the same time, power is netted - intake.setPower(netPower);//sets the power on the intake moter based on the values from the triggers - - drive.setDrivePowers(new PoseVelocity2d( - new Vector2d( - -gamepad1.left_stick_y, - -gamepad1.left_stick_x - ), - -gamepad1.right_stick_x - )); - - drive.updatePoseEstimate(); - - Pose2d pose = drive.localizer.getPose(); - telemetry.addData("x", pose.position.x); - telemetry.addData("y", pose.position.y); - telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); - telemetry.update(); - - TelemetryPacket packet = new TelemetryPacket(); - packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), pose); - FtcDashboard.getInstance().sendTelemetryPacket(packet); + if (gamepad2.left_bumper) { + intake.setPower(1); + } else if (gamepad2.right_bumper) { + intake.setPower(-1);//sets the power on the intake motor based on the values from the bumps + } + + if (gamepad2.left_trigger > 0.25) { + shooter.setPower(1); + } else if (gamepad2.right_trigger > 0.25) { + shooter.setPower(0.5); + } + + if (gamepad2.y) { + carousel.flipFeedServo(); + } + + if(gamepad2.x) { + carousel.unflipFeedServo(); + } + + if (gamepad2.b) { + carousel.flipShooterServo(); + } + + if(gamepad2.a) { + carousel.unflipShooterServo(); + } + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x + ), + -gamepad1.right_stick_x + )); + + drive.updatePoseEstimate(); + + Pose2d pose = drive.localizer.getPose(); + telemetry.addData("x", pose.position.x); + telemetry.addData("y", pose.position.y); + telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); + telemetry.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), pose); + FtcDashboard.getInstance().sendTelemetryPacket(packet); + } } - } else { - throw new RuntimeException(); } } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java new file mode 100644 index 00000000000..3b85f8c16a9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java @@ -0,0 +1,30 @@ +package org.firstinspires.ftc.teamcode.robot; + +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +public class Carousel { + private final Servo feedServo; + private final Servo shooterServo; + private final CRServo spinServo; + + public Carousel(HardwareMap hardwareMap) { + feedServo = hardwareMap.get(Servo.class, "feedServo"); + shooterServo = hardwareMap.get(Servo.class, "shooterServo"); + spinServo = hardwareMap.get(CRServo.class, "spinServo"); + } + + public void flipFeedServo() { + feedServo.setPosition(1); + } + public void unflipFeedServo(){ + feedServo.setPosition(0); + } + public void flipShooterServo() { + shooterServo.setPosition(1); + } + public void unflipShooterServo(){ + shooterServo.setPosition(0); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/MecanumDrive.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/MecanumDrive.java index b21b30ea7e2..8bce853a995 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/MecanumDrive.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.robot; import androidx.annotation.NonNull; @@ -41,6 +41,8 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.Drawing; +import org.firstinspires.ftc.teamcode.Localizer; import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage; import org.firstinspires.ftc.teamcode.messages.MecanumCommandMessage; import org.firstinspires.ftc.teamcode.messages.MecanumLocalizerInputsMessage; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java new file mode 100644 index 00000000000..ad97311400e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.teamcode.robot; + +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class Shooter { + private final DcMotorEx shooterMotorLeft; + private final DcMotorEx shooterMotorRight; + + public Shooter(HardwareMap hardwareMap) { + shooterMotorLeft = hardwareMap.get(DcMotorEx.class, "shooterMotor"); + shooterMotorRight = hardwareMap.get(DcMotorEx.class, "shooterMotor"); + shooterMotorRight.setDirection(DcMotorSimple.Direction.REVERSE); + shooterMotorLeft.setDirection(DcMotorSimple.Direction.FORWARD); + } + + /** + * Set the power to the left and right motors + */ + public void setPower(double power) { + shooterMotorLeft.setPower(power); + shooterMotorRight.setPower(power); + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java index 151f7acb5c1..8761ad2183a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java @@ -9,7 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.Drawing; -import org.firstinspires.ftc.teamcode.MecanumDrive; +import org.firstinspires.ftc.teamcode.robot.MecanumDrive; import org.firstinspires.ftc.teamcode.TankDrive; public class LocalizationTest extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java index 0b6f1933e04..5d00451f44b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java @@ -4,7 +4,7 @@ import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.MecanumDrive; +import org.firstinspires.ftc.teamcode.robot.MecanumDrive; import org.firstinspires.ftc.teamcode.TankDrive; import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer; import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java index d70825e829b..cf4d7d1b0b3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java @@ -5,7 +5,7 @@ import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.MecanumDrive; +import org.firstinspires.ftc.teamcode.robot.MecanumDrive; import org.firstinspires.ftc.teamcode.TankDrive; public final class SplineTest extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java index ef5958db72e..62c5ce53a99 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java @@ -40,7 +40,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; import org.firstinspires.ftc.robotcore.internal.opmode.OpModeMeta; -import org.firstinspires.ftc.teamcode.MecanumDrive; +import org.firstinspires.ftc.teamcode.robot.MecanumDrive; import org.firstinspires.ftc.teamcode.OTOSLocalizer; import org.firstinspires.ftc.teamcode.PinpointLocalizer; import org.firstinspires.ftc.teamcode.TankDrive; From 6ec244e7296a3f40e5c809c730a4ccb5442367bc Mon Sep 17 00:00:00 2001 From: Brenda Date: Sun, 30 Nov 2025 20:56:07 -0500 Subject: [PATCH 05/16] Adding sample files for pinpoint and limelight --- .../ftc/teamcode/SpiritTeleop.java | 137 ++++++++++++++---- .../ftc/teamcode/robot/Carousel.java | 52 ++++--- .../ftc/teamcode/robot/MecanumDrive.java | 7 +- .../ftc/teamcode/robot/Shooter.java | 4 +- .../ftc/teamcode/tempBKFPlaygroundTeleop.java | 112 ++++++++++++++ 5 files changed, 257 insertions(+), 55 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tempBKFPlaygroundTeleop.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java index 0c6cd250fb8..22eb47a64e0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java @@ -20,6 +20,10 @@ public class SpiritTeleop extends LinearOpMode { @Override public void runOpMode() { telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + double RampUpTimer = 0; + boolean ShooterFlag = false; + int i = 0;//a variable to give make the code "wait" until the required number of seconds have passed to begin launching the artifact + int carouselCounter = 0;//counts the number of times the carousel has advanced 120 degrees. After 3 advances, we need to rest the carousel's position if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); @@ -35,49 +39,118 @@ public void runOpMode() { } else if (gamepad2.right_bumper) { intake.setPower(-1);//sets the power on the intake motor based on the values from the bumps } +//------------------CODE FOR WHEN THE TRIGGERS ARE PRESSED---------------------------------- - if (gamepad2.left_trigger > 0.25) { - shooter.setPower(1); - } else if (gamepad2.right_trigger > 0.25) { - shooter.setPower(0.5); + if ((gamepad2.left_trigger > 0.25) && (gamepad2.right_trigger < .01)) { + if (ShooterFlag = false) { + ShooterFlag = true; + RampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds + shooter.setPower(.6);//ramp up the shooter + } + //check to see if the shooter has ramped up for the required number of seconds. + //if so, reapply power, advance carousel, engage teardrop, reset teardrop, + //and count how many times the carousel has advanced (at 3, reset it to original position) + if (ShooterFlag = true) { + if (getRuntime() > RampUpTimer) { + if (carouselCounter < 5) { + shooter.setPower(.6); + //add code to advance carousel by 120 degrees + + //carousel.flipShooterServo(); + //carousel.unflipShooterServo(); + carouselCounter++; + } else { + //add code to reset carousel to original position + } +//this is a loop to make the code "wait" until the require number of seconds has passed (if statement cannot be empty so we gave it a task to do + // else { + // i = i++; + }else { + i=i++; + } + } else { + shooter.setPower(0); + + //else { + // shooter.setPower(0); + } +//------------------------------------ CODE FOR RIGHT TRIGGER ---------------------------- + if ((gamepad2.left_trigger <= 0.01) && (gamepad2.right_trigger < .25)) { + if (ShooterFlag = false) { + ShooterFlag = true; + RampUpTimer = getRuntime() + 2; //set a timer to the length of time the op has been running + 2 seconds + shooter.setPower(.6);//ramp up the shooter + } } - if (gamepad2.y) { - carousel.flipFeedServo(); + //check to see if the shooter has ramped up for the required number of seconds. + //if so, reapply power, advance carousel, engage teardrop, reset teardrop, + //and count how many times the carousel has advanced (at 3, reset it to original position) + if (ShooterFlag = true) { + if (getRuntime() > RampUpTimer) { + if (carouselCounter < 3) { + shooter.setPower(.6); + //add code to advance carousel by 120 degrees + //carousel.flipShooterServo(); + // carousel.unflipShooterServo(); + carouselCounter++; + } else { + //add code to reset carousel to original position + } + + } else { + i = i++; + } } + } else { + shooter.setPower(0); + } - if(gamepad2.x) { - carousel.unflipFeedServo(); +//-------------------------------------------------------- + if (gamepad2.y) { + // carousel.flipFeedServo(); } - if (gamepad2.b) { - carousel.flipShooterServo(); + if (gamepad2.x) { + // carousel.unflipFeedServo(); } - if(gamepad2.a) { - carousel.unflipShooterServo(); + if (gamepad2.a) { + if (carouselCounter <= 5){ + // carousel.advanceCarousel(); + carouselCounter++; + } + else{ + // + // carousel.resetCarousel(); + carouselCounter=0; + } } - drive.setDrivePowers(new PoseVelocity2d( - new Vector2d( - -gamepad1.left_stick_y, - -gamepad1.left_stick_x - ), - -gamepad1.right_stick_x - )); - - drive.updatePoseEstimate(); - - Pose2d pose = drive.localizer.getPose(); - telemetry.addData("x", pose.position.x); - telemetry.addData("y", pose.position.y); - telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); - telemetry.update(); - - TelemetryPacket packet = new TelemetryPacket(); - packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), pose); - FtcDashboard.getInstance().sendTelemetryPacket(packet); + + if (gamepad2.b) { + intake.setPower(0); } + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x + ), + -gamepad1.right_stick_x + )); + + drive.updatePoseEstimate(); + + Pose2d pose = drive.localizer.getPose(); + telemetry.addData("x", pose.position.x); + telemetry.addData("y", pose.position.y); + telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); + telemetry.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), pose); + FtcDashboard.getInstance().sendTelemetryPacket(packet); } } } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java index 3b85f8c16a9..71ceb622220 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java @@ -5,26 +5,42 @@ import com.qualcomm.robotcore.hardware.Servo; public class Carousel { - private final Servo feedServo; - private final Servo shooterServo; - private final CRServo spinServo; + // private final Servo feedServo; + //private final Servo shooterServo; + // private final Servo spinServo; + private final Servo tiltServo; public Carousel(HardwareMap hardwareMap) { - feedServo = hardwareMap.get(Servo.class, "feedServo"); - shooterServo = hardwareMap.get(Servo.class, "shooterServo"); - spinServo = hardwareMap.get(CRServo.class, "spinServo"); - } + // feedServo = hardwareMap.get(Servo.class, "feedServo");//teardrop from intake to carousel + // shooterServo = hardwareMap.get(Servo.class, "shooterServo");//teardrop from carousel to shooters + //spinServo = hardwareMap.get(Servo.class, "spinServo");//advances the carousel + tiltServo = hardwareMap.get(Servo.class, "tiltServo");//controls angle of shooters - public void flipFeedServo() { - feedServo.setPosition(1); - } - public void unflipFeedServo(){ - feedServo.setPosition(0); } - public void flipShooterServo() { - shooterServo.setPosition(1); - } - public void unflipShooterServo(){ - shooterServo.setPosition(0); +//feeds an artifact into the carousel + // public void flipFeedServo() { + // feedServo.setPosition(1); + // } + //resets the teardrop that feeds the carousel + //public void unflipFeedServo(){ + //feedServo.setPosition(0); + // } + + //feeds an artifact to the shooter + //public void flipShooterServo() { + // shooterServo.setPosition(1); + //} + //resets tej teardrop that feeds the shooter + // public void unflipShooterServo(){ + // shooterServo.setPosition(0); + // } + + // public void advanceCarousel() { + // spinServo.setPosition(spinServo.getPosition() + 120); + // } + + // public void resetCarousel(){ + // spinServo.setPosition(0); + // } } -} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/MecanumDrive.java index 8bce853a995..a1c5c4a3d33 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/MecanumDrive.java @@ -238,9 +238,10 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); // TODO: reverse motor directions if needed - // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftBack.setDirection(DcMotorSimple.Direction.REVERSE); + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + //leftBack.setDirection(DcMotorSimple.Direction.REVERSE); + // leftBack.setDirection(DcMotorSimple.Direction.REVERSE); + rightBack.setDirection(DcMotorSimple.Direction.REVERSE); // TODO: make sure your config has an IMU with this name (can be BNO or BHI) // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index ad97311400e..8b044150baa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -9,8 +9,8 @@ public class Shooter { private final DcMotorEx shooterMotorRight; public Shooter(HardwareMap hardwareMap) { - shooterMotorLeft = hardwareMap.get(DcMotorEx.class, "shooterMotor"); - shooterMotorRight = hardwareMap.get(DcMotorEx.class, "shooterMotor"); + shooterMotorLeft = hardwareMap.get(DcMotorEx.class, "shooterMotorLeft"); + shooterMotorRight = hardwareMap.get(DcMotorEx.class, "shooterMotorRight"); shooterMotorRight.setDirection(DcMotorSimple.Direction.REVERSE); shooterMotorLeft.setDirection(DcMotorSimple.Direction.FORWARD); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tempBKFPlaygroundTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tempBKFPlaygroundTeleop.java new file mode 100644 index 00000000000..f8f45c77a11 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tempBKFPlaygroundTeleop.java @@ -0,0 +1,112 @@ +package org.firstinspires.ftc.teamcode; + +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.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.robot.Carousel; +import org.firstinspires.ftc.teamcode.robot.Intake; +import org.firstinspires.ftc.teamcode.robot.MecanumDrive; +import org.firstinspires.ftc.teamcode.robot.Shooter; +import org.firstinspires.ftc.teamcode.tuning.TuningOpModes; + +@TeleOp(name = "tempBKFPlaygroundTeleop", group = "Teleop") +public class tempBKFPlaygroundTeleop extends LinearOpMode { + @Override + public void runOpMode() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + double RampUpTimer = 0; + boolean ShooterFlag = false; + int i = 0;//a variable to give make the code "wait" until the required number of seconds have passed to begin launching the artifact + int carouselCounter = 0;//counts the number of times the carousel has advanced 120 degrees. After 3 advances, we need to rest the carousel's position + + if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + Intake intake = new Intake(hardwareMap);//instantiate a new intake motor + Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter + Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel + + waitForStart(); + + while (opModeIsActive()) { + if (gamepad2.left_bumper) { + intake.setPower(1); + } else if (gamepad2.right_bumper) { + intake.setPower(-1);//sets the power on the intake motor based on the values from the bumps + } +//------------------CODE FOR WHEN THE TRIGGERS ARE PRESSED---------------------------------- + + if ((gamepad2.left_trigger > 0.25) && (gamepad2.right_trigger < .01)) { + if (ShooterFlag = false) { + ShooterFlag = true; + RampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds + shooter.setPower(.6);//ramp up the shooter + } + //check to see if the shooter has ramped up for the required number of seconds. + //if so, reapply power, advance carousel, engage teardrop, reset teardrop, + //and count how many times the carousel has advanced (at 3, reset it to original position) + if (ShooterFlag = true) { + if (getRuntime() > RampUpTimer) { + if (carouselCounter < 3) { + shooter.setPower(.6); + //add code to advance carousel by 120 degrees + //carousel.flipShooterServo(); + //carousel.unflipShooterServo(); + carouselCounter++; + } else { + //add code to reset carousel to original position + } + + } else { + i = i++; + } + } + } else { + shooter.setPower(0); + } + + if ((gamepad2.left_trigger <= 0.25) && (gamepad2.right_trigger < .01)) { + //Braelan to add the code for the left trigger + } +//-------------------------------------------------------- + if (gamepad2.y) { + // carousel.flipFeedServo(); + } + + if (gamepad2.x) { + // carousel.unflipFeedServo(); + } + + if (gamepad2.a) { + shooter.setPower(0); + } + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x + ), + -gamepad1.right_stick_x + )); + + drive.updatePoseEstimate(); + + Pose2d pose = drive.localizer.getPose(); + telemetry.addData("x", pose.position.x); + telemetry.addData("y", pose.position.y); + telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); + telemetry.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), pose); + FtcDashboard.getInstance().sendTelemetryPacket(packet); + } + } + } +} + From ee8881659c22328f6f57be683e4db71f50e64a94 Mon Sep 17 00:00:00 2001 From: Brenda Date: Sun, 30 Nov 2025 21:19:32 -0500 Subject: [PATCH 06/16] compare not assign --- .../java/org/firstinspires/ftc/teamcode/SpiritTeleop.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java index 22eb47a64e0..4c960a9990d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java @@ -42,7 +42,7 @@ public void runOpMode() { //------------------CODE FOR WHEN THE TRIGGERS ARE PRESSED---------------------------------- if ((gamepad2.left_trigger > 0.25) && (gamepad2.right_trigger < .01)) { - if (ShooterFlag = false) { + if (ShooterFlag == false) { ShooterFlag = true; RampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds shooter.setPower(.6);//ramp up the shooter @@ -50,7 +50,7 @@ public void runOpMode() { //check to see if the shooter has ramped up for the required number of seconds. //if so, reapply power, advance carousel, engage teardrop, reset teardrop, //and count how many times the carousel has advanced (at 3, reset it to original position) - if (ShooterFlag = true) { + if (ShooterFlag == true) { if (getRuntime() > RampUpTimer) { if (carouselCounter < 5) { shooter.setPower(.6); From c72b865b4dd9a7fdd99b178ac0a599de6e1187c4 Mon Sep 17 00:00:00 2001 From: Brenda Date: Sun, 30 Nov 2025 21:38:15 -0500 Subject: [PATCH 07/16] compare not assign --- .../java/org/firstinspires/ftc/teamcode/SpiritTeleop.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java index 4c960a9990d..6ae503bdf7a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java @@ -76,7 +76,7 @@ public void runOpMode() { } //------------------------------------ CODE FOR RIGHT TRIGGER ---------------------------- if ((gamepad2.left_trigger <= 0.01) && (gamepad2.right_trigger < .25)) { - if (ShooterFlag = false) { + if (ShooterFlag == false) { ShooterFlag = true; RampUpTimer = getRuntime() + 2; //set a timer to the length of time the op has been running + 2 seconds shooter.setPower(.6);//ramp up the shooter @@ -86,7 +86,7 @@ public void runOpMode() { //check to see if the shooter has ramped up for the required number of seconds. //if so, reapply power, advance carousel, engage teardrop, reset teardrop, //and count how many times the carousel has advanced (at 3, reset it to original position) - if (ShooterFlag = true) { + if (ShooterFlag == true) { if (getRuntime() > RampUpTimer) { if (carouselCounter < 3) { shooter.setPower(.6); From 61522d3068af78b95842ef46491dbdc8ace0f9d8 Mon Sep 17 00:00:00 2001 From: Brenda Date: Mon, 1 Dec 2025 22:36:13 -0500 Subject: [PATCH 08/16] additional compare not assign corrections --- .../ftc/teamcode/SpiritTeleop.java | 236 ++++++++++-------- .../ftc/teamcode/robot/Carousel.java | 53 ++-- .../ftc/teamcode/robot/Shooter.java | 4 + 3 files changed, 167 insertions(+), 126 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java index 6ae503bdf7a..a61908f15ce 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java @@ -20,8 +20,11 @@ public class SpiritTeleop extends LinearOpMode { @Override public void runOpMode() { telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - double RampUpTimer = 0; - boolean ShooterFlag = false; + double rampUpTimer = 0; + boolean rampUpFlag = false; + double closeShooterPower = .4;//speed for shooter if close to target + double farShooterPower = .6;//speed for shooter if far from target + double shooterPower = 0;//we will set shooterPower to either closeShooterPower or farShooterPower depending on which trigger is pulled. int i = 0;//a variable to give make the code "wait" until the required number of seconds have passed to begin launching the artifact int carouselCounter = 0;//counts the number of times the carousel has advanced 120 degrees. After 3 advances, we need to rest the carousel's position @@ -33,124 +36,157 @@ public void runOpMode() { waitForStart(); - while (opModeIsActive()) { + while (opModeIsActive()) + { if (gamepad2.left_bumper) { intake.setPower(1); } else if (gamepad2.right_bumper) { intake.setPower(-1);//sets the power on the intake motor based on the values from the bumps + } else { + intake.setPower(0); } //------------------CODE FOR WHEN THE TRIGGERS ARE PRESSED---------------------------------- - - if ((gamepad2.left_trigger > 0.25) && (gamepad2.right_trigger < .01)) { - if (ShooterFlag == false) { - ShooterFlag = true; - RampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds - shooter.setPower(.6);//ramp up the shooter + //--------------LEFT TRIGGER PULLED (CLOSE TO TARGET) ------------------------------ + + if ((gamepad2.left_trigger > 0.25) && (gamepad2.right_trigger < .01)) + { + shooterPower = closeShooterPower; + if (rampUpFlag == false) + { + rampUpFlag = true; + rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds + shooter.setPower(shooterPower); } - //check to see if the shooter has ramped up for the required number of seconds. - //if so, reapply power, advance carousel, engage teardrop, reset teardrop, - //and count how many times the carousel has advanced (at 3, reset it to original position) - if (ShooterFlag == true) { - if (getRuntime() > RampUpTimer) { - if (carouselCounter < 5) { - shooter.setPower(.6); - //add code to advance carousel by 120 degrees - - //carousel.flipShooterServo(); - //carousel.unflipShooterServo(); - carouselCounter++; - } else { - //add code to reset carousel to original position - } -//this is a loop to make the code "wait" until the require number of seconds has passed (if statement cannot be empty so we gave it a task to do - // else { - // i = i++; - }else { - i=i++; - } - } else { + else if (getRuntime() > rampUpTimer){ + shooter.setPower(shooterPower);//ramp up the shooter + } + //Otherwise, trigger is not engaged so stop shooter and reset rampUpFlag + else{ shooter.setPower(0); - - //else { - // shooter.setPower(0); + rampUpFlag = false; + } } -//------------------------------------ CODE FOR RIGHT TRIGGER ---------------------------- - if ((gamepad2.left_trigger <= 0.01) && (gamepad2.right_trigger < .25)) { - if (ShooterFlag == false) { - ShooterFlag = true; - RampUpTimer = getRuntime() + 2; //set a timer to the length of time the op has been running + 2 seconds - shooter.setPower(.6);//ramp up the shooter +//--------------LEFT TRIGGER PULLED (FAR FROM TARGET) ------------------------------ + if ((gamepad2.right_trigger > 0.25) && (gamepad2.left_trigger < .01)) + { + shooterPower = farShooterPower; + if (rampUpFlag == false) + { + rampUpFlag = true; + rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds + shooter.setPower(shooterPower); + } + else if (getRuntime() > rampUpTimer){ + shooter.setPower(shooterPower);//ramp up the shooter + } + //Otherwise, no trigger is engaged so stop shooters and reset rampUpFlag to false + else{ + shooter.setPower(0); + rampUpFlag = false; } } - //check to see if the shooter has ramped up for the required number of seconds. - //if so, reapply power, advance carousel, engage teardrop, reset teardrop, - //and count how many times the carousel has advanced (at 3, reset it to original position) - if (ShooterFlag == true) { - if (getRuntime() > RampUpTimer) { - if (carouselCounter < 3) { - shooter.setPower(.6); - //add code to advance carousel by 120 degrees - //carousel.flipShooterServo(); - // carousel.unflipShooterServo(); + /*If either trigger is pulled and ramp up is complete, reapply power, engage teardrop, + reset teardrop, and count how many times the carousel has advanced (at 5, reset it to original + position). */ + if ((rampUpFlag == true) && (getRuntime()>rampUpTimer)) { + shooter.setPower(shooterPower); + if (carouselCounter < 5) { + carousel.flipShooterServo(); + carousel.unflipShooterServo(); + //carousel.advanceCarousel(); maybe later, add code to advance carousel by 120 degrees which is .06 + // (120 degrees/1800 total degrees) but for now, we are advancing carousel manually to control color carouselCounter++; } else { - //add code to reset carousel to original position + carousel.resetCarousel(); + carouselCounter = 0; } - - } else { - i = i++; } - } - } else { - shooter.setPower(0); - } -//-------------------------------------------------------- - if (gamepad2.y) { - // carousel.flipFeedServo(); - } + /* THIS CODE IS NOT NEEDED AND WILL BE DELETED AFTER THE CLASS IS REWRITTEN +//------------------------------------ CODE FOR RIGHT TRIGGER ---------------------------- + if ((gamepad2.right_trigger >.25) && (gamepad2.left_trigger <.01)) { + if (rampUpFlag == false) { + rampUpFlag = true; + rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds + shooter.setPower(.4);//ramp up the shooter + }else{ + shooter.setPower(0); + rampUpFlag = false; + } + //check to see if the shooter has ramped up for the required number of seconds. + //if so, reapply power, advance carousel, engage teardrop, reset teardrop, + //and count how many times the carousel has advanced (at 3, reset it to original position) + if (rampUpFlag == true) { + shooter.setPower(.4); + if (getRuntime() > rampUpTimer) { + if (carouselCounter < 5) { + //add code to advance carousel by 120 degrees which is .06 (120 degrees/1800 total degrees) + //carousel.flipShooterServo(); + //carousel.unflipShooterServo(); + carouselCounter++; + } else { + //carousel.resetCarousel(0); + carouselCounter = 0; + } +//this is a loop to make the code "wait" until the require number of seconds has passed (if statement cannot be empty so we gave it a task to do + } else { + i++; + } + } //else { + // shooter.setPower(0); + // shooterFlag = false; + // } + +//------------------END CODE FOR TRIGGERS-------------------------------------- +*/ + if (gamepad2.y) { + // carousel.flipFeedServo(); + } - if (gamepad2.x) { - // carousel.unflipFeedServo(); - } + if (gamepad2.x) { + // carousel.unflipFeedServo(); + } - if (gamepad2.a) { - if (carouselCounter <= 5){ - // carousel.advanceCarousel(); - carouselCounter++; - } - else{ - // - // carousel.resetCarousel(); - carouselCounter=0; - } - } + if (gamepad2.a) { + if (carouselCounter <= 5) { + // carousel.advanceCarousel(); + carouselCounter++; + } else { + // + // carousel.resetCarousel(); + carouselCounter = 0; + } + } - if (gamepad2.b) { - intake.setPower(0); - } - drive.setDrivePowers(new PoseVelocity2d( - new Vector2d( - -gamepad1.left_stick_y, - -gamepad1.left_stick_x - ), - -gamepad1.right_stick_x - )); - - drive.updatePoseEstimate(); - - Pose2d pose = drive.localizer.getPose(); - telemetry.addData("x", pose.position.x); - telemetry.addData("y", pose.position.y); - telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); + if (gamepad2.b) { + intake.setPower(0); + telemetry.addData("button b", "button b"); + } + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x + ), + -gamepad1.right_stick_x + )); + + drive.updatePoseEstimate(); + + Pose2d pose = drive.localizer.getPose(); + telemetry.addData("x", pose.position.x); + telemetry.addData("y", pose.position.y); + telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); + telemetry.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), pose); + FtcDashboard.getInstance().sendTelemetryPacket(packet); + } + } telemetry.update(); - - TelemetryPacket packet = new TelemetryPacket(); - packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), pose); - FtcDashboard.getInstance().sendTelemetryPacket(packet); + } } } - } -} + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java index 71ceb622220..ad68ebeffcc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java @@ -5,42 +5,43 @@ import com.qualcomm.robotcore.hardware.Servo; public class Carousel { - // private final Servo feedServo; - //private final Servo shooterServo; - // private final Servo spinServo; - private final Servo tiltServo; + private final Servo feedServo; + private final Servo shooterServo; + private final Servo spinServo; + public Carousel(HardwareMap hardwareMap) { - // feedServo = hardwareMap.get(Servo.class, "feedServo");//teardrop from intake to carousel - // shooterServo = hardwareMap.get(Servo.class, "shooterServo");//teardrop from carousel to shooters - //spinServo = hardwareMap.get(Servo.class, "spinServo");//advances the carousel - tiltServo = hardwareMap.get(Servo.class, "tiltServo");//controls angle of shooters + feedServo = hardwareMap.get(Servo.class, "feedServo");//teardrop from intake to carousel + shooterServo = hardwareMap.get(Servo.class, "shooterServo");//teardrop from carousel to shooters + spinServo = hardwareMap.get(Servo.class, "spinServo");//advances the carousel + } //feeds an artifact into the carousel - // public void flipFeedServo() { - // feedServo.setPosition(1); - // } + public void flipFeedServo() { + feedServo.setPosition(1); + } //resets the teardrop that feeds the carousel - //public void unflipFeedServo(){ - //feedServo.setPosition(0); - // } + public void unflipFeedServo(){ + feedServo.setPosition(0); + } //feeds an artifact to the shooter - //public void flipShooterServo() { - // shooterServo.setPosition(1); - //} + public void flipShooterServo() { + shooterServo.setPosition(1); + } //resets tej teardrop that feeds the shooter - // public void unflipShooterServo(){ - // shooterServo.setPosition(0); - // } + public void unflipShooterServo(){ + shooterServo.setPosition(0); + } - // public void advanceCarousel() { - // spinServo.setPosition(spinServo.getPosition() + 120); - // } + public void advanceCarousel() + { + spinServo.setPosition(spinServo.getPosition() + .06);//120 degrees/1800 servo degree range -= .06 + } - // public void resetCarousel(){ - // spinServo.setPosition(0); - // } + public void resetCarousel(){ + spinServo.setPosition(0); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index 8b044150baa..bd722dedffa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -2,15 +2,18 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.HardwareMap; public class Shooter { private final DcMotorEx shooterMotorLeft; private final DcMotorEx shooterMotorRight; + private final Servo tiltServo; public Shooter(HardwareMap hardwareMap) { shooterMotorLeft = hardwareMap.get(DcMotorEx.class, "shooterMotorLeft"); shooterMotorRight = hardwareMap.get(DcMotorEx.class, "shooterMotorRight"); + tiltServo = hardwareMap.get(Servo.class, "tiltServo");//controls angle of shooters shooterMotorRight.setDirection(DcMotorSimple.Direction.REVERSE); shooterMotorLeft.setDirection(DcMotorSimple.Direction.FORWARD); } @@ -22,5 +25,6 @@ public void setPower(double power) { shooterMotorLeft.setPower(power); shooterMotorRight.setPower(power); } + } From 34f9f152a4270bab90de0a820f477c35366d5b33 Mon Sep 17 00:00:00 2001 From: Brenda Date: Tue, 2 Dec 2025 20:40:47 -0500 Subject: [PATCH 09/16] rewrite class to improve organization --- .../{SpiritTeleop.java => SpiritTeleop1.java} | 12 +- .../ftc/teamcode/robot/Shooter.java | 9 +- .../ftc/teamcode/robot/SpiritTeleop2.java | 155 ++++++++++++++++++ 3 files changed, 168 insertions(+), 8 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{SpiritTeleop.java => SpiritTeleop1.java} (95%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop1.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop1.java index a61908f15ce..676d3aea629 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop1.java @@ -16,15 +16,12 @@ import org.firstinspires.ftc.teamcode.tuning.TuningOpModes; @TeleOp(name = "Spirit", group = "Teleop") -public class SpiritTeleop extends LinearOpMode { +public class SpiritTeleop1 extends LinearOpMode { @Override public void runOpMode() { telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); double rampUpTimer = 0; boolean rampUpFlag = false; - double closeShooterPower = .4;//speed for shooter if close to target - double farShooterPower = .6;//speed for shooter if far from target - double shooterPower = 0;//we will set shooterPower to either closeShooterPower or farShooterPower depending on which trigger is pulled. int i = 0;//a variable to give make the code "wait" until the required number of seconds have passed to begin launching the artifact int carouselCounter = 0;//counts the number of times the carousel has advanced 120 degrees. After 3 advances, we need to rest the carousel's position @@ -50,12 +47,13 @@ public void runOpMode() { if ((gamepad2.left_trigger > 0.25) && (gamepad2.right_trigger < .01)) { - shooterPower = closeShooterPower; + shooter.shooterPower = shooter.closeShooterPower; + shooterTilt = closeShooterTilt; if (rampUpFlag == false) { rampUpFlag = true; rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds - shooter.setPower(shooterPower); + shooter.setPower(shooter.shooterPower); } else if (getRuntime() > rampUpTimer){ shooter.setPower(shooterPower);//ramp up the shooter @@ -70,10 +68,12 @@ else if (getRuntime() > rampUpTimer){ if ((gamepad2.right_trigger > 0.25) && (gamepad2.left_trigger < .01)) { shooterPower = farShooterPower; + shooterTilt = farShooterTilt; if (rampUpFlag == false) { rampUpFlag = true; rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds + shooter.setPower(shooterPower); } else if (getRuntime() > rampUpTimer){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index bd722dedffa..e5ada5121ec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -10,7 +10,10 @@ public class Shooter { private final DcMotorEx shooterMotorRight; private final Servo tiltServo; + + public Shooter(HardwareMap hardwareMap) { + shooterMotorLeft = hardwareMap.get(DcMotorEx.class, "shooterMotorLeft"); shooterMotorRight = hardwareMap.get(DcMotorEx.class, "shooterMotorRight"); tiltServo = hardwareMap.get(Servo.class, "tiltServo");//controls angle of shooters @@ -24,7 +27,9 @@ public Shooter(HardwareMap hardwareMap) { public void setPower(double power) { shooterMotorLeft.setPower(power); shooterMotorRight.setPower(power); - } + } + public void setShooterTilt(double shooterTiltPositon){ + tiltServo.setPosition(shooterTiltPositon); + } } - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java new file mode 100644 index 00000000000..90cb05331f1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java @@ -0,0 +1,155 @@ +package org.firstinspires.ftc.teamcode; + +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.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.robot.Carousel; +import org.firstinspires.ftc.teamcode.robot.Intake; +import org.firstinspires.ftc.teamcode.robot.MecanumDrive; +import org.firstinspires.ftc.teamcode.robot.Shooter; +import org.firstinspires.ftc.teamcode.tuning.TuningOpModes; + +@TeleOp(name = "Spirit", group = "Teleop") +public class SpiritTeleop2 extends LinearOpMode { + @Override + public void runOpMode() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + double rampUpTimer = 0; + boolean rampUpFlag = false; + int i = 0;//a variable to give make the code "wait" until the required number of seconds have passed to begin launching the artifact + int carouselCounter = 0;//counts the number of times the carousel has advanced 120 degrees. After 3 advances, we need to rest the carousel's position + + + if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + Intake intake = new Intake(hardwareMap);//instantiate a new intake motor + Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter + Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel + + waitForStart(); + + while (opModeIsActive()) + { + if (gamepad2.left_bumper) { + intake.setPower(1); + } else if (gamepad2.right_bumper) { + intake.setPower(-1);//sets the power on the intake motor based on the values from the bumps + } else { + intake.setPower(0); + } +//------------------CODE FOR WHEN THE TRIGGERS ARE PRESSED---------------------------------- + //--------------LEFT TRIGGER PULLED (CLOSE TO TARGET) ------------------------------ + + switch (currentState) { + + case IDLE: + handleIdle(); + break; + + case RIGHT_TRIGGER_SEQUENCE: + updateRightSequence(); + break; + + case LEFT_TRIGGER_SEQUENCE: + updateLeftSequence(); + break; + } + //------------------------END OF SHOOTER CONTROL----------------------------------- + + + /* THIS CODE IS NOT NEEDED AND WILL BE DELETED AFTER THE CLASS IS REWRITTEN +//------------------------------------ CODE FOR RIGHT TRIGGER ---------------------------- + if ((gamepad2.right_trigger >.25) && (gamepad2.left_trigger <.01)) { + if (rampUpFlag == false) { + rampUpFlag = true; + rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds + shooter.setPower(.4);//ramp up the shooter + }else{ + shooter.setPower(0); + rampUpFlag = false; + } + //check to see if the shooter has ramped up for the required number of seconds. + //if so, reapply power, advance carousel, engage teardrop, reset teardrop, + //and count how many times the carousel has advanced (at 3, reset it to original position) + if (rampUpFlag == true) { + shooter.setPower(.4); + if (getRuntime() > rampUpTimer) { + if (carouselCounter < 5) { + //add code to advance carousel by 120 degrees which is .06 (120 degrees/1800 total degrees) + //carousel.flipShooterServo(); + //carousel.unflipShooterServo(); + carouselCounter++; + } else { + //carousel.resetCarousel(0); + carouselCounter = 0; + } +//this is a loop to make the code "wait" until the require number of seconds has passed (if statement cannot be empty so we gave it a task to do + } else { + i++; + } + } //else { + // shooter.setPower(0); + // shooterFlag = false; + // } + +//------------------END CODE FOR TRIGGERS-------------------------------------- +*/ + if (gamepad2.y) { + // carousel.flipFeedServo(); + } + + if (gamepad2.x) { + // carousel.unflipFeedServo(); + } + + if (gamepad2.a) { + if (carouselCounter <= 5) { + // carousel.advanceCarousel(); + carouselCounter++; + } else { + // + // carousel.resetCarousel(); + carouselCounter = 0; + } + } + + if (gamepad2.b) { + intake.setPower(0); + telemetry.addData("button b", "button b"); + } + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x + ), + -gamepad1.right_stick_x + )); + + drive.updatePoseEstimate(); + + Pose2d pose = drive.localizer.getPose(); + telemetry.addData("x", pose.position.x); + telemetry.addData("y", pose.position.y); + telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); + telemetry.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), pose); + FtcDashboard.getInstance().sendTelemetryPacket(packet); + } + } + telemetry.update(); + + private void handleIdle() { + shooter.setPower(0); + } + } +} + } From 5df2381b37b36015d2ae1a53db1fcb070a5cb01d Mon Sep 17 00:00:00 2001 From: Brenda Date: Tue, 2 Dec 2025 20:59:51 -0500 Subject: [PATCH 10/16] added SpiritTeleop2 to rewrite the class --- .../ftc/teamcode/robot/SpiritTeleop2.java | 24 ++++-- .../ftc/teamcode/robot/tempr.java | 74 +++++++++++++++++++ 2 files changed, 91 insertions(+), 7 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/tempr.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java index 90cb05331f1..49d30eb6955 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java @@ -26,6 +26,14 @@ public void runOpMode() { int carouselCounter = 0;//counts the number of times the carousel has advanced 120 degrees. After 3 advances, we need to rest the carousel's position + enum State { + IDLE, + RIGHT_TRIGGER_SEQUENCE, + LEFT_TRIGGER_SEQUENCE + } + State currentState = State.IDLE; + double stateStartTime = 0; + if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor @@ -49,15 +57,19 @@ public void runOpMode() { switch (currentState) { case IDLE: - handleIdle(); + //handleIdle(); + shooter.setPower(0); + if ((gamepad2.right_trigger >.25) && (gamepad2.left_trigger <.01)) { + + } break; case RIGHT_TRIGGER_SEQUENCE: - updateRightSequence(); + //updateRightSequence(); break; case LEFT_TRIGGER_SEQUENCE: - updateLeftSequence(); + //updateLeftSequence(); break; } //------------------------END OF SHOOTER CONTROL----------------------------------- @@ -147,9 +159,7 @@ public void runOpMode() { } telemetry.update(); - private void handleIdle() { - shooter.setPower(0); + } } -} - } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/tempr.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/tempr.java new file mode 100644 index 00000000000..352fcba1fe0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/tempr.java @@ -0,0 +1,74 @@ +package org.firstinspires.ftc.teamcode.robot; +@TeleOp(name="TriggerStateMachine") +public class TriggerStateMachine extends OpMode { + + private DcMotor dcMotor; + private DcMotor gobildaMotor; + private Servo servo; + private Servo gobildaServo; + + // State variables + private enum State { + IDLE, + RIGHT_TRIGGER_RUNNING, + LEFT_TRIGGER_RUNNING + } + + private State currentState = State.IDLE; + private double stateStartTime = 0; + + @Override + public void init() { + dcMotor = hardwareMap.get(DcMotor.class, "dcMotor"); + gobildaMotor = hardwareMap.get(DcMotor.class, "gobildaMotor"); + servo = hardwareMap.get(Servo.class, "servo"); + gobildaServo = hardwareMap.get(Servo.class, "gobildaServo"); + } + + @Override + public void loop() { + + switch (currentState) { + + case IDLE: + // Default motor state + dcMotor.setPower(0); + + // Start RIGHT sequence + if (gamepad1.right_trigger > 0.1) { + dcMotor.setPower(1.0); // Start action + currentState = State.RIGHT_TRIGGER_RUNNING; + stateStartTime = runtime.seconds(); + } + + // Start LEFT sequence + else if (gamepad1.left_trigger > 0.1) { + dcMotor.setPower(0.5); // Start action + currentState = State.LEFT_TRIGGER_RUNNING; + stateStartTime = runtime.seconds(); + } + + break; + + case RIGHT_TRIGGER_RUNNING: + // Has 2 seconds passed? + if (runtime.seconds() - stateStartTime >= 2.0) { + dcMotor.setPower(0.2); + servo.setPosition(0.25); + currentState = State.IDLE; // Reset to idle state + } + break; + + case LEFT_TRIGGER_RUNNING: + // Has 2 seconds passed? + if (runtime.seconds() - stateStartTime >= 2.0) { + gobildaMotor.setPower(0.6); + gobildaServo.setPosition(0.6); + currentState = State.IDLE; // Reset to idle state + } + break; + } + } +} + + From 03411de12edef1a1c0a730b06f6121a401ece786 Mon Sep 17 00:00:00 2001 From: Family Date: Wed, 3 Dec 2025 21:02:15 -0500 Subject: [PATCH 11/16] Rewrite using state engine --- .../ftc/teamcode/robot/Carousel.java | 10 ++-- .../ftc/teamcode/robot/Shooter.java | 20 ++++--- .../ftc/teamcode/robot/SpiritTeleop2.java | 54 ++++++++++++++----- build.gradle | 1 + 4 files changed, 60 insertions(+), 25 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java index ad68ebeffcc..151c27cd788 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java @@ -17,21 +17,21 @@ public Carousel(HardwareMap hardwareMap) { } -//feeds an artifact into the carousel - public void flipFeedServo() { + //feeds an artifact into the carousel + public void engageFeedServo() { feedServo.setPosition(1); } //resets the teardrop that feeds the carousel - public void unflipFeedServo(){ + public void resetFeedServo(){ feedServo.setPosition(0); } //feeds an artifact to the shooter - public void flipShooterServo() { + public void engageShooterServo() { shooterServo.setPosition(1); } //resets tej teardrop that feeds the shooter - public void unflipShooterServo(){ + public void releaseShooterServo(){ shooterServo.setPosition(0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index e5ada5121ec..c3b92387164 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -6,9 +6,9 @@ import com.qualcomm.robotcore.hardware.HardwareMap; public class Shooter { - private final DcMotorEx shooterMotorLeft; - private final DcMotorEx shooterMotorRight; - private final Servo tiltServo; + public final DcMotorEx shooterMotorLeft; + public final DcMotorEx shooterMotorRight; + public final Servo tiltServo; @@ -24,12 +24,18 @@ public Shooter(HardwareMap hardwareMap) { /** * Set the power to the left and right motors */ - public void setPower(double power) { + public void setShooterPower(double power) { shooterMotorLeft.setPower(power); shooterMotorRight.setPower(power); } + public void setTiltServo(double shooterTiltPositon){ + tiltServo.setPosition(shooterTiltPositon); + } + } + +/** + Set position of servo that tilts the shooters + * + */ - public void setShooterTilt(double shooterTiltPositon){ - tiltServo.setPosition(shooterTiltPositon); - } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java index 49d30eb6955..5dafe234c2d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java @@ -1,5 +1,4 @@ -package org.firstinspires.ftc.teamcode; - +package org.firstinspires.ftc.teamcode.robot; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; @@ -24,12 +23,14 @@ public void runOpMode() { boolean rampUpFlag = false; int i = 0;//a variable to give make the code "wait" until the required number of seconds have passed to begin launching the artifact int carouselCounter = 0;//counts the number of times the carousel has advanced 120 degrees. After 3 advances, we need to rest the carousel's position - - +double nearShooterPower = .4; +double farShooterPower = .6; +double shooterPower = 0; enum State { IDLE, - RIGHT_TRIGGER_SEQUENCE, - LEFT_TRIGGER_SEQUENCE + RAMPING, + LAUNCHING, + } State currentState = State.IDLE; double stateStartTime = 0; @@ -58,20 +59,47 @@ enum State { case IDLE: //handleIdle(); - shooter.setPower(0); - if ((gamepad2.right_trigger >.25) && (gamepad2.left_trigger <.01)) { - + shooter.setShooterPower(0.00);//stop shooter + carousel.releaseShooterServo();//move shooter feeder to down position + + if ((gamepad2.right_trigger >.25) && (gamepad2.left_trigger <.01)) + { + shooterPower = nearShooterPower; + shooter.setShooterPower(shooterPower);//set power to shoot near target + currentState = State.RAMPING; + rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds + } + if ((gamepad2.left_trigger >.25) && (gamepad2.right_trigger <.01)) + { + shooterPower=farShooterPower; + shooter.setShooterPower(shooterPower);//set power to shoot far from target + currentState = State.RAMPING; + rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds } break; - case RIGHT_TRIGGER_SEQUENCE: - //updateRightSequence(); + case RAMPING: + if(getRuntime() > rampUpTimer) { + shooter.setShooterPower(shooterPower);//set power to shooter + currentState = State.LAUNCHING; + } break; - case LEFT_TRIGGER_SEQUENCE: - //updateLeftSequence(); + case LAUNCHING: + if(getRuntime() > rampUpTimer) { + shooter.setShooterPower(shooterPower);//reapply power to shooter + carousel.engageShooterServo(); + carousel.resetCarousel(); + currentState = State.IDLE; + } break; + + } + telemetry.addData("State", currentState); + telemetry.addData("Shooter Power", shooterPower); + //telemetry.addData("Servo Pos", carousel.getPosition()); + telemetry.update(); //------------------------END OF SHOOTER CONTROL----------------------------------- diff --git a/build.gradle b/build.gradle index 6dd7fd3483c..d9fca8588c6 100644 --- a/build.gradle +++ b/build.gradle @@ -27,3 +27,4 @@ allprojects { repositories { mavenCentral() } + From 5d3b06cbe85b07d759e78af04279d8982d4486b4 Mon Sep 17 00:00:00 2001 From: Brenda Date: Wed, 3 Dec 2025 21:54:09 -0500 Subject: [PATCH 12/16] Code with state engine is working. Bot intakes, shoots and drives as of 12/3/2025 --- .../ftc/teamcode/SpiritTeleop1.java | 192 ------------------ .../ftc/teamcode/robot/Carousel.java | 32 ++- .../ftc/teamcode/robot/Shooter.java | 2 +- .../ftc/teamcode/robot/SpiritTeleop2.java | 20 +- .../ftc/teamcode/robot/tempr.java | 74 ------- .../ftc/teamcode/tempBKFPlaygroundTeleop.java | 112 ---------- 6 files changed, 26 insertions(+), 406 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop1.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/tempr.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tempBKFPlaygroundTeleop.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop1.java deleted file mode 100644 index 676d3aea629..00000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpiritTeleop1.java +++ /dev/null @@ -1,192 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -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.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.robot.Carousel; -import org.firstinspires.ftc.teamcode.robot.Intake; -import org.firstinspires.ftc.teamcode.robot.MecanumDrive; -import org.firstinspires.ftc.teamcode.robot.Shooter; -import org.firstinspires.ftc.teamcode.tuning.TuningOpModes; - -@TeleOp(name = "Spirit", group = "Teleop") -public class SpiritTeleop1 extends LinearOpMode { - @Override - public void runOpMode() { - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - double rampUpTimer = 0; - boolean rampUpFlag = false; - int i = 0;//a variable to give make the code "wait" until the required number of seconds have passed to begin launching the artifact - int carouselCounter = 0;//counts the number of times the carousel has advanced 120 degrees. After 3 advances, we need to rest the carousel's position - - if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { - MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); - Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter - Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel - - waitForStart(); - - while (opModeIsActive()) - { - if (gamepad2.left_bumper) { - intake.setPower(1); - } else if (gamepad2.right_bumper) { - intake.setPower(-1);//sets the power on the intake motor based on the values from the bumps - } else { - intake.setPower(0); - } -//------------------CODE FOR WHEN THE TRIGGERS ARE PRESSED---------------------------------- - //--------------LEFT TRIGGER PULLED (CLOSE TO TARGET) ------------------------------ - - if ((gamepad2.left_trigger > 0.25) && (gamepad2.right_trigger < .01)) - { - shooter.shooterPower = shooter.closeShooterPower; - shooterTilt = closeShooterTilt; - if (rampUpFlag == false) - { - rampUpFlag = true; - rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds - shooter.setPower(shooter.shooterPower); - } - else if (getRuntime() > rampUpTimer){ - shooter.setPower(shooterPower);//ramp up the shooter - } - //Otherwise, trigger is not engaged so stop shooter and reset rampUpFlag - else{ - shooter.setPower(0); - rampUpFlag = false; - } - } -//--------------LEFT TRIGGER PULLED (FAR FROM TARGET) ------------------------------ - if ((gamepad2.right_trigger > 0.25) && (gamepad2.left_trigger < .01)) - { - shooterPower = farShooterPower; - shooterTilt = farShooterTilt; - if (rampUpFlag == false) - { - rampUpFlag = true; - rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds - - shooter.setPower(shooterPower); - } - else if (getRuntime() > rampUpTimer){ - shooter.setPower(shooterPower);//ramp up the shooter - } - //Otherwise, no trigger is engaged so stop shooters and reset rampUpFlag to false - else{ - shooter.setPower(0); - rampUpFlag = false; - } - } - - /*If either trigger is pulled and ramp up is complete, reapply power, engage teardrop, - reset teardrop, and count how many times the carousel has advanced (at 5, reset it to original - position). */ - if ((rampUpFlag == true) && (getRuntime()>rampUpTimer)) { - shooter.setPower(shooterPower); - if (carouselCounter < 5) { - carousel.flipShooterServo(); - carousel.unflipShooterServo(); - //carousel.advanceCarousel(); maybe later, add code to advance carousel by 120 degrees which is .06 - // (120 degrees/1800 total degrees) but for now, we are advancing carousel manually to control color - carouselCounter++; - } else { - carousel.resetCarousel(); - carouselCounter = 0; - } - } - - /* THIS CODE IS NOT NEEDED AND WILL BE DELETED AFTER THE CLASS IS REWRITTEN -//------------------------------------ CODE FOR RIGHT TRIGGER ---------------------------- - if ((gamepad2.right_trigger >.25) && (gamepad2.left_trigger <.01)) { - if (rampUpFlag == false) { - rampUpFlag = true; - rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds - shooter.setPower(.4);//ramp up the shooter - }else{ - shooter.setPower(0); - rampUpFlag = false; - } - //check to see if the shooter has ramped up for the required number of seconds. - //if so, reapply power, advance carousel, engage teardrop, reset teardrop, - //and count how many times the carousel has advanced (at 3, reset it to original position) - if (rampUpFlag == true) { - shooter.setPower(.4); - if (getRuntime() > rampUpTimer) { - if (carouselCounter < 5) { - //add code to advance carousel by 120 degrees which is .06 (120 degrees/1800 total degrees) - //carousel.flipShooterServo(); - //carousel.unflipShooterServo(); - carouselCounter++; - } else { - //carousel.resetCarousel(0); - carouselCounter = 0; - } -//this is a loop to make the code "wait" until the require number of seconds has passed (if statement cannot be empty so we gave it a task to do - } else { - i++; - } - } //else { - // shooter.setPower(0); - // shooterFlag = false; - // } - -//------------------END CODE FOR TRIGGERS-------------------------------------- -*/ - if (gamepad2.y) { - // carousel.flipFeedServo(); - } - - if (gamepad2.x) { - // carousel.unflipFeedServo(); - } - - if (gamepad2.a) { - if (carouselCounter <= 5) { - // carousel.advanceCarousel(); - carouselCounter++; - } else { - // - // carousel.resetCarousel(); - carouselCounter = 0; - } - } - - if (gamepad2.b) { - intake.setPower(0); - telemetry.addData("button b", "button b"); - } - drive.setDrivePowers(new PoseVelocity2d( - new Vector2d( - -gamepad1.left_stick_y, - -gamepad1.left_stick_x - ), - -gamepad1.right_stick_x - )); - - drive.updatePoseEstimate(); - - Pose2d pose = drive.localizer.getPose(); - telemetry.addData("x", pose.position.x); - telemetry.addData("y", pose.position.y); - telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); - telemetry.update(); - - TelemetryPacket packet = new TelemetryPacket(); - packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), pose); - FtcDashboard.getInstance().sendTelemetryPacket(packet); - } - } - telemetry.update(); - } - } - } - } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java index 151c27cd788..71689befa30 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java @@ -1,47 +1,41 @@ package org.firstinspires.ftc.teamcode.robot; -import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; public class Carousel { - private final Servo feedServo; + private final Servo shooterServo; - private final Servo spinServo; + //private final Servo spinServo; public Carousel(HardwareMap hardwareMap) { - feedServo = hardwareMap.get(Servo.class, "feedServo");//teardrop from intake to carousel + shooterServo = hardwareMap.get(Servo.class, "shooterServo");//teardrop from carousel to shooters - spinServo = hardwareMap.get(Servo.class, "spinServo");//advances the carousel + // spinServo = hardwareMap.get(Servo.class, "spinServo");//advances the carousel + } - //feeds an artifact into the carousel - public void engageFeedServo() { - feedServo.setPosition(1); - } - //resets the teardrop that feeds the carousel - public void resetFeedServo(){ - feedServo.setPosition(0); - } + //feeds an artifact to the shooter public void engageShooterServo() { shooterServo.setPosition(1); } //resets tej teardrop that feeds the shooter - public void releaseShooterServo(){ + + public void releaseShooterServo(){ shooterServo.setPosition(0); } - +/* public void advanceCarousel() { spinServo.setPosition(spinServo.getPosition() + .06);//120 degrees/1800 servo degree range -= .06 } - - public void resetCarousel(){ - spinServo.setPosition(0); - } +*/ + //public void resetCarousel(){ + // spinServo.setPosition(0); + //} } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index c3b92387164..9ebabe974e2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -38,4 +38,4 @@ public void setTiltServo(double shooterTiltPositon){ * */ -} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java index 5dafe234c2d..fc6751a0280 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java @@ -26,12 +26,6 @@ public void runOpMode() { double nearShooterPower = .4; double farShooterPower = .6; double shooterPower = 0; - enum State { - IDLE, - RAMPING, - LAUNCHING, - - } State currentState = State.IDLE; double stateStartTime = 0; @@ -88,8 +82,11 @@ enum State { case LAUNCHING: if(getRuntime() > rampUpTimer) { shooter.setShooterPower(shooterPower);//reapply power to shooter + //add line to lift carousel.engageShooterServo(); - carousel.resetCarousel(); + carousel.releaseShooterServo(); + + //carousel.resetCarousel(); currentState = State.IDLE; } break; @@ -181,7 +178,7 @@ enum State { TelemetryPacket packet = new TelemetryPacket(); packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), pose); + //Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); } } @@ -189,5 +186,12 @@ enum State { } + + private enum State { + IDLE, + RAMPING, + LAUNCHING, + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/tempr.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/tempr.java deleted file mode 100644 index 352fcba1fe0..00000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/tempr.java +++ /dev/null @@ -1,74 +0,0 @@ -package org.firstinspires.ftc.teamcode.robot; -@TeleOp(name="TriggerStateMachine") -public class TriggerStateMachine extends OpMode { - - private DcMotor dcMotor; - private DcMotor gobildaMotor; - private Servo servo; - private Servo gobildaServo; - - // State variables - private enum State { - IDLE, - RIGHT_TRIGGER_RUNNING, - LEFT_TRIGGER_RUNNING - } - - private State currentState = State.IDLE; - private double stateStartTime = 0; - - @Override - public void init() { - dcMotor = hardwareMap.get(DcMotor.class, "dcMotor"); - gobildaMotor = hardwareMap.get(DcMotor.class, "gobildaMotor"); - servo = hardwareMap.get(Servo.class, "servo"); - gobildaServo = hardwareMap.get(Servo.class, "gobildaServo"); - } - - @Override - public void loop() { - - switch (currentState) { - - case IDLE: - // Default motor state - dcMotor.setPower(0); - - // Start RIGHT sequence - if (gamepad1.right_trigger > 0.1) { - dcMotor.setPower(1.0); // Start action - currentState = State.RIGHT_TRIGGER_RUNNING; - stateStartTime = runtime.seconds(); - } - - // Start LEFT sequence - else if (gamepad1.left_trigger > 0.1) { - dcMotor.setPower(0.5); // Start action - currentState = State.LEFT_TRIGGER_RUNNING; - stateStartTime = runtime.seconds(); - } - - break; - - case RIGHT_TRIGGER_RUNNING: - // Has 2 seconds passed? - if (runtime.seconds() - stateStartTime >= 2.0) { - dcMotor.setPower(0.2); - servo.setPosition(0.25); - currentState = State.IDLE; // Reset to idle state - } - break; - - case LEFT_TRIGGER_RUNNING: - // Has 2 seconds passed? - if (runtime.seconds() - stateStartTime >= 2.0) { - gobildaMotor.setPower(0.6); - gobildaServo.setPosition(0.6); - currentState = State.IDLE; // Reset to idle state - } - break; - } - } -} - - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tempBKFPlaygroundTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tempBKFPlaygroundTeleop.java deleted file mode 100644 index f8f45c77a11..00000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tempBKFPlaygroundTeleop.java +++ /dev/null @@ -1,112 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -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.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.robot.Carousel; -import org.firstinspires.ftc.teamcode.robot.Intake; -import org.firstinspires.ftc.teamcode.robot.MecanumDrive; -import org.firstinspires.ftc.teamcode.robot.Shooter; -import org.firstinspires.ftc.teamcode.tuning.TuningOpModes; - -@TeleOp(name = "tempBKFPlaygroundTeleop", group = "Teleop") -public class tempBKFPlaygroundTeleop extends LinearOpMode { - @Override - public void runOpMode() { - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - double RampUpTimer = 0; - boolean ShooterFlag = false; - int i = 0;//a variable to give make the code "wait" until the required number of seconds have passed to begin launching the artifact - int carouselCounter = 0;//counts the number of times the carousel has advanced 120 degrees. After 3 advances, we need to rest the carousel's position - - if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { - MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); - Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter - Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel - - waitForStart(); - - while (opModeIsActive()) { - if (gamepad2.left_bumper) { - intake.setPower(1); - } else if (gamepad2.right_bumper) { - intake.setPower(-1);//sets the power on the intake motor based on the values from the bumps - } -//------------------CODE FOR WHEN THE TRIGGERS ARE PRESSED---------------------------------- - - if ((gamepad2.left_trigger > 0.25) && (gamepad2.right_trigger < .01)) { - if (ShooterFlag = false) { - ShooterFlag = true; - RampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds - shooter.setPower(.6);//ramp up the shooter - } - //check to see if the shooter has ramped up for the required number of seconds. - //if so, reapply power, advance carousel, engage teardrop, reset teardrop, - //and count how many times the carousel has advanced (at 3, reset it to original position) - if (ShooterFlag = true) { - if (getRuntime() > RampUpTimer) { - if (carouselCounter < 3) { - shooter.setPower(.6); - //add code to advance carousel by 120 degrees - //carousel.flipShooterServo(); - //carousel.unflipShooterServo(); - carouselCounter++; - } else { - //add code to reset carousel to original position - } - - } else { - i = i++; - } - } - } else { - shooter.setPower(0); - } - - if ((gamepad2.left_trigger <= 0.25) && (gamepad2.right_trigger < .01)) { - //Braelan to add the code for the left trigger - } -//-------------------------------------------------------- - if (gamepad2.y) { - // carousel.flipFeedServo(); - } - - if (gamepad2.x) { - // carousel.unflipFeedServo(); - } - - if (gamepad2.a) { - shooter.setPower(0); - } - drive.setDrivePowers(new PoseVelocity2d( - new Vector2d( - -gamepad1.left_stick_y, - -gamepad1.left_stick_x - ), - -gamepad1.right_stick_x - )); - - drive.updatePoseEstimate(); - - Pose2d pose = drive.localizer.getPose(); - telemetry.addData("x", pose.position.x); - telemetry.addData("y", pose.position.y); - telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); - telemetry.update(); - - TelemetryPacket packet = new TelemetryPacket(); - packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), pose); - FtcDashboard.getInstance().sendTelemetryPacket(packet); - } - } - } -} - From 71f33b12684ff8799bb7f62b431c88b040c082eb Mon Sep 17 00:00:00 2001 From: Brenda Date: Fri, 5 Dec 2025 21:16:31 -0500 Subject: [PATCH 13/16] Code with state engine is working. Bot intakes, shoots and drives as of 12/5/2025. Setting up code to tilt and lift teardrop --- .../ftc/teamcode/robot/Carousel.java | 21 +++--- .../ftc/teamcode/robot/MecanumDrive.java | 8 ++- .../ftc/teamcode/robot/Shooter.java | 8 ++- .../ftc/teamcode/robot/SpiritTeleop2.java | 66 +++++-------------- 4 files changed, 38 insertions(+), 65 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java index 71689befa30..a389418527b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java @@ -5,25 +5,22 @@ public class Carousel { - private final Servo shooterServo; - //private final Servo spinServo; + private final Servo shooterServo;//lifts the teardrop to feed the artifact to the shooter + private final Servo carouselServo;//turns the carousel public Carousel(HardwareMap hardwareMap) { - - shooterServo = hardwareMap.get(Servo.class, "shooterServo");//teardrop from carousel to shooters - // spinServo = hardwareMap.get(Servo.class, "spinServo");//advances the carousel - - - + shooterServo = hardwareMap.get(Servo.class, "shooterServo"); + carouselServo = hardwareMap.get(Servo.class, "carouselServo"); } + public void tinyLiftShooterServo(){ + shooterServo.setPosition(.2); + } - //feeds an artifact to the shooter - public void engageShooterServo() { - shooterServo.setPosition(1); + public void fullLiftShooterServo(){ + shooterServo.setPosition(.8); } - //resets tej teardrop that feeds the shooter public void releaseShooterServo(){ shooterServo.setPosition(0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/MecanumDrive.java index a1c5c4a3d33..9a2e594f750 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/MecanumDrive.java @@ -238,10 +238,12 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); // TODO: reverse motor directions if needed - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - //leftBack.setDirection(DcMotorSimple.Direction.REVERSE); + //leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + rightFront.setDirection(DcMotorSimple.Direction.REVERSE); // leftBack.setDirection(DcMotorSimple.Direction.REVERSE); - rightBack.setDirection(DcMotorSimple.Direction.REVERSE); + //rightBack.setDirection(DcMotorSimple.Direction.REVERSE); + + // TODO: make sure your config has an IMU with this name (can be BNO or BHI) // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index 9ebabe974e2..63ab65f71e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -16,9 +16,11 @@ public Shooter(HardwareMap hardwareMap) { shooterMotorLeft = hardwareMap.get(DcMotorEx.class, "shooterMotorLeft"); shooterMotorRight = hardwareMap.get(DcMotorEx.class, "shooterMotorRight"); + tiltServo = hardwareMap.get(Servo.class, "tiltServo");//controls angle of shooters - shooterMotorRight.setDirection(DcMotorSimple.Direction.REVERSE); - shooterMotorLeft.setDirection(DcMotorSimple.Direction.FORWARD); + + shooterMotorRight.setDirection(DcMotorSimple.Direction.FORWARD); + shooterMotorLeft.setDirection(DcMotorSimple.Direction.REVERSE); } /** @@ -28,6 +30,8 @@ public void setShooterPower(double power) { shooterMotorLeft.setPower(power); shooterMotorRight.setPower(power); } + + public void setTiltServo(double shooterTiltPositon){ tiltServo.setPosition(shooterTiltPositon); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java index fc6751a0280..797e27c55f7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java @@ -23,11 +23,11 @@ public void runOpMode() { boolean rampUpFlag = false; int i = 0;//a variable to give make the code "wait" until the required number of seconds have passed to begin launching the artifact int carouselCounter = 0;//counts the number of times the carousel has advanced 120 degrees. After 3 advances, we need to rest the carousel's position -double nearShooterPower = .4; -double farShooterPower = .6; -double shooterPower = 0; + double nearShooterPower = .4;//power for shooter if bot is near target + double farShooterPower = .6;//power for shooter if bot is far from target + double shooterPower = 0;//power for shooter which is set to nearShooterPower or farShooterPower based on which trigger is pulled State currentState = State.IDLE; - double stateStartTime = 0; + if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); @@ -38,6 +38,7 @@ public void runOpMode() { waitForStart(); while (opModeIsActive()) + //Operate Intake: left bumper is intake and right bumper is eject { if (gamepad2.left_bumper) { intake.setPower(1); @@ -46,6 +47,8 @@ public void runOpMode() { } else { intake.setPower(0); } + + if () //------------------CODE FOR WHEN THE TRIGGERS ARE PRESSED---------------------------------- //--------------LEFT TRIGGER PULLED (CLOSE TO TARGET) ------------------------------ @@ -81,9 +84,12 @@ public void runOpMode() { case LAUNCHING: if(getRuntime() > rampUpTimer) { - shooter.setShooterPower(shooterPower);//reapply power to shooter - //add line to lift - carousel.engageShooterServo(); + //shooter.setShooterPower(shooterPower);//reapply power to shooter + carousel.tinyLiftShooterServo(); //the shooter servo a tiny bit so artifact is held + //addline to tilt shooters to near or far posiiton + //addline to lift the shooterServo to launch artifact + carousel.fullLiftShooterServo(); + wait carousel.releaseShooterServo(); //carousel.resetCarousel(); @@ -100,49 +106,14 @@ public void runOpMode() { //------------------------END OF SHOOTER CONTROL----------------------------------- - /* THIS CODE IS NOT NEEDED AND WILL BE DELETED AFTER THE CLASS IS REWRITTEN -//------------------------------------ CODE FOR RIGHT TRIGGER ---------------------------- - if ((gamepad2.right_trigger >.25) && (gamepad2.left_trigger <.01)) { - if (rampUpFlag == false) { - rampUpFlag = true; - rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds - shooter.setPower(.4);//ramp up the shooter - }else{ - shooter.setPower(0); - rampUpFlag = false; - } - //check to see if the shooter has ramped up for the required number of seconds. - //if so, reapply power, advance carousel, engage teardrop, reset teardrop, - //and count how many times the carousel has advanced (at 3, reset it to original position) - if (rampUpFlag == true) { - shooter.setPower(.4); - if (getRuntime() > rampUpTimer) { - if (carouselCounter < 5) { - //add code to advance carousel by 120 degrees which is .06 (120 degrees/1800 total degrees) - //carousel.flipShooterServo(); - //carousel.unflipShooterServo(); - carouselCounter++; - } else { - //carousel.resetCarousel(0); - carouselCounter = 0; - } -//this is a loop to make the code "wait" until the require number of seconds has passed (if statement cannot be empty so we gave it a task to do - } else { - i++; - } - } //else { - // shooter.setPower(0); - // shooterFlag = false; - // } - -//------------------END CODE FOR TRIGGERS-------------------------------------- -*/ + -------END CODE FOR TRIGGERS-------------------------------------- + if (gamepad2.y) { - // carousel.flipFeedServo(); + // } if (gamepad2.x) { - // carousel.unflipFeedServo(); + // } if (gamepad2.a) { @@ -157,8 +128,7 @@ public void runOpMode() { } if (gamepad2.b) { - intake.setPower(0); - telemetry.addData("button b", "button b"); + // } drive.setDrivePowers(new PoseVelocity2d( new Vector2d( From f0276564cb0a03a31abe65588fd75de0d2276ded Mon Sep 17 00:00:00 2001 From: Brenda Date: Sat, 6 Dec 2025 20:20:02 -0500 Subject: [PATCH 14/16] Clean code. values set for kicker's low, tiny lift, and full lift positions. Values set for new and far shooter tilt. Code with state engine is working. Bot intakes, shoots, lifts the kicker, tilts the shooter, and drives as of 12/6/2025. --- .../ftc/teamcode/robot/Carousel.java | 43 ++-- .../ftc/teamcode/robot/Shooter.java | 21 +- .../ftc/teamcode/robot/SpiritTeleop2.java | 214 +++++++++++------- 3 files changed, 167 insertions(+), 111 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java index a389418527b..8f075048051 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java @@ -7,32 +7,41 @@ public class Carousel { private final Servo shooterServo;//lifts the teardrop to feed the artifact to the shooter private final Servo carouselServo;//turns the carousel + double downKickerPosition = .9;//value to position kicker down + double tinyLiftKickerPosition = .8;//value to cause kicker to lift a tiny bit to allow tilt + double fullLiftKickerPosition = .5;//value to life kicker up to shooter + double carouselHomePosition = 0;//starting position of carousel + double carouselPosition = .33;//position to which carousel will move to advance + // double carouselIncrement = .33;//value needed to advance carousel by 120 degres public Carousel(HardwareMap hardwareMap) { shooterServo = hardwareMap.get(Servo.class, "shooterServo"); carouselServo = hardwareMap.get(Servo.class, "carouselServo"); } - - public void tinyLiftShooterServo(){ - shooterServo.setPosition(.2); - } - - public void fullLiftShooterServo(){ - shooterServo.setPosition(.8); + //-------------------KICK SHOOTER SERVO POSITIONING ---------------------- + //positions the shooter servo in the down position + public void downPositionKickerServo(){ + shooterServo.setPosition(downKickerPosition); } - public void releaseShooterServo(){ - shooterServo.setPosition(0); + //lifts the shooter servo up slightly so the carousel can move + public void tinyLiftKickerServo(){ + shooterServo.setPosition(tinyLiftKickerPosition); } -/* - public void advanceCarousel() - { - spinServo.setPosition(spinServo.getPosition() + .06);//120 degrees/1800 servo degree range -= .06 + //makes the shooter servo feed the artifact to the shooters + public void fullLiftKickerServo(){ + shooterServo.setPosition(fullLiftKickerPosition); } -*/ - //public void resetCarousel(){ - // spinServo.setPosition(0); - //} + + public void spinCarouselHome(){ + carouselServo.setPosition(carouselHomePosition); +} + public void spinCarousel() { + carouselServo.setPosition(carouselPosition); + } + // public void spinCarouselBackward(){ + // carouselServo.setPosition(); + // } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index 63ab65f71e3..5233f211fc1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -9,7 +9,9 @@ public class Shooter { public final DcMotorEx shooterMotorLeft; public final DcMotorEx shooterMotorRight; public final Servo tiltServo; - + public double nearTiltPosition = .4; + public double farTiltPosition = .5; + public double homeTiltPosition = 0; public Shooter(HardwareMap hardwareMap) { @@ -26,16 +28,23 @@ public Shooter(HardwareMap hardwareMap) { /** * Set the power to the left and right motors */ - public void setShooterPower(double power) { + public void setShooterPower(double power) { shooterMotorLeft.setPower(power); shooterMotorRight.setPower(power); - } + } + public void setHomeTiltServo( + tiltServo.setPosition(homeTiltPosition); + ) + + public void setNearTiltServo() { + tiltServo.setPosition(nearTiltPosition); + } - public void setTiltServo(double shooterTiltPositon){ - tiltServo.setPosition(shooterTiltPositon); - } + public void setFarTiltServo() { + tiltServo.setPosition(farTiltPosition); } +} /** Set position of servo that tilts the shooters diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java index 797e27c55f7..c1191a682fb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java @@ -21,11 +21,13 @@ public void runOpMode() { telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); double rampUpTimer = 0; boolean rampUpFlag = false; - int i = 0;//a variable to give make the code "wait" until the required number of seconds have passed to begin launching the artifact + //int i = 0;//a variable to give make the code "wait" until the required number of seconds have passed to begin launching the artifact int carouselCounter = 0;//counts the number of times the carousel has advanced 120 degrees. After 3 advances, we need to rest the carousel's position double nearShooterPower = .4;//power for shooter if bot is near target double farShooterPower = .6;//power for shooter if bot is far from target double shooterPower = 0;//power for shooter which is set to nearShooterPower or farShooterPower based on which trigger is pulled + + State currentState = State.IDLE; @@ -48,120 +50,156 @@ public void runOpMode() { intake.setPower(0); } - if () + +//----------------------JUST FOR TESTING POSITON OF SHOOTER SERVO------- + if (gamepad2.a) { + carousel.downPositionKickerServo(); + + } + if (gamepad2.x) { + carousel.tinyLiftKickerServo(); + } + if (gamepad2.y) { + carousel.fullLiftKickerServo(); + } +//------------------JUST FOR TESTING POSITION OF TILT SERVO + if (gamepad1.a) { + shooter.setNearTiltServo(); + } + if (gamepad1.b) { + shooter.setFarTiltServo(); + } +//------------------JUST FOR TESTING POSITION OF CAROUSEL + if (gamepad1.x) { + carousel.spinCarousel(); + } + + if (gamepad1.y) { + carousel.spinCarousel(); + } + //--------------------------------END CODE FOR TESTING POSITION OF SERVOS + + } //------------------CODE FOR WHEN THE TRIGGERS ARE PRESSED---------------------------------- - //--------------LEFT TRIGGER PULLED (CLOSE TO TARGET) ------------------------------ + //--------------LEFT TRIGGER PULLED (CLOSE TO TARGET) ------------------------------ - switch (currentState) { + switch (currentState) { - case IDLE: - //handleIdle(); - shooter.setShooterPower(0.00);//stop shooter - carousel.releaseShooterServo();//move shooter feeder to down position + case IDLE: + //handleIdle(); + shooter.setShooterPower(0.00);//stop shooter + carousel.downPositionKickerServo();//make sure kicker is in down position + shooter.setHomeTiltServo(); - if ((gamepad2.right_trigger >.25) && (gamepad2.left_trigger <.01)) - { - shooterPower = nearShooterPower; - shooter.setShooterPower(shooterPower);//set power to shoot near target - currentState = State.RAMPING; - rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds - } - if ((gamepad2.left_trigger >.25) && (gamepad2.right_trigger <.01)) - { - shooterPower=farShooterPower; - shooter.setShooterPower(shooterPower);//set power to shoot far from target - currentState = State.RAMPING; - rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds - } - break; + if ((gamepad2.right_trigger > .25) && (gamepad2.left_trigger < .01)) { + shooterPower = nearShooterPower; + shooter.setShooterPower(shooterPower);//set power to shoot near target + currentState = State.RAMPING; + rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds + } + if ((gamepad2.left_trigger > .25) && (gamepad2.right_trigger < .01)) { + shooterPower = farShooterPower; + shooter.setShooterPower(shooterPower);//set power to shoot far from target + currentState = State.RAMPING; + rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds + } + break; - case RAMPING: - if(getRuntime() > rampUpTimer) { - shooter.setShooterPower(shooterPower);//set power to shooter - currentState = State.LAUNCHING; - } - break; + case RAMPING: + if (getRuntime() > rampUpTimer) { + shooter.setShooterPower(shooterPower);//set power to shooter + currentState = State.LAUNCHING; + } + break; + + case LAUNCHING: + if (getRuntime() > rampUpTimer) { + + for (int i = 1; 1 <= 3; i++) { + + //lift kicker a tiny bit & cup the artifact so the tilt can happen + //tilt to near or far + //lift kicker the whole way to launch + //set tilt to home position + //lower kicker whole way + //rotate carousel 120 degrees + + //sleep 500 //firing duration + //reset tilt to home position + //reset kicker to down position - case LAUNCHING: - if(getRuntime() > rampUpTimer) { - //shooter.setShooterPower(shooterPower);//reapply power to shooter - carousel.tinyLiftShooterServo(); //the shooter servo a tiny bit so artifact is held - //addline to tilt shooters to near or far posiiton - //addline to lift the shooterServo to launch artifact - carousel.fullLiftShooterServo(); - wait - carousel.releaseShooterServo(); //carousel.resetCarousel(); currentState = State.IDLE; } + } + //set carousel back to home position break; - } - telemetry.addData("State", currentState); - telemetry.addData("Shooter Power", shooterPower); - //telemetry.addData("Servo Pos", carousel.getPosition()); - telemetry.update(); - //------------------------END OF SHOOTER CONTROL----------------------------------- + } + telemetry.addData("State", currentState); + telemetry.addData("Shooter Power", shooterPower); + //telemetry.addData("Servo Pos", carousel.getPosition()); + telemetry.update(); + //------------------------END OF SHOOTER CONTROL----------------------------------- - -------END CODE FOR TRIGGERS-------------------------------------- + //-------END CODE FOR TRIGGERS-------------------------------------- - if (gamepad2.y) { - // - } - - if (gamepad2.x) { - // - } + if (gamepad2.y) { + // + } - if (gamepad2.a) { - if (carouselCounter <= 5) { - // carousel.advanceCarousel(); - carouselCounter++; - } else { + if (gamepad2.x) { // - // carousel.resetCarousel(); - carouselCounter = 0; } - } - if (gamepad2.b) { - // - } - drive.setDrivePowers(new PoseVelocity2d( - new Vector2d( - -gamepad1.left_stick_y, - -gamepad1.left_stick_x - ), - -gamepad1.right_stick_x - )); - - drive.updatePoseEstimate(); - - Pose2d pose = drive.localizer.getPose(); - telemetry.addData("x", pose.position.x); - telemetry.addData("y", pose.position.y); - telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); - telemetry.update(); - - TelemetryPacket packet = new TelemetryPacket(); - packet.fieldOverlay().setStroke("#3F51B5"); - //Drawing.drawRobot(packet.fieldOverlay(), pose); - FtcDashboard.getInstance().sendTelemetryPacket(packet); + if (gamepad2.a) { + if (carouselCounter <= 5) { + // carousel.advanceCarousel(); + carouselCounter++; + } else { + // + // carousel.resetCarousel(); + carouselCounter = 0; + } + } + + if (gamepad2.b) { + // + } + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x + ), + -gamepad1.right_stick_x + )); + + drive.updatePoseEstimate(); + + Pose2d pose = drive.localizer.getPose(); + telemetry.addData("x", pose.position.x); + telemetry.addData("y", pose.position.y); + telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); + telemetry.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + //Drawing.drawRobot(packet.fieldOverlay(), pose); + FtcDashboard.getInstance().sendTelemetryPacket(packet); } } - telemetry.update(); - } + } - private enum State { + enum State { IDLE, RAMPING, LAUNCHING, - } -} + + From 6a121f60315d3f75923bc0bee9a273c6dc784ba1 Mon Sep 17 00:00:00 2001 From: Brenda Date: Sat, 6 Dec 2025 21:34:46 -0500 Subject: [PATCH 15/16] Added gamepad2 button b to advance carousel manually --- .../ftc/teamcode/robot/Carousel.java | 13 +++-- .../ftc/teamcode/robot/Shooter.java | 23 ++++---- .../ftc/teamcode/robot/SpiritTeleop2.java | 53 ++++++++++--------- 3 files changed, 49 insertions(+), 40 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java index 8f075048051..d5c1a5636eb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java @@ -1,12 +1,15 @@ package org.firstinspires.ftc.teamcode.robot; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.PwmControl; import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.ServoImplEx; public class Carousel { private final Servo shooterServo;//lifts the teardrop to feed the artifact to the shooter - private final Servo carouselServo;//turns the carousel + // private final Servo carouselServo;//turns the carousel + private final ServoImplEx carouselServo;//turns the carousel double downKickerPosition = .9;//value to position kicker down double tinyLiftKickerPosition = .8;//value to cause kicker to lift a tiny bit to allow tilt double fullLiftKickerPosition = .5;//value to life kicker up to shooter @@ -17,14 +20,16 @@ public class Carousel { public Carousel(HardwareMap hardwareMap) { shooterServo = hardwareMap.get(Servo.class, "shooterServo"); - carouselServo = hardwareMap.get(Servo.class, "carouselServo"); + carouselServo = hardwareMap.get(ServoImplEx.class, "carouselServo"); + // carouselServo.setPwmRange(new PwmRange(500, 2500)); + carouselServo.setPwmRange(new PwmControl.PwmRange(500,2500)); } //-------------------KICK SHOOTER SERVO POSITIONING ---------------------- + //positions the shooter servo in the down position - public void downPositionKickerServo(){ + public void setDownPositionKickerServo(){ shooterServo.setPosition(downKickerPosition); } - //lifts the shooter servo up slightly so the carousel can move public void tinyLiftKickerServo(){ shooterServo.setPosition(tinyLiftKickerPosition); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index 5233f211fc1..32600ee5875 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -9,9 +9,9 @@ public class Shooter { public final DcMotorEx shooterMotorLeft; public final DcMotorEx shooterMotorRight; public final Servo tiltServo; - public double nearTiltPosition = .4; - public double farTiltPosition = .5; - public double homeTiltPosition = 0; + public double nearTiltPosition = .4;//used for testing + public double farTiltPosition = .5;//used for testing + public double homeTiltPosition = 0; public Shooter(HardwareMap hardwareMap) { @@ -33,17 +33,20 @@ public void setShooterPower(double power) { shooterMotorRight.setPower(power); } - public void setHomeTiltServo( - tiltServo.setPosition(homeTiltPosition); - ) - - public void setNearTiltServo() { - tiltServo.setPosition(nearTiltPosition); + public void setTiltServo(double tiltPosition) { + tiltServo.setPosition(tiltPosition); + } + //used for testing + public void setNearTiltServo(){ + tiltServo.setPosition(nearTiltPosition); } - public void setFarTiltServo() { + //used for testing + public void setFarTiltServo(){ tiltServo.setPosition(farTiltPosition); } + + } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java index c1191a682fb..d186e0252cc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java @@ -7,6 +7,7 @@ import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.PwmControl.PwmRange; import org.firstinspires.ftc.teamcode.robot.Carousel; import org.firstinspires.ftc.teamcode.robot.Intake; @@ -21,12 +22,13 @@ public void runOpMode() { telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); double rampUpTimer = 0; boolean rampUpFlag = false; - //int i = 0;//a variable to give make the code "wait" until the required number of seconds have passed to begin launching the artifact int carouselCounter = 0;//counts the number of times the carousel has advanced 120 degrees. After 3 advances, we need to rest the carousel's position - double nearShooterPower = .4;//power for shooter if bot is near target - double farShooterPower = .6;//power for shooter if bot is far from target + double nearShooterPower = .4;//power for shooter if bot is near target + double farShooterPower = .6;//power for shooter if bot is far from target double shooterPower = 0;//power for shooter which is set to nearShooterPower or farShooterPower based on which trigger is pulled - + double nearTiltPosition = 0;//position of tilt for shooting near the target + double farTiltPosition = 0;//position of the tilt for shooting far from the target + double tiltPosition = 0;//position of the tile. Will be set to NearTiltPosition or farTiltPosition in the code's IDLE case State currentState = State.IDLE; @@ -53,7 +55,7 @@ public void runOpMode() { //----------------------JUST FOR TESTING POSITON OF SHOOTER SERVO------- if (gamepad2.a) { - carousel.downPositionKickerServo(); + carousel.setDownPositionKickerServo(); } if (gamepad2.x) { @@ -88,17 +90,20 @@ public void runOpMode() { case IDLE: //handleIdle(); shooter.setShooterPower(0.00);//stop shooter - carousel.downPositionKickerServo();//make sure kicker is in down position - shooter.setHomeTiltServo(); - + carousel.setDownPositionKickerServo();//make sure kicker is in down position + tiltPosition = 0;//make sure shooter is not tilted (prevents kicker from lifting) + shooter.setTiltServo(tiltPosition);//make sure shooter is not tilted (prevents kicker from lifting) + //Right trigger pulled: if ((gamepad2.right_trigger > .25) && (gamepad2.left_trigger < .01)) { shooterPower = nearShooterPower; + tiltPosition = nearTiltPosition; shooter.setShooterPower(shooterPower);//set power to shoot near target currentState = State.RAMPING; rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds } if ((gamepad2.left_trigger > .25) && (gamepad2.right_trigger < .01)) { shooterPower = farShooterPower; + tiltPosition = farTiltPosition; shooter.setShooterPower(shooterPower);//set power to shoot far from target currentState = State.RAMPING; rampUpTimer = getRuntime() + 2;//set a timer to the length of time the op has been running + 2 seconds @@ -116,24 +121,21 @@ public void runOpMode() { if (getRuntime() > rampUpTimer) { for (int i = 1; 1 <= 3; i++) { + carousel.spinCarousel();//spin carousel + carousel.tinyLiftKickerServo();//lift kicker a tiny bit & cup the artifact so the tilt can happen + shooter.setTiltServo(tiltPosition);//tilt for near or far shooting based on value set in IDLE case + carousel.fullLiftKickerServo();//lift kicker the whole way to launch + sleep(2000);//pause to launch + //Reset tilt and kicker + tiltPosition = 0; + shooter.setTiltServo(tiltPosition);//reset tilt to home position + carousel.setDownPositionKickerServo();//lower the kicker the whole way - //lift kicker a tiny bit & cup the artifact so the tilt can happen - //tilt to near or far - //lift kicker the whole way to launch - //set tilt to home position - //lower kicker whole way - //rotate carousel 120 degrees - - //sleep 500 //firing duration - //reset tilt to home position - //reset kicker to down position - - - //carousel.resetCarousel(); currentState = State.IDLE; } } //set carousel back to home position + carousel.spinCarouselHome(); break; @@ -155,13 +157,12 @@ public void runOpMode() { // } - if (gamepad2.a) { - if (carouselCounter <= 5) { - // carousel.advanceCarousel(); + if (gamepad2.b) { + if (carouselCounter <= 3) { + carousel.spinCarousel(); carouselCounter++; } else { - // - // carousel.resetCarousel(); + carousel.spinCarouselHome(); carouselCounter = 0; } } From 29a69626b41b458b3010c8cff4085102ba7ca653 Mon Sep 17 00:00:00 2001 From: Brenda Date: Sun, 7 Dec 2025 19:30:09 -0500 Subject: [PATCH 16/16] commit to pull previous version --- .../firstinspires/ftc/teamcode/robot/Carousel.java | 14 ++++++++------ .../ftc/teamcode/robot/SpiritTeleop2.java | 14 ++++++++------ 2 files changed, 16 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java index d5c1a5636eb..4ed9bb6acb5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java @@ -3,26 +3,28 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.PwmControl; import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.robotcore.hardware.ServoImplEx; +//import com.qualcomm.robotcore.hardware.ServoImplEx; public class Carousel { private final Servo shooterServo;//lifts the teardrop to feed the artifact to the shooter // private final Servo carouselServo;//turns the carousel - private final ServoImplEx carouselServo;//turns the carousel + //private final ServoImplEx carouselServo;//turns the carousel + private final Servo carouselServo;//turns the carousel double downKickerPosition = .9;//value to position kicker down double tinyLiftKickerPosition = .8;//value to cause kicker to lift a tiny bit to allow tilt double fullLiftKickerPosition = .5;//value to life kicker up to shooter double carouselHomePosition = 0;//starting position of carousel - double carouselPosition = .33;//position to which carousel will move to advance - // double carouselIncrement = .33;//value needed to advance carousel by 120 degres + double carouselPosition = .2;//position to which carousel will move to advance + double carouselPositionIncrement = .2;//value needed to advance carousel by 120 degrees public Carousel(HardwareMap hardwareMap) { shooterServo = hardwareMap.get(Servo.class, "shooterServo"); - carouselServo = hardwareMap.get(ServoImplEx.class, "carouselServo"); + carouselServo = hardwareMap.get(Servo.class, "carouselServo"); + // carouselServo = hardwareMap.get(ServoImplEx.class, "carouselServo"); // carouselServo.setPwmRange(new PwmRange(500, 2500)); - carouselServo.setPwmRange(new PwmControl.PwmRange(500,2500)); + // carouselServo.setPwmRange(new PwmControl.PwmRange(500,2500)); } //-------------------KICK SHOOTER SERVO POSITIONING ---------------------- diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java index d186e0252cc..1331a7d9b95 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java @@ -72,10 +72,12 @@ public void runOpMode() { shooter.setFarTiltServo(); } //------------------JUST FOR TESTING POSITION OF CAROUSEL + + //spin carousel forward if (gamepad1.x) { carousel.spinCarousel(); } - + //spin carousel backward if (gamepad1.y) { carousel.spinCarousel(); } @@ -149,15 +151,15 @@ public void runOpMode() { //-------END CODE FOR TRIGGERS-------------------------------------- - if (gamepad2.y) { + //if (gamepad2.y) { // - } + // } - if (gamepad2.x) { + // if (gamepad2.x) { // - } + // } - if (gamepad2.b) { + if (gamepad1.x) { if (carouselCounter <= 3) { carousel.spinCarousel(); carouselCounter++;