Some tele fixeds i hope

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java

@@ -168,7 +168,7 @@ public class Shooter implements Subsystem {
     public void setTurretMode(String mode){ turretMode = mode;}
 
 
-    public double trackGoal(Pose2d robotPose, Pose2d goalPose){
+    public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset){
 
         fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
@@ -196,7 +196,7 @@ public class Shooter implements Subsystem {
 
 //        hoodServo.setPosition(hoodAngle);
 
-        moveTurret(getTurretPosByDeltaPose(deltaPose));
+        moveTurret(getTurretPosByDeltaPose(deltaPose, offset));
 
         return distance;
 
@@ -209,7 +209,7 @@ public class Shooter implements Subsystem {
 
     }
 
-    public double getTurretPosByDeltaPose (Pose2d dPose){
+    public double getTurretPosByDeltaPose (Pose2d dPose, double offset){
 
         double deltaAngle = Math.toDegrees(dPose.heading.toDouble());
 
@@ -235,7 +235,7 @@ public class Shooter implements Subsystem {
         telemetry.addData("AtanAngle", aTanAngle);
 
 
-        return (0.30-deltaAngle) ;
+        return ((0.30-deltaAngle) + offset);
 
 
 

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

@@ -47,7 +47,7 @@ public class TeleopV1 extends LinearOpMode {
 
     public static double power = 0.0;
 
-    public static double pos = 0.5;
+    public static double pos = 0.54;
 
     ToggleButtonReader g1RightBumper;
 
@@ -65,6 +65,10 @@ public class TeleopV1 extends LinearOpMode {
     ToggleButtonReader g2DpadUp;
 
     ToggleButtonReader g2DpadDown;
+
+    ToggleButtonReader g2DpadRight;
+
+    ToggleButtonReader g2DpadLeft;
     public double g1RightBumperStamp = 0.0;
 
     public double g2LeftBumperStamp = 0.0;
@@ -79,6 +83,10 @@ public class TeleopV1 extends LinearOpMode {
 
     public boolean autotrack = true;
 
+    public double offset = 0.0;
+
+    public boolean notShooting = true;
+
 
 
     @Override
@@ -89,6 +97,7 @@ public class TeleopV1 extends LinearOpMode {
 
 
 
+
         robot = new Robot(hardwareMap);
 
         TELE = new MultipleTelemetry(
@@ -134,6 +143,16 @@ public class TeleopV1 extends LinearOpMode {
                 g2, GamepadKeys.Button.DPAD_DOWN
         );
 
+        g2DpadLeft  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_LEFT
+        );
+
+
+        g2DpadRight  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_RIGHT
+        );
+
+
 
 
         drivetrain = new Drivetrain(robot, TELE, g1);
@@ -204,9 +223,21 @@ public class TeleopV1 extends LinearOpMode {
                 pos +=0.02;
             }
 
-            if (gamepad2.dpad_left){
+            g2DpadLeft.readValue();
+
+            g2DpadRight.readValue();
+
+            if(g2DpadLeft.wasJustPressed()){
+                offset -=0.02;
+            }
+
+            if(g2DpadRight.wasJustPressed()){
+                offset +=0.02;
+            }
+
+            if (gamepad2.right_trigger > 0.5){
                 pos = shooter.getAngleByDist(
-                        shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0))
+                        shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0),  offset)
                 );
 
             }
@@ -218,18 +249,20 @@ public class TeleopV1 extends LinearOpMode {
 
 
 
+
+
             if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) {
 
 
 
 
-                shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0));
+                shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset);
 
             } else {
 
                 autotrack = false;
 
-                shooter.moveTurret(shooter.getTurretPosition() - gamepad2.right_stick_x* 0.04);
+                shooter.moveTurret(shooter.getTurretPosition() - gamepad2.right_stick_x* 0.02);
 
             }
 
@@ -244,6 +277,8 @@ public class TeleopV1 extends LinearOpMode {
                 transfer.transferIn();
                 shooter.setManualPower(1);
 
+                notShooting = false;
+
             }
 
             if (g2RightBumper.wasJustReleased()){
@@ -260,13 +295,14 @@ public class TeleopV1 extends LinearOpMode {
 
             if (g2LeftBumper.wasJustPressed()){
                 g2LeftBumperStamp = getRuntime();
+                notShooting = false;
                 scoreAll = true;
             }
 
             if (scoreAll) {
                 double time = getRuntime() - g2LeftBumperStamp;
 
-                shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-2, 0, 0));
+                shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset);
 
                 shooter.setManualPower(1);
 
@@ -335,7 +371,9 @@ public class TeleopV1 extends LinearOpMode {
 
         if (g1RightBumper.wasJustPressed()){
 
-            shooter.setManualPower(0);
+            notShooting = true;
+
+
 
 
             if (getRuntime() - g1RightBumperStamp < 0.3){
@@ -344,6 +382,13 @@ public class TeleopV1 extends LinearOpMode {
                 intake.toggle();
             }
 
+            if (intake.getIntakeState()==1){
+                shooter.setManualPower(0);
+            }
+
+
+
+
             spindexer.intake();
             
             transfer.transferOut();
@@ -353,9 +398,14 @@ public class TeleopV1 extends LinearOpMode {
 
         }
 
-        if (intake.getIntakeState()==1) {
+
+        if (intake.getIntakeState()==1 && notShooting) {
+
             spindexer.intakeShake(getRuntime());
-        } else {
+
+        } else
+
+        {
 
 
             if (g2Circle.wasJustPressed()){