Added tilt thing

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

@@ -28,9 +28,13 @@ public class TeleopV4 extends LinearOpMode {
     Turret turret;
     Flywheel flywheel;
 
+    ParkTilter parkTilter;
+
     @Override
     public void runOpMode() throws InterruptedException {
 
+        Robot.resetInstance();
+
         robot = Robot.getInstance(hardwareMap);
 
         TELE = new MultipleTelemetry(
@@ -45,6 +49,8 @@ public class TeleopV4 extends LinearOpMode {
         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);
@@ -66,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 ||
@@ -82,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)) {
 
@@ -90,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(
@@ -99,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/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

@@ -86,8 +86,8 @@ public class Shooter {
                 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(),
@@ -97,8 +97,8 @@ 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(),
@@ -109,14 +109,14 @@ public class Shooter {
                 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(),
@@ -129,8 +129,8 @@ public class Shooter {
             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(),
@@ -142,8 +142,8 @@ public class Shooter {
 
             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(),

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
     }