teleop almost there

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

@@ -53,7 +53,7 @@ public class TeleopV4 extends LinearOpMode {
 
         parkTilter = new ParkTilter(robot);
 
-        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel);
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
         shooter.setState(Shooter.ShooterState.TRACK_GOAL);
         spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
         spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
@@ -64,9 +64,7 @@ public class TeleopV4 extends LinearOpMode {
         if (isStopRequested()) return;
 
         while (opModeIsActive()) {
-
             //Drivetrain
-
             drivetrain.drive(
                     -gamepad1.right_stick_y,
                     gamepad1.right_stick_x,
@@ -113,6 +111,11 @@ public class TeleopV4 extends LinearOpMode {
                 parkTilter.unpark();
             }
 
+            TELE.addData("Distance From Goal", commander.getDistance());
+            TELE.addData("Hood Position", commander.getHoodPredicted());
+            TELE.addData("Transfer Power", robot.transfer.getPower());
+            TELE.addData("Velocity RPM", commander.getPredictedRPM());
+
             TELE.update();
         }
 

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

@@ -14,6 +14,7 @@ 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;
+import org.firstinspires.ftc.teamcode.utilsv2.VelocityCommander;
 
 @Config
 @TeleOp
@@ -25,6 +26,7 @@ public class NewShooterTest extends LinearOpMode {
     Shooter shooter;
     MultipleTelemetry TELE;
     Follower follower;
+    VelocityCommander commander;
 
 
     public static boolean intake = true;
@@ -61,6 +63,7 @@ public class NewShooterTest extends LinearOpMode {
 
         flywheel = new Flywheel(robot);
         turret = new Turret(robot);
+        commander = new VelocityCommander();
 
         shooter = new Shooter(
                 robot,
@@ -68,7 +71,8 @@ public class NewShooterTest extends LinearOpMode {
                 follower,
                 true,
                 turret,
-                flywheel
+                flywheel,
+                commander
         );
 
         shooter.setState(Shooter.ShooterState.MANUAL);

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

@@ -21,12 +21,12 @@ public class Shooter {
 
     Follower follow;
 
-    public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance, Turret turret, Flywheel flywheel) {
+    public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance, Turret turret, Flywheel flywheel, VelocityCommander com) {
         this.robot = rob;
         this.fly = flywheel;
         this.turr = turret;
         this.follow = follower;
-        this.commander = new VelocityCommander();
+        this.commander = com;
 
         setRedAlliance(redAlliance);
 

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

@@ -66,10 +66,6 @@ public class SpindexerTransferIntake {
 
     public void update() {
 
-        if (mode == SpindexerMode.RAPID && rapidMode == RapidMode.INTAKE){
-            robot.setTransferPower(-0.7);
-        }
-
         switch (mode) {
 
             case RAPID:
@@ -89,6 +85,7 @@ public class SpindexerTransferIntake {
                         robot.setSpinPos(
                                 ServoPositions.spindexer_A2
                         );
+                        robot.setTransferPower(-0.7);
                         robot.setTransferServoPos(
                                 ServoPositions.transferServo_out
                         );
@@ -106,6 +103,7 @@ public class SpindexerTransferIntake {
                         if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                             setRapidMode(RapidMode.BEFORE_PULSE_OUT);
                         }
+                        robot.setTransferPower(-0.3);
 
                         break;
 
@@ -163,6 +161,8 @@ public class SpindexerTransferIntake {
                             setRapidMode(RapidMode.SHOOT);
                         }
 
+                        robot.setTransferPower(commander.getTransferPow());
+
                         break;
 
                     case SHOOT:
@@ -173,6 +173,9 @@ public class SpindexerTransferIntake {
                         if (stateTime() >= 400) {
                             setRapidMode(RapidMode.INTAKE);
                         }
+
+                        robot.setTransferPower(commander.getTransferPow());
+
                         break;
                 }
                 break;

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

@@ -8,6 +8,7 @@ public class VelocityCommander {
     private final double yAccK = 0; // TODO: Tune
     private double hoodPos = 0.88;
     private double transferPow = -1;
+    private double veloRPM = 2000;
 
     public VelocityCommander() {}
 
@@ -46,7 +47,7 @@ public class VelocityCommander {
     }
 
     private final double hoodA = -4.3276177*Math.pow(10, -13);
-    private final double hoodB = 2.68062979*Math.pow(10, -11);
+    private final double hoodB = 2.68062979*Math.pow(10, -10);
     private final double hoodC = -7.12859632*Math.pow(10, -8);
     private final double hoodD = 0.0000106010785;
     private final double hoodE = -0.000960693973;
@@ -73,6 +74,9 @@ public class VelocityCommander {
             hoodPos = Math.max(0.35, Math.min(0.88, hoodPos));
         }
     }
+    public double getHoodPredicted(){
+        return hoodPos;
+    }
 
     private void distToTransferPow(double dist){
         if (dist < 118){
@@ -83,13 +87,8 @@ public class VelocityCommander {
             transferPow = -0.5;
         }
     }
-
     public double getTransferPow(){return transferPow;}
 
-    public double getHoodPredicted(){
-        return hoodPos;
-    }
-
     // 27
     public double getVeloStationary (double distance){
         return distToRPM(distance);
@@ -105,9 +104,12 @@ public class VelocityCommander {
         predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight);
 
         distToHood(predictedDist);
-
+        distToTransferPow(predictedDist);
-        return distToRPM(predictedDist);
+        veloRPM = distToRPM(predictedDist);
+        return veloRPM;
     }
 
+    public double getPredictedRPM(){return veloRPM;}
+
     public double getDistance(){return predictedDist;}
 }