temp commit

src/main/java/team3647/frc2024/auto/AutoCommands.java

@@ -176,10 +176,6 @@ public double getPivotAngleByPose(Supplier<Pose2d> pose) {
         return targeting.getPivotAngleByPose(pose.get()) * 180 / Math.PI;
     }
 
-    public double getPivotAngle() {
-        return targeting.getPivotAngle(FieldConstants.kBlueSpeaker) * 180 / Math.PI;
-    }
-
     public Command target() {
         return Commands.run(() -> swerve.drive(0, 0, deeThetaOnTheMove()), swerve).withTimeout(2);
     }
@@ -211,12 +207,6 @@ public Command pathAndShootWithOverride(String path, Alliance color) {
                 superstructure.shootStow(), followChoreoPathWithOverride(path, color));
     }
 
-    public Command followChoreoPathWithOverrideAndPivot(String path, Alliance color) {
-        return Commands.deadline(
-                followChoreoPathWithOverride(path, color),
-                superstructure.pivotCommands.setAngle(() -> getPivotAngle()));
-    }
-
     public Command followChoreoPathWithOverrideFast(String path, Alliance color) {
         ChoreoTrajectory traj = Choreo.getTrajectory(path);
         boolean mirror = color == Alliance.Red;

src/main/java/team3647/frc2024/commands/ShooterCommands.java

@@ -10,6 +10,7 @@
 import java.util.Map;
 import java.util.Set;
 import java.util.function.DoubleSupplier;
+import team3647.frc2024.constants.ShooterConstants;
 import team3647.frc2024.subsystems.ShooterLeft;
 import team3647.frc2024.subsystems.ShooterRight;
 import team3647.lib.LinearRegression;
@@ -19,8 +20,9 @@ public class ShooterCommands {
     public Command openLoop(DoubleSupplier bill) {
         return Commands.run(
                 () -> {
-                    shooterRight.openLoop(bill.getAsDouble() * 0.8);
-                    shooterLeft.openLoop(MathUtil.clamp(bill.getAsDouble() * 1.2, 0, 1));
+                    shooterRight.openLoop(bill.getAsDouble() * ShooterConstants.kRightRatio);
+                    shooterLeft.openLoop(
+                            MathUtil.clamp(bill.getAsDouble() * ShooterConstants.kLeftRatio, 0, 1));
                 },
                 shooterRight,
                 shooterLeft);
@@ -29,8 +31,18 @@ public Command openLoop(DoubleSupplier bill) {
     public Command setVelocity(DoubleSupplier bill) {
         return Commands.run(
                 () -> {
-                    shooterRight.setVelocity(bill.getAsDouble() * 0.85);
-                    shooterLeft.setVelocity(bill.getAsDouble() * 1.15);
+                    shooterRight.setVelocity(bill.getAsDouble() * ShooterConstants.kRightRatio);
+                    shooterLeft.setVelocity(bill.getAsDouble() * ShooterConstants.kLeftRatio);
+                },
+                shooterRight,
+                shooterLeft);
+    }
+
+    public Command setVelocity(DoubleSupplier bill, DoubleSupplier ratio) {
+        return Commands.run(
+                () -> {
+                    shooterRight.setVelocity(bill.getAsDouble() * ratio.getAsDouble());
+                    shooterLeft.setVelocity(bill.getAsDouble() * (2 - ratio.getAsDouble()));
                 },
                 shooterRight,
                 shooterLeft);

src/main/java/team3647/frc2024/constants/PivotConstants.java

@@ -5,8 +5,11 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.*;
 import com.playingwithfusion.TimeOfFlight;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.math.util.Units;
@@ -34,7 +37,7 @@ public class PivotConstants {
     public static final double kMaxDegree = 61.2;
     public static final double kMaxDegreeUnderStage = 30;
 
-    private static final double masterKP = 1.5;
+    private static final double masterKP = 3;
     private static final double masterKI = 0;
     private static final double masterKD = 0;
 
@@ -51,6 +54,9 @@ public class PivotConstants {
                     new Translation3d(Units.inchesToMeters(0.3), 0, Units.inchesToMeters(17.2)),
                     new Rotation3d());
 
+    public static final Transform2d robotToPivot2d =
+            new Transform2d(new Translation2d(Units.inchesToMeters(0.3), 0), new Rotation2d());
+
     public static final TimeOfFlight tofBack =
             new TimeOfFlight(GlobalConstants.SensorIds.pivotBackId);
     public static final TimeOfFlight tofFront =

src/main/java/team3647/frc2024/constants/ShooterConstants.java

@@ -41,6 +41,9 @@ public class ShooterConstants {
     public static final double kStallCurrent = 55.0;
     public static final double kMaxCurrent = 40.0;
 
+    public static final double kLeftRatio = 1.5;
+    public static final double kRightRatio = 2 - kLeftRatio;
+
     static {
         Slot0Configs kRightSlot0 = new Slot0Configs();
         Slot0Configs kLeftSlot0 = new Slot0Configs();

src/main/java/team3647/frc2024/constants/VisionConstants.java

@@ -24,7 +24,7 @@ public class VisionConstants {
                             Units.inchesToMeters(8),
                             Units.inchesToMeters(8.7885)),
                     new Rotation3d(0, -Math.PI / 180 * 28.12, 0)
-                            .rotateBy(new Rotation3d(0, 0, Math.PI * 5 / 6 - 0.2)));
+                            .rotateBy(new Rotation3d(0, 0, Math.PI * 5 / 6 + 0.2)));
 
     public static final Transform3d robotToBackRight =
             new Transform3d(

src/main/java/team3647/frc2024/robot/RobotContainer.java

@@ -175,7 +175,7 @@ private void configureDefaultCommands() {
                         autoDrive::getMode,
                         autoDrive::getEnabled,
                         autoDrive::getVelocities));
-        pivot.setDefaultCommand(superstructure.pivotCommands.holdPositionAtCall());
+        pivot.setDefaultCommand(superstructure.prep());
         intake.setDefaultCommand(superstructure.intakeCommands.kill());
         kicker.setDefaultCommand(superstructure.kickerCommands.kill());
         wrist.setDefaultCommand(superstructure.wristCommands.holdPositionAtCall());
@@ -188,23 +188,25 @@ public void teleopInit() {}
     void configTestCommands() {}
 
     public void configureSmartDashboardLogging() {
-        SmartDashboard.putNumber("wrist kg", 0);
-        printer.addDouble("wanted pivot", superstructure::getWantedPivot);
-        printer.addDouble("back tof", pivot::tofBack);
-        printer.addBoolean("front tof", pivot::frontPiece);
-        printer.addDouble("wrist", wrist::getAngle);
-        printer.addDouble("pivot", pivot::getAngle);
-        printer.addBoolean("under stage", swerve::underStage);
-        printer.addBoolean("set piece", () -> setPiece.getAsBoolean());
-        SmartDashboard.putNumber("pivot interp", 35);
+        // SmartDashboard.putNumber("wrist kg", 0);
+        // printer.addDouble("wanted pivot", superstructure::getWantedPivot);
+        // printer.addDouble("back tof", pivot::tofBack);
+        // printer.addBoolean("front tof", pivot::frontPiece);
+        // printer.addDouble("wrist", wrist::getAngle);
+        // printer.addDouble("pivot", pivot::getAngle);
+        // printer.addBoolean("under stage", swerve::underStage);
+        // printer.addBoolean("set piece", () -> setPiece.getAsBoolean());
+        SmartDashboard.putNumber("pivot interp angle", 35);
         printer.addDouble("shooter distance squared", targetingUtil::distance);
-        printer.addBoolean("current sensing", () -> autoCommands.currentYes.getAsBoolean());
-        printer.addBoolean("wrist at stow", superstructure::wristAtStow);
-        printer.addDouble("climb len", () -> climb.getLength());
-        printer.addBoolean("shooter ready", superstructure::flywheelReadY);
-        printer.addBoolean("pivot ready", superstructure::pivotReady);
-        printer.addBoolean("swerve ready", superstructure::swerveReady);
-        printer.addDouble("tx", detector::getTX);
+        // printer.addBoolean("current sensing", () -> autoCommands.currentYes.getAsBoolean());
+        // printer.addBoolean("wrist at stow", superstructure::wristAtStow);
+        // printer.addDouble("climb len", () -> climb.getLength());
+        // printer.addBoolean("shooter ready", superstructure::flywheelReadY);
+        // printer.addBoolean("pivot ready", superstructure::pivotReady);
+        // printer.addBoolean("swerve ready", superstructure::swerveReady);
+        // printer.addDouble("tx", detector::getTX);
+        SmartDashboard.putNumber("shoot speed", 30);
+        SmartDashboard.putNumber("ratio", 1);
         // printer.addDouble("auto drive", () -> autoDrive.getVelocities().dtheta);
     }
 
@@ -307,7 +309,8 @@ public Command getAutonomousCommand() {
             new AprilTagPhotonVision(VisionConstants.right, VisionConstants.robotToRight);
 
     private final VisionController visionController =
-            new VisionController(swerve::addVisionData, swerve::shouldAddData, backRight);
+            new VisionController(
+                    swerve::addVisionData, swerve::shouldAddData, backLeft, backRight, right, left);
 
     //     private final LEDs LEDs = new LEDs(LEDConstants.m_candle);
 
@@ -319,7 +322,7 @@ public Command getAutonomousCommand() {
                     FieldConstants.kBlueAmp,
                     swerve::getOdoPose,
                     swerve::getChassisSpeeds,
-                    PivotConstants.robotToPivot);
+                    PivotConstants.robotToPivot2d);
 
     public final AutoDrive autoDrive = new AutoDrive(swerve, detector, targetingUtil);
 

src/main/java/team3647/frc2024/subsystems/Superstructure.java

@@ -10,6 +10,7 @@
 import team3647.frc2024.commands.PivotCommands;
 import team3647.frc2024.commands.ShooterCommands;
 import team3647.frc2024.commands.WristCommands;
+import team3647.frc2024.constants.ShooterConstants;
 
 public class Superstructure {
 
@@ -69,11 +70,12 @@ public Command feed() {
     }
 
     public Command spinUp() {
-        return shooterCommands.setVelocity(() -> shooterSpeedSupplier.getAsDouble());
+        return shooterCommands.setVelocity(
+                () -> getDesiredSpeed(), () -> ShooterConstants.kLeftRatio);
     }
 
     public Command spinUpAmp() {
-        return shooterCommands.setVelocity(() -> shooterSpeedSupplier.getAsDouble() * 5.5 / 25);
+        return shooterCommands.setVelocity(() -> shooterSpeedSupplier.getAsDouble() * 7 / 25);
     }
 
     public double getDesiredSpeed() {
@@ -101,7 +103,7 @@ public Command ejectPiece() {
     }
 
     public boolean flywheelReadY() {
-        return shooterLeft.velocityReached(shootSpeed * 1.15, 1);
+        return shooterLeft.velocityReached(30, 1);
     }
 
     public boolean pivotReady() {
@@ -117,14 +119,14 @@ public Command shoot() {
                 prep(),
                 spinUp(),
                 Commands.sequence(
-                        // Commands.waitSeconds(3),
+                        // Commands.waitSeconds(2.5),
                         Commands.waitUntil(
                                         () ->
-                                                shooterLeft.velocityReached(shootSpeed * 1.15, 1)
+                                                shooterLeft.velocityReached(30, 1)
                                                         && pivot.angleReached(
                                                                 pivotAngleSupplier.getAsDouble(), 5)
                                                         && swerveAimed.getAsBoolean())
-                                .withTimeout(0.7),
+                                .withTimeout(1.2),
                         feed()));
     }
 
@@ -133,11 +135,16 @@ public Command shootAmp() {
                 prep(),
                 spinUpAmp(),
                 Commands.sequence(
-                        // Commands.waitSeconds(3),
+                        // Commands.waitSeconds(2.5),
                         Commands.waitUntil(
                                         () ->
                                                 shooterLeft.velocityReached(
-                                                                shootSpeed * 5.5 / 25 * 1.15, 1)
+                                                                shootSpeed
+                                                                        * 7
+                                                                        / getDesiredSpeed()
+                                                                        * ShooterConstants
+                                                                                .kLeftRatio,
+                                                                1)
                                                         && pivot.angleReached(
                                                                 pivotAngleSupplier.getAsDouble(), 5)
                                                         && swerveAimed.getAsBoolean())
@@ -150,10 +157,10 @@ public Command batterShot() {
                 batterPrep(),
                 spinUp(),
                 Commands.sequence(
-                        // Commands.waitSeconds(3),
+                        Commands.waitSeconds(2.5),
                         Commands.waitUntil(
                                         () ->
-                                                shooterLeft.velocityReached(shootSpeed * 1.15, 1)
+                                                shooterLeft.velocityReached(30, 1)
                                                         && pivot.angleReached(60, 5))
                                 .withTimeout(0.5),
                         feed()));
@@ -172,14 +179,10 @@ public Command shootStow() {
     }
 
     public Command prep() {
-        // SmartDashboard.putNumber("pivot supplier", pivotAngleSupplier.getAsDouble());
-        // return pivotCommands.setAngle(() -> pivotAngleSupplier.getAsDouble());
-        return pivotCommands.setAngle(() -> pivotAngleSupplier.getAsDouble());
+        return pivotCommands.setAngle(() -> SmartDashboard.getNumber("pivot interp angle", 35));
     }
 
     public Command batterPrep() {
-        // SmartDashboard.putNumber("pivot supplier", pivotAngleSupplier.getAsDouble());
-        // return pivotCommands.setAngle(() -> pivotAngleSupplier.getAsDouble());
         return pivotCommands.setAngle(() -> 60);
     }
 

src/main/java/team3647/frc2024/subsystems/SwerveDrive.java

@@ -164,13 +164,13 @@ public void periodic() {
     }
 
     public boolean underStage() {
-        return ((getOdoPose().getX() > 3.5 && getOdoPose().getX() < 6.2)
-                        || (getOdoPose().getX() > 10.2 && getOdoPose().getX() < 12.9))
-                && ((Math.abs(getOdoPose().getY() - 4) < ((getOdoPose().getX() - 2.9) * 1 / 1.73)
-                                && getOdoPose().getX() < 6.2)
+        return ((getOdoPose().getX() > 3.2 && getOdoPose().getX() < 6.5)
+                        || (getOdoPose().getX() > 9.9 && getOdoPose().getX() < 13.3))
+                && ((Math.abs(getOdoPose().getY() - 4) < ((getOdoPose().getX() - 2.6) * 1 / 1.73)
+                                && getOdoPose().getX() < 6.5)
                         || (Math.abs(getOdoPose().getY() - 4)
-                                        < ((13.6 - getOdoPose().getX()) * 1 / 1.73)
-                                && getOdoPose().getX() > 10.2));
+                                        < ((13.9 - getOdoPose().getX()) * 1 / 1.73)
+                                && getOdoPose().getX() > 9.9));
     }
 
     public Command runDriveQuasiTest(Direction direction) {

src/main/java/team3647/frc2024/util/AprilTagCamera.java

@@ -6,4 +6,6 @@ public interface AprilTagCamera {
     public Optional<VisionMeasurement> QueueToInputs();
 
     public int getTagNum();
+
+    public String getName();
 }

src/main/java/team3647/frc2024/util/AprilTagPhotonVision.java

@@ -4,6 +4,7 @@
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import java.util.Optional;
 import org.photonvision.PhotonCamera;
@@ -17,6 +18,9 @@ public class AprilTagPhotonVision extends PhotonCamera implements AprilTagCamera
             AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
     PhotonPoseEstimator photonPoseEstimator;
     Transform3d robotToCam;
+    private final edu.wpi.first.math.Vector<N3> baseStdDevs = VecBuilder.fill(0.3, 0.3, 0.3);
+    private final edu.wpi.first.math.Vector<N3> multiStdDevs =
+            VecBuilder.fill(0.00096, 0.00096, 0.02979);
 
     public AprilTagPhotonVision(String camera, Transform3d robotToCam) {
         super(NetworkTableInstance.getDefault(), camera);
@@ -29,7 +33,7 @@ public AprilTagPhotonVision(String camera, Transform3d robotToCam) {
         this.robotToCam = robotToCam;
     }
 
-    public static AprilTagId getId(int id) {
+    public AprilTagId getId(int id) {
         var adjustedId = id - 1;
         var possibleValues = AprilTagId.values();
         if (adjustedId < 0 || adjustedId > possibleValues.length) {
@@ -39,6 +43,10 @@ public static AprilTagId getId(int id) {
         return possibleValues[adjustedId];
     }
 
+    public String getName() {
+        return this.getName();
+    }
+
     public Optional<VisionMeasurement> QueueToInputs() {
         var update = photonPoseEstimator.update();
         var result = this.getLatestResult();
@@ -51,10 +59,10 @@ public Optional<VisionMeasurement> QueueToInputs() {
                         .getTranslation()
                         .toTranslation2d()
                         .getNorm();
-        if (targetDistance > 2.5) {
+        if (targetDistance > 4) {
             return Optional.empty();
         }
-        final var stdDevs = VecBuilder.fill(0.1, 0.1, 0.1).times(targetDistance);
+        final var stdDevs = baseStdDevs.times(targetDistance).times(1 / result.getTargets().size());
         VisionMeasurement measurement =
                 VisionMeasurement.fromEstimatedRobotPose(update.get(), stdDevs);
         return Optional.of(measurement);

src/main/java/team3647/frc2024/util/AutoDrive.java

@@ -73,9 +73,6 @@ public void periodic() {
             targetRot = targeting.shootAtAmp().rotation;
         }
         SmartDashboard.putNumber("auto drive", getRot());
-        SmartDashboard.putNumber(
-                "pivot setpoint",
-                targeting.getPivotAngle(FieldConstants.kBlueSpeaker) * 180 / Math.PI);
     }
 
     public boolean swerveAimed() {

src/main/java/team3647/frc2024/util/InverseKinematics.java

@@ -60,7 +60,7 @@ public static double getWristHandoffAngleByPivot(double pivot) {
             wristAngle += Math.PI;
         }
         wristAngle += wristAngleoffSet; // difference between measured angle and angle to rollers;
-        return Math.toDegrees(wristAngle);
+        return 0;
     }
 
     public static Pose2d getCircleIntersection(Pose2d c1, double r1, Pose2d c2, double r2) {

src/main/java/team3647/frc2024/util/ModifiedSignalLogger.java

@@ -7,7 +7,7 @@
 
 public class ModifiedSignalLogger extends SignalLogger {
     public static Consumer<SysIdRoutineLog.State> logState() {
-        // start(); // Start logging if we get the consumer, so we have some data before the start
+        start(); // Start logging if we get the consumer, so we have some data before the start
         // of
         // the motion
         return (SysIdRoutineLog.State state) -> writeString("State", state.toString());