diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java index 377903a..15a24fb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java @@ -13,7 +13,6 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.ParallelAction; import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.SequentialAction; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -37,6 +36,12 @@ public class redDaniel extends LinearOpMode { AprilTag aprilTag; + 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 + // TODO: change this velocity PID public Action initShooter(int velocity){ return new Action(){ double velo = 0.0; @@ -67,6 +72,10 @@ public class redDaniel extends LinearOpMode { }; } + public void Obelisk (){ + // TODO: write the code to detect order + } + public Action Shoot(double spindexer){ return new Action() { boolean transfer = false; @@ -87,6 +96,93 @@ public class redDaniel extends LinearOpMode { }; } + public Action intake (){ + return new Action() { + double position = 0.0; + final double intakeTime = 4.0; // TODO: change this so it serves as a backup + final double stamp = getRuntime(); + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if ((getRuntime() % 0.3) >0.15) { + position = spindexer_intakePos1 + 0.02; + } else { + position = spindexer_intakePos1 - 0.02; + } + robot.spin1.setPosition(position); + robot.spin2.setPosition(1-position); + + robot.intake.setPower(1); + + return !(robot.pin1.getState() && robot.pin3.getState() && robot.pin5.getState()) || getRuntime() - stamp > intakeTime; + } + }; + } + + public Action ColorDetect (){ + return new Action() { + int t1 = 0; + int t2 = 0; + int t3 = 0; + int tP1 = 0; + int tP2 = 0; + int tP3 = 0; + final double stamp = getRuntime(); + final double detectTime = 3.0; + double position = 0.0; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if ((getRuntime() % 0.3) >0.15) { + position = spindexer_intakePos1 + 0.02; + } else { + position = spindexer_intakePos1 - 0.02; + } + robot.spin1.setPosition(position); + robot.spin2.setPosition(1-position); + if (robot.pin1.getState()) { + t1 += 1; + if (robot.pin0.getState()){ + tP1 += 1; + } + } + if (robot.pin3.getState()) { + t2 += 1; + if (robot.pin0.getState()){ + tP2 += 1; + } + } + if (robot.pin5.getState()) { + t3 += 1; + if (robot.pin0.getState()){ + tP3 += 1; + } + } + if (t1 > 20){ + if (tP1 > 20){ + b1 = 2; + } else { + b1 = 1; + } + } + if (t2 > 20){ + if (tP2 > 20){ + b2 = 2; + } else { + b2 = 1; + } + } + if (t3 > 20){ + if (tP3 > 20){ + b3 = 2; + } else { + b3 = 1; + } + } + return !(b1 + b2 + b3 >= 5) || (getRuntime() - stamp < detectTime); + + } + }; + } + @Override public void runOpMode() throws InterruptedException { 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 d2ef894..e65c56e 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 @@ -6,7 +6,11 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos = 0.665; + public static double spindexer_intakePos1 = 0.665; + + public static double spindexer_intakePos3 = 0.29; + + public static double spindexer_intakePos2 = 0.99; public static double spindexer_outtakeBall3 = 0.845; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java index e91820a..df5f001 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java @@ -9,8 +9,6 @@ import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.teamcode.utils.Robot; -import java.util.ArrayList; - public class Spindexer implements Subsystem{ private Servo s1; @@ -118,14 +116,14 @@ public class Spindexer implements Subsystem{ } public void intake () { - position = spindexer_intakePos; + position = spindexer_intakePos1; } public void intakeShake(double runtime) { if ((runtime % 0.25) >0.125) { - position = spindexer_intakePos + 0.04; + position = spindexer_intakePos1 + 0.04; } else { - position = spindexer_intakePos - 0.04; + position = spindexer_intakePos1 - 0.04; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java index 226701c..9615325 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java @@ -49,9 +49,9 @@ public class ActiveColorSensorTest extends LinearOpMode { while (opModeIsActive()){ if ((getRuntime() % 0.3) >0.15) { - position = spindexer_intakePos + 0.02; + position = spindexer_intakePos1 + 0.015; } else { - position = spindexer_intakePos - 0.02; + position = spindexer_intakePos1 - 0.015; } robot.spin1.setPosition(position); robot.spin2.setPosition(1-position); @@ -126,4 +126,4 @@ public class ActiveColorSensorTest extends LinearOpMode { } } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java index dc9cf69..8c1cd26 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java @@ -29,6 +29,8 @@ public class ColorSensorTest extends LinearOpMode{ TELE.addData("Purple2:", robot.pin2.getState()); TELE.addData("Green3:", robot.pin5.getState()); TELE.addData("Purple3:", robot.pin4.getState()); + TELE.addData("Hello Keshav (analog)", robot.analogInput.getVoltage()); + TELE.addData("Hello again (analog2)", robot.analogInput2.getVoltage()); TELE.update(); } 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 dd7159d..91ec518 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 @@ -30,7 +30,12 @@ public class ShooterTest extends LinearOpMode { double initPos = 0.0; + double velo = 0.0; double velo1 = 0.0; + double velo2 = 0.0; + double velo3 = 0.0; + double velo4 = 0.0; + double velo5 = 0.0; double stamp1 = 0.0; @@ -38,7 +43,7 @@ public class ShooterTest extends LinearOpMode { double powPID = 0.0; - public static int maxVel = 4000; + public static int maxVel = 4500; public static boolean shoot = false; @@ -48,7 +53,9 @@ public class ShooterTest extends LinearOpMode { public static int tolerance = 50; - public static double kP = 0.0005; // small proportional gain (tune this) + double stamp = 0.0; + + public static double kP = 0.001; // small proportional gain (tune this) public static double maxStep = 0.06; // prevents sudden jumps public static double distance = 50; @@ -72,12 +79,16 @@ public class ShooterTest extends LinearOpMode { initPos = shooter.getECPRPosition(); + int ticker = 0; + waitForStart(); if (isStopRequested()) return; while (opModeIsActive()) { + ticker++; + if (AutoTrack){ hoodPos = hoodAnglePrediction(distance); vel = velPrediction(distance); @@ -85,69 +96,88 @@ public class ShooterTest extends LinearOpMode { - shooter.setShooterMode(flyMode); + shooter.setShooterMode(flyMode); - shooter.setManualPower(pow); + shooter.setManualPower(pow); - robot.hood.setPosition(hoodPos); - robot.turr1.setPosition(turretPos); - robot.turr2.setPosition(1 - turretPos); - if (intake) { - robot.transfer.setPower(0); - robot.intake.setPower(0.75); - robot.spin1.setPosition(spindexer_intakePos); - robot.spin2.setPosition(1 - spindexer_intakePos); - } else { - robot.transfer.setPower(1); - robot.intake.setPower(0.75 + (powPID/4)); - if (spindexPos == 1) { - robot.spin1.setPosition(spindexer_outtakeBall1); - robot.spin2.setPosition(1 - spindexer_outtakeBall1); - } else if (spindexPos == 2) { - robot.spin1.setPosition(spindexer_outtakeBall2); - robot.spin2.setPosition(1 - spindexer_outtakeBall2); - } else if (spindexPos == 3) { - robot.spin1.setPosition(spindexer_outtakeBall3); - robot.spin2.setPosition(1 - spindexer_outtakeBall3); - } + robot.hood.setPosition(hoodPos); + robot.turr1.setPosition(turretPos); + robot.turr2.setPosition(1 - turretPos); + if (intake) { + robot.transfer.setPower(0); + robot.intake.setPower(0.75); + robot.spin1.setPosition(spindexer_intakePos1); + robot.spin2.setPosition(1 - spindexer_intakePos1); + } else { + robot.transfer.setPower(.75 + (powPID/4)); + robot.intake.setPower(0); + if (spindexPos == 1) { + robot.spin1.setPosition(spindexer_outtakeBall1); + robot.spin2.setPosition(1 - spindexer_outtakeBall1); + } else if (spindexPos == 2) { + robot.spin1.setPosition(spindexer_outtakeBall2); + robot.spin2.setPosition(1 - spindexer_outtakeBall2); + } else if (spindexPos == 3) { + robot.spin1.setPosition(spindexer_outtakeBall3); + robot.spin2.setPosition(1 - spindexer_outtakeBall3); } + } - velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1)); - stamp1 = getRuntime(); - initPos1 = shooter.getECPRPosition(); + double penguin = 0; + if (ticker % 8 ==0){ + penguin = shooter.getECPRPosition(); + stamp = getRuntime(); + velo1 = -60 * ((penguin - initPos1) / (stamp - stamp1)); + initPos1 = penguin; + stamp1 = stamp; + } - double feed = vel / maxVel; // Example: vel=2500 → feed=0.5 - // --- PROPORTIONAL CORRECTION --- - double error = vel - velo1; - double correction = kP * error; + velo = velo1; - // limit how fast power changes (prevents oscillation) - correction = Math.max(-maxStep, Math.min(maxStep, correction)); + double feed = vel / maxVel; // Example: vel=2500 → feed=0.5 - // --- FINAL MOTOR POWER --- - powPID = feed + correction; + if (vel > 500){ + feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18; + } - // clamp to allowed range - powPID = Math.max(0, Math.min(1, powPID)); - shooter.setVelocity(powPID); + // --- PROPORTIONAL CORRECTION --- + double error = vel - velo1; + double correction = kP * error; - if (shoot) { - robot.transferServo.setPosition(transferServo_in); - } else { - robot.transferServo.setPosition(transferServo_out); - } + // limit how fast power changes (prevents oscillation) + correction = Math.max(-maxStep, Math.min(maxStep, correction)); - shooter.update(); + // --- FINAL MOTOR POWER --- + powPID = feed + correction; - TELE.addData("Revolutions", shooter.getECPRPosition()); - TELE.addData("hoodPos", shooter.gethoodPosition()); - TELE.addData("turretPos", shooter.getTurretPosition()); - TELE.addData("Power Fly 1", robot.shooter1.getPower()); - TELE.addData("Power Fly 2", robot.shooter2.getPower()); - TELE.addData("powPID", shooter.getpowPID()); - TELE.addData("Velocity", velo1); - TELE.update(); + // 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; + } + + shooter.setVelocity(powPID); + + if (shoot) { + robot.transferServo.setPosition(transferServo_in); + } else { + robot.transferServo.setPosition(transferServo_out); + } + + shooter.update(); + + TELE.addData("Revolutions", shooter.getECPRPosition()); + TELE.addData("hoodPos", shooter.gethoodPosition()); + TELE.addData("turretPos", shooter.getTurretPosition()); + TELE.addData("Power Fly 1", robot.shooter1.getPower()); + TELE.addData("Power Fly 2", robot.shooter2.getPower()); + TELE.addData("powPID", shooter.getpowPID()); + TELE.addData("Velocity", velo); + TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java index 48250e9..a556440 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java @@ -22,10 +22,10 @@ public class ConfigureColorRangefinder extends LinearOpMode { /* Using this example configuration, you can detect both artifact colors based on which pin is reading true: pin0 --> purple pin1 --> green */ - crf.setPin1Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // green - crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple - crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255); - crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement + crf.setPin0Analog(ColorRangefinder.AnalogMode.GREEN); // green +// crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple +// crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255); +// crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement crf.setLedBrightness(LED_Brightness); }