added regression

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

@@ -27,6 +27,7 @@ public class TeleopV4 extends LinearOpMode {
     SpindexerTransferIntake spindexerTransferIntake;
     Turret turret;
     Flywheel flywheel;
+    VelocityCommander commander;
 
     ParkTilter parkTilter;
 
@@ -41,6 +42,7 @@ public class TeleopV4 extends LinearOpMode {
                 FtcDashboard.getInstance().getTelemetry(), telemetry
         );
 
+        commander = new VelocityCommander();
         drivetrain = new Drivetrain(robot, TELE);
         follower = Constants.createFollower(hardwareMap);
         Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
@@ -51,10 +53,9 @@ public class TeleopV4 extends LinearOpMode {
 
         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 = new SpindexerTransferIntake(robot, TELE, commander);
         spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
 
 

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

@@ -36,7 +36,7 @@ public class NewShooterTest extends LinearOpMode {
     public static double hoodPos = 0.51;
     public static double flywheel_velo = 0;
 
-    public static double shooterP = 255, shooterI = 0, shooterD = 0, shooterF = 75;
+    public static double shooterP = 500, shooterI = 1, shooterD = 0, shooterF = 93;
 
     private enum ShootState {
         IDLE,

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

@@ -14,10 +14,10 @@ public class Flywheel {
 
 //    public PIDFCoefficients shooterPIDF1, shooterPIDF2;
     public static PIDFCoefficients shooterPIDF;
-    public static double shooterPIDF_P = 255;
+    public static double shooterPIDF_P = 500;
-    public static double shooterPIDF_I = 0.0;
+    public static double shooterPIDF_I = 1;
     public static double shooterPIDF_D = 0.0;
-    public static double shooterPIDF_F = 75;
+    public static double shooterPIDF_F = 93;
 
     private double velo = 0.0;
     private double velo1 = 0.0;

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

@@ -169,7 +169,7 @@ public class Robot {
     // Voids below are used to minimize hardware calls to minimize loop times
 
     // Used to cut off digits that are negligible
-    private final int maxDigits = 5;
+    private final int maxDigits = 3;
     private final int roundingFactor = (int) Math.pow(10, maxDigits);
 
     private double prevFrontLeftPower = -10.501;

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

@@ -81,7 +81,6 @@ public class Shooter {
             case NOTHING:
                 break;
             case MANUAL:
-
                 commander.getVeloPredictive(
                         (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
                         (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
@@ -115,6 +114,7 @@ public class Shooter {
                         follow.getAcceleration().getYComponent()
                 );
 
+                robot.setHoodPos(commander.getHoodPredicted());
                 fly.manageFlywheel(flywheelVelocity);
                 break;
             case READ_OBELISK:
@@ -146,6 +146,7 @@ public class Shooter {
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent()
                 );
+                robot.setHoodPos(commander.getHoodPredicted());
 
                 fly.manageFlywheel(flywheelVelocity);
                 break;

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

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import android.health.connect.datatypes.units.Velocity;
+
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
@@ -9,8 +11,11 @@ public class SpindexerTransferIntake {
 
     private final Robot robot;
 
-    public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE) {
+    VelocityCommander commander;
+
+    public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE, VelocityCommander com) {
         this.robot = rob;
+        this.commander = com;
     }
 
     private final double sensorDistanceThreshold = 6.0;
@@ -61,6 +66,10 @@ public class SpindexerTransferIntake {
 
     public void update() {
 
+        if (mode == SpindexerMode.RAPID && rapidMode == RapidMode.INTAKE){
+            robot.setTransferPower(-0.7);
+        }
+
         switch (mode) {
 
             case RAPID:
@@ -74,7 +83,6 @@ public class SpindexerTransferIntake {
                     case INTAKE:
 
                         robot.setIntakePower(1);
-                        robot.setTransferPower(-0.7);
                         robot.setRapidFireBlockerPos(
                                 ServoPositions.rapidFireBlocker_Closed
                         );
@@ -95,8 +103,6 @@ public class SpindexerTransferIntake {
 
                     case TRANSFER_OFF:
 
-                        robot.setTransferPower(-0.7);
-
                         if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                             setRapidMode(RapidMode.BEFORE_PULSE_OUT);
                         }

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

@@ -6,15 +6,90 @@ public class VelocityCommander {
     private final double xAccK = 0; // TODO: Tune
     private final double yVelK = 0; // TODO: Tune
     private final double yAccK = 0; // TODO: Tune
+    private double hoodPos = 0.88;
+    private double transferPow = -1;
 
-    public VelocityCommander() {
+    public VelocityCommander() {}
-
-    }
 
+    private final double veloA = -2.703087757*Math.pow(10, -14);
+    private final double veloB = 2.904756341*Math.pow(10, -11);
+    private final double veloC = -1.381814293*Math.pow(10, -8);
+    private final double veloD = 0.000003829224585;
+    private final double veloE = -0.000684090204;
+    private final double veloF = 0.0822754689;
+    private final double veloG = -6.743119277;
+    private final double veloH = 371.7359504;
+    private final double veloI = -13189.70958;
+    private final double veloJ = 272005.7124;
+    private final double veloK = -2474581.713;
     private double distToRPM (double dist){
-        return Math.sqrt(dist*dist + goalH*goalH) * 20;
+        double velo = 0;
-        //TODO: Add regression here using goalH
+        if (dist < 49) {
+            velo = 2000;
+        } else if (dist > 165){
+            velo = 3760;
+        } else {
+            velo = veloA*Math.pow(dist, 10) +
+                    veloB*Math.pow(dist, 9) +
+                    veloC*Math.pow(dist, 8) +
+                    veloD*Math.pow(dist, 7) +
+                    veloE*Math.pow(dist, 6) +
+                    veloF*Math.pow(dist, 5) +
+                    veloG*Math.pow(dist, 4) +
+                    veloH*Math.pow(dist, 3) +
+                    veloI*Math.pow(dist, 2) +
+                    veloJ*Math.pow(dist, 1) +
+                    veloK;
+            velo = Math.max(2000, Math.min(3760, velo));
+        }
+        return velo;
     }
+
+    private final double hoodA = -4.3276177*Math.pow(10, -13);
+    private final double hoodB = 2.68062979*Math.pow(10, -11);
+    private final double hoodC = -7.12859632*Math.pow(10, -8);
+    private final double hoodD = 0.0000106010785;
+    private final double hoodE = -0.000960693973;
+    private final double hoodF = 0.0540375808;
+    private final double hoodG = -1.82724027;
+    private final double hoodH = 33.4797545;
+    private final double hoodI = -246.888632;
+    private void distToHood (double dist){
+        if (dist > 112){
+            hoodPos = 0.35;
+        } else if (dist < 49){
+            hoodPos = 0.88;
+        } else {
+            hoodPos = hoodA*Math.pow(dist, 8) +
+                    hoodB*Math.pow(dist, 7) +
+                    hoodC*Math.pow(dist, 6) +
+                    hoodD*Math.pow(dist, 5) +
+                    hoodE*Math.pow(dist, 4) +
+                    hoodF*Math.pow(dist, 3) +
+                    hoodG*Math.pow(dist, 2) +
+                    hoodH*Math.pow(dist, 1) +
+                    hoodI;
+
+            hoodPos = Math.max(0.35, Math.min(0.88, hoodPos));
+        }
+    }
+
+    private void distToTransferPow(double dist){
+        if (dist < 118){
+            transferPow = -1;
+        } else if (dist < 125){
+            transferPow = -0.7;
+        } else {
+            transferPow = -0.5;
+        }
+    }
+
+    public double getTransferPow(){return transferPow;}
+
+    public double getHoodPredicted(){
+        return hoodPos;
+    }
+
     // 27
     public double getVeloStationary (double distance){
         return distToRPM(distance);
@@ -29,6 +104,8 @@ public class VelocityCommander {
 
         predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight);
 
+        distToHood(predictedDist);
+
         return distToRPM(predictedDist);
     }