moved in tele

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

@@ -1,17 +1,22 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
+import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodDefault;
+
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.Pose2d;
 import com.arcrobotics.ftclib.gamepad.GamepadEx;
 import com.arcrobotics.ftclib.gamepad.GamepadKeys;
 import com.arcrobotics.ftclib.gamepad.ToggleButtonReader;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
 import org.firstinspires.ftc.teamcode.subsystems.Intake;
-import org.firstinspires.ftc.teamcode.subsystems.Rejecter;
 import org.firstinspires.ftc.teamcode.subsystems.Shooter;
 import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
 import org.firstinspires.ftc.teamcode.subsystems.Transfer;
@@ -23,7 +28,6 @@ import org.firstinspires.ftc.teamcode.utils.Robot;
 
 public class TeleopV1 extends LinearOpMode {
 
-    public static double rpos = 0.5;
 
     Robot robot;
 
@@ -31,14 +35,10 @@ public class TeleopV1 extends LinearOpMode {
 
     Intake intake;
 
-    Rejecter rejecter;
-
     Spindexer spindexer;
 
     Transfer transfer;
 
-    Shooter shooter;
-
     MultipleTelemetry TELE;
 
     GamepadEx g1;
@@ -49,34 +49,85 @@ public class TeleopV1 extends LinearOpMode {
 
     public static double slowMoSpeed = 0.4;
 
-    public static double shooterPower = 0.0;
+    public static double power = 0.0;
 
-    public static double turretPosition = 0.501;
+    public static double pos = hoodDefault;
 
-    public static double hoodPosition = 0.501;
+    public boolean all = false;
+
+    public int ticker = 0;
 
     ToggleButtonReader g1RightBumper;
 
-    ToggleButtonReader g1LeftBumper;
-
     ToggleButtonReader g2Circle;
 
     ToggleButtonReader g2Square;
 
-    ToggleButtonReader g2LeftBumper;
 
     ToggleButtonReader g2Triangle;
+
+    ToggleButtonReader g2RightBumper;
+
+    ToggleButtonReader g1LeftBumper;
+
+    ToggleButtonReader g2LeftBumper;
+
+    ToggleButtonReader g2DpadUp;
+
+    ToggleButtonReader g2DpadDown;
+
+    ToggleButtonReader g2DpadRight;
+
+    ToggleButtonReader g2DpadLeft;
+
+    public boolean leftBumper = false;
     public double g1RightBumperStamp = 0.0;
 
+    public double g1LeftBumperStamp = 0.0;
+
+
+    public double g2LeftBumperStamp = 0.0;
+
     public static int spindexerPos = 0;
 
-    public double time = 0.0;
+    public boolean green = false;
+
+    Shooter shooter;
+
+    public boolean scoreAll = false;
+
+    MecanumDrive drive ;
+
+    public boolean autotrack = false;
+
+    public int last = 0;
+    public int second = 0;
+
+    public double offset = 0.0;
+
+    public static double rIn = 0.59;
+
+    public static double rOut = 0;
+
+    public boolean notShooting = true;
+
+    public boolean circle = false;
+
+    public boolean square = false;
+
+    public boolean tri = false;
 
 
 
     @Override
     public void runOpMode() throws InterruptedException {
 
+        drive  = new MecanumDrive(hardwareMap, teleStart);
+
+
+
+
+
         robot = new Robot(hardwareMap);
 
         TELE = new MultipleTelemetry(
@@ -90,12 +141,12 @@ public class TeleopV1 extends LinearOpMode {
                 g1, GamepadKeys.Button.RIGHT_BUMPER
         );
 
+        g2 = new GamepadEx(gamepad2);
+
         g1LeftBumper = new ToggleButtonReader(
                 g1, GamepadKeys.Button.LEFT_BUMPER
         );
 
-        g2 = new GamepadEx(gamepad2);
-
         g2Circle  = new ToggleButtonReader(
                 g2, GamepadKeys.Button.B
         );
@@ -108,10 +159,33 @@ public class TeleopV1 extends LinearOpMode {
                 g2, GamepadKeys.Button.X
         );
 
+        g2RightBumper  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.RIGHT_BUMPER
+        );
+
+
         g2LeftBumper  = new ToggleButtonReader(
                 g2, GamepadKeys.Button.LEFT_BUMPER
         );
 
+        g2DpadUp  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_UP
+        );
+
+
+        g2DpadDown  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_DOWN
+        );
+
+        g2DpadLeft  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_LEFT
+        );
+
+
+        g2DpadRight  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_RIGHT
+        );
+
 
 
 
