Added tilt thing

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java

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

@@ -25,10 +25,16 @@ public class TeleopV4 extends LinearOpMode {
     MultipleTelemetry TELE;
     Follower follower;
     SpindexerTransferIntake spindexerTransferIntake;
+    Turret turret;
+    Flywheel flywheel;
+
+    ParkTilter parkTilter;
 
     @Override
     public void runOpMode() throws InterruptedException {
 
+        Robot.resetInstance();
+
         robot = Robot.getInstance(hardwareMap);
 
         TELE = new MultipleTelemetry(
@@ -40,7 +46,13 @@ public class TeleopV4 extends LinearOpMode {
         Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
         follower.setStartingPose(start);
 
-        shooter = new Shooter(robot, TELE, follower, Color.redAlliance);
+        flywheel = new Flywheel(robot);
+        turret = new Turret(robot);
+
+        parkTilter = new ParkTilter(robot);
+
+
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel);
         shooter.setState(Shooter.ShooterState.TRACK_GOAL);
         spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE);
         spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
@@ -60,12 +72,14 @@ public class TeleopV4 extends LinearOpMode {
                     gamepad1.left_stick_x
             );
 
+            follower.update();
+
             shooter.update();
             spindexerTransferIntake.update();
 
             SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
 
-            if (gamepad1.xWasPressed() &&
+            if (gamepad1.leftBumperWasPressed() &&
                     (state == SpindexerTransferIntake.RapidMode.INTAKE ||
                             state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
                             state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
@@ -76,7 +90,7 @@ public class TeleopV4 extends LinearOpMode {
                 spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
             }
 
-            if (gamepad1.aWasPressed() &&
+            if (gamepad1.right_trigger > 0.5 &&
                     (state == SpindexerTransferIntake.RapidMode.INTAKE ||
                             state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
 
@@ -84,8 +98,7 @@ public class TeleopV4 extends LinearOpMode {
                         SpindexerTransferIntake.RapidMode.HOLD_BALLS
                 );
             }
-
-            if (gamepad1.yWasPressed()
+            if (gamepad1.rightBumperWasPressed()
                     && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
 
                 spindexerTransferIntake.setRapidMode(
@@ -93,6 +106,11 @@ public class TeleopV4 extends LinearOpMode {
                 );
             }
 
+            if (gamepad1.dpad_down){
+                parkTilter.park();
+            } else if (gamepad1.dpad_up) {
+                parkTilter.unpark();
+            }
 
             TELE.update();
         }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java

@@ -130,7 +130,7 @@ public class Hardware_Tester extends LinearOpMode {
 //            TELE.addData("Beam Break 3?", robot.beam3.isPressed());
 
             NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
-            TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.MM));
+            TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.CM));
             TELE.addData("REV Green", revColor.green / (revColor.red + revColor.blue + revColor.green));
 
             TELE.addData("Voltage Sensor", robot.voltage.getVoltage());

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java

@@ -11,15 +11,17 @@ import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
 import org.firstinspires.ftc.teamcode.utilsv2.Robot;
 import org.firstinspires.ftc.teamcode.utilsv2.Shooter;
+import org.firstinspires.ftc.teamcode.utilsv2.Turret;
 
 @Config
 @TeleOp
 public class NewShooterTest extends LinearOpMode {
 
     Robot robot;
+    Flywheel flywheel;
+    Turret turret;
     MultipleTelemetry TELE;
-    Shooter shooter;
-    Flywheel rpmFlywheel;
+
 
     public static boolean intake = true;
     public static boolean shoot = false;
@@ -28,7 +30,7 @@ public class NewShooterTest extends LinearOpMode {
     public static double transferIntakePower = -1;
     public static double turretPos = 0.51;
     public static double hoodPos = 0.51;
-    public static double flywheel = 0;
+    public static double flywheel_velo = 0;
 
     public static double shooterP = 255, shooterI = 0, shooterD = 0, shooterF = 75;
 
@@ -50,15 +52,19 @@ public class NewShooterTest extends LinearOpMode {
 
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        shooter = new Shooter(
+
+        flywheel = new Flywheel(robot);
+        turret = new Turret(robot);
+
+        Shooter shooter = new Shooter(
                 robot,
                 TELE,
                 Constants.createFollower(hardwareMap),
-                true
+                true,
+                turret,
+                flywheel
         );
 
-        rpmFlywheel = new Flywheel(robot);
-
         shooter.setState(Shooter.ShooterState.MANUAL);
 
         waitForStart();
@@ -69,10 +75,9 @@ public class NewShooterTest extends LinearOpMode {
 
             robot.setHoodPos(hoodPos);
             shooter.setTurretPosition(turretPos);
-            shooter.setFlywheelVelocity(flywheel);
-            rpmFlywheel.manageFlywheel(shooter.getFlywheelVelocity());
+            shooter.setFlywheelVelocity(flywheel_velo);
             double voltage = robot.voltage.getVoltage();
-            rpmFlywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage);
+            flywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage);
 
             robot.setSpinPos(ServoPositions.spindexer_A2);
 
@@ -131,9 +136,9 @@ public class NewShooterTest extends LinearOpMode {
 
             TELE.addData("Flywheel Velocity1", (robot.shooter1.getVelocity() * 60) / 28);
             TELE.addData("Flywheel Velocity2", (robot.shooter2.getVelocity() * 60) / 28);
-            TELE.addData("Flywheel Averag Velocity", rpmFlywheel.getAverageVelocity());
+            TELE.addData("Flywheel Averag Velocity", flywheel.getAverageVelocity());
             TELE.addData("PIDF Coefficients", Flywheel.shooterPIDF);
-            TELE.addData("Power", rpmFlywheel.getShooterPower());
+            TELE.addData("Power", flywheel.getShooterPower());
             TELE.update();
 
             shooter.update();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/ParkTilter.java

@@ -0,0 +1,22 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+
+public class ParkTilter {
+    Robot robot;
+    public ParkTilter (Robot rob) {
+        this.robot = rob;
+    }
+
+    public void park() {
+        robot.setTilt1Pos(ServoPositions.tilt1_down);
+        robot.setTilt2Pos(ServoPositions.tilt2_down);
+    }
+
+    public void unpark() {
+        robot.setTilt1Pos(ServoPositions.tilt1_up);
+        robot.setTilt2Pos(ServoPositions.tilt2_up);
+    }
+}

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

@@ -7,6 +7,7 @@ import com.pedropathing.follower.Follower;
 public class Shooter {
 
     Robot robot;
+    Flywheel fly;
     Turret turr;
     VelocityCommander commander;
 
@@ -20,15 +21,21 @@ public class Shooter {
 
     Follower follow;
 
-    public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance) {
+    public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance, Turret turret, Flywheel flywheel) {
         this.robot = rob;
-        this.turr = new Turret(rob);
+        this.fly = flywheel;
+        this.turr = turret;
         this.follow = follower;
         this.commander = new VelocityCommander();
 
         setRedAlliance(redAlliance);
 
-        if (redAlliance) {
+    }
+
+    public void setRedAlliance(boolean input) {
+        this.red = input;
+
+        if (this.red) {
             goalX = 144;
         } else {
             goalX = 0;
@@ -36,10 +43,6 @@ public class Shooter {
         goalY = 144;
     }
 
-    public void setRedAlliance(boolean input) {
-        this.red = input;
-    }
-
     private double flywheelVelocity = 0.0;
     private double turretPosition = 0.5;
 
@@ -66,7 +69,7 @@ public class Shooter {
     public void setFlywheelVelocity(double input) {
         this.flywheelVelocity = input;
     }
-    public double getFlywheelVelocity(){return this.flywheelVelocity;}
+
     public int getObeliskID() {
         return turr.getObeliskID();
     }
@@ -78,12 +81,13 @@ public class Shooter {
             case NOTHING:
                 break;
             case MANUAL:
+                fly.manageFlywheel(flywheelVelocity);
                 turr.manual(turretPosition);
                 break;
             case TRACK_GOAL:
                 turr.trackGoal(
-                        (follow.getPose().getX() - goalX),
-                        (follow.getPose().getY() - goalY),
+                        (goalX - follow.getPose().getX()),
+                        (goalY - follow.getPose().getY()),
                         follow.getHeading(),
                         follow.getAngularVelocity(),
                         follow.getVelocity().getXComponent(),
@@ -93,50 +97,53 @@ public class Shooter {
                 );
 
                 flywheelVelocity = commander.getVeloPredictive(
-                        (follow.getPose().getX() - goalX),
-                        (follow.getPose().getY() - goalY),
+                        (goalX - follow.getPose().getX()),
+                        (goalY - follow.getPose().getY()),
                         follow.getVelocity().getXComponent(),
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent()
                 );
 
+                fly.manageFlywheel(flywheelVelocity);
                 break;
             case READ_OBELISK:
                 turr.trackObelisk(
-                        (follow.getPose().getX() - goalX),
-                        (follow.getPose().getY() - goalY),
+                        (goalX - follow.getPose().getX()),
+                        (goalY - follow.getPose().getY()),
                         follow.getHeading()
                 );
 
                 flywheelVelocity = commander.getVeloPredictive(
-                        (follow.getPose().getX() - goalX),
-                        (follow.getPose().getY() - goalY),
+                        (goalX - follow.getPose().getX()),
+                        (goalY - follow.getPose().getY()),
                         follow.getVelocity().getXComponent(),
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent()
                 );
 
+                fly.manageFlywheel(flywheelVelocity);
                 break;
 
             case MANUAL_TURRET_TRACK_FLY:
                 turr.manual(turretPosition);
                 flywheelVelocity = commander.getVeloPredictive(
-                        (follow.getPose().getX() - goalX),
-                        (follow.getPose().getY() - goalY),
+                        (goalX - follow.getPose().getX()),
+                        (goalY - follow.getPose().getY()),
                         follow.getVelocity().getXComponent(),
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent()
                 );
 
+                fly.manageFlywheel(flywheelVelocity);
                 break;
 
             case MANUAL_FLYWHEEL_TRACK_TURR:
                 turr.trackGoal(
-                        (follow.getPose().getX() - goalX),
-                        (follow.getPose().getY() - goalY),
+                        (goalX - follow.getPose().getX()),
+                        (goalY - follow.getPose().getY()),
                         follow.getHeading(),
                         follow.getAngularVelocity(),
                         follow.getVelocity().getXComponent(),
@@ -144,6 +151,7 @@ public class Shooter {
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent()
                 );
+                fly.manageFlywheel(flywheelVelocity);
                 break;
 
         }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java

@@ -13,8 +13,8 @@ public class SpindexerTransferIntake {
         this.robot = rob;
     }
 
-    private final double sensorDistanceThreshold = 4.0;
-    private final long pulseTime = 50; // ms
+    private final double sensorDistanceThreshold = 6.0;
+    private final long pulseTime = 100; // ms
 
     public enum SpindexerMode {
         RAPID,
@@ -74,7 +74,7 @@ public class SpindexerTransferIntake {
                     case INTAKE:
 
                         robot.setIntakePower(1);
-                        robot.setTransferPower(1);
+                        robot.setTransferPower(-0.7);
                         robot.setRapidFireBlockerPos(
                                 ServoPositions.rapidFireBlocker_Closed
                         );
@@ -95,7 +95,7 @@ public class SpindexerTransferIntake {
 
                     case TRANSFER_OFF:
 
-                        robot.setTransferPower(0.3);
+                        robot.setTransferPower(-0.7);
 
                         if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                             setRapidMode(RapidMode.BEFORE_PULSE_OUT);
@@ -145,9 +145,6 @@ public class SpindexerTransferIntake {
 
                             robot.setIntakePower(1);
                         }
-
-
-
                         break;
 
                     case OPEN_GATE:

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java

@@ -12,7 +12,7 @@ public class VelocityCommander {
     }
 
     private double distToRPM (double dist){
-        return Math.sqrt(dist*dist + goalH*goalH);
+        return Math.sqrt(dist*dist + goalH*goalH) * 20;
         //TODO: Add regression here using goalH
     }