From b026a597b4b1185fa35e85b45988fd10e908c03e Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 4 Nov 2025 20:28:17 -0600 Subject: [PATCH] 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 | 16 +- 4 files changed, 15 insertions(+), 204 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 f806789..331c1e1 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 0b6f5f9..bd373f5 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 @@ -25,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; } public void transferOut(){