Merge branch 'master' into daniel

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java

@@ -0,0 +1,21 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class ServoPositions {
+
+    public static double spindexer_intakePos = 0.665;
+
+    public static double spindexer_outtakeBall3 = 0.845;
+
+    public static double spindexer_outtakeBall2 = 0.48;
+    public static double spindexer_outtakeBall1 = 0.1;
+
+    public static double transferServo_out = 0.13;
+
+    public static double transferServo_in = 0.4;
+
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java

@@ -34,6 +34,7 @@ import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.VoltageSensor;
@@ -57,33 +58,33 @@ public final class MecanumDrive {
         // TODO: fill in these values based on
         //   see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
         public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
-                RevHubOrientationOnRobot.LogoFacingDirection.UP;
+                RevHubOrientationOnRobot.LogoFacingDirection.RIGHT;
         public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
-                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+                RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD;
 
         // drive model parameters
-        public double inPerTick = 1;
+        public double inPerTick = 0.001978956;
-        public double lateralInPerTick = inPerTick;
+        public double lateralInPerTick = 0.0013863732202094405;
-        public double trackWidthTicks = 0;
+        public double trackWidthTicks = 6488.883015684446;
 
         // feedforward parameters (in tick units)
-        public double kS = 0;
+        public double kS = 1.2147826978829488;
-        public double kV = 0;
+        public double kV = 0.00032;
-        public double kA = 0;
+        public double kA = 0.000046;
 
         // path profile parameters (in inches)
-        public double maxWheelVel = 50;
+        public double maxWheelVel = 80;
         public double minProfileAccel = -30;
-        public double maxProfileAccel = 50;
+        public double maxProfileAccel = 80;
 
         // turn profile parameters (in radians)
-        public double maxAngVel = Math.PI; // shared with path
+        public double maxAngVel = 2* Math.PI; // shared with path
-        public double maxAngAccel = Math.PI;
+        public double maxAngAccel = 2* Math.PI;
 
         // path controller gains
-        public double axialGain = 0.0;
+        public double axialGain = 4;
-        public double lateralGain = 0.0;
+        public double lateralGain = 4;
-        public double headingGain = 0.0; // shared with turn
+        public double headingGain = 4; // shared with turn
 
         public double axialVelGain = 0.0;
         public double lateralVelGain = 0.0;
@@ -224,10 +225,10 @@ public final class MecanumDrive {
 
         // TODO: make sure your config has motors with these names (or change them)
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
+        leftFront = hardwareMap.get(DcMotorEx.class, "fl");
-        leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
+        leftBack = hardwareMap.get(DcMotorEx.class, "bl");
-        rightBack = hardwareMap.get(DcMotorEx.class, "rightBack");
+        rightBack = hardwareMap.get(DcMotorEx.class, "br");
-        rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
+        rightFront = hardwareMap.get(DcMotorEx.class, "fr");
 
         leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -235,7 +236,10 @@ public final class MecanumDrive {
         rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
         // TODO: reverse motor directions if needed
-        //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+        //
+        leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
 
         // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
@@ -244,7 +248,7 @@ public final class MecanumDrive {
 
         voltageSensor = hardwareMap.voltageSensor.iterator().next();
 
-        localizer = new DriveLocalizer(pose);
+        localizer = new PinpointLocalizer(hardwareMap, PARAMS.inPerTick, pose);
 
         FlightRecorder.write("MECANUM_PARAMS", PARAMS);
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java

@@ -16,8 +16,8 @@ import java.util.Objects;
 @Config
 public final class PinpointLocalizer implements Localizer {
     public static class Params {
-        public double parYTicks = 0.0; // 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 = 0.0; // x position of the perpendicular encoder (in tick units)
+        public double perpXTicks = -1962.6377639490684; // x position of the perpendicular encoder (in tick units)
     }
 
     public static Params PARAMS = new Params();
@@ -38,7 +38,7 @@ public final class PinpointLocalizer implements Localizer {
         driver.setOffsets(mmPerTick * PARAMS.parYTicks, mmPerTick * PARAMS.perpXTicks, DistanceUnit.MM);
 
         // TODO: reverse encoder directions if needed
-        initialParDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD;
+        initialParDirection = GoBildaPinpointDriver.EncoderDirection.REVERSED;
         initialPerpDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD;
 
         driver.setEncoderDirections(initialParDirection, initialPerpDirection);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java

@@ -55,7 +55,7 @@ public class Drivetrain implements Subsystem {
         this.slowSpeed = speed;
     }
 
-    public void FieldCentric(double fwd, double strafe, double turn, double turbo){
+    public void RobotCentric(double fwd, double strafe, double turn, double turbo){
 
         double y = -fwd; // Remember, Y stick value is reversed
         double x = strafe * 1.1; // Counteract imperfect strafing
@@ -83,7 +83,7 @@ public class Drivetrain implements Subsystem {
 
         if (Objects.equals(Mode, "Default")) {
 
-            FieldCentric(
+            RobotCentric(
                     gamepad.getRightY(),
                     gamepad.getRightX(),
                     gamepad.getLeftX(),

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java

@@ -1,56 +1,77 @@
 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.qualcomm.robotcore.hardware.Servo;
+
 
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
 public class Intake implements Subsystem {
 
+    private GamepadEx gamepad;
+
     public MultipleTelemetry TELE;
 
-    private final DcMotorEx intake;
 
-    private final Servo kicker;
+    private DcMotorEx intake;
+
+    private double intakePower = 1.0;
+
+    private int intakeState = 0;
 
 
-    private double intakePower = 0.0;
+    public Intake (Robot robot){
 
-    private double kickerPos = 0.0;
-
-    public Intake (Robot robot, MultipleTelemetry telemetry){
 
         this.intake = robot.intake;
-        this.kicker = robot.rejecter;
 
+
+    }
+
+    public int getIntakeState() {
+        return intakeState;
+    }
+
+    public void toggle(){
+        if (intakeState !=0){
+            intakeState = 0;
+        } else {
+            intakeState = 1;
+        }
     }
 
     public void setIntakePower(double pow){
-        this.intakePower = pow;
+        intakePower = pow;
     }
 
-    public double getIntakePower() {
+    public void intake(){
-        return this.intakePower;
+        intakeState =1;
     }
 
-    public void kickOut (){
+    public void reverse(){
-        this.kickerPos = rejecter_Out;
+        intakeState =-1;
     }
 
-    public void kickIn (){
+
-        this.kickerPos = rejecter_In;
+    public void stop(){
+        intakeState =-1;
     }
 
+
+
+
     @Override
     public void update() {
-        kicker.setPosition(kickerPos);
+
+        if (intakeState == 1){
             intake.setPower(intakePower);
-    }
+        } else if (intakeState == -1){
+            intake.setPower(-intakePower);
+        } else {
+            intake.setPower(0);
         }
 
-
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java

@@ -0,0 +1,204 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.hardware.AnalogInput;
+import com.qualcomm.robotcore.hardware.DigitalChannel;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+import java.util.ArrayList;
+
+public class Spindexer implements Subsystem{
+
+    private Servo s1;
+    private Servo s2;
+
+    private DigitalChannel p0;
+
+    private DigitalChannel p1;
+    private DigitalChannel p2;
+    private DigitalChannel p3;
+    private DigitalChannel p4;
+
+    private DigitalChannel p5;
+
+    private AnalogInput input;
+
+    private AnalogInput input2;
+
+
+    private MultipleTelemetry TELE;
+
+    private double position = 0.501;
+
+    private boolean telemetryOn = false;
+
+    private boolean ball0 = false;
+
+    private boolean ball1 = false;
+
+    private boolean ball2 = false;
+
+    private boolean green0 = false;
+
+    private boolean green1 = false;
+
+    private boolean green2 = false;
+
+
+
+
+    public Spindexer (Robot robot, MultipleTelemetry tele){
+
+        this.s1 = robot.spin1;
+        this.s2 = robot.spin2;
+
+        this.p0 = robot.pin0;
+        this.p1 = robot.pin1;
+        this.p2 = robot.pin2;
+        this.p3 = robot.pin3;
+        this.p4 = robot.pin4;
+        this.p5 = robot.pin5;
+
+        this.input = robot.analogInput;
+
+        this.input2 = robot.analogInput2;
+
+        this.TELE = tele;
+
+    }
+
+    public void setTelemetryOn(boolean state){
+        telemetryOn = state;
+    }
+
+    public void colorSensorTelemetry() {
+
+
+        TELE.addData("ball0", ball0);
+        TELE.addData("ball1", ball1);
+        TELE.addData("ball2", ball2);
+        TELE.addData("green0", green0);
+        TELE.addData("green1", green1);
+        TELE.addData("green2", green2);
+
+
+
+    }
+
+    public void checkForBalls() {
+        if (p0.getState()){
+            ball0 = true;
+            green0 = p1.getState();
+        } else {
+            ball0 = false;
+        }
+
+        if (p2.getState()){
+            ball1 = true;
+            green1 = p3.getState();
+        } else {
+            ball1 = false;
+        }
+
+        if (p4.getState()){
+            ball2 = true;
+            green2 = p5.getState();
+        } else {
+            ball2 = false;
+        }
+    }
+
+    public void setPosition (double pos) {
+        position = pos;
+    }
+
+    public void intake () {
+        position = spindexer_intakePos;
+    }
+
+    public void intakeShake(double runtime) {
+        if ((runtime % 0.33) >0.167) {
+            position = spindexer_intakePos + 0.02;
+        } else {
+            position = spindexer_intakePos - 0.02;
+
+        }
+    }
+
+    public void outtake3 () {
+        position = spindexer_outtakeBall3;
+    }
+
+    public void outtake2 () {
+        position = spindexer_outtakeBall2;
+    }
+
+    public void outtake1 () {
+        position = spindexer_outtakeBall1;
+    }
+
+    public void outtakeGreen() {
+        if (green0) {
+            outtake1();
+        } else if (green1){
+            outtake2();
+        } else if (green2) {
+            outtake3();
+        }
+    }
+
+    public void outtakePurpleFs() {
+        if (!green0 && ball0) {
+            outtake1();
+        } else if (!green1 && ball1){
+            outtake2();
+        } else if (!green2 && ball2) {
+            outtake3();
+        }
+    }
+
+    public void outtakeGreenFs() {
+        if (green0 && ball0) {
+            outtake1();
+        } else if (green1 && ball1){
+            outtake2();
+        } else if (green2 && ball2) {
+            outtake3();
+        }
+    }
+
+    public void outtakePurple() {
+        if (!green0) {
+            outtake1();
+        } else if (!green1){
+            outtake2();
+        } else if (!green2) {
+            outtake3();
+        }
+    }
+
+
+
+
+    @Override
+    public void update() {
+
+        if (position !=0.501) {
+
+            s1.setPosition(position);
+            s2.setPosition(1 - position);
+        }
+
+
+        if (telemetryOn) {
+            colorSensorTelemetry();
+        }
+
+        checkForBalls();
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java

@@ -0,0 +1,46 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+public class Transfer implements Subsystem{
+
+    private final Servo servo;
+
+    private final DcMotorEx transfer;
+
+    private double motorPow = 0.0;
+
+    private double servoPos = 0.501;
+
+    public Transfer (Robot robot){
+
+        this.servo = robot.transferServo;
+
+        this.transfer = robot.transfer;
+
+    }
+
+    public void setTransferPosition(double pos){
+        this.servoPos  = pos;
+    }
+
+    public void setTransferPower (double pow){
+        this.motorPow = pow;
+    }
+
+
+
+    @Override
+    public void update() {
+
+        if (servoPos!=0.501){
+            servo.setPosition(servoPos);
+        }
+
+        transfer.setPower(motorPow);
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java

@@ -0,0 +1,204 @@
+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.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.subsystems.Drivetrain;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
+import org.firstinspires.ftc.teamcode.subsystems.Transfer;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+
+@Config
+@TeleOp
+
+public class TeleopV1 extends LinearOpMode {
+
+
+    Robot robot;
+
+    Drivetrain drivetrain;
+
+    Intake intake;
+
+    Spindexer spindexer;
+
+    Transfer transfer;
+
+    MultipleTelemetry TELE;
+
+    GamepadEx g1;
+
+    GamepadEx g2;
+
+    public static double defaultSpeed = 1;
+
+    public static double slowMoSpeed = 0.4;
+
+    public static double power = 0.0;
+
+    public static double pos = 0.501;
+
+    ToggleButtonReader g1RightBumper;
+
+    ToggleButtonReader g2Circle;
+
+    ToggleButtonReader g2Square;
+
+
+    ToggleButtonReader g2Triangle;
+    public double g1RightBumperStamp = 0.0;
+
+    public static int spindexerPos = 0;
+
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+
+        TELE = new MultipleTelemetry(
+                FtcDashboard.getInstance().getTelemetry(),
+                telemetry
+        );
+
+        g1 = new GamepadEx(gamepad1);
+
+        g1RightBumper  = new ToggleButtonReader(
+            g1, GamepadKeys.Button.RIGHT_BUMPER
+        );
+
+        g2 = new GamepadEx(gamepad2);
+
+        g2Circle  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.B
+        );
+
+        g2Triangle  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.Y
+        );
+
+        g2Square  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.X
+        );
+
+
+
+        drivetrain = new Drivetrain(robot, TELE, g1);
+
+        drivetrain.setMode("Default");
+
+        drivetrain.setDefaultSpeed(defaultSpeed);
+
+        drivetrain.setSlowSpeed(slowMoSpeed);
+
+        intake = new Intake(robot);
+
+        transfer = new Transfer(robot);
+
+
+        spindexer = new Spindexer(robot, TELE);
+
+        spindexer.setTelemetryOn(true);
+
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while(opModeIsActive()){
+
+            intake();
+
+            drivetrain.update();
+
+            TELE.update();
+
+            transfer.setTransferPower(power);
+
+            transfer.setTransferPosition(pos);
+
+
+            transfer.update();
+
+
+
+
+
+
+
+
+        }
+
+
+
+
+    }
+
+    public void intake(){
+
+
+        g1RightBumper.readValue();
+
+        g2Circle.readValue();
+
+        g2Square.readValue();
+
+        g2Triangle.readValue();
+
+        if (g1RightBumper.wasJustPressed()){
+
+
+            if (getRuntime() - g1RightBumperStamp < 0.3){
+                intake.reverse();
+            } else {
+                intake.toggle();
+            }
+
+            spindexer.intake();
+
+
+            g1RightBumperStamp = getRuntime();
+
+        }
+
+        if (intake.getIntakeState()==1) {
+            spindexer.intakeShake(getRuntime());
+        } else {
+
+
+            if (g2Circle.wasJustPressed()){
+                spindexer.outtake3();
+            }
+
+            if (g2Triangle.wasJustPressed()){
+                spindexer.outtake2();
+            }
+
+            if (g2Square.wasJustPressed()){
+                spindexer.outtake1();
+            }
+
+        }
+
+        intake.update();
+
+
+
+
+        spindexer.update();
+
+
+
+
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/MotorDirectionDebugger.java

@@ -0,0 +1,45 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+@TeleOp
+public class MotorDirectionDebugger extends LinearOpMode {
+
+    public static double flPower = 0.0;
+
+    public static double frPower = 0.0;
+
+    public static double blPower = 0.0;
+    public static double brPower = 0.0;
+
+    Robot robot;
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+
+
+        waitForStart();
+
+        if(isStopRequested()) return;
+
+        while(opModeIsActive()){
+
+            robot.frontLeft.setPower(flPower);
+            robot.frontRight.setPower(frPower);
+            robot.backRight.setPower(brPower);
+            robot.backLeft.setPower(blPower);
+
+
+
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java

@@ -0,0 +1,224 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.qualcomm.hardware.rev.RevColorSensorV3;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
+
+
+@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);
+
+    }
+}
+
+/**
+ * Helper class for configuring the Brushland Labs Color Rangefinder.
+ * Online documentation: <a href="https://docs.brushlandlabs.com">...</a>
+ */
+class ColorRangefinder {
+    private final I2cDeviceSynchSimple i2c;
+
+    public ColorRangefinder(RevColorSensorV3 emulator) {
+        this.i2c = emulator.getDeviceClient();
+        this.i2c.enableWriteCoalescing(true);
+    }
+
+    /**
+     * Configure Pin 0 to be in digital mode, and add a threshold.
+     * Multiple thresholds can be added to the same pin by calling this function repeatedly.
+     * For colors, bounds should be from 0-255, and for distance, bounds should be from 0-100 (mm).
+     */
+    public void setPin0Digital(DigitalMode digitalMode, double lowerBound, double higherBound) {
+        setDigital(PinNum.PIN0, digitalMode, lowerBound, higherBound);
+    }
+
+    /**
+     * Configure Pin 1 to be in digital mode, and add a threshold.
+     * Multiple thresholds can be added to the same pin by calling this function repeatedly.
+     * For colors, bounds should be from 0-255, and for distance, bounds should be from 0-100 (mm).
+     */
+    public void setPin1Digital(DigitalMode digitalMode, double lowerBound, double higherBound) {
+        setDigital(PinNum.PIN1, digitalMode, 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.
+     */
+    public void setPin0DigitalMaxDistance(DigitalMode digitalMode, double mmRequirement) {
+        setPin0Digital(digitalMode, mmRequirement, mmRequirement);
+    }
+
+    /**
+     * Sets the maximum distance (in millimeters) within which an object must be located for Pin 1's thresholds to trigger.
+     * This is most useful when we want to know if an object is both close and the correct color.
+     */
+    public void setPin1DigitalMaxDistance(DigitalMode digitalMode, double mmRequirement) {
+        setPin1Digital(digitalMode, mmRequirement, mmRequirement);
+    }
+
+    /**
+     * 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 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.
+     * For the full range of that channel, leave the denominator as 65535 for colors or 100 for distance.
+     * Smaller values will clip off higher ranges of the data in exchange for higher resolution within a lower range.
+     */
+    public void setPin0Analog(AnalogMode analogMode, int denominator) {
+        byte denom0 = (byte) (denominator & 0xFF);
+        byte denom1 = (byte) ((denominator & 0xFF00) >> 8);
+        i2c.write(PinNum.PIN0.modeAddress, new byte[]{analogMode.value, denom0, denom1});
+    }
+
+    /**
+     * Configure Pin 0 as analog output of one of the six data channels.
+     * To read analog, make sure the physical switch on the sensor is flipped away from the
+     * connector side.
+     */
+    public void setPin0Analog(AnalogMode analogMode) {
+        setPin0Analog(analogMode, analogMode == AnalogMode.DISTANCE ? 100 : 0xFFFF);
+    }
+
+    public float[] getCalibration() {
+        java.nio.ByteBuffer bytes =
+                java.nio.ByteBuffer.wrap(i2c.read(CALIB_A_VAL_0, 16)).order(java.nio.ByteOrder.LITTLE_ENDIAN);
+        return new float[]{bytes.getFloat(), bytes.getFloat(), bytes.getFloat(), bytes.getFloat()};
+    }
+
+    /**
+     * Save a brightness value of the LED to the sensor.
+     *
+     * @param value brightness between 0-255
+     */
+    public void setLedBrightness(int value) {
+        i2c.write8(LED_BRIGHTNESS, value);
+    }
+
+    /**
+     * Change the I2C address at which the sensor will be found. The address can be reset to the
+     * default of 0x52 by holding the reset button.
+     *
+     * @param value new I2C address from 1 to 127
+     */
+    public void setI2cAddress(int value) {
+        i2c.write8(I2C_ADDRESS_REG, value << 1);
+    }
+
+    /**
+     * Read distance via I2C
+     * @return distance in millimeters
+     */
+    public double readDistance() {
+        java.nio.ByteBuffer bytes =
+                java.nio.ByteBuffer.wrap(i2c.read(PS_DISTANCE_0, 4)).order(java.nio.ByteOrder.LITTLE_ENDIAN);
+        return bytes.getFloat();
+    }
+
+    private void setDigital(
+            PinNum pinNum,
+            DigitalMode digitalMode,
+            double lowerBound,
+            double higherBound
+    ) {
+        int lo, hi;
+        if (lowerBound == higherBound) {
+            lo = (int) lowerBound;
+            hi = (int) higherBound;
+        } else if (digitalMode.value <= DigitalMode.HSV.value) { // color value 0-255
+            lo = (int) Math.round(lowerBound / 255.0 * 65535);
+            hi = (int) Math.round(higherBound / 255.0 * 65535);
+        } else { // distance in mm
+            float[] calib = getCalibration();
+            if (lowerBound < .5) hi = 2048;
+            else hi = rawFromDistance(calib[0], calib[1], calib[2], calib[3], lowerBound);
+            lo = rawFromDistance(calib[0], calib[1], calib[2], calib[3], higherBound);
+        }
+
+        byte lo0 = (byte) (lo & 0xFF);
+        byte lo1 = (byte) ((lo & 0xFF00) >> 8);
+        byte hi0 = (byte) (hi & 0xFF);
+        byte hi1 = (byte) ((hi & 0xFF00) >> 8);
+        i2c.write(pinNum.modeAddress, new byte[]{digitalMode.value, lo0, lo1, hi0, hi1});
+        try {
+            Thread.sleep(25);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
+        }
+    }
+
+    private double root(double n, double v) {
+        double val = Math.pow(v, 1.0 / Math.abs(n));
+        if (n < 0) val = 1.0 / val;
+        return val;
+    }
+
+    private int rawFromDistance(float a, float b, float c, float x0, double mm) {
+        return (int) (root(b, (mm - c) / a) + x0);
+    }
+
+    private enum PinNum {
+        PIN0(0x28), PIN1(0x2D);
+
+        private final byte modeAddress;
+
+        PinNum(int modeAddress) {
+            this.modeAddress = (byte) modeAddress;
+        }
+    }
+
+    // other writeable registers
+    private static final byte CALIB_A_VAL_0 = 0x32;
+    private static final byte PS_DISTANCE_0 = 0x42;
+    private static final byte LED_BRIGHTNESS = 0x46;
+    private static final byte I2C_ADDRESS_REG = 0x47;
+
+    public static int invertHue(int hue360) {
+        return ((hue360 - 180) % 360);
+    }
+
+    public enum DigitalMode {
+        RED(1), BLUE(2), GREEN(3), ALPHA(4), HSV(5), DISTANCE(6);
+        public final byte value;
+
+        DigitalMode(int value) {
+            this.value = (byte) value;
+        }
+    }
+
+    public enum AnalogMode {
+        RED(13), BLUE(14), GREEN(15), ALPHA(16), HSV(17), DISTANCE(18);
+        public final byte value;
+
+        AnalogMode(int value) {
+            this.value = (byte) value;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import com.qualcomm.robotcore.hardware.AnalogInput;
+import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorImplEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -22,33 +24,45 @@ public class Robot {
 
     public DcMotorEx intake;
 
+    public DcMotorEx transfer;
+
+
+
     public DcMotorEx shooter1;
     public DcMotorEx shooter2;
     public Servo hood;
 
+    public Servo transferServo;
+
     public Servo rejecter;
 
     public Servo turr1;
 
     public Servo turr2;
 
-    public DcMotorEx ballUpMotor;
+    public Servo spin1;
+
+    public Servo spin2;
+
+    public DigitalChannel pin0;
+
+    public DigitalChannel pin1;
+    public DigitalChannel pin2;
+    public DigitalChannel pin3;
+    public DigitalChannel pin4;
+
+    public DigitalChannel pin5;
+
+    public AnalogInput analogInput;
+
+    public AnalogInput analogInput2;
 
-    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;
 
 
 
@@ -62,33 +76,55 @@ public class Robot {
         frontRight = hardwareMap.get(DcMotorEx.class, "fr");
         backLeft = hardwareMap.get(DcMotorEx.class, "bl");
         backRight = hardwareMap.get(DcMotorEx.class, "br");
+        frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+        backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
 
         intake = hardwareMap.get(DcMotorEx.class, "intake");
         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");
+        spin1 = hardwareMap.get(Servo.class, "spin1");
-        ballUpServo = hardwareMap.get(Servo.class, "ballUpServo");
 
-        spindex1 = hardwareMap.get(Servo.class, "spindex1");
+        spin2 = hardwareMap.get(Servo.class, "spin2");
-        spindex2 = hardwareMap.get(Servo.class, "spindex2");
 
-        color1Green = hardwareMap.get(DigitalChannel.class, "color1Green");
+        pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
-        color1Purple = hardwareMap.get(DigitalChannel.class, "color1Purple");
 
-        color2Green = hardwareMap.get(DigitalChannel.class, "color2Green");
+        pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
-        color2Purple = hardwareMap.get(DigitalChannel.class, "color2Purple");
 
-        color3Green = hardwareMap.get(DigitalChannel.class, "color1Green");
+        pin2 = hardwareMap.get(DigitalChannel.class, "pin2");
-        color3Purple = hardwareMap.get(DigitalChannel.class, "color3Purple");
+
+        pin3 = hardwareMap.get(DigitalChannel.class, "pin3");
+
+        pin4 = hardwareMap.get(DigitalChannel.class, "pin4");
+
+        pin5 = hardwareMap.get(DigitalChannel.class, "pin5");
+
+
+
+        analogInput = hardwareMap.get(AnalogInput.class, "analog");
+
+
+        analogInput2 = hardwareMap.get(AnalogInput.class, "analog2");
+
+        transfer = hardwareMap.get(DcMotorEx.class, "transfer");
+
+        transferServo = hardwareMap.get(Servo.class, "transferServo");
 
 
     }