pre day 4 of my misery

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

@@ -8,6 +8,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -77,6 +78,29 @@ public AutoCommands(
 
     public void registerCommands() {}
 
+    public AutonomousMode getSix_S1N1F1N2N3ByColor(Alliance color) {
+        return new AutonomousMode(six_S1N1F1F2N2N3(color), getInitial(s1_to_n1_to_f1));
+    }
+
+    public AutonomousMode getFive_S1N1F1N2N3ByColor(Alliance color) {
+        return new AutonomousMode(five_S1N1F1N2N3(color), getInitial(s1_to_n1_to_f1));
+    }
+
+    public AutonomousMode getFour_S1N1F1N2N3ByColor(Alliance color) {
+        return new AutonomousMode(four_S1N1N2N3(color), getInitial(s1_to_n1));
+    }
+
+    public Command six_S1N1F1F2N2N3(Alliance color) {
+        return Commands.parallel(
+                masterSuperstructureSequence(),
+                Commands.sequence(
+                        followChoreoPathWithOverride(s1_to_n1_to_f1, color),
+                        followChoreoPathWithOverride(f1_to_shoot1, color),
+                        followChoreoPathWithOverride(shoot1_to_f2, color),
+                        followChoreoPathWithOverride(f2_to_n2, color),
+                        followChoreoPathWithOverride(n2_to_n3, color)));
+    }
+
     public Command five_S1N1F1N2N3(Alliance color) {
         return Commands.parallel(
                 masterSuperstructureSequence(),
@@ -103,10 +127,14 @@ public Command masterSuperstructureSequence() {
                         superstructure.prep(),
                         // superstructure.fastFeed(),
                         continuouslyIntakeForShoot().repeatedly(),
-                        superstructure.autoFeed(
-                                () ->
-                                        swerve.getOdoPose().getX()
-                                                < AutoConstants.kDrivetrainXShootingThreshold)));
+                        superstructure.autoFeed(() -> goodToGo())));
+    }
+
+    public boolean goodToGo() {
+        return DriverStation.getAlliance().get() == Alliance.Blue
+                ? swerve.getOdoPose().getX() < AutoConstants.kDrivetrainXShootingThreshold
+                : swerve.getOdoPose().getX()
+                        > FieldConstants.kFieldLength - AutoConstants.kDrivetrainXShootingThreshold;
     }
 
     public Command scorePreload() {
@@ -136,7 +164,7 @@ public double getPivotAngleByPose(Supplier<Pose2d> pose) {
     }
 
     public double getPivotAngle() {
-        return targeting.getPivotAngle(FieldConstants.kBlueSpeaker) * 180 / Math.PI;
+        return targeting.getPivotAngle(FieldConstants.kSpeaker) * 180 / Math.PI;
     }
 
     public Command target() {
@@ -176,6 +204,24 @@ public Command followChoreoPathWithOverrideAndPivot(String path, Alliance color)
                 superstructure.pivotCommands.setAngle(() -> getPivotAngle()));
     }
 
