shooting is ok but NOT PERFECT

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

@@ -55,6 +55,7 @@ public class TeleopV4 extends LinearOpMode {
 
         shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
         shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+        shooter.setRedAlliance(Color.redAlliance);
         spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
         spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
 
@@ -73,7 +74,8 @@ public class TeleopV4 extends LinearOpMode {
 
             follower.update();
 
-            shooter.update();
+
+            shooter.update(robot.voltage.getVoltage());
             spindexerTransferIntake.update();
 
             SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
@@ -114,7 +116,8 @@ public class TeleopV4 extends LinearOpMode {
             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.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
+            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
 
             TELE.update();
         }

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

@@ -1,53 +1,38 @@
 package org.firstinspires.ftc.teamcode.tests;
 
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
+
 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;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
-import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
+import org.firstinspires.ftc.teamcode.utilsv2.*;
-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
+@Config
 public class NewShooterTest extends LinearOpMode {
-
     Robot robot;
-    Flywheel flywheel;
+    Drivetrain drivetrain;
-    Turret turret;
     Shooter shooter;
     MultipleTelemetry TELE;
     Follower follower;
+    SpindexerTransferIntake spindexerTransferIntake;
+    Turret turret;
+    Flywheel flywheel;
     VelocityCommander commander;
+    ParkTilter parkTilter;
 
-
+    public static int flywheelVelo = 0;
-    public static boolean intake = true;
+    public static double hoodPos = 0.5;
-    public static boolean shoot = false;
+//    public static double turretPos = 0.51;
-    public static double intakePower = 1.0;
-    public static double transferShootPower = -1;
-    public static double transferIntakePower = -1;
-    public static double turretPos = 0.51;
-    public static double hoodPos = 0.51;
-    public static double flywheel_velo = 0;
-
-    public static double shooterP = 500, shooterI = 1, shooterD = 0, shooterF = 93;
-
-    private enum ShootState {
-        IDLE,
-        WAIT_GATE,
-        WAIT_PUSH
-    }
-
-    private ShootState shootState = ShootState.IDLE;
-    private long timestamp = 0;
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -56,105 +41,90 @@ public class NewShooterTest extends LinearOpMode {
 
         robot = Robot.getInstance(hardwareMap);
 
-        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        TELE = new MultipleTelemetry(
+                FtcDashboard.getInstance().getTelemetry(), telemetry
+        );
 
+        commander = new VelocityCommander();
+        drivetrain = new Drivetrain(robot, TELE);
         follower = Constants.createFollower(hardwareMap);
-        follower.setStartingPose(new Pose(72, 72, 0));
+        Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
+        follower.setStartingPose(start);
 
         flywheel = new Flywheel(robot);
         turret = new Turret(robot);
-        commander = new VelocityCommander();
 
-        shooter = new Shooter(
+        parkTilter = new ParkTilter(robot);
-                robot,
-                TELE,
-                follower,
-                true,
-                turret,
-                flywheel,
-                commander
-        );
 
-        shooter.setState(Shooter.ShooterState.MANUAL);
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
+        shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+        shooter.setRedAlliance(Color.redAlliance);
+        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
+        spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
 
         waitForStart();
 
         if (isStopRequested()) return;
 
         while (opModeIsActive()) {
+            //Drivetrain
+            drivetrain.drive(
+                    -gamepad1.right_stick_y,
+                    gamepad1.right_stick_x,
+                    gamepad1.left_stick_x
+            );
 
             follower.update();
 
+            shooter.setFlywheelVelocity(flywheelVelo);
             robot.setHoodPos(hoodPos);
-            shooter.setTurretPosition(turretPos);
+//            shooter.setTurretPosition(turretPos);
-            shooter.setFlywheelVelocity(flywheel_velo);
+            shooter.update(robot.voltage.getVoltage());
-            double voltage = robot.voltage.getVoltage();
+            spindexerTransferIntake.update();
-            flywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage);
 
-            robot.setSpinPos(ServoPositions.spindexer_A2);
+            SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
 
-            if (intake && !shoot) {
+            if (gamepad1.leftBumperWasPressed() &&
+                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
+                            state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
+                            state == SpindexerTransferIntake.RapidMode.PULSE_OUT ||
+                            state == SpindexerTransferIntake.RapidMode.PULSE_IN ||
+                            state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
 
-                shootState = ShootState.IDLE;
+                spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
-
-                robot.setRapidFireBlockerPos(
-                        ServoPositions.rapidFireBlocker_Closed);
-
-                robot.setTransferPower(transferIntakePower);
-                robot.setIntakePower(intakePower);
-                robot.setTransferServoPos(
-                        ServoPositions.transferServo_out);
-            } else if (shoot) {
-                robot.setIntakePower(intakePower);
-
-
-                switch (shootState) {
-
-                    case IDLE:
-
-                        robot.setTransferPower(transferShootPower);
-
-                        timestamp = System.currentTimeMillis();
-                        shootState = ShootState.WAIT_GATE;
-
-                        break;
-
-                    case WAIT_GATE:
-
-                        if (System.currentTimeMillis() - timestamp >= 300) {
-
-                            robot.setRapidFireBlockerPos(
-                                    ServoPositions.rapidFireBlocker_Open);
-
-                            timestamp = System.currentTimeMillis();
-                            shootState = ShootState.WAIT_PUSH;
             }
 
-                        break;
+            if (gamepad1.right_trigger > 0.5 &&
+                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
 
-                    case WAIT_PUSH:
+                spindexerTransferIntake.setRapidMode(
+                        SpindexerTransferIntake.RapidMode.HOLD_BALLS
+                );
+            }
+            if (gamepad1.rightBumperWasPressed()
+                    && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
 
-                        if (System.currentTimeMillis() - timestamp >= 100) {
+                spindexerTransferIntake.setRapidMode(
-
+                        SpindexerTransferIntake.RapidMode.INTAKE
-                            robot.setTransferServoPos(
+                );
-                                    ServoPositions.transferServo_in);
-
-                            shootState = ShootState.IDLE;
             }
 
-                        break;
+            if (gamepad1.dpad_down){
-                }
+                parkTilter.park();
+            } else if (gamepad1.dpad_up) {
+                parkTilter.unpark();
             }
 
-            TELE.addData("Flywheel Velocity1", (robot.shooter1.getVelocity() * 60) / 28);
+            TELE.addData("Distance From Goal", commander.getDistance());
-            TELE.addData("Flywheel Velocity2", (robot.shooter2.getVelocity() * 60) / 28);
+            TELE.addData("Hood Position", commander.getHoodPredicted());
-            TELE.addData("Flywheel Average Velocity", flywheel.getAverageVelocity());
+            TELE.addData("Transfer Power", robot.transfer.getPower());
-            TELE.addData("PIDF Coefficients", Flywheel.shooterPIDF);
+            TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
-            TELE.addData("Power", flywheel.getShooterPower());
+            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
-            TELE.addData("Distance", shooter.getDistance());
+
             TELE.update();
-
-            shooter.update();
         }
+
     }
 }

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

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -9,6 +10,7 @@ import org.firstinspires.ftc.teamcode.utilsv2.Robot;
 
 import java.util.LinkedList;
 
+@Config
 public class Flywheel {
     Robot robot;
 
@@ -70,8 +72,9 @@ public class Flywheel {
 
     private double prevF = 0;
 
-    public static double voltagePIDFDifference = 0.8;
+    public static double voltagePIDFDifference = 1;
-    public void setF(double f){
+    public void setF(double voltage){
+        double f = shooterPIDF_F / voltage;
         if (Math.abs(prevF - f) > voltagePIDFDifference) {
             shooterPIDF.f = f;
             robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);

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

@@ -1,9 +1,12 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import static org.firstinspires.ftc.teamcode.utilsv2.Flywheel.*;
+
+import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.pedropathing.follower.Follower;
 
-
+@Config
 public class Shooter {
 
     Robot robot;
@@ -15,6 +18,8 @@ public class Shooter {
     double goalY = 0.0;
     double obeliskX = 72;
     double obeliskY = 144;
+    double turretGoalX = 0;
+    double turretGoalY = 0;
 
     private boolean red = true;
 
@@ -37,10 +42,13 @@ public class Shooter {
 
         if (this.red) {
             goalX = 144;
+            turretGoalX = 136;
         } else {
             goalX = 0;
+            turretGoalX = 8;
         }
         goalY = 144;
+        turretGoalY = 136;
     }
 
     private double flywheelVelocity = 0.0;
@@ -75,7 +83,7 @@ public class Shooter {
     }
 
     private final double shooterDistFromCenter = 1.545;
-    public void update() {
+    public void update(double voltage) {
 
         switch (state) {
             case NOTHING:
@@ -87,16 +95,18 @@ public class Shooter {
                         follow.getVelocity().getXComponent(),
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
-                        follow.getAcceleration().getYComponent()
+                        follow.getAcceleration().getYComponent(),
+                        voltage
                 );
 
                 fly.manageFlywheel(flywheelVelocity);
+                fly.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / voltage);
                 turr.manual(turretPosition);
                 break;
             case TRACK_GOAL:
                 turr.trackGoal(
-                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (turretGoalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
-                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
+                        (turretGoalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getHeading(),
                         follow.getAngularVelocity(),
                         follow.getVelocity().getXComponent(),
@@ -105,17 +115,21 @@ public class Shooter {
                         follow.getAcceleration().getYComponent()
                 );
 
-                flywheelVelocity = commander.getVeloPredictive(
+                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()
+                        follow.getAcceleration().getYComponent(),
+                        voltage
                 );
 
+                flywheelVelocity = commander.getPredictedRPM();
+
                 robot.setHoodPos(commander.getHoodPredicted());
                 fly.manageFlywheel(flywheelVelocity);
+                fly.setF(voltage);
                 break;
             case READ_OBELISK:
                 turr.trackObelisk(
@@ -124,28 +138,35 @@ public class Shooter {
                         follow.getHeading()
                 );
 
-                flywheelVelocity = commander.getVeloPredictive(
+                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()
+                        follow.getAcceleration().getYComponent(),
+                        voltage
                 );
 
+                flywheelVelocity = commander.getPredictedRPM();
+
                 fly.manageFlywheel(flywheelVelocity);
+                fly.setF(voltage);
                 break;
 
             case MANUAL_TURRET_TRACK_FLY:
                 turr.manual(turretPosition);
-                flywheelVelocity = commander.getVeloPredictive(
+                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()
+                        follow.getAcceleration().getYComponent(),
+                        voltage
                 );
+
+                flywheelVelocity = commander.getPredictedRPM();
                 robot.setHoodPos(commander.getHoodPredicted());
 
                 fly.manageFlywheel(flywheelVelocity);
@@ -153,8 +174,8 @@ public class Shooter {
 
             case MANUAL_FLYWHEEL_TRACK_TURR:
                 turr.trackGoal(
-                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (turretGoalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
-                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
+                        (turretGoalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getHeading(),
                         follow.getAngularVelocity(),
                         follow.getVelocity().getXComponent(),
@@ -162,7 +183,18 @@ public class Shooter {
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent()
                 );
+                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(),
+                        voltage
+                );
                 fly.manageFlywheel(flywheelVelocity);
+                fly.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / voltage);
+                fly.setF(voltage);
                 break;
 
         }

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

@@ -2,11 +2,13 @@ package org.firstinspires.ftc.teamcode.utilsv2;
 
 import android.health.connect.datatypes.units.Velocity;
 
+import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 
+@Config
 public class SpindexerTransferIntake {
 
     private final Robot robot;

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

@@ -1,14 +1,16 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
 public class VelocityCommander {
-    private final double goalH = 20.0; //TODO: Tune
+    public static double xVelK = 0.05; // TODO: Tune
-    private final double xVelK = 0; // TODO: Tune
+    public static double xAccK = 0.025; // TODO: Tune
-    private final double xAccK = 0; // TODO: Tune
+    public static double yVelK = 0.05; // TODO: Tune
-    private final double yVelK = 0; // TODO: Tune
+    public static double yAccK = 0.025; // TODO: Tune
-    private final double yAccK = 0; // TODO: Tune
     private double hoodPos = 0.88;
     private double transferPow = -1;
-    private double veloRPM = 2000;
+    private double velo = 0;
 
     public VelocityCommander() {}
 
@@ -24,7 +26,6 @@ public class VelocityCommander {
     private final double veloJ = 272005.7124;
     private final double veloK = -2474581.713;
     private double distToRPM (double dist){
-        double velo = 0;
         if (dist < 49) {
             velo = 2000;
         } else if (dist > 165){
@@ -78,7 +79,7 @@ public class VelocityCommander {
         return hoodPos;
     }
 
-    private void distToTransferPow(double dist){
+    private void distToTransferPow(double dist, double voltage){
         if (dist < 118){
             transferPow = -1;
         } else if (dist < 125){
@@ -86,6 +87,8 @@ public class VelocityCommander {
         } else {
             transferPow = -0.5;
         }
+
+//        transferPow = Math.max(-0.5, Math.min(-1, transferPow * (14/voltage)));
     }
     public double getTransferPow(){return transferPow;}
 
@@ -94,22 +97,21 @@ public class VelocityCommander {
         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) {
+    public void getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc, double voltage) {
 
         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
 
-        predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight);
+        double goalHeight = 28;
+        predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight * goalHeight);
 
         distToHood(predictedDist);
-        distToTransferPow(predictedDist);
+        distToTransferPow(predictedDist, voltage);
-        veloRPM = distToRPM(predictedDist);
+        distToRPM(predictedDist);
-        return veloRPM;
     }
 
-    public double getPredictedRPM(){return veloRPM;}
+    public double getPredictedRPM(){return velo;}
 
     public double getDistance(){return predictedDist;}
 }