@@ -127,36 +201,594 @@ public class TeleopV1 extends LinearOpMode {
 
         transfer = new Transfer(robot);
 
-        spindexer = new Spindexer(robot, TELE);
 
-        shooter = new Shooter(robot, TELE);
+        spindexer = new Spindexer(robot, TELE);
 
         spindexer.setTelemetryOn(true);
 
-        time = getRuntime();
+        shooter = new Shooter(robot, TELE);
+
+        shooter.setShooterMode("MANUAL");
+
+        robot.rejecter.setPosition(rIn);
+
+
+
 
 
         waitForStart();
 
         if (isStopRequested()) return;
 
+        drive  = new MecanumDrive(hardwareMap, teleStart);
+
+
         while(opModeIsActive()){
 
-            intake();
+            drive.updatePoseEstimate();
 
-            rejecter.rejecterPos(rpos);
+            TELE.addData("pose", drive.localizer.getPose());
+
+            TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
+
+
+            TELE.addData("off", offset);
+
+
+            robot.hood.setPosition(pos);
+
+            g1LeftBumper.readValue();
+
+            if (g1LeftBumper.wasJustPressed()){
+                g2LeftBumperStamp = getRuntime();
+
+
+
+                spindexer.intakeShake(getRuntime());
+
+                leftBumper = true;
+            }
+
+            if (leftBumper){
+                double time = getRuntime() - g2LeftBumperStamp;
+
+                if (time < 1.0){
+                    robot.rejecter.setPosition(rOut);
+                } else {
+                    robot.rejecter.setPosition(rIn);
+                }
+
+            }
+
+
+
+
+            intake();
 
             drivetrain.update();
 
             TELE.update();
 
-            transfer();
+            transfer.update();
 
-            shooter.setManualPower(shooterPower);
+            g2RightBumper.readValue();
 
-            shooter.sethoodPosition(hoodPosition);
+            g2LeftBumper.readValue();
+
+            g2DpadDown.readValue();
+
+            g2DpadUp.readValue();
+
+            if (!scoreAll){
+                spindexer.checkForBalls();
+            }
+
+            if(g2DpadUp.wasJustPressed()){
+                pos -=0.02;
+            }
+
+            if(g2DpadDown.wasJustPressed()){
+                pos +=0.02;
+            }
+
+            g2DpadLeft.readValue();
+
+            g2DpadRight.readValue();
+
+            if(g2DpadLeft.wasJustPressed()){
+                offset -=0.02;
+            }
+
+            if(g2DpadRight.wasJustPressed()){
+                offset +=0.02;
+            }
+
+
+
+            TELE.addData("hood", pos);
+
+
+
+
+
+
+
+
+            if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) {
+
+
+
+
+                shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset);
+
+            } else {
+
+                autotrack = false;
+
+                shooter.moveTurret(0.3+offset);
+
+            }
+
+            if (gamepad2.right_stick_button){
+                autotrack = true;
+            }
+
+
+
+            if (g2RightBumper.wasJustPressed()){
+                transfer.setTransferPower(1);
+                transfer.transferIn();
+                shooter.setManualPower(1);
+
+                notShooting = false;
+
+            }
+
+            if (g2RightBumper.wasJustReleased()){
+                transfer.setTransferPower(1);
+                transfer.transferOut();
+            }
+
+            if (gamepad2.left_stick_y>0.5){
+
+                shooter.setManualPower(0);
+            } else if (gamepad2.left_stick_y<-0.5){
+                shooter.setManualPower(1);
+            }
+
+            if (g2LeftBumper.wasJustPressed()){
+                g2LeftBumperStamp = getRuntime();
+                notShooting = false;
+                scoreAll = true;
+            }
+
+            if (scoreAll) {
+                double time = getRuntime() - g2LeftBumperStamp;
+
+
+                shooter.setManualPower(1);
+
+                TELE.addData("greenImportant", green);
+
+                TELE.addData("last", last);
+                TELE.addData("2ndLast", second);
+
+                int numGreen = spindexer.greens();
+
+                if (square) {
+
+
+                    if (time < 0.3) {
+
+                        ticker = 0;
+
+                        last = 0;
+                        second = 0;
+
+
+                        transfer.transferOut();
+                        transfer.setTransferPower(1);
+                    } else if (time < 2) {
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakePurple(second, last);
+                                second = last;
+                            } else {
+                                last = spindexer.outtakeGreen(second, last);
+                                second = last;
+
+                            }
+                        }
+
+                        second = last;
+
+                        ticker++;
+
+
+                    } else if (time < 2.5) {
+
+                        ticker = 0;
+
+                        second = last;
+
+
+                        transfer.transferIn();
+                    } else if (time < 4) {
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+                    } else if (time < 4.5) {
+
+                        ticker = 0;
+
+
+                        transfer.transferIn();
+                    } else if (time < 6) {
+
+
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+
+                    } else if (time < 6.5) {
+                        transfer.transferIn();
+                    } else {
+
+                        ticker = 0;
+
+
+                        scoreAll = false;
+                        transfer.transferOut();
+
+                        shooter.setManualPower(0);
+
+                    }
+                } else if (tri) {
+
+
+                    if (time < 0.3) {
+
+                        ticker = 0;
+
+                        last = 0;
+                        second = 0;
+
+
+                        transfer.transferOut();
+                        transfer.setTransferPower(1);
+                    } else if (time < 2) {
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                                second = last;
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+                                second = last;
+
+                            }
+                        }
+
+                        second = last;
+
+                        ticker++;
+
+
+                    } else if (time < 2.5) {
+
+                        ticker = 0;
+
+                        second = last;
+
+
+                        transfer.transferIn();
+                    } else if (time < 4) {
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakePurple(second, last);
+                            } else {
+                                last = spindexer.outtakeGreen(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+                    } else if (time < 4.5) {
+
+                        ticker = 0;
+
+
+                        transfer.transferIn();
+                    } else if (time < 6) {
+
+
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+
+                    } else if (time < 6.5) {
+                        transfer.transferIn();
+                    } else {
+
+                        ticker = 0;
+
+
+                        scoreAll = false;
+                        transfer.transferOut();
+
+                        shooter.setManualPower(0);
+
+                    }
+                } else if (circle){
+
+
+                    if (time < 0.3) {
+
+                        ticker = 0;
+
+                        last = 0;
+                        second = 0;
+
+
+                        transfer.transferOut();
+                        transfer.setTransferPower(1);
+                    } else if (time < 2) {
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                                second = last;
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+                                second = last;
+
+                            }
+                        }
+
+                        second = last;
+
+                        ticker++;
+
+
+                    } else if (time < 2.5) {
+
+                        ticker = 0;
+
+                        second = last;
+
+
+                        transfer.transferIn();
+                    } else if (time < 4) {
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+                    } else if (time < 4.5) {
+
+                        ticker = 0;
+
+
+                        transfer.transferIn();
+                    } else if (time < 6) {
+
+
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakePurple(second, last);
+                            } else {
+                                last = spindexer.outtakeGreen(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+
+                    } else if (time < 6.5) {
+                        transfer.transferIn();
+                    } else {
+
+                        ticker = 0;
+
+
+                        scoreAll = false;
+                        transfer.transferOut();
+
+                        shooter.setManualPower(0);
+
+                    }
+                } else{
+
+
+                    if (time < 0.3) {
+
+                        ticker = 0;
+
+                        last = 0;
+                        second = 0;
+
+                        if (gamepad2.right_trigger > 0.5) {
+                            green = false;
+
+                            all = gamepad2.left_trigger > 0.5;
+
+                        } else if (gamepad2.left_trigger > 0.5) {
+                            green = true;
+
+                            all = false;
+                        } else {
+                            all = true;
+                        }
+
+
+                        transfer.transferOut();
+                        transfer.setTransferPower(1);
+                    } else if (time < 2) {
+
+                        if (ticker == 0) {
+
+                            if (all) {
+                                spindexer.outtake3();
+                                last = 3;
+                                second = 3;
+                            } else if (green) {
+                                last = spindexer.outtakeGreen(second, last);
+                                second = last;
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+                                second = last;
+
+                            }
+                        }
+
+                        second = last;
+
+                        ticker++;
+
+
+                    } else if (time < 2.5) {
+
+                        ticker = 0;
+
+                        second = last;
+
+                        if (gamepad2.right_trigger > 0.5) {
+                            green = false;
+
+                            all = gamepad2.left_trigger > 0.5;
+
+                        } else if (gamepad2.left_trigger > 0.5) {
+                            green = true;
+
+                            all = false;
+
+
+                        }
+
+                        transfer.transferIn();
+                    } else if (time < 4) {
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (all) {
+                                spindexer.outtake2();
+
+                                last = 2;
+                            } else if (green) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+                    } else if (time < 4.5) {
+
+                        ticker = 0;
+
+
+                        if (gamepad2.right_trigger > 0.5) {
+                            green = false;
+
+                            all = gamepad2.left_trigger > 0.5;
+
+                        } else if (gamepad2.left_trigger > 0.5) {
+                            green = true;
+
+                            all = false;
+                        }
+
+                        transfer.transferIn();
+                    } else if (time < 6) {
+
+
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (all) {
+                                spindexer.outtake1();
+                            } else if (green) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+
+                    } else if (time < 6.5) {
+                        transfer.transferIn();
+                    } else {
+
+                        ticker = 0;
+
+
+                        scoreAll = false;
+                        transfer.transferOut();
+
+                        shooter.setManualPower(0);
+
+                    }
+
+
+                }
+            }
+
+
+            shooter.update();
 
