Merge branch 'Serializer' into Intake

src/main/java/frc4388/robot/RobotContainer.java

@@ -62,6 +62,8 @@ import frc4388.robot.commands.Shoot;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Hood;
 import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.Serializer;
+import frc4388.robot.subsystems.Storage;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Turret;
 import frc4388.robot.subsystems.Vision;
@@ -83,10 +85,20 @@ public class RobotContainer {
   /* RobotMap */
   private final RobotMap m_robotMap = new RobotMap();
 
-  /* Subsystems */
-  public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack,
-      m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
-
+  // Subsystems 
+  private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
+    m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor,
+    m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor,
+    m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor,
+    m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor,
+    m_robotMap.leftFrontEncoder,
+    m_robotMap.rightFrontEncoder,
+    m_robotMap.leftBackEncoder,
+    m_robotMap.rightBackEncoder
+  );
+  private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor);
+  private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, m_robotMap.serializerShooterBelt, m_robotMap.serializerBeam);
+  private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter);
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
   private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
   private final Hood m_robotHood = new Hood();

src/main/java/frc4388/robot/RobotMap.java

@@ -15,10 +15,13 @@ import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.Constants.SerializerConstants;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.subsystems.SwerveModule;
 
@@ -210,4 +213,18 @@ public class RobotMap {
 
   }
 
-       
+       
+  /* Serializer Subsystem */
+  public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
+  public final CANSparkMax serializerShooterBelt = new CANSparkMax(SerializerConstants.SERIALIZER_SHOOTER_BELT, MotorType.kBrushless);
+  public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM);
+  
+  /* Intake Subsytem */
+  public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
+  public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
+
+  /* Storage Subsystem */
+  public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
+  public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
+  public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
+}

src/main/java/frc4388/robot/subsystems/Serializer.java

@@ -12,10 +12,10 @@ public class Serializer extends SubsystemBase{
   private DigitalInput m_serializerBeam;
   private boolean serializerState;
 
-  public Serializer(CANSparkMax serializerBelt, CANSparkMax serializerShooterBelt) {
+  public Serializer(CANSparkMax serializerBelt, CANSparkMax serializerShooterBelt, DigitalInput serializerBeam) { //TODO: Only one motor lol
     m_serializerBelt = serializerBelt;
     m_serializerShooterBelt = serializerShooterBelt;
-    m_serializerBeam = new DigitalInput(Constants.SerializerConstants.SERIALIZER_BELT_BEAM);
+    m_serializerBeam = serializerBeam;
 
     serializerState = false;
     setSerializerState(serializerState);

src/main/java/frc4388/robot/subsystems/Storage.java

@@ -11,20 +11,22 @@ import edu.wpi.first.wpilibj.DigitalInput;
 import frc4388.robot.Constants.StorageConstants;
 
 public class Storage extends SubsystemBase {
-  public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
-  private DigitalInput m_beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
-  private DigitalInput m_beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
+  public CANSparkMax m_storageMotor;
+  private DigitalInput m_beamShooter;
+  private DigitalInput m_beamIntake;
 
   /** Creates a new Storage. */
-  public Storage() {
-
+  public Storage(CANSparkMax storageMotor, DigitalInput beamShooter, DigitalInput beamIntake) {
+    m_storageMotor = storageMotor;
+    m_beamShooter = beamShooter;
+    m_beamIntake = beamIntake;
   }
   /**
    * If The Beam Is Broken, Run Storage
    * If Else, Stop Running Storage
    */
   public void manageStorage() {
-    if (m_beamShooter.get()) {
+    if (isBeamIntakeBroken()) { //Maybe needs to be shooter
       runStorage(1.d);
     } else { runStorage(0.d); }
   }