From 25426d7f330423fe04f3386eed8e4baabf0d0ecc Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Thu, 17 Mar 2022 11:54:05 -0600
Subject: [PATCH] turret soft + hard limits + zero WORKS

---
 src/main/java/frc4388/robot/Constants.java    | 10 ++++-----
 .../java/frc4388/robot/RobotContainer.java    |  4 +++-
 .../java/frc4388/robot/subsystems/Turret.java | 22 +++++++++++++++++--
 3 files changed, 28 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 0273228..cb10859 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -204,18 +204,18 @@ public final class Constants {
     // ID
     public static final int TURRET_MOTOR_CAN_ID = 19;
       //Gains for turret
-    public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, TURRET_SPEED_MULTIPLIER);
-    public static final double SHOOTER_TURRET_MIN = -TURRET_SPEED_MULTIPLIER;
+    public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, 0.3/*TURRET_SPEED_MULTIPLIER*/);
+    public static final double SHOOTER_TURRET_MIN = -0.3;//-TURRET_SPEED_MULTIPLIER;
       //Gains for hood
     public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7);
 
     //#region test start
     
     //#endregion test end
-    public static final double TURRET_FORWARD_HARD_LIMIT = 18.4;
-    public static final double TURRET_REVERSE_HARD_LIMIT = -104.3;
+    public static final double TURRET_FORWARD_HARD_LIMIT = 18.429;
+    public static final double TURRET_REVERSE_HARD_LIMIT = -106.454;
 
-    public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 2;
+    public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 4;
     public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2;
       //Shooter gains for actual Drum
     public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0);
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 7253a31..80aae83 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -303,7 +303,9 @@ public class RobotContainer {
     //   .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
 
     new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
-        .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom));
+        // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom));\
+        .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret))
+        .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret));
 
     new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
         .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom));
diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java
index cc96886..4c04332 100644
--- a/src/main/java/frc4388/robot/subsystems/Turret.java
+++ b/src/main/java/frc4388/robot/subsystems/Turret.java
@@ -45,6 +45,8 @@ public class Turret extends SubsystemBase {
   long leftCurrentTime;
   long leftElapsedTime;
 
+  double speedLimiter;
+
   public Turret(CANSparkMax boomBoomRotateMotor) {
 
     m_boomBoomRotateMotor = boomBoomRotateMotor;
@@ -59,9 +61,10 @@ public class Turret extends SubsystemBase {
     
     m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
     m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
-    // setTurretLimitSwitches(true);
     m_boomBoomRightLimit.enableLimitSwitch(true);
     m_boomBoomLeftLimit.enableLimitSwitch(true);
+
+    this.speedLimiter = 1.0;
     
     setTurretPIDGains();
   }
@@ -99,6 +102,7 @@ public class Turret extends SubsystemBase {
     m_boomBoomRotatePIDController.setFF(m_shooterTGains.kF);
     m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone);
     m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput);
+    // TODO: add 0.1
   }
   
   @Override
@@ -140,6 +144,20 @@ public class Turret extends SubsystemBase {
     }
 
     leftPrevState = leftState; // * Update the state of the left limit switch.
+  
+  
+    // speed limiting near hard limits
+    double currentPos = this.getEncoderPosition();
+    double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT);
+    double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT);
+
+    if (forwardDistance < 20.0) {
+      this.speedLimiter = 0.2 + (forwardDistance * 0.05);
+    }
+
+    if (reverseDistance < 20.0) {
+      this.speedLimiter = 0.2 + (reverseDistance * 0.05);
+    }
   }
 
   /**
@@ -169,7 +187,7 @@ public class Turret extends SubsystemBase {
    * @param input from -1.0 to 1.0, positive is clockwise
    */
   public void runTurretWithInput(double input) {
-    m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER);
+    m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter);
   }
 
   public void runShooterRotatePID(double targetAngle) {