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"); }