From 615dfd82d1a66372fa54f2b1dcdd5364cb241e53 Mon Sep 17 00:00:00 2001
From: aborunda <90010674+aborunda@users.noreply.github.com>
Date: Wed, 12 Jan 2022 06:00:44 +0530
Subject: [PATCH 01/42] Create Intake.java

---
 .../java/frc4388/robot/subsystems/Intake.java | 24 +++++++++++++++++++
 1 file changed, 24 insertions(+)
 create mode 100644 src/main/java/frc4388/robot/subsystems/Intake.java

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
new file mode 100644
index 0000000..50d508d
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -0,0 +1,24 @@
+// 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 frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Intake extends SubsystemBase {
+
+  /** Creates a new Intake. */
+  public Intake() {
+
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+
+  public void runWithTriggers(double leftTrigger, double rightTrigger){
+
+  }
+}

From 4d7dc303c8133ddc332294af9179c6e2b574d295 Mon Sep 17 00:00:00 2001
From: aborunda <90010674+aborunda@users.noreply.github.com>
Date: Wed, 12 Jan 2022 06:34:13 +0530
Subject: [PATCH 02/42] Update Intake.java

---
 src/main/java/frc4388/robot/subsystems/Intake.java | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 50d508d..b5041e8 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -21,4 +21,12 @@ public class Intake extends SubsystemBase {
   public void runWithTriggers(double leftTrigger, double rightTrigger){
 
   }
+
+  public void runIntake(double input) {
+    m_intakeMotor.set(input);
+  }
+
+  Public vod runExtender(double input) {
+    m_extenderMotor.set(input);
+  }
 }

From 26f79fa8d5254ec35047536354212847519d0b35 Mon Sep 17 00:00:00 2001
From: aborunda <90010674+aborunda@users.noreply.github.com>
Date: Thu, 13 Jan 2022 05:41:53 +0530
Subject: [PATCH 03/42] Update Intake.java

---
 src/main/java/frc4388/robot/subsystems/Intake.java | 8 ++------
 1 file changed, 2 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index b5041e8..6f220c1 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -18,15 +18,11 @@ public class Intake extends SubsystemBase {
     // This method will be called once per scheduler run
   }
 
-  public void runWithTriggers(double leftTrigger, double rightTrigger){
-
-  }
-
-  public void runIntake(double input) {
+  public void runWithTriggers(double leftTrigger, double rightTrigger) {
     m_intakeMotor.set(input);
   }
 
-  Public vod runExtender(double input) {
+  public void runExtender(double input) {
     m_extenderMotor.set(input);
   }
 }

From d2d44dc2c481b08a65fb4d83b3875dc08aa9d7d7 Mon Sep 17 00:00:00 2001
From: aborunda <90010674+aborunda@users.noreply.github.com>
Date: Thu, 13 Jan 2022 05:50:39 +0530
Subject: [PATCH 04/42] Update Intake.java

---
 src/main/java/frc4388/robot/subsystems/Intake.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 6f220c1..fc655ea 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -19,10 +19,10 @@ public class Intake extends SubsystemBase {
   }
 
   public void runWithTriggers(double leftTrigger, double rightTrigger) {
-    m_intakeMotor.set(input);
+    m_intakeMotor.set(rightTrigger - leftTrigger);
   }
 
-  public void runExtender(double input) {
-    m_extenderMotor.set(input);
+  public void runExtender(boolean extended) {
+    m_extenderMotor.set(extended ? 1 : 0);
   }
 }

From 78e1c1a94e46aa03d143c7f7c63bfd91110f50fc Mon Sep 17 00:00:00 2001
From: aarav18 <57469559+aarav18@users.noreply.github.com>
Date: Thu, 13 Jan 2022 18:06:38 -0700
Subject: [PATCH 05/42] Update Intake.java

---
 src/main/java/frc4388/robot/subsystems/Intake.java | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index fc655ea..7db1b7f 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -4,13 +4,19 @@
 
 package frc4388.robot.subsystems;
 
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Intake extends SubsystemBase {
 
-  /** Creates a new Intake. */
-  public Intake() {
+  private WPI_TalonFX m_intakeMotor;
+  private WPI_TalonFX m_extenderMotor;
 
+  /** Creates a new Intake. */
+  public Intake(WPI_TalonFX m_intakeMotor, WPI_TalonFX m_extenderMotor) {
+    this.m_intakeMotor = m_intakeMotor;
+    this.m_extenderMotor = m_extenderMotor; 
   }
 
   @Override

From 2f8a45fcb0e1650b9a6139543103b550f896e669 Mon Sep 17 00:00:00 2001
From: aborunda <90010674+aborunda@users.noreply.github.com>
Date: Thu, 13 Jan 2022 19:03:01 -0700
Subject: [PATCH 06/42] Create intake.java

---
 .../java/frc4388/robot/commands/intake.java     | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)
 create mode 100644 src/main/java/frc4388/robot/commands/intake.java

diff --git a/src/main/java/frc4388/robot/commands/intake.java b/src/main/java/frc4388/robot/commands/intake.java
new file mode 100644
index 0000000..04fc49c
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/intake.java
@@ -0,0 +1,17 @@
+// 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 frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class intake extends SubsystemBase {
+  /** Creates a new intake. */
+  public intake() {}
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

From 5927e54b568b3748fc7681216795b2c8e0dd3919 Mon Sep 17 00:00:00 2001
From: aarav18 <57469559+aarav18@users.noreply.github.com>
Date: Thu, 13 Jan 2022 19:05:30 -0700
Subject: [PATCH 07/42] Update intake.java

---
 src/main/java/frc4388/robot/commands/intake.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/commands/intake.java b/src/main/java/frc4388/robot/commands/intake.java
index 04fc49c..dbf192a 100644
--- a/src/main/java/frc4388/robot/commands/intake.java
+++ b/src/main/java/frc4388/robot/commands/intake.java
@@ -6,7 +6,7 @@ package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class intake extends SubsystemBase {
+public class Intake extends SubsystemBase {
   /** Creates a new intake. */
   public intake() {}
 

From a58ee0c7952e48cf14450771a2375b9fa666a9b1 Mon Sep 17 00:00:00 2001
From: aarav18 <57469559+aarav18@users.noreply.github.com>
Date: Thu, 13 Jan 2022 19:13:59 -0700
Subject: [PATCH 08/42] Delete intake.java

---
 .../java/frc4388/robot/commands/intake.java     | 17 -----------------
 1 file changed, 17 deletions(-)
 delete mode 100644 src/main/java/frc4388/robot/commands/intake.java

diff --git a/src/main/java/frc4388/robot/commands/intake.java b/src/main/java/frc4388/robot/commands/intake.java
deleted file mode 100644
index dbf192a..0000000
--- a/src/main/java/frc4388/robot/commands/intake.java
+++ /dev/null
@@ -1,17 +0,0 @@
-// 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 frc4388.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-public class Intake extends SubsystemBase {
-  /** Creates a new intake. */
-  public intake() {}
-
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-  }
-}

From e3954863cd6c3e455daa92dd2833eb4a36f12ff9 Mon Sep 17 00:00:00 2001
From: aarav18 <57469559+aarav18@users.noreply.github.com>
Date: Thu, 13 Jan 2022 19:26:37 -0700
Subject: [PATCH 09/42] Update RobotContainer.java

---
 src/main/java/frc4388/robot/RobotContainer.java | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 7b69652..929f15c 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
+import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
@@ -39,6 +40,7 @@ public class RobotContainer {
     m_robotMap.rightBackEncoder
   );
 
+  private final Intake m_intake = new Intake(null, null);
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
   /* Controllers */
@@ -59,6 +61,7 @@ public class RobotContainer {
 
     // continually sends updates to the Blinkin LED controller to keep the lights on
     m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
+    // dri
   }
 
   /**
@@ -71,6 +74,10 @@ public class RobotContainer {
     /* Driver Buttons */
 
     /* Operator Buttons */
+    new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
+        .whenPressed(() -> m_intake.runExtender(true))
+        .whenReleased(() -> m_intake.runExtender(false));
+
     // activates "Lit Mode"
     new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
         .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))

From f483e4c21b42d39361ba448c10ffc30534737f99 Mon Sep 17 00:00:00 2001
From: aarav18 <57469559+aarav18@users.noreply.github.com>
Date: Thu, 13 Jan 2022 19:29:53 -0700
Subject: [PATCH 10/42] Update RobotContainer.java

---
 src/main/java/frc4388/robot/RobotContainer.java | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 929f15c..efad255 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -74,6 +74,8 @@ public class RobotContainer {
     /* Driver Buttons */
 
     /* Operator Buttons */
+
+    // extends and retracts the extender 
     new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
         .whenPressed(() -> m_intake.runExtender(true))
         .whenReleased(() -> m_intake.runExtender(false));

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 11/42] 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;
+    }
+}

From 3eef4fa0d05805df7805dd3663edd8b8923ae19c Mon Sep 17 00:00:00 2001
From: 66945 <66945@psdschools.org>
Date: Sat, 15 Jan 2022 14:26:36 -0700
Subject: [PATCH 12/42] Fished initializing

---
 src/main/java/frc4388/robot/RobotContainer.java | 2 +-
 src/main/java/frc4388/robot/RobotMap.java       | 5 +++++
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index cf0220c..7f2cbcf 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -40,7 +40,7 @@ public class RobotContainer {
     m_robotMap.rightBackEncoder
   );
 
-  private final Serializer m_robotSerializer = new Serializer();
+  private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, m_robotMap.serializerShooterBelt);
 
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 843323c..250c8cb 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -9,6 +9,7 @@ import com.ctre.phoenix.sensors.CANCoder;
 
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import frc4388.robot.Constants.LEDConstants;
+import frc4388.robot.Constants.SerializerConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 
 /**
@@ -92,4 +93,8 @@ public class RobotMap {
     //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
   }
 
+  /* Serializer Subsystem */
+
+  public final Spark serializerBelt = new Spark(SerializerConstants.SERIALIZER_BELT);
+  public final Spark serializerShooterBelt = new Spark(SerializerConstants.SERIALIZER_SHOOTER_BELT);
 }

From 2527ab20bef3db6a4a005cccb28c00df1ebe39cf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=AEZach=20Wilke?=
 <90875734+76842@users.noreply.github.com>
Date: Sat, 15 Jan 2022 15:21:46 -0700
Subject: [PATCH 13/42] b e a m

