diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java index 1f518d4..17832ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Poses.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; -import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*; import androidx.annotation.NonNull; @@ -26,6 +25,8 @@ import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Servos; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; @Config @Autonomous(preselectTeleOp = "TeleopV2") @@ -41,6 +42,8 @@ public class Red_V2 extends LinearOpMode { Flywheel flywheel; + Servos servo; + double velo = 0.0; public static double intake1Time = 2.9; @@ -56,19 +59,14 @@ public class Red_V2 extends LinearOpMode { double powPID = 0.0; + double bearing = 0.0; + int b1 = 0; // 0 = no ball, 1 = green, 2 = purple int b2 = 0;// 0 = no ball, 1 = green, 2 = purple int b3 = 0;// 0 = no ball, 1 = green, 2 = purple - boolean spindexPosEqual(double spindexer) { - TELE.addLine("spindex equal"); - TELE.update(); - return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 && - scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01); - } - public Action initShooter(int vel) { return new Action() { double stamp = 0.0; @@ -125,7 +123,7 @@ public class Red_V2 extends LinearOpMode { TELE.addData("Velocity", velo); TELE.update(); - + detectTag(); if (last && !steady){ stamp = getRuntime(); @@ -171,9 +169,10 @@ public class Red_V2 extends LinearOpMode { TELE.update(); if (gpp || pgp || ppg){ - robot.turr1.setPower(turret_red); - robot.turr2.setPower(1 - turret_red); - return false; + double turretPID = servo.setTurrPos(turret_red); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + return !servo.turretEqual(turret_red); } else { return true; } @@ -200,7 +199,7 @@ public class Red_V2 extends LinearOpMode { teleStart = drive.localizer.getPose(); - return !spindexPosEqual(spindexer); + return !servo.spinEqual(spindexer); } }; } @@ -222,6 +221,7 @@ public class Red_V2 extends LinearOpMode { robot.shooter2.setPower(powPID); drive.updatePoseEstimate(); + detectTag(); teleStart = drive.localizer.getPose(); @@ -454,8 +454,9 @@ public class Red_V2 extends LinearOpMode { robot.hood.setPosition(hoodAuto); - robot.turr1.setPower(turret_detectRed); - robot.turr2.setPower(1 - turret_detectRed); + double turretPID = servo.setTurrPos(turret_detectRed); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); robot.transferServo.setPosition(transferServo_out); @@ -574,6 +575,25 @@ public class Red_V2 extends LinearOpMode { } + public void detectTag(){ + AprilTagDetection d20 = aprilTag.getTagById(20); + AprilTagDetection d24 = aprilTag.getTagById(24); + + if (d20 != null) { + bearing = d20.ftcPose.bearing; + TELE.addData("Bear", bearing); + } + + if (d24 != null) { + bearing = d24.ftcPose.bearing; + TELE.addData("Bear", bearing); + } + double turretPos = servo.getTurrPos() - (bearing / 1300); + double turretPID = servo.setTurrPos(turretPos); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + } + public void shootingSequence() { TELE.addData("Velocity", velo); if (gpp) { 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 7a9b262..422eb65 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 @@ -16,7 +16,9 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; +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.vision.apriltag.AprilTagDetection; import java.util.ArrayList; @@ -25,7 +27,8 @@ import java.util.List; @TeleOp @Config public class TeleopV2 extends LinearOpMode { - + Servos servo; + Flywheel flywheel; public static double manualVel = 3000; public static double hood = 0.5; public static double hoodDefaultPos = 0.5; @@ -88,6 +91,11 @@ public class TeleopV2 extends LinearOpMode { private double transferStamp = 0.0; private int tickerA = 1; private boolean transferIn = false; + double turretPID = 0.0; + double turretPos = 0.5; + double spindexPID = 0.0; + double spindexPos = spindexer_intakePos1; + double error = 0.0; public static double velPrediction(double distance) { @@ -119,6 +127,8 @@ public class TeleopV2 extends LinearOpMode { TELE = new MultipleTelemetry( telemetry, FtcDashboard.getInstance().getTelemetry() ); + servo = new Servos(hardwareMap); + flywheel = new Flywheel(); drive = new MecanumDrive(hardwareMap, teleStart); @@ -126,9 +136,6 @@ public class TeleopV2 extends LinearOpMode { aprilTagWebcam.init(new Robot(hardwareMap), TELE); - robot.turr1.setPower(0.4); - robot.turr2.setPower(1 - 0.4); - waitForStart(); if (isStopRequested()) return; while (opModeIsActive()) { @@ -150,6 +157,19 @@ public class TeleopV2 extends LinearOpMode { robot.frontRight.setPower(frontRightPower); robot.backRight.setPower(backRightPower); + // PID SERVOS + turretPID = servo.setTurrPos(turretPos); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + + if (!servo.spinEqual(spindexPos)){ + robot.spin1.setPower(spindexPID); + robot.spin2.setPower(-spindexPID); + } else{ + robot.spin1.setPower(0); + robot.spin2.setPower(0); + } + //INTAKE: if (gamepad1.rightBumperWasPressed()) { @@ -173,22 +193,15 @@ public class TeleopV2 extends LinearOpMode { robot.intake.setPower(1); - double position; - if ((getRuntime() % 0.3) > 0.15) { - position = spindexer_intakePos1 + 0.015; + spindexPos = spindexer_intakePos1 + 0.015; } else { - position = spindexer_intakePos1 - 0.015; + spindexPos = spindexer_intakePos1 - 0.015; } - robot.spin1.setPower(position); - robot.spin2.setPower(1 - position); - } else if (reject) { robot.intake.setPower(-1); - double position = spindexer_intakePos1; - robot.spin1.setPower(position); - robot.spin2.setPower(1 - position); + spindexPos = spindexer_intakePos1; } else { robot.intake.setPower(0); } @@ -270,46 +283,7 @@ public class TeleopV2 extends LinearOpMode { //SHOOTER: - double penguin = 0; - - if (ticker % 8 == 0) { - penguin = (double) robot.shooterEncoder.getCurrentPosition() / 2048; - double stamp = getRuntime(); - velo1 = -60 * ((penguin - initPos) / (stamp - stamp1)); - initPos = penguin; - stamp1 = stamp; - } - - velo = velo1; - - double feed = vel / 4500; - - if (vel > 500) { - feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18; - } - - // --- PROPORTIONAL CORRECTION --- - double error = vel - velo1; - double correction = kP * error; - - // limit how fast power changes (prevents oscillation) - correction = Math.max(-maxStep, Math.min(maxStep, correction)); - - // --- FINAL MOTOR POWER --- - double powPID = feed + correction; - - // clamp to allowed range - powPID = Math.max(0, Math.min(1, powPID)); - - if (vel - velo > 1000) { - powPID = 1; - } else if (velo - vel > 1000) { - powPID = 0; - } - - TELE.addData("PIDPower", powPID); - - TELE.addData("vel", velo1); + double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition()); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); @@ -345,7 +319,7 @@ public class TeleopV2 extends LinearOpMode { offset -= 360; } - double pos = turrDefault; + double pos = turrDefault + error; TELE.addData("offset", offset); @@ -357,14 +331,33 @@ public class TeleopV2 extends LinearOpMode { pos = 0.97; } + if (y < 0.1 && y > -0.1 && x < 0.1 && x > -0.1 && rx < 0.1 && rx > -0.1){ + AprilTagDetection d20 = aprilTagWebcam.getTagById(20); + AprilTagDetection d24 = aprilTagWebcam.getTagById(24); + + if (d20 != null) { + overrideTurr = true; + double bearing = d20.ftcPose.bearing; + turretPos = servo.getTurrPos() - (bearing/1300); + TELE.addData("Bear", bearing); + } + + if (d24 != null) { + overrideTurr = true; + double bearing = d24.ftcPose.bearing; + turretPos = servo.getTurrPos() - (bearing/1300); + TELE.addData("Bear", bearing); + } + } + if (manualTurret) { pos = turrDefault + (manualOffset / 100); } if (!overrideTurr) { - - robot.turr1.setPower(pos); - robot.turr2.setPower(1 - pos); + turretPos = pos; + } else{ + error = servo.getSpinPos() - pos; } if (gamepad2.dpad_right) { @@ -445,12 +438,9 @@ public class TeleopV2 extends LinearOpMode { reject = true; if (getRuntime() % 3 > 1.5) { - robot.spin1.setPower(0); - robot.spin2.setPower(1); + spindexPos = 1; } else { - - robot.spin1.setPower(1); - robot.spin2.setPower(0); + spindexPos = 0; } robot.transferServo.setPosition(transferServo_out); @@ -468,39 +458,14 @@ public class TeleopV2 extends LinearOpMode { int currentSlot = shootOrder.get(0); // Peek, do NOT remove yet boolean shootingDone = false; - AprilTagDetection d20 = aprilTagWebcam.getTagById(20); - AprilTagDetection d24 = aprilTagWebcam.getTagById(24); - - if (d20 != null) { - overrideTurr = true; - double bearing = d20.ftcPose.bearing; - - double finalPos = robot.turr1.getPower() - (bearing / 1300); - robot.turr1.setPower(finalPos); - robot.turr2.setPower(1 - finalPos); - - TELE.addData("Bear", bearing); - - } - - if (d24 != null) { - overrideTurr = true; - - double bearing = d24.ftcPose.bearing; - double finalPos = robot.turr1.getPower() - (bearing / 1300); - robot.turr1.setPower(finalPos); - robot.turr2.setPower(1 - finalPos); - - } - if (!outtake1) { - outtake1 = (spindexPosEqual(spindexer_outtakeBall1)); + outtake1 = (servo.spinEqual(spindexer_outtakeBall1)); } if (!outtake2) { - outtake2 = (spindexPosEqual(spindexer_outtakeBall2)); + outtake2 = (servo.spinEqual(spindexer_outtakeBall2)); } if (!outtake3) { - outtake3 = (spindexPosEqual(spindexer_outtakeBall3)); + outtake3 = (servo.spinEqual(spindexer_outtakeBall3)); } switch (currentSlot) { @@ -543,8 +508,7 @@ public class TeleopV2 extends LinearOpMode { } else { // Finished shooting all balls - robot.spin1.setPower(spindexer_intakePos1); - robot.spin2.setPower(1 - spindexer_intakePos1); + spindexPos = spindexer_intakePos1; shootA = true; shootB = true; shootC = true; @@ -757,15 +721,9 @@ public class TeleopV2 extends LinearOpMode { return countTrue > countWindow / 2.0; // more than 50% true } - boolean spindexPosEqual(double spindexer) { - return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 && - scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01); - } - public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) { // Set spin positions - robot.spin1.setPower(spindexer); - robot.spin2.setPower(1 - spindexer); + spindexPos = spindexer; // Check if spindexer has reached the target position if (spinOk || getRuntime() - stamp > 1.5) { @@ -920,9 +878,4 @@ public class TeleopV2 extends LinearOpMode { } return true; // default } - - public double turretPos() { - return (scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3)); - } - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java index 0dce6e4..07ae970 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java @@ -53,7 +53,7 @@ public class IntakeTest extends LinearOpMode { stamp = getRuntime(); } ticker++; - if (getRuntime() - stamp < 0.5) { + if (getRuntime() - stamp < 0.5) { robot.intake.setPower(-1); } else { robot.intake.setPower(0);