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"); }