Added shooter class to manager flywheel turret and targetter

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

@@ -26,9 +26,9 @@ public class Flywheel {
 
     private final LinkedList<Double> velocityHistory = new LinkedList<>();
 
-    public Flywheel(HardwareMap hardwareMap) {
+    public Flywheel(Robot rob) {
 
-        robot = new Robot(hardwareMap);
+        robot = rob;
         shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
         shooterPIDF2 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
     }

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

@@ -1,5 +1,162 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
 public class Shooter {
 
+    Robot robot;
+    Flywheel fly;
+    Turret turr;
+    VelocityCommander commander;
+
+    double goalX = 0.0;
+    double goalY = 0.0;
+    double obeliskX = 72;
+    double obeliskY = 144;
+
+    private boolean red = true;
+
+
+    Follower follow;
+
+    public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance) {
+        this.robot = rob;
+        this.fly = new Flywheel(rob);
+        this.turr = new Turret(rob);
+        this.follow = follower;
+        this.commander = new VelocityCommander();
+
+        setRedAlliance(redAlliance);
+
+        if (redAlliance) {
+            goalX = 144;
+            goalY = 144;
+        } else {
+            goalX = 0;
+            goalY = 144;
+        }
+    }
+
+    public void setRedAlliance(boolean input) {
+        this.red = input;
+    }
+
+    private double flywheelVelocity = 0.0;
+    private double turretPosition = 0.5;
+
+    enum ShooterState {
+        READ_OBELISK,
+        TRACK_GOAL,
+        MANUAL_FLYWHEEL_TRACK_TURR,
+        MANUAL_TURRET_TRACK_FLY,
+        MANUAL,
+        NOTHING
+    }
+
+
+    private ShooterState state = ShooterState.NOTHING;
+
+    public void setState(ShooterState shooterState) {
+        this.state = shooterState;
+    }
+
+    public void setTurretPosition(double input) {
+        this.turretPosition = input;
+    }
+
+    public void setFlywheelVelocity(double input) {
+        this.flywheelVelocity = input;
+    }
+
+    public int getObeliskID() {
+        return turr.getObeliskID();
+    }
+
+
+    public void update() {
+
+        switch (state) {
+            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),
+                        follow.getHeading(),
+                        follow.getAngularVelocity(),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+
+                flywheelVelocity = commander.getVeloPredictive(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        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),
+                        follow.getHeading()
+                );
+
+                flywheelVelocity = commander.getVeloPredictive(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        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),
+                        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),
+                        follow.getHeading(),
+                        follow.getAngularVelocity(),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+                fly.manageFlywheel(flywheelVelocity);
+                break;
+
+        }
+
+    }
+
 }

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

@@ -18,7 +18,6 @@ public class Turret {
     private final double turretMin = 0.04; //TODO: Tune
     private final double turretMax = 0.94; //TODO: Tune
     private final double hVelK = 0; // TODO: Tune
-    private final double hAccK = 0; // TODO: Tune
     private final double xVelK = 0; // TODO: Tune
     private final double xAccK = 0; // TODO: Tune
     private final double yVelK = 0; // TODO: Tune
@@ -81,14 +80,19 @@ public class Turret {
         return obeliskID;
     }
 
+    public void manual (double pos) {
+        robot.turr1.setPosition(pos);
+        robot.turr2.setPosition(pos);
+    }
 
-    public void trackGoal(double dx, double dy, double h, double hVel, double hAcc, double xVel, double xAcc, double yVel, double yAcc) {
+
+    public void trackGoal(double dx, double dy, double h, double hVel, double xVel, double xAcc, double yVel, double yAcc) {
         // dx, dy, dz is target - robot
         // h is the raw heading where 0 degrees is positive x in the system of x, y
 
         double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot
         double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK);  // Negative bc dy = target - robot
-        double predictedH = h + (hVel * hVelK) + (0.5 * hAcc * hAccK); // Positive bc h = robot heading
+        double predictedH = h + (hVel * hVelK); // Positive bc h = robot heading
 
         predictedH = wrapAngle(predictedH);
 

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

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+public class VelocityCommander {
+    private final double goalH = 20.0; //TODO: Tune
+    private final double xVelK = 0; // TODO: Tune
+    private final double xAccK = 0; // TODO: Tune
+    private final double yVelK = 0; // TODO: Tune
+    private final double yAccK = 0; // TODO: Tune
+
+    public VelocityCommander() {
+
+    }
+
+    private double distToRPM (double dist){
+        return Math.sqrt(dist*dist + goalH*goalH);
+        //TODO: Add regression here using goalH
+    }
+
+    public double getVeloStationary (double distance){
+        return distToRPM(distance);
+    }
+
+    public double getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc) {
+
+        double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot
+        double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK);  // Negative bc dy = target - robot
+
+        double predictedDist = Math.sqrt(dx*dx + dy*dy);
+
+        return distToRPM(predictedDist);
+    }
+
+
+}