teleop almost there

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

@@ -27,6 +27,9 @@ public class TeleopV4 extends LinearOpMode {
     SpindexerTransferIntake spindexerTransferIntake;
     Turret turret;
     Flywheel flywheel;
+    VelocityCommander commander;
+
+    ParkTilter parkTilter;
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -39,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));
@@ -47,10 +51,11 @@ 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 = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
         shooter.setState(Shooter.ShooterState.TRACK_GOAL);
-        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE);
+        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
         spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
 
 
@@ -59,9 +64,7 @@ public class TeleopV4 extends LinearOpMode {
         if (isStopRequested()) return;
 
         while (opModeIsActive()) {
-
             //Drivetrain
-
             drivetrain.drive(
                     -gamepad1.right_stick_y,
                     gamepad1.right_stick_x,
@@ -102,6 +105,17 @@ public class TeleopV4 extends LinearOpMode {
                 );
             }
 
+            if (gamepad1.dpad_down){
+                parkTilter.park();
+            } else if (gamepad1.dpad_up) {
+                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

@@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.tests;
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
@@ -12,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
@@ -20,7 +23,10 @@ public class NewShooterTest extends LinearOpMode {
     Robot robot;
     Flywheel flywheel;
     Turret turret;
+    Shooter shooter;
     MultipleTelemetry TELE;
+    Follower follower;
+    VelocityCommander commander;
 
 
     public static boolean intake = true;
@@ -32,7 +38,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,
@@ -52,17 +58,21 @@ public class NewShooterTest extends LinearOpMode {
 
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
 
+        follower = Constants.createFollower(hardwareMap);
+        follower.setStartingPose(new Pose(72, 72, 0));
 
         flywheel = new Flywheel(robot);
         turret = new Turret(robot);
+        commander = new VelocityCommander();
 
-        Shooter shooter = new Shooter(
+        shooter = new Shooter(
                 robot,
                 TELE,
-                Constants.createFollower(hardwareMap),
+                follower,
                 true,
                 turret,
-                flywheel
+                flywheel,
+                commander
         );
 
         shooter.setState(Shooter.ShooterState.MANUAL);
@@ -73,6 +83,8 @@ public class NewShooterTest extends LinearOpMode {
 
         while (opModeIsActive()) {
 
+            follower.update();
+
             robot.setHoodPos(hoodPos);
             shooter.setTurretPosition(turretPos);
             shooter.setFlywheelVelocity(flywheel_velo);
@@ -136,9 +148,10 @@ 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", flywheel.getAverageVelocity());
+            TELE.addData("Flywheel Average Velocity", flywheel.getAverageVelocity());
             TELE.addData("PIDF Coefficients", Flywheel.shooterPIDF);
             TELE.addData("Power", flywheel.getShooterPower());
+            TELE.addData("Distance", shooter.getDistance());
             TELE.update();
 
             shooter.update();

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_I = 0.0;
+    public static double shooterPIDF_P = 500;
+    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;
@@ -128,5 +128,7 @@ public class Flywheel {
 
         steady = (Math.abs(commandedVelocity - averageVelocity) < 50);
     }
+
+
     public double getShooterPower(){return power;}
 }

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/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

@@ -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);
 
@@ -74,20 +74,29 @@ public class Shooter {
         return turr.getObeliskID();
     }
 
-
+    private final double shooterDistFromCenter = 1.545;
     public void update() {
 
         switch (state) {
             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())),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+
                 fly.manageFlywheel(flywheelVelocity);
                 turr.manual(turretPosition);
                 break;
             case TRACK_GOAL:
                 turr.trackGoal(
-                        (goalX - follow.getPose().getX()),
-                        (goalY - follow.getPose().getY()),
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getHeading(),
                         follow.getAngularVelocity(),
                         follow.getVelocity().getXComponent(),
@@ -97,26 +106,27 @@ public class Shooter {
                 );
 
                 flywheelVelocity = commander.getVeloPredictive(
-                        (goalX - follow.getPose().getX()),
-                        (goalY - follow.getPose().getY()),
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getVelocity().getXComponent(),
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent()
                 );
 
+                robot.setHoodPos(commander.getHoodPredicted());
                 fly.manageFlywheel(flywheelVelocity);
                 break;
             case READ_OBELISK:
                 turr.trackObelisk(
-                        (goalX - follow.getPose().getX()),
-                        (goalY - follow.getPose().getY()),
+                        (obeliskX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (obeliskY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getHeading()
                 );
 
                 flywheelVelocity = commander.getVeloPredictive(
-                        (goalX - follow.getPose().getX()),
-                        (goalY - follow.getPose().getY()),
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getVelocity().getXComponent(),
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
@@ -129,21 +139,22 @@ public class Shooter {
             case MANUAL_TURRET_TRACK_FLY:
                 turr.manual(turretPosition);
                 flywheelVelocity = commander.getVeloPredictive(
-                        (goalX - follow.getPose().getX()),
-                        (goalY - follow.getPose().getY()),
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getVelocity().getXComponent(),
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent()
                 );
+                robot.setHoodPos(commander.getHoodPredicted());
 
                 fly.manageFlywheel(flywheelVelocity);
                 break;
 
             case MANUAL_FLYWHEEL_TRACK_TURR:
                 turr.trackGoal(
-                        (goalX - follow.getPose().getX()),
-                        (goalY - follow.getPose().getY()),
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getHeading(),
                         follow.getAngularVelocity(),
                         follow.getVelocity().getXComponent(),
@@ -158,4 +169,7 @@ public class Shooter {
 
     }
 
+    public double getDistance(){return commander.getDistance();}
+
+
 }

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;
@@ -74,13 +79,13 @@ public class SpindexerTransferIntake {
                     case INTAKE:
 
                         robot.setIntakePower(1);
-                        robot.setTransferPower(-0.7);
                         robot.setRapidFireBlockerPos(
                                 ServoPositions.rapidFireBlocker_Closed
                         );
                         robot.setSpinPos(
                                 ServoPositions.spindexer_A2
                         );
+                        robot.setTransferPower(-0.7);
                         robot.setTransferServoPos(
                                 ServoPositions.transferServo_out
                         );
@@ -95,11 +100,10 @@ public class SpindexerTransferIntake {
 
                     case TRANSFER_OFF:
 
-                        robot.setTransferPower(-0.7);
-
                         if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                             setRapidMode(RapidMode.BEFORE_PULSE_OUT);
                         }
+                        robot.setTransferPower(-0.3);
 
                         break;
 
@@ -157,6 +161,8 @@ public class SpindexerTransferIntake {
                             setRapidMode(RapidMode.SHOOT);
                         }
 
+                        robot.setTransferPower(commander.getTransferPow());
+
                         break;
 
                     case SHOOT:
@@ -167,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/Turret.java

@@ -83,7 +83,7 @@ public class Turret {
         robot.setTurretPos(pos);
 
     }
-
+    // 1.545
 
     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

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

@@ -6,29 +6,110 @@ 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;
+    private double veloRPM = 2000;
 
-    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;
-        //TODO: Add regression here using goalH
+        double velo = 0;
+        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, -10);
+    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));
+        }
+    }
+    public double getHoodPredicted(){
+        return 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;}
+
+    // 27
     public double getVeloStationary (double distance){
         return distToRPM(distance);
     }
 
+    private final double goalHeight = 28;
+    double predictedDist = 0;
     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);
+        predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight);
 
-        return distToRPM(predictedDist);
+        distToHood(predictedDist);
+        distToTransferPow(predictedDist);
+        veloRPM = distToRPM(predictedDist);
+        return veloRPM;
     }
 
+    public double getPredictedRPM(){return veloRPM;}
 
+    public double getDistance(){return predictedDist;}
 }