sending to houston now

adding minimum wait for dynamic, spindexer jamming should be more accurate, drive offsets redone

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 94b0c4288fe1dba8ea8a30d91a49db9c432f5458..87b7731bceb40ccafd2d8ae693de372368769fa6 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -65,7 +65,7 @@ public class RobotContainer {
   private Hood hood = null;
   private Spindexer spindexer = null;
   private Intake intake = null;
-  private LED led = null;
+  // private LED led = null;
 
   // Controllers are defined here
   private BaseDriverConfig driver = null;
@@ -107,7 +107,7 @@ public class RobotContainer {
       case PrimeJr: // AKA Valence
         spindexer = new Spindexer();
         intake = new Intake();
-        led = new LED();
+        // led = new LED();
         breaker = new EMABreaker();
 
       case WaffleHouse: // AKA Betabot
@@ -156,6 +156,7 @@ public class RobotContainer {
         }
         break;
     }
+    
 
        if (intake != null && hood != null && turret != null)
                // CommandScheduler.getInstance().schedule(new HardstopWarning(hood, intake, turret)); (no more crt for this)
@@ -315,6 +316,8 @@ public class RobotContainer {
     String rightDynamicLiberalDoubleSwipe = "RightDynamicDoubleLiberalSwipe";
     String leftDynamicConservativeDoubleSwipe = "LeftDynamicDoubleConservativeSwipe";
     String rightDynamicConservativeDoubleSwipe = "RightDynamicDoubleConservativeSwipe";
+    String leftDynamicShallowDoubleSwipe = "LeftDynamicShallowDoubleSwipe";
+    String rightDynamicShallowDoubleSwipe = "RightDynamicShallowDoubleSwipe";
 
     // add commands
     addAuto(leftDynamicLiberalDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleLiberalSwipe(true));

diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
index 9bd1550c867169e718f8d7e7cc9f29a923ae98b5..f2ba4ba1f5be3842441ad0adf393a3d5563abd15 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
+++ b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
@@ -63,7 +63,7 @@ public class RunSpindexer extends Command {
             reversing = false;
             return; // this is so the balls don't fly out when unaligned
         }
-        boolean jammed = spindexer.getSubsystemStatorCurrent() / 2 > SpindexerConstants.JAM_CURRENT_THRESHOLD;
+        boolean jammed = spindexer.getMotorOneVelocity() < SpindexerConstants.JAM_CURRENT_THRESHOLD && spindexer.getMotorOneStatorCurrent() > SpindexerConstants.JAM_CURRENT_THRESHOLD;
         Logger.recordOutput("SpindexerJammed", jammed);
         if (jam_debouncer.calculate(jammed)) {
             Logger.recordOutput("SpindexerJammedDebounced", jammed);

diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java b/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java
index 6a00feb4e60bef26a3d0405ccb08ab235b2dabf2..742a0fb491938c2565e78d95d9f85825d4738922 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java
+++ b/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java
@@ -35,6 +35,8 @@ public class RunSpindexerWithStop extends Command {
 
     private Debouncer debouncer = new Debouncer(0.3, DebounceType.kRising);
 
+    private final double interval = 0.6; // Change this to make it faster/slower (seconds)
+
     public RunSpindexerWithStop(Spindexer spindexer, Turret turret, Hood hood, Intake intake) {
         this.spindexer = spindexer;
         this.turret = turret;
@@ -95,16 +97,25 @@ public class RunSpindexerWithStop extends Command {
         if (!Constants.DISABLE_SMART_DASHBOARD) {
             SmartDashboard.putBoolean("Spindexer Jamming", reversing);
         }
+
+        if ((int) (Timer.getFPGATimestamp() / interval) % 2 == 0) {
+            intake.extend();
+        } else {
+            intake.intermediateExtend();
+        }
+
     }
 
     @Override
     public void end(boolean interrupted) {
         spindexer.stopSpindexer();
         reversing = false;
+
+        intake.extend();
     }
 
     @Override
     public boolean isFinished() {
-        return runTimer.hasElapsed(1.0) && debouncer.calculate(spindexer.spinningAir());
+        return (runTimer.hasElapsed(1.0) && debouncer.calculate(spindexer.spinningAir())) || runTimer.hasElapsed(4.0);
     }
 }

diff --git a/src/main/java/frc/robot/constants/swerve/DriveConstants.java b/src/main/java/frc/robot/constants/swerve/DriveConstants.java
index 4f1064245e71cc3e8cd951d6b2e20b67d61f6bc2..d136d1088cf084356b8390fae5bb22b993c45134 100644 (file)
--- a/src/main/java/frc/robot/constants/swerve/DriveConstants.java
+++ b/src/main/java/frc/robot/constants/swerve/DriveConstants.java
@@ -208,10 +208,10 @@ public class DriveConstants {
          */
         public static void update(RobotId robotId) {
             if (robotId == RobotId.PrimeJr) {
-                STEER_OFFSET_FRONT_LEFT = 188.26+180;
-                STEER_OFFSET_FRONT_RIGHT = 162.71+180+180;
-                STEER_OFFSET_BACK_LEFT = 196.69+180;
-                STEER_OFFSET_BACK_RIGHT = 357.90+180+180;
+                STEER_OFFSET_FRONT_LEFT = 187.64+180; // module zero
+                STEER_OFFSET_FRONT_RIGHT = 162+180+180; // module one
+                STEER_OFFSET_BACK_LEFT = 196.3+180; // module two
+                STEER_OFFSET_BACK_RIGHT = 357+180+180; // module three
                 
                 // MK5n 
                 INVERT_STEER_MOTOR = InvertedValue.CounterClockwise_Positive;

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index 2f32fe1db30d96a6dd0d5732f4dc07889c182cf3..e4bccebdbe9544a8d3df352dcff38722a680e68a 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -2,6 +2,7 @@ package frc.robot.subsystems.spindexer;
 
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
@@ -89,7 +90,6 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         }
 
         Logger.recordOutput("HasBalls", spinningAir());
-
     }
 
     public void setMotorVoltages(double voltage) {
@@ -144,6 +144,14 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         return getMotorOneStatorCurrent() < 16.0 && getMotorTwoStatorCurrent() < 28.0;
     }
 
+    public double getMotorOneVelocity() {
+        return inputs.spindexerOneVelocity;
+    }
+
+    public double getMotorTwoVelocity() {
+        return inputs.spindexerTwoVelocity;
+    }
+
     @Override
     public void updateInputs() {
         inputs.spindexerOneVelocity = motorOne.getVelocity().getValueAsDouble();

diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
index e4d5708352657ea890ff7805e51b8ab24024b1f4..54b026ea37c2b03d7ffdd554dc3bb8fde640fd63 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
@@ -2,7 +2,7 @@ package frc.robot.subsystems.spindexer;
 
 public class SpindexerConstants {
     public static final double spindexerVelocityWithBall = 6.0; // rps (for counting balls)
-    public static final double SUPPLY_CURRENT_LIMIT = 40; // A
+    public static final double SUPPLY_CURRENT_LIMIT = 80; // A
     public static final double spindexerForwardVoltage = 1.00; // Volts (set low for testing)
     public static final double spindexerReverseVoltage = -1.00; // Volts
     public static final double GEAR_RATIO = 27.0; // unused & both motors have same gearing
@@ -10,7 +10,8 @@ public class SpindexerConstants {
     public static final double CURRENT_FORWARD_STATOR_LIMIT = 150.0;
     public static final double CURRENT_REVERSE_STATOR_LIMIT = 20.0;
     public static final double CURRENT_TIME_LIMIT = 1.0; //s
-    public static final double JAM_CURRENT_THRESHOLD = 40.0; // A
+    public static final double JAM_CURRENT_THRESHOLD = 10.0; // A
+    public static final double JAM_VELOCITY_THRESHOLD = 10.0; // A
     public static final double JAM_DEBOUNCE_TIME = 0.3; // seconds
     public static final double REVERSE_DEBOUNCE_TIME = 0.25; // seconds
 }