Alr we got teleop working but just barely....almost finished with the gridn yk?

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java

@@ -16,7 +16,7 @@ import java.util.Objects;
 @Config
 public final class PinpointLocalizer implements Localizer {
     public static class Params {
-        public double parYTicks = 3765.023079161767; // y position of the parallel encoder (in tick units)
+        public double parYTicks = -3765.023079161767; // y position of the parallel encoder (in tick units)
         public double perpXTicks = -1962.6377639490684; // x position of the perpendicular encoder (in tick units)
     }
 

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 void trackGoal(Pose2d robotPose, Pose2d goalPose, boolean shooterOn){
+    public double trackGoal(Pose2d robotPose, Pose2d goalPose){
 
         fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
@@ -178,7 +178,7 @@ public class Shooter implements Subsystem {
         Pose2d deltaPose = new Pose2d(
                 goalPose.position.x - robotPose.position.x,
                 goalPose.position.y - robotPose.position.y,
-                goalPose.heading.toDouble() - robotPose.heading.toDouble()
+                goalPose.heading.toDouble() - (robotPose.heading.toDouble())
         );
 
         double distance = Math.sqrt(
@@ -187,25 +187,19 @@ public class Shooter implements Subsystem {
                 + Poses.relativeGoalHeight * Poses.relativeGoalHeight
         );
 
+        telemetry.addData("dst", distance);
+
         double shooterPow = getPowerByDist(distance);
 
         double hoodAngle = getAngleByDist(distance);
 
-        if (shooterOn){
 
-            fly1.setVelocity(shooterPow);
+//        hoodServo.setPosition(hoodAngle);
-            fly2.setPower(fly1.getPower());
-
-        } else {
-            fly1.setPower(0);
-            fly2.setPower(0);
-        }
-
-
-        hoodServo.setPosition(hoodAngle);
 
         moveTurret(getTurretPosByDeltaPose(deltaPose));
 
+        return distance;
+
 
 
 
@@ -220,17 +214,38 @@ public class Shooter implements Subsystem {
         double deltaAngle = Math.toDegrees(dPose.heading.toDouble());
 
 
-        if (deltaAngle < -180) {
+        double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y/dPose.position.x));
-            deltaAngle +=360;
+
+        telemetry.addData("deltaAngle", deltaAngle);
+
+
+
+        if (deltaAngle > 90) {
+            deltaAngle -=360;
         }
 
-        deltaAngle /= (0.9974*355);
 
-        return (0.5+deltaAngle) ;
+
+//        deltaAngle += aTanAngle;
+
+        deltaAngle /= (335);
+
+        telemetry.addData("dAngle", deltaAngle);
+
+        telemetry.addData("AtanAngle", aTanAngle);
+
+
+        return (0.30-deltaAngle) ;
+
 
 
     }
 
+    //62, 0.44
+
+    //56.5, 0.5
+
+
     public double getPowerByDist(double dist){
 
         //TODO: ADD LOGIC
@@ -239,8 +254,15 @@ public class Shooter implements Subsystem {
 
     public double getAngleByDist(double dist){
 
-        //TODO: ADD LOGIC
+
-        return  dist;
+        double newDist = dist - 56.5;
+
+        double pos = newDist*((0.44-0.5)/(62-56.5)) + 0.46;
+
+
+
+
+        return  pos;
     }
 
 
@@ -285,7 +307,7 @@ public class Shooter implements Subsystem {
         }
 
         if (Objects.equals(turretMode, "MANUAL")){
-            hoodServo.setPosition(hoodPos);
+//            hoodServo.setPosition(hoodPos);
 
             moveTurret(turretPos);
 

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

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.subsystems;
 
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.Servo;
 
@@ -31,6 +33,16 @@ public class Transfer implements Subsystem{
         this.motorPow = pow;
     }
 
+    public void transferOut(){
+        this.setTransferPosition(transferServo_out);
+    }
+
+    public void transferIn(){
+        this.setTransferPosition(transferServo_in);
+    }
+
+
+
 
 
     @Override

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

@@ -3,14 +3,17 @@ package org.firstinspires.ftc.teamcode.teleop;
 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.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
 import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.Shooter;
 import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
 import org.firstinspires.ftc.teamcode.subsystems.Transfer;
 import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -44,7 +47,7 @@ public class TeleopV1 extends LinearOpMode {
 
     public static double power = 0.0;
 
-    public static double pos = 0.501;
+    public static double pos = 0.5;
 
     ToggleButtonReader g1RightBumper;
 
@@ -54,15 +57,38 @@ public class TeleopV1 extends LinearOpMode {
 
 
     ToggleButtonReader g2Triangle;
+
+    ToggleButtonReader g2RightBumper;
+
+    ToggleButtonReader g2LeftBumper;
+
+    ToggleButtonReader g2DpadUp;
+
+    ToggleButtonReader g2DpadDown;
     public double g1RightBumperStamp = 0.0;
 
+    public double g2LeftBumperStamp = 0.0;
+
     public static int spindexerPos = 0;
 
+    Shooter shooter;
+
+    public boolean scoreAll = false;
+
+    MecanumDrive drive ;
+
+    public boolean autotrack = true;
+
 
 
     @Override
     public void runOpMode() throws InterruptedException {
 
+        drive  = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
+
+
+
+
         robot = new Robot(hardwareMap);
 
         TELE = new MultipleTelemetry(
@@ -90,6 +116,24 @@ 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
+        );
+
 
 
         drivetrain = new Drivetrain(robot, TELE, g1);
@@ -109,26 +153,161 @@ public class TeleopV1 extends LinearOpMode {
 
         spindexer.setTelemetryOn(true);
 
+        shooter = new Shooter(robot, TELE);
+
+        shooter.setShooterMode("MANUAL");
+
+
+
+
 
         waitForStart();
 
         if (isStopRequested()) return;
 
+        drive  = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
+
+
         while(opModeIsActive()){
 
+            drive.updatePoseEstimate();
+
+            TELE.addData("pose", drive.localizer.getPose());
+
+            TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
+
+
+            robot.hood.setPosition(pos);
+
+
             intake();
 
             drivetrain.update();
 
             TELE.update();
 
-            transfer.setTransferPower(power);
-
-            transfer.setTransferPosition(pos);
-
-
             transfer.update();
 
+            g2RightBumper.readValue();
+
+            g2LeftBumper.readValue();
+
+            g2DpadDown.readValue();
+
+            g2DpadUp.readValue();
+
+            if(g2DpadUp.wasJustPressed()){
+                pos -=0.02;
+            }
+
+            if(g2DpadDown.wasJustPressed()){
+                pos +=0.02;
+            }
+
+            if (gamepad2.dpad_left){
+                pos = shooter.getAngleByDist(
+                        shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0))
+                );
+
+            }
+
+            TELE.addData("hood", pos);
+
+
+
+
+
+
+            if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) {
+
+
+
+
+                shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0));
+
+            } else {
+
+                autotrack = false;
+
+                shooter.moveTurret(shooter.getTurretPosition() - gamepad2.right_stick_x* 0.04);
+
+            }
+
+            if (gamepad2.right_stick_button){
+                autotrack = true;
+            }
+
+
+
+            if (g2RightBumper.wasJustPressed()){
+                transfer.setTransferPower(1);
+                transfer.transferIn();
+                shooter.setManualPower(1);
+
+            }
+
+            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();
+                scoreAll = true;
+            }
+
+            if (scoreAll) {
+                double time = getRuntime() - g2LeftBumperStamp;
+
+                shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-2, 0, 0));
+
+                shooter.setManualPower(1);
+
+
+
+                if (time < 0.3) {
+                    transfer.transferOut();
+                    transfer.setTransferPower(1);
+                } else if (time < 1.5){
+                    spindexer.outtake3();
+                } else  if (time < 2.0){
+                    transfer.transferIn();
+                } else if (time < 3){
+                    transfer.transferOut();
+                    spindexer.outtake2();
+                } else  if (time < 3.5){
+                    transfer.transferIn();
+                } else if (time < 4.5){
+                    transfer.transferOut();
+                    spindexer.outtake1();
+                }  else  if (time < 5){
+                    transfer.transferIn();
+                } else {
+
+                    scoreAll = false;
+                    transfer.transferOut();
+
+                    shooter.setManualPower(0);
+                }
+
+
+
+            }
+
+
+            shooter.update();
+
+
+
+
+
 
 
 
@@ -156,6 +335,8 @@ public class TeleopV1 extends LinearOpMode {
 
         if (g1RightBumper.wasJustPressed()){
 
+            shooter.setManualPower(0);
+
 
             if (getRuntime() - g1RightBumperStamp < 0.3){
                 intake.reverse();
@@ -165,6 +346,8 @@ public class TeleopV1 extends LinearOpMode {
 
             spindexer.intake();
             
+            transfer.transferOut();
+
 
             g1RightBumperStamp = getRuntime();
 
@@ -177,6 +360,9 @@ public class TeleopV1 extends LinearOpMode {
 
             if (g2Circle.wasJustPressed()){
                 spindexer.outtake3();
+
+
+                
             }
 
             if (g2Triangle.wasJustPressed()){
@@ -187,8 +373,27 @@ public class TeleopV1 extends LinearOpMode {
                 spindexer.outtake1();
             }
 
+
+            if (g2Circle.wasJustReleased()){
+                transfer.setTransferPower(1);
+
+            }
+
+            if (g2Triangle.wasJustReleased()){
+                transfer.setTransferPower(1);
+            }
+
+            if (g2Square.wasJustReleased()){
+                transfer.setTransferPower(1);
+            }
+
+
+
+
+
         }
 
+
         intake.update();
 
 

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

@@ -126,6 +126,8 @@ public class Robot {
 
         transferServo = hardwareMap.get(Servo.class, "transferServo");
 
+        transfer.setDirection(DcMotorSimple.Direction.REVERSE);
+
 
     }
 }