Merge pull request #23 from frc-862/18-clean-up-collector

[#18] Cleaned up collector code and moved logic to new command

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

@@ -10,7 +10,6 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory;
 import com.ctre.phoenix6.signals.NeutralModeValue;
-
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import frc.robot.Constants.RobotMap.CAN;
@@ -33,8 +32,8 @@ public class RobotMap {
         public class CAN {
             // Front Left
             private static final int kFrontLeftDriveMotorId = 1;
-        	private static final int kFrontLeftSteerMotorId = 2;
-        	private static final int kFrontLeftEncoderId = 31;
+                private static final int kFrontLeftSteerMotorId = 2;
+                private static final int kFrontLeftEncoderId = 31;
 
 			// Front Right
         	private static final int kFrontRightDriveMotorId = 3;
@@ -50,7 +49,7 @@ public class CAN {
         	private static final int kBackRightDriveMotorId = 5;
         	private static final int kBackRightSteerMotorId = 6;
         	private static final int kBackRightEncoderId = 32;
-
+                
 			public static final int PigeonId = 23;            
             public static final int COLLECTOR_MOTOR = 9;
             public static final int FLYWHEEL_MOTOR_1 = 0; //TODO Get real
@@ -163,6 +162,14 @@ public class VisionConstants  {
             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 CollectorConstants {
+        public static final boolean COLLECTOR_MOTOR_INVERTED = false;
+        public static final int COLLECTOR_MOTOR_SUPPLY_CURRENT_LIMIT = 0; // TODO: make sure they are not set to 0
+        public static final int COLLECTOR_MOTOR_STATOR_CURRENT_LIMIT = 0;
+        public static final NeutralModeValue COLLECTOR_MOTOR_NEUTRAL_MODE = NeutralModeValue.Coast;
+    }
+
 
     public class FlywheelConstants {
         public static final boolean FLYWHEEL_MOTOR_1_INVERT = false;
@@ -216,4 +223,4 @@ public enum SHOOTER_STATES {
             put(0d, 0d);
         }};
     }
-}
+}

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

@@ -57,15 +57,6 @@ protected void configureDefaultCommands() {
                     .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), 0.1) * drivetrainConstants.MaxSpeed) // Drive left with negative X (left)
                     .withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(), 0.1) * drivetrainConstants.MaxAngularRate) // Drive counterclockwise with negative X (left)
                 ));
-
-                // 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)); // TODO teach not to do this (was temp for teaching rookies)
 	}
 
         @Override

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

@@ -0,0 +1,46 @@
+// 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.subsystems.Collector;
+
+public class RunCollectorOnBeamBreak extends Command {
+
+  /*
+   * When the beam is interfered with by a note, the collector will run at full power forward.
+   * When the beam is not interfered with by a note, the collector will stop.
+   */
+
+  // Declare subsystems needed by command
+  private Collector collector;
+
+  public RunCollectorOnBeamBreak(Collector collector) {
+    this.collector = collector;
+    addRequirements(collector);
+  }
+
+  @Override
+  public void initialize() {}
+
+  @Override
+  public void execute() {
+    if (!collector.getEntryBeamBreakState()) {
+        collector.setPower(1d);
+    } else {
+        collector.stop();
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    collector.stop();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -5,9 +5,13 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
+import frc.robot.Constants.RobotMap;
+import frc.robot.Constants.RobotMap.CAN;
+import frc.robot.Constants.CollectorConstants;
+import frc.thunder.config.FalconConfig;
 
 public class Collector extends SubsystemBase {
 
@@ -17,8 +21,8 @@ public class Collector extends SubsystemBase {
 
   public Collector() {
     // Initialize collector hardware
-    collectorMotor = new TalonFX(Constants.RobotMap.CAN.COLLECTOR_MOTOR);
-    collectorEntryBeamBreak = new DigitalInput(Constants.RobotMap.COLLECTOR_ENTRY_BEAMBREAK);
+    collectorMotor = FalconConfig.createMotor(CAN.COLLECTOR_MOTOR, getName(), CollectorConstants.COLLECTOR_MOTOR_INVERTED, CollectorConstants.COLLECTOR_MOTOR_SUPPLY_CURRENT_LIMIT, CollectorConstants.COLLECTOR_MOTOR_STATOR_CURRENT_LIMIT, CollectorConstants.COLLECTOR_MOTOR_NEUTRAL_MODE);
+    collectorEntryBeamBreak = new DigitalInput(RobotMap.COLLECTOR_ENTRY_BEAMBREAK);
   }
 
   @Override