spindex class - 11/1

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;
+
 }

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;

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){

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){
+    public void spindexGreen(){
-        if (green1){
+        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){
+    public void spindexPurple(){
-        if (purple1){
+        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();
     }
 }

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