---
 src/main/java/frc4388/robot/Constants.java    |  5 ++-
 .../java/frc4388/robot/RobotContainer.java    |  4 ++
 .../frc4388/robot/subsystems/Serializer.java  | 43 ++++++++++++-------
 3 files changed, 34 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 68c6003..f7d5575 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -72,8 +72,9 @@ public final class Constants {
     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 int SERIALIZER_BELT = -1; // TODO
+    public static final int SERIALIZER_SHOOTER_BELT = -1; // TODO
+    public static final int SERIALIZER_BELT_BEAM = -1; // TODO
   }
 
   public static final class LEDConstants {
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 7f2cbcf..d1282b1 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -78,6 +78,10 @@ public class RobotContainer {
     new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
         .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+    
+    new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
+        .whenPressed(() -> m_robotSerializer.setSerializerStateWithBeam(true, m_robotSerializer.getBeam()))
+        .whenReleased(() -> m_robotSerializer.setSerializerStateWithBeam(false, m_robotSerializer.getBeam()));
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java
index ad25a41..141de75 100644
--- a/src/main/java/frc4388/robot/subsystems/Serializer.java
+++ b/src/main/java/frc4388/robot/subsystems/Serializer.java
@@ -1,31 +1,42 @@
 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;
+import edu.wpi.first.wpilibj.DigitalInput;
 
 public class Serializer extends SubsystemBase{
   private Spark m_serializerBelt;
   private Spark m_serializerShooterBelt;
-
+  private DigitalInput m_serializerBeam;
   private boolean serializerState;
 
-    public Serializer(Spark serializerBelt, Spark serializerShooterBelt) {
-        m_serializerBelt = serializerBelt;
-        m_serializerShooterBelt = serializerShooterBelt;
+  public Serializer(Spark serializerBelt, Spark serializerShooterBelt) {
+    m_serializerBelt = serializerBelt;
+    m_serializerShooterBelt = serializerShooterBelt;
+    m_serializerBeam = new DigitalInput(Constants.SerializerConstants.SERIALIZER_BELT_BEAM);
 
-        serializerState = false;
-        setSerializerState(serializerState);
-    }
+    serializerState = false;
+    setSerializerState(serializerState);
+  }
+  public boolean getBeam() {
+    return m_serializerBeam.get();
+  }
+  public void setSerializerStateWithBeam(boolean ctrlbutter, boolean beambroken) {
+    boolean total = ctrlbutter || beambroken;
+    setSerializerState(total);
+  }
+  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;
 
-    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);
 
-      m_serializerBelt.set(serializerBeltSpeed);
-      m_serializerShooterBelt.set(serializerShooterBeltSpeed);
-
-      serializerState = state;
-    }
+    serializerState = state;
+  }
+  
+  public boolean getSerializerState() {
+    return serializerState;
+  }
 }

From 1af611d2e47375d2377b5a2286f08616a832f664 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=AEZach=20Wilke?=
 <90875734+76842@users.noreply.github.com>
Date: Sat, 15 Jan 2022 15:27:56 -0700
Subject: [PATCH 14/42] Create readme.md

---
 readme.md | 1 +
 1 file changed, 1 insertion(+)
 create mode 100644 readme.md

diff --git a/readme.md b/readme.md
new file mode 100644
index 0000000..b6681c9
--- /dev/null
+++ b/readme.md
@@ -0,0 +1 @@
+# I don't know what to put here
\ No newline at end of file

From 23f9f7c5ba62219961fa56d845faf9982a215d00 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=AEZach=20Wilke?=
 <90875734+76842@users.noreply.github.com>
Date: Tue, 18 Jan 2022 18:04:58 -0700
Subject: [PATCH 15/42] Update Serializer.java

---
 .../frc4388/robot/subsystems/Serializer.java   | 18 ++++++++++++------
 1 file changed, 12 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java
index 141de75..06815e8 100644
--- a/src/main/java/frc4388/robot/subsystems/Serializer.java
+++ b/src/main/java/frc4388/robot/subsystems/Serializer.java
@@ -27,14 +27,20 @@ public class Serializer extends SubsystemBase{
     setSerializerState(total);
   }
   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);
-
+    setSerializerBeltState(state);
+    setSerializerShooterBeltState(state);
     serializerState = state;
   }
+
+  public void setSerializerBeltState(boolean state) {
+    double serializerBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_BELT_SPEED : 0.d;
+    m_serializerBelt.set(serializerBeltSpeed);
+  }
+
+  public void setSerializerShooterBeltState(boolean state) {
+    double serializerShooterBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_SHOOTER_BELT_SPEED : 0.d;
+    m_serializerShooterBelt.set(serializerShooterBeltSpeed);
+  }
   
   public boolean getSerializerState() {
     return serializerState;

From 21294706a51b49bc6c3d5a83bd73207473d80ae2 Mon Sep 17 00:00:00 2001
From: aborunda <90010674+aborunda@users.noreply.github.com>
Date: Thu, 20 Jan 2022 18:08:31 -0700
Subject: [PATCH 16/42] Update Intake.java

---
 src/main/java/frc4388/robot/subsystems/Intake.java | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 7db1b7f..8c34527 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -14,9 +14,9 @@ public class Intake extends SubsystemBase {
   private WPI_TalonFX m_extenderMotor;
 
   /** Creates a new Intake. */
-  public Intake(WPI_TalonFX m_intakeMotor, WPI_TalonFX m_extenderMotor) {
-    this.m_intakeMotor = m_intakeMotor;
-    this.m_extenderMotor = m_extenderMotor; 
+  public Intake(WPI_TalonFX intakeMotor, WPI_TalonFX extenderMotor) {
+    m_intakeMotor = intakeMotor;
+    m_extenderMotor = extenderMotor; 
   }
 
   @Override
@@ -29,6 +29,12 @@ public class Intake extends SubsystemBase {
   }
 
   public void runExtender(boolean extended) {
-    m_extenderMotor.set(extended ? 1 : 0);
+    m_extenderMotor.set(extended ? 1 : -1);
   }
 }
+
+/*
+ Function toggle extender
+            bool = !bool
+Configure limit switches forward and reverse
+*/
\ No newline at end of file

From c6083d8276c019b5ad4664b719adc3298d260cb0 Mon Sep 17 00:00:00 2001
From: aborunda <90010674+aborunda@users.noreply.github.com>
Date: Thu, 20 Jan 2022 18:10:55 -0700
Subject: [PATCH 17/42] Update RobotContainer.java

---
 src/main/java/frc4388/robot/RobotContainer.java | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index efad255..85b3bf4 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -40,7 +40,7 @@ public class RobotContainer {
     m_robotMap.rightBackEncoder
   );
 
-  private final Intake m_intake = new Intake(null, null);
+  private final Intake m_robotIntake = new Intake(null, null);
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
   /* Controllers */
@@ -59,8 +59,12 @@ public class RobotContainer {
         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDriverController().getLeftXAxis(),
             getDriverController().getLeftYAxis(), getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
 
+    m_robotIntake.setDefaultCommand(
+        new RunCommand(() -> m_robotIntake.runWithTriggers(
+          getDriverController().getLeftTriggerAxis(), getDriverController().getRightTriggerAxis()),m_robotIntake));
     // continually sends updates to the Blinkin LED controller to keep the lights on
-    m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
+    m_robotLED.setDefaultCommand(
+        new RunCommand(m_robotLED::updateLED, m_robotLED));                      
     // dri
   }
 
@@ -77,8 +81,8 @@ public class RobotContainer {
 
     // extends and retracts the extender 
     new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-        .whenPressed(() -> m_intake.runExtender(true))
-        .whenReleased(() -> m_intake.runExtender(false));
+        .whenPressed(() -> m_robotIntake.runExtender(true))
+        .whenReleased(() -> m_robotIntake.runExtender(false));
 
     // activates "Lit Mode"
     new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)

From e9bb3890b1ab11a1f270f32095f5ddcc17c33214 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=AEZach=20Wilke?=
 <90875734+76842@users.noreply.github.com>
Date: Fri, 21 Jan 2022 16:38:44 -0700
Subject: [PATCH 18/42] Update .gitignore

---
 .gitignore | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.gitignore b/.gitignore
index 9535c83..de0a2d1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -160,3 +160,4 @@ bin/
 
 # Simulation GUI and other tools window save file
 *-window.json
+.vscode/launch.json

From 5b14ed76ece5ae8cc0c3577826a090602e03af57 Mon Sep 17 00:00:00 2001
From: aborunda <90010674+aborunda@users.noreply.github.com>
Date: Fri, 21 Jan 2022 17:09:53 -0700
Subject: [PATCH 19/42] Create REVLib.json

---
 vendordeps/REVLib.json | 73 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 73 insertions(+)
 create mode 100644 vendordeps/REVLib.json

diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
new file mode 100644
index 0000000..997e2a4
--- /dev/null
+++ b/vendordeps/REVLib.json
@@ -0,0 +1,73 @@
+{
+    "fileName": "REVLib.json",
+    "name": "REVLib",
+    "version": "2022.1.1",
+    "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+    "mavenUrls": [
+        "https://maven.revrobotics.com/"
+    ],
+    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-java",
+            "version": "2022.1.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-driver",
+            "version": "2022.1.1",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxaarch64bionic",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxraspbian",
+                "osxx86-64"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-cpp",
+            "version": "2022.1.1",
+            "libName": "REVLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxaarch64bionic",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxraspbian",
+                "osxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-driver",
+            "version": "2022.1.1",
+            "libName": "REVLibDriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxaarch64bionic",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxraspbian",
+                "osxx86-64"
+            ]
+        }
+    ]
+}
\ No newline at end of file

From 1e498bef56eba5fce2d3ce7d35831de721c0bc21 Mon Sep 17 00:00:00 2001
From: aborunda <90010674+aborunda@users.noreply.github.com>
Date: Fri, 21 Jan 2022 17:09:57 -0700
Subject: [PATCH 20/42] Update Intake.java

---
 .../java/frc4388/robot/subsystems/Intake.java | 42 ++++++++++++++-----
 1 file changed, 32 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 8c34527..571e332 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -4,19 +4,42 @@
 
 package frc4388.robot.subsystems;
 
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.SparkMaxLimitSwitch;
+import com.revrobotics.CANSparkMax.IdleMode;
 
 public class Intake extends SubsystemBase {
 
   private WPI_TalonFX m_intakeMotor;
-  private WPI_TalonFX m_extenderMotor;
+  private CANSparkMax m_extenderMotor;
+  private SparkMaxLimitSwitch m_inLimit;
+  private SparkMaxLimitSwitch m_outLimit;
+
+  public boolean toggle;
 
   /** Creates a new Intake. */
-  public Intake(WPI_TalonFX intakeMotor, WPI_TalonFX extenderMotor) {
+  public Intake(WPI_TalonFX intakeMotor, Spark extenderMotor) {
     m_intakeMotor = intakeMotor;
-    m_extenderMotor = extenderMotor; 
+    //m_extenderMotor = extenderMotor; 
+
+    
+    // m_intakeMotor.restoreFactoryDefaults();
+    // m_extenderMotor.restoreFactoryDefaults();
+
+    m_intakeMotor.setNeutralMode(NeutralMode.Brake);
+    m_extenderMotor.setIdleMode(IdleMode.kBrake);
+    m_intakeMotor.setInverted(false);
+    m_extenderMotor.setInverted(true);
+    
+    m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_inLimit.enableLimitSwitch(true);
+    m_outLimit.enableLimitSwitch(true);
   }
 
   @Override
@@ -29,12 +52,11 @@ public class Intake extends SubsystemBase {
   }
 
   public void runExtender(boolean extended) {
-    m_extenderMotor.set(extended ? 1 : -1);
+    //m_extenderMotor.set(extended ? 1 : -1);
   }
-}
 
-/*
- Function toggle extender
-            bool = !bool
-Configure limit switches forward and reverse
-*/
\ No newline at end of file
+  public void toggleExtender() {
+    toggle = !toggle;
+    runExtender(toggle);
+  }
+}
\ No newline at end of file

