[#12] Created basic collector with beambreak

src/main/java/frc/robot/Constants.java

@@ -10,19 +10,23 @@
 /** Add your docs here. */
 public class Constants {
     public class drivetrainConstants {
-        
+
     }
 
     public class RobotMap {
         public class CAN {
-
+            public static final int COLLECTOR_MOTOR = 9;
         }
+
+        public static final int COLLECTOR_ENTRY_BEAMBREAK = 1;
     }
 
-    public class VisionConstants  {
-         //This is a magic number from gridlock, may need to be changed or removed entirely
-            public static final double PROCESS_LATENCY = 0.0472; // TODO test
-            public static final Translation2d FIELD_LIMIT = new Translation2d(Units.feetToMeters(54.0), Units.feetToMeters(26.0));
-            public static final Translation2d VISION_LIMIT = new Translation2d(Units.feetToMeters(9), Units.feetToMeters(5));
+    public class VisionConstants {
+        // This is a magic number from gridlock, may need to be changed or removed entirely
+        public static final double PROCESS_LATENCY = 0.0472; // TODO test
+        public static final Translation2d FIELD_LIMIT =
+                new Translation2d(Units.feetToMeters(54.0), Units.feetToMeters(26.0));
+        public static final Translation2d VISION_LIMIT =
+                new Translation2d(Units.feetToMeters(9), Units.feetToMeters(5));
     }
-}
+}

src/main/java/frc/robot/RobotContainer.java

@@ -1,17 +1,43 @@
 package frc.robot;
 
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.command.*;
+import frc.robot.subsystem.*;
 import frc.thunder.LightningContainer;
 
 public class RobotContainer extends LightningContainer {
+
+    /* Setting up bindings for necessary control of the swerve drive platform */
+    XboxController driver = new XboxController(0); // My joystick
+
+    // Create subsystems
+    Collector collector = new Collector();
+
     @Override
-    protected void configureButtonBindings() {}
+    protected void configureButtonBindings() {
+        new Trigger(driver::getRightBumper).whileTrue(new Collect(collector));
+    }
 
     @Override
-    protected void configureDefaultCommands() {}
+    protected void configureDefaultCommands() {
+        // Get collector entry beam break state, then run collector if object is present
+        collector.setDefaultCommand(new RunCommand(() -> {
+            if (!collector.getEntryBeamBreakState()) {
+                collector.setPower(1d);
+            } else {
+                collector.stop();
+            }
+        }, collector));
+    }
 
     @Override
     protected void configureAutonomousCommands() {}
-    
+
     @Override
     protected void releaseDefaultCommands() {}
 

src/main/java/frc/robot/command/Collect.java

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.command;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystem.Collector;
+
+public class Collect extends Command {
+
+  // Declares collector
+  private Collector collector;
+
+  public Collect(Collector collector) {
+    // Sets collector from parameter
+    this.collector = collector;
+    addRequirements(collector);
+  }
+
+  @Override
+  public void initialize() {
+    collector.setPower(1d);
+  }
+
+  @Override
+  public void execute() {}
+
+  @Override
+  public void end(boolean interrupted) {
+    collector.stop();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/subsystem/Collector.java

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystem;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class Collector extends SubsystemBase {
+
+  // Declare collector hardware
+  private TalonFX collectorMotor;
+  private DigitalInput collectorEntryBeamBreak;
+
+  public Collector() {
+    // Initialize collector hardware
+    collectorMotor = new TalonFX(Constants.RobotMap.CAN.COLLECTOR_MOTOR);
+    collectorEntryBeamBreak = new DigitalInput(Constants.RobotMap.COLLECTOR_ENTRY_BEAMBREAK);
+  }
+
+  @Override
+  public void periodic() {
+
+  }
+
+  /**
+   * @return When an object is present, returns false, otherwise returns true
+   */
+  public boolean getEntryBeamBreakState() {
+    return collectorEntryBeamBreak.get();
+  }
+
+  /**
+   * Sets the power of the collector motor
+   * @param power Double value from -1.0 to 1.0 (positive collects inwards)
+   */
+  public void setPower(double power) {
+    collectorMotor.set(power);
+  }
+
+  /**
+   * Stops the collector
+   */
+  public void stop() {
+    setPower(0d);
+  }
+
+}