From 84c9b082054888fb09e4d84488d507bde9fbf52e Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Wed, 26 Nov 2025 19:03:43 -0600 Subject: [PATCH] 11/26 edits --- .../ftc/teamcode/autonomous/redDaniel.java | 203 ++++++++++++++++++ .../ftc/teamcode/constants/Poses.java | 6 +- .../teamcode/constants/ServoPositions.java | 10 +- .../ftc/teamcode/constants/ShooterVars.java | 2 + .../ftc/teamcode/tests/ShooterTest.java | 6 +- .../utils/ConfigureColorRangefinder.java | 2 +- .../utils/PositionalServoProgrammer.java | 9 + .../ftc/teamcode/utils/Robot.java | 24 +++ 8 files changed, 252 insertions(+), 10 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java new file mode 100644 index 0000000..f1864f1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java @@ -0,0 +1,203 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.constants.Poses.*; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; +import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; + + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.ParallelAction; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.SequentialAction; +import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.ftc.Actions; + +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.subsystems.AprilTag; +import org.firstinspires.ftc.teamcode.utils.Robot; + +@Config +@Autonomous +public class redDaniel extends LinearOpMode { + + Robot robot; + + MultipleTelemetry TELE; + + MecanumDrive drive; + + AprilTag aprilTag; + + public Action initShooter(int velocity){ + return new Action(){ + double velo = 0.0; + double initPos = 0.0; + double stamp = 0.0; + double powPID = 0.0; + final int maxVel = 4500; + double ticker = 0.0; + public boolean run(@NonNull TelemetryPacket telemetryPacket){ + velo = -60 * ((((double) robot.shooter1.getCurrentPosition() / 2048) - initPos) / (getRuntime() - stamp)); + stamp = getRuntime(); + initPos = (double) robot.shooter1.getCurrentPosition() / 2048; + if (Math.abs(velocity - velo) > 3 * velTolerance) { + powPID = (double) velocity / maxVel; + ticker = getRuntime(); + } else if (velocity - velTolerance > velo) { + powPID = powPID + 0.0001; + ticker = getRuntime(); + } else if (velocity + velTolerance < velo) { + powPID = powPID - 0.0001; + ticker = getRuntime(); + } + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + robot.transfer.setPower((powPID / 2) + 0.5); + + return getRuntime() - ticker < 0.5; + } + }; + } + + public Action Shoot(double spindexer){ + return new Action() { + boolean transfer = false; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + robot.spin1.setPosition(spindexer); + robot.spin2.setPosition(1-spindexer); + if (robot.spin1Pos.getVoltage() / 3.3 == spindexer){ + robot.transferServo.setPosition(transferServo_in); + transfer = true; + } + if (robot.transferServoPos.getVoltage() / 3.3 == transferServo_in && transfer){ + robot.transferServo.setPosition(transferServo_out); + return false; + } + return true; + } + }; + } + + @Override + public void runOpMode() throws InterruptedException { + + robot = new Robot(hardwareMap); + + TELE = new MultipleTelemetry( + telemetry, FtcDashboard.getInstance().getTelemetry() + ); + + drive = new MecanumDrive(hardwareMap, new Pose2d( + 0, 0, 0 + )); + + aprilTag = new AprilTag(robot, TELE); + + TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) + .strafeToLinearHeading(new Vector2d(x1, y1), h1); + + TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1)) + .turnTo(Math.toRadians(h2)) + .strafeToLinearHeading(new Vector2d(x2, y2), h2); + + TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(x2, y2, h2)) + .strafeToLinearHeading(new Vector2d(x1, y1), h1); + + TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(x1, y1, h1)) + + .strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b) + + .strafeToLinearHeading(new Vector2d(x3, y3), h3); + + TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(x3, y3, h3)) + .strafeToLinearHeading(new Vector2d(x1, y1), h1); + + TrajectoryActionBuilder park = drive.actionBuilder(new Pose2d(x1, y1, h1)) + .strafeToLinearHeading(new Vector2d(x1, y1 + 30), h1); + + while (opModeInInit()) { + + if (gamepad2.dpadUpWasPressed()) { + hoodDefault -= 0.01; + } + + if (gamepad2.dpadDownWasPressed()) { + hoodDefault += 0.01; + } + + robot.hood.setPosition(hoodDefault); + + robot.turr1.setPosition(turret_red); + robot.turr2.setPosition(1 - turret_red); + + robot.transferServo.setPosition(transferServo_out); + + aprilTag.initTelemetry(); + + aprilTag.update(); + + TELE.update(); + } + + waitForStart(); + + if (isStopRequested()) return; + + if (opModeIsActive()) { + + robot.hood.setPosition(hoodDefault); + + Actions.runBlocking( + new ParallelAction( + shoot0.build() + ) + ); + + + + Actions.runBlocking( + pickup1.build() + ); + + Actions.runBlocking( + shoot1.build() + ); + + + Actions.runBlocking( + pickup2.build() + ); + + Actions.runBlocking( + shoot2.build() + ); + + + Actions.runBlocking( + park.build() + ); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + TELE.addLine("finished"); + + TELE.update(); + + sleep(2000); + + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java index 6cc3514..1f3cedb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -15,12 +15,12 @@ public class Poses { public static double x1 = 50, y1 = 0, h1 = 0; - public static double x2 = 31, y2 = 32, h2 = Math.toRadians(135); + public static double x2 = 31, y2 = 32, h2 = Math.toRadians(140); - public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(135); + public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(140); - public static double x3 = 34, y3 = 58, h3 = Math.toRadians(135); + public static double x3 = 34, y3 = 58, h3 = Math.toRadians(140); public static Pose2d teleStart = new Pose2d(x1,-10,0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 3e87944..d7b0916 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -15,12 +15,16 @@ public class ServoPositions { public static double transferServo_out = 0.13; - public static double transferServo_in = 0.4; + public static double transferServo_in = 0.36; public static double hoodDefault = 0.35; - public static double turret_red = 0.33; - public static double turret_blue = 0.3; + public static double hoodHigh = 0.21; + + public static double hoodLow = 1.0; + + public static double turret_red = 0.43; + public static double turret_blue = 0.4; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java index b2c70b8..fe75b9b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java @@ -7,4 +7,6 @@ public class ShooterVars { public static double turret_GearRatio = 0.9974; public static double turret_Range = 355; + + public static int velTolerance = 500; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index 934106f..ad70ecd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -83,7 +83,7 @@ public class ShooterTest extends LinearOpMode { robot.hood.setPosition(hoodPos); robot.turr1.setPosition(turretPos); - robot.turr2.setPosition(turretPos); + robot.turr2.setPosition(1-turretPos); velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1)); stamp1 = getRuntime(); @@ -91,9 +91,9 @@ public class ShooterTest extends LinearOpMode { if (Math.abs(vel - velo1) > 3 * tolerance) { powPID = vel / maxVel; } else if (vel - tolerance > velo1) { - powPID = powPID + 0.001; + powPID = powPID + 0.0001; } else if (vel + tolerance < velo1) { - powPID = powPID - 0.001; + powPID = powPID - 0.0001; } shooter.setVelocity(powPID); robot.transfer.setPower((powPID / 2) + 0.5); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java index e6edc8b..48250e9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java @@ -11,7 +11,7 @@ public class ConfigureColorRangefinder extends LinearOpMode { public static int LED_Brightness = 50; - public static int lowerGreen = 100; + public static int lowerGreen = 120; public static int higherGreen = 150; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java index 019ecce..0734891 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java @@ -1,6 +1,8 @@ package org.firstinspires.ftc.teamcode.utils; +import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -8,6 +10,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @Config public class PositionalServoProgrammer extends LinearOpMode { Robot robot; + MultipleTelemetry TELE; public static double spindexPos = 0.501; public static double turretPos = 0.501; public static double transferPos = 0.501; @@ -15,6 +18,7 @@ public class PositionalServoProgrammer extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); waitForStart(); if (isStopRequested()) return; while (opModeIsActive()){ @@ -32,6 +36,11 @@ public class PositionalServoProgrammer extends LinearOpMode { if (hoodPos != 0.501){ robot.hood.setPosition(hoodPos); } + TELE.addData("spindexer", 1.111111111*((robot.spin1Pos.getVoltage() / 3.3) - 0.05)); + TELE.addData("hood", 1.111111111*((robot.hoodPos.getVoltage() / 3.3) - 0.05)); + TELE.addData("transferServo", 1.111111111*((robot.transferServoPos.getVoltage() / 3.3) - 0.05)); + TELE.addData("turret", 1.111111111*((robot.turr1Pos.getVoltage() / 3.3) - 0.05)); + TELE.update(); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index f0f5f6a..910e671 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -59,6 +59,18 @@ public class Robot { public AnalogInput analogInput2; + public AnalogInput spin1Pos; + + public AnalogInput spin2Pos; + + public AnalogInput hoodPos; + + public AnalogInput turr1Pos; + + public AnalogInput turr2Pos; + + public AnalogInput transferServoPos; + public AprilTagProcessor aprilTagProcessor; public WebcamName webcam; @@ -92,14 +104,24 @@ public class Robot { hood = hardwareMap.get(Servo.class, "hood"); + hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos"); + turr1 = hardwareMap.get(Servo.class, "t1"); + turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); + turr2 = hardwareMap.get(Servo.class, "t2"); + turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos"); + spin1 = hardwareMap.get(Servo.class, "spin1"); + spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos"); + spin2 = hardwareMap.get(Servo.class, "spin2"); + spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos"); + pin0 = hardwareMap.get(DigitalChannel.class, "pin0"); pin1 = hardwareMap.get(DigitalChannel.class, "pin1"); @@ -120,6 +142,8 @@ public class Robot { transferServo = hardwareMap.get(Servo.class, "transferServo"); + transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos"); + transfer.setDirection(DcMotorSimple.Direction.REVERSE); aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();