other auto work and shuttling works

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index e1c335bb7221a3588e891204d23b3492a120c384..895a1e9a51669ce1c1b491023365f1e8249d5309 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.commands.DoNothing;
 import frc.robot.commands.LogCommand;
+import frc.robot.commands.Music;
 import frc.robot.commands.auto_comm.DynamicAutoBuilder;
 import frc.robot.commands.drive_comm.DefaultDriveCommand;
 import frc.robot.commands.gpm.IntakeMovementCommand;
@@ -293,8 +294,11 @@ public class RobotContainer {
     addAuto(leftDoNothing);
     addAuto(rightDoNothing);
     addAuto(centerDoNothing);
+    addAuto("LeftShallowDoubleSwipe");
+    addAuto("RightShallowDoubleSwipe");
 
-    DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder(spindexer, turret, hood, intake);
+
+    DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder(spindexer, turret, hood, intake, drive);
     
     // names
     String leftDynamicLiberalDoubleSwipe = "LeftDynamicDoubleLiberalSwipe";

diff --git a/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java
index 09461d53fa718e56eabe6d576a9b9847fb9a06d7..1fcdf01605b53972adb9329b65621f08340f2985 100644 (file)
--- a/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java
+++ b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java
@@ -1,5 +1,6 @@
 package frc.robot.commands.auto_comm;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.*;
 import frc.robot.commands.gpm.IntakeCommand;
@@ -8,6 +9,7 @@ import frc.robot.commands.gpm.RunSpindexerWithStop;
 import frc.robot.subsystems.Intake.Intake;
 import frc.robot.subsystems.PowerControl.BreakerConstants;
 import frc.robot.subsystems.PowerControl.EMABreaker;
+import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.hood.Hood;
 import frc.robot.subsystems.spindexer.Spindexer;
 import frc.robot.subsystems.turret.Turret;
@@ -20,17 +22,20 @@ public class DynamicAutoBuilder {
     private final Turret turret;
     private final Hood hood;
     private final Intake intake;
+    private final Drivetrain drive;
 
-    public DynamicAutoBuilder(Spindexer spindexer, Turret turret, Hood hood, Intake intake) {
+    public DynamicAutoBuilder(Spindexer spindexer, Turret turret, Hood hood, Intake intake, Drivetrain drive) {
         this.spindexer = spindexer;
         this.turret = turret;
         this.hood = hood;
         this.intake = intake;
+        this.drive = drive;
     }
 
     /*
      * Autos have no named commands within them. They must be added here
-     * Still need to make one method to call that four command block in each sequential
+     * Still need to make one method to call that four command block in each
+     * sequential
      */
 
     public Command getDynamicDoubleLiberalSwipe(boolean left) {
@@ -41,22 +46,24 @@ public class DynamicAutoBuilder {
                 runSpindexerWithAbort(),
 
                 departCommand(),
+                // new InstantCommand(() -> drive.setPose(Translation2d.kZero)),
                 new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
                 startShootingCommand(),
                 runSpindexerWithAbort(),
 
                 departCommand(),
+                // new InstantCommand(() -> drive.setPose(Translation2d.kZero)),
                 new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
                 startShootingCommand(),
                 runSpindexerWithAbort(),
-                
+
                 departCommand(),
+                // new InstantCommand(() -> drive.setPose(Translation2d.kZero)),
                 new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
                 startShootingCommand(),
-                runSpindexerWithAbort()
-        ); 
+                runSpindexerWithAbort());
     }
-        
+
     public Command getDynamicDoubleConservativeSwipe(boolean left) {
         return new SequentialCommandGroup(
                 departCommand(),
@@ -73,12 +80,11 @@ public class DynamicAutoBuilder {
                 new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
                 startShootingCommand(),
                 runSpindexerWithAbort(),
-                
+
                 departCommand(),
                 new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
                 startShootingCommand(),
-                runSpindexerWithAbort()
-        ); 
+                runSpindexerWithAbort());
     }
 
     public Command getDynamicDoubleShallowSwipe(boolean left) {
@@ -97,12 +103,11 @@ public class DynamicAutoBuilder {
                 new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
                 startShootingCommand(),
                 runSpindexerWithAbort(),
-                
+
                 departCommand(),
                 new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
                 startShootingCommand(),
-                runSpindexerWithAbort()
-        ); 
+                runSpindexerWithAbort());
     }
 
     private Command departCommand() {
@@ -124,9 +129,12 @@ public class DynamicAutoBuilder {
         // timer.start();
 
         // return new RunSpindexer(spindexer, turret, hood, intake).raceWith(
-        //         new WaitUntilCommand(() -> spindexer.spinningAir() && timer.hasElapsed(3.0)));
-        // return new RunSpindexer(spindexer, turret, hood, intake).raceWith(new WaitCommand(3.0));
-        // return new RunSpindexer(spindexer, turret, hood, intake).until(() -> spindexer.spinningAir() && timer.hasElapsed(3.0));
+        // new WaitUntilCommand(() -> spindexer.spinningAir() &&
+        // timer.hasElapsed(3.0)));
+        // return new RunSpindexer(spindexer, turret, hood, intake).raceWith(new
+        // WaitCommand(3.0));
+        // return new RunSpindexer(spindexer, turret, hood, intake).until(() ->
+        // spindexer.spinningAir() && timer.hasElapsed(3.0));
 
         // has an isFinnished so it should work
         return new RunSpindexerWithStop(spindexer, turret, hood, intake);

diff --git a/src/main/java/frc/robot/constants/ShuttleInterpolation.java b/src/main/java/frc/robot/constants/ShuttleInterpolation.java
index 4ba1a7b31863e4c79556ff251227a1a88c426d2f..7cf9b4a75cd83bc0775757b665e9b78b8ac874c2 100644 (file)
--- a/src/main/java/frc/robot/constants/ShuttleInterpolation.java
+++ b/src/main/java/frc/robot/constants/ShuttleInterpolation.java
@@ -21,7 +21,7 @@ public class ShuttleInterpolation {
         shooterVelocityMap.put(0.0, 9.0);
         shooterVelocityMap.put(4.0, 12.0 * 1.3); // tuned by wesley
         shooterVelocityMap.put(8.0, 22.0 * 1.075); // tuned by wesley
-        shooterVelocityMap.put(16.5, 70.0); // tuned by taren
+        shooterVelocityMap.put(16.0, 100.0); // tuned by taren
         
         // always shoot at low angle to ground.
         newHoodMap.put(0.0, 55.0); // min angle (w/ 0.5 deg buffer)

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 3569dba25774bfeabba8bb0513e791a701954854..fb8dfd34716c5e5023cb6847edc0066e1defbf67 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -125,7 +125,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
 
             // Toggle spindexer
             controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue(
-                new RunSpindexerWithStop(spindexer, turret, hood, intake)
+                new RunSpindexer(spindexer, turret, hood, intake)
             );
         }
 

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index 0db0422a3dfeb472a75bc9a4a4636344c9c55fbd..acd45cbedb199415b00f36f568d6d878aa036468 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -10,6 +10,7 @@ import java.util.function.Supplier;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
+import com.ctre.phoenix6.hardware.TalonFX;
 import com.pathplanner.lib.util.PathPlannerLogging;
 
 import edu.wpi.first.math.VecBuilder;
@@ -17,6 +18,7 @@ import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -28,6 +30,7 @@ import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.commands.Music;
 import frc.robot.constants.Constants;
 import frc.robot.constants.FieldConstants;
 import frc.robot.constants.GyroBiasConstants;
@@ -189,6 +192,25 @@ public class Drivetrain extends SubsystemBase {
         if (!Constants.DISABLE_SMART_DASHBOARD) {
             SmartDashboard.putData("Field", field);
         }
+
+        addMusic();
+
+    }
+
+    public void setPose(Translation2d pose) {
+        poseEstimator.resetTranslation(pose);
+    }
+
+    public void addMusic() {
+        ArrayList<TalonFX> motors = new ArrayList<>();
+        for (Module m: modules) {
+            motors.add(m.getMotors()[0]);
+            motors.add(m.getMotors()[1]);
+        }
+
+        TalonFX[] f = new TalonFX[8];
+        
+        SmartDashboard.putData("Chirp", new Music(motors.toArray(f)));
     }
 
     public void close() {