diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java index 187896f..2c835ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java @@ -10,6 +10,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -24,6 +25,7 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import java.util.ArrayList; import java.util.List; +@Disabled @TeleOp @Config public class TeleopV2 extends LinearOpMode { 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 a2cdc2d..0fd9dfa 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 @@ -2,38 +2,21 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.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_intakePos2; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos3; -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.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; -import static org.firstinspires.ftc.teamcode.utils.Servos.spinD; -import static org.firstinspires.ftc.teamcode.utils.Servos.spinF; -import static org.firstinspires.ftc.teamcode.utils.Servos.spinI; -import static org.firstinspires.ftc.teamcode.utils.Servos.spinP; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.ProfileAccelConstraint; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.TranslationalVelConstraint; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.Actions; import com.arcrobotics.ftclib.controller.PIDFController; -import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.teamcode.constants.ServoPositions; 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; @@ -41,40 +24,27 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Targeting; import org.firstinspires.ftc.teamcode.utils.Turret; -import java.util.ArrayList; import java.util.List; @Config @TeleOp public class TeleopV3 extends LinearOpMode { public static double manualVel = 3000; - public static int intakeJamSwap = 12; public static double hoodDefaultPos = 0.5; - public static double desiredTurretAngle = 180; - public static double shootStamp2 = 0.0; - public static double spinningPow = 0.2; - public static double spindexPos = spindexer_intakePos1; + public static double spinPow = 0.09; - public static double bMult = 1, bDiv = 2200; public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0; - public static boolean manualTurret = true; + public static double spinSpeedIncrease = 0.03; + public static int resetSpinTicks = 4; public double vel = 3000; public boolean autoVel = true; - public boolean targetingVel = true; public boolean targetingHood = true; public double manualOffset = 0.0; public boolean autoHood = true; - public boolean green1 = false; - public boolean green2 = false; - public boolean green3 = false; + public double shootStamp = 0.0; - public boolean circle = false; - public boolean square = false; - public boolean triangle = false; - public TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200); - public ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200); + boolean fixedTurret = false; - PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF); Robot robot; MultipleTelemetry TELE; Servos servo; @@ -83,8 +53,8 @@ public class TeleopV3 extends LinearOpMode { Spindexer spindexer; Targeting targeting; Targeting.Settings targetingSettings; + Drivetrain drivetrain; double autoHoodOffset = 0.0; - int shooterTicker = 0; boolean intake = false; boolean reject = false; @@ -92,59 +62,18 @@ public class TeleopV3 extends LinearOpMode { double yOffset = 0.0; double headingOffset = 0.0; int ticker = 0; - int camTicker = 0; - List s1G = new ArrayList<>(); - List s2G = new ArrayList<>(); - List s3G = new ArrayList<>(); - List s1T = new ArrayList<>(); - List s2T = new ArrayList<>(); - List s3T = new ArrayList<>(); - List s1 = new ArrayList<>(); - List s2 = new ArrayList<>(); - List s3 = new ArrayList<>(); - boolean oddBallColor = false; + double hoodOffset = 0.0; - boolean shootA = true; - boolean shootB = true; - boolean shootC = true; + boolean autoSpintake = false; boolean enableSpindexerManager = true; - List shootOrder = new ArrayList<>(); - boolean outtake1 = false; - boolean outtake2 = false; - boolean outtake3 = false; - boolean overrideTurr = false; - double turretPID = 0.0; - double turretPos = 0; - double spindexPID = 0.0; - double error = 0.0; - double spinCurrentPos = 0.0, spinInitPos = 0.0, intakeStamp = 0.0; - boolean reverse = false; - int intakeTicker = 0; - Pose2d brakePos = new Pose2d(0, 0, 0); - boolean autoDrive = false; - private boolean shootAll = false; - private double transferStamp = 0.0; - private int tickerA = 1; - private boolean transferIn = false; - boolean turretInterpolate = false; - public static double spinSpeedIncrease = 0.03; - public static int resetSpinTicks = 4; - public static double velPrediction(double distance) { - if (distance < 30) { - return 2750; - } else if (distance > 100) { - if (distance > 120) { - return 4500; - } - return 3700; - } else { - // linear interpolation between 40->2650 and 120->3600 - double slope = (3700.0 - 2750.0) / (100.0 - 30); - return (int) Math.round(2750 + slope * (distance - 30)); - } - } + boolean overrideTurr = false; + + int intakeTicker = 0; + + boolean turretInterpolate = false; + private boolean shootAll = false; @Override public void runOpMode() throws InterruptedException { @@ -156,7 +85,6 @@ public class TeleopV3 extends LinearOpMode { hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); } - TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); servo = new Servos(hardwareMap); flywheel = new Flywheel(hardwareMap); @@ -165,13 +93,15 @@ public class TeleopV3 extends LinearOpMode { targeting = new Targeting(); targetingSettings = new Targeting.Settings(0.0, 0.0); + drivetrain = new Drivetrain(robot, drive); + PIDFController tController = new PIDFController(tp, ti, td, tf); tController.setTolerance(0.001); Turret turret = new Turret(robot, TELE, robot.limelight); robot.light.setPosition(1); - while (opModeInInit()){ + while (opModeInInit()) { robot.limelight.start(); if (redAlliance) { robot.limelight.pipelineSwitch(4); @@ -194,63 +124,14 @@ public class TeleopV3 extends LinearOpMode { //DRIVETRAIN: - double y = 0.0; - double x = 0.0; - double rx = 0.0; - if (!autoDrive) { + drivetrain.drive( + gamepad1.right_stick_y, + gamepad1.right_stick_x, + gamepad1.left_stick_x, + gamepad1.left_trigger + ); - y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed - x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing - rx = gamepad1.left_stick_x; - - double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); - double frontLeftPower = (y + x + rx) / denominator; - double backLeftPower = (y - x + rx) / denominator; - double frontRightPower = (y - x - rx) / denominator; - double backRightPower = (y + x - rx) / denominator; - - robot.frontLeft.setPower(frontLeftPower); - robot.backLeft.setPower(backLeftPower); - robot.frontRight.setPower(frontRightPower); - robot.backRight.setPower(backRightPower); - } - - if (gamepad1.left_trigger > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) { - robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - drive.updatePoseEstimate(); - - brakePos = drive.localizer.getPose(); - autoDrive = true; - - } else if (gamepad1.left_trigger > 0.4) { - drive.updatePoseEstimate(); - - Pose2d currentPos = drive.localizer.getPose(); - - TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos) - .strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT); - - if (Math.abs(currentPos.position.x - brakePos.position.x) > 1 || Math.abs(currentPos.position.y - brakePos.position.y) > 1) { - Actions.runBlocking( - traj2.build() - ); - } - } else { - autoDrive = false; - robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - - } - - //TODO: Use color sensors to switch between positions...switch after ball detected in - - if (gamepad1.right_bumper){ + if (gamepad1.right_bumper) { shootAll = false; robot.transferServo.setPosition(transferServo_out); @@ -277,14 +158,14 @@ public class TeleopV3 extends LinearOpMode { double distanceToGoal = Math.sqrt(dx * dx + dy * dy); targetingSettings = targeting.calculateSettings - (robotX,robotY,robotHeading,0.0, turretInterpolate); + (robotX, robotY, robotHeading, 0.0, turretInterpolate); turret.trackGoal(deltaPose); //VELOCITY AUTOMATIC if (autoVel) { vel = targetingSettings.flywheelRPM; - } else { + } else { vel = manualVel; } @@ -311,7 +192,7 @@ public class TeleopV3 extends LinearOpMode { if (targetingHood) { robot.hood.setPosition(targetingSettings.hoodAngle); - } else { + } else { robot.hood.setPosition(hoodDefaultPos + hoodOffset); } @@ -351,7 +232,7 @@ public class TeleopV3 extends LinearOpMode { if (enableSpindexerManager) { //if (!shootAll) { - spindexer.processIntake(); + spindexer.processIntake(); //} // RIGHT_BUMPER @@ -398,7 +279,7 @@ public class TeleopV3 extends LinearOpMode { //spindexer.processIntake(); } - if (gamepad1.left_stick_button){ + if (gamepad1.left_stick_button) { robot.transferServo.setPosition(transferServo_out); //spindexPos = spindexer_intakePos1; @@ -407,8 +288,6 @@ public class TeleopV3 extends LinearOpMode { } } - - //EXTRA STUFFINESS: drive.updatePoseEstimate(); @@ -457,167 +336,4 @@ public class TeleopV3 extends LinearOpMode { } } - // Helper methods - private boolean checkGreen(List s, List sT) { - if (s.isEmpty()) return false; - - double lastTime = sT.get(sT.size() - 1); - int countTrue = 0; - int countWindow = 0; - - for (int i = 0; i < s.size(); i++) { - if (lastTime - sT.get(i) <= 3.0) { // element is within 2s of last - countWindow++; - if (s.get(i)) { - countTrue++; - } - } - } - - if (countWindow == 0) return false; // avoid divide by zero - return countTrue > countWindow / 2.0; // more than 50% true - } - - // -// public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) { -// // Set spin positions -// spindexPos = spindexer; -// -// // Check if spindexer has reached the target position -// if (spinOk || getRuntime() - stamp > 1.5) { -// if (tickerA == 1) { -// transferStamp = getRuntime(); -// tickerA++; -// TELE.addLine("tickerSet"); -// } -// -// if (getRuntime() - transferStamp > waitTransfer && !transferIn) { -// robot.transferServo.setPosition(transferServo_in); -// transferIn = true; -// TELE.addLine("transferring"); -// -// return true; // still in progress -// -// } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) { -// robot.transferServo.setPosition(transferServo_out); -// transferIn = false; // reset for next shot -// tickerA = 1; // reset ticker -// transferStamp = 0.0; -// -// TELE.addLine("shotFinished"); -// -// return false; // finished shooting -// } else { -// TELE.addLine("sIP"); -// return true; // still in progress -// } -// } else { -// robot.transferServo.setPosition(transferServo_out); -// tickerA = 1; -// transferStamp = getRuntime(); -// transferIn = false; -// return true; // still moving spin -// } -// } -// - public double hoodAnglePrediction(double x) { - double a = 1.44304; - double b = 0.0313707; - double c = 0.0931136; - - double result = a * Math.exp(-b * x) + c; - - // Clamp between min and max - if (result < 0.1) { - return 0.1; - } else if (result > 0.96) { - return 0.96; - } else { - return result; - } - } - - // -// void addOddThenRest(List order, boolean oddColor) { -// // Odd ball first -// for (int i = 1; i <= 3; i++) if (getBallColor(i) == oddColor) order.add(i); -// TELE.addData("1", shootOrder); -// for (int i = 1; i <= 3; i++) if (getBallColor(i) != oddColor) order.add(i); -// TELE.addData("works", shootOrder); -// TELE.addData("oddBall", oddColor); -// shootAll = true; -// -// } -// -// void addOddInMiddle(List order, boolean oddColor) { -// -// boolean[] used = new boolean[4]; // index 1..3 -// -// // 1) Add a NON-odd ball first -// for (int i = 1; i <= 3; i++) { -// if (getBallColor(i) != oddColor) { -// order.add(i); -// used[i] = true; -// break; -// } -// } -// -// // 2) Add the odd ball second -// for (int i = 1; i <= 3; i++) { -// if (!used[i] && getBallColor(i) == oddColor) { -// order.add(i); -// used[i] = true; -// break; -// } -// } -// -// // 3) Add the remaining non-odd ball third -// for (int i = 1; i <= 3; i++) { -// if (!used[i] && getBallColor(i) != oddColor) { -// order.add(i); -// used[i] = true; -// break; -// } -// } -// -// TELE.addData("works", order); -// TELE.addData("oddBall", oddColor); -// shootAll = true; -// -// } -// -// void addOddLast(List order, boolean oddColor) { -// // Odd ball last -// for (int i = 1; i <= 3; i++) if (getBallColor(i) != oddColor) order.add(i); -// TELE.addData("1", shootOrder); -// for (int i = 1; i <= 3; i++) if (getBallColor(i) == oddColor) order.add(i); -// TELE.addData("works", shootOrder); -// TELE.addData("oddBall", oddColor); -// shootAll = true; -// -// } -// -// // Returns color of ball in slot i (1-based) -// boolean getBallColor(int slot) { -// switch (slot) { -// case 1: -// return green1; -// case 2: -// return green2; -// case 3: -// return green3; -// } -// return false; // default -// } -// - boolean ballIn(int slot) { - List times = - slot == 1 ? s1T : - slot == 2 ? s2T : - slot == 3 ? s3T : null; - - if (times == null || times.isEmpty()) return false; - - return times.get(times.size() - 1) > getRuntime() - 2; - } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java new file mode 100644 index 0000000..375721f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java @@ -0,0 +1,86 @@ +package org.firstinspires.ftc.teamcode.utils; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.ProfileAccelConstraint; +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.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; + +public class Drivetrain { + + Robot robot; + boolean autoDrive = false; + + Pose2d brakePos = new Pose2d(0, 0, 0); + + MecanumDrive drive; + + private final TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200); + private final ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200); + + + public Drivetrain (Robot rob, MecanumDrive mecanumDrive){ + + this.robot = rob; + this.drive = mecanumDrive; + + } + + public void drive(double y, double x, double rx, double brake){ + + if (!autoDrive) { + + x = x* 1.1; // Counteract imperfect strafing + + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + robot.frontLeft.setPower(frontLeftPower); + robot.backLeft.setPower(backLeftPower); + robot.frontRight.setPower(frontRightPower); + robot.backRight.setPower(backRightPower); + } + + if (brake > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) { + robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + drive.updatePoseEstimate(); + + brakePos = drive.localizer.getPose(); + autoDrive = true; + + } else if (brake > 0.4) { + drive.updatePoseEstimate(); + + Pose2d currentPos = drive.localizer.getPose(); + + TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos) + .strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT); + + if (Math.abs(currentPos.position.x - brakePos.position.x) > 1 || Math.abs(currentPos.position.y - brakePos.position.y) > 1) { + Actions.runBlocking( + traj2.build() + ); + } + } else { + autoDrive = false; + robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + } + + } +}