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 05ffa94..ba7cfa5 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,10 +1,14 @@ package org.firstinspires.ftc.teamcode.teleop; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; +import static org.firstinspires.ftc.teamcode.tests.ShooterTest.*; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.utils.Robot; import java.util.ArrayList; @@ -17,11 +21,18 @@ public class TeleopV2 extends LinearOpMode { boolean intake = false; boolean reject = false; + private double lastEncoderRevolutions = 0.0; + private double lastTimeStamp = 0.0; + + private double vel = 3000; List allHubs = hardwareMap.getAll(LynxModule.class); - List s1 = new ArrayList<>(); - List s2 = new ArrayList<>(); - List s3 = new ArrayList<>(); + List s1G = new ArrayList<>(); + List s2G = new ArrayList<>(); + List s3G = new ArrayList<>(); + List s1 = new ArrayList<>(); + List s2 = new ArrayList<>(); + List s3 = new ArrayList<>(); @Override public void runOpMode() throws InterruptedException { @@ -39,7 +50,7 @@ public class TeleopV2 extends LinearOpMode { if (isStopRequested()) return; while (opModeIsActive()) { - //DRIVETRAIN: + //DRIVETRAIN: double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing @@ -56,7 +67,7 @@ public class TeleopV2 extends LinearOpMode { robot.frontRight.setPower(frontRightPower); robot.backRight.setPower(backRightPower); - //INTAKE: + //INTAKE: if (gamepad1.rightBumperWasPressed()) { intake = true; @@ -64,21 +75,123 @@ public class TeleopV2 extends LinearOpMode { if (intake) { robot.intake.setPower(1); + + double position; + + if ((getRuntime() % 0.3) > 0.15) { + position = spindexer_intakePos1 + 0.015; + } else { + position = spindexer_intakePos1 - 0.015; + } + + robot.spin1.setPosition(position); + robot.spin2.setPosition(1 - position); + } else if (reject) { robot.intake.setPower(-1); + double position = spindexer_intakePos1; + robot.spin1.setPosition(position); + robot.spin2.setPosition(1 - position); } else { robot.intake.setPower(0); } + //COLOR: - //COLOR: + double s1D = robot.color1.getDistance(DistanceUnit.MM); + double s2D = robot.color2.getDistance(DistanceUnit.MM); + double s3D = robot.color3.getDistance(DistanceUnit.MM); - double s1D = robot. + if (s1D < 40) { + + double green = robot.color1.getNormalizedColors().green; + double red = robot.color1.getNormalizedColors().red; + double blue = robot.color1.getNormalizedColors().blue; + + double gP = green / (green + red + blue); + + s1G.add(gP); + + if (gP >= 0.43) { + s1.add(true); + } + } + + if (s2D < 40) { + + double green = robot.color2.getNormalizedColors().green; + double red = robot.color2.getNormalizedColors().red; + double blue = robot.color2.getNormalizedColors().blue; + + double gP = green / (green + red + blue); + + s2G.add(gP); + + if (gP >= 0.43) { + s2.add(true); + } + } + + if (s3D < 30) { + + double green = robot.color3.getNormalizedColors().green; + double red = robot.color3.getNormalizedColors().red; + double blue = robot.color3.getNormalizedColors().blue; + + double gP = green / (green + red + blue); + + s3G.add(gP); + + if (gP >= 0.43) { + s3.add(true); + } + } + + boolean green1 = s1.get(s1.size() - 1); + boolean green2 = s2.get(s2.size() - 1); + boolean green3 = s3.get(s3.size() - 1); + + //SHOOTER: + + double kF = 1.0 / MAX_RPM; // baseline feedforward + + double encoderRevolutions = (double) robot.shooterEncoder.getCurrentPosition() / 2048; + + double velocity = -60 * (encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp); + + double velPID; + + // --- FEEDFORWARD BASE POWER --- + double feed = kF * vel; // Example: vel=2500 → feed=0.5 + + // --- PROPORTIONAL CORRECTION --- + double error = vel - velocity; + double correction = kP * error; + + // limit how fast power changes (prevents oscillation) + correction = Math.max(-maxStep, Math.min(maxStep, correction)); + + // --- FINAL MOTOR POWER --- + velPID = feed + correction; + + // clamp to allowed range + velPID = Math.max(0, Math.min(1, velPID)); + + robot.shooter1.setPower(velPID); + robot.shooter2.setPower(velPID); + + //TODO: ADD CODE TO CHANGE VARIABLE VEL BASED ON POSITION + + //MISC: for (LynxModule hub : allHubs) { hub.clearBulkCache(); } + TELE.addData("Spin1Green", s1.get(s1.size() - 1)); + TELE.addData("Spin2Green", s2.get(s2.size() - 1)); + TELE.addData("Spin3Green", s3.get(s3.size() - 1)); + 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 deleted file mode 100644 index d4e8275..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/AprilTag.java +++ /dev/null @@ -1,238 +0,0 @@ -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 deleted file mode 100644 index b605afb..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Drivetrain.java +++ /dev/null @@ -1,99 +0,0 @@ -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 deleted file mode 100644 index 2e95308..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Intake.java +++ /dev/null @@ -1,80 +0,0 @@ -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 deleted file mode 100644 index 9750d9c..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Shooter.java +++ /dev/null @@ -1,249 +0,0 @@ -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 deleted file mode 100644 index 673dfb5..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Spindexer.java +++ /dev/null @@ -1,256 +0,0 @@ -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 deleted file mode 100644 index 575275e..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Subsystem.java +++ /dev/null @@ -1,6 +0,0 @@ -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 deleted file mode 100644 index 72c79b5..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Transfer.java +++ /dev/null @@ -1,59 +0,0 @@ -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); - - } -} 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 3b56922..da51023 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,19 +1,16 @@ package org.firstinspires.ftc.teamcode.utils; +import com.qualcomm.hardware.rev.RevColorSensorV3; 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; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; -import org.openftc.easyopencv.OpenCvWebcam; public class Robot { @@ -30,8 +27,6 @@ public class Robot { public DcMotorEx transfer; - - public DcMotorEx shooter1; public DcMotorEx shooter2; public Servo hood; @@ -40,7 +35,6 @@ public class Robot { public Servo rejecter; - public Servo turr1; public Servo turr2; @@ -62,27 +56,31 @@ public class Robot { public AnalogInput analogInput2; - public AprilTagProcessor aprilTagProcessor; + public AnalogInput spin1Pos; + public AnalogInput spin2Pos; + + public AnalogInput hoodPos; + + public AnalogInput turr1Pos; + + public AnalogInput turr2Pos; + + public AnalogInput transferServoPos; + + public AprilTagProcessor aprilTagProcessor; public WebcamName webcam; public DcMotorEx shooterEncoder; + public RevColorSensorV3 color1; + public RevColorSensorV3 color2; + public RevColorSensorV3 color3; - - - - - - - - - - - public Robot (HardwareMap hardwareMap) { + public Robot(HardwareMap hardwareMap) { //Define components w/ hardware map @@ -109,14 +107,24 @@ public class Robot { hood = hardwareMap.get(Servo.class, "hood"); + hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos"); + turr1 = hardwareMap.get(Servo.class, "t1"); + turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); + turr2 = hardwareMap.get(Servo.class, "t2"); + turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos"); + spin1 = hardwareMap.get(Servo.class, "spin1"); + spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos"); + spin2 = hardwareMap.get(Servo.class, "spin2"); + spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos"); + pin0 = hardwareMap.get(DigitalChannel.class, "pin0"); pin1 = hardwareMap.get(DigitalChannel.class, "pin1"); @@ -129,27 +137,26 @@ public class Robot { 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"); + transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos"); + transfer.setDirection(DcMotorSimple.Direction.REVERSE); aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults(); - webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); + color1 = hardwareMap.get(RevColorSensorV3.class, "c1"); + color2 = hardwareMap.get(RevColorSensorV3.class, "c2"); - - + color3 = hardwareMap.get(RevColorSensorV3.class, "c3"); } }