modified collector light for current spikes

MOERobotics · Apr 20, 2024 · 5a8d0ca · 5a8d0ca
1 parent 327df5c
commit 5a8d0ca
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -20,5 +20,5 @@ public static class OperatorConstants {
 	}
     public static Rotation2d collectorShoulder = Rotation2d.fromDegrees(83.6);
     public static Rotation2d collectorWrist = Rotation2d.fromDegrees(-45);
-    public static double subShotSpeed = 2700;
+    public static double subShotSpeed = 2500;
 }
diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java
@@ -223,8 +223,8 @@ public class FortissiMOEContainer{
 
         SmartDashboard.putNumber("Roll", pigeon.getRoll());
         SmartDashboard.putNumber("Pitch", pigeon.getPitch());
-        pdh.setSwitchableChannel((collectorSubsystem.isCollected() && ((System.currentTimeMillis()/100)%2 == 0))
-                ||  (shooterSubsystem.shooterAtSpeed() && shooterSubsystem.getDesiredTopSpeed() != 0 && collectorSubsystem.isCollected()));
+        pdh.setSwitchableChannel(((collectorSubsystem.isCollected() || collectorSubsystem.getCollectorAmps() > 20) && ((System.currentTimeMillis()/100)%2 == 0))
+                ||  (shooterSubsystem.shooterAtSpeed() && shooterSubsystem.getDesiredTopSpeed() != 0 && (collectorSubsystem.isCollected() || collectorSubsystem.getCollectorAmps() > 20)));
     });
     //weirdest command ever - climbing & pdh logic
 
@@ -285,7 +285,6 @@ public FortissiMOEContainer() {
         m_chooser.addOption("driveForward", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).rollOutAuto());
         m_chooser.addOption("3 Note Centerline Auto (DC3C2)", new tripleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).DC3C2());
         m_chooser.addOption("2 Note Centerline Auto Obj Detect", new tripleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0,0).DC3ObjDetect());
-        m_chooser.addOption("CenterLine Pass Auto Obj Detect (DC5C4PassC3OD)", new doubleNoteAutos(swerveSubsystem, armSubsystem, shooterSubsystem, collectorSubsystem, 0, 0).DC5C4PassC3ObjDet());
         SmartDashboard.putData("chooser", m_chooser);
     }
 

diff --git a/src/main/java/frc/robot/subsystems/CollectorSubsystem.java b/src/main/java/frc/robot/subsystems/CollectorSubsystem.java
@@ -46,6 +46,9 @@ public void setCollectorSpeed(double speed){
         }
     }
 
+    public double getCollectorAmps(){
+        return collector.getOutputCurrent();
+    }
 
     public Command runCollectorForAuto(final double speed) {
         Command cmd = Commands.run(() -> updateCollectorSpeed(speed));