From 24473aeabbcb7f97c2f794deab0a6fdad9253f43 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Tue, 4 Nov 2025 12:59:36 -0600 Subject: [PATCH 1/2] Git changes --- .../teamcode/constants/ServoPositions.java | 17 ++ .../ftc/teamcode/subsystems/Drivetrain.java | 4 +- .../ftc/teamcode/subsystems/Intake.java | 57 ++++- .../ftc/teamcode/subsystems/Spindexer.java | 111 +++++++++ .../ftc/teamcode/teleop/TeleopV1.java | 133 +++++++++++ .../tests/MotorDirectionDebugger.java | 45 ++++ .../utils/ConfigureColorRangefinder.java | 220 ++++++++++++++++++ .../ftc/teamcode/utils/Robot.java | 53 ++++- 8 files changed, 632 insertions(+), 8 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/MotorDirectionDebugger.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java new file mode 100644 index 0000000..04a1628 --- /dev/null +++ b/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; + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java index d975600..e192049 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java +++ b/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(), 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 6ce7191..c55fb2a 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 @@ -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); + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java new file mode 100644 index 0000000..05fc5a1 --- /dev/null +++ b/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(); + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java new file mode 100644 index 0000000..8db8191 --- /dev/null +++ b/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(); + + + + + + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/MotorDirectionDebugger.java new file mode 100644 index 0000000..fb1c65b --- /dev/null +++ b/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); + + + + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java new file mode 100644 index 0000000..b42faf1 --- /dev/null +++ b/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: ... + */ +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; + } + } +} 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..9d8e5bc 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 @@ -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"); From a278fc048980ffb0b2678106758d327cbfde9f08 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Tue, 4 Nov 2025 19:38:50 -0600 Subject: [PATCH 2/2] @DANIEELL --- .../teamcode/constants/ServoPositions.java | 4 + .../ftc/teamcode/libs/RR/MecanumDrive.java | 46 ++++---- .../teamcode/libs/RR/PinpointLocalizer.java | 6 +- .../ftc/teamcode/subsystems/Spindexer.java | 109 ++++++++++++++++-- .../ftc/teamcode/subsystems/Transfer.java | 46 ++++++++ .../ftc/teamcode/teleop/TeleopV1.java | 73 +++++++++++- .../utils/ConfigureColorRangefinder.java | 6 +- .../ftc/teamcode/utils/Robot.java | 16 ++- 8 files changed, 268 insertions(+), 38 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 04a1628..dec5f78 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/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; + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java index 6cc00c9..6341a84 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java +++ b/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 lateralInPerTick = inPerTick; - public double trackWidthTicks = 0; + public double inPerTick = 0.001978956; + public double lateralInPerTick = 0.0013863732202094405; + public double trackWidthTicks = 6488.883015684446; // feedforward parameters (in tick units) - public double kS = 0; - public double kV = 0; - public double kA = 0; + public double kS = 1.2147826978829488; + public double kV = 0.00032; + 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 maxAngAccel = Math.PI; + public double maxAngVel = 2* Math.PI; // shared with path + public double maxAngAccel = 2* Math.PI; // path controller gains - public double axialGain = 0.0; - public double lateralGain = 0.0; - public double headingGain = 0.0; // shared with turn + public double axialGain = 4; + public double lateralGain = 4; + 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"); - leftBack = hardwareMap.get(DcMotorEx.class, "leftBack"); - rightBack = hardwareMap.get(DcMotorEx.class, "rightBack"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront = hardwareMap.get(DcMotorEx.class, "fl"); + leftBack = hardwareMap.get(DcMotorEx.class, "bl"); + rightBack = hardwareMap.get(DcMotorEx.class, "br"); + 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); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java index 5d16196..678c8ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java +++ b/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 perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units) + public double parYTicks = 3765.023079161767; // y position of the parallel 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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java index 05fc5a1..804276f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java +++ b/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(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java new file mode 100644 index 0000000..805073d --- /dev/null +++ b/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); + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java index 8db8191..f3ba06d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java +++ b/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(); + + + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java index b42faf1..22c434e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java +++ b/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); + } } 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 9d8e5bc..5ca970e 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 @@ -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); - frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + 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"); @@ -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"); }