+    public Command followChoreoPathWithOverrideFast(String path, Alliance color) {
+        ChoreoTrajectory traj = Choreo.getTrajectory(path);
+        boolean mirror = color == Alliance.Red;
+        return customChoreoFolloweForOverride(
+                traj,
+                swerve::getOdoPose,
+                choreoSwerveController(
+                        AutoConstants.kXController,
+                        AutoConstants.kYController,
+                        new PIDController(0, 0, 0)),
+                (ChassisSpeeds speeds) ->
+                        swerve.drive(
+                                speeds.vxMetersPerSecond * 0.7,
+                                speeds.vyMetersPerSecond * 0.7,
+                                deeThetaOnTheMove()),
+                () -> mirror);
+    }
+
     public Command followChoreoPathWithOverride(String path, Alliance color) {
         ChoreoTrajectory traj = Choreo.getTrajectory(path);
         boolean mirror = color == Alliance.Red;

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

@@ -6,9 +6,9 @@
 import com.ctre.phoenix6.signals.*;
 
 public class ClimbConstants {
-    public static final TalonFX kMaster =
+    public static final TalonFX kLeft =
             new TalonFX(GlobalConstants.ClimbIds.kLeftId, GlobalConstants.subsystemsLoopName);
-    public static final TalonFX kSlave =
+    public static final TalonFX kRight =
             new TalonFX(GlobalConstants.ClimbIds.kRightId, GlobalConstants.subsystemsLoopName);
 
     public static final boolean kMasterInvert = false;
@@ -43,8 +43,10 @@ public class ClimbConstants {
         Slot0Configs kMasterSlot0 = new Slot0Configs();
         MotorOutputConfigs kMasterMotorOutput = new MotorOutputConfigs();
         SoftwareLimitSwitchConfigs kMasterSoftLimit = new SoftwareLimitSwitchConfigs();
-        TalonFXConfigurator kMasterConfigurator = kMaster.getConfigurator();
-        kMasterConfigurator.apply(kMasterConfig);
+        TalonFXConfigurator kLeftConfigurator = kLeft.getConfigurator();
+        TalonFXConfigurator kRightConfigurator = kRight.getConfigurator();
+        kLeftConfigurator.apply(kMasterConfig);
+        kRightConfigurator.apply(kMasterConfig);
 
         kMasterSlot0.kP = masterKP;
         kMasterSlot0.kI = masterKI;
@@ -55,6 +57,13 @@ public class ClimbConstants {
         kMasterSoftLimit.ForwardSoftLimitThreshold = 90;
         kMasterSoftLimit.ReverseSoftLimitEnable = true;
         kMasterSoftLimit.ReverseSoftLimitThreshold = 0;
+
+        kLeftConfigurator.apply(kMasterSlot0);
+        kLeftConfigurator.apply(kMasterMotorOutput);
+        kLeftConfigurator.apply(kMasterSoftLimit);
+        kRightConfigurator.apply(kMasterSlot0);
+        kRightConfigurator.apply(kMasterMotorOutput);
+        kRightConfigurator.apply(kMasterSoftLimit);
     }
 
     private ClimbConstants() {}

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

@@ -11,6 +11,8 @@
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
 /** Add your docs here. */
 public class FieldConstants {
@@ -27,10 +29,19 @@ public class FieldConstants {
 
     public static final Pose3d kBlueSpeaker = new Pose3d(0, 5.5, kSpeakerHeight, new Rotation3d());
 
+    public static final Pose3d kRedSpeaker =
+            new Pose3d(16.5, 5.5, kSpeakerHeight, new Rotation3d());
+
+    public static final Pose3d kSpeaker;
+
     public static final double kAmpHeight = Units.inchesToMeters(35);
 
     public static final Pose3d kBlueAmp = new Pose3d(1.957, 8.218, kAmpHeight, new Rotation3d());
 
+    public static final Pose3d kRedAmp = new Pose3d(14.630, 8.218, kAmpHeight, new Rotation3d());
+
+    public static final Pose3d kAmp;
+
     public static final Pose2d[] kBlueAutoNotePoses = {
         new Pose2d(2.912, 6.951, kZero), // n1
         new Pose2d(2.912, 5.508, kZero), // n2
@@ -63,5 +74,10 @@ public static Transform2d flipBlueTransform(Transform2d transform) {
         return new Transform2d(kOrigin, flipBluePose(kOrigin.transformBy(transform)));
     }
 
+    static {
+        kSpeaker = DriverStation.getAlliance().get() == Alliance.Blue ? kBlueSpeaker : kRedSpeaker;
+        kAmp = DriverStation.getAlliance().get() == Alliance.Blue ? kBlueAmp : kRedAmp;
+    }
+
     private FieldConstants() {}
 }

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

@@ -49,7 +49,7 @@ public void robotInit() {
             Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
             new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
         } else {
-            setUseTiming(false); // Run as fast as possible
+            setUseTiming(true); // Run as fast as possible
             String logPath =
                     LogFileUtil
                             .findReplayLog(); // Pull the replay log from AdvantageScope (or prompt

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

@@ -64,7 +64,6 @@ public RobotContainer() {
         configureButtonBindings();
         configureSmartDashboardLogging();
         autoCommands.registerCommands();
-        runningMode = autoCommands.blueFive_S1N1F1N2N3;
         pivot.setEncoder(PivotConstants.kInitialAngle);
         wrist.setEncoder(WristConstants.kInitialDegree);
         climb.setEncoder(0);
@@ -194,6 +193,9 @@ public void configureSmartDashboardLogging() {
         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("auto drive", () -> autoDrive.getVelocities().dtheta);
     }
 
@@ -267,8 +269,8 @@ public Command getAutonomousCommand() {
 
     private final Climb climb =
             new Climb(
-                    ClimbConstants.kMaster,
-                    ClimbConstants.kSlave,
+                    ClimbConstants.kLeft,
+                    ClimbConstants.kRight,
                     1,
                     1,
                     ClimbConstants.kMinLength,
@@ -304,8 +306,8 @@ public Command getAutonomousCommand() {
 
     public final TargetingUtil targetingUtil =
             new TargetingUtil(
-                    FieldConstants.kBlueSpeaker,
-                    FieldConstants.kBlueAmp,
+                    FieldConstants.kSpeaker,
+                    FieldConstants.kAmp,
                     swerve::getOdoPose,
                     swerve::getChassisSpeeds,
                     PivotConstants.robotToPivot);
@@ -323,6 +325,7 @@ public Command getAutonomousCommand() {
                     pivot,
                     wrist,
                     autoDrive::getPivotAngle,
+                    autoDrive::getShootSpeed,
                     targetingUtil.exitVelocity(),
                     inverseKinematics::getWristHandoffAngleByPivot,
                     autoDrive::swerveAimed);

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

@@ -26,6 +26,7 @@ public class Superstructure {
     public final WristCommands wristCommands;
 
     private final DoubleSupplier pivotAngleSupplier;
+    private final DoubleSupplier shooterSpeedSupplier;
     private final double pivotStowAngle = 40;
     private final double wristStowAngle = 100;
     private final double wristIntakeAngle = 0;
@@ -44,6 +45,7 @@ public Superstructure(
             Pivot pivot,
             Wrist wrist,
             DoubleSupplier pivotAngleSupplier,
+            DoubleSupplier shooterSpeedSuppler,
             double shootSpeed,
             DoubleSupplier wristInverseKinematics,
             BooleanSupplier swerveAimed) {
@@ -53,6 +55,7 @@ public Superstructure(
         this.shooterLeft = shooterLeft;
         this.pivot = pivot;
         this.pivotAngleSupplier = pivotAngleSupplier;
+        this.shooterSpeedSupplier = shooterSpeedSuppler;
         this.shootSpeed = shootSpeed;
         this.wrist = wrist;
         this.wristInverseKinematics = wristInverseKinematics;
@@ -70,7 +73,7 @@ public Command feed() {
     }
 
     public Command spinUp() {
-        return shooterCommands.setVelocity(() -> shootSpeed);
+        return shooterCommands.setVelocity(() -> shooterSpeedSupplier.getAsDouble());
     }
 
     public double getDesiredSpeed() {
@@ -101,6 +104,18 @@ public Command ejectPiece() {
         return Commands.runOnce(() -> this.hasPiece = false);
     }
 
+    public boolean flywheelReadY() {
+        return shooterLeft.velocityReached(shootSpeed * 1.1, 1);
+    }
+
+    public boolean pivotReady() {
+        return pivot.angleReached(pivotAngleSupplier.getAsDouble(), 3);
+    }
+
+    public boolean swerveReady() {
+        return swerveAimed.getAsBoolean();
+    }
+
     public Command shoot() {
         return Commands.parallel(
                 prep(),

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

@@ -108,6 +108,20 @@ public DriveMode getMode() {
         return this.mode;
     }
 
+    public double getShootSpeed() {
+        if (DriverStation.isAutonomous()) {
+            return targeting.shootAtSpeaker().shootSpeed;
+        }
+        switch (mode) {
+            case SHOOT_ON_THE_MOVE:
+                return targeting.shootAtSpeaker().shootSpeed;
+            case SHOOT_AT_AMP:
+                return targeting.shootAtAmp().shootSpeed;
+            default:
+                return 25;
+        }
+    }
+
     public double getPivotAngle() {
         if (DriverStation.isAutonomous()) {
             return Units.radiansToDegrees(targeting.shootAtSpeaker().pivot);

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

@@ -31,10 +31,18 @@ public StatusCode apply(
                 module.getSteerMotor().setControl(m_motionMagicControl);
 
                 // Command drive motor to torque
-                module.getDriveMotor().setControl(m_torqueCurrentFOC.withOutput(m_targetTorque));
+                module.getDriveMotor()
+                        .setControl(
+                                m_torqueCurrentFOC
+                                        .withOutput(m_targetTorque)
+                                        .withMaxAbsDutyCycle(0.7));
             } else {
                 // Command steer motor to torque
-                module.getSteerMotor().setControl(m_torqueCurrentFOC.withOutput(m_targetTorque));
+                module.getSteerMotor()
+                        .setControl(
+                                m_torqueCurrentFOC
+                                        .withOutput(m_targetTorque)
+                                        .withMaxAbsDutyCycle(0.7));
 
                 // Command drive motor to zero
                 module.getDriveMotor().setControl(m_motionMagicControl);

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

@@ -21,7 +21,7 @@ public class TargetingUtil {
     private final Supplier<Pose2d> drivePose;
     private final Supplier<ChassisSpeeds> robotRelativeSpeeds;
     private final Transform3d robotToShooter;
-    private final double shootSpeed = 5.5;
+    private final double shootSpeed = 25;
     private double offset = 0;
     double kDt = 0.02;
 
@@ -52,7 +52,11 @@ public Command offsetDown() {
 
     // returns an object storing a pair of doubles, swerve angle change and pivot angle
     public AimingParameters shootAtPose(Pose3d pose) {
-        return new AimingParameters(robotAngleToPose(pose), getPivotAngle(pose));
+        return new AimingParameters(robotAngleToPose(pose), getPivotAngle(pose), shootSpeed);
+    }
+
+    public AimingParameters shootAtPose(Pose3d pose, double shootSpeed) {
+        return new AimingParameters(robotAngleToPose(pose), getPivotAngle(pose), shootSpeed);
     }
 
     // returns the parameters for aiming at the speaker
@@ -62,7 +66,7 @@ public AimingParameters shootAtSpeaker() {
 
     // returns the parameters for aiming at the speaker
     public AimingParameters shootAtAmp() {
-        return shootAtPose(ampPose);
+        return shootAtPose(ampPose, 5.5);
     }
 
     // returns the angle between the vector pointing straight back and the pose
@@ -222,10 +226,12 @@ public double exitVelocity() {
     public class AimingParameters {
         public double rotation;
         public double pivot;
+        public double shootSpeed;
 
-        public AimingParameters(double rotation, double pivot) {
+        public AimingParameters(double rotation, double pivot, double shootSpeed) {
             this.rotation = rotation;
             this.pivot = pivot;
+            this.shootSpeed = shootSpeed;
         }
     }
 }