Merge remote-tracking branch 'origin/danielv5' into update-teleop

# 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,6 +25,8 @@ public class TeleopV4 extends LinearOpMode {
     MultipleTelemetry TELE;
     Follower follower;
     SpindexerTransferIntake spindexerTransferIntake;
+    Turret turret;
+    Flywheel flywheel;
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -40,7 +42,11 @@ 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);
+
+
+        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);

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);
+            shooter.setFlywheelVelocity(flywheel_velo);
-            rpmFlywheel.manageFlywheel(shooter.getFlywheelVelocity());
             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/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,6 +81,7 @@ public class Shooter {
             case NOTHING:
                 break;
             case MANUAL:
+                fly.manageFlywheel(flywheelVelocity);
                 turr.manual(turretPosition);
                 break;
             case TRACK_GOAL:
@@ -101,6 +105,7 @@ public class Shooter {
                         follow.getAcceleration().getYComponent()
                 );
 
+                fly.manageFlywheel(flywheelVelocity);
                 break;
             case READ_OBELISK:
                 turr.trackObelisk(
@@ -118,6 +123,7 @@ public class Shooter {
                         follow.getAcceleration().getYComponent()
                 );
 
+                fly.manageFlywheel(flywheelVelocity);
                 break;
 
             case MANUAL_TURRET_TRACK_FLY:
@@ -131,6 +137,7 @@ public class Shooter {
                         follow.getAcceleration().getYComponent()
                 );
 
+                fly.manageFlywheel(flywheelVelocity);
                 break;
 
             case MANUAL_FLYWHEEL_TRACK_TURR:
@@ -144,6 +151,7 @@ public class Shooter {
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent()
                 );
+                fly.manageFlywheel(flywheelVelocity);
                 break;
 
         }