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(); + } +} 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..4ed9bb6acb5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java @@ -0,0 +1,54 @@ +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 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 = .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(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 setDownPositionKickerServo(){ + shooterServo.setPosition(downKickerPosition); + } + //lifts the shooter servo up slightly so the carousel can move + public void tinyLiftKickerServo(){ + shooterServo.setPosition(tinyLiftKickerPosition); + } + //makes the shooter servo feed the artifact to the shooters + public void fullLiftKickerServo(){ + shooterServo.setPosition(fullLiftKickerPosition); + } + + 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/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/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/MecanumDrive.java similarity index 97% 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 b919194ab23..9a2e594f750 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; @@ -58,9 +60,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; @@ -236,7 +238,12 @@ 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); + rightFront.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 new file mode 100644 index 00000000000..32600ee5875 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -0,0 +1,57 @@ +package org.firstinspires.ftc.teamcode.robot; + +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 { + public final DcMotorEx shooterMotorLeft; + public final DcMotorEx shooterMotorRight; + public final Servo tiltServo; + public double nearTiltPosition = .4;//used for testing + public double farTiltPosition = .5;//used for testing + public double homeTiltPosition = 0; + + + 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.FORWARD); + shooterMotorLeft.setDirection(DcMotorSimple.Direction.REVERSE); + } + + /** + * Set the power to the left and right motors + */ + public void setShooterPower(double power) { + shooterMotorLeft.setPower(power); + shooterMotorRight.setPower(power); + } + + public void setTiltServo(double tiltPosition) { + tiltServo.setPosition(tiltPosition); + } + //used for testing + public void setNearTiltServo(){ + tiltServo.setPosition(nearTiltPosition); + } + + //used for testing + 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 new file mode 100644 index 00000000000..1331a7d9b95 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java @@ -0,0 +1,208 @@ +package org.firstinspires.ftc.teamcode.robot; +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 com.qualcomm.robotcore.hardware.PwmControl.PwmRange; + +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 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 + 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; + + + 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()) + //Operate Intake: left bumper is intake and right bumper is eject + { + 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); + } + + +//----------------------JUST FOR TESTING POSITON OF SHOOTER SERVO------- + if (gamepad2.a) { + carousel.setDownPositionKickerServo(); + + } + 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 + + //spin carousel forward + if (gamepad1.x) { + carousel.spinCarousel(); + } + //spin carousel backward + 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) ------------------------------ + + switch (currentState) { + + case IDLE: + //handleIdle(); + shooter.setShooterPower(0.00);//stop shooter + 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 + } + 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++) { + 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 + + currentState = State.IDLE; + } + } + //set carousel back to home position + carousel.spinCarouselHome(); + break; + + + } + 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-------------------------------------- + + //if (gamepad2.y) { + // + // } + + // if (gamepad2.x) { + // + // } + + if (gamepad1.x) { + if (carouselCounter <= 3) { + carousel.spinCarousel(); + carouselCounter++; + } else { + carousel.spinCarouselHome(); + 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); + } + } + + + } + + enum State { + IDLE, + RAMPING, + LAUNCHING, + } + + + 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; diff --git a/build.gradle b/build.gradle index e70f2095b7a..d9fca8588c6 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' } } @@ -27,3 +27,4 @@ allprojects { repositories { mavenCentral() } + 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