-            shooter.setTurretPosition(turretPosition);
 
 
 
@@ -188,6 +820,10 @@ public class TeleopV1 extends LinearOpMode {
 
         if (g1RightBumper.wasJustPressed()){
 
+            notShooting = true;
+
+
+
 
             if (getRuntime() - g1RightBumperStamp < 0.3){
                 intake.reverse();
@@ -195,47 +831,65 @@ public class TeleopV1 extends LinearOpMode {
                 intake.toggle();
             }
 
+            if (intake.getIntakeState()==1){
+                shooter.setManualPower(0);
+            }
+
+
+
+
             spindexer.intake();
 
+            transfer.transferOut();
+
 
             g1RightBumperStamp = getRuntime();
 
         }
 
-        if (intake.getIntakeState()==1) {
+
+        if (intake.getIntakeState()==1 && notShooting) {
+
             spindexer.intakeShake(getRuntime());
-            transfer.setTransferPowerOff();
+
-            transfer.setTransferPositionOff();
         } else {
-
-
             if (g2Circle.wasJustPressed()){
-                transfer.setTransferPositionOff();
+                circle = true;
-                intake.intakeMinPower();
+                tri = false;
-                spindexer.outtake3();
+                square = false;
-                transfer.setTransferPowerOn();
+
+
+
             }
 
             if (g2Triangle.wasJustPressed()){
-                transfer.setTransferPositionOff();
+                circle = false;
-                intake.intakeMinPower();
+                tri = true;
-                spindexer.outtake2();
+                square = false;
-                transfer.setTransferPowerOn();
             }
 
             if (g2Square.wasJustPressed()){
-                transfer.setTransferPositionOff();
+                circle = false;
-                intake.intakeMinPower();
+                tri = false;
-                spindexer.outtake1();
+                square = true;
-                transfer.setTransferPowerOn();
             }
 
+            if (gamepad2.x){
+                circle = false;
+                tri = false;
+                square = false;
             }
 
+
+
+
+        }
+
+
         intake.update();
 
 
-        transfer.update();
+
 
         spindexer.update();
 
@@ -243,17 +897,5 @@ public class TeleopV1 extends LinearOpMode {
 
 
 
-    }
-
-    public void transfer(){
-
-        g1LeftBumper.readValue();
-
-        if (g1LeftBumper.wasJustPressed()){
-            transfer.setTransferPositionOn();
-        }
-
-        transfer.update();
-
     }
 }