From 4b96775161af15b4c46f227a1f310f495c5a8e0d Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Sun, 30 Nov 2025 16:55:20 -0600 Subject: [PATCH] Telelop drivetrain --- .../ftc/teamcode/teleop/TeleopV2.java | 38 ++- .../teleop/old/subsystems/AprilTag.java | 238 ++++++++++++++++ .../teleop/old/subsystems/Drivetrain.java | 99 +++++++ .../teleop/old/subsystems/Intake.java | 80 ++++++ .../teleop/old/subsystems/Shooter.java | 249 +++++++++++++++++ .../teleop/old/subsystems/Spindexer.java | 256 ++++++++++++++++++ .../teleop/old/subsystems/Subsystem.java | 6 + .../teleop/old/subsystems/Transfer.java | 59 ++++ 8 files changed, 1023 insertions(+), 2 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/AprilTag.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Drivetrain.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Intake.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Shooter.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Spindexer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Subsystem.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Transfer.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java index 17fd9b8..7e528db 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java @@ -1,14 +1,48 @@ package org.firstinspires.ftc.teamcode.teleop; +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.utils.Robot; + public class TeleopV2 extends LinearOpMode { - Rob - + Robot robot; + MultipleTelemetry TELE; @Override public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); + TELE = new MultipleTelemetry( + telemetry, FtcDashboard.getInstance().getTelemetry() + ); + + waitForStart(); + if (isStopRequested()) return; + while (opModeIsActive()) { + + //DRIVETRAIN: + + double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed + double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing + double rx = gamepad1.left_stick_x; + + + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + robot.frontLeft.setPower(frontLeftPower); + robot.backLeft.setPower(backLeftPower); + robot.frontRight.setPower(frontRightPower); + robot.backRight.setPower(backRightPower); + + TELE.update(); + + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/AprilTag.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/AprilTag.java new file mode 100644 index 0000000..d4e8275 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/AprilTag.java @@ -0,0 +1,238 @@ +package org.firstinspires.ftc.teamcode.teleop.old.subsystems; + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; + +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.subsystems.Subsystem; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +public class AprilTag implements Subsystem { + + private AprilTagProcessor aprilTag; + private VisionPortal visionPortal; + + private MultipleTelemetry TELE; + + private boolean teleOn = false; + + private int detections = 0; + + List currentDetections; + + ArrayList> Data = new ArrayList<>(); + + + + public AprilTag(Robot robot, MultipleTelemetry tele) { + + + + this.aprilTag = robot.aprilTagProcessor; + + + this.TELE = tele; + + + + + } + + @Override + public void update() { + + currentDetections = aprilTag.getDetections(); + + UpdateData(); + + if(teleOn){ + tagTELE(); + initTelemetry(); + } + + + + + } + + public void initTelemetry (){ + + TELE.addData("Camera Preview", "Check Driver Station for stream"); + TELE.addData("Status", "Initialized - Press START"); + + + + } + + public void tagTELE () { + + TELE.addData("# AprilTags Detected", detections); + + // Display info for each detected tag + for (ArrayList detection : Data) { + if (detection.get(0) ==1) { + // Known AprilTag with metadata + TELE.addLine(String.format("\n==== (ID %d) %s ====", + detection.get(1).intValue(), "")); + + TELE.addLine(String.format("XYZ: %6.1f %6.1f %6.1f (inch)", + detection.get(2), + detection.get(3), + detection.get(4))); + + TELE.addData("Distance", getDistance(detection.get(1).intValue())); + + TELE.addLine(String.format("PRY: %6.1f %6.1f %6.1f (deg)", + detection.get(5), + detection.get(6), + detection.get(7))); + + TELE.addLine(String.format("RBE: %6.1f %6.1f %6.1f (inch, deg, deg)", + detection.get(8), + detection.get(9), + detection.get(10))); + } + } + } + + + public void turnTelemetryOn(boolean bool) { + + teleOn = bool; + + } + + + + public void UpdateData () { + + Data.clear(); // <--- THIS FIXES YOUR ISSUE + + + detections = currentDetections.size(); + + + for (AprilTagDetection detection : currentDetections) { + + ArrayList detectionData = new ArrayList(); + + + + if (detection.metadata != null) { + + detectionData.add(1.0); + // Known AprilTag with metadata + + detectionData.add( (double) detection.id); + + + + detectionData.add(detection.ftcPose.x); + detectionData.add(detection.ftcPose.y); + detectionData.add(detection.ftcPose.z); + + + + detectionData.add(detection.ftcPose.pitch); + detectionData.add(detection.ftcPose.roll); + detectionData.add(detection.ftcPose.yaw); + + detectionData.add(detection.ftcPose.range); + detectionData.add(detection.ftcPose.bearing); + detectionData.add(detection.ftcPose.elevation); + + } else { + + detectionData.add(0, 0.0); + + } + + Data.add(detectionData); + } + + } + + public int getDetectionCount() { + + return detections; + + } + + public boolean isDetected (int id){ + return (!filterID(Data, (double) id ).isEmpty()); + } + + + + public double getDistance(int id) { + ArrayList d = filterID(Data, (double) id); + if (d.size() >= 5) { + double x = d.get(2); + double y = d.get(3); + double z = d.get(4); + return Math.sqrt(x*x + y*y + z*z); + } + return -1; // tag not found + } + + // Returns the position as [x, y, z] + public List getPosition(int id) { + ArrayList d = filterID(Data, (double) id); + if (d.size() >= 5) { + List pos = new ArrayList<>(); + pos.add(d.get(2)); + pos.add(d.get(3)); + pos.add(d.get(4)); + return pos; + } + return Collections.emptyList(); + } + + // Returns orientation as [pitch, roll, yaw] + public List getOrientation(int id) { + ArrayList d = filterID(Data, (double) id); + if (d.size() >= 8) { + List ori = new ArrayList<>(); + ori.add(d.get(5)); + ori.add(d.get(6)); + ori.add(d.get(7)); + return ori; + } + return Collections.emptyList(); + } + + // Returns range, bearing, elevation as [range, bearing, elevation] + public List getRBE(int id) { + ArrayList d = filterID(Data, (double) id); + if (d.size() >= 11) { + List rbe = new ArrayList<>(); + rbe.add(d.get(8)); + rbe.add(d.get(9)); + rbe.add(d.get(10)); + return rbe; + } + return Collections.emptyList(); + } + + // Returns full raw data for debugging or custom processing + public ArrayList getRawData(int id) { + return filterID(Data, (double) id); + } + + public static ArrayList filterID(ArrayList> data, double x) { + for (ArrayList innerList : data) { + // Ensure it has a second element + if (innerList.size() > 1 && Math.abs(innerList.get(1) - x) < 1e-9) { + return innerList; // Return the first match + } + } + // Return an empty ArrayList if no match found + return new ArrayList<>(); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Drivetrain.java new file mode 100644 index 0000000..b605afb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Drivetrain.java @@ -0,0 +1,99 @@ +package org.firstinspires.ftc.teamcode.teleop.old.subsystems; + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.subsystems.Subsystem; + +import java.util.Objects; + +public class Drivetrain implements Subsystem { + + private final GamepadEx gamepad; + + public MultipleTelemetry TELE; + + private String Mode = "Default"; + + private final DcMotorEx fl; + + private final DcMotorEx fr; + private final DcMotorEx bl; + private final DcMotorEx br; + + private double defaultSpeed = 0.7; + + private double slowSpeed = 0.3; + public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1){ + + this.fl = robot.frontLeft; + this.fr = robot.frontRight; + this.br = robot.backRight; + this.bl = robot.backLeft; + + this.gamepad = gamepad1; + + this.TELE = tele; + + + + } + + public void setMode (String mode){ + this.Mode = mode; + } + + public void setDefaultSpeed (double speed){ + this.defaultSpeed = speed; + } + + public void setSlowSpeed (double speed){ + this.slowSpeed = speed; + } + + 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 + double rx = turn; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if at least one is out of the range [-1, 1] + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + fl.setPower(frontLeftPower*turbo); + bl.setPower(backLeftPower*turbo); + fr.setPower(frontRightPower*turbo); + br.setPower(backRightPower*turbo); + + } + + + @Override + public void update() { + + if (Objects.equals(Mode, "Default")) { + + RobotCentric( + gamepad.getRightY(), + gamepad.getRightX(), + gamepad.getLeftX(), + (gamepad.getTrigger( + GamepadKeys.Trigger.RIGHT_TRIGGER) * (1-defaultSpeed) + - gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed + + defaultSpeed + ) + ); + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Intake.java new file mode 100644 index 0000000..2e95308 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Intake.java @@ -0,0 +1,80 @@ +package org.firstinspires.ftc.teamcode.teleop.old.subsystems; + +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 org.firstinspires.ftc.teamcode.subsystems.Subsystem; +import org.firstinspires.ftc.teamcode.utils.Robot; + +public class Intake implements Subsystem { + + private GamepadEx gamepad; + + public MultipleTelemetry TELE; + + + private DcMotorEx intake; + + private double intakePower = 1.0; + + 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 intakeMinPower(){ + intakeState = 2; + } + + public void intake(){ + intakeState =1; + } + + public void reverse(){ + intakeState =-1; + } + + + public void stop(){ + intakeState =0; + } + + + + + @Override + public void update() { + + if (intakeState == 1){ + intake.setPower(intakePower); + } else if (intakeState == -1){ + intake.setPower(-intakePower); + } else if (intakeState == 2){ + intake.setPower(intakePower); + }else { + intake.setPower(0); + } + + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Shooter.java new file mode 100644 index 0000000..9750d9c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Shooter.java @@ -0,0 +1,249 @@ +package org.firstinspires.ftc.teamcode.teleop.old.subsystems; + +import static org.firstinspires.ftc.teamcode.tests.ShooterTest.*; + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.Pose2d; +import com.arcrobotics.ftclib.controller.PIDController; +import com.arcrobotics.ftclib.controller.PIDFController; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.PIDCoefficients; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.constants.Poses; +import org.firstinspires.ftc.teamcode.subsystems.Subsystem; +import org.firstinspires.ftc.teamcode.utils.Robot; + +import java.util.Objects; + +public class Shooter implements Subsystem { + private final DcMotorEx fly1; + private final DcMotorEx fly2; + + private final DcMotorEx encoder; + private final Servo hoodServo; + + private final Servo turret1; + + private final Servo turret2; + + private final MultipleTelemetry telemetry; + + private boolean telemetryOn = false; + + private double manualPower = 0.0; + private double hoodPos = 0.0; + + private double turretPos = 0.0; + private double velocity = 0.0; + private double posPower = 0.0; + + public double velo = 0.0; + + private int targetPosition = 0; + + public double powPID = 1.0; + + private double p = 0.0003, i = 0, d = 0.00001, f=0; + + private PIDFController controller; + private double pow = 0.0; + + private String shooterMode = "AUTO"; + + private String turretMode = "AUTO"; + + public Shooter(Robot robot, MultipleTelemetry TELE) { + this.fly1 = robot.shooter1; + this.fly2 = robot.shooter2; + this.telemetry = TELE; + this.hoodServo = robot.hood; + + // Reset encoders + fly1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + fly2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + controller = new PIDFController(p, i, d, f); + + controller.setPIDF(p, i, d, f); + + this.turret1 = robot.turr1; + + this.turret2 = robot.turr2; + + this.encoder = robot.shooterEncoder; + + } + + public double gethoodPosition() { + return (hoodServo.getPosition()); + } + + public void sethoodPosition(double pos) { hoodPos = pos; } + + public double getTurretPosition() { + return ((turret1.getPosition() + (1 - turret2.getPosition())) / 2); + } + + public void setTurretPosition(double pos) { turretPos = pos; } + + public double getVelocity(double vel) { + return vel; + } + + public void setVelocity(double vel) { velocity = vel; } + + public void setPosPower(double power) { posPower = power; } + + public void setTargetPosition(int pos) { + targetPosition = pos; + } + + public void setTolerance(int tolerance) { + controller.setTolerance(tolerance); + } + + public void setControllerCoefficients(double kp, double ki, double kd, double kf) { + p = kp; + i = ki; + d = kd; + f = kf; + controller.setPIDF(p, i, d, f); + + } + + public PIDCoefficients getControllerCoefficients() { + + return new PIDCoefficients(p, i, d); + + } + + public void setManualPower(double power) { manualPower = power; } + + public String getShooterMode() { return shooterMode; } + + public String getTurretMode() { return turretMode; } + + public double getECPRPosition() { + return fly1.getCurrentPosition() / (2 * ecpr); + } + + public double getMCPRPosition() { + return (double) fly1.getCurrentPosition() / 4; + } + + public void setShooterMode(String mode) { shooterMode = mode; } + + public void setTurretMode(String mode) { turretMode = mode; } + + public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset) { + + fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + Pose2d deltaPose = new Pose2d( + goalPose.position.x - robotPose.position.x, + goalPose.position.y - robotPose.position.y, + goalPose.heading.toDouble() - (robotPose.heading.toDouble()) + ); + + double distance = Math.sqrt( + deltaPose.position.x * deltaPose.position.x + + deltaPose.position.y * deltaPose.position.y + + Poses.relativeGoalHeight * Poses.relativeGoalHeight + ); + + telemetry.addData("dst", distance); + + double shooterPow = getPowerByDist(distance); + + double hoodAngle = getAngleByDist(distance); + +// hoodServo.setPosition(hoodAngle); + + moveTurret(getTurretPosByDeltaPose(deltaPose, offset)); + + return distance; + + //0.9974 * 355 + + } + + public double getTurretPosByDeltaPose(Pose2d dPose, double offset) { + + double deltaAngle = Math.toDegrees(dPose.heading.toDouble()); + + double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y / dPose.position.x)); + + telemetry.addData("deltaAngle", deltaAngle); + + if (deltaAngle > 90) { + deltaAngle -= 360; + } + +// deltaAngle += aTanAngle; + + deltaAngle /= (335); + + telemetry.addData("dAngle", deltaAngle); + + telemetry.addData("AtanAngle", aTanAngle); + + return ((0.30 - deltaAngle) + offset); + + } + + //62, 0.44 + + //56.5, 0.5 + + public double getPowerByDist(double dist) { + + //TODO: ADD LOGIC + return dist; + } + + public double getAngleByDist(double dist) { + + double newDist = dist - 56.5; + + double pos = newDist * ((0.44 - 0.5) / (62 - 56.5)) + 0.46; + + return pos; + } + + public void setTelemetryOn(boolean state) { telemetryOn = state; } + + public void moveTurret(double pos) { + turret1.setPosition(pos); + turret2.setPosition(1 - pos); + } + + public double getpowPID() { + return powPID; + } + + @Override + public void update() { + + if (Objects.equals(shooterMode, "MANUAL")) { + fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + fly1.setPower(manualPower); + fly2.setPower(manualPower); + } else if (Objects.equals(shooterMode, "VEL")) { + powPID = velocity; + + fly1.setPower(powPID); + fly2.setPower(powPID); + + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Spindexer.java new file mode 100644 index 0000000..673dfb5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Spindexer.java @@ -0,0 +1,256 @@ +package org.firstinspires.ftc.teamcode.teleop.old.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.subsystems.Subsystem; +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; + + 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_intakePos1; + } + + public void intakeShake(double runtime) { + if ((runtime % 0.25) >0.125) { + position = spindexer_intakePos1 + 0.04; + } else { + position = spindexer_intakePos1 - 0.04; + + } + } + + public void outtake3Shake(double runtime) { + if ((runtime % 0.25) >0.125) { + position = spindexer_outtakeBall3 + 0.04; + } else { + position = spindexer_outtakeBall3 - 0.04; + + } + } + + + public void outtake3 () { + position = spindexer_outtakeBall3; + } + + public void outtake2 () { + position = spindexer_outtakeBall2; + } + + public void outtake1 () { + position = spindexer_outtakeBall1; + } + + + public int outtakeGreen(int secLast, int Last) { + if (green2 && (secLast!=3) && (Last!=3)) { + outtake3(); + return 3; + } else if (green1 && (secLast!=2) && (Last!=2)){ + outtake2(); + return 2; + } else if (green0 && (secLast!=1) && (Last!=1)) { + outtake1(); + return 1; + } else { + + if (secLast!=1 && Last!= 1){ + outtake1(); + return 1; + } else if (secLast!=2 && Last!=2){ + outtake2(); + return 2; + } else { + outtake3(); + return 3; + } + + } + } + + + + public void outtakeGreenFs() { + if (green0 && ball0) { + outtake1(); + } else if (green1 && ball1){ + outtake2(); + } else if (green2 && ball2) { + outtake3(); + } + } + + public int greens() { + int num = 0; + + if (green0){num++;} + + if (green1){num++;} + + + if (green2){num++;} + + return num; + + + } + + + public int outtakePurple(int secLast, int Last) { + if (!green2 && (secLast!=3) && (Last!=3)) { + outtake3(); + return 3; + } else if (!green1 && (secLast!=2) && (Last!=2)){ + outtake2(); + return 2; + } else if (!green0 && (secLast!=1) && (Last!=1)) { + outtake1(); + return 1; + } else { + + if (secLast!=1 && Last!= 1){ + outtake1(); + return 1; + } else if (secLast!=2 && Last!=2){ + outtake2(); + return 2; + } else { + outtake3(); + return 3; + } + + + } + } + + + + + @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/old/subsystems/Subsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Subsystem.java new file mode 100644 index 0000000..575275e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Subsystem.java @@ -0,0 +1,6 @@ +package org.firstinspires.ftc.teamcode.teleop.old.subsystems; + +public interface Subsystem { + + public void update(); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Transfer.java new file mode 100644 index 0000000..72c79b5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Transfer.java @@ -0,0 +1,59 @@ +package org.firstinspires.ftc.teamcode.teleop.old.subsystems; + +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; + +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.subsystems.Subsystem; +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; + } + + public void transferOut(){ + this.setTransferPosition(transferServo_out); + } + + public void transferIn(){ + this.setTransferPosition(transferServo_in); + } + + + + + + @Override + public void update() { + + if (servoPos!=0.501){ + servo.setPosition(servoPos); + } + + transfer.setPower(motorPow); + + } +}