From 0c81ca6a1ab9292ab6fb2daa2f011faf5a843171 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Sat, 1 Nov 2025 16:56:39 -0500 Subject: [PATCH 01/10] Ready Daniel??? --- .../java/org/firstinspires/ftc/teamcode/subsystems/Intake.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java index 6ce7191..0175714 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java @@ -22,5 +22,6 @@ public class Intake extends Subsystem { + } } From e7c18a671a80e8a35d972d2834a7b1232b12e87a Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 1 Nov 2025 17:34:47 -0500 Subject: [PATCH 02/10] intake coded --- .../ftc/teamcode/constants/ServoVars.java | 16 ++++++++ .../ftc/teamcode/subsystems/Drivetrain.java | 10 ++--- .../ftc/teamcode/subsystems/Intake.java | 41 ++++++++++++++++--- .../ftc/teamcode/subsystems/Shooter.java | 5 ++- 4 files changed, 59 insertions(+), 13 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoVars.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoVars.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoVars.java new file mode 100644 index 0000000..9ffd71b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoVars.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.teamcode.constants; + +import com.acmerobotics.dashboard.config.Config; + +@Config +public class ServoVars { + + + //Standard Naming Convention: servoName_VarInSentenceCase + + public static double turret_Range = 355; //In Degrees + public static double turret_GearRatio = 0.9974; + + public static double rejecter_Out = 1.0; + public static double rejecter_In = 0.0; +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java index d975600..319b6da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java @@ -13,17 +13,17 @@ import java.util.Objects; public class Drivetrain implements Subsystem { - private GamepadEx gamepad; + private final GamepadEx gamepad; public MultipleTelemetry TELE; private String Mode = "Default"; - private DcMotorEx fl; + private final DcMotorEx fl; - private DcMotorEx fr; - private DcMotorEx bl; - private DcMotorEx br; + private final DcMotorEx fr; + private final DcMotorEx bl; + private final DcMotorEx br; private double defaultSpeed = 0.7; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java index 0175714..77ad54a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java @@ -1,27 +1,56 @@ package org.firstinspires.ftc.teamcode.subsystems; +import static org.firstinspires.ftc.teamcode.constants.ServoVars.*; + import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.Gamepad; -import com.rowanmcalpin.nextftc.core.Subsystem; +import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.teamcode.utils.Robot; -public class Intake extends Subsystem { - - private GamepadEx gamepad; +public class Intake implements Subsystem { public MultipleTelemetry TELE; - private DcMotorEx intake; + private final DcMotorEx intake; + private final Servo kicker; - public Intake (Robot robot, MultipleTelemetry telemetry, GamepadEx gamepad){ + private double intakePower = 0.0; + private double kickerPos = 0.0; + public Intake (Robot robot, MultipleTelemetry telemetry){ + this.intake = robot.intake; + this.kicker = robot.rejecter; } + + public void setIntakePower (double pow){ + this.intakePower = pow; + } + + public double getIntakePower() { + return this.intakePower; + } + + public void kickOut (){ + this.kickerPos = rejecter_Out; + } + + public void kickIn (){ + this.kickerPos = rejecter_In; + } + + @Override + public void update() { + kicker.setPosition(kickerPos); + intake.setPower(intakePower); + } } + + 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..bb5977d 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 @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystems; +import static org.firstinspires.ftc.teamcode.constants.ServoVars.*; + import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.controller.PIDController; @@ -210,7 +212,6 @@ public class Shooter implements Subsystem { - //0.9974 * 355 } @@ -224,7 +225,7 @@ public class Shooter implements Subsystem { deltaAngle +=360; } - deltaAngle /= (0.9974*355); + deltaAngle /= (turret_GearRatio*turret_Range); return (0.5+deltaAngle) ; From a1b1cb99f63855b6dc5dde318d4352dad1f5d8e5 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 1 Nov 2025 17:38:30 -0500 Subject: [PATCH 03/10] as commit test --- .../java/org/firstinspires/ftc/teamcode/subsystems/Intake.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java index 77ad54a..2370d9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java @@ -19,6 +19,7 @@ public class Intake implements Subsystem { private final Servo kicker; + private double intakePower = 0.0; private double kickerPos = 0.0; From 77a68937f11bd53780366cbd1c954db52a502315 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 1 Nov 2025 20:16:30 -0500 Subject: [PATCH 04/10] as commit test --- .../ftc/teamcode/subsystems/Spindex.java | 123 ++++++++++++++++++ 1 file changed, 123 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindex.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindex.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindex.java new file mode 100644 index 0000000..b6fd313 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindex.java @@ -0,0 +1,123 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import static org.firstinspires.ftc.teamcode.constants.ServoVars.*; + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DigitalChannel; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.utils.Robot; + +import java.util.Objects; + + +public class Spindex implements Subsystem{ + + public MultipleTelemetry TELE; + + private final DcMotorEx ballUpMotor; + + private final Servo ballUpServo; + + private final Servo spindex1; + + private final Servo spindex2; + + private double ballUpMotorPower = 0.0; + + private double ballUpServoPos = 0.0; + + private double manualSpindexPos = 0.0; + + private double autoSpindexPos = 0.0; + + public String spindexMode = "MANUAL"; + + public Spindex (Robot robot, MultipleTelemetry telemetry){ + this.ballUpMotor = robot.ballUpMotor; + this.ballUpServo = robot.ballUpServo; + this.spindex1 = robot.spindex1; + this.spindex2 = robot.spindex2; + } + + public String getSpindexMode(){return spindexMode} + + public void setballUpMotorPower (double pow){ + this.ballUpMotorPower = pow; + } + + public double getballUpMotorPower (){ + return this.ballUpMotorPower; + } + + public void ballUpOn(){ + this.ballUpServoPos = ballUpServo_On; + } + + public void ballUpOff(){ + this.ballUpServoPos = ballUpServo_Off; + } + + //Manual spindex: + + public void spindexPos1(){ + this.manualSpindexPos = spindex_Pos1; + } + + public void spindexPos2(){ + this.manualSpindexPos = spindex_Pos2; + } + + public void spindexPos3(){ + this.manualSpindexPos = spindex_Pos3; + } + + public void spindexIntakeColorPos(){ + this.manualSpindexPos = spindex_IntakeColor; + } + + public void spindexColorBackupPos(){ + this.manualSpindexPos = spindex_BackupColor; + } + + //Automatic spindex (adjust position based on coloring): + + public void spindexGreen(boolean green1, boolean green2, boolean green3){ + if (green1){ + this.autoSpindexPos = spindex_Pos1; + } else if (green2){ + this.autoSpindexPos = spindex_Pos2; + } else if (green3){ + this.autoSpindexPos = spindex_Pos3; + } else { + this.autoSpindexPos = spindex_IntakeColor; + } + } + + public void spindexPurple(boolean purple1, boolean purple2, boolean purple3){ + if (purple1){ + this.autoSpindexPos = spindex_Pos1; + } else if (purple2){ + this.autoSpindexPos = spindex_Pos2; + } else if (purple3){ + this.autoSpindexPos = spindex_Pos3; + } else { + this.autoSpindexPos = spindex_IntakeColor; + } + } + + @Override + public void update() { + if (Objects.equals(spindexMode, "MANUAL")){ + spindex1.setPosition(manualSpindexPos); + spindex2.setPosition(1-manualSpindexPos); + } else if (Objects.equals(spindexMode, "AUTO")){ + spindex1.setPosition(autoSpindexPos); + spindex2.setPosition(1-autoSpindexPos); + } + + ballUpServo.setPosition(ballUpServoPos); + ballUpMotor.setPower(ballUpMotorPower); + } +} From 96f4f1c6392049a87e16ae3f6c94fd06267718a2 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 1 Nov 2025 20:38:06 -0500 Subject: [PATCH 05/10] spindex class - 11/1 --- .../ftc/teamcode/constants/ServoVars.java | 10 +++ .../ftc/teamcode/subsystems/Intake.java | 1 - .../ftc/teamcode/subsystems/Shooter.java | 7 -- .../ftc/teamcode/subsystems/Spindex.java | 71 ++++++++++++++++--- .../ftc/teamcode/utils/Robot.java | 37 +++++++--- 5 files changed, 98 insertions(+), 28 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoVars.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoVars.java index 9ffd71b..6be1749 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoVars.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoVars.java @@ -13,4 +13,14 @@ public class ServoVars { public static double rejecter_Out = 1.0; public static double rejecter_In = 0.0; + + public static double ballUpServo_On = 1.0; + public static double ballUpServo_Off = 0.0; + + public static double spindex_Pos1 = 0.0; + public static double spindex_Pos2 = 0.5; + public static double spindex_Pos3 = 1.0; + public static double spindex_IntakeColor = 0.25; + public static double spindex_BackupColor = 0.3; + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java index 2370d9a..de236ec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java @@ -14,7 +14,6 @@ public class Intake implements Subsystem { public MultipleTelemetry TELE; - private final DcMotorEx intake; private final Servo kicker; 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 bb5977d..770bba7 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 @@ -203,17 +203,10 @@ public class Shooter implements Subsystem { fly2.setPower(0); } - hoodServo.setPosition(hoodAngle); moveTurret(getTurretPosByDeltaPose(deltaPose)); - - - - - - } public double getTurretPosByDeltaPose (Pose2d dPose){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindex.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindex.java index b6fd313..23043d0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindex.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindex.java @@ -7,6 +7,7 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.teamcode.utils.Robot; import java.util.Objects; @@ -14,7 +15,7 @@ import java.util.Objects; public class Spindex implements Subsystem{ - public MultipleTelemetry TELE; + public MultipleTelemetry telemetry; private final DcMotorEx ballUpMotor; @@ -24,6 +25,13 @@ public class Spindex implements Subsystem{ private final Servo spindex2; + private final DigitalChannel color1Green; + private final DigitalChannel color1Purple; + private final DigitalChannel color2Green; + private final DigitalChannel color2Purple; + private final DigitalChannel color3Green; + private final DigitalChannel color3Purple; + private double ballUpMotorPower = 0.0; private double ballUpServoPos = 0.0; @@ -34,15 +42,54 @@ public class Spindex implements Subsystem{ public String spindexMode = "MANUAL"; - public Spindex (Robot robot, MultipleTelemetry telemetry){ + private boolean telemetryOn = false; + + public Spindex (Robot robot, MultipleTelemetry TELE){ this.ballUpMotor = robot.ballUpMotor; this.ballUpServo = robot.ballUpServo; + this.spindex1 = robot.spindex1; this.spindex2 = robot.spindex2; + + this.color1Green = robot.color1Green; + this.color1Purple = robot.color1Purple; + this.color2Green = robot.color2Green; + this.color2Purple = robot.color2Purple; + this.color3Green = robot.color3Green; + this.color3Purple = robot.color3Purple; + + this.telemetry = TELE; } - public String getSpindexMode(){return spindexMode} + public String getSpindexMode(){return spindexMode;} + public void telemetryUpdate() { + String color1 = ""; + String color2 = ""; + String color3 = ""; + // Telemetry + if(this.color1Green.getState()){ + color1 = "green"; + } else if (this.color1Purple.getState()){ + color1 = "purple"; + } + + if(this.color2Green.getState()){ + color2 = "green"; + } else if (this.color2Purple.getState()){ + color2 = "purple"; + } + + if(this.color3Green.getState()){ + color3 = "green"; + } else if (this.color3Purple.getState()){ + color3 = "purple"; + } + + telemetry.addData("Color 1", color1); + telemetry.addData("Color2", color2); + telemetry.addData("Color3", color3); + } public void setballUpMotorPower (double pow){ this.ballUpMotorPower = pow; } @@ -83,24 +130,24 @@ public class Spindex implements Subsystem{ //Automatic spindex (adjust position based on coloring): - public void spindexGreen(boolean green1, boolean green2, boolean green3){ - if (green1){ + public void spindexGreen(){ + if (this.color1Green.getState()){ this.autoSpindexPos = spindex_Pos1; - } else if (green2){ + } else if (this.color2Green.getState()){ this.autoSpindexPos = spindex_Pos2; - } else if (green3){ + } else if (this.color3Green.getState()){ this.autoSpindexPos = spindex_Pos3; } else { this.autoSpindexPos = spindex_IntakeColor; } } - public void spindexPurple(boolean purple1, boolean purple2, boolean purple3){ - if (purple1){ + public void spindexPurple(){ + if (this.color1Purple.getState()){ this.autoSpindexPos = spindex_Pos1; - } else if (purple2){ + } else if (this.color2Purple.getState()){ this.autoSpindexPos = spindex_Pos2; - } else if (purple3){ + } else if (this.color3Purple.getState()){ this.autoSpindexPos = spindex_Pos3; } else { this.autoSpindexPos = spindex_IntakeColor; @@ -119,5 +166,7 @@ public class Spindex implements Subsystem{ ballUpServo.setPosition(ballUpServoPos); ballUpMotor.setPower(ballUpMotorPower); + + telemetryUpdate(); } } 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 215578e..49f897e 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 @@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.utils; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorImplEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -31,6 +32,23 @@ public class Robot { public Servo turr2; + public DcMotorEx ballUpMotor; + + public Servo ballUpServo; + + public Servo spindex1; + + public Servo spindex2; + + public DigitalChannel color1Green; + public DigitalChannel color1Purple; + + + public DigitalChannel color2Green; + public DigitalChannel color2Purple; + + public DigitalChannel color3Green; + public DigitalChannel color3Purple; @@ -49,27 +67,28 @@ public class Robot { rejecter = hardwareMap.get(Servo.class, "rejecter"); shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1"); - shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); - shooter1.setDirection(DcMotorSimple.Direction.REVERSE); hood = hardwareMap.get(Servo.class, "hood"); turr1 = hardwareMap.get(Servo.class, "t1"); - turr2 = hardwareMap.get(Servo.class, "t2"); + ballUpMotor = hardwareMap.get(DcMotorEx.class, "ballUpMotor"); + ballUpServo = hardwareMap.get(Servo.class, "ballUpServo"); + spindex1 = hardwareMap.get(Servo.class, "spindex1"); + spindex2 = hardwareMap.get(Servo.class, "spindex2"); + color1Green = hardwareMap.get(DigitalChannel.class, "color1Green"); + color1Purple = hardwareMap.get(DigitalChannel.class, "color1Purple"); + color2Green = hardwareMap.get(DigitalChannel.class, "color2Green"); + color2Purple = hardwareMap.get(DigitalChannel.class, "color2Purple"); - - - - - - + color3Green = hardwareMap.get(DigitalChannel.class, "color1Green"); + color3Purple = hardwareMap.get(DigitalChannel.class, "color3Purple"); } From 34f2f4b593fa93b9e595517505b21af0d489815c Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 4 Nov 2025 20:28:17 -0600 Subject: [PATCH 06/10] commit 1 --- .../ftc/teamcode/constants/ServoVars.java | 26 --- .../ftc/teamcode/subsystems/Intake.java | 5 +- .../ftc/teamcode/subsystems/Spindex.java | 172 ------------------ .../ftc/teamcode/subsystems/Transfer.java | 18 +- .../ftc/teamcode/teleop/TeleopV1.java | 12 +- 5 files changed, 24 insertions(+), 209 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoVars.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindex.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoVars.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoVars.java deleted file mode 100644 index 6be1749..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoVars.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.firstinspires.ftc.teamcode.constants; - -import com.acmerobotics.dashboard.config.Config; - -@Config -public class ServoVars { - - - //Standard Naming Convention: servoName_VarInSentenceCase - - public static double turret_Range = 355; //In Degrees - public static double turret_GearRatio = 0.9974; - - public static double rejecter_Out = 1.0; - public static double rejecter_In = 0.0; - - public static double ballUpServo_On = 1.0; - public static double ballUpServo_Off = 0.0; - - public static double spindex_Pos1 = 0.0; - public static double spindex_Pos2 = 0.5; - public static double spindex_Pos3 = 1.0; - public static double spindex_IntakeColor = 0.25; - public static double spindex_BackupColor = 0.3; - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java index c55fb2a..ca154e7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java @@ -42,8 +42,9 @@ public class Intake implements Subsystem { } } - public void setIntakePower(double pow){ - intakePower = pow; + public void intakeMinPower(){ + intakePower = 0.5; + intakeState = 1; } public void intake(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindex.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindex.java deleted file mode 100644 index 23043d0..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindex.java +++ /dev/null @@ -1,172 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import static org.firstinspires.ftc.teamcode.constants.ServoVars.*; - -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DigitalChannel; -import com.qualcomm.robotcore.hardware.Servo; - -import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; -import org.firstinspires.ftc.teamcode.utils.Robot; - -import java.util.Objects; - - -public class Spindex implements Subsystem{ - - public MultipleTelemetry telemetry; - - private final DcMotorEx ballUpMotor; - - private final Servo ballUpServo; - - private final Servo spindex1; - - private final Servo spindex2; - - private final DigitalChannel color1Green; - private final DigitalChannel color1Purple; - private final DigitalChannel color2Green; - private final DigitalChannel color2Purple; - private final DigitalChannel color3Green; - private final DigitalChannel color3Purple; - - private double ballUpMotorPower = 0.0; - - private double ballUpServoPos = 0.0; - - private double manualSpindexPos = 0.0; - - private double autoSpindexPos = 0.0; - - public String spindexMode = "MANUAL"; - - private boolean telemetryOn = false; - - public Spindex (Robot robot, MultipleTelemetry TELE){ - this.ballUpMotor = robot.ballUpMotor; - this.ballUpServo = robot.ballUpServo; - - this.spindex1 = robot.spindex1; - this.spindex2 = robot.spindex2; - - this.color1Green = robot.color1Green; - this.color1Purple = robot.color1Purple; - this.color2Green = robot.color2Green; - this.color2Purple = robot.color2Purple; - this.color3Green = robot.color3Green; - this.color3Purple = robot.color3Purple; - - this.telemetry = TELE; - } - - public String getSpindexMode(){return spindexMode;} - - public void telemetryUpdate() { - String color1 = ""; - String color2 = ""; - String color3 = ""; - // Telemetry - if(this.color1Green.getState()){ - color1 = "green"; - } else if (this.color1Purple.getState()){ - color1 = "purple"; - } - - if(this.color2Green.getState()){ - color2 = "green"; - } else if (this.color2Purple.getState()){ - color2 = "purple"; - } - - if(this.color3Green.getState()){ - color3 = "green"; - } else if (this.color3Purple.getState()){ - color3 = "purple"; - } - - telemetry.addData("Color 1", color1); - telemetry.addData("Color2", color2); - telemetry.addData("Color3", color3); - } - public void setballUpMotorPower (double pow){ - this.ballUpMotorPower = pow; - } - - public double getballUpMotorPower (){ - return this.ballUpMotorPower; - } - - public void ballUpOn(){ - this.ballUpServoPos = ballUpServo_On; - } - - public void ballUpOff(){ - this.ballUpServoPos = ballUpServo_Off; - } - - //Manual spindex: - - public void spindexPos1(){ - this.manualSpindexPos = spindex_Pos1; - } - - public void spindexPos2(){ - this.manualSpindexPos = spindex_Pos2; - } - - public void spindexPos3(){ - this.manualSpindexPos = spindex_Pos3; - } - - public void spindexIntakeColorPos(){ - this.manualSpindexPos = spindex_IntakeColor; - } - - public void spindexColorBackupPos(){ - this.manualSpindexPos = spindex_BackupColor; - } - - //Automatic spindex (adjust position based on coloring): - - public void spindexGreen(){ - if (this.color1Green.getState()){ - this.autoSpindexPos = spindex_Pos1; - } else if (this.color2Green.getState()){ - this.autoSpindexPos = spindex_Pos2; - } else if (this.color3Green.getState()){ - this.autoSpindexPos = spindex_Pos3; - } else { - this.autoSpindexPos = spindex_IntakeColor; - } - } - - public void spindexPurple(){ - if (this.color1Purple.getState()){ - this.autoSpindexPos = spindex_Pos1; - } else if (this.color2Purple.getState()){ - this.autoSpindexPos = spindex_Pos2; - } else if (this.color3Purple.getState()){ - this.autoSpindexPos = spindex_Pos3; - } else { - this.autoSpindexPos = spindex_IntakeColor; - } - } - - @Override - public void update() { - if (Objects.equals(spindexMode, "MANUAL")){ - spindex1.setPosition(manualSpindexPos); - spindex2.setPosition(1-manualSpindexPos); - } else if (Objects.equals(spindexMode, "AUTO")){ - spindex1.setPosition(autoSpindexPos); - spindex2.setPosition(1-autoSpindexPos); - } - - ballUpServo.setPosition(ballUpServoPos); - ballUpMotor.setPower(ballUpMotorPower); - - telemetryUpdate(); - } -} 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..2cc3bae 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; @@ -23,12 +25,20 @@ public class Transfer implements Subsystem{ } - public void setTransferPosition(double pos){ - this.servoPos = pos; + public void setTransferPositionOn(){ + this.servoPos = transferServo_in; } - public void setTransferPower (double pow){ - this.motorPow = pow; + public void setTransferPositionOff(){ + this.servoPos = transferServo_out; + } + + public void setTransferPowerOn (){ + this.motorPow = -1; + } + + public void setTransferPowerOff (){ + this.motorPow = 0; } 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..493cdb5 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 @@ -58,6 +58,8 @@ public class TeleopV1 extends LinearOpMode { public static int spindexerPos = 0; + public double time = 0.0; + @Override @@ -109,6 +111,8 @@ public class TeleopV1 extends LinearOpMode { spindexer.setTelemetryOn(true); + time = getRuntime(); + waitForStart(); @@ -122,11 +126,6 @@ public class TeleopV1 extends LinearOpMode { TELE.update(); - transfer.setTransferPower(power); - - transfer.setTransferPosition(pos); - - transfer.update(); @@ -176,14 +175,17 @@ public class TeleopV1 extends LinearOpMode { if (g2Circle.wasJustPressed()){ + intake.intakeMinPower(); spindexer.outtake3(); } if (g2Triangle.wasJustPressed()){ + intake.intakeMinPower(); spindexer.outtake2(); } if (g2Square.wasJustPressed()){ + intake.intakeMinPower(); spindexer.outtake1(); } From c517443459adefadd4e81c20c06334357a9d1bb1 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 4 Nov 2025 21:29:46 -0600 Subject: [PATCH 07/10] teleop ground logic --- .../ftc/teamcode/constants/ShooterVars.java | 10 ++++ .../ftc/teamcode/subsystems/Intake.java | 9 +-- .../ftc/teamcode/subsystems/Shooter.java | 6 +- .../ftc/teamcode/teleop/TeleopV1.java | 56 +++++++++++++++++-- 4 files changed, 70 insertions(+), 11 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java 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 new file mode 100644 index 0000000..b2c70b8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.teamcode.constants; + +import com.acmerobotics.dashboard.config.Config; + +@Config +public class ShooterVars { + public static double turret_GearRatio = 0.9974; + + public static double turret_Range = 355; +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java index ca154e7..0126637 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java @@ -43,8 +43,7 @@ public class Intake implements Subsystem { } public void intakeMinPower(){ - intakePower = 0.5; - intakeState = 1; + intakeState = 2; } public void intake(){ @@ -57,7 +56,7 @@ public class Intake implements Subsystem { public void stop(){ - intakeState =-1; + intakeState =0; } @@ -70,7 +69,9 @@ public class Intake implements Subsystem { intake.setPower(intakePower); } else if (intakeState == -1){ intake.setPower(-intakePower); - } else { + } else if (intakeState == 2){ + intake.setPower(intakePower); + }else { intake.setPower(0); } 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 770bba7..352becc 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 @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode.subsystems; -import static org.firstinspires.ftc.teamcode.constants.ServoVars.*; +import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; @@ -50,9 +50,9 @@ public class Shooter implements Subsystem { - private String shooterMode = "AUTO"; + private String shooterMode = "MANUAL"; - private String turretMode = "AUTO"; + private String turretMode = "MANUAL"; public Shooter(Robot robot, MultipleTelemetry TELE) { 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 493cdb5..7c1ba43 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 @@ -11,6 +11,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; 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; @@ -32,6 +33,8 @@ public class TeleopV1 extends LinearOpMode { Transfer transfer; + Shooter shooter; + MultipleTelemetry TELE; GamepadEx g1; @@ -42,16 +45,21 @@ public class TeleopV1 extends LinearOpMode { public static double slowMoSpeed = 0.4; - public static double power = 0.0; + public static double shooterPower = 0.0; - public static double pos = 0.501; + public static double turretPosition = 0.501; + + public static double hoodPosition = 0.501; ToggleButtonReader g1RightBumper; + ToggleButtonReader g1LeftBumper; + ToggleButtonReader g2Circle; ToggleButtonReader g2Square; + ToggleButtonReader g2LeftBumper; ToggleButtonReader g2Triangle; public double g1RightBumperStamp = 0.0; @@ -78,6 +86,10 @@ public class TeleopV1 extends LinearOpMode { g1, GamepadKeys.Button.RIGHT_BUMPER ); + g1LeftBumper = new ToggleButtonReader( + g1, GamepadKeys.Button.LEFT_BUMPER + ); + g2 = new GamepadEx(gamepad2); g2Circle = new ToggleButtonReader( @@ -92,6 +104,11 @@ public class TeleopV1 extends LinearOpMode { g2, GamepadKeys.Button.X ); + g2LeftBumper = new ToggleButtonReader( + g2, GamepadKeys.Button.LEFT_BUMPER + ); + + drivetrain = new Drivetrain(robot, TELE, g1); @@ -109,6 +126,8 @@ public class TeleopV1 extends LinearOpMode { spindexer = new Spindexer(robot, TELE); + shooter = new Shooter(robot, TELE); + spindexer.setTelemetryOn(true); time = getRuntime(); @@ -126,7 +145,16 @@ public class TeleopV1 extends LinearOpMode { TELE.update(); - transfer.update(); + transfer(); + + shooter.setManualPower(shooterPower); + + shooter.sethoodPosition(hoodPosition); + + shooter.setTurretPosition(turretPosition); + + + @@ -171,22 +199,30 @@ public class TeleopV1 extends LinearOpMode { if (intake.getIntakeState()==1) { spindexer.intakeShake(getRuntime()); + transfer.setTransferPowerOff(); + transfer.setTransferPositionOff(); } else { if (g2Circle.wasJustPressed()){ + transfer.setTransferPositionOff(); intake.intakeMinPower(); spindexer.outtake3(); + transfer.setTransferPowerOn(); } if (g2Triangle.wasJustPressed()){ + transfer.setTransferPositionOff(); intake.intakeMinPower(); spindexer.outtake2(); + transfer.setTransferPowerOn(); } if (g2Square.wasJustPressed()){ + transfer.setTransferPositionOff(); intake.intakeMinPower(); spindexer.outtake1(); + transfer.setTransferPowerOn(); } } @@ -194,7 +230,7 @@ public class TeleopV1 extends LinearOpMode { intake.update(); - + transfer.update(); spindexer.update(); @@ -202,5 +238,17 @@ public class TeleopV1 extends LinearOpMode { + } + + public void transfer(){ + + g1LeftBumper.readValue(); + + if (g1LeftBumper.wasJustPressed()){ + transfer.setTransferPositionOn(); + } + + transfer.update(); + } } From bb821d9108c194963dddfd73fa3a91faf8bbe932 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Thu, 6 Nov 2025 19:21:56 -0600 Subject: [PATCH 08/10] color test --- .../ftc/teamcode/tests/ColorSensorTest.java | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java new file mode 100644 index 0000000..33a9572 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java @@ -0,0 +1,37 @@ +package org.firstinspires.ftc.teamcode.tests; + +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; + +import org.firstinspires.ftc.teamcode.utils.Robot; + +@TeleOp +@Config +public class ColorSensorTest extends LinearOpMode{ + Robot robot; + MultipleTelemetry TELE; + + @Override + public void runOpMode() throws InterruptedException{ + robot = new Robot(hardwareMap); + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + waitForStart(); + if (isStopRequested()) return; + + while (opModeIsActive()){ + TELE.addData("Green1:", robot.pin0.getState()); + TELE.addData("Purple1:", robot.pin1.getState()); + TELE.addData("Green2:", robot.pin2.getState()); + TELE.addData("Purple2:", robot.pin3.getState()); + TELE.addData("Green3:", robot.pin4.getState()); + TELE.addData("Purple3:", robot.pin5.getState()); + + TELE.update(); + } + } + +} From 03d72af8d27874f7f2bbb9ebbbfed2b5f65a19db Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Thu, 6 Nov 2025 22:27:02 -0600 Subject: [PATCH 09/10] color test --- .../ftc/teamcode/tests/ColorSensorTest.java | 12 ++-- .../utils/ConfigureColorRangefinder.java | 65 ++++++++++++++----- 2 files changed, 54 insertions(+), 23 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java index 33a9572..dc9cf69 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java @@ -23,12 +23,12 @@ public class ColorSensorTest extends LinearOpMode{ if (isStopRequested()) return; while (opModeIsActive()){ - TELE.addData("Green1:", robot.pin0.getState()); - TELE.addData("Purple1:", robot.pin1.getState()); - TELE.addData("Green2:", robot.pin2.getState()); - TELE.addData("Purple2:", robot.pin3.getState()); - TELE.addData("Green3:", robot.pin4.getState()); - TELE.addData("Purple3:", robot.pin5.getState()); + TELE.addData("Green1:", robot.pin1.getState()); + TELE.addData("Purple1:", robot.pin0.getState()); + TELE.addData("Green2:", robot.pin3.getState()); + TELE.addData("Purple2:", robot.pin2.getState()); + TELE.addData("Green3:", robot.pin5.getState()); + TELE.addData("Purple3:", robot.pin4.getState()); TELE.update(); } 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 22c434e..7d59ecd 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 @@ -1,25 +1,41 @@ package org.firstinspires.ftc.teamcode.utils; +import com.acmerobotics.dashboard.config.Config; +import com.bylazar.configurables.annotations.Configurable; import com.qualcomm.hardware.rev.RevColorSensorV3; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; +@Config +@Autonomous + -@TeleOp public class ConfigureColorRangefinder extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { ColorRangefinder crf = new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "color")); - waitForStart(); - /* Using this example configuration, you can detect both artifact colors based on which pin is reading true: - pin0 --> purple - pin1 --> green */ - crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0 / 360.0 * 255, 360 / 360.0 * 255); // purple - crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement - crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 110 / 360.0 * 255, 140 / 360.0 * 255); // green - crf.setLedBrightness(2); + /* + Using this example configuration, you can detect all three sample colors based on which pin is reading true: + both --> yellow + only pin0 --> blue + only pin1 --> red + neither --> no object + */ + crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 80 / 360.0 * 255, 140 / 360.0 * 255); // green + crf.setPin1Saturation(175, 255); + crf.setPin1Value(100,200); + crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 20mm or closer requirement + + crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // purple + + crf.setLedBrightness(0); + + waitForStart(); + + stop(); } } @@ -28,9 +44,12 @@ public class ConfigureColorRangefinder extends LinearOpMode { * Online documentation: ... */ class ColorRangefinder { + public static int LED_VALUE = 15; + public final RevColorSensorV3 emulator; private final I2cDeviceSynchSimple i2c; public ColorRangefinder(RevColorSensorV3 emulator) { + this.emulator = emulator; this.i2c = emulator.getDeviceClient(); this.i2c.enableWriteCoalescing(true); } @@ -53,6 +72,22 @@ class ColorRangefinder { setDigital(PinNum.PIN1, digitalMode, lowerBound, higherBound); } + public void setPin0Saturation(double lowerBound, double higherBound) { + setDigital(PinNum.PIN0, DigitalMode.SATURATION, lowerBound, higherBound); + } + + public void setPin1Saturation(double lowerBound, double higherBound) { + setDigital(PinNum.PIN1, DigitalMode.SATURATION, lowerBound, higherBound); + } + + // Optional: Easy methods for value/brightness thresholding + public void setPin0Value(double lowerBound, double higherBound) { + setDigital(PinNum.PIN0, DigitalMode.VALUE, lowerBound, higherBound); + } + + public void setPin1Value(double lowerBound, double higherBound) { + setDigital(PinNum.PIN1, DigitalMode.VALUE, lowerBound, higherBound); + } /** * Sets the maximum distance (in millimeters) within which an object must be located for Pin 0's thresholds to trigger. * This is most useful when we want to know if an object is both close and the correct color. @@ -74,18 +109,13 @@ class ColorRangefinder { * This is useful if we want to threshold red; instead of having two thresholds we would invert * the color and look for blue. */ - public void setPin0InvertHue() { - setPin0DigitalMaxDistance(DigitalMode.HSV, 200); - } + /** * Invert the hue value before thresholding it, meaning that the colors become their opposite. * This is useful if we want to threshold red; instead of having two thresholds we would invert * the color and look for blue. */ - public void setPin1InvertHue() { - setPin1DigitalMaxDistance(DigitalMode.HSV, 200); - } /** * The denominator is what the raw sensor readings will be divided by before being scaled to 12-bit analog. @@ -152,7 +182,7 @@ class ColorRangefinder { if (lowerBound == higherBound) { lo = (int) lowerBound; hi = (int) higherBound; - } else if (digitalMode.value <= DigitalMode.HSV.value) { // color value 0-255 + } else if (digitalMode.value <= DigitalMode.VALUE.value) { // HSV/HUE/SATURATION/VALUE color range lo = (int) Math.round(lowerBound / 255.0 * 65535); hi = (int) Math.round(higherBound / 255.0 * 65535); } else { // distance in mm @@ -205,7 +235,7 @@ class ColorRangefinder { } public enum DigitalMode { - RED(1), BLUE(2), GREEN(3), ALPHA(4), HSV(5), DISTANCE(6); + RED(1), BLUE(2), GREEN(3), ALPHA(4), HSV(5), DISTANCE(6), SATURATION(7), VALUE(8); public final byte value; DigitalMode(int value) { @@ -222,3 +252,4 @@ class ColorRangefinder { } } } + From ee2208922bc02685b8f48c90776321ac44627c0d Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 7 Nov 2025 20:22:41 -0600 Subject: [PATCH 10/10] rejecter --- .../ftc/teamcode/subsystems/Rejecter.java | 27 +++++++++++++++++++ .../ftc/teamcode/teleop/TeleopV1.java | 7 ++++- 2 files changed, 33 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Rejecter.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Rejecter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Rejecter.java new file mode 100644 index 0000000..450d18c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Rejecter.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.subsystems; + + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.utils.Robot; + +public class Rejecter implements Subsystem{ + + private final Servo servo; + + public double rpos = 0.5; + + public Rejecter(Robot robot){ + this.servo = robot.rejecter; + } + + public void rejecterPos(double pos){ + this.rpos = pos; + } + + @Override + public void update() { + this.servo.setPosition(rpos); + } +} 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 7c1ba43..97026e9 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 @@ -11,6 +11,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; import org.firstinspires.ftc.teamcode.subsystems.Intake; +import org.firstinspires.ftc.teamcode.subsystems.Rejecter; import org.firstinspires.ftc.teamcode.subsystems.Shooter; import org.firstinspires.ftc.teamcode.subsystems.Spindexer; import org.firstinspires.ftc.teamcode.subsystems.Transfer; @@ -22,6 +23,7 @@ import org.firstinspires.ftc.teamcode.utils.Robot; public class TeleopV1 extends LinearOpMode { + public static double rpos = 0.5; Robot robot; @@ -29,6 +31,8 @@ public class TeleopV1 extends LinearOpMode { Intake intake; + Rejecter rejecter; + Spindexer spindexer; Transfer transfer; @@ -123,7 +127,6 @@ public class TeleopV1 extends LinearOpMode { transfer = new Transfer(robot); - spindexer = new Spindexer(robot, TELE); shooter = new Shooter(robot, TELE); @@ -141,6 +144,8 @@ public class TeleopV1 extends LinearOpMode { intake(); + rejecter.rejecterPos(rpos); + drivetrain.update(); TELE.update();