Git changes

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

@@ -0,0 +1,17 @@
+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;
+
+
+}

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

@@ -4,11 +4,11 @@ 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 org.firstinspires.ftc.teamcode.utils.Robot;
 
-public class Intake extends Subsystem {
+public class Intake implements Subsystem {
 
     private GamepadEx gamepad;
 
@@ -17,10 +17,61 @@ public class Intake extends Subsystem {
 
     private DcMotorEx intake;
 
+    private double intakePower = 1.0;
 
-    public Intake (Robot robot, MultipleTelemetry telemetry, GamepadEx gamepad){
+    private int intakeState = 0;
 
 
+    public Intake (Robot robot){
+
+
+        this.intake = robot.intake;
+
+
+    }
+
+    public int getIntakeState() {
+        return intakeState;
+    }
+
+    public void toggle(){
+        if (intakeState !=0){
+            intakeState = 0;
+        } else {
+            intakeState = 1;
+        }
+    }
+
+    public void setIntakePower(double pow){
+        intakePower = pow;
+    }
+
+    public void intake(){
+        intakeState =1;
+    }
+
+    public void reverse(){
+        intakeState =-1;
+    }
+
+
+    public void stop(){
+        intakeState =-1;
+    }
+
+
+
+
+    @Override
+    public void update() {
+
+        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,111 @@
+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;
+
+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;
+
+    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("pin0", p0.getState());
+        TELE.addData("pin1", p1.getState());
+        TELE.addData("pin2", p2.getState());
+        TELE.addData("pin3", p3.getState());
+        TELE.addData("pin4", p4.getState());
+        TELE.addData("pin5", p5.getState());
+        TELE.addData("Analog", (input.getVoltage()));
+
+        TELE.addData("Analog2", (input2.getVoltage()));
+
+    }
+
+    public void setPosition (double pos) {
+        position = pos;
+    }
+
+    public void intake () {
+        position = spindexer_intakePos;
+    }
+
+    public void outtake3 () {
+        position = spindexer_outtakeBall3;
+    }
+
+    public void outtake2 () {
+        position = spindexer_outtakeBall2;
+    }
+
+    public void outtake1 () {
+        position = spindexer_outtakeBall1;
+    }
+
+
+
+    @Override
+    public void update() {
+
+        if (position !=0.501) {
+
+            s1.setPosition(position);
+            s2.setPosition(1 - position);
+        }
+
+
+        if (telemetryOn) {
+            colorSensorTelemetry();
+        }
+
+    }
+}

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

@@ -0,0 +1,133 @@
+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.utils.Robot;
+
+
+@Config
+@TeleOp
+
+public class TeleopV1 extends LinearOpMode {
+
+
+    Robot robot;
+
+    Drivetrain drivetrain;
+
+    Intake intake;
+
+    Spindexer spindexer;
+
+    MultipleTelemetry TELE;
+
+    GamepadEx g1;
+
+    public static double defaultSpeed = 1;
+
+    public static double slowMoSpeed = 0.4;
+
+    ToggleButtonReader g1RightBumper;
+
+    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
+        );
+
+
+        drivetrain = new Drivetrain(robot, TELE, g1);
+
+        drivetrain.setMode("Default");
+
+        drivetrain.setDefaultSpeed(defaultSpeed);
+
+        drivetrain.setSlowSpeed(slowMoSpeed);
+
+        intake = new Intake(robot);
+
+
+        spindexer = new Spindexer(robot, TELE);
+
+        spindexer.setTelemetryOn(true);
+
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while(opModeIsActive()){
+
+            intake();
+
+            drivetrain.update();
+
+            TELE.update();
+
+            spindexer.update();
+
+
+
+
+        }
+
+
+
+
+    }
+
+    public void intake(){
+
+
+        g1RightBumper.readValue();
+
+        if (g1RightBumper.wasJustPressed()){
+
+
+            if (getRuntime() - g1RightBumperStamp < 0.3){
+                intake.reverse();
+            } else {
+                intake.toggle();
+            }
+
+            spindexer.intake();
+
+            g1RightBumperStamp = getRuntime();
+
+        }
+
+        intake.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,220 @@
+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.setPin0Analog(ColorRangefinder.AnalogMode.HSV);
+    }
+}
+
+/**
+ * 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,8 +1,11 @@
 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;
+import com.qualcomm.robotcore.hardware.DigitalChannel;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 
@@ -31,6 +34,29 @@ public class Robot {
 
     public Servo turr2;
 
+    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;
+
+
+
+
+
+
+
 
 
 
@@ -44,6 +70,13 @@ 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.BRAKE);
+        frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
         intake = hardwareMap.get(DcMotorEx.class, "intake");
         rejecter = hardwareMap.get(Servo.class, "rejecter");
@@ -60,14 +93,28 @@ public class Robot {
 
         turr2 = hardwareMap.get(Servo.class, "t2");
 
+        spin1 = hardwareMap.get(Servo.class, "spin1");
+
+        spin2 = hardwareMap.get(Servo.class, "spin2");
+
+        pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
+
+        pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
+
+        pin2 = hardwareMap.get(DigitalChannel.class, "pin2");
+
+        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");