stash

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

@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+public class Color {
+}

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

@@ -0,0 +1,29 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.Pose2d;
+
+@Config
+public class Poses {
+
+    public static double goalHeight = 42; //in inches
+
+    public static double turretHeight = 12;
+
+    public static double relativeGoalHeight = goalHeight - turretHeight;
+
+    public static double x1 = 50, y1 = 0, h1 = 0;
+
+    public static double x2 = 31, y2 = 32, h2 = Math.toRadians(140);
+
+    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);
+
+
+
+}

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

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class ServoPositions {
+
+    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;
+
+    public static double spindexer_outtakeBall2 = 0.48;
+    public static double spindexer_outtakeBall1 = 0.1;
+
+    public static double transferServo_out = 0.15;
+
+    public static double transferServo_in = 0.38;
+
+    public static double hoodDefault = 0.35;
+
+    public static double hoodHigh = 0.21;
+
+    public static double hoodLow = 1.0;
+
+    public static double turret_red = 0.43;
+    public static double turret_blue = 0.4;
+
+
+}

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

@@ -0,0 +1,16 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class ShooterVars {
+    public static double turret_GearRatio = 0.9974;
+
+    public static double turret_Range = 355;
+
+    public static int velTolerance = 300;
+
+    public static int initTolerance = 1000;
+
+    public static int maxVel = 4000;
+}

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

@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+public class blank {
+}

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

@@ -2,18 +2,34 @@ package org.firstinspires.ftc.teamcode.teleop;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
+import java.util.ArrayList;
+import java.util.List;
+
 public class TeleopV2 extends LinearOpMode {
 
     Robot robot;
     MultipleTelemetry TELE;
 
+    boolean intake = false;
+    boolean reject = false;
+
+    List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
+    List<Double> s1 = new ArrayList<>();
+    List<Double> s2 = new ArrayList<>();
+    List<Double> s3 = new ArrayList<>();
+
     @Override
     public void runOpMode() throws InterruptedException {
 
+        for (LynxModule hub : allHubs) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+        }
+
         robot = new Robot(hardwareMap);
         TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
@@ -29,7 +45,6 @@ public class TeleopV2 extends LinearOpMode {
             double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
             double 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;
@@ -41,6 +56,29 @@ public class TeleopV2 extends LinearOpMode {
             robot.frontRight.setPower(frontRightPower);
             robot.backRight.setPower(backRightPower);
 
+            //INTAKE:
+
+            if (gamepad1.rightBumperWasPressed()) {
+                intake = true;
+            }
+
+            if (intake) {
+                robot.intake.setPower(1);
+            } else if (reject) {
+                robot.intake.setPower(-1);
+            } else {
+                robot.intake.setPower(0);
+            }
+
+
+            //COLOR:
+
+            double s1D = robot.
+
+            for (LynxModule hub : allHubs) {
+                hub.clearBulkCache();
+            }
+
             TELE.update();
 
         }