From f9a12a3b7af661323704a061c7a63985530569a4 Mon Sep 17 00:00:00 2001
From: 66945 <66945@psdschools.org>
Date: Fri, 28 Jan 2022 16:27:11 -0700
Subject: [PATCH] Added limit switches

and auto open
---
 .../java/frc4388/robot/subsystems/Hooks.java  | 24 +++++++++++++++----
 1 file changed, 20 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Hooks.java b/src/main/java/frc4388/robot/subsystems/Hooks.java
index 8eb2dfd..de07e93 100644
--- a/src/main/java/frc4388/robot/subsystems/Hooks.java
+++ b/src/main/java/frc4388/robot/subsystems/Hooks.java
@@ -1,5 +1,6 @@
 package frc4388.robot.subsystems;
 
+import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
 import com.revrobotics.CANSparkMax;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -9,16 +10,23 @@ public class Hooks extends SubsystemBase {
   private CANSparkMax m_leftHook;
   private CANSparkMax m_rightHook;
 
-  double m_leftOffset;
-  double m_rightOffset;
+  private LimitSwitchNormal m_limitSwitch;
+
+  private double m_leftOffset;
+  private double m_rightOffset;
 
   private boolean m_open;
 
-  public Hooks(CANSparkMax leftHook, CANSparkMax rightHook) {
+  public Hooks(CANSparkMax leftHook, CANSparkMax rightHook, LimitSwitchNormal limitSwitch) {
     m_leftHook = leftHook;
     m_rightHook = rightHook;
 
-    setOpen(false);
+    m_limitSwitch = limitSwitch;
+
+    m_open = false;
+
+    m_leftHook.set(.1);
+    m_rightHook.set(.1);
   }
 
   public void setOpen(boolean open) {
@@ -36,4 +44,12 @@ public class Hooks extends SubsystemBase {
   public boolean getOpen() {
     return m_open;
   }
+
+  @Override
+  public void periodic() {
+    if(m_limitSwitch.compareTo(LimitSwitchNormal.NormallyClosed) == 1) {
+      m_leftOffset = m_leftHook.getEncoder().getPosition();
+      m_leftOffset = m_leftHook.getEncoder().getPosition();
+    }
+  }
 }