From cc682cfd8675ae54a304a2b6ef7f7521e8751ffe Mon Sep 17 00:00:00 2001
From: 66945 <66945@psdschools.org>
Date: Sat, 15 Jan 2022 14:20:49 -0700
Subject: [PATCH] Created subsystem and initial behavior

---
 src/main/java/frc4388/robot/Constants.java    |  9 ++++++
 .../java/frc4388/robot/RobotContainer.java    |  3 ++
 .../frc4388/robot/subsystems/Serializer.java  | 31 +++++++++++++++++++
 3 files changed, 43 insertions(+)
 create mode 100644 src/main/java/frc4388/robot/subsystems/Serializer.java

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 5581adf..68c6003 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -67,6 +67,15 @@ public final class Constants {
     public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
   }
 
+  public static final class SerializerConstants {
+    public static final double SERIALIZER_BELT_SPEED = 1.0; // TODO (currently max power, DO NOT RUN)
+    public static final double SERIALIZER_SHOOTER_BELT_SPEED = 1.0; // TODO (currently max power, DO NOT RUN)
+    
+    // CAN IDs
+    public static final int SERIALIZER_BELT = 1; // TODO
+    public static final int SERIALIZER_SHOOTER_BELT = 1; // TODO
+  }
+
   public static final class LEDConstants {
     public static final int LED_SPARK_ID = 0;
 
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 38c3459..cf0220c 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.Serializer;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.controller.IHandController;
@@ -39,6 +40,8 @@ public class RobotContainer {
     m_robotMap.rightBackEncoder
   );
 
+  private final Serializer m_robotSerializer = new Serializer();
+
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
   /* Controllers */
diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java
new file mode 100644
index 0000000..ad25a41
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Serializer.java
@@ -0,0 +1,31 @@
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants;
+
+public class Serializer extends SubsystemBase{
+  private Spark m_serializerBelt;
+  private Spark m_serializerShooterBelt;
+
+  private boolean serializerState;
+
+    public Serializer(Spark serializerBelt, Spark serializerShooterBelt) {
+        m_serializerBelt = serializerBelt;
+        m_serializerShooterBelt = serializerShooterBelt;
+
+        serializerState = false;
+        setSerializerState(serializerState);
+    }
+
+    public void setSerializerState(boolean state) {
+      double serializerBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_BELT_SPEED : 0.d;
+      double serializerShooterBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_SHOOTER_BELT_SPEED : 0.d;
+
+      m_serializerBelt.set(serializerBeltSpeed);
+      m_serializerShooterBelt.set(serializerShooterBeltSpeed);
+
+      serializerState = state;
+    }
+}