stash

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java

@@ -1,6 +1,5 @@
 package org.firstinspires.ftc.teamcode.constants;
 
-
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.Pose2d;
 
@@ -19,11 +18,11 @@ public class Poses {
 
     public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(140);
 
-
     public static double x3 = 34, y3 = 58, h3 = Math.toRadians(140);
 
-    public static Pose2d teleStart = new Pose2d(x1,-10,0);
-
+    public static double tx = 0, ty = 0, th = 0;
+    public static Pose2d teleStart = new Pose2d(tx, ty, th);
 
+    public static Pose2d goalPose = new Pose2d(-15, 0, 0);
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java

@@ -13,4 +13,7 @@ public class ShooterVars {
     public static int initTolerance = 1000;
 
     public static int maxVel = 4000;
+    public static double waitTransfer = 0.4;
+    public static double waitTransferOut = 0.6;
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java

@@ -1,13 +1,20 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
+import static org.firstinspires.ftc.teamcode.constants.Poses.goalPose;
 import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
+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.constants.ShooterVars.waitTransfer;
 import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP;
 import static org.firstinspires.ftc.teamcode.tests.ShooterTest.maxStep;
+import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.restPos;
+import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.scalar;
 
 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.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -33,6 +40,9 @@ public class TeleopV2 extends LinearOpMode {
     List<Double> s1G = new ArrayList<>();
     List<Double> s2G = new ArrayList<>();
     List<Double> s3G = new ArrayList<>();
+    List<Double> s1T = new ArrayList<>();
+    List<Double> s2T = new ArrayList<>();
+    List<Double> s3T = new ArrayList<>();
     List<Boolean> s1 = new ArrayList<>();
     List<Boolean> s2 = new ArrayList<>();
     List<Boolean> s3 = new ArrayList<>();
@@ -44,6 +54,8 @@ public class TeleopV2 extends LinearOpMode {
     private double stamp1, stamp, initPos;
     private boolean shootAll = false;
 
+    boolean shoot1 = false;
+
     @Override
     public void runOpMode() throws InterruptedException {
 
@@ -87,7 +99,15 @@ public class TeleopV2 extends LinearOpMode {
                 intake = true;
             }
 
+            if (gamepad1.leftBumperWasPressed()) {
+                intake = false;
+            }
+
             if (intake) {
+
+                robot.transferServo.setPosition(transferServo_out);
+
+
                 robot.intake.setPower(1);
 
                 double position;
@@ -131,6 +151,9 @@ public class TeleopV2 extends LinearOpMode {
                 } else {
                     s1.add(false);
                 }
+
+                s1T.add(getRuntime());
+
             }
 
             if (s2D < 40) {
@@ -148,6 +171,8 @@ public class TeleopV2 extends LinearOpMode {
                 } else {
                     s2.add(false);
                 }
+
+                s2T.add(getRuntime());
             }
 
             if (s3D < 30) {
@@ -165,6 +190,8 @@ public class TeleopV2 extends LinearOpMode {
                 } else {
                     s3.add(false);
                 }
+
+                s3T.add(getRuntime());
             }
 
             boolean green1 = false;
@@ -172,13 +199,12 @@ public class TeleopV2 extends LinearOpMode {
             boolean green3 = false;
 
             if (!s1.isEmpty()) {
-                green1 = s1.get(s1.size() - 1);
+                green1 = checkGreen(s1, s1T);
             }
             if (!s2.isEmpty()) {
-                green2 = s2.get(s2.size() - 1);
-            }
+                green2 = checkGreen(s2, s2T);
             if (!s3.isEmpty()) {
-                green3 = s3.get(s3.size() - 1);
+                green3 = checkGreen(s3, s3T);            }
             }
 
             //SHOOTER:
@@ -201,6 +227,7 @@ public class TeleopV2 extends LinearOpMode {
                 feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
             }
 
+
             // --- PROPORTIONAL CORRECTION ---
             double error = vel - velo1;
             double correction = kP * error;
@@ -220,11 +247,19 @@ public class TeleopV2 extends LinearOpMode {
                 powPID = 0;
             }
 
+            TELE.addData("PIDPower", powPID);
+
+            TELE.addData("vel", velo1);
+
+
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
 
+            robot.transfer.setPower(1);
+
             robot.hood.setPosition(hood);
 
+
             //TODO: ADD CODE TO CHANGE VARIABLE VEL BASED ON POSITION
 
             //TODO: ADD CODE TO CHANGE VARIABLE HOOD ANGLE BASED ON POSITION
@@ -239,13 +274,30 @@ public class TeleopV2 extends LinearOpMode {
                 intake = false;
                 reject = false;
 
+                if (robot.)
+
             }
 
             //TURRET:
 
             double offset;
 
-            offset = desiredTurretAngle - Math.toDegrees(drive.localizer.getPose().heading.toDouble());
+            double robotX = drive.localizer.getPose().position.x;
+            double robotY = drive.localizer.getPose().position.y;
+
+            double goalX = -10;
+            double goalY = 0;
+
+            double dx = goalX - robotX;  // delta x from robot to goal
+            double dy = goalY - robotY;  // delta y from robot to goal
+
+
+
+            desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360;
+
+            offset = desiredTurretAngle - 180 - Math.toDegrees(drive.localizer.getPose().heading.toDouble());
+
+
 
             if (offset > 90) {
                 offset -= 360;
@@ -253,6 +305,9 @@ public class TeleopV2 extends LinearOpMode {
 
             double pos = 0.3;
 
+            TELE.addData("offset", offset);
+
+
             pos -= offset * (0.9 / 360);
 
             if (pos < 0.02) {
@@ -261,11 +316,24 @@ public class TeleopV2 extends LinearOpMode {
                 pos = 0.91;
             }
 
+
+
             robot.turr1.setPosition(pos);
             robot.turr2.setPosition(1 - pos);
 
+            //SPINDEXER:
+
+            if (gamepad2.squareWasPressed()){
+                shoot1 = true;
+            }
+
+
+
+
+
             //MISC:
 
+
             drive.updatePoseEstimate();
 
             for (LynxModule hub : allHubs) {
@@ -285,4 +353,70 @@ public class TeleopV2 extends LinearOpMode {
 
         }
     }
+
+    // Helper method
+    private boolean checkGreen(List<Boolean> s, List<Double> 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
+    }
+
+    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);
+    }
+
+    // Fields to keep state across calls
+    private double transferStamp = 0.0;
+    private int ticker = 1;
+
+    public boolean shootTeleop(double spindexer) {
+        // Spin motors
+        robot.spin1.setPosition(spindexer);
+        robot.spin2.setPosition(1 - spindexer);
+
+        // Default transfer position
+        robot.transferServo.setPosition(transferServo_out);
+
+        if (spindexPosEqual(spindexer)) {
+            if (ticker == 1) {
+                transferStamp = getRuntime();
+                ticker++;
+            }
+
+            if (getRuntime() - transferStamp > waitTransfer) {
+                robot.transferServo.setPosition(transferServo_in);
+            } else {
+                robot.transferServo.setPosition(transferServo_out);
+            }
+        } else {
+            robot.transferServo.setPosition(transferServo_out);
+            ticker = 1;
+            transferStamp = getRuntime();
+        }
+
+        TELE.addLine("shooting");
+        TELE.update();
+
+        // Return true while transfer servo hasn't reached "in" position
+        return !transferPosEqual(transferServo_in);
+    }
+
+
+
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java

@@ -0,0 +1,53 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+@TeleOp
+@Config
+public class PositionalServoProgrammer extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    public static double spindexPos = 0.501;
+    public static double turretPos = 0.501;
+    public static double transferPos = 0.501;
+    public static double hoodPos = 0.501;
+
+    public static double scalar = 1.112;
+    public static double restPos = 0.158;
+    @Override
+    public void runOpMode() throws InterruptedException {
+        robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        waitForStart();
+        if (isStopRequested()) return;
+        while (opModeIsActive()){
+            if (spindexPos != 0.501){
+                robot.spin1.setPosition(spindexPos);
+                robot.spin2.setPosition(1-spindexPos);
+            }
+            if (turretPos != 0.501){
+                robot.turr1.setPosition(turretPos);
+                robot.turr2.setPosition(1-turretPos);
+            }
+            if (transferPos != 0.501){
+                robot.transferServo.setPosition(transferPos);
+            }
+            if (hoodPos != 0.501){
+                robot.hood.setPosition(hoodPos);
+            }
+            TELE.addData("spindexer", scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3));
+            TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3));
+            TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3));
+            TELE.addData("turret", scalar*((robot.turr1Pos.getVoltage() - restPos) / 3.3));
+            TELE.addData("spindexerA", robot.spin1Pos.getVoltage());
+            TELE.addData("hoodA", robot.hoodPos.getVoltage());
+            TELE.addData("transferServoA", robot.transferServoPos.getVoltage());
+            TELE.addData("turretA", robot.turr1Pos.getVoltage());
+            TELE.update();
+        }
+    }
+}