From 9112c871561236aeb342ed1338914e60c95945ad Mon Sep 17 00:00:00 2001
From: Ryan Manley <ryan123rudder@gmail.com>
Date: Fri, 21 Jan 2022 19:00:11 -0700
Subject: [PATCH 21/42] Add Intake to robotmap and constants

Co-Authored-By: 76842 <90875734+76842@users.noreply.github.com>
---
 src/main/java/frc4388/robot/Constants.java      | 12 ++++++++----
 src/main/java/frc4388/robot/RobotContainer.java |  2 +-
 src/main/java/frc4388/robot/RobotMap.java       |  5 +++++
 3 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index f7d5575..48d9057 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -72,11 +72,15 @@ public final class Constants {
     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 int SERIALIZER_BELT_BEAM = -1; // TODO
+    public static final int SERIALIZER_BELT = 1; // TODO
+    public static final int SERIALIZER_SHOOTER_BELT = 2; // TODO
+    public static final int SERIALIZER_BELT_BEAM = 3; // TODO
+  }
+  public static final class IntakeConstants {
+    // CAN IDs
+    public static final int INTAKE_MOTOR = 3;
+    public static final int EXTENDER_MOTOR = 4;
   }
-
   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 362c955..2d1eba1 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -41,7 +41,7 @@ public class RobotContainer {
     m_robotMap.rightBackEncoder
   );
 
-  private final Intake m_robotIntake = new Intake(null, null);
+  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);
 
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 250c8cb..26af808 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.SerializerConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
@@ -97,4 +98,8 @@ public class RobotMap {
 
   public final Spark serializerBelt = new Spark(SerializerConstants.SERIALIZER_BELT);
   public final Spark serializerShooterBelt = new Spark(SerializerConstants.SERIALIZER_SHOOTER_BELT);
+  
+  /* Intake Subsytem */
+  public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
+  public final Spark extenderMotor = new Spark(IntakeConstants.EXTENDER_MOTOR);
 }

From f1f1de18bb1aebe299dcac422e4ee280888e1bcf Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Tue, 25 Jan 2022 17:17:07 -0700
Subject: [PATCH 22/42] Done Test code

Co-Authored-By: 76842 <90875734+76842@users.noreply.github.com>
---
 src/main/java/frc4388/robot/Constants.java    | 10 ++++----
 .../java/frc4388/robot/RobotContainer.java    | 20 ++++++++--------
 src/main/java/frc4388/robot/RobotMap.java     | 12 ++++++----
 .../java/frc4388/robot/subsystems/Intake.java | 23 +++++++++----------
 .../frc4388/robot/subsystems/Serializer.java  | 18 +++++++++++----
 5 files changed, 46 insertions(+), 37 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 48d9057..3651898 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -68,18 +68,18 @@ public final class Constants {
   }
 
   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)
+    public static final double SERIALIZER_BELT_SPEED = 1.d; // TODO (currently max power, DO NOT RUN)
+    public static final double SERIALIZER_SHOOTER_BELT_SPEED = 1.d; // TODO (currently max power, DO NOT RUN)
     
     // CAN IDs
