From 4e5f0dd43d89bc813a660458d6f37c4104486565 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Fri, 7 Nov 2025 00:10:18 -0600 Subject: [PATCH] Alr we got teleop working but just barely....almost finished with the gridn yk? --- .../teamcode/libs/RR/PinpointLocalizer.java | 2 +- .../ftc/teamcode/subsystems/Shooter.java | 62 +++-- .../ftc/teamcode/subsystems/Transfer.java | 12 + .../ftc/teamcode/teleop/TeleopV1.java | 217 +++++++++++++++++- .../ftc/teamcode/utils/Robot.java | 2 + 5 files changed, 268 insertions(+), 27 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java index 678c8ba..6464c63 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java @@ -16,7 +16,7 @@ import java.util.Objects; @Config public final class PinpointLocalizer implements Localizer { public static class Params { - public double parYTicks = 3765.023079161767; // y position of the parallel encoder (in tick units) + public double parYTicks = -3765.023079161767; // y position of the parallel encoder (in tick units) public double perpXTicks = -1962.6377639490684; // x position of the perpendicular encoder (in tick units) } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java index ab33e6d..a850a84 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java @@ -168,7 +168,7 @@ public class Shooter implements Subsystem { public void setTurretMode(String mode){ turretMode = mode;} - public void trackGoal(Pose2d robotPose, Pose2d goalPose, boolean shooterOn){ + public double trackGoal(Pose2d robotPose, Pose2d goalPose){ fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); @@ -178,7 +178,7 @@ public class Shooter implements Subsystem { Pose2d deltaPose = new Pose2d( goalPose.position.x - robotPose.position.x, goalPose.position.y - robotPose.position.y, - goalPose.heading.toDouble() - robotPose.heading.toDouble() + goalPose.heading.toDouble() - (robotPose.heading.toDouble()) ); double distance = Math.sqrt( @@ -187,25 +187,19 @@ public class Shooter implements Subsystem { + Poses.relativeGoalHeight * Poses.relativeGoalHeight ); + telemetry.addData("dst", distance); + double shooterPow = getPowerByDist(distance); double hoodAngle = getAngleByDist(distance); - if (shooterOn){ - fly1.setVelocity(shooterPow); - fly2.setPower(fly1.getPower()); - - } else { - fly1.setPower(0); - fly2.setPower(0); - } - - - hoodServo.setPosition(hoodAngle); +// hoodServo.setPosition(hoodAngle); moveTurret(getTurretPosByDeltaPose(deltaPose)); + return distance; + @@ -220,17 +214,38 @@ public class Shooter implements Subsystem { double deltaAngle = Math.toDegrees(dPose.heading.toDouble()); - if (deltaAngle < -180) { - deltaAngle +=360; + double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y/dPose.position.x)); + + telemetry.addData("deltaAngle", deltaAngle); + + + + if (deltaAngle > 90) { + deltaAngle -=360; } - deltaAngle /= (0.9974*355); - return (0.5+deltaAngle) ; + +// deltaAngle += aTanAngle; + + deltaAngle /= (335); + + telemetry.addData("dAngle", deltaAngle); + + telemetry.addData("AtanAngle", aTanAngle); + + + return (0.30-deltaAngle) ; + } + //62, 0.44 + + //56.5, 0.5 + + public double getPowerByDist(double dist){ //TODO: ADD LOGIC @@ -239,8 +254,15 @@ public class Shooter implements Subsystem { public double getAngleByDist(double dist){ - //TODO: ADD LOGIC - return dist; + + double newDist = dist - 56.5; + + double pos = newDist*((0.44-0.5)/(62-56.5)) + 0.46; + + + + + return pos; } @@ -285,7 +307,7 @@ public class Shooter implements Subsystem { } if (Objects.equals(turretMode, "MANUAL")){ - hoodServo.setPosition(hoodPos); +// hoodServo.setPosition(hoodPos); moveTurret(turretPos); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java index 805073d..0b6f5f9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystems; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; + import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.Servo; @@ -31,6 +33,16 @@ public class Transfer implements Subsystem{ this.motorPow = pow; } + public void transferOut(){ + this.setTransferPosition(transferServo_out); + } + + public void transferIn(){ + this.setTransferPosition(transferServo_in); + } + + + @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java index f3ba06d..b9df4cd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java @@ -3,14 +3,17 @@ package org.firstinspires.ftc.teamcode.teleop; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.arcrobotics.ftclib.gamepad.ToggleButtonReader; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; import org.firstinspires.ftc.teamcode.subsystems.Intake; +import org.firstinspires.ftc.teamcode.subsystems.Shooter; import org.firstinspires.ftc.teamcode.subsystems.Spindexer; import org.firstinspires.ftc.teamcode.subsystems.Transfer; import org.firstinspires.ftc.teamcode.utils.Robot; @@ -44,7 +47,7 @@ public class TeleopV1 extends LinearOpMode { public static double power = 0.0; - public static double pos = 0.501; + public static double pos = 0.5; ToggleButtonReader g1RightBumper; @@ -54,15 +57,38 @@ public class TeleopV1 extends LinearOpMode { ToggleButtonReader g2Triangle; + + ToggleButtonReader g2RightBumper; + + ToggleButtonReader g2LeftBumper; + + ToggleButtonReader g2DpadUp; + + ToggleButtonReader g2DpadDown; public double g1RightBumperStamp = 0.0; + public double g2LeftBumperStamp = 0.0; + public static int spindexerPos = 0; + Shooter shooter; + + public boolean scoreAll = false; + + MecanumDrive drive ; + + public boolean autotrack = true; + @Override public void runOpMode() throws InterruptedException { + drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); + + + + robot = new Robot(hardwareMap); TELE = new MultipleTelemetry( @@ -90,6 +116,24 @@ public class TeleopV1 extends LinearOpMode { g2, GamepadKeys.Button.X ); + g2RightBumper = new ToggleButtonReader( + g2, GamepadKeys.Button.RIGHT_BUMPER + ); + + + g2LeftBumper = new ToggleButtonReader( + g2, GamepadKeys.Button.LEFT_BUMPER + ); + + g2DpadUp = new ToggleButtonReader( + g2, GamepadKeys.Button.DPAD_UP + ); + + + g2DpadDown = new ToggleButtonReader( + g2, GamepadKeys.Button.DPAD_DOWN + ); + drivetrain = new Drivetrain(robot, TELE, g1); @@ -109,26 +153,161 @@ public class TeleopV1 extends LinearOpMode { spindexer.setTelemetryOn(true); + shooter = new Shooter(robot, TELE); + + shooter.setShooterMode("MANUAL"); + + + + waitForStart(); if (isStopRequested()) return; + drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); + + while(opModeIsActive()){ + drive.updatePoseEstimate(); + + TELE.addData("pose", drive.localizer.getPose()); + + TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); + + + robot.hood.setPosition(pos); + + intake(); drivetrain.update(); TELE.update(); - transfer.setTransferPower(power); - - transfer.setTransferPosition(pos); - - transfer.update(); + g2RightBumper.readValue(); + + g2LeftBumper.readValue(); + + g2DpadDown.readValue(); + + g2DpadUp.readValue(); + + if(g2DpadUp.wasJustPressed()){ + pos -=0.02; + } + + if(g2DpadDown.wasJustPressed()){ + pos +=0.02; + } + + if (gamepad2.dpad_left){ + pos = shooter.getAngleByDist( + shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0)) + ); + + } + + TELE.addData("hood", pos); + + + + + + + if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) { + + + + + shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0)); + + } else { + + autotrack = false; + + shooter.moveTurret(shooter.getTurretPosition() - gamepad2.right_stick_x* 0.04); + + } + + if (gamepad2.right_stick_button){ + autotrack = true; + } + + + + if (g2RightBumper.wasJustPressed()){ + transfer.setTransferPower(1); + transfer.transferIn(); + shooter.setManualPower(1); + + } + + if (g2RightBumper.wasJustReleased()){ + transfer.setTransferPower(1); + transfer.transferOut(); + } + + if (gamepad2.left_stick_y>0.5){ + + shooter.setManualPower(0); + } else if (gamepad2.left_stick_y<-0.5){ + shooter.setManualPower(1); + } + + if (g2LeftBumper.wasJustPressed()){ + g2LeftBumperStamp = getRuntime(); + scoreAll = true; + } + + if (scoreAll) { + double time = getRuntime() - g2LeftBumperStamp; + + shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-2, 0, 0)); + + shooter.setManualPower(1); + + + + if (time < 0.3) { + transfer.transferOut(); + transfer.setTransferPower(1); + } else if (time < 1.5){ + spindexer.outtake3(); + } else if (time < 2.0){ + transfer.transferIn(); + } else if (time < 3){ + transfer.transferOut(); + spindexer.outtake2(); + } else if (time < 3.5){ + transfer.transferIn(); + } else if (time < 4.5){ + transfer.transferOut(); + spindexer.outtake1(); + } else if (time < 5){ + transfer.transferIn(); + } else { + + scoreAll = false; + transfer.transferOut(); + + shooter.setManualPower(0); + } + + + + } + + + shooter.update(); + + + + + @@ -156,6 +335,8 @@ public class TeleopV1 extends LinearOpMode { if (g1RightBumper.wasJustPressed()){ + shooter.setManualPower(0); + if (getRuntime() - g1RightBumperStamp < 0.3){ intake.reverse(); @@ -164,6 +345,8 @@ public class TeleopV1 extends LinearOpMode { } spindexer.intake(); + + transfer.transferOut(); g1RightBumperStamp = getRuntime(); @@ -177,6 +360,9 @@ public class TeleopV1 extends LinearOpMode { if (g2Circle.wasJustPressed()){ spindexer.outtake3(); + + + } if (g2Triangle.wasJustPressed()){ @@ -187,8 +373,27 @@ public class TeleopV1 extends LinearOpMode { spindexer.outtake1(); } + + if (g2Circle.wasJustReleased()){ + transfer.setTransferPower(1); + + } + + if (g2Triangle.wasJustReleased()){ + transfer.setTransferPower(1); + } + + if (g2Square.wasJustReleased()){ + transfer.setTransferPower(1); + } + + + + + } + intake.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 5ca970e..3411afa 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 @@ -126,6 +126,8 @@ public class Robot { transferServo = hardwareMap.get(Servo.class, "transferServo"); + transfer.setDirection(DcMotorSimple.Direction.REVERSE); + } }