@DANIEELL

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

@@ -13,5 +13,9 @@ public class ServoPositions {
     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/Spindexer.java

@@ -9,6 +9,8 @@ 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;
@@ -34,6 +36,21 @@ public class Spindexer implements Subsystem{
 
     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;
@@ -59,16 +76,40 @@ public class Spindexer implements Subsystem{
     }
 
     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()));
 
+        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) {
@@ -79,6 +120,15 @@ public class Spindexer implements Subsystem{
         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;
     }
@@ -91,6 +141,47 @@ public class Spindexer implements Subsystem{
         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
@@ -107,5 +198,7 @@ public class Spindexer implements Subsystem{
             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

@@ -12,6 +12,7 @@ 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;
 
 
@@ -29,16 +30,30 @@ public class TeleopV1 extends LinearOpMode {
 
     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;
@@ -61,6 +76,21 @@ public class TeleopV1 extends LinearOpMode {
             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);
 
@@ -72,6 +102,8 @@ public class TeleopV1 extends LinearOpMode {
 
         intake = new Intake(robot);
 
+        transfer = new Transfer(robot);
+
 
         spindexer = new Spindexer(robot, TELE);
 
@@ -90,7 +122,16 @@ public class TeleopV1 extends LinearOpMode {
 
             TELE.update();
 
-            spindexer.update();
+            transfer.setTransferPower(power);
+
+            transfer.setTransferPosition(pos);
+
+
+            transfer.update();
+
+
+
+
 
 
 
@@ -107,6 +148,12 @@ public class TeleopV1 extends LinearOpMode {
 
         g1RightBumper.readValue();
 
+        g2Circle.readValue();
+
+        g2Square.readValue();
+
+        g2Triangle.readValue();
+
         if (g1RightBumper.wasJustPressed()){
 
 
@@ -118,15 +165,39 @@ public class TeleopV1 extends LinearOpMode {
 
             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/utils/ConfigureColorRangefinder.java

@@ -15,7 +15,11 @@ public class ConfigureColorRangefinder extends LinearOpMode {
         /* 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);
+        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);
+
     }
 }
 

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

@@ -24,10 +24,16 @@ 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;
@@ -73,10 +79,10 @@ public class Robot {
         frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
         backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
 
-        frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-        frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-        backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-        backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
 
         intake = hardwareMap.get(DcMotorEx.class, "intake");
         rejecter = hardwareMap.get(Servo.class, "rejecter");
@@ -116,7 +122,9 @@ public class Robot {
 
         analogInput2 = hardwareMap.get(AnalogInput.class, "analog2");
 
+        transfer = hardwareMap.get(DcMotorEx.class, "transfer");
 
+        transferServo = hardwareMap.get(Servo.class, "transferServo");
 
 
     }