Incorperate beambreak sensors into codriver controls

WTMCRobotics · Mar 13, 2024 · c778820 · c778820
1 parent ec74da1
commit c778820
Show file tree

Hide file tree

Showing 8 changed files with 126 additions and 40 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -27,6 +27,15 @@ public class Constants {
 
     public static final double SHOOTER_INTAKE_PERCENT = -0.25;
 
+    public static final double MOVEMENT_TIME_AFTER_FLOOR_INTAKE_BEAMBREAK_TRIGGER = 1;
+    //The 2 below is unused for now
+    public static final double FEEDER_MOVEMENT_SPEED_AFTER_FLOOR_INTAKE_BEAMBREAK_TRIGGER = 0.25;
+    public static final double FLYWHEEL_MOVEMENT_SPEED_AFTER_FLOOR_INTAKE_BEAMBREAK_TRIGGER = -0.1;
+
+    public static final double MOVEMENT_TIME_AFTER_SOURCE_INTAKE_BEAMBREAK_TRIGGER = 0.25;
+    public static final double FEEDER_MOVEMENT_SPEED_AFTER_SOURCE_INTAKE_BEAMBREAK_TRIGGER = 0.25;
+    public static final double FLYWHEEL_MOVEMENT_SPEED_AFTER_SOURCE_INTAKE_BEAMBREAK_TRIGGER = -0.1;
+
     /*The amount of seconds that the feeder should wait before turning off after shooting */
     public static final double FEEDER_MOTOR_SHUTOFF_TIME = 2;
 

diff --git a/src/main/java/frc/robot/InputtedCoDriverControls.java b/src/main/java/frc/robot/InputtedCoDriverControls.java
@@ -13,6 +13,7 @@ public static void onTeleopInit() {
 
     /**Prevents the codriver from accidentally moving the arms down */
     static boolean hasGoneUp = false;
+    static int lastPOV = -1;
 
     public static void onEveryFrame() {
         XboxController controller = Robot.getCoDriverController();
@@ -28,13 +29,11 @@ public static void onEveryFrame() {
             Shooter.stopShooterMotors();
         }
 
-        if (controller.getBButton()) {
-            Shooter.startShooterIntake();
-            Intake.startFloorIntake();
+        if (controller.getAButtonPressed()) {
+            Intake.startSourceIntake();
         }
-        if (controller.getBButtonReleased()) {
-            Intake.stopFloorIntake();
-            Shooter.stopShooterMotors();
+        if (controller.getAButtonReleased()) {
+            Intake.stopSourceIntake();
         }
 
         if (controller.getBackButton()) {
@@ -51,12 +50,18 @@ public static void onEveryFrame() {
         } else if (controller.getLeftTriggerAxis() >= 0.29 && controller.getLeftTriggerAxis() <= 0.71) {
             Shooter.moveHoodToShootingPosition();
         }
-        if (controller.getAButtonPressed()) {
+        if (controller.getBButtonPressed()) {
+            Intake.startFloorIntake();
+        } else if (controller.getBButtonReleased()) {
+            Intake.stopFloorIntake();
+        }
+        if (controller.getPOV() == 0) {
             Intake.backtrack();
             Shooter.backtrack();
-        } else if (controller.getAButtonReleased()) {
+        } else if (controller.getPOV() == -1 && lastPOV != -1) {
             Intake.stopFloorIntake();
             Shooter.stopShooterMotors();
         }
+        lastPOV = controller.getPOV();
     }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.UsbCamera;
 import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
@@ -101,6 +102,12 @@ public static ParallelActionRunner getTeleopActionRunner() {
         return teleopActionRunner;
     }
 
+    private static final DigitalInput shooterBeamBreakSensor = new DigitalInput(Constants.INTAKE_BEAM_BREAK_ID);
+
+    public static DigitalInput getShooterBeambreakSensor() {
+        return shooterBeamBreakSensor;
+    }
+
     private final SendableChooser<ArrayDeque<AutonAction>> autonRouteChooser = new SendableChooser<>();
 
     public static ADXRS450_Gyro getGyroscope() {

diff --git a/src/main/java/frc/robot/RobotComponents/Intake.java b/src/main/java/frc/robot/RobotComponents/Intake.java
@@ -1,28 +1,34 @@
 package frc.robot.robotcomponents;
 
 import frc.robot.Robot;
-import frc.robot.auton.AutonIntake;
+import frc.robot.auton.AutonFloorIntake;
+import frc.robot.auton.AutonSourceIntake;
 
 public class Intake {
 
     public static void autoIntake() {
-        Robot.getTeleopActionRunner().removeActionsOfType(AutonIntake.class);
+        Robot.getTeleopActionRunner().removeActionsOfType(AutonFloorIntake.class);
         // Robot.getTeleopActionRunner().addActionToRun(new AutonIntake());
     }
 
     public static void startFloorIntake() {
-        // Robot.motors.getIntake().set(Constants.INTAKE_SPEED);
+        Robot.getTeleopActionRunner().addActionToRun(new AutonFloorIntake());
+    }
+
+    public static void startSourceIntake() {
+        Robot.getTeleopActionRunner().addActionToRun(new AutonSourceIntake());
     }
 
     /* This will spin the motor backwards in an attempt to eject a stuck note*/
     public static void backtrack() {
         // Robot.motors.getIntake().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT);
     }
 
+    public static void stopSourceIntake() {
+        Robot.getTeleopActionRunner().removeActionsOfType(AutonSourceIntake.class);
+    }
+
     public static void stopFloorIntake() {
-        Robot.getTeleopActionRunner().removeActionsOfType(AutonIntake.class);
-        // Robot.motors.getIntake().stopMotor();
-        Robot.motors.getLeftFlywheel().set(0);
-        Robot.motors.getRightFlywheel().set(0);
+        Robot.getTeleopActionRunner().removeActionsOfType(AutonFloorIntake.class);
     }
 }
diff --git a/src/main/java/frc/robot/auton/AutonFloorIntake.java b/src/main/java/frc/robot/auton/AutonFloorIntake.java
@@ -0,0 +1,34 @@
+package frc.robot.auton;
+
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants;
+import frc.robot.Robot;
+
+public class AutonFloorIntake extends AutonAction {
+
+    double targetTime = -1;
+
+    @Override
+    public boolean isDone() {
+        if (Robot.getShooterBeambreakSensor().get() && targetTime == -1) {
+            targetTime = Timer.getFPGATimestamp() + Constants.MOVEMENT_TIME_AFTER_FLOOR_INTAKE_BEAMBREAK_TRIGGER;
+            Robot.motors.getFeeder().set(Constants.FEEDER_MOVEMENT_SPEED_AFTER_FLOOR_INTAKE_BEAMBREAK_TRIGGER);
+        }
+        if (targetTime != -1 && Timer.getFPGATimestamp() >= targetTime) {
+            return true;
+        }
+        return false;
+    }
+
+    @Override
+    public void initiate() {
+        Robot.motors.getIntake().set(Constants.INTAKE_SPEED);
+        Robot.motors.getFeeder().set(Constants.FEEDER_MOTOR_SPEED);
+    }
+
+    @Override
+    public void shutdown() {
+        Robot.motors.getIntake().stopMotor();
+        Robot.motors.getFeeder().stopMotor();
+    }
+}
diff --git a/src/main/java/frc/robot/auton/AutonIntake.java b/src/main/java/frc/robot/auton/AutonIntake.java
diff --git a/src/main/java/frc/robot/auton/AutonRoutes.java b/src/main/java/frc/robot/auton/AutonRoutes.java
@@ -89,7 +89,7 @@ public static void setupCorrectAutonPaths() {
     public static final ArrayDeque<AutonAction> SHOOT_BACKUP_INTAKE_FORWARD_SHOOT = new ArrayDeque<AutonAction>(
         List.of(
             new AutonShoot(),
-            new AutonParallelAction(new AutonIntake(), new AutonMoveInches(-84)),
+            new AutonParallelAction(new AutonFloorIntake(), new AutonMoveInches(-84)),
             new AutonMoveInches(84),
             new AutonShoot()
         )

diff --git a/src/main/java/frc/robot/auton/AutonSourceIntake.java b/src/main/java/frc/robot/auton/AutonSourceIntake.java
@@ -0,0 +1,49 @@
+package frc.robot.auton;
+
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.robotcomponents.Shooter;
+
+public class AutonSourceIntake extends AutonAction {
+
+    double targetTime = -1;
+    boolean seenRing;
+
+    private static boolean isCurrentlyPosititionRing = false;
+
+    public static boolean isCurrentlyPosititionRing() {
+        return isCurrentlyPosititionRing;
+    }
+
+    @Override
+    public boolean isDone() {
+        if (Robot.getShooterBeambreakSensor().get()) {
+            seenRing = true;
+            isCurrentlyPosititionRing = true;
+        } else if (seenRing && targetTime == -1) {
+            targetTime = Timer.getFPGATimestamp() + Constants.MOVEMENT_TIME_AFTER_SOURCE_INTAKE_BEAMBREAK_TRIGGER;
+            Robot.motors.getFeeder().set(Constants.FEEDER_MOVEMENT_SPEED_AFTER_SOURCE_INTAKE_BEAMBREAK_TRIGGER);
+            Robot.motors.getLeftFlywheel().set(Constants.FLYWHEEL_MOVEMENT_SPEED_AFTER_SOURCE_INTAKE_BEAMBREAK_TRIGGER);
+            Robot.motors
+                .getRightFlywheel()
+                .set(Constants.FLYWHEEL_MOVEMENT_SPEED_AFTER_SOURCE_INTAKE_BEAMBREAK_TRIGGER);
+        }
+        if (targetTime != -1 && Timer.getFPGATimestamp() >= targetTime) {
+            return true;
+        }
+        return false;
+    }
+
+    @Override
+    public void initiate() {
+        Robot.motors.getFeeder().set(Constants.FEEDER_MOTOR_SPEED);
+        isCurrentlyPosititionRing = false;
+    }
+
+    @Override
+    public void shutdown() {
+        isCurrentlyPosititionRing = false;
+        Shooter.stopShooterMotors();
+    }
+}