diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java index 8005df8..aeb0d7b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java @@ -102,7 +102,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { public static double shoot0Time = 1.6; public static double intake1Time = 3.0; public static double flywheel0Time = 3.5; - public static double pickup1Speed = 25; + public static double pickup1Speed = 17; // ---- SECOND SHOT / PICKUP ---- public static double shoot1Vel = 2300; public static double shoot1Hood = 0.93; @@ -720,7 +720,6 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { new TranslationalVelConstraint(pickup1Speed)); shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b)) - .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) @@ -729,7 +728,6 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { new TranslationalVelConstraint(pickup1Speed)); shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b)) - .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1)) @@ -737,7 +735,6 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { .strafeToLinearHeading(new Vector2d(x4b, y4b), h4b, new TranslationalVelConstraint(pickup1Speed)); shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b)) - .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java index 77d7299..1b38734 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java @@ -111,7 +111,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public static double shoot0Time = 1.6; public static double intake1Time = 3.0; public static double flywheel0Time = 3.5; - public static double pickup1Speed = 25; + public static double pickup1Speed = 17; // ---- SECOND SHOT / PICKUP ---- public static double shoot1Vel = 2300; public static double shoot1Hood = 0.93; @@ -836,7 +836,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { new TranslationalVelConstraint(pickup1Speed)); shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b)) - .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) @@ -845,7 +844,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { new TranslationalVelConstraint(pickup1Speed)); shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b)) - .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1)) @@ -853,7 +851,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { .strafeToLinearHeading(new Vector2d(x4b, y4b), h4b, new TranslationalVelConstraint(pickup1Speed)); shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b)) - .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); TELE.addData("Red?", redAlliance); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java index 02ac4df..fea5d76 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java @@ -7,5 +7,5 @@ public class Color { public static boolean redAlliance = true; public static double Light0 = 0.28, Light1 = 0.67, Light2 = 0.36, Light3 = 0.5; - public static double colorFilterAlpha = 0.15; + public static double colorFilterAlpha = 1; } 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 d9d4b96..941f3da 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 @@ -16,10 +16,11 @@ public class ServoPositions { public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6; public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4; - public static double spinStartPos = spindexer_outtakeBall3 - 0.1; + public static double spinStartPos = 0.25; + public static double spinEndPos = 0.85; public static double shootAllAutoSpinStartPos = 0.2; - public static double shootAllSpindexerSpeedIncrease = 0.02; + public static double shootAllSpindexerSpeedIncrease = 0.04; public static double shootAllTime = 1.8; public static double transferServo_out = 0.15; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 84210e9..99e1697 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -168,14 +168,6 @@ public class TeleopV3 extends LinearOpMode { PIDFController tController = new PIDFController(tp, ti, td, tf); tController.setTolerance(0.001); -// -// if (redAlliance) { -// robot.limelight.pipelineSwitch(3); -// } else { -// robot.limelight.pipelineSwitch(2); -// } - -// robot.limelight.start(); Turret turret = new Turret(robot, TELE, robot.limelight); robot.light.setPosition(1); @@ -264,57 +256,6 @@ public class TeleopV3 extends LinearOpMode { } -// if (autoSpintake) { -// -// if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) { -// -// -// robot.spin1.setPosition(spindexPos); -// robot.spin2.setPosition(1-spindexPos); -// -// } -// -// if (gamepad1.right_bumper) { -// -// shootAll = false; -// -// intakeTicker++; -// -//// if (intakeTicker % 20 < 2) { -//// -//// robot.spin1.setPower(-1); -//// robot.spin2.setPower(1); -//// -//// } else if (intakeTicker % 20 < 10) { -//// robot.spin1.setPower(-0.5); -//// robot.spin2.setPower(0.5); -//// } else if (intakeTicker % 20 < 12) { -//// robot.spin1.setPower(1); -//// robot.spin2.setPower(-1); -//// } else { -//// robot.spin1.setPower(0.5); -//// robot.spin2.setPower(-0.5); -//// } -// -// robot.intake.setPower(1); -// intakeStamp = getRuntime(); -// TELE.addData("Reverse?", reverse); -// TELE.update(); -// } else { -// if (!servo.spinEqual(spindexPos)) { -// robot.spin1.setPosition(spindexPos); -// robot.spin2.setPosition(1-spindexPos); -// -// } -// -// spindexPos = spindexer_intakePos1; -// -// robot.intake.setPower(0); -// -// intakeTicker = 0; -// } -// } - //COLOR: double s1D = robot.color1.getDistance(DistanceUnit.MM); @@ -447,31 +388,6 @@ public class TeleopV3 extends LinearOpMode { //SHOOTER: flywheel.manageFlywheel(vel); - //TODO: test the camera teleop code - -// if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving -// double bearing; -// -// LLResult result = robot.light.getLatestResult(); -// if (result != null) { -// if (result.isValid()) { -// bearing = result.getTx() * bMult; -// -// double bearingCorrection = bearing / bDiv; -// -// error += bearingCorrection; -// -// camTicker++; -// TELE.addData("tx", bearingCorrection); -// TELE.addData("ty", result.getTy()); -// } -// } -// -// } else { -// camTicker = 0; -// overrideTurr = false; -// } - //HOOD: if (targetingHood) { @@ -491,7 +407,6 @@ public class TeleopV3 extends LinearOpMode { autoHoodOffset += 0.02; } -// if (gamepad2.cross) { manualOffset = 0; @@ -527,77 +442,6 @@ public class TeleopV3 extends LinearOpMode { // yOffset = robotY; // headingOffset = robotHeading; // } -// } - - //if (gamepad1.left_bumper && !enableSpindexerManager) { - - // robot.transferServo.setPosition(transferServo_out); - -// autoSpintake = false; -// -// intakeTicker++; -// -// if (intakeTicker % 10 < 1) { -// -// robot.spin1.setPower(-1); -// robot.spin2.setPower(1); -// -// } else if (intakeTicker % 10 < 5) { -// robot.spin1.setPower(-0.5); -// robot.spin2.setPower(0.5); -// } else if (intakeTicker % 10 < 6) { -// robot.spin1.setPower(1); -// robot.spin2.setPower(-1); -// } else { -// robot.spin1.setPower(0.5); -// robot.spin2.setPower(-0.5); -// } -// -// intake = false; -// reject = false; -// -// robot.intake.setPower(0.5); -// -// } -// -// if (gamepad1.leftBumperWasReleased() && !enableSpindexerManager) { -// shootStamp = getRuntime(); -// shootAll = true; -// -// shooterTicker = 0; -// } -// -// if (shootAll && !enableSpindexerManager) { -// -// TELE.addData("100% works", shootOrder); -// -// intake = false; -// reject = false; -// -// shooterTicker++; -// -// spindexPos = spindexer_intakePos1; -// -// if (getRuntime() - shootStamp < 3.5) { -// -// robot.transferServo.setPosition(transferServo_in); -// -// autoSpintake = false; -// -// robot.spin1.setPower(-spinPow); -// robot.spin2.setPower(spinPow); -// -// } else { -// robot.transferServo.setPosition(transferServo_out); -// spindexPos = spindexer_intakePos1; -// -// shootAll = false; -// -// autoSpintake = true; -// -// robot.transferServo.setPosition(transferServo_out); -// } -// // } if (enableSpindexerManager) { @@ -672,12 +516,12 @@ public class TeleopV3 extends LinearOpMode { if (shooterTicker == 0) { - spindexer.prepareShootAll(); + spindexer.prepareShootAllContinous(); TELE.addLine("preparing to shoot"); - } else if (shooterTicker == 2) { - //robot.transferServo.setPosition(transferServo_in); - spindexer.shootAll(); - TELE.addLine("starting to shoot"); +// } else if (shooterTicker == 2) { +// //robot.transferServo.setPosition(transferServo_in); +// spindexer.shootAll(); +// TELE.addLine("starting to shoot"); } else if (!spindexer.shootAllComplete()) { robot.transferServo.setPosition(transferServo_in); TELE.addLine("shoot"); @@ -699,7 +543,6 @@ public class TeleopV3 extends LinearOpMode { shootAll = false; spindexer.resetSpindexer(); - spindexer.processIntake(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index 670d7cb..057035a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -5,6 +5,9 @@ import com.arcrobotics.ftclib.controller.PIDFController; import com.qualcomm.robotcore.hardware.HardwareMap; import static org.firstinspires.ftc.teamcode.constants.Color.*; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos2; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos3; @@ -72,7 +75,9 @@ public class Spindexer { SHOOTMOVING, SHOOTWAIT, SHOOT_ALL_PREP, - SHOOT_ALL_READY + SHOOT_ALL_READY, + SHOOT_PREP_CONTINOUS, + SHOOT_CONTINOUS } int shootWaitCount = 0; @@ -143,6 +148,9 @@ public class Spindexer { ballPositions[pos].isEmpty = true; ballPositions[pos].foundEmpty = 0; ballPositions[pos].ballColor = BallColor.UNKNOWN; + distanceRearCenter = 61; + distanceFrontDriver = 61; + distanceFrontPassenger = 61; } public void resetSpindexer () { @@ -263,8 +271,8 @@ public class Spindexer { private void moveSpindexerToPos(double pos) { robot.spin1.setPosition(pos); robot.spin2.setPosition(1-pos); - double currentPos = servos.getSpinPos(); - if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ +// double currentPos = servos.getSpinPos(); +// if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ // if (currentPos > pos){ // robot.spin1.setPosition(servos.getSpinPos() + 0.05); // robot.spin2.setPosition(1 - servos.getSpinPos() - 0.05); @@ -272,8 +280,8 @@ public class Spindexer { // robot.spin1.setPosition(servos.getSpinPos() - 0.05); // robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05); // } - } - prevPos = currentPos; +// } +// prevPos = currentPos; } public void stopSpindexer() { @@ -309,6 +317,8 @@ public class Spindexer { public boolean isFull () { return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty); } + + private double intakeTicker = 0; public boolean processIntake() { switch (currentIntakeState) { @@ -359,15 +369,15 @@ public class Spindexer { commandedIntakePosition = 0; currentIntakeState = Spindexer.IntakeState.MOVING; } - proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos); + //proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos); //if (ballPositions[1].isEmpty && (proposedTravelDistance < commandedtravelDistance)) { - if (ballPositions[1].isEmpty) { + else if (ballPositions[1].isEmpty) { // Position 2 commandedIntakePosition = 1; currentIntakeState = Spindexer.IntakeState.MOVING; } - proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos); - if (ballPositions[2].isEmpty) { + //proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos); + else if (ballPositions[2].isEmpty) { // Position 3 commandedIntakePosition = 2; currentIntakeState = Spindexer.IntakeState.MOVING; @@ -383,8 +393,13 @@ public class Spindexer { case MOVING: // Stopping when we get to the new position if (servos.spinEqual(intakePositions[commandedIntakePosition])) { - currentIntakeState = Spindexer.IntakeState.INTAKE; - stopSpindexer(); + if (intakeTicker > 1){ + currentIntakeState = Spindexer.IntakeState.INTAKE; + stopSpindexer(); + intakeTicker = 0; + } else { + intakeTicker++; + } //detectBalls(false, false); } else { // Keep moving the spindexer @@ -476,10 +491,33 @@ public class Spindexer { //detectBalls(true, false); } // Keep moving the spindexer - spindexerOuttakeWiggle *= -1.01; + spindexerOuttakeWiggle *= -1.0; moveSpindexerToPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle); break; + case SHOOT_PREP_CONTINOUS: + if (servos.spinEqual(spinStartPos)){ + currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; + } else { + moveSpindexerToPos(spinStartPos); + } + break; + + case SHOOT_CONTINOUS: + ballPositions[0].isEmpty = false; + ballPositions[1].isEmpty = false; + ballPositions[2].isEmpty = false; + if (servos.getSpinPos() > spinEndPos){ + currentIntakeState = IntakeState.FINDNEXT; + } else { + double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease; + if (spinPos > spinEndPos + 0.03){ + spinPos = spinEndPos + 0.03; + } + moveSpindexerToPos(spinPos); + } + break; + default: // Statements to execute if no case matches } @@ -539,12 +577,21 @@ public class Spindexer { public void prepareShootAll(){ currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP; } + public void prepareShootAllContinous(){ + currentIntakeState = Spindexer.IntakeState.SHOOT_PREP_CONTINOUS; + } public void shootAll () { ballPositions[0].isEmpty = false; ballPositions[1].isEmpty = false; ballPositions[2].isEmpty = false; currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; } + public void shootAllContinous(){ + ballPositions[0].isEmpty = false; + ballPositions[1].isEmpty = false; + ballPositions[2].isEmpty = false; + currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; + } public boolean shootAllComplete () { @@ -552,7 +599,9 @@ public class Spindexer { (currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_READY) && (currentIntakeState != Spindexer.IntakeState.SHOOTMOVING) && (currentIntakeState != Spindexer.IntakeState.SHOOTNEXT) && - (currentIntakeState != Spindexer.IntakeState.SHOOTWAIT)); + (currentIntakeState != Spindexer.IntakeState.SHOOTWAIT) && + (currentIntakeState != Spindexer.IntakeState.SHOOT_PREP_CONTINOUS) && + (currentIntakeState != Spindexer.IntakeState.SHOOT_CONTINOUS)); } void shootAllToIntake () {