-    public static final int SERIALIZER_BELT = 1; // TODO
-    public static final int SERIALIZER_SHOOTER_BELT = 2; // TODO
+    public static final int SERIALIZER_BELT = 2; // TODO
+    public static final int SERIALIZER_SHOOTER_BELT = 5; // TODO
     public static final int SERIALIZER_BELT_BEAM = 3; // TODO
   }
   public static final class IntakeConstants {
     // CAN IDs
     public static final int INTAKE_MOTOR = 3;
-    public static final int EXTENDER_MOTOR = 4;
+    public static final int EXTENDER_MOTOR = 6;
   }
   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 2d1eba1..c2bdb96 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -29,7 +29,7 @@ public class RobotContainer {
   /* RobotMap */
   private final RobotMap m_robotMap = new RobotMap();
 
-  /* Subsystems */
+  /* Subsystems 
   private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
     m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor,
     m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor,
@@ -40,7 +40,7 @@ public class RobotContainer {
     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);
 
@@ -58,9 +58,9 @@ public class RobotContainer {
 
     /* Default Commands */
     // drives the swerve drive with a two-axis input from the driver controller
-    m_robotSwerveDrive.setDefaultCommand(
-        new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
-            getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
+    // m_robotSwerveDrive.setDefaultCommand(
+    //     new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
+    //         getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
 
     m_robotIntake.setDefaultCommand(
         new RunCommand(() -> m_robotIntake.runWithTriggers(
@@ -88,13 +88,13 @@ public class RobotContainer {
         .whenReleased(() -> m_robotIntake.runExtender(false));
 
     // activates "Lit Mode"
-    new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-        .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
-        .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+    // new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
+    //     .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
+    //     .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
     
     new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
-        .whenPressed(() -> m_robotSerializer.setSerializerStateWithBeam(true, m_robotSerializer.getBeam()))
-        .whenReleased(() -> m_robotSerializer.setSerializerStateWithBeam(false, m_robotSerializer.getBeam()));
+        .whenPressed(() -> m_robotSerializer.setSerializerState(true))
+        .whenReleased(() -> m_robotSerializer.setSerializerState(false));
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 26af808..c9f4012 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -6,12 +6,14 @@ package frc4388.robot;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
+import com.revrobotics.CANSparkMax;
 
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.SerializerConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 /**
  * Defines and holds all I/O objects on the Roborio. This is useful for unit
@@ -21,7 +23,7 @@ public class RobotMap {
 
   public RobotMap() {
     configureLEDMotorControllers();
-    configureSwerveMotorControllers();
+    //configureSwerveMotorControllers();
   }
 
   /* LED Subsystem */
@@ -31,7 +33,7 @@ public class RobotMap {
     
   }
   
-  /* Swerve Subsystem */
+  /* Swerve Subsystem 
   public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
   public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
   public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
@@ -96,10 +98,10 @@ public class RobotMap {
 
   /* Serializer Subsystem */
 
-  public final Spark serializerBelt = new Spark(SerializerConstants.SERIALIZER_BELT);
-  public final Spark serializerShooterBelt = new Spark(SerializerConstants.SERIALIZER_SHOOTER_BELT);
+  public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
+  public final CANSparkMax serializerShooterBelt = new CANSparkMax(SerializerConstants.SERIALIZER_SHOOTER_BELT, MotorType.kBrushless);
   
   /* Intake Subsytem */
   public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
-  public final Spark extenderMotor = new Spark(IntakeConstants.EXTENDER_MOTOR);
+  public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 571e332..7d4997e 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -17,29 +17,27 @@ public class Intake extends SubsystemBase {
 
   private WPI_TalonFX m_intakeMotor;
   private CANSparkMax m_extenderMotor;
-  private SparkMaxLimitSwitch m_inLimit;
-  private SparkMaxLimitSwitch m_outLimit;
+  // private SparkMaxLimitSwitch m_inLimit;
+  // private SparkMaxLimitSwitch m_outLimit;
 
   public boolean toggle;
 
   /** Creates a new Intake. */
-  public Intake(WPI_TalonFX intakeMotor, Spark extenderMotor) {
+  public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor) {
     m_intakeMotor = intakeMotor;
-    //m_extenderMotor = extenderMotor; 
+    m_extenderMotor = extenderMotor; 
 
     
-    // m_intakeMotor.restoreFactoryDefaults();
-    // m_extenderMotor.restoreFactoryDefaults();
+    m_extenderMotor.restoreFactoryDefaults();
 
     m_intakeMotor.setNeutralMode(NeutralMode.Brake);
-    m_extenderMotor.setIdleMode(IdleMode.kBrake);
     m_intakeMotor.setInverted(false);
     m_extenderMotor.setInverted(true);
     
-    m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
-    m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
-    m_inLimit.enableLimitSwitch(true);
-    m_outLimit.enableLimitSwitch(true);
+    // m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    // m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    // m_inLimit.enableLimitSwitch(true);
+    // m_outLimit.enableLimitSwitch(true);
   }
 
   @Override
@@ -52,7 +50,8 @@ public class Intake extends SubsystemBase {
   }
 
   public void runExtender(boolean extended) {
-    //m_extenderMotor.set(extended ? 1 : -1);
+    double extenderMotorSpeed = extended ? 0.25d : 0.d;
+    m_extenderMotor.set(extenderMotorSpeed);
   }
 
   public void toggleExtender() {
diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java
index 06815e8..8ef9612 100644
--- a/src/main/java/frc4388/robot/subsystems/Serializer.java
+++ b/src/main/java/frc4388/robot/subsystems/Serializer.java
@@ -4,37 +4,45 @@ import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants;
 import edu.wpi.first.wpilibj.DigitalInput;
+import com.revrobotics.CANSparkMax;
 
 public class Serializer extends SubsystemBase{
-  private Spark m_serializerBelt;
-  private Spark m_serializerShooterBelt;
+  private CANSparkMax m_serializerBelt;
+  private CANSparkMax m_serializerShooterBelt;
   private DigitalInput m_serializerBeam;
   private boolean serializerState;
 
-  public Serializer(Spark serializerBelt, Spark serializerShooterBelt) {
+  public Serializer(CANSparkMax serializerBelt, CANSparkMax serializerShooterBelt) {
     m_serializerBelt = serializerBelt;
     m_serializerShooterBelt = serializerShooterBelt;
-    m_serializerBeam = new DigitalInput(Constants.SerializerConstants.SERIALIZER_BELT_BEAM);
+    //m_serializerBeam = new DigitalInput(Constants.SerializerConstants.SERIALIZER_BELT_BEAM);
 
     serializerState = false;
     setSerializerState(serializerState);
+    m_serializerBelt.set(0);
+    m_serializerShooterBelt.set(0);
+    
   }
   public boolean getBeam() {
-    return m_serializerBeam.get();
+    System.out.println("oi");
+    return false;
   }
   public void setSerializerStateWithBeam(boolean ctrlbutter, boolean beambroken) {
     boolean total = ctrlbutter || beambroken;
     setSerializerState(total);
   }
   public void setSerializerState(boolean state) {
+    System.out.println(state);
     setSerializerBeltState(state);
     setSerializerShooterBeltState(state);
     serializerState = state;
   }
 
   public void setSerializerBeltState(boolean state) {
+    System.out.println("oi");
     double serializerBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_BELT_SPEED : 0.d;
     m_serializerBelt.set(serializerBeltSpeed);
+    System.out.println("oi2");
   }
 
   public void setSerializerShooterBeltState(boolean state) {

From 2814a5b44236f79b964960e66f308cfbd89435a3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=AEZach=20Wilke?=
 <90875734+76842@users.noreply.github.com>
Date: Tue, 25 Jan 2022 17:58:23 -0700
Subject: [PATCH 23/42] S T O R A G E

---
 src/main/java/frc4388/robot/Constants.java    |  5 +++
 .../frc4388/robot/subsystems/Serializer.java  |  3 --
 .../frc4388/robot/subsystems/Storage.java     | 39 +++++++++++++++++++
 3 files changed, 44 insertions(+), 3 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/subsystems/Storage.java

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 3651898..23afeef 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -81,6 +81,11 @@ public final class Constants {
     public static final int INTAKE_MOTOR = 3;
     public static final int EXTENDER_MOTOR = 6;
   }
+  public static final class StorageConstants {
+    public static final int STORAGE_CAN_ID = -1; //TODO
+    public static final int BEAM_SENSOR_SHOOTER = -1; //TODO
+    public static final int BEAM_SENSOR_INTAKE = -1; //TODO
+  }
   public static final class LEDConstants {
     public static final int LED_SPARK_ID = 0;
 
diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java
index 8ef9612..2d76887 100644
--- a/src/main/java/frc4388/robot/subsystems/Serializer.java
+++ b/src/main/java/frc4388/robot/subsystems/Serializer.java
@@ -32,17 +32,14 @@ public class Serializer extends SubsystemBase{
     setSerializerState(total);
   }
   public void setSerializerState(boolean state) {
-    System.out.println(state);
     setSerializerBeltState(state);
     setSerializerShooterBeltState(state);
     serializerState = state;
   }
 
   public void setSerializerBeltState(boolean state) {
-    System.out.println("oi");
     double serializerBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_BELT_SPEED : 0.d;
     m_serializerBelt.set(serializerBeltSpeed);
-    System.out.println("oi2");
   }
 
   public void setSerializerShooterBeltState(boolean state) {
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
new file mode 100644
index 0000000..fa03e2e
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -0,0 +1,39 @@
+// 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 frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+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);
+
+  /** Creates a new Storage. */
+  public Storage() {
+
+  }
+
+  public void runStorage(double input) {
+    m_storageMotor.set(input);
+  }
+
+  public boolean getBeamShooter(){
+    return m_beamShooter.get();
+  }
+
+  public boolean getBeamIntake(){
+    return m_beamIntake.get();
+  }
+  
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

From 27102b6dd52abcc471977b79ed0a27fb9ddb5bb0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=AEZach=20Wilke?=
 <90875734+76842@users.noreply.github.com>
Date: Tue, 25 Jan 2022 18:04:34 -0700
Subject: [PATCH 24/42] manageStorage

---
 src/main/java/frc4388/robot/subsystems/Storage.java | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index fa03e2e..e2c65f2 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -19,7 +19,11 @@ public class Storage extends SubsystemBase {
   public Storage() {
 
   }
-
+  public void manageStorage() {
+    if (m_beamShooter.get()) {
+      runStorage(1.d);
+    } else { runStorage(0.d); }
+  }
   public void runStorage(double input) {
     m_storageMotor.set(input);
   }
@@ -31,7 +35,7 @@ public class Storage extends SubsystemBase {
   public boolean getBeamIntake(){
     return m_beamIntake.get();
   }
-  
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

From 5f1411b5184cd77719170d014ec23d3f15bd3fa1 Mon Sep 17 00:00:00 2001
From: aborunda <90010674+aborunda@users.noreply.github.com>
Date: Thu, 27 Jan 2022 18:44:55 -0700
Subject: [PATCH 25/42] Update Intake.java

---
 src/main/java/frc4388/robot/subsystems/Intake.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 7d4997e..2fd2531 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -58,4 +58,5 @@ public class Intake extends SubsystemBase {
     toggle = !toggle;
     runExtender(toggle);
   }
+  //Test
 }
\ No newline at end of file

From 1ceef732a2d2056d281d4588e578eea8c6a90711 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=AEZach=20Wilke?=
 <90875734+76842@users.noreply.github.com>
Date: Fri, 28 Jan 2022 16:14:07 -0700
Subject: [PATCH 26/42] Update Storage.java

---
 src/main/java/frc4388/robot/subsystems/Storage.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index e2c65f2..9fc2df0 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -38,6 +38,6 @@ public class Storage extends SubsystemBase {
 
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
+    manageStorage();
   }
 }

From 61819114e490022f1f53d3391a30a34e71352831 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=AEZach=20Wilke?=
 <90875734+76842@users.noreply.github.com>
Date: Fri, 28 Jan 2022 17:59:25 -0700
Subject: [PATCH 27/42] ready code

---
 src/main/java/frc4388/robot/RobotContainer.java        | 10 +++++-----
 src/main/java/frc4388/robot/subsystems/Serializer.java |  5 ++---
 2 files changed, 7 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index c2bdb96..c352bc2 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -88,13 +88,13 @@ public class RobotContainer {
         .whenReleased(() -> m_robotIntake.runExtender(false));
 
     // activates "Lit Mode"
-    // new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-    //     .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
-    //     .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+    new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
+        .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
+        .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
     
     new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
-        .whenPressed(() -> m_robotSerializer.setSerializerState(true))
-        .whenReleased(() -> m_robotSerializer.setSerializerState(false));
+        .whenPressed(() -> m_robotSerializer.setSerializerStateWithBeam(true, m_robotSerializer.getBeam()))
+        .whenReleased(() -> m_robotSerializer.setSerializerStateWithBeam(false, m_robotSerializer.getBeam()));
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java
index 2d76887..e9ec997 100644
--- a/src/main/java/frc4388/robot/subsystems/Serializer.java
+++ b/src/main/java/frc4388/robot/subsystems/Serializer.java
@@ -15,7 +15,7 @@ public class Serializer extends SubsystemBase{
   public Serializer(CANSparkMax serializerBelt, CANSparkMax serializerShooterBelt) {
     m_serializerBelt = serializerBelt;
     m_serializerShooterBelt = serializerShooterBelt;
-    //m_serializerBeam = new DigitalInput(Constants.SerializerConstants.SERIALIZER_BELT_BEAM);
+    m_serializerBeam = new DigitalInput(Constants.SerializerConstants.SERIALIZER_BELT_BEAM);
 
     serializerState = false;
     setSerializerState(serializerState);
@@ -24,8 +24,7 @@ public class Serializer extends SubsystemBase{
     
   }
   public boolean getBeam() {
-    System.out.println("oi");
-    return false;
+    return m_serializerBeam.get();
   }
   public void setSerializerStateWithBeam(boolean ctrlbutter, boolean beambroken) {
     boolean total = ctrlbutter || beambroken;

From 4dabf7021e1b13fbe613aa2ebb925c06d5e87fb3 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Thu, 3 Feb 2022 19:51:12 -0700
Subject: [PATCH 28/42] Update Intake.java

---
 src/main/java/frc4388/robot/subsystems/Intake.java | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 2fd2531..ffb1b94 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -4,6 +4,10 @@
 
 package frc4388.robot.subsystems;
 
+//Imported Limit switch ONLY
+import com.revrobotics.SparkMaxLimitSwitch;
+import com.revrobotics.SparkMaxLimitSwitch.Type;
+
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 

From 44870c8aa13ca5c9fc89f680caf7c83af9d4158b Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Tue, 8 Feb 2022 17:24:17 -0700
Subject: [PATCH 29/42] limit switches

---
 simgui-ds.json                                | 101 ++++++++++++++++++
 simgui.json                                   |  26 +++++
 .../java/frc4388/robot/subsystems/Intake.java |  14 ++-
 3 files changed, 133 insertions(+), 8 deletions(-)
 create mode 100644 simgui-ds.json
 create mode 100644 simgui.json

diff --git a/simgui-ds.json b/simgui-ds.json
new file mode 100644
index 0000000..b16ea5c
--- /dev/null
+++ b/simgui-ds.json
@@ -0,0 +1,101 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "useGamepad": true
+    },
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    }
+  ]
+}
diff --git a/simgui.json b/simgui.json
new file mode 100644
index 0000000..4af852e
--- /dev/null
+++ b/simgui.json
@@ -0,0 +1,26 @@
+{
+  "HALProvider": {
+    "Other Devices": {
+      "SPARK MAX [5]": {
+        "header": {
+          "open": true
+        }
+      },
+      "SPARK MAX [6]": {
+        "header": {
+          "open": true
+        }
+      }
+    }
+  },
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/LiveWindow/Intake": "Subsystem",
+      "/LiveWindow/LED": "Subsystem",
+      "/LiveWindow/Serializer": "Subsystem",
+      "/LiveWindow/Ungrouped/DigitalInput[3]": "Digital Input",
+      "/LiveWindow/Ungrouped/Scheduler": "Scheduler"
+    }
+  }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index ffb1b94..9bc97d7 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -14,15 +14,14 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import com.revrobotics.CANSparkMax;
-import com.revrobotics.SparkMaxLimitSwitch;
 import com.revrobotics.CANSparkMax.IdleMode;
 
 public class Intake extends SubsystemBase {
 
   private WPI_TalonFX m_intakeMotor;
   private CANSparkMax m_extenderMotor;
-  // private SparkMaxLimitSwitch m_inLimit;
-  // private SparkMaxLimitSwitch m_outLimit;
+  private SparkMaxLimitSwitch m_inLimit;
+  private SparkMaxLimitSwitch m_outLimit;
 
   public boolean toggle;
 
@@ -31,17 +30,16 @@ public class Intake extends SubsystemBase {
     m_intakeMotor = intakeMotor;
     m_extenderMotor = extenderMotor; 
 
-    
     m_extenderMotor.restoreFactoryDefaults();
 
     m_intakeMotor.setNeutralMode(NeutralMode.Brake);
     m_intakeMotor.setInverted(false);
     m_extenderMotor.setInverted(true);
     
-    // m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
-    // m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
-    // m_inLimit.enableLimitSwitch(true);
-    // m_outLimit.enableLimitSwitch(true);
+    m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_inLimit.enableLimitSwitch(true);
+    m_outLimit.enableLimitSwitch(true);
   }
 
   @Override

From 8e9f1d31cb41ffa09b0886edd79fae82839384ec Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=AEAarav=20Shah?= <76842@psdschools.org>
Date: Fri, 18 Feb 2022 16:38:22 -0700
Subject: [PATCH 30/42] new hood controls

---
 src/main/java/frc4388/robot/RobotContainer.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index c352bc2..e4324af 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -79,7 +79,7 @@ public class RobotContainer {
    */
   private void configureButtonBindings() {
     /* Driver Buttons */
-
+    
     /* Operator Buttons */
 
     // extends and retracts the extender 

From 820ed2ee9060fa61f3e604e562dd2665d5192ca0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=AEAarav=20Shah?= <76842@psdschools.org>
Date: Fri, 18 Feb 2022 17:25:21 -0700
Subject: [PATCH 31/42] boom boom bug fix

---
 build.gradle | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/build.gradle b/build.gradle
index c1f07ee..7e60634 100644
--- a/build.gradle
+++ b/build.gradle
@@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO
 
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2022.1.1"
+    id "edu.wpi.first.GradleRIO" version "2022.2.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

From 0baff66d9de2cacd9b17c7d2a928cc37ec1b325f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=AEAarav=20Shah?= <76842@psdschools.org>
Date: Fri, 18 Feb 2022 19:33:32 -0700
Subject: [PATCH 32/42] intake buttons (incomplete)

---
 src/main/java/frc4388/robot/RobotContainer.java | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index e4324af..1a60a13 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -91,10 +91,16 @@ public class RobotContainer {
     new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
         .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
-    
+    // revisit this later, not sure if we will still use this 
     new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
         .whenPressed(() -> m_robotSerializer.setSerializerStateWithBeam(true, m_robotSerializer.getBeam()))
         .whenReleased(() -> m_robotSerializer.setSerializerStateWithBeam(false, m_robotSerializer.getBeam()));
+        
+    new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
+        .whenPressed(() -> m_robotIntake.runExtender(true)); 
+
+    new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
+      .whenPressed(() -> m_robotIntake.runExtender(false));
   }
 
   /**

From fcdda5682d27e339502f65d0efa861d917a902b2 Mon Sep 17 00:00:00 2001
From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 26 Feb 2022 15:41:54 -0700
Subject: [PATCH 33/42] Cleaning

---
 .../java/frc4388/robot/RobotContainer.java    | 16 +++++++++-----
 src/main/java/frc4388/robot/RobotMap.java     |  9 +++++++-
 .../frc4388/robot/subsystems/Serializer.java  |  4 ++--
 .../frc4388/robot/subsystems/Storage.java     | 22 ++++++++++---------
 4 files changed, 32 insertions(+), 19 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 1a60a13..4949708 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -13,6 +13,7 @@ import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Serializer;
+import frc4388.robot.subsystems.Storage;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.controller.IHandController;
@@ -42,8 +43,8 @@ public class RobotContainer {
   );
   */
   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);
-
+  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);
 
   /* Controllers */
@@ -63,12 +64,15 @@ public class RobotContainer {
     //         getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
 
     m_robotIntake.setDefaultCommand(
-        new RunCommand(() -> m_robotIntake.runWithTriggers(
-          getDriverController().getLeftTriggerAxis(), getDriverController().getRightTriggerAxis()),m_robotIntake));
+      new RunCommand(() -> m_robotIntake.runWithTriggers(
+                                        getDriverController().getLeftTriggerAxis(),
+                                        getDriverController().getRightTriggerAxis()),
+                                        m_robotIntake));
     // continually sends updates to the Blinkin LED controller to keep the lights on
     m_robotLED.setDefaultCommand(
-        new RunCommand(m_robotLED::updateLED, m_robotLED));                      
-    // dri
+      new RunCommand(m_robotLED::updateLED, m_robotLED));                      
+    m_robotStorage.setDefaultCommand(
+      new RunCommand(m_robotStorage::manageStorage, m_robotStorage));
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index c9f4012..1085616 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -8,10 +8,12 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.revrobotics.CANSparkMax;
 
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.SerializerConstants;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
@@ -97,11 +99,16 @@ 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);
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java
index e9ec997..f902fb0 100644
--- a/src/main/java/frc4388/robot/subsystems/Serializer.java
+++ b/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) {
     m_serializerBelt = serializerBelt;
     m_serializerShooterBelt = serializerShooterBelt;
-    m_serializerBeam = new DigitalInput(Constants.SerializerConstants.SERIALIZER_BELT_BEAM);
+    m_serializerBeam = serializerBeam;
 
     serializerState = false;
     setSerializerState(serializerState);
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 9fc2df0..2a63bbc 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -11,16 +11,18 @@ 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;
   }
   public void manageStorage() {
-    if (m_beamShooter.get()) {
+    if (isBeamIntakeBroken()) { //Maybe needs to be shooter
       runStorage(1.d);
     } else { runStorage(0.d); }
   }
@@ -28,12 +30,12 @@ public class Storage extends SubsystemBase {
     m_storageMotor.set(input);
   }
 
-  public boolean getBeamShooter(){
-    return m_beamShooter.get();
+  public boolean isBeamShooterBroken(){
+    return !m_beamShooter.get();
   }
 
-  public boolean getBeamIntake(){
-    return m_beamIntake.get();
+  public boolean isBeamIntakeBroken(){
+    return !m_beamIntake.get();
   }
 
   @Override

From 70c17009c9a7032c3a269a1ae0dd40d9e02d7f0c Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Mon, 28 Feb 2022 16:43:44 -0700
Subject: [PATCH 34/42] Note

---
 src/main/java/frc4388/robot/subsystems/Intake.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 9bc97d7..f87227c 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -51,7 +51,7 @@ public class Intake extends SubsystemBase {
     m_intakeMotor.set(rightTrigger - leftTrigger);
   }
 
-  public void runExtender(boolean extended) {
+  public void runExtender(boolean extended) { //TODO: Do not bring intake in if there is a ball in the extender (check if intake being in brakes the beam brake?)
     double extenderMotorSpeed = extended ? 0.25d : 0.d;
     m_extenderMotor.set(extenderMotorSpeed);
   }

From 8ac9dcddca1343a37bc2807bab3941ec15f046f0 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Mon, 28 Feb 2022 16:44:54 -0700
Subject: [PATCH 35/42] note

---
 src/main/java/frc4388/robot/subsystems/Serializer.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java
index f902fb0..270d92a 100644
--- a/src/main/java/frc4388/robot/subsystems/Serializer.java
+++ b/src/main/java/frc4388/robot/subsystems/Serializer.java
@@ -12,7 +12,7 @@ public class Serializer extends SubsystemBase{
   private DigitalInput m_serializerBeam;
   private boolean serializerState;
 
-  public Serializer(CANSparkMax serializerBelt, CANSparkMax serializerShooterBelt, DigitalInput serializerBeam) {
+  public Serializer(CANSparkMax serializerBelt, CANSparkMax serializerShooterBelt, DigitalInput serializerBeam) { //TODO: Only one motor lol
     m_serializerBelt = serializerBelt;
     m_serializerShooterBelt = serializerShooterBelt;
     m_serializerBeam = serializerBeam;
@@ -26,7 +26,7 @@ public class Serializer extends SubsystemBase{
   public boolean getBeam() {
     return m_serializerBeam.get();
   }
-  public void setSerializerStateWithBeam(boolean ctrlbutter, boolean beambroken) {
+  public void setSerializerStateWithBeam(boolean ctrlbutter, boolean beambroken) { 
     boolean total = ctrlbutter || beambroken;
     setSerializerState(total);
   }

From 28967825af462a30488e40730c22d1ce6a972e76 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=AEAarav=20Shah?= <76842@psdschools.org>
Date: Mon, 28 Feb 2022 16:57:41 -0700
Subject: [PATCH 36/42] set intake

---
 .../java/frc4388/robot/RobotContainer.java    | 12 +--
 .../java/frc4388/robot/subsystems/Hood.java   | 88 +++++++++++++++++++
 .../java/frc4388/robot/subsystems/Intake.java |  6 +-
 3 files changed, 97 insertions(+), 9 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/subsystems/Hood.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 1a60a13..039eaf9 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -84,8 +84,8 @@ public class RobotContainer {
 
     // extends and retracts the extender 
     new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-        .whenPressed(() -> m_robotIntake.runExtender(true))
-        .whenReleased(() -> m_robotIntake.runExtender(false));
+        .whenPressed(() -> m_robotIntake.extendExtender(true))
+        .whenReleased(() -> m_robotIntake.extendExtender(false));
 
     // activates "Lit Mode"
     new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
@@ -96,11 +96,11 @@ public class RobotContainer {
         .whenPressed(() -> m_robotSerializer.setSerializerStateWithBeam(true, m_robotSerializer.getBeam()))
         .whenReleased(() -> m_robotSerializer.setSerializerStateWithBeam(false, m_robotSerializer.getBeam()));
         
-    new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-        .whenPressed(() -> m_robotIntake.runExtender(true)); 
+    // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
+        // .whenPressed(() -> m_robotIntake.extendExtender(true)); 
 
-    new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-      .whenPressed(() -> m_robotIntake.runExtender(false));
+    new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
+      .whenPressed(() -> m_robotIntake.toggleExtender());
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java
new file mode 100644
index 0000000..3895645
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Hood.java
@@ -0,0 +1,88 @@
+// 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 frc4388.robot.subsystems;
+
+import javax.sql.rowset.WebRowSet;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxPIDController;
+import com.revrobotics.SparkMaxLimitSwitch;
+import com.revrobotics.CANSparkMax.ControlType;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMax.SoftLimitDirection;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+// import frc4388.robot.Constants.ShooterConstants;
+import frc4388.utility.Gains;
+
+public class Hood extends SubsystemBase {
+  // public BoomBoom m_shooterSubsystem;
+  
+  public SparkMaxLimitSwitch m_hoodUpLimitSwitch;
+  public SparkMaxLimitSwitch m_hoodDownLimitSwitch;
+  // public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
+  public RelativeEncoder m_angleEncoder;
+  public CANSparkMax m_angleAdjustMotor;
+  public SparkMaxPIDController m_angleAdjusterPIDController;
+  
+  
+  public boolean m_isHoodReady = false;
+
+public double m_fireAngle;
+  
+
+  /** Creates a new Hood. */
+  public Hood(CANSparkMax angleAdjustMotor) {
+    m_angleAdjustMotor = angleAdjustMotor;
+     m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
+     m_angleEncoder= m_angleAdjustMotor.getEncoder();
+     m_angleAdjusterPIDController = m_angleAdjustMotor.getPIDController();
+    m_hoodUpLimitSwitch = m_angleAdjustMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_hoodDownLimitSwitch = m_angleAdjustMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_hoodUpLimitSwitch.enableLimitSwitch(true);
+    m_hoodDownLimitSwitch.enableLimitSwitch(true);
+  }
+    
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+  // public void runAngleAdjustPID(double targetAngle) 
+  // {
+  //   //Set PID Coefficients
+  //   m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP);
+  //   m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI);
+  //   m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD);
+  //   m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone);
+  //   m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF);
+  //   m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN / 5, m_angleAdjusterGains.m_kPeakOutput / 5);
+
+  //   m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition);
+  // }
+  
+
+  public void runHood(double input) {
+    input *= .6;
+    m_angleAdjustMotor.set(input);
+  }
+
+
+
+  public void resetGyroAngleAdj(){
+    m_angleEncoder.setPosition(0);
+  }
+
+  public double getAnglePositionPID() {
+    return m_angleEncoder.getPosition();
+  }
+
+  // public double getAnglePositionDegrees(){
+    // return ((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90;
+  // }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 9bc97d7..25bbd39 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -51,14 +51,14 @@ public class Intake extends SubsystemBase {
     m_intakeMotor.set(rightTrigger - leftTrigger);
   }
 
-  public void runExtender(boolean extended) {
-    double extenderMotorSpeed = extended ? 0.25d : 0.d;
+  public void extendExtender(boolean extended) {
+    double extenderMotorSpeed = extended ? 0.25d : -0.25d;
     m_extenderMotor.set(extenderMotorSpeed);
   }
 
   public void toggleExtender() {
     toggle = !toggle;
-    runExtender(toggle);
+    extendExtender(toggle);
   }
   //Test
 }
\ No newline at end of file

From 78f3b47846daa63046d0baa8c788787bb88c115e Mon Sep 17 00:00:00 2001
From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 5 Mar 2022 11:04:40 -0700
Subject: [PATCH 37/42] JavaDocs

---
 .../java/frc4388/robot/RobotContainer.java    | 38 +++----------------
 src/main/java/frc4388/robot/RobotMap.java     | 16 +-------
 .../java/frc4388/robot/subsystems/Hood.java   | 14 +++++--
 .../java/frc4388/robot/subsystems/Intake.java | 17 +++++++--
 .../frc4388/robot/subsystems/Serializer.java  | 31 +++++++++++++--
 .../frc4388/robot/subsystems/Storage.java     | 20 +++++++++-
 6 files changed, 78 insertions(+), 58 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 039eaf9..38c3459 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -10,9 +10,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
-import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
-import frc4388.robot.subsystems.Serializer;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.controller.IHandController;
@@ -29,7 +27,7 @@ public class RobotContainer {
   /* RobotMap */
   private final RobotMap m_robotMap = new RobotMap();
 
-  /* Subsystems 
+  /* Subsystems */
   private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
     m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor,
     m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor,
@@ -40,9 +38,6 @@ public class RobotContainer {
     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);
 
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
@@ -58,17 +53,12 @@ public class RobotContainer {
 
     /* Default Commands */
     // drives the swerve drive with a two-axis input from the driver controller
-    // m_robotSwerveDrive.setDefaultCommand(
-    //     new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
-    //         getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
+    m_robotSwerveDrive.setDefaultCommand(
+        new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
+            getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
 
-    m_robotIntake.setDefaultCommand(
-        new RunCommand(() -> m_robotIntake.runWithTriggers(
-          getDriverController().getLeftTriggerAxis(), getDriverController().getRightTriggerAxis()),m_robotIntake));
     // continually sends updates to the Blinkin LED controller to keep the lights on
-    m_robotLED.setDefaultCommand(
-        new RunCommand(m_robotLED::updateLED, m_robotLED));                      
-    // dri
+    m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
   }
 
   /**
@@ -79,28 +69,12 @@ public class RobotContainer {
    */
   private void configureButtonBindings() {
     /* Driver Buttons */
-    
+
     /* Operator Buttons */
-
-    // extends and retracts the extender 
-    new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-        .whenPressed(() -> m_robotIntake.extendExtender(true))
-        .whenReleased(() -> m_robotIntake.extendExtender(false));
-
     // activates "Lit Mode"
     new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
         .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
-    // revisit this later, not sure if we will still use this 
-    new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
-        .whenPressed(() -> m_robotSerializer.setSerializerStateWithBeam(true, m_robotSerializer.getBeam()))
-        .whenReleased(() -> m_robotSerializer.setSerializerStateWithBeam(false, m_robotSerializer.getBeam()));
-        
-    // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
-        // .whenPressed(() -> m_robotIntake.extendExtender(true)); 
-
-    new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
-      .whenPressed(() -> m_robotIntake.toggleExtender());
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index c9f4012..843323c 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -6,14 +6,10 @@ package frc4388.robot;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
-import com.revrobotics.CANSparkMax;
 
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
-import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.Constants.LEDConstants;
-import frc4388.robot.Constants.SerializerConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
-import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 /**
  * Defines and holds all I/O objects on the Roborio. This is useful for unit
@@ -23,7 +19,7 @@ public class RobotMap {
 
   public RobotMap() {
     configureLEDMotorControllers();
-    //configureSwerveMotorControllers();
+    configureSwerveMotorControllers();
   }
 
   /* LED Subsystem */
@@ -33,7 +29,7 @@ public class RobotMap {
     
   }
   
-  /* Swerve Subsystem 
+  /* Swerve Subsystem */
   public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
   public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
   public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
@@ -96,12 +92,4 @@ public class RobotMap {
     //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
   }
 
-  /* 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);
-  
-  /* Intake Subsytem */
-  public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
-  public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java
index 3895645..0ecb88b 100644
--- a/src/main/java/frc4388/robot/subsystems/Hood.java
+++ b/src/main/java/frc4388/robot/subsystems/Hood.java
@@ -66,18 +66,26 @@ public double m_fireAngle;
   //   m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition);
   // }
   
-
+  /**
+   * Runs The Hood
+   * @param input The Speed Times 0.6
+   */
   public void runHood(double input) {
     input *= .6;
     m_angleAdjustMotor.set(input);
   }
 
 
-
+  /**
+   * Resets The Encoder
+   */
   public void resetGyroAngleAdj(){
     m_angleEncoder.setPosition(0);
   }
-
+  /**
+   * Gets The Encoders Position
+   * @return The Encoders Position
+   */
   public double getAnglePositionPID() {
     return m_angleEncoder.getPosition();
   }
diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 5e4d994..56c80b8 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -46,19 +46,28 @@ public class Intake extends SubsystemBase {
   public void periodic() {
     // This method will be called once per scheduler run
   }
-
+  /**
+   * Runs The Intake With Triggers.
+   * @param leftTrigger Left Trigger to Run -
+   * @param rightTrigger Right Trigger to Run +
+   */
   public void runWithTriggers(double leftTrigger, double rightTrigger) {
     m_intakeMotor.set(rightTrigger - leftTrigger);
   }
-
+  /**
+   * Runs The Extender
+   * @param extended Wether the Extender Is Extended
+   */
   public void runExtender(boolean extended) { //TODO: Do not bring intake in if there is a ball in the extender (check if intake being in brakes the beam brake?)
     double extenderMotorSpeed = extended ? 0.25d : 0.d;
     m_extenderMotor.set(extenderMotorSpeed);
   }
-
+  /**
+   * Toggles The Extender
+  */
   public void toggleExtender() {
     toggle = !toggle;
-    extendExtender(toggle);
+    runExtender(toggle);
   }
   //Test
 }
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java
index e9ec997..a3cf25e 100644
--- a/src/main/java/frc4388/robot/subsystems/Serializer.java
+++ b/src/main/java/frc4388/robot/subsystems/Serializer.java
@@ -23,29 +23,52 @@ public class Serializer extends SubsystemBase{
     m_serializerShooterBelt.set(0);
     
   }
+  /**
+   * Gets The State Of The Beam
+   * @return The State Of The Beam
+   */
   public boolean getBeam() {
     return m_serializerBeam.get();
   }
-  public void setSerializerStateWithBeam(boolean ctrlbutter, boolean beambroken) {
-    boolean total = ctrlbutter || beambroken;
+  /**
+   * Sets The Serializer State With The Beam
+   * @param state Your State Of The Button
+   * @param beambroken The State of the Beam Senser
+   */
+  public void setSerializerStateWithBeam(boolean state, boolean beambroken) {
+    boolean total = state || beambroken;
     setSerializerState(total);
   }
+  /**
+   * Sets The Serializer State With The Beam 
+   * @param state Your State Of The Button
+   */
   public void setSerializerState(boolean state) {
     setSerializerBeltState(state);
     setSerializerShooterBeltState(state);
     serializerState = state;
   }
-
+  /**
+   * Sets the Serializer Belt State
+   * @param state Your State Of The Button
+   */
   public void setSerializerBeltState(boolean state) {
     double serializerBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_BELT_SPEED : 0.d;
     m_serializerBelt.set(serializerBeltSpeed);
   }
-
+  /**
+   * Sets the Shooter Belt State
+   * @param state Your State Of The Button
+   */
   public void setSerializerShooterBeltState(boolean state) {
     double serializerShooterBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_SHOOTER_BELT_SPEED : 0.d;
     m_serializerShooterBelt.set(serializerShooterBeltSpeed);
   }
   
+  /**
+   * Gets the Serializer State
+   * @return The Serializer State
+   */
   public boolean getSerializerState() {
     return serializerState;
   }
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 9fc2df0..a922e27 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -19,24 +19,42 @@ public class Storage extends SubsystemBase {
   public Storage() {
 
   }
+  /**
+   * If The Beam Is Broken, Run Storage
+   * If Else, Stop Running Storage
+   */
   public void manageStorage() {
     if (m_beamShooter.get()) {
       runStorage(1.d);
     } else { runStorage(0.d); }
   }
+  /**
+   * Runs The Storage at a Specifyed Speed
+   * @param input The Specifyed Speed
+   */
   public void runStorage(double input) {
     m_storageMotor.set(input);
   }
-
+  /**
+   * Gets The Beam State On The Shooter
+   * @return The State Of The Beam on the Shooter
+   */
   public boolean getBeamShooter(){
     return m_beamShooter.get();
   }
 
+  /**
+   * Gets The Beam State Of The Intake
+   * @return The Beam State Of The Intake
+   */
   public boolean getBeamIntake(){
     return m_beamIntake.get();
   }
 
   @Override
+  /**
+   * Every Robot Tick Manage The Storage
+   */
   public void periodic() {
     manageStorage();
   }

From 8acedd78695e2c4bcb5e0bfadd28dec7b26c69d6 Mon Sep 17 00:00:00 2001
From: Evan Lanham <44246276+Aquaticholic@users.noreply.github.com>
Date: Sat, 5 Mar 2022 14:35:50 -0700
Subject: [PATCH 38/42] slowed serializer, set some ids

---
 src/main/java/frc4388/robot/Constants.java | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 04e0193..147c16b 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -120,21 +120,21 @@ public final class Constants {
   }
 
   public static final class SerializerConstants {
-    public static final double SERIALIZER_BELT_SPEED = 1.d; // TODO (currently max power, DO NOT RUN)
-    public static final double SERIALIZER_SHOOTER_BELT_SPEED = 1.d; // TODO (currently max power, DO NOT RUN)
+    public static final double SERIALIZER_BELT_SPEED = 0.01d
+    public static final double SERIALIZER_SHOOTER_BELT_SPEED = 0.01d
     
     // CAN IDs
-    public static final int SERIALIZER_BELT = 2; // TODO
-    public static final int SERIALIZER_SHOOTER_BELT = 5; // TODO
-    public static final int SERIALIZER_BELT_BEAM = 3; // TODO
+    public static final int SERIALIZER_BELT = 16;
+    public static final int SERIALIZER_SHOOTER_BELT = -1; // TODO (What is this for? - evan)
+    public static final int SERIALIZER_BELT_BEAM = -1; // TODO
   }
   public static final class IntakeConstants {
     // CAN IDs
-    public static final int INTAKE_MOTOR = 3;
-    public static final int EXTENDER_MOTOR = 6;
+    public static final int INTAKE_MOTOR = 14;
+    public static final int EXTENDER_MOTOR = 15;
   }
   public static final class StorageConstants {
-    public static final int STORAGE_CAN_ID = -1; //TODO
+    public static final int STORAGE_CAN_ID = 17;
     public static final int BEAM_SENSOR_SHOOTER = -1; //TODO
     public static final int BEAM_SENSOR_INTAKE = -1; //TODO
   }

From 25970209991a31f8ce98c839f0ae01a97138c7e3 Mon Sep 17 00:00:00 2001
From: evan <aquaticholic@gmail.com>
Date: Sat, 5 Mar 2022 14:42:09 -0700
Subject: [PATCH 39/42] whoops im stupid

---
 src/main/java/frc4388/robot/Constants.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 147c16b..01f3d43 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -120,8 +120,8 @@ public final class Constants {
   }
 
   public static final class SerializerConstants {
-    public static final double SERIALIZER_BELT_SPEED = 0.01d
-    public static final double SERIALIZER_SHOOTER_BELT_SPEED = 0.01d
+    public static final double SERIALIZER_BELT_SPEED = 0.01d;
+    public static final double SERIALIZER_SHOOTER_BELT_SPEED = 0.01d;
     
     // CAN IDs
     public static final int SERIALIZER_BELT = 16;

From 77e4ea124077f60487763be95a041de858bff0ca Mon Sep 17 00:00:00 2001
From: evan <aquaticholic@gmail.com>
Date: Sat, 5 Mar 2022 15:32:48 -0700
Subject: [PATCH 40/42] now it builds (big money :) )

---
 simgui.json                                   | 36 ++++++++++++++++++-
 src/main/java/frc4388/robot/Constants.java    |  8 ++---
 .../java/frc4388/robot/RobotContainer.java    | 14 ++------
 src/main/java/frc4388/robot/RobotMap.java     |  5 ++-
 .../frc4388/robot/subsystems/Serializer.java  | 26 +++++++-------
 .../frc4388/robot/subsystems/Storage.java     |  4 ++-
 6 files changed, 59 insertions(+), 34 deletions(-)

diff --git a/simgui.json b/simgui.json
index 4af852e..55c0b83 100644
--- a/simgui.json
+++ b/simgui.json
@@ -10,17 +10,51 @@
         "header": {
           "open": true
         }
+      },
+      "Talon FX[9]/Integrated Sensor": {
+        "header": {
+          "open": true
+        }
       }
     }
   },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/LiveWindow/BoomBoom": "Subsystem",
+      "/LiveWindow/Hood": "Subsystem",
       "/LiveWindow/Intake": "Subsystem",
       "/LiveWindow/LED": "Subsystem",
       "/LiveWindow/Serializer": "Subsystem",
+      "/LiveWindow/Storage": "Subsystem",
+      "/LiveWindow/SwerveDrive": "Subsystem",
+      "/LiveWindow/SwerveModule": "Subsystem",
+      "/LiveWindow/Turret": "Subsystem",
+      "/LiveWindow/Ungrouped/DigitalInput[27]": "Digital Input",
+      "/LiveWindow/Ungrouped/DigitalInput[28]": "Digital Input",
+      "/LiveWindow/Ungrouped/DigitalInput[29]": "Digital Input",
       "/LiveWindow/Ungrouped/DigitalInput[3]": "Digital Input",
-      "/LiveWindow/Ungrouped/Scheduler": "Scheduler"
+      "/LiveWindow/Ungrouped/PIDController[1]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[2]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[3]": "PIDController",
+      "/LiveWindow/Ungrouped/Scheduler": "Scheduler",
+      "/LiveWindow/Ungrouped/Spark[0]": "Motor Controller",
+      "/LiveWindow/Ungrouped/Talon FX [14]": "Motor Controller",
+      "/LiveWindow/Ungrouped/Talon FX [23]": "Motor Controller",
+      "/LiveWindow/Ungrouped/Talon FX [24]": "Motor Controller",
+      "/LiveWindow/Ungrouped/Talon FX [2]": "Motor Controller",
+      "/LiveWindow/Ungrouped/Talon FX [3]": "Motor Controller",
+      "/LiveWindow/Ungrouped/Talon FX [4]": "Motor Controller",
+      "/LiveWindow/Ungrouped/Talon FX [5]": "Motor Controller",
+      "/LiveWindow/Ungrouped/Talon FX [6]": "Motor Controller",
+      "/LiveWindow/Ungrouped/Talon FX [7]": "Motor Controller",
+      "/LiveWindow/Ungrouped/Talon FX [8]": "Motor Controller",
+      "/LiveWindow/Ungrouped/Talon FX [9]": "Motor Controller",
+      "/LiveWindow/Vision": "Subsystem",
+      "/SmartDashboard/Field": "Field2d",
+      "/SmartDashboard/JVM Memory": "Command",
+      "/SmartDashboard/Scheduler": "Scheduler",
+      "/SmartDashboard/Usable Deploy Space": "Command"
     }
   }
 }
diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 01f3d43..8b0c190 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -121,12 +121,10 @@ public final class Constants {
 
   public static final class SerializerConstants {
     public static final double SERIALIZER_BELT_SPEED = 0.01d;
-    public static final double SERIALIZER_SHOOTER_BELT_SPEED = 0.01d;
     
     // CAN IDs
     public static final int SERIALIZER_BELT = 16;
-    public static final int SERIALIZER_SHOOTER_BELT = -1; // TODO (What is this for? - evan)
-    public static final int SERIALIZER_BELT_BEAM = -1; // TODO
+    public static final int SERIALIZER_BELT_BEAM = 27; // TODO
   }
   public static final class IntakeConstants {
     // CAN IDs
@@ -135,8 +133,8 @@ public final class Constants {
   }
   public static final class StorageConstants {
     public static final int STORAGE_CAN_ID = 17;
-    public static final int BEAM_SENSOR_SHOOTER = -1; //TODO
-    public static final int BEAM_SENSOR_INTAKE = -1; //TODO
+    public static final int BEAM_SENSOR_SHOOTER = 28; //TODO
+    public static final int BEAM_SENSOR_INTAKE = 29; //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 cdd61ae..b40b252 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -61,6 +61,7 @@ import frc4388.robot.commands.AimToCenter;
 import frc4388.robot.commands.Shoot;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Hood;
+import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Serializer;
 import frc4388.robot.subsystems.Storage;
@@ -86,18 +87,9 @@ public class RobotContainer {
   private final RobotMap m_robotMap = new RobotMap();
 
   // 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
-  );
+  public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
   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 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);
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index aedf266..f41012d 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -17,7 +17,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
-
+import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.SerializerConstants;
@@ -211,12 +211,11 @@ 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 CANSparkMax serializerShooterBelt = new CANSparkMax(SerializerConstants.SERIALIZER_SHOOTER_BELT, MotorType.kBrushless);
   public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM);
   
   /* Intake Subsytem */
diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java
index b194035..1309e01 100644
--- a/src/main/java/frc4388/robot/subsystems/Serializer.java
+++ b/src/main/java/frc4388/robot/subsystems/Serializer.java
@@ -8,19 +8,19 @@ import com.revrobotics.CANSparkMax;
 
 public class Serializer extends SubsystemBase{
   private CANSparkMax m_serializerBelt;
-  private CANSparkMax m_serializerShooterBelt;
+  // private CANSparkMax m_serializerShooterBelt;
   private DigitalInput m_serializerBeam;
   private boolean serializerState;
 
-  public Serializer(CANSparkMax serializerBelt, CANSparkMax serializerShooterBelt, DigitalInput serializerBeam) { //TODO: Only one motor lol
+  public Serializer(CANSparkMax serializerBelt, /*CANSparkMax serializerShooterBelt,*/ DigitalInput serializerBeam) { 
     m_serializerBelt = serializerBelt;
-    m_serializerShooterBelt = serializerShooterBelt;
+    // m_serializerShooterBelt = serializerShooterBelt;
     m_serializerBeam = serializerBeam;
 
     serializerState = false;
     setSerializerState(serializerState);
     m_serializerBelt.set(0);
-    m_serializerShooterBelt.set(0);
+    // m_serializerShooterBelt.set(0);
     
   }
   /**
@@ -45,7 +45,7 @@ public class Serializer extends SubsystemBase{
    */
   public void setSerializerState(boolean state) {
     setSerializerBeltState(state);
-    setSerializerShooterBeltState(state);
+    // setSerializerShooterBeltState(state);
     serializerState = state;
   }
   /**
@@ -56,14 +56,14 @@ public class Serializer extends SubsystemBase{
     double serializerBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_BELT_SPEED : 0.d;
     m_serializerBelt.set(serializerBeltSpeed);
   }
-  /**
-   * Sets the Shooter Belt State
-   * @param state Your State Of The Button
-   */
-  public void setSerializerShooterBeltState(boolean state) {
-    double serializerShooterBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_SHOOTER_BELT_SPEED : 0.d;
-    m_serializerShooterBelt.set(serializerShooterBeltSpeed);
-  }
+  // /**
+  //  * Sets the Shooter Belt State
+  //  * @param state Your State Of The Button
+  //  */
+  // public void setSerializerShooterBeltState(boolean state) {
+  //   double serializerShooterBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_SHOOTER_BELT_SPEED : 0.d;
+  //   m_serializerShooterBelt.set(serializerShooterBeltSpeed);
+  // }
   
   /**
    * Gets the Serializer State
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 51b0281..7b0a4d7 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -26,10 +26,11 @@ public class Storage extends SubsystemBase {
    * If Else, Stop Running Storage
    */
   public void manageStorage() {
-    if (isBeamIntakeBroken()) { //Maybe needs to be shooter
+    if (getBeamIntake()) { //Maybe needs to be shooter
       runStorage(1.d);
     } else { runStorage(0.d); }
   }
+  
   /**
    * Runs The Storage at a Specifyed Speed
    * @param input The Specifyed Speed
@@ -52,6 +53,7 @@ public class Storage extends SubsystemBase {
   public boolean getBeamIntake(){
     return m_beamIntake.get();
   }
+  
 
   @Override
   /**

From 0e063176cd270f11f6be2e9af5c52c22d32a27c8 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 5 Mar 2022 22:57:55 -0700
Subject: [PATCH 41/42] Intake fixes

---
 src/main/java/frc4388/robot/Constants.java    |  1 +
 .../java/frc4388/robot/RobotContainer.java    | 52 +++++++++++++++----
 .../java/frc4388/robot/subsystems/Intake.java | 13 +++--
 .../frc4388/robot/subsystems/Serializer.java  | 40 ++++----------
 .../frc4388/robot/subsystems/Storage.java     |  9 ++--
 5 files changed, 65 insertions(+), 50 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 8b0c190..3945aed 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -135,6 +135,7 @@ public final class Constants {
     public static final int STORAGE_CAN_ID = 17;
     public static final int BEAM_SENSOR_SHOOTER = 28; //TODO
     public static final int BEAM_SENSOR_INTAKE = 29; //TODO
+    public static final double STORAGE_SPEED = 0.3;
   }
   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 b40b252..b7b81df 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -56,6 +56,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.OIConstants;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.AimToCenter;
 import frc4388.robot.commands.Shoot;
@@ -88,14 +89,15 @@ public class RobotContainer {
 
   // Subsystems 
   public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
-  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 Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer);
   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();
   private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
   private final Vision m_robotVision = new Vision(m_robotTurret, m_robotBoomBoom);
+  
   /* Controllers */
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -131,6 +133,7 @@ public class RobotContainer {
     // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret,
     // m_robotSwerveDrive));
 
+    //Swerve Drive
     m_robotSwerveDrive.setDefaultCommand(
         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
             getDriverController().getLeftX(),
@@ -139,6 +142,20 @@ public class RobotContainer {
             getDriverController().getRightY(),
             true),
             m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
+    //Intake with Triggers
+    m_robotIntake.setDefaultCommand(
+        new RunCommand(() -> m_robotIntake.runWithTriggers(
+            getOperatorController().getLeftTriggerAxis(), 
+            getOperatorController().getRightTriggerAxis()),
+            m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
+    //Storage Management
+    m_robotStorage.setDefaultCommand(
+        new RunCommand(() -> m_robotStorage.manageStorage(), 
+        m_robotStorage).withName("Storage manageStorage defaultCommand"));
+    //Serializer Management
+    m_robotSerializer.setDefaultCommand(
+        new RunCommand(() -> m_robotSerializer.setSerializerStateWithBeam(), 
+        m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
 
     // continually sends updates to the Blinkin LED controller to keep the lights on
     /*
@@ -157,9 +174,10 @@ public class RobotContainer {
    * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
+    
     /* Driver Buttons */
     // "XboxController.Button.kBack" was undefined yet, 7 works just fine
-    new JoystickButton(getDriverController(), 7)
+    new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
         .whenPressed(m_robotSwerveDrive::resetGyro);
 
     new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
@@ -174,27 +192,39 @@ public class RobotContainer {
 
     new JoystickButton(getDriverController(), XboxController.Button.kA.value)
         .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
-    new JoystickButton(getDriverController(), XboxController.Button.kX.value)
+
+    new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp
         .whenPressed(() -> m_robotMap.leftFront.reset())
         .whenPressed(() -> m_robotMap.rightFront.reset())
         .whenPressed(() -> m_robotMap.leftBack.reset())
         .whenPressed(() -> m_robotMap.rightBack.reset());
 
     /* Operator Buttons */
-    // activates "Lit Mode"
     /*
-     * new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
-     * .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
-     * .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
-     * 
-     * new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
-     * .whenPressed(new InstantCommand());
-     * 
      * // activates "BoomBoom"
      * new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
      * .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret,
      * m_robotHood));
      */
+
+    //Extender
+    new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
+        .whenPressed(() -> m_robotIntake.runExtender(true));
+
+    new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
+        .whenPressed(() -> m_robotIntake.runExtender(false));
+
+
+
+    //Storage
+    new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
+        .whenPressed(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))
+        .whenReleased(() -> m_robotStorage.runStorage(0.0));
+
+
+    new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
+        .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))
+        .whenReleased(() -> m_robotStorage.runStorage(0.0));
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 56c80b8..0c63afc 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -20,15 +20,17 @@ public class Intake extends SubsystemBase {
 
   private WPI_TalonFX m_intakeMotor;
   private CANSparkMax m_extenderMotor;
+  private Serializer m_serializer;
   private SparkMaxLimitSwitch m_inLimit;
   private SparkMaxLimitSwitch m_outLimit;
 
   public boolean toggle;
 
   /** Creates a new Intake. */
-  public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor) {
+  public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor, Serializer serializer) {
     m_intakeMotor = intakeMotor;
     m_extenderMotor = extenderMotor; 
+    m_serializer = serializer;
 
     m_extenderMotor.restoreFactoryDefaults();
 
@@ -59,9 +61,15 @@ public class Intake extends SubsystemBase {
    * @param extended Wether the Extender Is Extended
    */
   public void runExtender(boolean extended) { //TODO: Do not bring intake in if there is a ball in the extender (check if intake being in brakes the beam brake?)
-    double extenderMotorSpeed = extended ? 0.25d : 0.d;
+    if (!m_serializer.getBeam() && !extended) return;
+    double extenderMotorSpeed = extended ? 0.25d : -0.25d;
     m_extenderMotor.set(extenderMotorSpeed);
   }
+
+  public void runExtender(double input) {
+    if (!m_serializer.getBeam() && input < 0.) return;
+    m_extenderMotor.set(input);
+  }
   /**
    * Toggles The Extender
   */
@@ -69,5 +77,4 @@ public class Intake extends SubsystemBase {
     toggle = !toggle;
     runExtender(toggle);
   }
-  //Test
 }
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java
index 1309e01..5263fb8 100644
--- a/src/main/java/frc4388/robot/subsystems/Serializer.java
+++ b/src/main/java/frc4388/robot/subsystems/Serializer.java
@@ -3,6 +3,7 @@ package frc4388.robot.subsystems;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants;
+import frc4388.robot.Constants.SerializerConstants;
 import edu.wpi.first.wpilibj.DigitalInput;
 import com.revrobotics.CANSparkMax;
 
@@ -14,15 +15,16 @@ public class Serializer extends SubsystemBase{
 
   public Serializer(CANSparkMax serializerBelt, /*CANSparkMax serializerShooterBelt,*/ DigitalInput serializerBeam) { 
     m_serializerBelt = serializerBelt;
-    // m_serializerShooterBelt = serializerShooterBelt;
     m_serializerBeam = serializerBeam;
 
-    serializerState = false;
-    setSerializerState(serializerState);
     m_serializerBelt.set(0);
     // m_serializerShooterBelt.set(0);
     
   }
+
+  public void setSerializer(double input){
+    m_serializerBelt.set(input);
+  }
   /**
    * Gets The State Of The Beam
    * @return The State Of The Beam
@@ -30,40 +32,16 @@ public class Serializer extends SubsystemBase{
   public boolean getBeam() {
     return m_serializerBeam.get();
   }
+
   /**
    * Sets The Serializer State With The Beam
    * @param state Your State Of The Button
    * @param beambroken The State of the Beam Senser
    */
-  public void setSerializerStateWithBeam(boolean state, boolean beambroken) {
-    boolean total = state || beambroken;
-    setSerializerState(total);
+  public void setSerializerStateWithBeam() {
+    if (m_serializerBeam.get()) setSerializer(0.0);
+    else setSerializer(SerializerConstants.SERIALIZER_BELT_SPEED);
   }
-  /**
-   * Sets The Serializer State With The Beam 
-   * @param state Your State Of The Button
-   */
-  public void setSerializerState(boolean state) {
-    setSerializerBeltState(state);
-    // setSerializerShooterBeltState(state);
-    serializerState = state;
-  }
-  /**
-   * Sets the Serializer Belt State
-   * @param state Your State Of The Button
-   */
-  public void setSerializerBeltState(boolean state) {
-    double serializerBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_BELT_SPEED : 0.d;
-    m_serializerBelt.set(serializerBeltSpeed);
-  }
-  // /**
-  //  * Sets the Shooter Belt State
-  //  * @param state Your State Of The Button
-  //  */
-  // public void setSerializerShooterBeltState(boolean state) {
-  //   double serializerShooterBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_SHOOTER_BELT_SPEED : 0.d;
-  //   m_serializerShooterBelt.set(serializerShooterBeltSpeed);
-  // }
   
   /**
    * Gets the Serializer State
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 7b0a4d7..1e3126a 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -26,9 +26,8 @@ public class Storage extends SubsystemBase {
    * If Else, Stop Running Storage
    */
   public void manageStorage() {
-    if (getBeamIntake()) { //Maybe needs to be shooter
-      runStorage(1.d);
-    } else { runStorage(0.d); }
+    if (getBeamIntake()) runStorage(0.d);
+    else runStorage(1.d);
   }
   
   /**
@@ -43,7 +42,7 @@ public class Storage extends SubsystemBase {
    * @return The State Of The Beam on the Shooter
    */
   public boolean getBeamShooter(){
-    return m_beamShooter.get();
+    return m_beamShooter.get();//True if open
   }
 
   /**
@@ -51,7 +50,7 @@ public class Storage extends SubsystemBase {
    * @return The Beam State Of The Intake
    */
   public boolean getBeamIntake(){
-    return m_beamIntake.get();
+    return m_beamIntake.get(); //True if open
   }
   
 

From 2182b5ff8163fa6e14b92c61f40e844e7aa68149 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 5 Mar 2022 23:03:36 -0700
Subject: [PATCH 42/42] speed

---
 src/main/java/frc4388/robot/Constants.java | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 3945aed..04134fc 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -120,12 +120,13 @@ public final class Constants {
   }
 
   public static final class SerializerConstants {
-    public static final double SERIALIZER_BELT_SPEED = 0.01d;
+    public static final double SERIALIZER_BELT_SPEED = 0.1d;
     
     // CAN IDs
     public static final int SERIALIZER_BELT = 16;
     public static final int SERIALIZER_BELT_BEAM = 27; // TODO
   }
+  
   public static final class IntakeConstants {
     // CAN IDs
     public static final int INTAKE_MOTOR = 14;