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..dec5f78 --- /dev/null +++ b/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; + + +} 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/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java index 319b6da..c7c143c 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 de236ec..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 @@ -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 void setIntakePower (double pow){ - this.intakePower = pow; + public int getIntakeState() { + return intakeState; } - public double getIntakePower() { - return this.intakePower; + public void toggle(){ + if (intakeState !=0){ + intakeState = 0; + } else { + intakeState = 1; + } } - public void kickOut (){ - this.kickerPos = rejecter_Out; + public void setIntakePower(double pow){ + intakePower = pow; } - public void kickIn (){ - this.kickerPos = rejecter_In; + public void intake(){ + intakeState =1; } + public void reverse(){ + intakeState =-1; + } + + + public void stop(){ + intakeState =-1; + } + + + + @Override public void update() { - kicker.setPosition(kickerPos); - intake.setPower(intakePower); + + 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..804276f --- /dev/null +++ b/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(); + + } +} 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 new file mode 100644 index 0000000..f3ba06d --- /dev/null +++ b/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(); + + + + + + } +} 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..22c434e --- /dev/null +++ b/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: ... + */ +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 49f897e..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 @@ -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"); - ballUpServo = hardwareMap.get(Servo.class, "ballUpServo"); + spin1 = hardwareMap.get(Servo.class, "spin1"); - spindex1 = hardwareMap.get(Servo.class, "spindex1"); - spindex2 = hardwareMap.get(Servo.class, "spindex2"); + spin2 = hardwareMap.get(Servo.class, "spin2"); - color1Green = hardwareMap.get(DigitalChannel.class, "color1Green"); - color1Purple = hardwareMap.get(DigitalChannel.class, "color1Purple"); + pin0 = hardwareMap.get(DigitalChannel.class, "pin0"); - color2Green = hardwareMap.get(DigitalChannel.class, "color2Green"); - color2Purple = hardwareMap.get(DigitalChannel.class, "color2Purple"); + pin1 = hardwareMap.get(DigitalChannel.class, "pin1"); - color3Green = hardwareMap.get(DigitalChannel.class, "color1Green"); - color3Purple = hardwareMap.get(DigitalChannel.class, "color3Purple"); + 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"); + + transfer = hardwareMap.get(DcMotorEx.class, "transfer"); + + transferServo = hardwareMap.get(Servo.class, "transferServo"); }