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.java similarity index 58% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 766378e..3e8e9c4 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.java @@ -1,62 +1,9 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; @@ -64,6 +11,8 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; +import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; +import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import androidx.annotation.NonNull; @@ -82,7 +31,6 @@ import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.constants.Poses_V2; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; @@ -95,30 +43,30 @@ import java.util.Objects; @Config @Autonomous(preselectTeleOp = "TeleopV3") -public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { +public class Auto_LT_Close extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset; public static double autoSpinStartPos = 0.2; - public static double shoot0SpinSpeedIncrease = 0.015; + public static double shoot0SpinSpeedIncrease = 0.02; public static double spindexerSpeedIncrease = 0.03; - public static double finalSpindexerSpeedIncrease = 0.025; + public static double finalSpindexerSpeedIncrease = 0.03; + // These values are ADDED to turrDefault + public static double redObeliskTurrPos1 = 0.12; + public static double redObeliskTurrPos2 = 0.13; + public static double redObeliskTurrPos3 = 0.14; + public static double blueObeliskTurrPos1 = -0.12; + public static double blueObeliskTurrPos2 = -0.13; + public static double blueObeliskTurrPos3 = -0.14; + public static double redTurretShootPos = 0.1; + public static double blueTurretShootPos = -0.14; - public static double redObeliskTurrPos1 = turrDefault + 0.12; - public static double redObeliskTurrPos2 = turrDefault + 0.13; - public static double redObeliskTurrPos3 = turrDefault + 0.14; - - public static double blueObeliskTurrPos1 = turrDefault - 0.12; - public static double blueObeliskTurrPos2 = turrDefault - 0.13; - public static double blueObeliskTurrPos3 = turrDefault - 0.14; double obeliskTurrPos1 = 0.0; double obeliskTurrPos2 = 0.0; double obeliskTurrPos3 = 0.0; public static double normalIntakeTime = 3.3; public static double shoot1Turr = 0.57; public static double shoot0XTolerance = 1.0; - public static double redTurretShootPos = turrDefault + 0.12; - public static double blueTurretShootPos = turrDefault - 0.14; double turretShootPos = 0.0; public static double finalShootAllTime = 3.0; @@ -130,7 +78,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public static double intake3Time = 4.2; public static double flywheel0Time = 3.5; - public static double pickup1Speed = 23; + public static double pickup1Speed = 15; // ---- SECOND SHOT / PICKUP ---- public static double shoot1Vel = 2300; public static double shootAllVelocity = 2500; @@ -163,21 +111,22 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { Targeting.Settings targetingSettings; private double firstSpindexShootPos = autoSpinStartPos; private boolean shootForward = true; - private double x1, y1, h1; + double x1, y1, h1; - private double x2a, y2a, h2a, t2a; + double x2a, y2a, h2a, t2a; - private double x2b, y2b, h2b, t2b; - private double x2c, y2c, h2c, t2c; + double x2b, y2b, h2b, t2b; + double x2c, y2c, h2c, t2c; - private double x3a, y3a, h3a; - private double x3b, y3b, h3b; - private double x4a, y4a, h4a; - private double x4b, y4b, h4b; + double x3a, y3a, h3a; + double x3b, y3b, h3b; + double x4a, y4a, h4a; + double x4b, y4b, h4b; - private double xShoot, yShoot, hShoot; - private double xGate, yGate, hGate; - private double xPrep, yPrep, hPrep; + double xShoot, yShoot, hShoot; + double xGate, yGate, hGate; + double xPrep, yPrep, hPrep; + double xLeave, yLeave, hLeave; private double shoot1Tangent; @@ -186,6 +135,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { private int rearSlotGreen = 0; private int mostGreenSlot = 0; + int ballCycles = 3; + int prevMotif = 0; public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { return new Action() { @@ -202,14 +153,29 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { stamp = System.currentTimeMillis(); } ticker++; - robot.transferServo.setPosition(transferServo_out); - - turret.manualSetTurret(turretShootPos); - + servos.setTransferPos(transferServo_out); drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + TELE.addData("Velocity", flywheel.getVelo()); TELE.addData("Hood", robot.hood.getPosition()); TELE.addData("motif", motif_id); @@ -219,8 +185,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { spindexerWiggle *= -1.0; - robot.spin1.setPosition(spindexer_intakePos1 + spindexerWiggle); - robot.spin2.setPosition(1 - spindexer_intakePos1 - spindexerWiggle); + servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle); spindexer.detectBalls(true, true); @@ -231,11 +196,12 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) { passengerSlotGreen++; } + if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) { rearSlotGreen++; } - robot.intake.setPower(1); + spindexer.setIntakePower(1); decideGreenSlot = true; @@ -250,8 +216,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { mostGreenSlot = 1; } - - decideGreenSlot = false; if (motif_id == 21) { @@ -293,14 +257,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { return true; } else if ((System.currentTimeMillis() - stamp) < (time * 1000)) { +// TELE.addData("MostGreenSlot", mostGreenSlot); +// TELE.update(); + spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); - TELE.addData("MostGreenSlot", mostGreenSlot); - - - robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); - - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); + servos.setSpinPos(firstSpindexShootPos); return true; } else { @@ -314,11 +275,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public Action shootAll(int vel, double shootTime, double spindexSpeed) { return new Action() { int ticker = 1; - double stamp = 0.0; - double velo = vel; - int shooterTicker = 0; @Override @@ -327,6 +285,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { TELE.addData("Hood", robot.hood.getPosition()); TELE.update(); + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); velo = flywheel.getVelo(); @@ -334,36 +294,51 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { teleStart = drive.localizer.getPose(); - robot.intake.setPower(-0.3); + spindexer.setIntakePower(-0.3); if (ticker == 1) { stamp = getRuntime(); } ticker++; - robot.intake.setPower(0); + spindexer.setIntakePower(0); drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); - if (getRuntime() - stamp < shootTime) { + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + + if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); + servos.setSpinPos(autoSpinStartPos); } else { - robot.transferServo.setPosition(transferServo_in); + servos.setTransferPos(transferServo_in); shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); + double prevSpinPos = servos.getSpinCmdPos(); + servos.setSpinPos(prevSpinPos + spindexSpeed); } return true; } else { - robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; + servos.setTransferPos(transferServo_out); spindexer.resetSpindexer(); spindexer.processIntake(); @@ -398,43 +373,57 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { teleStart = drive.localizer.getPose(); - robot.intake.setPower(-0.3); + spindexer.setIntakePower(-0.3); if (ticker == 1) { stamp = getRuntime(); } ticker++; - robot.intake.setPower(0); + spindexer.setIntakePower(0); drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + if (getRuntime() - stamp < shootTime) { if (getRuntime() - stamp < firstShootTime) { - robot.transferServo.setPosition(transferServo_in); - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); + servos.setTransferPos(transferServo_out); + servos.setSpinPos(firstSpindexShootPos); } else { - robot.transferServo.setPosition(transferServo_in); + servos.setTransferPos(transferServo_in); shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); + double prevSpinPos = servos.getSpinCmdPos(); if (shootForward) { - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); + servos.setSpinPos(prevSpinPos + spindexSpeed); } else { - robot.spin1.setPosition(prevSpinPos - spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos + spindexSpeed); + servos.setSpinPos(prevSpinPos - spindexSpeed); } } return true; } else { - robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; + servos.setTransferPos(transferServo_out); spindexer.resetSpindexer(); spindexer.processIntake(); @@ -459,18 +448,17 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { ticker++; spindexer.processIntake(); - robot.intake.setPower(1); + spindexer.setIntakePower(1); spindexer.ballCounterLight(); drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); + + TELE.addData("Full?", spindexer.isFull()); TELE.update(); - return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); - + return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); } }; } @@ -507,8 +495,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { ticker++; motif = turret.detectObelisk(); - robot.turr1.setPosition(turrPos); - robot.turr2.setPosition(1 - turrPos); + turret.setTurret(turrPos); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; @@ -568,8 +555,10 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { ticker++; + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); - robot.hood.setPosition(hoodPos); + servos.setHoodPos(hoodPos); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; @@ -637,8 +626,10 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { turret.trackGoal(deltaPose); - robot.hood.setPosition(targetingSettings.hoodAngle); + servos.setHoodPos(targetingSettings.hoodAngle); + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(targetingSettings.flywheelRPM); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; @@ -706,8 +697,10 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { targetingSettings = targeting.calculateSettings (robotX, robotY, robotHeading, 0.0, false); - robot.hood.setPosition(targetingSettings.hoodAngle); + servos.setHoodPos(targetingSettings.hoodAngle); + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(targetingSettings.flywheelRPM); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; @@ -728,6 +721,15 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { }; } + // initialize path variables here + TrajectoryActionBuilder shoot0 = null; + TrajectoryActionBuilder pickup1 = null; + TrajectoryActionBuilder shoot1 = null; + TrajectoryActionBuilder pickup2 = null; + TrajectoryActionBuilder shoot2 = null; + TrajectoryActionBuilder pickup3 = null; + TrajectoryActionBuilder shoot3 = null; + @Override public void runOpMode() throws InterruptedException { @@ -746,62 +748,49 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { servos = new Servos(hardwareMap); - robot.limelight.start(); - - robot.limelight.pipelineSwitch(1); - - turret = new Turret(robot, TELE, robot.limelight); - turret.manualSetTurret(turrDefault); + turret.setTurret(turrDefault); drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); + servos.setSpinPos(autoSpinStartPos); - robot.transferServo.setPosition(transferServo_out); - - TrajectoryActionBuilder shoot0 = null; - TrajectoryActionBuilder pickup1 = null; - TrajectoryActionBuilder shoot1 = null; - TrajectoryActionBuilder pickup2 = null; - TrajectoryActionBuilder shoot2 = null; - TrajectoryActionBuilder pickup3 = null; - TrajectoryActionBuilder shoot3 = null; + servos.setTransferPos(transferServo_out); + limelightUsed = false; robot.light.setPosition(1); while (opModeInInit()) { - robot.hood.setPosition(shoot0Hood); - turret.manualSetTurret(turrDefault); + servos.setHoodPos(shoot0Hood); + turret.setTurret(turrDefault); if (gamepad2.crossWasPressed()) { redAlliance = !redAlliance; } if (gamepad2.dpadLeftWasPressed()) { - turrDefault -=0.01; + turrDefault -= 0.01; } if (gamepad2.dpadRightWasPressed()) { - turrDefault +=0.01; + turrDefault += 0.01; } - redObeliskTurrPos1 = turrDefault + 0.12; - redObeliskTurrPos2 = turrDefault + 0.13; - redObeliskTurrPos3 = turrDefault + 0.14; - - blueObeliskTurrPos1 = turrDefault - 0.12; - blueObeliskTurrPos2 = turrDefault - 0.13; - blueObeliskTurrPos3 = turrDefault - 0.14; - - redTurretShootPos = turrDefault + 0.12; - blueTurretShootPos = turrDefault - 0.14; - + if (gamepad2.rightBumperWasPressed()){ + ballCycles++; + } + if (gamepad2.leftBumperWasPressed()){ + ballCycles--; + } + if (gamepad2.squareWasPressed()){ + robot.limelight.start(); + robot.limelight.pipelineSwitch(1); + gamepad2.rumble(500); + } if (redAlliance) { robot.light.setPosition(0.28); @@ -836,11 +825,14 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { xShoot = rShootX; yShoot = rShootY; hShoot = rShootH; + xLeave = rLeaveX; + yLeave = rLeaveY; + hLeave = rLeaveH; - obeliskTurrPos1 = redObeliskTurrPos1; - obeliskTurrPos2 = redObeliskTurrPos2; - obeliskTurrPos3 = redObeliskTurrPos3; - turretShootPos = redTurretShootPos; + obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; + obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; + obeliskTurrPos3 = turrDefault + redObeliskTurrPos3; + turretShootPos = turrDefault + redTurretShootPos; } else { robot.light.setPosition(0.6); @@ -876,44 +868,57 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { xShoot = bShootX; yShoot = bShootY; hShoot = bShootH; + xLeave = bLeaveX; + yLeave = bLeaveY; + hLeave = bLeaveH; - obeliskTurrPos1 = blueObeliskTurrPos1; - obeliskTurrPos2 = blueObeliskTurrPos2; - obeliskTurrPos3 = blueObeliskTurrPos3; - turretShootPos = blueTurretShootPos; + obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; + obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; + obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; + turretShootPos = turrDefault + blueTurretShootPos; } shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(x1, y1), h1); + .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1)); - pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - .strafeToLinearHeading(new Vector2d(x2a, y2a), h2a) - .strafeToLinearHeading(new Vector2d(x2b, y2b), h2b, + pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1))) + .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) + .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b), new TranslationalVelConstraint(pickup1Speed)); - shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b)) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + if (ballCycles < 2){ + shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + } else { + shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); + } - pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) - .strafeToLinearHeading(new Vector2d(x3a, y3a), h3a) - .strafeToLinearHeading(new Vector2d(x3b, y3b), h3b, + pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a)) + .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b), new TranslationalVelConstraint(pickup1Speed)); - shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b)) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + if (ballCycles < 3){ + shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + } else { + shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hLeave)); + } - pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - .strafeToLinearHeading(new Vector2d(x4a, y4a), h4a) - .strafeToLinearHeading(new Vector2d(x4b, y4b), h4b, + pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(x4a, y4a), Math.toRadians(h4a)) + .strafeToLinearHeading(new Vector2d(x4b, y4b), Math.toRadians(h4b), new TranslationalVelConstraint(pickup1Speed)); - shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b)) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + + shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, Math.toRadians(h4b))) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); - - + TELE.addData("Ball Cycles", ballCycles); TELE.update(); } @@ -926,202 +931,236 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { robot.transfer.setPower(1); - assert shoot0 != null; + startAuto(); - Actions.runBlocking( - new ParallelAction( - shoot0.build(), - manageFlywheel( - shoot0Vel, - shoot0Hood, - flywheel0Time, - x1, - 0.501, - shoot0XTolerance, - 0.501 - ) + if (ballCycles > 0){ + cycleStackClose(); + } - ) - ); + if (ballCycles > 1){ + cycleStackMiddle(); + } - Actions.runBlocking( - shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) - ); + if (ballCycles > 2){ + cycleStackFar(); + } - Actions.runBlocking( - new ParallelAction( - pickup1.build(), - manageFlywheel( - shootAllVelocity, - shootAllHood, - intake1Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake1Time), - detectObelisk( - intake1Time, - 0.501, - 0.501, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos1 - ) + while (opModeIsActive()) { - ) - ); + drive.updatePoseEstimate(); - motif = turret.getObeliskID(); + teleStart = drive.localizer.getPose(); + flywheel.manageFlywheel(0); - if (motif == 0) motif = 22; - - - Actions.runBlocking( - new ParallelAction( - manageFlywheel( - shootAllVelocity, - shootAllHood, - shoot1Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot1.build(), - prepareShootAll(colorSenseTime, shoot1Time, motif) - ) - ); - - - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - shootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(shootAllTime, spindexerSpeedIncrease) - ) - - ); - - Actions.runBlocking( - new ParallelAction( - pickup2.build(), - manageShooterAuto( - intake2Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake2Time), - detectObelisk( - intake2Time, - 0.501, - 0.501, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos2 - ) - - ) - ); - - Actions.runBlocking( - new ParallelAction( - manageFlywheelAuto( - shoot2Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot2.build(), - prepareShootAll(colorSenseTime, shoot2Time, motif) - ) - ); - - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - shootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(shootAllTime, spindexerSpeedIncrease) - ) - - ); - - Actions.runBlocking( - new ParallelAction( - pickup3.build(), - manageShooterAuto( - intake3Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake3Time), - detectObelisk( - intake3Time, - 0.501, - 0.501, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos3 - ) - - ) - ); - - Actions.runBlocking( - new ParallelAction( - manageFlywheelAuto( - shoot3Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot3.build(), - prepareShootAll(colorSenseTime, shoot3Time, motif) - ) - ); - - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - finalShootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) - ) - - ); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addLine("finished"); - TELE.update(); - - sleep(2000); + TELE.addLine("finished"); + TELE.update(); + } } } + + void startAuto() { + assert shoot0 != null; + + Actions.runBlocking( + new ParallelAction( + shoot0.build(), + manageFlywheel( + shoot0Vel, + shoot0Hood, + flywheel0Time, + x1, + 0.501, + shoot0XTolerance, + 0.501 + ) + + ) + ); + + Actions.runBlocking( + shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) + ); + } + + void cycleStackClose(){ + Actions.runBlocking( + new ParallelAction( + pickup1.build(), + manageFlywheel( + shootAllVelocity, + shootAllHood, + intake1Time, + 0.501, + 0.501, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake1Time), + detectObelisk( + intake1Time, + 0.501, + 0.501, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos1 + ) + + ) + ); + + motif = turret.getObeliskID(); + + if (motif == 0) motif = 22; + prevMotif = motif; + + Actions.runBlocking( + new ParallelAction( + manageFlywheel( + shootAllVelocity, + shootAllHood, + shoot1Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot1.build(), + prepareShootAll(colorSenseTime, shoot1Time, motif) + ) + ); + + + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + shootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) + + ); + } + + void cycleStackMiddle(){ + Actions.runBlocking( + new ParallelAction( + pickup2.build(), + manageShooterAuto( + intake2Time, + 0.501, + 0.501, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake2Time), + detectObelisk( + intake2Time, + 0.501, + 0.501, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos2 + ) + + ) + ); + + motif = turret.getObeliskID(); + + if (motif == 0) motif = prevMotif; + prevMotif = motif; + + Actions.runBlocking( + new ParallelAction( + manageFlywheelAuto( + shoot2Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot2.build(), + prepareShootAll(colorSenseTime, shoot2Time, motif) + ) + ); + + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + shootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) + + ); + } + + void cycleStackFar(){ + Actions.runBlocking( + new ParallelAction( + pickup3.build(), + manageShooterAuto( + intake3Time, + 0.501, + 0.501, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake3Time), + detectObelisk( + intake3Time, + 0.501, + 0.501, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos3 + ) + + ) + ); + + motif = turret.getObeliskID(); + + if (motif == 0) motif = prevMotif; + prevMotif = motif; + + Actions.runBlocking( + new ParallelAction( + manageFlywheelAuto( + shoot3Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot3.build(), + prepareShootAll(colorSenseTime, shoot3Time, motif) + ) + ); + + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + finalShootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) + ) + + ); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java new file mode 100644 index 0000000..c6eebf2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -0,0 +1,938 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*; +import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; +import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; +import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.ParallelAction; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.TranslationalVelConstraint; +import com.acmerobotics.roadrunner.Vector2d; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Servos; +import org.firstinspires.ftc.teamcode.utils.Spindexer; +import org.firstinspires.ftc.teamcode.utils.Targeting; +import org.firstinspires.ftc.teamcode.utils.Turret; + +import java.util.Objects; + +@Config +@Autonomous(preselectTeleOp = "TeleopV3") +public class Auto_LT_Far extends LinearOpMode { + public static double shoot0Vel = 3300, shoot0Hood = 0.48 + hoodOffset; + public static double autoSpinStartPos = 0.2; + public static double shoot0SpinSpeedIncrease = 0.015; + public static double shoot0XTolerance = 1.0; + public static double redTurretShootPos = 0.05; + public static double blueTurretShootPos = -0.05; + public static int fwdTime = 200, strafeTime = 2300; + double xLeave, yLeave, hLeave; + public static int sleepTime = 1300; + public int motif = 0; + double turretShootPos = 0.0; + Robot robot; + MultipleTelemetry TELE; + MecanumDrive drive; + Servos servos; + Spindexer spindexer; + Flywheel flywheel; + Turret turret; + Targeting targeting; + Targeting.Settings targetingSettings; + double firstSpindexShootPos = autoSpinStartPos; + boolean shootForward = true; + double xShoot, yShoot, hShoot; + private int driverSlotGreen = 0; + private int passengerSlotGreen = 0; + int rearSlotGreen = 0; + int mostGreenSlot = 0; + double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; + double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0; + public static double firstShootTime = 0.3; + public static double flywheel0Time = 3.5; + public static double shoot0Time = 2; + boolean gatePickup = false; + boolean stack3 = true; + double xStackPickupA, yStackPickupA, hStackPickupA; + double xStackPickupB, yStackPickupB, hStackPickupB; + public static int pickupStackSpeed = 15; + int prevMotif = 0; + + public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { + return new Action() { + double stamp = 0.0; + int ticker = 0; + + double spindexerWiggle = 0.01; + + boolean decideGreenSlot = false; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + ticker++; + servos.setTransferPos(transferServo_out); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.addData("motif", motif_id); + TELE.update(); + + if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) { + + spindexerWiggle *= -1.0; + + servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle); + + spindexer.detectBalls(true, true); + + if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) { + driverSlotGreen++; + } + + if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) { + passengerSlotGreen++; + } + + if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) { + rearSlotGreen++; + } + + spindexer.setIntakePower(1); + + decideGreenSlot = true; + + return true; + } else if (decideGreenSlot) { + + if (driverSlotGreen >= passengerSlotGreen && driverSlotGreen >= rearSlotGreen) { + mostGreenSlot = 3; + } else if (passengerSlotGreen >= driverSlotGreen && passengerSlotGreen >= rearSlotGreen) { + mostGreenSlot = 2; + } else { + mostGreenSlot = 1; + } + + decideGreenSlot = false; + + if (motif_id == 21) { + if (mostGreenSlot == 1) { + firstSpindexShootPos = spindexer_outtakeBall1; + shootForward = true; + } else if (mostGreenSlot == 2) { + firstSpindexShootPos = spindexer_outtakeBall2; + shootForward = false; + } else { + firstSpindexShootPos = spindexer_outtakeBall3; + shootForward = false; + } + } else if (motif_id == 22) { + if (mostGreenSlot == 1) { + firstSpindexShootPos = spindexer_outtakeBall2; + shootForward = false; + } else if (mostGreenSlot == 2) { + firstSpindexShootPos = spindexer_outtakeBall3; + shootForward = false; + } else { + firstSpindexShootPos = spindexer_outtakeBall2; + shootForward = true; + } + + } else { + if (mostGreenSlot == 1) { + firstSpindexShootPos = spindexer_outtakeBall3; + shootForward = false; + } else if (mostGreenSlot == 2) { + firstSpindexShootPos = spindexer_outtakeBall3b; + shootForward = true; + } else { + firstSpindexShootPos = spindexer_outtakeBall1; + shootForward = true; + } + + } + + return true; + } else if ((System.currentTimeMillis() - stamp) < (time * 1000)) { +// TELE.addData("MostGreenSlot", mostGreenSlot); +// TELE.update(); + spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); + + servos.setSpinPos(firstSpindexShootPos); + + return true; + } else { + return false; + } + + } + }; + } + + public Action shootAll(int vel, double shootTime, double spindexSpeed) { + return new Action() { + int ticker = 1; + double stamp = 0.0; + double velo = vel; + int shooterTicker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); + flywheel.manageFlywheel(vel); + velo = flywheel.getVelo(); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + spindexer.setIntakePower(-0.3); + + if (ticker == 1) { + stamp = getRuntime(); + } + ticker++; + + spindexer.setIntakePower(0); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + + if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { + + if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { + servos.setSpinPos(autoSpinStartPos); + } else { + servos.setTransferPos(transferServo_in); + shooterTicker++; + double prevSpinPos = servos.getSpinCmdPos(); + servos.setSpinPos(prevSpinPos + spindexSpeed); + } + + return true; + + } else { + servos.setTransferPos(transferServo_out); + + spindexer.resetSpindexer(); + spindexer.processIntake(); + + return false; + + } + + } + }; + } + + public Action shootAllAuto(double shootTime, double spindexSpeed) { + return new Action() { + int ticker = 1; + + double stamp = 0.0; + + double velo = 0.0; + + int shooterTicker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + velo = flywheel.getVelo(); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + spindexer.setIntakePower(-0.3); + + if (ticker == 1) { + stamp = getRuntime(); + } + ticker++; + + spindexer.setIntakePower(0); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + + if (getRuntime() - stamp < shootTime) { + + if (getRuntime() - stamp < firstShootTime) { + servos.setTransferPos(transferServo_out); + servos.setSpinPos(firstSpindexShootPos); + } else { + servos.setTransferPos(transferServo_in); + shooterTicker++; + double prevSpinPos = servos.getSpinCmdPos(); + + if (shootForward) { + servos.setSpinPos(prevSpinPos + spindexSpeed); + } else { + servos.setSpinPos(prevSpinPos - spindexSpeed); + } + } + + return true; + + } else { + servos.setTransferPos(transferServo_out); + + spindexer.resetSpindexer(); + spindexer.processIntake(); + + return false; + + } + } + }; + } + + public Action intake(double intakeTime) { + return new Action() { + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + ticker++; + + spindexer.processIntake(); + spindexer.setIntakePower(1); + + spindexer.ballCounterLight(); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + TELE.addData("Full?", spindexer.isFull()); + TELE.update(); + + return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); + } + }; + } + + public Action detectObelisk( + double time, + double posX, + double posY, + double posXTolerance, + double posYTolerance, + double turrPos + ) { + + boolean timeFallback = (time != 0.501); + boolean posXFallback = (posX != 0.501); + boolean posYFallback = (posY != 0.501); + + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + robot.limelight.pipelineSwitch(1); + } + + ticker++; + motif = turret.detectObelisk(); + + turret.setTurret(turrPos); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; + boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; + + boolean shouldFinish = timeDone || xDone || yDone; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + if (shouldFinish){ + if (redAlliance){ + robot.limelight.pipelineSwitch(4); + } else { + robot.limelight.pipelineSwitch(2); + } + return false; + } else { + return true; + } + + } + }; + } + + public Action manageFlywheel( + double vel, + double hoodPos, + double time, + double posX, + double posY, + double posXTolerance, + double posYTolerance + ) { + + boolean timeFallback = (time != 0.501); + boolean posXFallback = (posX != 0.501); + boolean posYFallback = (posY != 0.501); + + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + + ticker++; + + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); + flywheel.manageFlywheel(vel); + servos.setHoodPos(hoodPos); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; + boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; + + boolean shouldFinish = timeDone || xDone || yDone; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + return !shouldFinish; + + } + }; + } + + public Action manageShooterAuto( + double time, + double posX, + double posY, + double posXTolerance, + double posYTolerance + ) { + + boolean timeFallback = (time != 0.501); + boolean posXFallback = (posX != 0.501); + boolean posYFallback = (posY != 0.501); + + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + + ticker++; + + double robotX = drive.localizer.getPose().position.x; + double robotY = drive.localizer.getPose().position.y; + + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robotX - goalX; // delta x from robot to goal + double dy = robotY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robotX, robotY, robotHeading, 0.0, false); + + turret.trackGoal(deltaPose); + + servos.setHoodPos(targetingSettings.hoodAngle); + + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); + flywheel.manageFlywheel(targetingSettings.flywheelRPM); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; + boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; + + boolean shouldFinish = timeDone || xDone || yDone; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + return !shouldFinish; + + } + }; + } + + public Action manageFlywheelAuto( + double time, + double posX, + double posY, + double posXTolerance, + double posYTolerance + ) { + + boolean timeFallback = (time != 0.501); + boolean posXFallback = (posX != 0.501); + boolean posYFallback = (posY != 0.501); + + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + + ticker++; + + double robotX = drive.localizer.getPose().position.x; + double robotY = drive.localizer.getPose().position.y; + + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robotX - goalX; // delta x from robot to goal + double dy = robotY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robotX, robotY, robotHeading, 0.0, false); + + servos.setHoodPos(targetingSettings.hoodAngle); + + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); + flywheel.manageFlywheel(targetingSettings.flywheelRPM); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; + boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; + + boolean shouldFinish = timeDone || xDone || yDone; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + return !shouldFinish; + + } + }; + } + + // initialize path variables here + TrajectoryActionBuilder leave3Ball = null; + TrajectoryActionBuilder leaveFromShoot = null; + TrajectoryActionBuilder pickup3 = null; + TrajectoryActionBuilder shoot3 = null; + + @Override + public void runOpMode() throws InterruptedException { + + robot = new Robot(hardwareMap); + + TELE = new MultipleTelemetry( + telemetry, FtcDashboard.getInstance().getTelemetry() + ); + + flywheel = new Flywheel(hardwareMap); + + targeting = new Targeting(); + targetingSettings = new Targeting.Settings(0.0, 0.0); + + spindexer = new Spindexer(hardwareMap); + + servos = new Servos(hardwareMap); + + robot.limelight.start(); + + robot.limelight.pipelineSwitch(1); + + turret = new Turret(robot, TELE, robot.limelight); + + turret.setTurret(turrDefault); + + drive = new MecanumDrive(hardwareMap, autoStart); + + servos.setSpinPos(autoSpinStartPos); + + servos.setTransferPos(transferServo_out); + + while (opModeInInit()) { + + // Recalibration in initialization + drive.updatePoseEstimate(); + if (gamepad2.triangle) { + autoStart = drive.localizer.getPose(); // use this position as starting position + gamepad2.rumble(1000); + } + + if (gamepad2.squareWasPressed()){ + robot.limelight.start(); + robot.limelight.pipelineSwitch(1); + gamepad2.rumble(500); + } + + if (gamepad2.leftBumperWasPressed()){ + gatePickup = !gatePickup; + } + if (gamepad2.rightBumperWasPressed()){ + stack3 = !stack3; + } + + turret.setTurret(turretShootPos); + + robot.hood.setPosition(shoot0Hood); + + if (gamepad2.crossWasPressed()) { + redAlliance = !redAlliance; + } + + if (gamepad2.dpadLeftWasPressed()) { + turrDefault -= 0.01; + } + + if (gamepad2.dpadRightWasPressed()) { + turrDefault += 0.01; + } + + if (redAlliance) { + robot.light.setPosition(0.28); + + xLeave = rLeaveX; + yLeave = rLeaveY; + hLeave = rLeaveH; + + xShoot = rShootX; + yShoot = rShootY; + hShoot = rShootH; + + xStackPickupA = rStackPickupAX; + yStackPickupA = rStackPickupAY; + hStackPickupA = rStackPickupAH; + + xStackPickupB = rStackPickupBX; + yStackPickupB = rStackPickupBY; + hStackPickupB = rStackPickupBH; + + turretShootPos = turrDefault + redTurretShootPos; + } else { + robot.light.setPosition(0.6); + + xLeave = bLeaveX; + yLeave = bLeaveY; + hLeave = bLeaveH; + + xShoot = bShootX; + yShoot = bShootY; + hShoot = bShootH; + + xStackPickupA = bStackPickupAX; + yStackPickupA = bStackPickupAY; + hStackPickupA = bStackPickupAH; + + xStackPickupB = bStackPickupBX; + yStackPickupB = bStackPickupBY; + hStackPickupB = bStackPickupBH; + + turretShootPos = turrDefault + blueTurretShootPos; + } + + leave3Ball = drive.actionBuilder(autoStart) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + + leaveFromShoot = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + + pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA)) + .strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB), + new TranslationalVelConstraint(pickupStackSpeed)); + + shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB))) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); + + TELE.addData("Red?", redAlliance); + TELE.addData("Turret Default", turrDefault); + TELE.addData("Gate Cycle?", gatePickup); + TELE.addData("Pickup Stack?", stack3); + TELE.addData("Start Position", autoStart); + TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift + TELE.update(); + } + + waitForStart(); + + if (isStopRequested()) return; + + // Currently only shoots; keep this start and modify times and then add extra paths + if (opModeIsActive()) { + + double stamp = getRuntime(); + + robot.transfer.setPower(1); + + startAuto(); + + if (stack3){ + //cycleStackFar(); + } + + if (gatePickup || stack3){ + leave(); + } else { + leave3Ball(); + } + + // Actual way to end autonomous in to find final position + while (opModeIsActive()) { + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + flywheel.manageFlywheel(0); + + TELE.addLine("finished"); + TELE.update(); + } + + } + + } + + void startAuto(){ + Actions.runBlocking( + new ParallelAction( + manageFlywheel( + shoot0Vel, + shoot0Hood, + flywheel0Time, + 0.501, + 0.501, + shoot0XTolerance, + 0.501 + ) + + ) + ); + + Actions.runBlocking( + shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) + ); + } + + void leave3Ball(){ + assert leave3Ball != null; + Actions.runBlocking(leave3Ball.build()); + } + + void leave(){ + assert leaveFromShoot != null; + Actions.runBlocking(leaveFromShoot.build()); + } + +// void cycleStackFar(){ +// Actions.runBlocking( +// new ParallelAction( +// pickup3.build(), +// manageShooterAuto( +// intake3Time, +// 0.501, +// 0.501, +// 0.501, +// 0.501 +// ), +// intake(intake3Time), +// detectObelisk( +// intake3Time, +// 0.501, +// 0.501, +// 0.501, +// 0.501, +// obeliskTurrPos3 +// ) +// +// ) +// ); +// +// motif = turret.getObeliskID(); +// +// if (motif == 0) motif = prevMotif; +// prevMotif = motif; +// +// Actions.runBlocking( +// new ParallelAction( +// manageFlywheelAuto( +// shoot3Time, +// 0.501, +// 0.501, +// 0.501, +// 0.501 +// ), +// shoot3.build(), +// prepareShootAll(colorSenseTime, shoot3Time, motif) +// ) +// ); +// +// Actions.runBlocking( +// new ParallelAction( +// manageShooterAuto( +// finalShootAllTime, +// 0.501, +// 0.501, +// 0.501, +// 0.501 +// ), +// shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) +// ) +// +// ); +// } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_3Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_3Ball.java deleted file mode 100644 index 12dab12..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_3Ball.java +++ /dev/null @@ -1,403 +0,0 @@ -package org.firstinspires.ftc.teamcode.autonomous; - -import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; -import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; - -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.ParallelAction; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.ftc.Actions; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.utils.Flywheel; -import org.firstinspires.ftc.teamcode.utils.Robot; -import org.firstinspires.ftc.teamcode.utils.Servos; -import org.firstinspires.ftc.teamcode.utils.Spindexer; -import org.firstinspires.ftc.teamcode.utils.Targeting; -import org.firstinspires.ftc.teamcode.utils.Turret; - -@Config -@Autonomous(preselectTeleOp = "TeleopV3") -public class Auto_LT_Far_3Ball extends LinearOpMode { - public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset; - public static double autoSpinStartPos = 0.2; - public static double shoot0SpinSpeedIncrease = 0.015; - - public static double redObeliskTurrPos1 = turrDefault + 0.12; - public static double redObeliskTurrPos2 = turrDefault + 0.13; - public static double redObeliskTurrPos3 = turrDefault + 0.14; - - public static double blueObeliskTurrPos1 = turrDefault - 0.12; - public static double blueObeliskTurrPos2 = turrDefault - 0.13; - public static double blueObeliskTurrPos3 = turrDefault - 0.14; - - public static double shoot0XTolerance = 1.0; - public static double redTurretShootPos = turrDefault; - public static double blueTurretShootPos = turrDefault; - public static int fwdTime = 200, strafeTime = 2300; - public static double rPickupGateX = 1, rPickupGateY = 1, rPickupGateH = 1; - public static double rPickupZoneX = 1, rPickupZoneY = 1, rPickupZoneH = 1; - public static double rShootX = 1, rShootY = 1, rShootH = 1; - public static double bPickupZoneX = 1, bPickupZoneY = 1, bPickupZoneH = 1; - public static double bPickupGateX = 1, bPickupGateY = 1, bPickupGateH = 1; - public static double bShootX = 1, bShootY = 1, bShootH = 1; - public static int sleepTime = 1300; - public int motif = 0; - double turretShootPos = 0.0; - Robot robot; - MultipleTelemetry TELE; - MecanumDrive drive; - Servos servos; - Spindexer spindexer; - Flywheel flywheel; - Turret turret; - Targeting targeting; - Targeting.Settings targetingSettings; - private double firstSpindexShootPos = autoSpinStartPos; - private boolean shootForward = true; - private double xShoot, yShoot, hShoot; - private int driverSlotGreen = 0; - private int passengerSlotGreen = 0; - private int rearSlotGreen = 0; - private int mostGreenSlot = 0; - private double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; - private double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0; - - public Action shootAll(int vel, double shootTime, double spindexSpeed) { - return new Action() { - int ticker = 1; - - double stamp = 0.0; - - double velo = vel; - - int shooterTicker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - flywheel.manageFlywheel(vel); - velo = flywheel.getVelo(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.intake.setPower(-0.3); - - if (ticker == 1) { - stamp = getRuntime(); - } - ticker++; - - robot.intake.setPower(0); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - if (getRuntime() - stamp < shootTime) { - - if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); - } else { - robot.transferServo.setPosition(transferServo_in); - shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); - } - - return true; - - } else { - robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; - - spindexer.resetSpindexer(); - spindexer.processIntake(); - - return false; - - } - - } - }; - } - - public Action intake(double intakeTime) { - return new Action() { - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - ticker++; - - spindexer.processIntake(); - robot.intake.setPower(1); - - spindexer.ballCounterLight(); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); - - } - }; - } - - public Action manageFlywheel( - double vel, - double hoodPos, - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance - ) { - - boolean timeFallback = (time != 0.501); - boolean posXFallback = (posX != 0.501); - boolean posYFallback = (posY != 0.501); - - return new Action() { - - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); - - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - - ticker++; - - flywheel.manageFlywheel(vel); - robot.hood.setPosition(hoodPos); - - boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; - boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; - boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; - - boolean shouldFinish = timeDone || xDone || yDone; - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - return !shouldFinish; - - } - }; - } - - @Override - public void runOpMode() throws InterruptedException { - - robot = new Robot(hardwareMap); - - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - flywheel = new Flywheel(hardwareMap); - - targeting = new Targeting(); - targetingSettings = new Targeting.Settings(0.0, 0.0); - - spindexer = new Spindexer(hardwareMap); - - servos = new Servos(hardwareMap); - - robot.limelight.start(); - - robot.limelight.pipelineSwitch(1); - - turret = new Turret(robot, TELE, robot.limelight); - - turret.manualSetTurret(turrDefault); - - drive = new MecanumDrive(hardwareMap, teleEnd); - - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); - - robot.transferServo.setPosition(transferServo_out); - - robot.light.setPosition(1); - - while (opModeInInit()) { - - robot.hood.setPosition(shoot0Hood); - turret.manualSetTurret(turretShootPos); - - if (gamepad2.crossWasPressed()) { - redAlliance = !redAlliance; - } - - if (gamepad2.dpadLeftWasPressed()) { - turrDefault -= 0.01; - } - - if (gamepad2.dpadRightWasPressed()) { - turrDefault += 0.01; - } - - redObeliskTurrPos1 = turrDefault + 0.12; - redObeliskTurrPos2 = turrDefault + 0.13; - redObeliskTurrPos3 = turrDefault + 0.14; - - blueObeliskTurrPos1 = turrDefault - 0.12; - blueObeliskTurrPos2 = turrDefault - 0.13; - blueObeliskTurrPos3 = turrDefault - 0.14; - - redTurretShootPos = turrDefault + 0.05; - - if (redAlliance) { - robot.light.setPosition(0.28); - - pickupGateX = rPickupGateX; - pickupGateY = rPickupGateY; - pickupGateH = rPickupGateH; - - pickupZoneX = rPickupZoneX; - pickupZoneY = rPickupZoneY; - pickupZoneH = rPickupZoneH; - - xShoot = rShootX; - yShoot = rShootY; - hShoot = rShootH; - - turretShootPos = redTurretShootPos; - - } else { - robot.light.setPosition(0.6); - - pickupGateX = bPickupGateX; - pickupGateY = bPickupGateY; - pickupGateH = bPickupGateH; - - pickupZoneX = bPickupZoneX; - pickupZoneY = bPickupZoneY; - pickupZoneH = bPickupZoneH; - - xShoot = bShootX; - yShoot = bShootY; - hShoot = bShootH; - - turretShootPos = blueTurretShootPos; - - } - - TELE.addData("Red?", redAlliance); - TELE.addData("Turret Default", turrDefault); - - TELE.update(); - } - - waitForStart(); - - if (isStopRequested()) return; - - if (opModeIsActive()) { - - robot.transfer.setPower(1); - - Actions.runBlocking( - new ParallelAction( - manageFlywheel( - shoot0Vel, - shoot0Hood, - 9, - 0.501, - 0.501, - shoot0XTolerance, - 0.501 - ) - - ) - ); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - shootAll((int) shoot0Vel, 6, shoot0SpinSpeedIncrease) - ); - - robot.frontLeft.setPower(-0.4); - robot.frontRight.setPower(-0.4); - robot.backLeft.setPower(-0.4); - robot.backRight.setPower(-0.4); - - sleep(fwdTime); - - robot.frontLeft.setPower(0); - robot.frontRight.setPower(0); - robot.backLeft.setPower(0); - robot.backRight.setPower(0); - - sleep(sleepTime); - - robot.frontLeft.setPower(-0.4); - robot.frontRight.setPower(0.4); - robot.backLeft.setPower(0.4); - robot.backRight.setPower(-0.4); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - sleep(strafeTime); - - robot.frontLeft.setPower(0); - robot.frontRight.setPower(0); - robot.backLeft.setPower(0); - robot.backRight.setPower(0); - - while (opModeIsActive()) { - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addLine("finished"); - TELE.update(); - } - - } - - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/Actions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/Actions.java new file mode 100644 index 0000000..b337c56 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/Actions.java @@ -0,0 +1,648 @@ +package org.firstinspires.ftc.teamcode.autonomous.actions; + +import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; +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_outtakeBall1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; +import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.Pose2d; + +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.utils.Drivetrain; +import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Servos; +import org.firstinspires.ftc.teamcode.utils.Spindexer; +import org.firstinspires.ftc.teamcode.utils.Targeting; +import org.firstinspires.ftc.teamcode.utils.Turret; + +import java.util.Objects; + +public class Actions{ + Robot robot; + MultipleTelemetry TELE; + Servos servos; + Flywheel flywheel; + MecanumDrive drive; + Spindexer spindexer; + Targeting targeting; + Targeting.Settings targetingSettings; + Turret turret; + private int driverSlotGreen = 0; + private int passengerSlotGreen = 0; + private int rearSlotGreen = 0; + private int mostGreenSlot = 0; + private double firstSpindexShootPos = spinStartPos; + private boolean shootForward = true; + public static double firstShootTime = 0.3; + public int motif = 0; + + public Actions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur){ + this.robot = rob; + this.drive = dri; + this.TELE = tel; + this.servos = ser; + this.flywheel = fly; + this.spindexer = spi; + this.targeting = tar; + this.targetingSettings = tS; + this.turret = tur; + } + public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { + return new Action() { + double stamp = 0.0; + int ticker = 0; + + double spindexerWiggle = 0.01; + + boolean decideGreenSlot = false; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + ticker++; + servos.setTransferPos(transferServo_out); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.addData("motif", motif_id); + TELE.update(); + + if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) { + + spindexerWiggle *= -1.0; + + servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle); + + spindexer.detectBalls(true, true); + + if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) { + driverSlotGreen++; + } + + if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) { + passengerSlotGreen++; + } + + if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) { + rearSlotGreen++; + } + + spindexer.setIntakePower(1); + + decideGreenSlot = true; + + return true; + } else if (decideGreenSlot) { + + if (driverSlotGreen >= passengerSlotGreen && driverSlotGreen >= rearSlotGreen) { + mostGreenSlot = 3; + } else if (passengerSlotGreen >= driverSlotGreen && passengerSlotGreen >= rearSlotGreen) { + mostGreenSlot = 2; + } else { + mostGreenSlot = 1; + } + + decideGreenSlot = false; + + if (motif_id == 21) { + if (mostGreenSlot == 1) { + firstSpindexShootPos = spindexer_outtakeBall1; + shootForward = true; + } else if (mostGreenSlot == 2) { + firstSpindexShootPos = spindexer_outtakeBall2; + shootForward = false; + } else { + firstSpindexShootPos = spindexer_outtakeBall3; + shootForward = false; + } + } else if (motif_id == 22) { + if (mostGreenSlot == 1) { + firstSpindexShootPos = spindexer_outtakeBall2; + shootForward = false; + } else if (mostGreenSlot == 2) { + firstSpindexShootPos = spindexer_outtakeBall3; + shootForward = false; + } else { + firstSpindexShootPos = spindexer_outtakeBall2; + shootForward = true; + } + + } else { + if (mostGreenSlot == 1) { + firstSpindexShootPos = spindexer_outtakeBall3; + shootForward = false; + } else if (mostGreenSlot == 2) { + firstSpindexShootPos = spindexer_outtakeBall3b; + shootForward = true; + } else { + firstSpindexShootPos = spindexer_outtakeBall1; + shootForward = true; + } + + } + + return true; + } else if ((System.currentTimeMillis() - stamp) < (time * 1000)) { +// TELE.addData("MostGreenSlot", mostGreenSlot); +// TELE.update(); + spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); + + servos.setSpinPos(firstSpindexShootPos); + + return true; + } else { + return false; + } + + } + }; + } + + public Action shootAll(int vel, double shootTime, double spindexSpeed) { + return new Action() { + int ticker = 1; + double stamp = 0.0; + double velo = vel; + int shooterTicker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); + flywheel.manageFlywheel(vel); + velo = flywheel.getVelo(); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + spindexer.setIntakePower(-0.3); + + if (ticker == 1) { + stamp = System.currentTimeMillis(); + } + ticker++; + + spindexer.setIntakePower(0); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + + if ((System.currentTimeMillis() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { + + if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) { + servos.setSpinPos(spinStartPos); + } else { + servos.setTransferPos(transferServo_in); + shooterTicker++; + double prevSpinPos = servos.getSpinCmdPos(); + servos.setSpinPos(prevSpinPos + spindexSpeed); + } + + return true; + + } else { + servos.setTransferPos(transferServo_out); + + spindexer.resetSpindexer(); + spindexer.processIntake(); + + return false; + + } + + } + }; + } + + public Action shootAllAuto(double shootTime, double spindexSpeed) { + return new Action() { + int ticker = 1; + + double stamp = 0.0; + + double velo = 0.0; + + int shooterTicker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + velo = flywheel.getVelo(); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + spindexer.setIntakePower(-0.3); + + if (ticker == 1) { + stamp = System.currentTimeMillis(); + } + ticker++; + + spindexer.setIntakePower(0); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + + if (System.currentTimeMillis() - stamp < shootTime) { + + if (System.currentTimeMillis() - stamp < firstShootTime) { + servos.setTransferPos(transferServo_out); + servos.setSpinPos(firstSpindexShootPos); + } else { + servos.setTransferPos(transferServo_in); + shooterTicker++; + double prevSpinPos = servos.getSpinCmdPos(); + + if (shootForward) { + servos.setSpinPos(prevSpinPos + spindexSpeed); + } else { + servos.setSpinPos(prevSpinPos - spindexSpeed); + } + } + + return true; + + } else { + servos.setTransferPos(transferServo_out); + + spindexer.resetSpindexer(); + spindexer.processIntake(); + + return false; + + } + } + }; + } + + public Action intake(double intakeTime) { + return new Action() { + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + ticker++; + + spindexer.processIntake(); + spindexer.setIntakePower(1); + + spindexer.ballCounterLight(); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + TELE.addData("Full?", spindexer.isFull()); + TELE.update(); + + return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); + } + }; + } + + public Action detectObelisk( + double time, + double posX, + double posY, + double posXTolerance, + double posYTolerance, + double turrPos + ) { + + boolean timeFallback = (time != 0.501); + boolean posXFallback = (posX != 0.501); + boolean posYFallback = (posY != 0.501); + + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + robot.limelight.pipelineSwitch(1); + } + + ticker++; + motif = turret.detectObelisk(); + + turret.setTurret(turrPos); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; + boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; + + boolean shouldFinish = timeDone || xDone || yDone; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + if (shouldFinish){ + if (redAlliance){ + robot.limelight.pipelineSwitch(4); + } else { + robot.limelight.pipelineSwitch(2); + } + return false; + } else { + return true; + } + + } + }; + } + + public Action manageFlywheel( + double vel, + double hoodPos, + double time, + double posX, + double posY, + double posXTolerance, + double posYTolerance + ) { + + boolean timeFallback = (time != 0.501); + boolean posXFallback = (posX != 0.501); + boolean posYFallback = (posY != 0.501); + + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + + ticker++; + + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); + flywheel.manageFlywheel(vel); + servos.setHoodPos(hoodPos); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; + boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; + + boolean shouldFinish = timeDone || xDone || yDone; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + return !shouldFinish; + + } + }; + } + + public Action manageShooterAuto( + double time, + double posX, + double posY, + double posXTolerance, + double posYTolerance + ) { + + boolean timeFallback = (time != 0.501); + boolean posXFallback = (posX != 0.501); + boolean posYFallback = (posY != 0.501); + + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + + ticker++; + + double robotX = drive.localizer.getPose().position.x; + double robotY = drive.localizer.getPose().position.y; + + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robotX - goalX; // delta x from robot to goal + double dy = robotY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robotX, robotY, robotHeading, 0.0, false); + + turret.trackGoal(deltaPose); + + servos.setHoodPos(targetingSettings.hoodAngle); + + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); + flywheel.manageFlywheel(targetingSettings.flywheelRPM); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; + boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; + + boolean shouldFinish = timeDone || xDone || yDone; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + return !shouldFinish; + + } + }; + } + + public Action manageFlywheelAuto( + double time, + double posX, + double posY, + double posXTolerance, + double posYTolerance + ) { + + boolean timeFallback = (time != 0.501); + boolean posXFallback = (posX != 0.501); + boolean posYFallback = (posY != 0.501); + + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + + ticker++; + + double robotX = drive.localizer.getPose().position.x; + double robotY = drive.localizer.getPose().position.y; + + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robotX - goalX; // delta x from robot to goal + double dy = robotY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robotX, robotY, robotHeading, 0.0, false); + + servos.setHoodPos(targetingSettings.hoodAngle); + + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); + flywheel.manageFlywheel(targetingSettings.flywheelRPM); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; + boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; + + boolean shouldFinish = timeDone || xDone || yDone; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + return !shouldFinish; + + } + }; + } +} + + 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/disabled/Auto_LT_Close_12Ball.java similarity index 87% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java index 56376fe..b4ca67a 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/disabled/Auto_LT_Close_12Ball.java @@ -1,61 +1,61 @@ -package org.firstinspires.ftc.teamcode.autonomous; +package org.firstinspires.ftc.teamcode.autonomous.disabled; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; @@ -76,7 +76,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.constants.Poses_V2; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; @@ -170,7 +169,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { robot.transferServo.setPosition(transferServo_out); - turret.manualSetTurret(turretShootPos); + turret.setTurret(turretShootPos); robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000); drive.updatePoseEstimate(); @@ -612,7 +611,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - turret.manualSetTurret(0.4); + turret.setTurret(0.4); drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java similarity index 89% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java index 268855c..c32dc38 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java @@ -1,61 +1,61 @@ -package org.firstinspires.ftc.teamcode.autonomous; +package org.firstinspires.ftc.teamcode.autonomous.disabled; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; @@ -78,6 +78,7 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.constants.Poses_V2; @@ -91,6 +92,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret; import java.util.Objects; +@Disabled @Config @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Close_GateOpen extends LinearOpMode { @@ -205,7 +207,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { ticker++; robot.transferServo.setPosition(transferServo_out); - turret.manualSetTurret(turretShootPos); + turret.setTurret(turretShootPos); drive.updatePoseEstimate(); @@ -750,7 +752,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - turret.manualSetTurret(0.4); + turret.setTurret(0.4); drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java index d2b5214..755b71f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java @@ -1,49 +1,49 @@ package org.firstinspires.ftc.teamcode.autonomous.disabled; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java new file mode 100644 index 0000000..c091ccd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -0,0 +1,23 @@ +package org.firstinspires.ftc.teamcode.constants; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.Pose2d; + +@Config +public class Back_Poses { + public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50.1; + public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50; + + public static double rShootX = 95, rShootY = 85, rShootH = 90; + public static double bShootX = 95, bShootY = -85, bShootH = -90; + + public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140; + public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140; + + public static double rStackPickupBX = 50, rStackPickupBY = 78, rStackPickupBH = 140.1; + public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1; + + public static Pose2d autoStart = new Pose2d(0, 0, 0); // TODO: find this position + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java new file mode 100644 index 0000000..63f7f6e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.constants; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.Pose2d; + +@Config +public class Front_Poses { + + + public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1; + public static double bx1 = 20, by1 = 0.5, bh1 = 0.1; + + public static double rx2a = 41, ry2a = 18, rh2a = 140; + public static double bx2a = 41, by2a = -18, bh2a = -140; + + public static double rx2b = 23, ry2b = 36, rh2b = 140.1; + public static double bx2b = 19, by2b = -40, bh2b = -140.1; + + public static double rx3a = 55, ry3a = 39, rh3a = 140; + public static double bx3a = 55, by3a = -39, bh3a = -140; + + public static double rx3aG = 60, ry3aG = 34, rh3aG = 140; + public static double bx3aG = 55, by3aG = -43, bh3aG = -140; + + public static double rx3b = 36, ry3b = 58, rh3b = 140.1; + public static double bx3b = 41, by3b = -59, bh3b = -140.1; + + public static double rx4a = 75, ry4a = 53, rh4a = 140; + public static double bx4a = 75, by4a = -53, bh4a = -140; + + public static double rx4b = 50, ry4b = 78, rh4b = 140.1; + public static double bx4b = 47, by4b = -85, bh4b = -140.1; + + public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; + + public static double rShootX = 40, rShootY = 10, rShootH = 50; + public static double bShootX = 40, bShootY = 0, bShootH = -50; + + public static double rxPrep = 45, ryPrep = 10, rhPrep = 50; + public static double bxPrep = 45, byPrep = -10, bhPrep = -50; + + public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50; + public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50; + + public static Pose2d teleStart = new Pose2d(0, 0, 0); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java deleted file mode 100644 index fbdeb9a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ /dev/null @@ -1,49 +0,0 @@ -package org.firstinspires.ftc.teamcode.constants; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.Pose2d; - -@Config -public class Poses { - - - public static double rx1 = 20, ry1 = 0.5, rh1 = Math.toRadians(0.1); - public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140); - public static double rx2b = 19, ry2b = 40, rh2b = Math.toRadians(140.1); - - - public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140); - - public static double rx3aG = 60, ry3aG = 34, rh3aG = Math.toRadians(140); - - public static double rx3b = 36, ry3b = 58, rh3b = Math.toRadians(140.1); - - public static double rx4a = 75, ry4a = 53, rh4a = Math.toRadians(140); - public static double rx4b = 45, ry4b = 83, rh4b = Math.toRadians(140.1); - - public static double bx1 = 20, by1 = 0.5, bh1 = Math.toRadians(0.1); - public static double bx2a = 41, by2a = -18, bh2a = Math.toRadians(-140); - public static double bx2b = 19, by2b = -40, bh2b = Math.toRadians(-140.1); - - public static double bx3a = 55, by3a = -39, bh3a = Math.toRadians(-140); - public static double bx3b = 41, by3b = -59, bh3b = Math.toRadians(-140.1); - public static double bx3aG = 55, by3aG = -43, bh3aG = Math.toRadians(-140); - - - public static double bx4a = 75, by4a = -53, bh4a = Math.toRadians(-140); - public static double bx4b = 47, by4b = -85, bh4b = Math.toRadians(-140.1); - public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this - - public static double rShootX = 40, rShootY = -7, rShootH = Math.toRadians(50); - public static double rxPrep = 45, ryPrep = 10, rhPrep = Math.toRadians(50); - - public static double bShootX = 40, bShootY = 7, bShootH = Math.toRadians(-50); - public static double bxPrep = 45, byPrep = -10, bhPrep = Math.toRadians(-50); - - - public static Pose2d teleStart = new Pose2d(0, 0, 0); - - public static Pose2d teleEnd = new Pose2d(0, 0, 0); - - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java index 53fabef..cd02540 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java @@ -1,7 +1,9 @@ package org.firstinspires.ftc.teamcode.constants; import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +@Disabled @Config public class Poses_V2 { 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 0acbe4e..225a0fb 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 @@ -32,4 +32,14 @@ public class ServoPositions { public static double turret_redClose = 0.42; public static double turret_blueClose = 0.38; + // These values are ADDED to turrDefault + public static double redObeliskTurrPos1 = 0.12; + public static double redObeliskTurrPos2 = 0.13; + public static double redObeliskTurrPos3 = 0.14; + public static double blueObeliskTurrPos1 = -0.12; + public static double blueObeliskTurrPos2 = -0.13; + public static double blueObeliskTurrPos3 = -0.14; + public static double redTurretShootPos = 0.1; + public static double blueTurretShootPos = -0.14; + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java index bd096b9..a3f9347 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java @@ -1,7 +1,9 @@ package org.firstinspires.ftc.teamcode.constants; import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +@Disabled @Config public class ShooterVars { 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 dc86c83..932fd11 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 @@ -1,10 +1,10 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; +import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import com.acmerobotics.dashboard.FtcDashboard; @@ -22,6 +22,7 @@ import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Drivetrain; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Light; +import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Spindexer; @@ -33,8 +34,7 @@ import java.util.List; @Config @TeleOp public class TeleopV3 extends LinearOpMode { - public static double manualVel = 1000; - //Testing a new comment + public static double manualVel = 3000; public static double hoodDefaultPos = 0.5; public static double spinPow = 0.09; @@ -59,6 +59,7 @@ public class TeleopV3 extends LinearOpMode { Targeting targeting; Targeting.Settings targetingSettings; Drivetrain drivetrain; + MeasuringLoopTimes loopTimes; double autoHoodOffset = 0.0; int shooterTicker = 0; boolean intake = false; @@ -74,8 +75,6 @@ public class TeleopV3 extends LinearOpMode { boolean overrideTurr = false; int intakeTicker = 0; - - boolean turretInterpolate = false; private boolean shootAll = false; @Override @@ -98,6 +97,9 @@ public class TeleopV3 extends LinearOpMode { drivetrain = new Drivetrain(robot, drive); + loopTimes = new MeasuringLoopTimes(); + loopTimes.init(); + PIDFController tController = new PIDFController(tp, ti, td, tf); tController.setTolerance(0.001); @@ -108,6 +110,9 @@ public class TeleopV3 extends LinearOpMode { light.init(robot.light, spindexer, turret); light.setState(StateEnums.LightState.MANUAL); + limelightUsed = true; + + robot.light.setPosition(1); while (opModeInInit()) { robot.limelight.start(); if (redAlliance) { @@ -127,7 +132,8 @@ public class TeleopV3 extends LinearOpMode { waitForStart(); if (isStopRequested()) return; - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); + robot.transfer.setPower(1); while (opModeIsActive()) { @@ -145,7 +151,7 @@ public class TeleopV3 extends LinearOpMode { if (gamepad1.right_bumper) { shootAll = false; - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); light.setState(StateEnums.LightState.BALL_COUNT); @@ -156,9 +162,7 @@ public class TeleopV3 extends LinearOpMode { light.setState(StateEnums.LightState.GOAL_LOCK); } - robot.transfer.setPower(1); - - double offset; + //TURRET TRACKING double robX = drive.localizer.getPose().position.x; double robY = drive.localizer.getPose().position.y; @@ -205,14 +209,16 @@ public class TeleopV3 extends LinearOpMode { } //SHOOTER: + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); //HOOD: if (targetingHood) { - robot.hood.setPosition(targetingSettings.hoodAngle + autoHoodOffset); + servo.setHoodPos(targetingSettings.hoodAngle + autoHoodOffset); } else { - robot.hood.setPosition(hoodDefaultPos); + servo.setHoodPos(hoodDefaultPos); } if (gamepad2.dpadUpWasPressed()) { @@ -244,6 +250,8 @@ public class TeleopV3 extends LinearOpMode { drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); } + + if (enableSpindexerManager) { //if (!shootAll) { spindexer.processIntake(); @@ -251,13 +259,12 @@ public class TeleopV3 extends LinearOpMode { // RIGHT_BUMPER if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) { - robot.intake.setPower(1); + spindexer.setIntakePower(1); } else if (gamepad1.cross) { - robot.intake.setPower(-1); + spindexer.setIntakePower(-1); } else { - robot.intake.setPower(0); - + spindexer.setIntakePower(0); } // LEFT_BUMPER @@ -278,14 +285,14 @@ public class TeleopV3 extends LinearOpMode { spindexer.prepareShootAllContinous(); //TELE.addLine("preparing to shoot"); // } else if (shooterTicker == 2) { -// //robot.transferServo.setPosition(transferServo_in); +// //servo.setTransferPos(transferServo_in); // spindexer.shootAll(); // TELE.addLine("starting to shoot"); } else if (!spindexer.shootAllComplete()) { - robot.transferServo.setPosition(transferServo_in); + servo.setTransferPos(transferServo_in); //TELE.addLine("shoot"); } else { - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); //spindexPos = spindexer_intakePos1; shootAll = false; spindexer.resetSpindexer(); @@ -297,28 +304,20 @@ public class TeleopV3 extends LinearOpMode { } if (gamepad1.left_stick_button) { - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); //spindexPos = spindexer_intakePos1; - shootAll = false; spindexer.resetSpindexer(); } } - if (gamepad2.square) { - drive.updatePoseEstimate(); - - teleEnd = drive.localizer.getPose(); - gamepad2.rumble(1000); - - } - //EXTRA STUFFINESS: drive.updatePoseEstimate(); for (LynxModule hub : allHubs) { hub.clearBulkCache(); } + loopTimes.loop(); // // TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); // TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); @@ -342,7 +341,7 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("spinTestCounter", spindexer.counter); // TELE.addData("autoSpintake", autoSpintake); // - TELE.addData("shootall commanded", shootAll); +// TELE.addData("shootall commanded", shootAll); // Targeting Debug TELE.addData("robotX", robotX); TELE.addData("robotY", robotY); @@ -354,6 +353,10 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); TELE.addData("timeSinceStamp", getRuntime() - shootStamp); + TELE.addData("Voltage", voltage); // returns alleged recorded voltage (not same as driver hub) + TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); + TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); + TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java index 6cbbbc4..3880604 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java @@ -60,7 +60,7 @@ public class LimelightTest extends LinearOpMode { if (turretMode){ if (turretPos != 0.501){ - turret.manualSetTurret(turretPos); + turret.setTurret(turretPos); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index d45ffc0..dcb2d73 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -1,23 +1,30 @@ package org.firstinspires.ftc.teamcode.tests; +import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; 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_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spinSpeedIncrease; +import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotorEx; +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Spindexer; +import org.firstinspires.ftc.teamcode.utils.Targeting; +import org.firstinspires.ftc.teamcode.utils.Turret; @Config @TeleOp @@ -26,22 +33,23 @@ public class ShooterTest extends LinearOpMode { public static double parameter = 0.0; // --- CONSTANTS YOU TUNE --- - //TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED public static double Velocity = 0.0; public static double P = 255.0; public static double I = 0.0; public static double D = 0.0; - public static double F = 7.5; + public static double F = 90; public static double transferPower = 1.0; public static double hoodPos = 0.501; public static double turretPos = 0.501; public static boolean shoot = false; public static boolean intake = false; + public static boolean turretTrack = true; Robot robot; Flywheel flywheel; Servos servo; - + MecanumDrive drive; + Turret turret; double shootStamp = 0.0; boolean shootAll = false; @@ -51,6 +59,7 @@ public class ShooterTest extends LinearOpMode { public double hoodAdjust = 0.0; public static double hoodAdjustFactor = 1.0; private int shooterTicker = 0; + public static double spinSpeed = 0.02; Spindexer spindexer ; @Override @@ -62,22 +71,58 @@ public class ShooterTest extends LinearOpMode { flywheel = new Flywheel(hardwareMap); spindexer = new Spindexer(hardwareMap); servo = new Servos(hardwareMap); + drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); MultipleTelemetry TELE = new MultipleTelemetry( telemetry, FtcDashboard.getInstance().getTelemetry() ); + turret = new Turret(robot, TELE, robot.limelight); + Turret.limelightUsed = true; waitForStart(); + robot.limelight.start(); + if (isStopRequested()) return; while (opModeIsActive()) { + if (redAlliance){ + robot.limelight.pipelineSwitch(4); + } else { + robot.limelight.pipelineSwitch(2); + } + + //TURRET TRACKING + drive.updatePoseEstimate(); + + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + if (turretTrack){ + turret.trackGoal(deltaPose); + } else if (turretPos != 0.501){ + turret.setTurret(turretPos); + } + + double voltage = robot.voltage.getVoltage(); + if (mode == 0) { rightShooter.setPower(parameter); leftShooter.setPower(parameter); } else if (mode == 1) { - flywheel.setPIDF(P, I, D, F); + flywheel.setPIDF(P, I, D, F / voltage); flywheel.manageFlywheel((int) Velocity); } @@ -109,7 +154,6 @@ public class ShooterTest extends LinearOpMode { //intake = false; //reject = false; - // TODO: Change starting position based on desired order to shoot green ball //spindexPos = spindexer_intakePos1; if (getRuntime() - shootStamp < 3.5) { @@ -120,8 +164,11 @@ public class ShooterTest extends LinearOpMode { robot.transferServo.setPosition(transferServo_in); shooterTicker++; double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); - robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); + + if (prevSpinPos < 0.9){ + robot.spin1.setPosition(prevSpinPos + spinSpeed); + robot.spin2.setPosition(1 - prevSpinPos - spinSpeed); + } } @@ -150,6 +197,7 @@ public class ShooterTest extends LinearOpMode { TELE.addData("Power", robot.shooter1.getPower()); TELE.addData("Steady?", flywheel.getSteady()); TELE.addData("Position", robot.shooter1.getCurrentPosition()); + TELE.addData("Voltage", voltage); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java index b03db69..b5e017f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java @@ -12,6 +12,7 @@ public class Flywheel { public double velo1 = 0.0; public double velo2 = 0.0; double targetVelocity = 0.0; + double previousTargetVelocity = 0.0; double powPID = 0.0; boolean steady = false; public Flywheel (HardwareMap hardwareMap) { @@ -37,6 +38,8 @@ public class Flywheel { } // Set the robot PIDF for the next cycle. + private double prevF = 0.501; + public static int voltagePIDFDifference = 5; public void setPIDF(double p, double i, double d, double f) { shooterPIDF1.p = p; shooterPIDF1.i = i; @@ -46,6 +49,10 @@ public class Flywheel { shooterPIDF2.i = i; shooterPIDF2.d = d; shooterPIDF2.f = f; + if (Math.abs(prevF - f) > voltagePIDFDifference){ + robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); + robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); + } } // Convert from RPM to Ticks per Second @@ -55,20 +62,21 @@ public class Flywheel { private double TPS_to_RPM (double TPS) { return (TPS*60.0)/28.0;} public double manageFlywheel(double commandedVelocity) { - targetVelocity = commandedVelocity; - // Add code here to set PIDF based on desired RPM + if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) { + targetVelocity = commandedVelocity; + // Add code here to set PIDF based on desired RPM + //robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); + //robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); + robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); + robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); - robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); - robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); - robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); - robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); - - // Record Current Velocity - velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); - velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); - velo = Math.max(velo1,velo2); + // Record Current Velocity + velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); + velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); + velo = Math.max(velo1, velo2); + } // really should be a running average of the last 5 steady = (Math.abs(targetVelocity - velo) < 200.0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java new file mode 100644 index 0000000..d7d0d1e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java @@ -0,0 +1,66 @@ +package org.firstinspires.ftc.teamcode.utils; + +import com.qualcomm.robotcore.util.ElapsedTime; + +public class MeasuringLoopTimes { + private ElapsedTime elapsedtime; + private double minLoopTime = 999999999999.0; + + private double maxLoopTime = 0.0; + private double mainLoopTime = 0.0; + + private double MeasurementStart = 0.0; + private double currentTime = 0.0; + + private double avgLoopTime = 0.0; + private int avgLoopTimeTicker = 0; + private double avgLoopTimeSum = 0; + + private double getTimeSeconds () + { + return (double) System.currentTimeMillis()/1000.0; + } + + public void init() { + elapsedtime = new ElapsedTime(); + elapsedtime.reset(); + + MeasurementStart = getTimeSeconds(); + } + + + public double getAvgLoopTime() { + return avgLoopTime; + } + + public double getMaxLoopTimeOneMin() { + return maxLoopTime; + } + + public double getMinLoopTimeOneMin() { + return minLoopTime; + } + + public void loop() { + currentTime = getTimeSeconds(); + if ((MeasurementStart + 15.0) < currentTime) + { + minLoopTime = 9999999.0; + maxLoopTime = 0.0; + MeasurementStart = currentTime; + + avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker; + avgLoopTimeSum = 0.0; + avgLoopTimeTicker = 0; + } + + mainLoopTime = elapsedtime.milliseconds(); + elapsedtime.reset(); + + avgLoopTimeSum += mainLoopTime; + avgLoopTimeTicker++; + minLoopTime = Math.min(minLoopTime,mainLoopTime); + maxLoopTime = Math.max(maxLoopTime,mainLoopTime); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java index 18e229a..92e8500 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java @@ -35,7 +35,7 @@ public class PositionalServoProgrammer extends LinearOpMode { waitForStart(); if (isStopRequested()) return; while (opModeIsActive()){ - if (spindexPos != 0.501 && !servo.spinEqual(spindexPos)){ + if (spindexPos != 0.501){ robot.spin1.setPosition(spindexPos); robot.spin2.setPosition(1-spindexPos); } @@ -52,16 +52,6 @@ public class PositionalServoProgrammer extends LinearOpMode { if (light !=0.501){ robot.light.setPosition(light); } - // To check configuration of spindexer: - // Set "mode" to 1 and spindexPow to 0.1 - // If the spindexer is turning clockwise, the servos are reversed. Swap the configuration of the two servos, DO NOT TOUCH THE ACTUAL CODE - // Do the previous test again to confirm - // Set "mode" to 0 but keep spindexPos at 0.501 - // Manually turn the spindexer until "spindexer pos" is set close to 0 - // Check each spindexer voltage: - // If "spindexer voltage 1" is closer to 0 than "spindexer voltage 2," then you are done! - // If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE - //TODO: @KeshavAnandCode do the above please TELE.addData("spindexer pos", servo.getSpinPos()); TELE.addData("turret pos", robot.turr1.getPosition()); 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 7b07256..4ce0461 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 @@ -12,6 +12,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; @@ -33,7 +34,7 @@ public class Robot { public double shooterPIDF_P = 255.0; public double shooterPIDF_I = 0.0; public double shooterPIDF_D = 0.0; - public double shooterPIDF_F = 7.5; + public double shooterPIDF_F = 90; public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001}; public DcMotorEx shooter1; public DcMotorEx shooter2; @@ -56,11 +57,11 @@ public class Robot { public RevColorSensorV3 color3; public Limelight3A limelight; public Servo light; + public VoltageSensor voltage; public Robot(HardwareMap hardwareMap) { //Define components w/ hardware map - //TODO: fix the configuration of these - I trust you to figure it out yourself @KeshavAnandCode frontLeft = hardwareMap.get(DcMotorEx.class, "fl"); frontRight = hardwareMap.get(DcMotorEx.class, "fr"); backLeft = hardwareMap.get(DcMotorEx.class, "bl"); @@ -78,7 +79,7 @@ public class Robot { shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1"); shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); - //TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode + shooter1.setDirection(DcMotorSimple.Direction.REVERSE); shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F); shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); @@ -96,7 +97,6 @@ public class Robot { turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port - //TODO: check spindexer configuration (both servo and analog input) - check comments in PositionalServoProgrammer spin1 = hardwareMap.get(Servo.class, "spin2"); spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos"); @@ -128,5 +128,6 @@ public class Robot { } light = hardwareMap.get(Servo.class, "light"); + voltage = hardwareMap.voltageSensor.iterator().next(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java index 324a36f..c66ff79 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java @@ -18,6 +18,15 @@ public class Servos { PIDFController spinPID; PIDFController turretPID; + private double prevSpinPos = 0.0; + private boolean firstSpinPos = true; + + private double prevTransferPos = 0.0; + private boolean firstTransferPos = true; + + private double prevHoodPos = 0.0; + private boolean firstHoodPos = true; + public Servos(HardwareMap hardwareMap) { robot = new Robot(hardwareMap); spinPID = new PIDFController(spinP, spinI, spinD, spinF); @@ -32,8 +41,42 @@ public class Servos { return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3); } - public double setSpinPos(double pos) { + public double getSpinCmdPos() { + return prevSpinPos; + } + public static boolean servoPosEqual(double pos1, double pos2) { + return (Math.abs(pos1 - pos2) < 0.005); + } + + public double setTransferPos(double pos) { + if (firstTransferPos || !servoPosEqual(pos, prevTransferPos)) { + robot.transferServo.setPosition(pos); + firstTransferPos = false; + } + + prevTransferPos = pos; + return pos; + } + + public double setSpinPos(double pos) { + if (firstSpinPos || !servoPosEqual(pos, prevSpinPos)) { + robot.spin1.setPosition(pos); + robot.spin2.setPosition(1-pos); + firstSpinPos = false; + } + + prevSpinPos = pos; + return pos; + } + + public double setHoodPos(double pos){ + if (firstHoodPos || !servoPosEqual(pos, prevHoodPos)) { + robot.hood.setPosition(pos); + firstHoodPos = false; + } + + prevHoodPos = pos; return pos; } 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 30ea571..f395b14 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 @@ -1,5 +1,12 @@ package org.firstinspires.ftc.teamcode.utils; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.arcrobotics.ftclib.controller.PIDFController; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; + +import static org.firstinspires.ftc.teamcode.constants.Color.*; import static org.firstinspires.ftc.teamcode.constants.Color.Light0; import static org.firstinspires.ftc.teamcode.constants.Color.Light1; import static org.firstinspires.ftc.teamcode.constants.Color.Light2; @@ -44,6 +51,42 @@ public class Spindexer { public double spindexerWiggle = 0.01; public double spindexerOuttakeWiggle = 0.01; public double spindexerPosOffset = 0.00; + public static int shootWaitMax = 4; + public Types.Motif desiredMotif = Types.Motif.NONE; + // For Use + enum RotatedBallPositionNames { + REARCENTER, + FRONTDRIVER, + FRONTPASSENGER + } + // Array of commandedIntakePositions with contents + // {RearCenter, FrontDriver, FrontPassenger} + static final int[][] RotatedBallPositions = {{0,2,1}, {1,0,2}, {2,1,0}}; + class spindexerBallRoatation { + int rearCenter = 0; // aka commanded Position + int frontDriver = 0; + int frontPassenger = 0; + } + + enum IntakeState { + UNKNOWN_START, + UNKNOWN_MOVE, + UNKNOWN_DETECT, + INTAKE, + FINDNEXT, + MOVING, + FULL, + SHOOTNEXT, + SHOOTMOVING, + SHOOTWAIT, + SHOOT_ALL_PREP, + SHOOT_ALL_READY, + SHOOT_PREP_CONTINOUS, + SHOOT_CONTINOUS + } + + int shootWaitCount = 0; + public StateEnums.Motif desiredMotif = StateEnums.Motif.NONE; public IntakeState currentIntakeState = IntakeState.UNKNOWN_START; public IntakeState prevIntakeState = IntakeState.UNKNOWN_START; @@ -126,11 +169,9 @@ public class Spindexer { if (detectRearColor) { // Detect which color - double green = robot.color1.getNormalizedColors().green; - double red = robot.color1.getNormalizedColors().red; - double blue = robot.color1.getNormalizedColors().blue; + NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors(); - double gP = green / (green + red + blue); + double gP = color1RGBA.green / (color1RGBA.green + color1RGBA.red + color1RGBA.blue); // FIXIT - Add filtering to improve accuracy. if (gP >= 0.38) { @@ -147,11 +188,9 @@ public class Spindexer { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; if (detectFrontColor) { - double green = robot.color2.getNormalizedColors().green; - double red = robot.color2.getNormalizedColors().red; - double blue = robot.color2.getNormalizedColors().blue; + NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors(); - double gP = green / (green + red + blue); + double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue); if (gP >= 0.4) { ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green @@ -176,11 +215,9 @@ public class Spindexer { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; if (detectFrontColor) { - double green = robot.color3.getNormalizedColors().green; - double red = robot.color3.getNormalizedColors().red; - double blue = robot.color3.getNormalizedColors().blue; + NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors(); - double gP = green / (green + red + blue); + double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue); if (gP >= 0.42) { ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green @@ -212,8 +249,7 @@ public class Spindexer { // Has code to unjam spindexer private void moveSpindexerToPos(double pos) { - robot.spin1.setPosition(pos); - robot.spin2.setPosition(1 - pos); + servos.setSpinPos(pos); // double currentPos = servos.getSpinPos(); // if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ // if (currentPos > pos){ @@ -224,7 +260,7 @@ public class Spindexer { // robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05); // } // } -// prevPos = currentPos; +// prevPos = pos; } // private double getTimeSeconds () @@ -246,6 +282,8 @@ public class Spindexer { } + private double prevLight = 0.0; + public void ballCounterLight(){ public double getRearCenterLight() { BallColor color = GetRearCenterColor(); if (Objects.equals(color, BallColor.GREEN)) { @@ -291,6 +329,14 @@ public class Spindexer { counter++; } + double light; + if (counter == 3){ + light = Light3; + } else if (counter == 2){ + light = Light2; + } else if (counter == 1){ + light = Light1; + if (counter == 3) { return Light3; } else if (counter == 2) { @@ -298,8 +344,13 @@ public class Spindexer { } else if (counter == 1) { return Light1; } else { + light = Light0; return Light0; } + if (light != prevLight){ + robot.light.setPosition(light); + } + prevLight = light; } public boolean slotIsEmpty(int slot) { @@ -316,7 +367,7 @@ public class Spindexer { case UNKNOWN_START: // For now just set position ONE if UNKNOWN commandedIntakePosition = 0; - moveSpindexerToPos(intakePositions[0]); + servos.setSpinPos(intakePositions[0]); currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE; break; case UNKNOWN_MOVE: @@ -327,7 +378,7 @@ public class Spindexer { unknownColorDetect = 0; } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); } break; case UNKNOWN_DETECT: @@ -346,7 +397,7 @@ public class Spindexer { } else { // Maintain Position spindexerWiggle *= -1.0; - moveSpindexerToPos(intakePositions[commandedIntakePosition] + spindexerWiggle); + servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle); } break; case FINDNEXT: @@ -378,7 +429,7 @@ public class Spindexer { //commandedIntakePosition = bestFitMotif(); currentIntakeState = Spindexer.IntakeState.FULL; } - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); break; case MOVING: @@ -394,7 +445,7 @@ public class Spindexer { //detectBalls(false, false); } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); } break; @@ -407,7 +458,7 @@ public class Spindexer { } // Maintain Position spindexerWiggle *= -1.0; - moveSpindexerToPos(intakePositions[commandedIntakePosition] + spindexerWiggle); + servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle); break; case SHOOT_ALL_PREP: @@ -416,7 +467,7 @@ public class Spindexer { commandedIntakePosition = 0; if (!servos.spinEqual(outakePositions[commandedIntakePosition])) { // Keep moving the spindexer - moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + servos.setSpinPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" } break; @@ -428,7 +479,7 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; } // Maintain Position - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); break; case SHOOTNEXT: @@ -449,7 +500,7 @@ public class Spindexer { // Empty return to intake state currentIntakeState = IntakeState.FINDNEXT; } - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); break; case SHOOTMOVING: @@ -458,12 +509,11 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; } else { // Keep moving the spindexer - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); } break; case SHOOTWAIT: - double shootWaitMax = 4; // Stopping when we get to the new position if (prevIntakeState != currentIntakeState) { if (commandedIntakePosition == 2) { @@ -483,14 +533,14 @@ public class Spindexer { } // Keep moving the spindexer spindexerOuttakeWiggle *= -1.0; - moveSpindexerToPos(outakePositions[commandedIntakePosition] + spindexerOuttakeWiggle); + servos.setSpinPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle); break; case SHOOT_PREP_CONTINOUS: if (servos.spinEqual(spinStartPos)) { currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; } else { - moveSpindexerToPos(spinStartPos); + servos.setSpinPos(spinStartPos); } break; @@ -501,11 +551,11 @@ public class Spindexer { if (servos.getSpinPos() > spinEndPos) { currentIntakeState = IntakeState.FINDNEXT; } else { - double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease; - if (spinPos > spinEndPos + 0.03) { + double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease; + if (spinPos > spinEndPos + 0.03){ spinPos = spinEndPos + 0.03; } - moveSpindexerToPos(spinPos); + servos.setSpinPos(spinPos); } break; @@ -657,4 +707,11 @@ public class Spindexer { int foundEmpty = 0; BallColor ballColor = BallColor.UNKNOWN; } + private double prevPow = 0.501; + public void setIntakePower(double pow){ + if (prevPow != 0.501 && prevPow != pow){ + robot.intake.setPower(pow); + } + prevPow = pow; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index 6b32de0..0ca44b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -77,6 +77,7 @@ public class Targeting { double cancelOffsetY = 0.0; // was 7.0 double unitConversionFactor = 0.95; int tileSize = 24; //inches + public static boolean turretInterpolate = false; public Targeting() { } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index 157ace4..b1f0d8b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -10,6 +10,7 @@ import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.teamcode.constants.Color; @@ -37,7 +38,8 @@ public class Turret { // TODO: tune these values for limelight public static double clampTolerance = 0.03; - public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125; + //public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125; + public static double B_PID_P = 0.066, B_PID_I = 0.0, B_PID_D = 0.007; Robot robot; MultipleTelemetry TELE; Limelight3A webcam; @@ -56,6 +58,9 @@ public class Turret { private double permanentOffset = 0.0; private PIDController bearingPID; + private double prevTurretPos = 0.0; + private boolean firstTurretPos = true; + public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) { this.TELE = tele; this.robot = rob; @@ -76,10 +81,13 @@ public class Turret { return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault; } - - public void manualSetTurret(double pos) { - robot.turr1.setPosition(pos); - robot.turr2.setPosition(1 - pos); + private double prevTurrPos = 0.501; + public void setTurret(double pos) { + if (prevTurrPos != 0.501 && prevTurrPos != pos){ + robot.turr1.setPosition(pos); + robot.turr2.setPosition(1-pos); + } + prevTurrPos = pos; } public boolean turretEqual(double pos) { @@ -284,8 +292,7 @@ public class Turret { } // Set servo positions - robot.turr1.setPosition(turretPos + manualOffset); - robot.turr2.setPosition(1.0 - turretPos - manualOffset); + setTurret(turretPos + manualOffset); /* ---------------- TELEMETRY ---------------- */