From debd4b239525e05ce1cf505ce3ffaf1dbd6a8fab Mon Sep 17 00:00:00 2001
From: Geocentrix <marmpatel360@gmail.com>
Date: Tue, 9 Aug 2022 10:11:16 -0400
Subject: [PATCH 01/18] Basic Initialization

---
 .../robot/commands/climber/ElevatorDown.java  | 32 +++++++++++++++++++
 .../robot/commands/climber/ElevatorUp.java    | 32 +++++++++++++++++++
 .../robot/subsystems/ClimberSubsystem.java    | 17 ++++++++++
 3 files changed, 81 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/climber/ElevatorDown.java
 create mode 100644 src/main/java/frc/robot/commands/climber/ElevatorUp.java
 create mode 100644 src/main/java/frc/robot/subsystems/ClimberSubsystem.java

diff --git a/src/main/java/frc/robot/commands/climber/ElevatorDown.java b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
new file mode 100644
index 0000000..fb80895
--- /dev/null
+++ b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
@@ -0,0 +1,32 @@
+// 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 frc.robot.commands.climber;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class ElevatorDown extends CommandBase {
+  /** Creates a new ElevatorDown. */
+  public ElevatorDown() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorUp.java b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
new file mode 100644
index 0000000..030c394
--- /dev/null
+++ b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
@@ -0,0 +1,32 @@
+// 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 frc.robot.commands.climber;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class Elevator extends CommandBase {
+  /** Creates a new Elevator. */
+  public Elevator() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
new file mode 100644
index 0000000..6a5cbf6
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.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 frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class ClimberSubsystem extends SubsystemBase {
+  /** Creates a new ClimberSubsystem. */
+  public ClimberSubsystem() {}
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

From b8ace376104052486c40ae9530c44efdfc7447f3 Mon Sep 17 00:00:00 2001
From: Geocentrix <marmpatel360@gmail.com>
Date: Tue, 9 Aug 2022 10:36:24 -0400
Subject: [PATCH 02/18] Checkpoint

---
 src/main/java/frc/robot/RobotContainer.java   |  2 ++
 .../robot/commands/climber/ElevatorDown.java  |  1 +
 .../robot/commands/climber/ElevatorUp.java    | 16 +++++++++----
 .../robot/subsystems/ClimberSubsystem.java    | 23 +++++++++++++++++--
 4 files changed, 36 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d11f4d6..a413d15 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -22,6 +22,7 @@
 import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
+import frc.robot.subsystems.ClimberSubsystem;
 
 public class RobotContainer {
 
@@ -46,6 +47,7 @@ public class RobotContainer {
   private final TurretSubsystem m_turretSubsystem = new TurretSubsystem();
   private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
   private final IndexerSubsystem m_indexerSubsystem = new IndexerSubsystem();
+  private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
 
   private final JoystickButton m_circleButton = new JoystickButton(
     driverGamepad,
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorDown.java b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
index fb80895..40aea4c 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorDown.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
@@ -5,6 +5,7 @@
 package frc.robot.commands.climber;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystem.ClimberSubsystem;
 
 public class ElevatorDown extends CommandBase {
   /** Creates a new ElevatorDown. */
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorUp.java b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
index 030c394..f3f6987 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorUp.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
@@ -5,16 +5,22 @@
 package frc.robot.commands.climber;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystem.ClimberSubsystem;
 
-public class Elevator extends CommandBase {
+public class ElevatorUp extends CommandBase {
   /** Creates a new Elevator. */
-  public Elevator() {
+  public ElevatorUp() {
     // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(ClimberSubsystem);
+    if (getLimitSwitch = false) {
+      setElevator(RobotContainer.ELEVATOR_UP_SPEED);
+    }
   }
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
@@ -22,7 +28,9 @@ public void execute() {}
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    setElevator(0);
+  }
 
   // Returns true when the command should end.
   @Override
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 6a5cbf6..25dfae5 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -5,13 +5,32 @@
 package frc.robot.subsystems;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj.DigitalInput;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 public class ClimberSubsystem extends SubsystemBase {
   /** Creates a new ClimberSubsystem. */
-  public ClimberSubsystem() {}
+  private CANSPARKMAX climber_motor = new CANSparkMax(50, MotorType.kBrushless); //Magik Number Port
+  private DigitalInput limit_switch = new DigitalInput();
+
+  public ClimberSubsystem() {
+    
+  }
+  
+  private void setElevator(double speed) {
+    climber_motor(speed);
+  }
+
+  private boolean getLimitSwitch() {
+    return limit_switch.get();
+  }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
   }
-}
+
+  
+}
\ No newline at end of file

From 02c445e23be444219d690e9b0703a0a1541b72e4 Mon Sep 17 00:00:00 2001
From: Mina Girgis <minagirgis1231@gmail.com>
Date: Tue, 9 Aug 2022 11:57:56 -0400
Subject: [PATCH 03/18] Fixes to Elevator (unfinished)

---
 src/main/java/frc/robot/Constants.java        | 19 ++++++----
 .../robot/commands/climber/ElevatorDown.java  | 11 +++---
 .../robot/commands/climber/ElevatorUp.java    | 20 +++++++----
 .../robot/subsystems/ClimberSubsystem.java    | 35 ++++++++++++-------
 4 files changed, 55 insertions(+), 30 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 6f64c54..eecffe8 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -33,7 +33,7 @@ public static final class RobotMap {
         public static final int kLeftDriveCAN1 = 4;
         public static final int kLeftDriveCAN2 = 5;
         public static final int kLeftDriveCAN3 = 6;
-        
+
         public static final int kFeederCAN = 10;
         public static final int kFlywheelCAN = 11;
         public static final int kTurretCANId = 20;
@@ -65,7 +65,7 @@ public static final class DriveConstants {
     }
 
     public static final class LEDConstants {
-        public static final int kPwmPort = 0; 
+        public static final int kPwmPort = 0;
         public static final int kLength = 144;
         public static final double kInterval = Units.secondsToMilliseconds(0.1);
         public static final double kIntervalTrail = Units.secondsToMilliseconds(0.1);
@@ -82,11 +82,13 @@ public static final class StatusConstants {
         public static final String kShooter = "shooter";
         public static final String kIntake = "intake";
     }
+
     public static final class TurretConstants {
         public static final double kVisionThreshold = 5;
 
         public static final double kMaxSpeed = 0.05;
     }
+
     public static final class Limelight {
         public static final double kLimeLightMountAngleDegrees = 25.0;
         public static final double kLimelightLensHeightInches = 20.0;
@@ -95,13 +97,13 @@ public static final class Limelight {
 
     public static final class Shooter {
         public static final int kPIDLoopIDx = 0;
-        public static final double kGains_VelocitkF = 0.048973143759; //Feed Forward
+        public static final double kGains_VelocitkF = 0.048973143759; // Feed Forward
         public static final double kGains_VelocitkP = 0.085035;
         public static final double kGains_VelocitkI = 0.00035;
         public static final double kGains_VelocitkD = 0;
-        public static final double kConversionFactor = 2048/600;
+        public static final double kConversionFactor = 2048 / 600;
         public static final int kTimeoutMs = 0;
-        
+
         public static final int kMaxSpeed = 20000;
 
         public static final double kFlywheelA = 0;
@@ -111,6 +113,11 @@ public static final class Shooter {
         public static final double kFeederA = 0;
         public static final double kFeederB = 1;
         public static final double kFeederC = 0;
+    }
 
+    public static final class Climber {
+        public static final float kUpperLimit = 10; // Magik Number, Need to Find
+        public static final float kLowerLimit = 0;
+        public static final double kElevateUpSpeed = 0.2;
     }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorDown.java b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
index 40aea4c..04c8731 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorDown.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
@@ -5,7 +5,7 @@
 package frc.robot.commands.climber;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.subsystem.ClimberSubsystem;
+import frc.robot.subsystems.ClimberSubsystem;
 
 public class ElevatorDown extends CommandBase {
   /** Creates a new ElevatorDown. */
@@ -15,15 +15,18 @@ public ElevatorDown() {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+  }
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+  }
 
   // Returns true when the command should end.
   @Override
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorUp.java b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
index f3f6987..447da59 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorUp.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
@@ -5,15 +5,20 @@
 package frc.robot.commands.climber;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.subsystem.ClimberSubsystem;
+import frc.robot.subsystems.ClimberSubsystem;
+import frc.robot.Constants;
 
 public class ElevatorUp extends CommandBase {
   /** Creates a new Elevator. */
-  public ElevatorUp() {
+  private ClimberSubsystem m_climberSubsystem;
+
+  public ElevatorUp(double speed) {
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(ClimberSubsystem);
-    if (getLimitSwitch = false) {
-      setElevator(RobotContainer.ELEVATOR_UP_SPEED);
+    addRequirements(m_climberSubsystem);
+    if (!m_climberSubsystem.isFullyRetracted()) {
+      m_climberSubsystem.setSpeed(speed);
+    } else {
+      m_climberSubsystem.setSpeed(0);
     }
   }
 
@@ -24,12 +29,13 @@ public void initialize() {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+  }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    setElevator(0);
+    m_climberSubsystem.setSpeed(0);
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 25dfae5..67e05a5 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -4,33 +4,42 @@
 
 package frc.robot.subsystems;
 
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj.DigitalInput;
 import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMax.SoftLimitDirection;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.Climber;
+
 public class ClimberSubsystem extends SubsystemBase {
   /** Creates a new ClimberSubsystem. */
-  private CANSPARKMAX climber_motor = new CANSparkMax(50, MotorType.kBrushless); //Magik Number Port
-  private DigitalInput limit_switch = new DigitalInput();
+  private CANSparkMax climber_motor = new CANSparkMax(50, MotorType.kBrushless); // Magik Number Port
+  private DigitalInput limit_switch = new DigitalInput(50); // Magik Number Port
 
   public ClimberSubsystem() {
-    
+
   }
-  
-  private void setElevator(double speed) {
-    climber_motor(speed);
+
+  public void calibrate() {
+    climber_motor.setSoftLimit(SoftLimitDirection.kForward, Climber.kUpperLimit);
   }
 
-  private boolean getLimitSwitch() {
+  public void setSpeed(double speed) {
+    climber_motor.set(speed);
+  }
+
+  public boolean isFullyRetracted() {
     return limit_switch.get();
   }
 
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
+    // Need to Fix
+    // if (!isFullyRetracted()) {
+    // setSpeed(-Climber.kElevateUpSpeed);
+    // } else {
+    // setSpeed(0);
+    // }
   }
-
-  
 }
\ No newline at end of file

From f6ba68d5c91ed31350bd061517bca9618262ecf2 Mon Sep 17 00:00:00 2001
From: Mina Girgis <minagirgis1231@gmail.com>
Date: Tue, 9 Aug 2022 18:22:22 -0400
Subject: [PATCH 04/18] added methods for climbing, descending, stop, and  made
 them work with climber commands

---
 src/main/java/frc/robot/Constants.java        |  8 +++--
 src/main/java/frc/robot/RobotContainer.java   | 36 ++++++++-----------
 .../robot/commands/climber/ElevatorDown.java  |  9 ++++-
 .../robot/commands/climber/ElevatorUp.java    | 19 +++++-----
 .../robot/subsystems/ClimberSubsystem.java    | 34 ++++++++++++------
 5 files changed, 61 insertions(+), 45 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index eecffe8..1d743b2 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -116,8 +116,10 @@ public static final class Shooter {
     }
 
     public static final class Climber {
-        public static final float kUpperLimit = 10; // Magik Number, Need to Find
-        public static final float kLowerLimit = 0;
-        public static final double kElevateUpSpeed = 0.2;
+        public static final float kUpperLimit = 10; // unknown
+        public static final float kLowerLimit = 0; // unknown
+        public static final double kElevatorSpeed = 0.2;
+        public static final int LIMIT_SWITCH_PORT = 0; // unknown
+        public static final int CLIMBER_MOTOR_PORT = 0; // unknown
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a413d15..e4d2c7c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -28,16 +28,13 @@ public class RobotContainer {
 
   /* GAMEPADS */
   private static final PS4Controller driverGamepad = new PS4Controller(
-    Constants.RobotMap.kDriverControllerPort
-  );
+      Constants.RobotMap.kDriverControllerPort);
   private static final PS4Controller operatorGamepad = new PS4Controller(
-    Constants.RobotMap.kOperatorControllerPort
-  );
+      Constants.RobotMap.kOperatorControllerPort);
 
   private static final JoystickButton triangleButton = new JoystickButton(
-    operatorGamepad,
-    PS4Controller.Button.kTriangle.value
-  );
+      operatorGamepad,
+      PS4Controller.Button.kTriangle.value);
 
   private static final JoystickButton X_BUTTON = new JoystickButton(driverGamepad, PS4Controller.Button.kCross.value);
 
@@ -50,26 +47,23 @@ public class RobotContainer {
   private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
 
   private final JoystickButton m_circleButton = new JoystickButton(
-    driverGamepad,
-    PS4Controller.Button.kCircle.value
-  );
+      driverGamepad,
+      PS4Controller.Button.kCircle.value);
 
   public RobotContainer() {
     configureButtonBindings();
     CommandScheduler
-      .getInstance()
-      .schedule(new ToggleLED(m_ledSubsystem, true));
+        .getInstance()
+        .schedule(new ToggleLED(m_ledSubsystem, true));
     m_shooterSubsystem.setDefaultCommand(new ManualShoot(m_shooterSubsystem, 40, 40));
     m_driveSubsystem.setDefaultCommand(
-      new SimDrive(
-        m_driveSubsystem,
-        2,
-        driverGamepad::getR2Axis,
-        driverGamepad::getL2Axis,
-        driverGamepad::getLeftX,
-        driverGamepad::getRightX
-      )
-    );
+        new SimDrive(
+            m_driveSubsystem,
+            2,
+            driverGamepad::getR2Axis,
+            driverGamepad::getL2Axis,
+            driverGamepad::getLeftX,
+            driverGamepad::getRightX));
     m_intakeSubsystem.setDefaultCommand(new IntakeRun(m_intakeSubsystem));
   }
 
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorDown.java b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
index 04c8731..ea2aca6 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorDown.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
@@ -9,13 +9,19 @@
 
 public class ElevatorDown extends CommandBase {
   /** Creates a new ElevatorDown. */
-  public ElevatorDown() {
+  private ClimberSubsystem m_climberSubsystem;
+
+  public ElevatorDown(ClimberSubsystem climberSubsystem) {
     // Use addRequirements() here to declare subsystem dependencies.
+    m_climberSubsystem = climberSubsystem;
+    addRequirements(m_climberSubsystem);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    super.initialize();
+    m_climberSubsystem.descend();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -26,6 +32,7 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    m_climberSubsystem.stop();
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorUp.java b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
index 447da59..541f8a8 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorUp.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
@@ -6,25 +6,26 @@
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ClimberSubsystem;
-import frc.robot.Constants;
 
 public class ElevatorUp extends CommandBase {
   /** Creates a new Elevator. */
   private ClimberSubsystem m_climberSubsystem;
 
-  public ElevatorUp(double speed) {
-    // Use addRequirements() here to declare subsystem dependencies.
+  public ElevatorUp(ClimberSubsystem climbsubsystem) {
+    m_climberSubsystem = climbsubsystem;
     addRequirements(m_climberSubsystem);
-    if (!m_climberSubsystem.isFullyRetracted()) {
-      m_climberSubsystem.setSpeed(speed);
-    } else {
-      m_climberSubsystem.setSpeed(0);
-    }
+    // if (!m_climberSubsystem.isFullyRetracted()) {
+    // m_climberSubsystem.setSpeed(speed);
+    // } else {
+    // m_climberSubsystem.setSpeed(0);
+    // }
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    super.initialize();
+    m_climberSubsystem.climb();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -35,7 +36,7 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    m_climberSubsystem.setSpeed(0);
+    m_climberSubsystem.stop();
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 67e05a5..28eaeea 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -14,23 +14,17 @@
 
 public class ClimberSubsystem extends SubsystemBase {
   /** Creates a new ClimberSubsystem. */
-  private CANSparkMax climber_motor = new CANSparkMax(50, MotorType.kBrushless); // Magik Number Port
-  private DigitalInput limit_switch = new DigitalInput(50); // Magik Number Port
 
-  public ClimberSubsystem() {
-
-  }
+  private CANSparkMax m_climber_motor = new CANSparkMax(Climber.CLIMBER_MOTOR_PORT, MotorType.kBrushless);
+  private DigitalInput m_limit_switch = new DigitalInput(Climber.LIMIT_SWITCH_PORT);
+  public double m_climbSpeed = Climber.kElevatorSpeed;
 
-  public void calibrate() {
-    climber_motor.setSoftLimit(SoftLimitDirection.kForward, Climber.kUpperLimit);
-  }
+  public ClimberSubsystem() {
 
-  public void setSpeed(double speed) {
-    climber_motor.set(speed);
   }
 
   public boolean isFullyRetracted() {
-    return limit_switch.get();
+    return m_limit_switch.get();
   }
 
   @Override
@@ -42,4 +36,22 @@ public void periodic() {
     // setSpeed(0);
     // }
   }
+
+  public void setSpeed(double speed) {
+    m_climber_motor.set(speed);
+  }
+
+  public void climb() {
+    m_climber_motor.setSoftLimit(SoftLimitDirection.kForward, Climber.kUpperLimit);
+    m_climber_motor.set(Climber.kElevatorSpeed);
+  }
+
+  public void descend() {
+    m_climber_motor.setSoftLimit(SoftLimitDirection.kReverse, Climber.kLowerLimit);
+    m_climber_motor.set(Climber.kElevatorSpeed);
+  }
+
+  public void stop() {
+    m_climber_motor.set(0);
+  }
 }
\ No newline at end of file

From b88099af345b0a1787615da7ad37da742212cbec Mon Sep 17 00:00:00 2001
From: Mina Girgis <minagirgis1231@gmail.com>
Date: Wed, 10 Aug 2022 11:14:55 -0400
Subject: [PATCH 05/18] Created Soft Limits, set limit switch, modified
 commands, and added command for setting soft limits.

---
 src/main/java/frc/robot/Constants.java        |  4 +-
 src/main/java/frc/robot/RobotContainer.java   | 10 +++-
 .../frc/robot/commands/climber/Calibrate.java | 43 +++++++++++++++
 .../robot/commands/climber/ElevatorDown.java  |  1 +
 .../robot/commands/climber/ElevatorUp.java    |  6 +--
 .../robot/subsystems/ClimberSubsystem.java    | 53 ++++++++++++-------
 6 files changed, 91 insertions(+), 26 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/climber/Calibrate.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1d743b2..7a94710 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -116,9 +116,9 @@ public static final class Shooter {
     }
 
     public static final class Climber {
-        public static final float kUpperLimit = 10; // unknown
+        public static final float kUpperLimit = 0; // unknown
         public static final float kLowerLimit = 0; // unknown
-        public static final double kElevatorSpeed = 0.2;
+        public static final double kElevatorSpeed = 0.3;
         public static final int LIMIT_SWITCH_PORT = 0; // unknown
         public static final int CLIMBER_MOTOR_PORT = 0; // unknown
     }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index e4d2c7c..ca04bc1 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -10,6 +10,8 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.commands.RunIndexer;
 import frc.robot.commands.auto.DriveToDistance;
+import frc.robot.commands.climber.ElevatorDown;
+import frc.robot.commands.climber.ElevatorUp;
 import frc.robot.commands.drive.SimDrive;
 import frc.robot.commands.intake.IntakeRun;
 import frc.robot.commands.intake.ToggleIntake;
@@ -44,12 +46,15 @@ public class RobotContainer {
   private final TurretSubsystem m_turretSubsystem = new TurretSubsystem();
   private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
   private final IndexerSubsystem m_indexerSubsystem = new IndexerSubsystem();
-  private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
+  private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
 
   private final JoystickButton m_circleButton = new JoystickButton(
       driverGamepad,
       PS4Controller.Button.kCircle.value);
 
+  private final JoystickButton m_optionButton = new JoystickButton(driverGamepad, PS4Controller.Button.kOptions.value);
+  private final JoystickButton m_shareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kShare.value);
+
   public RobotContainer() {
     configureButtonBindings();
     CommandScheduler
@@ -71,6 +76,9 @@ private void configureButtonBindings() {
     triangleButton.whileHeld(new AutoAimTurret(m_turretSubsystem, 0.05));
     m_circleButton.whenPressed(new ToggleIntake(m_intakeSubsystem));
     X_BUTTON.whileHeld(new RunIndexer(m_indexerSubsystem));
+    m_optionButton.whileHeld(new ElevatorUp(m_climberSubsystem));
+    m_shareButton.whileHeld(new ElevatorDown(m_climberSubsystem));
+
   }
 
   public Command getAutonomousCommand() {
diff --git a/src/main/java/frc/robot/commands/climber/Calibrate.java b/src/main/java/frc/robot/commands/climber/Calibrate.java
new file mode 100644
index 0000000..2d2901e
--- /dev/null
+++ b/src/main/java/frc/robot/commands/climber/Calibrate.java
@@ -0,0 +1,43 @@
+// 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 frc.robot.commands.climber;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.ClimberSubsystem;
+
+public class Calibrate extends CommandBase {
+  /** Creates a new Calibrate. */
+  public ClimberSubsystem m_climberSubsystem;
+
+  public Calibrate(ClimberSubsystem climbSubsystem) {
+    m_climberSubsystem = climbSubsystem;
+    addRequirements(m_climberSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    super.initialize();
+    m_climberSubsystem.setSoftLimits();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_climberSubsystem.stop();
+    m_climberSubsystem.reset();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorDown.java b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
index ea2aca6..63288cf 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorDown.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
@@ -33,6 +33,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_climberSubsystem.stop();
+    m_climberSubsystem.reset();
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorUp.java b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
index 541f8a8..a00279e 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorUp.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
@@ -14,11 +14,6 @@ public class ElevatorUp extends CommandBase {
   public ElevatorUp(ClimberSubsystem climbsubsystem) {
     m_climberSubsystem = climbsubsystem;
     addRequirements(m_climberSubsystem);
-    // if (!m_climberSubsystem.isFullyRetracted()) {
-    // m_climberSubsystem.setSpeed(speed);
-    // } else {
-    // m_climberSubsystem.setSpeed(0);
-    // }
   }
 
   // Called when the command is initially scheduled.
@@ -37,6 +32,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_climberSubsystem.stop();
+    m_climberSubsystem.reset();
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 28eaeea..2e148fd 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -7,6 +7,7 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMax.SoftLimitDirection;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.RelativeEncoder;
 
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -14,44 +15,60 @@
 
 public class ClimberSubsystem extends SubsystemBase {
   /** Creates a new ClimberSubsystem. */
-
-  private CANSparkMax m_climber_motor = new CANSparkMax(Climber.CLIMBER_MOTOR_PORT, MotorType.kBrushless);
-  private DigitalInput m_limit_switch = new DigitalInput(Climber.LIMIT_SWITCH_PORT);
-  public double m_climbSpeed = Climber.kElevatorSpeed;
+  private CANSparkMax m_climberMotor;
+  private DigitalInput m_limitSwitch;
+  public double m_climbSpeed;
+  private final RelativeEncoder m_encoder;
+  private final float climbUpLimit;
+  private final float climbDownLimit;
 
   public ClimberSubsystem() {
-
+    m_climberMotor = new CANSparkMax(Climber.CLIMBER_MOTOR_PORT, MotorType.kBrushless);
+    m_limitSwitch = new DigitalInput(Climber.LIMIT_SWITCH_PORT);
+    m_encoder = m_climberMotor.getEncoder();
+    m_encoder.setPosition(0);
+    climbUpLimit = Climber.kUpperLimit - Float.valueOf(String.valueOf(m_encoder.getPosition()));
+    climbDownLimit = Climber.kLowerLimit - Float.valueOf(String.valueOf(m_encoder.getPosition()));
+    m_climbSpeed = Climber.kElevatorSpeed;
   }
 
   public boolean isFullyRetracted() {
-    return m_limit_switch.get();
+    return m_limitSwitch.get();
   }
 
   @Override
   public void periodic() {
-    // Need to Fix
-    // if (!isFullyRetracted()) {
-    // setSpeed(-Climber.kElevateUpSpeed);
-    // } else {
-    // setSpeed(0);
-    // }
+    if (!isFullyRetracted()) {
+      m_climberMotor.set(-m_climbSpeed);
+    } else {
+      m_climberMotor.set(0);
+    }
+  }
+
+  public void setSoftLimits() {
+    m_climberMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
+    m_climberMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
+    m_climberMotor.setSoftLimit(SoftLimitDirection.kForward, climbUpLimit); // 15
+    m_climberMotor.setSoftLimit(SoftLimitDirection.kReverse, climbDownLimit); // 0
   }
 
   public void setSpeed(double speed) {
-    m_climber_motor.set(speed);
+    m_climberMotor.set(speed);
+  }
+
+  public void reset() {
+    m_climberMotor.restoreFactoryDefaults();
   }
 
   public void climb() {
-    m_climber_motor.setSoftLimit(SoftLimitDirection.kForward, Climber.kUpperLimit);
-    m_climber_motor.set(Climber.kElevatorSpeed);
+    m_climberMotor.set(Climber.kElevatorSpeed);
   }
 
   public void descend() {
-    m_climber_motor.setSoftLimit(SoftLimitDirection.kReverse, Climber.kLowerLimit);
-    m_climber_motor.set(Climber.kElevatorSpeed);
+    m_climberMotor.set(-Climber.kElevatorSpeed);
   }
 
   public void stop() {
-    m_climber_motor.set(0);
+    m_climberMotor.set(0);
   }
 }
\ No newline at end of file

From d0e2f2aad04d2c0ae9a3ad3d0584a439a0fa19d2 Mon Sep 17 00:00:00 2001
From: Mina Girgis <minagirgis1231@gmail.com>
Date: Wed, 10 Aug 2022 12:01:12 -0400
Subject: [PATCH 06/18] Bug Fixes and additions to Calibrate

---
 src/main/java/frc/robot/Robot.java            | 55 +++++++++++++------
 src/main/java/frc/robot/RobotContainer.java   |  2 +
 .../frc/robot/commands/climber/Calibrate.java |  9 +--
 .../robot/commands/climber/ElevatorDown.java  |  1 +
 .../robot/commands/climber/ElevatorUp.java    |  1 +
 .../robot/subsystems/ClimberSubsystem.java    |  3 +-
 6 files changed, 49 insertions(+), 22 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 56a3d1c..2f8724a 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -9,9 +9,12 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 /**
- * The VM is configured to automatically run this class, and to call the functions corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the name of this class or
- * the package after creating this project, you must also update the build.gradle file in the
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the
+ * name of this class or
+ * the package after creating this project, you must also update the
+ * build.gradle file in the
  * project.
  */
 public class Robot extends TimedRobot {
@@ -20,40 +23,55 @@ public class Robot extends TimedRobot {
   private RobotContainer m_robotContainer;
 
   /**
-   * This function is run when the robot is first started up and should be used for any
+   * This function is run when the robot is first started up and should be used
+   * for any
    * initialization code.
    */
   @Override
   public void robotInit() {
-    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // Instantiate our RobotContainer. This will perform all our button bindings,
+    // and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+
   }
 
   /**
-   * This function is called every robot packet, no matter the mode. Use this for items like
-   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   * This function is called every robot packet, no matter the mode. Use this for
+   * items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and
+   * test.
    *
-   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * <p>
+   * This runs after the mode specific periodic functions, but before LiveWindow
+   * and
    * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
-    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-    // commands, running already-scheduled commands, removing finished or interrupted commands,
-    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // Runs the Scheduler. This is responsible for polling buttons, adding
+    // newly-scheduled
+    // commands, running already-scheduled commands, removing finished or
+    // interrupted commands,
+    // and running subsystem periodic() methods. This must be called from the
+    // robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
   }
 
   /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+  }
 
   @Override
-  public void disabledPeriodic() {}
+  public void disabledPeriodic() {
+  }
 
-  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  /**
+   * This autonomous runs the autonomous command selected by your
+   * {@link RobotContainer} class.
+   */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -66,7 +84,8 @@ public void autonomousInit() {
 
   /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {}
+  public void autonomousPeriodic() {
+  }
 
   @Override
   public void teleopInit() {
@@ -81,7 +100,8 @@ public void teleopInit() {
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+  }
 
   @Override
   public void testInit() {
@@ -91,5 +111,6 @@ public void testInit() {
 
   /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {}
+  public void testPeriodic() {
+  }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ca04bc1..af11869 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.commands.RunIndexer;
 import frc.robot.commands.auto.DriveToDistance;
+import frc.robot.commands.climber.Calibrate;
 import frc.robot.commands.climber.ElevatorDown;
 import frc.robot.commands.climber.ElevatorUp;
 import frc.robot.commands.drive.SimDrive;
@@ -60,6 +61,7 @@ public RobotContainer() {
     CommandScheduler
         .getInstance()
         .schedule(new ToggleLED(m_ledSubsystem, true));
+    CommandScheduler.getInstance().schedule(false, new Calibrate(m_climberSubsystem));
     m_shooterSubsystem.setDefaultCommand(new ManualShoot(m_shooterSubsystem, 40, 40));
     m_driveSubsystem.setDefaultCommand(
         new SimDrive(
diff --git a/src/main/java/frc/robot/commands/climber/Calibrate.java b/src/main/java/frc/robot/commands/climber/Calibrate.java
index 2d2901e..2a809f6 100644
--- a/src/main/java/frc/robot/commands/climber/Calibrate.java
+++ b/src/main/java/frc/robot/commands/climber/Calibrate.java
@@ -7,9 +7,10 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ClimberSubsystem;
 
+// for setting soft limits
 public class Calibrate extends CommandBase {
   /** Creates a new Calibrate. */
-  public ClimberSubsystem m_climberSubsystem;
+  private ClimberSubsystem m_climberSubsystem;
 
   public Calibrate(ClimberSubsystem climbSubsystem) {
     m_climberSubsystem = climbSubsystem;
@@ -19,13 +20,12 @@ public Calibrate(ClimberSubsystem climbSubsystem) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    super.initialize();
-    m_climberSubsystem.setSoftLimits();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    m_climberSubsystem.setSpeed(-0.2);
   }
 
   // Called once the command ends or is interrupted.
@@ -33,11 +33,12 @@ public void execute() {
   public void end(boolean interrupted) {
     m_climberSubsystem.stop();
     m_climberSubsystem.reset();
+    m_climberSubsystem.setSoftLimits();
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return m_climberSubsystem.isFullyRetracted();
   }
 }
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorDown.java b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
index 63288cf..0badfdb 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorDown.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ClimberSubsystem;
 
+// for making the elevator descend
 public class ElevatorDown extends CommandBase {
   /** Creates a new ElevatorDown. */
   private ClimberSubsystem m_climberSubsystem;
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorUp.java b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
index a00279e..97d7f3e 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorUp.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ClimberSubsystem;
 
+// for making the elevator raise up
 public class ElevatorUp extends CommandBase {
   /** Creates a new Elevator. */
   private ClimberSubsystem m_climberSubsystem;
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 2e148fd..6c7c3cb 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Climber;
+import frc.robot.commands.climber.Calibrate;
 
 public class ClimberSubsystem extends SubsystemBase {
   /** Creates a new ClimberSubsystem. */
@@ -57,7 +58,7 @@ public void setSpeed(double speed) {
   }
 
   public void reset() {
-    m_climberMotor.restoreFactoryDefaults();
+    m_encoder.setPosition(0);
   }
 
   public void climb() {

From 1400fa1cf0f68859399b54831b9d2363e04a2d60 Mon Sep 17 00:00:00 2001
From: Mina Girgis <minagirgis1231@gmail.com>
Date: Thu, 11 Aug 2022 09:52:10 -0400
Subject: [PATCH 07/18] Bug fixes and additions

---
 src/main/java/frc/robot/Constants.java               |  4 ++--
 src/main/java/frc/robot/RobotContainer.java          |  6 +++---
 .../frc/robot/commands/climber/ElevatorDown.java     |  2 +-
 .../java/frc/robot/commands/climber/ElevatorUp.java  |  2 +-
 .../java/frc/robot/subsystems/ClimberSubsystem.java  | 12 ++++--------
 5 files changed, 11 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 7a94710..53d6f4f 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -119,7 +119,7 @@ public static final class Climber {
         public static final float kUpperLimit = 0; // unknown
         public static final float kLowerLimit = 0; // unknown
         public static final double kElevatorSpeed = 0.3;
-        public static final int LIMIT_SWITCH_PORT = 0; // unknown
-        public static final int CLIMBER_MOTOR_PORT = 0; // unknown
+        public static final int kLimitSwitch_Port = 0; // unknown
+        public static final int kClimberMotor_Port = 0; // unknown
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index af11869..48e7870 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -55,6 +55,7 @@ public class RobotContainer {
 
   private final JoystickButton m_optionButton = new JoystickButton(driverGamepad, PS4Controller.Button.kOptions.value);
   private final JoystickButton m_shareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kShare.value);
+  private final JoystickButton m_squareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kSquare.value);
 
   public RobotContainer() {
     configureButtonBindings();
@@ -78,9 +79,8 @@ private void configureButtonBindings() {
     triangleButton.whileHeld(new AutoAimTurret(m_turretSubsystem, 0.05));
     m_circleButton.whenPressed(new ToggleIntake(m_intakeSubsystem));
     X_BUTTON.whileHeld(new RunIndexer(m_indexerSubsystem));
-    m_optionButton.whileHeld(new ElevatorUp(m_climberSubsystem));
-    m_shareButton.whileHeld(new ElevatorDown(m_climberSubsystem));
-
+    m_optionButton.whileHeld(new ElevatorUp(m_climberSubsystem)); // Elevator Up
+    m_shareButton.whileHeld(new ElevatorDown(m_climberSubsystem)); // Elevator Down
   }
 
   public Command getAutonomousCommand() {
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorDown.java b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
index 0badfdb..421e9cc 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorDown.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
@@ -22,12 +22,12 @@ public ElevatorDown(ClimberSubsystem climberSubsystem) {
   @Override
   public void initialize() {
     super.initialize();
-    m_climberSubsystem.descend();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    m_climberSubsystem.descend();
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorUp.java b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
index 97d7f3e..347c0a8 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorUp.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
@@ -21,12 +21,12 @@ public ElevatorUp(ClimberSubsystem climbsubsystem) {
   @Override
   public void initialize() {
     super.initialize();
-    m_climberSubsystem.climb();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    m_climberSubsystem.climb();
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 6c7c3cb..a67238a 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -20,16 +20,12 @@ public class ClimberSubsystem extends SubsystemBase {
   private DigitalInput m_limitSwitch;
   public double m_climbSpeed;
   private final RelativeEncoder m_encoder;
-  private final float climbUpLimit;
-  private final float climbDownLimit;
 
   public ClimberSubsystem() {
-    m_climberMotor = new CANSparkMax(Climber.CLIMBER_MOTOR_PORT, MotorType.kBrushless);
-    m_limitSwitch = new DigitalInput(Climber.LIMIT_SWITCH_PORT);
+    m_climberMotor = new CANSparkMax(Climber.kClimberMotor_Port, MotorType.kBrushless);
+    m_limitSwitch = new DigitalInput(Climber.kLimitSwitch_Port);
     m_encoder = m_climberMotor.getEncoder();
     m_encoder.setPosition(0);
-    climbUpLimit = Climber.kUpperLimit - Float.valueOf(String.valueOf(m_encoder.getPosition()));
-    climbDownLimit = Climber.kLowerLimit - Float.valueOf(String.valueOf(m_encoder.getPosition()));
     m_climbSpeed = Climber.kElevatorSpeed;
   }
 
@@ -49,8 +45,8 @@ public void periodic() {
   public void setSoftLimits() {
     m_climberMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
     m_climberMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
-    m_climberMotor.setSoftLimit(SoftLimitDirection.kForward, climbUpLimit); // 15
-    m_climberMotor.setSoftLimit(SoftLimitDirection.kReverse, climbDownLimit); // 0
+    m_climberMotor.setSoftLimit(SoftLimitDirection.kForward, Climber.kUpperLimit); // 15
+    m_climberMotor.setSoftLimit(SoftLimitDirection.kReverse, Climber.kLowerLimit); // 0
   }
 
   public void setSpeed(double speed) {

From 37fa9ce9da7ef3805ccef39b8d36f6b0c8cddbbb Mon Sep 17 00:00:00 2001
From: Geocentrix <marmpatel360@gmail.com>
Date: Tue, 16 Aug 2022 10:25:45 -0400
Subject: [PATCH 08/18] More stuff

---
 src/main/java/frc/robot/commands/climber/Calibrate.java    | 3 +--
 src/main/java/frc/robot/commands/climber/ElevatorDown.java | 1 +
 src/main/java/frc/robot/commands/climber/ElevatorUp.java   | 1 +
 3 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/commands/climber/Calibrate.java b/src/main/java/frc/robot/commands/climber/Calibrate.java
index 2a809f6..ed2378b 100644
--- a/src/main/java/frc/robot/commands/climber/Calibrate.java
+++ b/src/main/java/frc/robot/commands/climber/Calibrate.java
@@ -19,8 +19,7 @@ public Calibrate(ClimberSubsystem climbSubsystem) {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {
-  }
+  public void initialize() {}
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorDown.java b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
index 421e9cc..9249139 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorDown.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
@@ -21,6 +21,7 @@ public ElevatorDown(ClimberSubsystem climberSubsystem) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    m_climberSubsystem.reset();
     super.initialize();
   }
 
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorUp.java b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
index 347c0a8..842e977 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorUp.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
@@ -20,6 +20,7 @@ public ElevatorUp(ClimberSubsystem climbsubsystem) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    m_climberSubsystem.reset();
     super.initialize();
   }
 

From 6cbe5b6cdad95552586f2968829dd12c393a3671 Mon Sep 17 00:00:00 2001
From: Mina Girgis <minagirgis1231@gmail.com>
Date: Thu, 18 Aug 2022 11:01:51 -0400
Subject: [PATCH 09/18] Fixes Stuff and Added PID

---
 src/main/java/frc/robot/Constants.java        |  9 +++++---
 .../robot/subsystems/ClimberSubsystem.java    | 22 ++++++++++++++-----
 2 files changed, 22 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 53d6f4f..017a229 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -116,10 +116,13 @@ public static final class Shooter {
     }
 
     public static final class Climber {
-        public static final float kUpperLimit = 0; // unknown
+        public static final float kUpperLimit = 500; // unknown
         public static final float kLowerLimit = 0; // unknown
-        public static final double kElevatorSpeed = 0.3;
+        public static final double kElevatorSpeed = 0.2;
         public static final int kLimitSwitch_Port = 0; // unknown
-        public static final int kClimberMotor_Port = 0; // unknown
+        public static final int kClimberMotor_Port = 11;
+        public static final double kP = 0.0;
+        public static final double kI = 0.0;
+        public static final double kD = 0.0;
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index a67238a..d9457d6 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -9,10 +9,11 @@
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.RelativeEncoder;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Climber;
-import frc.robot.commands.climber.Calibrate;
 
 public class ClimberSubsystem extends SubsystemBase {
   /** Creates a new ClimberSubsystem. */
@@ -20,10 +21,13 @@ public class ClimberSubsystem extends SubsystemBase {
   private DigitalInput m_limitSwitch;
   public double m_climbSpeed;
   private final RelativeEncoder m_encoder;
+  PIDController pid;
 
   public ClimberSubsystem() {
     m_climberMotor = new CANSparkMax(Climber.kClimberMotor_Port, MotorType.kBrushless);
     m_limitSwitch = new DigitalInput(Climber.kLimitSwitch_Port);
+    pid = new PIDController(Climber.kP, Climber.kI, Climber.kD);
+    // pid.calculate(1, 0.3);
     m_encoder = m_climberMotor.getEncoder();
     m_encoder.setPosition(0);
     m_climbSpeed = Climber.kElevatorSpeed;
@@ -35,11 +39,11 @@ public boolean isFullyRetracted() {
 
   @Override
   public void periodic() {
-    if (!isFullyRetracted()) {
-      m_climberMotor.set(-m_climbSpeed);
-    } else {
-      m_climberMotor.set(0);
-    }
+    // if (!isFullyRetracted()) {
+    // m_climberMotor.set(-m_climbSpeed);
+    // } else {
+    // m_climberMotor.set(0);
+    // }
   }
 
   public void setSoftLimits() {
@@ -68,4 +72,10 @@ public void descend() {
   public void stop() {
     m_climberMotor.set(0);
   }
+
+  public void log() {
+    SmartDashboard.putNumber("Current Position", Double.valueOf(String.valueOf(m_climberMotor.getEncoder())));
+    SmartDashboard.putNumber("Upper Limit", Climber.kUpperLimit);
+    SmartDashboard.putNumber("Lower Limit", Climber.kLowerLimit);
+  }
 }
\ No newline at end of file

From 95a97fab5cd7546becbcb03394ea7a5e51649c21 Mon Sep 17 00:00:00 2001
From: Geocentrix <marmpatel360@gmail.com>
Date: Thu, 18 Aug 2022 11:11:46 -0400
Subject: [PATCH 10/18] Added PID Controller to Motor

---
 src/main/java/frc/robot/subsystems/ClimberSubsystem.java | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index d9457d6..46e02ab 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -27,7 +27,6 @@ public ClimberSubsystem() {
     m_climberMotor = new CANSparkMax(Climber.kClimberMotor_Port, MotorType.kBrushless);
     m_limitSwitch = new DigitalInput(Climber.kLimitSwitch_Port);
     pid = new PIDController(Climber.kP, Climber.kI, Climber.kD);
-    // pid.calculate(1, 0.3);
     m_encoder = m_climberMotor.getEncoder();
     m_encoder.setPosition(0);
     m_climbSpeed = Climber.kElevatorSpeed;
@@ -57,6 +56,10 @@ public void setSpeed(double speed) {
     m_climberMotor.set(speed);
   }
 
+  public void setPIDSpeed() {
+    m_climberMotor.set(pid.calculate(encoder.getDistance(), 0.3));
+  }
+
   public void reset() {
     m_encoder.setPosition(0);
   }

From 0909be20de8edab07cd8cef2e7d75ba24e609f12 Mon Sep 17 00:00:00 2001
From: Mina Girgis <minagirgis1231@gmail.com>
Date: Tue, 23 Aug 2022 11:14:49 -0400
Subject: [PATCH 11/18] Fixed Logic and PID, added Elevator Sim

---
 simgui.json                                   | 19 +++++--
 src/main/java/frc/robot/Constants.java        |  2 +-
 src/main/java/frc/robot/RobotContainer.java   |  2 +-
 .../frc/robot/commands/climber/Calibrate.java |  6 ++-
 .../robot/commands/climber/ElevatorDown.java  |  3 --
 .../robot/commands/climber/ElevatorUp.java    |  3 --
 .../robot/subsystems/ClimberSubsystem.java    | 54 ++++++++++++++-----
 7 files changed, 63 insertions(+), 26 deletions(-)

diff --git a/simgui.json b/simgui.json
index 6b22185..d0d56c6 100644
--- a/simgui.json
+++ b/simgui.json
@@ -2,9 +2,15 @@
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/LiveWindow/ClimberSubsystem": "Subsystem",
       "/LiveWindow/DriveSubsystem": "Subsystem",
+      "/LiveWindow/IndexerSubsystem": "Subsystem",
+      "/LiveWindow/IntakeSubsystem": "Subsystem",
       "/LiveWindow/LEDSubsystem": "Subsystem",
+      "/LiveWindow/ShooterSubsystem": "Subsystem",
+      "/LiveWindow/TurretSubsystem": "Subsystem",
       "/LiveWindow/Ungrouped/DifferentialDrive[1]": "DifferentialDrive",
+      "/LiveWindow/Ungrouped/DigitalInput[0]": "Digital Input",
       "/LiveWindow/Ungrouped/PIDController[1]": "PIDController",
       "/LiveWindow/Ungrouped/PIDController[2]": "PIDController",
       "/LiveWindow/Ungrouped/PIDController[3]": "PIDController",
@@ -12,11 +18,16 @@
       "/LiveWindow/Ungrouped/Scheduler": "Scheduler",
       "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro",
       "/Shuffleboard/Dashboard/Field View": "Field2d",
-      "/Shuffleboard/Dashboard/Gyro": "Gyro"
+      "/Shuffleboard/Dashboard/Gyro": "Gyro",
+      "/SmartDashboard/Elevator Sim": "Mechanism2d"
     },
     "windows": {
       "/Shuffleboard/Dashboard/Field View": {
-        "image": "/Users/siddharthlohani/Downloads/2022-field.png",
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/Elevator Sim": {
         "window": {
           "visible": true
         }
@@ -27,7 +38,9 @@
     "Shuffleboard": {
       "Dashboard": {
         "open": true
-      },
+      }
+    },
+    "SmartDashboard": {
       "open": true
     },
     "status": {
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 017a229..b7a16b9 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -121,7 +121,7 @@ public static final class Climber {
         public static final double kElevatorSpeed = 0.2;
         public static final int kLimitSwitch_Port = 0; // unknown
         public static final int kClimberMotor_Port = 11;
-        public static final double kP = 0.0;
+        public static final double kP = 1; // proportional to distance to target
         public static final double kI = 0.0;
         public static final double kD = 0.0;
     }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 48e7870..ad15340 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -84,6 +84,6 @@ private void configureButtonBindings() {
   }
 
   public Command getAutonomousCommand() {
-    return new DriveToDistance(m_driveSubsystem, 2);
+    return new ElevatorUp(m_climberSubsystem);
   }
 }
diff --git a/src/main/java/frc/robot/commands/climber/Calibrate.java b/src/main/java/frc/robot/commands/climber/Calibrate.java
index ed2378b..14d0c18 100644
--- a/src/main/java/frc/robot/commands/climber/Calibrate.java
+++ b/src/main/java/frc/robot/commands/climber/Calibrate.java
@@ -5,6 +5,7 @@
 package frc.robot.commands.climber;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants.Climber;
 import frc.robot.subsystems.ClimberSubsystem;
 
 // for setting soft limits
@@ -19,7 +20,8 @@ public Calibrate(ClimberSubsystem climbSubsystem) {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
@@ -38,6 +40,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return m_climberSubsystem.isFullyRetracted();
+    return m_climberSubsystem.fullyRetracted();
   }
 }
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorDown.java b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
index 9249139..22acec1 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorDown.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorDown.java
@@ -21,8 +21,6 @@ public ElevatorDown(ClimberSubsystem climberSubsystem) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_climberSubsystem.reset();
-    super.initialize();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -35,7 +33,6 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_climberSubsystem.stop();
-    m_climberSubsystem.reset();
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/commands/climber/ElevatorUp.java b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
index 842e977..18d725d 100644
--- a/src/main/java/frc/robot/commands/climber/ElevatorUp.java
+++ b/src/main/java/frc/robot/commands/climber/ElevatorUp.java
@@ -20,8 +20,6 @@ public ElevatorUp(ClimberSubsystem climbsubsystem) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_climberSubsystem.reset();
-    super.initialize();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -34,7 +32,6 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_climberSubsystem.stop();
-    m_climberSubsystem.reset();
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 46e02ab..1a6114c 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -10,7 +10,16 @@
 import com.revrobotics.RelativeEncoder;
 
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Climber;
@@ -22,6 +31,11 @@ public class ClimberSubsystem extends SubsystemBase {
   public double m_climbSpeed;
   private final RelativeEncoder m_encoder;
   PIDController pid;
+  ElevatorSim m_elevatorSimulation;
+  EncoderSim m_encoderSimulation;
+  private final Mechanism2d m_mech2d;
+  private final MechanismRoot2d m_mech2dRoot;
+  private final MechanismLigament2d m_elevatorMech2d;
 
   public ClimberSubsystem() {
     m_climberMotor = new CANSparkMax(Climber.kClimberMotor_Port, MotorType.kBrushless);
@@ -30,19 +44,23 @@ public ClimberSubsystem() {
     m_encoder = m_climberMotor.getEncoder();
     m_encoder.setPosition(0);
     m_climbSpeed = Climber.kElevatorSpeed;
-  }
-
-  public boolean isFullyRetracted() {
-    return m_limitSwitch.get();
+    m_elevatorSimulation = new ElevatorSim(DCMotor.getNEO(1), 100, 45, Units.inchesToMeters(2), 0, 1);
+    m_encoderSimulation = new EncoderSim(new Encoder(1, 2));
+    m_mech2d = new Mechanism2d(20, 50);
+    m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 2);
+    m_elevatorMech2d = m_mech2dRoot.append(
+        new MechanismLigament2d("Elevator", Units.metersToInches(m_elevatorSimulation.getPositionMeters()), 90));
+    SmartDashboard.putData("Elevator Sim", m_mech2d);
   }
 
   @Override
   public void periodic() {
-    // if (!isFullyRetracted()) {
-    // m_climberMotor.set(-m_climbSpeed);
-    // } else {
-    // m_climberMotor.set(0);
-    // }
+    // setSpeed(0.2);
+    SmartDashboard.putNumber("Elevator Sim Position", m_elevatorSimulation.getPositionMeters());
+  }
+
+  public boolean fullyRetracted() {
+    return m_limitSwitch.get();
   }
 
   public void setSoftLimits() {
@@ -56,8 +74,9 @@ public void setSpeed(double speed) {
     m_climberMotor.set(speed);
   }
 
-  public void setPIDSpeed() {
-    m_climberMotor.set(pid.calculate(encoder.getDistance(), 0.3));
+  public void setPIDSpeed(double point) {
+    m_climberMotor.set(pid.calculate(m_encoder.getPosition(), point));
+    SmartDashboard.putNumber("PID", pid.calculate(m_encoder.getPosition(), point));
   }
 
   public void reset() {
@@ -65,11 +84,11 @@ public void reset() {
   }
 
   public void climb() {
-    m_climberMotor.set(Climber.kElevatorSpeed);
+    setPIDSpeed(1);
   }
 
   public void descend() {
-    m_climberMotor.set(-Climber.kElevatorSpeed);
+    setPIDSpeed(0);
   }
 
   public void stop() {
@@ -81,4 +100,13 @@ public void log() {
     SmartDashboard.putNumber("Upper Limit", Climber.kUpperLimit);
     SmartDashboard.putNumber("Lower Limit", Climber.kLowerLimit);
   }
+
+  @Override
+  public void simulationPeriodic() {
+    m_elevatorSimulation.setInput(m_climberMotor.get() * RobotController.getBatteryVoltage());
+    m_elevatorSimulation.update(0.020);
+    m_encoderSimulation.setDistance(m_elevatorSimulation.getPositionMeters());
+    m_elevatorMech2d.setLength(Units.metersToInches(m_elevatorSimulation.getPositionMeters()));
+
+  }
 }
\ No newline at end of file

From 90f16236506fbe3d0a7efd8dba4dfe514c3239de Mon Sep 17 00:00:00 2001
From: Mina Girgis <minagirgis1231@gmail.com>
Date: Wed, 24 Aug 2022 09:20:42 -0400
Subject: [PATCH 12/18] Added Toggle mode (unfinished)

---
 .../frc/robot/commands/climber/Toggle.java    | 41 +++++++++++++++++++
 .../robot/subsystems/ClimberSubsystem.java    | 12 ++++++
 2 files changed, 53 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/climber/Toggle.java

diff --git a/src/main/java/frc/robot/commands/climber/Toggle.java b/src/main/java/frc/robot/commands/climber/Toggle.java
new file mode 100644
index 0000000..8dedf5f
--- /dev/null
+++ b/src/main/java/frc/robot/commands/climber/Toggle.java
@@ -0,0 +1,41 @@
+// 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 frc.robot.commands.climber;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.ClimberSubsystem;
+
+public class Toggle extends CommandBase {
+  /** Creates a new Toggle. */
+  ClimberSubsystem m_climberSubsystem;
+
+  public Toggle(ClimberSubsystem climbSubsystem) {
+    m_climberSubsystem = climbSubsystem;
+    addRequirements(m_climberSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_climberSubsystem.toggle();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_climberSubsystem.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 1a6114c..67a69bc 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -36,6 +36,7 @@ public class ClimberSubsystem extends SubsystemBase {
   private final Mechanism2d m_mech2d;
   private final MechanismRoot2d m_mech2dRoot;
   private final MechanismLigament2d m_elevatorMech2d;
+  private boolean x;
 
   public ClimberSubsystem() {
     m_climberMotor = new CANSparkMax(Climber.kClimberMotor_Port, MotorType.kBrushless);
@@ -51,6 +52,7 @@ public ClimberSubsystem() {
     m_elevatorMech2d = m_mech2dRoot.append(
         new MechanismLigament2d("Elevator", Units.metersToInches(m_elevatorSimulation.getPositionMeters()), 90));
     SmartDashboard.putData("Elevator Sim", m_mech2d);
+    x = false;
   }
 
   @Override
@@ -59,6 +61,16 @@ public void periodic() {
     SmartDashboard.putNumber("Elevator Sim Position", m_elevatorSimulation.getPositionMeters());
   }
 
+  public void toggle() {
+    if (!x) {
+      climb();
+      x = true;
+    } else {
+      descend();
+      x = false;
+    }
+  }
+
   public boolean fullyRetracted() {
     return m_limitSwitch.get();
   }

From bd31d25364cb961908b227d870ae0667130f3ade Mon Sep 17 00:00:00 2001
From: Mina Girgis <minagirgis1231@gmail.com>
Date: Wed, 24 Aug 2022 09:44:44 -0400
Subject: [PATCH 13/18] Started Toggle

---
 src/main/java/frc/robot/RobotContainer.java        |  2 ++
 .../frc/robot/subsystems/ClimberSubsystem.java     | 14 +++++++++++++-
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ad15340..0e2d61f 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -13,6 +13,7 @@
 import frc.robot.commands.climber.Calibrate;
 import frc.robot.commands.climber.ElevatorDown;
 import frc.robot.commands.climber.ElevatorUp;
+import frc.robot.commands.climber.Toggle;
 import frc.robot.commands.drive.SimDrive;
 import frc.robot.commands.intake.IntakeRun;
 import frc.robot.commands.intake.ToggleIntake;
@@ -81,6 +82,7 @@ private void configureButtonBindings() {
     X_BUTTON.whileHeld(new RunIndexer(m_indexerSubsystem));
     m_optionButton.whileHeld(new ElevatorUp(m_climberSubsystem)); // Elevator Up
     m_shareButton.whileHeld(new ElevatorDown(m_climberSubsystem)); // Elevator Down
+    m_squareButton.whenPressed(new Toggle(m_climberSubsystem));
   }
 
   public Command getAutonomousCommand() {
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 67a69bc..0778650 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -64,13 +64,25 @@ public void periodic() {
   public void toggle() {
     if (!x) {
       climb();
+      while (m_climberMotor.getSoftLimit(SoftLimitDirection.kForward) > 0) {
+        climb();
+      }
       x = true;
     } else {
-      descend();
+      while (!fullyRetracted()) {
+        descend();
+      }
       x = false;
     }
   }
 
+  public boolean toggleFinished() {
+    if (m_climberMotor.getSoftLimit(SoftLimitDirection.kForward) <= 0 || fullyRetracted()) {
+      return true;
+    } else
+      return false;
+  }
+
   public boolean fullyRetracted() {
     return m_limitSwitch.get();
   }

From 6ae137797669823bfa1c75a47b903b3d0e56aea7 Mon Sep 17 00:00:00 2001
From: Mina Girgis <minagirgis1231@gmail.com>
Date: Wed, 24 Aug 2022 11:31:27 -0400
Subject: [PATCH 14/18] Fixes to Toggle

---
 simgui-ds.json                                |  3 +-
 src/main/java/frc/robot/RobotContainer.java   |  6 ++-
 .../frc/robot/commands/climber/Toggle.java    |  5 +-
 .../robot/subsystems/ClimberSubsystem.java    | 51 ++++++++++++-------
 4 files changed, 43 insertions(+), 22 deletions(-)

diff --git a/simgui-ds.json b/simgui-ds.json
index f7356d4..59cd3f3 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -91,8 +91,7 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "030000005e0400008e02000000000000",
-      "useGamepad": true
+      "guid": "030000004c050000c405000000000000"
     }
   ]
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 0e2d61f..ef07b9b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -57,6 +57,8 @@ public class RobotContainer {
   private final JoystickButton m_optionButton = new JoystickButton(driverGamepad, PS4Controller.Button.kOptions.value);
   private final JoystickButton m_shareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kShare.value);
   private final JoystickButton m_squareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kSquare.value);
+  private final JoystickButton m_l1Button = new JoystickButton(driverGamepad, PS4Controller.Button.kL1.value);
+  private final JoystickButton m_r1Button = new JoystickButton(driverGamepad, PS4Controller.Button.kR1.value);
 
   public RobotContainer() {
     configureButtonBindings();
@@ -82,7 +84,9 @@ private void configureButtonBindings() {
     X_BUTTON.whileHeld(new RunIndexer(m_indexerSubsystem));
     m_optionButton.whileHeld(new ElevatorUp(m_climberSubsystem)); // Elevator Up
     m_shareButton.whileHeld(new ElevatorDown(m_climberSubsystem)); // Elevator Down
-    m_squareButton.whenPressed(new Toggle(m_climberSubsystem));
+    m_squareButton.toggleWhenPressed(new Toggle(m_climberSubsystem));
+    m_l1Button.toggleWhenPressed(new ElevatorDown(m_climberSubsystem));
+    m_r1Button.toggleWhenPressed(new ElevatorUp(m_climberSubsystem));
   }
 
   public Command getAutonomousCommand() {
diff --git a/src/main/java/frc/robot/commands/climber/Toggle.java b/src/main/java/frc/robot/commands/climber/Toggle.java
index 8dedf5f..bff5eb5 100644
--- a/src/main/java/frc/robot/commands/climber/Toggle.java
+++ b/src/main/java/frc/robot/commands/climber/Toggle.java
@@ -10,6 +10,7 @@
 public class Toggle extends CommandBase {
   /** Creates a new Toggle. */
   ClimberSubsystem m_climberSubsystem;
+  boolean y;
 
   public Toggle(ClimberSubsystem climbSubsystem) {
     m_climberSubsystem = climbSubsystem;
@@ -24,7 +25,9 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_climberSubsystem.toggle();
+    if (m_climberSubsystem.isAtBottom()) {
+
+    }
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 0778650..8f33bb7 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants.Climber;
 
 public class ClimberSubsystem extends SubsystemBase {
@@ -36,7 +37,9 @@ public class ClimberSubsystem extends SubsystemBase {
   private final Mechanism2d m_mech2d;
   private final MechanismRoot2d m_mech2dRoot;
   private final MechanismLigament2d m_elevatorMech2d;
-  private boolean x;
+  boolean isBottom;
+  boolean atTop;
+  boolean toggleLimit;
 
   public ClimberSubsystem() {
     m_climberMotor = new CANSparkMax(Climber.kClimberMotor_Port, MotorType.kBrushless);
@@ -52,35 +55,47 @@ public ClimberSubsystem() {
     m_elevatorMech2d = m_mech2dRoot.append(
         new MechanismLigament2d("Elevator", Units.metersToInches(m_elevatorSimulation.getPositionMeters()), 90));
     SmartDashboard.putData("Elevator Sim", m_mech2d);
-    x = false;
+    isBottom = true;
+    atTop = false;
   }
 
   @Override
   public void periodic() {
     // setSpeed(0.2);
     SmartDashboard.putNumber("Elevator Sim Position", m_elevatorSimulation.getPositionMeters());
+    SmartDashboard.putNumber("Soft Limits (Forward)", m_climberMotor.getSoftLimit(SoftLimitDirection.kForward));
+    SmartDashboard.putNumber("Soft Limits (Reverse)", m_climberMotor.getSoftLimit(SoftLimitDirection.kReverse));
+    SmartDashboard.putBoolean("Limit Switch", fullyRetracted());
   }
 
   public void toggle() {
-    if (!x) {
+    if (m_elevatorSimulation.getPositionMeters() == 0) {
+      isBottom = true;
+      atTop = false;
+    } else if (m_elevatorSimulation.getPositionMeters() == 1) {
+      isBottom = false;
+      atTop = true;
+    }
+    if (isBottom) {
       climb();
-      while (m_climberMotor.getSoftLimit(SoftLimitDirection.kForward) > 0) {
-        climb();
-      }
-      x = true;
-    } else {
-      while (!fullyRetracted()) {
-        descend();
-      }
-      x = false;
+      stop();
+    } else if (atTop) {
+      descend();
+      stop();
     }
+    // if (m_elevatorSimulation.getPositionMeters() >= 0) {
+    // climb();
+    // if (m_elevatorSimulation.getPositionMeters() == 0.99) {
+    // stop();
+    // }
+    // } else if (m_elevatorSimulation.getPositionMeters() == 0.99) {
+    // descend();
+    // if (m)
+    // }
   }
 
-  public boolean toggleFinished() {
-    if (m_climberMotor.getSoftLimit(SoftLimitDirection.kForward) <= 0 || fullyRetracted()) {
-      return true;
-    } else
-      return false;
+  public boolean isAtBottom() {
+    return isBottom;
   }
 
   public boolean fullyRetracted() {
@@ -112,7 +127,7 @@ public void climb() {
   }
 
   public void descend() {
-    setPIDSpeed(0);
+    setPIDSpeed(-1);
   }
 
   public void stop() {

From ea13fa848b3d8b5e2940c5de015383219caf040c Mon Sep 17 00:00:00 2001
From: Mina Girgis <minagirgis1231@gmail.com>
Date: Wed, 24 Aug 2022 11:47:30 -0400
Subject: [PATCH 15/18] Finished Toggle

---
 .../frc/robot/commands/climber/Toggle.java    |  5 +++-
 .../robot/subsystems/ClimberSubsystem.java    | 24 +++++--------------
 2 files changed, 10 insertions(+), 19 deletions(-)

diff --git a/src/main/java/frc/robot/commands/climber/Toggle.java b/src/main/java/frc/robot/commands/climber/Toggle.java
index bff5eb5..c9b4e8f 100644
--- a/src/main/java/frc/robot/commands/climber/Toggle.java
+++ b/src/main/java/frc/robot/commands/climber/Toggle.java
@@ -25,8 +25,11 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    m_climberSubsystem.getPosition();
     if (m_climberSubsystem.isAtBottom()) {
-
+      m_climberSubsystem.climb();
+    } else if (m_climberSubsystem.isAtTop()) {
+      m_climberSubsystem.descend();
     }
   }
 
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 8f33bb7..089044d 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -68,36 +68,24 @@ public void periodic() {
     SmartDashboard.putBoolean("Limit Switch", fullyRetracted());
   }
 
-  public void toggle() {
-    if (m_elevatorSimulation.getPositionMeters() == 0) {
+  public void getPosition() {
+    if (m_elevatorSimulation.getPositionMeters() == 0.01) {
       isBottom = true;
       atTop = false;
     } else if (m_elevatorSimulation.getPositionMeters() == 1) {
       isBottom = false;
       atTop = true;
     }
-    if (isBottom) {
-      climb();
-      stop();
-    } else if (atTop) {
-      descend();
-      stop();
-    }
-    // if (m_elevatorSimulation.getPositionMeters() >= 0) {
-    // climb();
-    // if (m_elevatorSimulation.getPositionMeters() == 0.99) {
-    // stop();
-    // }
-    // } else if (m_elevatorSimulation.getPositionMeters() == 0.99) {
-    // descend();
-    // if (m)
-    // }
   }
 
   public boolean isAtBottom() {
     return isBottom;
   }
 
+  public boolean isAtTop() {
+    return atTop;
+  }
+
   public boolean fullyRetracted() {
     return m_limitSwitch.get();
   }

From c623aac74da58f857d4df04b92177e930244026a Mon Sep 17 00:00:00 2001
From: Photon-Aiden <87028152+Photon-Aiden@users.noreply.github.com>
Date: Fri, 23 Sep 2022 16:29:01 -0400
Subject: [PATCH 16/18] Finished climber gamepad controls

smelly code
---
 simgui.json                                   |  3 ++
 src/main/java/frc/robot/RobotContainer.java   |  8 ++--
 .../frc/robot/commands/climber/Toggle.java    | 47 -------------------
 .../robot/subsystems/ClimberSubsystem.java    | 18 ++++++-
 4 files changed, 23 insertions(+), 53 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/climber/Toggle.java

diff --git a/simgui.json b/simgui.json
index d0d56c6..02065a4 100644
--- a/simgui.json
+++ b/simgui.json
@@ -41,6 +41,9 @@
       }
     },
     "SmartDashboard": {
+      "Elevator Sim": {
+        "open": true
+      },
       "open": true
     },
     "status": {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ef07b9b..5d40f75 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -8,12 +8,11 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import frc.robot.commands.RunIndexer;
+// import frc.robot.commands.RunIndexer;
 import frc.robot.commands.auto.DriveToDistance;
 import frc.robot.commands.climber.Calibrate;
 import frc.robot.commands.climber.ElevatorDown;
 import frc.robot.commands.climber.ElevatorUp;
-import frc.robot.commands.climber.Toggle;
 import frc.robot.commands.drive.SimDrive;
 import frc.robot.commands.intake.IntakeRun;
 import frc.robot.commands.intake.ToggleIntake;
@@ -56,7 +55,7 @@ public class RobotContainer {
 
   private final JoystickButton m_optionButton = new JoystickButton(driverGamepad, PS4Controller.Button.kOptions.value);
   private final JoystickButton m_shareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kShare.value);
-  private final JoystickButton m_squareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kSquare.value);
+  // private final JoystickButton m_squareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kSquare.value);
   private final JoystickButton m_l1Button = new JoystickButton(driverGamepad, PS4Controller.Button.kL1.value);
   private final JoystickButton m_r1Button = new JoystickButton(driverGamepad, PS4Controller.Button.kR1.value);
 
@@ -81,10 +80,9 @@ public RobotContainer() {
   private void configureButtonBindings() {
     triangleButton.whileHeld(new AutoAimTurret(m_turretSubsystem, 0.05));
     m_circleButton.whenPressed(new ToggleIntake(m_intakeSubsystem));
-    X_BUTTON.whileHeld(new RunIndexer(m_indexerSubsystem));
+    // X_BUTTON.whileHeld(new RunIndexer(m_indexerSubsystem));
     m_optionButton.whileHeld(new ElevatorUp(m_climberSubsystem)); // Elevator Up
     m_shareButton.whileHeld(new ElevatorDown(m_climberSubsystem)); // Elevator Down
-    m_squareButton.toggleWhenPressed(new Toggle(m_climberSubsystem));
     m_l1Button.toggleWhenPressed(new ElevatorDown(m_climberSubsystem));
     m_r1Button.toggleWhenPressed(new ElevatorUp(m_climberSubsystem));
   }
diff --git a/src/main/java/frc/robot/commands/climber/Toggle.java b/src/main/java/frc/robot/commands/climber/Toggle.java
deleted file mode 100644
index c9b4e8f..0000000
--- a/src/main/java/frc/robot/commands/climber/Toggle.java
+++ /dev/null
@@ -1,47 +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 frc.robot.commands.climber;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.subsystems.ClimberSubsystem;
-
-public class Toggle extends CommandBase {
-  /** Creates a new Toggle. */
-  ClimberSubsystem m_climberSubsystem;
-  boolean y;
-
-  public Toggle(ClimberSubsystem climbSubsystem) {
-    m_climberSubsystem = climbSubsystem;
-    addRequirements(m_climberSubsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_climberSubsystem.getPosition();
-    if (m_climberSubsystem.isAtBottom()) {
-      m_climberSubsystem.climb();
-    } else if (m_climberSubsystem.isAtTop()) {
-      m_climberSubsystem.descend();
-    }
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    m_climberSubsystem.stop();
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 089044d..b16d4c1 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -32,11 +32,14 @@ public class ClimberSubsystem extends SubsystemBase {
   public double m_climbSpeed;
   private final RelativeEncoder m_encoder;
   PIDController pid;
+
   ElevatorSim m_elevatorSimulation;
   EncoderSim m_encoderSimulation;
+
   private final Mechanism2d m_mech2d;
   private final MechanismRoot2d m_mech2dRoot;
   private final MechanismLigament2d m_elevatorMech2d;
+
   boolean isBottom;
   boolean atTop;
   boolean toggleLimit;
@@ -44,17 +47,30 @@ public class ClimberSubsystem extends SubsystemBase {
   public ClimberSubsystem() {
     m_climberMotor = new CANSparkMax(Climber.kClimberMotor_Port, MotorType.kBrushless);
     m_limitSwitch = new DigitalInput(Climber.kLimitSwitch_Port);
+
     pid = new PIDController(Climber.kP, Climber.kI, Climber.kD);
+
     m_encoder = m_climberMotor.getEncoder();
     m_encoder.setPosition(0);
+
     m_climbSpeed = Climber.kElevatorSpeed;
-    m_elevatorSimulation = new ElevatorSim(DCMotor.getNEO(1), 100, 45, Units.inchesToMeters(2), 0, 1);
+
+    m_elevatorSimulation = new ElevatorSim(DCMotor.getNEO(1),
+                                  100,
+                           45,
+                                          Units.inchesToMeters(2),
+                          0,
+                          1);
+
     m_encoderSimulation = new EncoderSim(new Encoder(1, 2));
+    
     m_mech2d = new Mechanism2d(20, 50);
     m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 2);
     m_elevatorMech2d = m_mech2dRoot.append(
         new MechanismLigament2d("Elevator", Units.metersToInches(m_elevatorSimulation.getPositionMeters()), 90));
+
     SmartDashboard.putData("Elevator Sim", m_mech2d);
+
     isBottom = true;
     atTop = false;
   }

From ac8cdc7d1c57af29cf63afc847577309c4163ce7 Mon Sep 17 00:00:00 2001
From: ProcrastinatorZZZ <sophia.cq.wang@gmail.com>
Date: Fri, 28 Oct 2022 16:56:47 -0400
Subject: [PATCH 17/18] merged main to climber

solved merged conflicts, deleted one curly bracket
---
 src/main/java/frc/robot/Constants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1d5eab5..6eda601 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -125,7 +125,7 @@ public static final class Climber {
         public static final double kI = 0.0;
         public static final double kD = 0.0;
     }
-}
+
     public static final class Indexer {
         public static final int kProximityThreshold = 200; //[0, 2047]
         public static final double kTransitionSpeed = 0.5;

From ada8de69cfebe3ba594dce3ebd652fa6bf826efb Mon Sep 17 00:00:00 2001
From: Astraea Robotics <ebfirstinc@gmail.com>
Date: Tue, 1 Nov 2022 15:42:47 -0400
Subject: [PATCH 18/18] Clean up for testing

---
 .../frc/robot/commands/climber/Calibrate.java |  4 +-
 .../robot/subsystems/ClimberSubsystem.java    | 55 ++++++++-----------
 2 files changed, 24 insertions(+), 35 deletions(-)

diff --git a/src/main/java/frc/robot/commands/climber/Calibrate.java b/src/main/java/frc/robot/commands/climber/Calibrate.java
index 14d0c18..6eb6d3b 100644
--- a/src/main/java/frc/robot/commands/climber/Calibrate.java
+++ b/src/main/java/frc/robot/commands/climber/Calibrate.java
@@ -5,7 +5,6 @@
 package frc.robot.commands.climber;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.Constants.Climber;
 import frc.robot.subsystems.ClimberSubsystem;
 
 // for setting soft limits
@@ -20,8 +19,7 @@ public Calibrate(ClimberSubsystem climbSubsystem) {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {
-  }
+  public void initialize() {}
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index b16d4c1..d486139 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -22,7 +22,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants.Climber;
 
 public class ClimberSubsystem extends SubsystemBase {
@@ -31,7 +30,6 @@ public class ClimberSubsystem extends SubsystemBase {
   private DigitalInput m_limitSwitch;
   public double m_climbSpeed;
   private final RelativeEncoder m_encoder;
-  PIDController pid;
 
   ElevatorSim m_elevatorSimulation;
   EncoderSim m_encoderSimulation;
@@ -45,29 +43,24 @@ public class ClimberSubsystem extends SubsystemBase {
   boolean toggleLimit;
 
   public ClimberSubsystem() {
-    m_climberMotor = new CANSparkMax(Climber.kClimberMotor_Port, MotorType.kBrushless);
+    m_climberMotor =
+        new CANSparkMax(Climber.kClimberMotor_Port, MotorType.kBrushless);
     m_limitSwitch = new DigitalInput(Climber.kLimitSwitch_Port);
 
-    pid = new PIDController(Climber.kP, Climber.kI, Climber.kD);
-
     m_encoder = m_climberMotor.getEncoder();
     m_encoder.setPosition(0);
 
     m_climbSpeed = Climber.kElevatorSpeed;
 
-    m_elevatorSimulation = new ElevatorSim(DCMotor.getNEO(1),
-                                  100,
-                           45,
-                                          Units.inchesToMeters(2),
-                          0,
-                          1);
+    m_elevatorSimulation = new ElevatorSim(DCMotor.getNEO(1), 100, 45,
+        Units.inchesToMeters(2), 0, 1);
 
     m_encoderSimulation = new EncoderSim(new Encoder(1, 2));
-    
+
     m_mech2d = new Mechanism2d(20, 50);
     m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 2);
-    m_elevatorMech2d = m_mech2dRoot.append(
-        new MechanismLigament2d("Elevator", Units.metersToInches(m_elevatorSimulation.getPositionMeters()), 90));
+    m_elevatorMech2d = m_mech2dRoot.append(new MechanismLigament2d("Elevator",
+        Units.metersToInches(m_elevatorSimulation.getPositionMeters()), 90));
 
     SmartDashboard.putData("Elevator Sim", m_mech2d);
 
@@ -78,9 +71,8 @@ public ClimberSubsystem() {
   @Override
   public void periodic() {
     // setSpeed(0.2);
-    SmartDashboard.putNumber("Elevator Sim Position", m_elevatorSimulation.getPositionMeters());
-    SmartDashboard.putNumber("Soft Limits (Forward)", m_climberMotor.getSoftLimit(SoftLimitDirection.kForward));
-    SmartDashboard.putNumber("Soft Limits (Reverse)", m_climberMotor.getSoftLimit(SoftLimitDirection.kReverse));
+    SmartDashboard.putNumber("Elevator Sim Position",
+        m_elevatorSimulation.getPositionMeters());
     SmartDashboard.putBoolean("Limit Switch", fullyRetracted());
   }
 
@@ -109,47 +101,46 @@ public boolean fullyRetracted() {
   public void setSoftLimits() {
     m_climberMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
     m_climberMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
-    m_climberMotor.setSoftLimit(SoftLimitDirection.kForward, Climber.kUpperLimit); // 15
-    m_climberMotor.setSoftLimit(SoftLimitDirection.kReverse, Climber.kLowerLimit); // 0
+    m_climberMotor.setSoftLimit(SoftLimitDirection.kForward,
+        Climber.kUpperLimit); // 15
+    m_climberMotor.setSoftLimit(SoftLimitDirection.kReverse,
+        Climber.kLowerLimit); // 0
   }
 
   public void setSpeed(double speed) {
     m_climberMotor.set(speed);
   }
 
-  public void setPIDSpeed(double point) {
-    m_climberMotor.set(pid.calculate(m_encoder.getPosition(), point));
-    SmartDashboard.putNumber("PID", pid.calculate(m_encoder.getPosition(), point));
-  }
 
   public void reset() {
     m_encoder.setPosition(0);
   }
 
   public void climb() {
-    setPIDSpeed(1);
+    setSpeed(Climber.kElevatorSpeed);
   }
 
   public void descend() {
-    setPIDSpeed(-1);
+    setSpeed(-Climber.kElevatorSpeed);
   }
 
   public void stop() {
-    m_climberMotor.set(0);
+    setSpeed(0);
   }
 
   public void log() {
-    SmartDashboard.putNumber("Current Position", Double.valueOf(String.valueOf(m_climberMotor.getEncoder())));
-    SmartDashboard.putNumber("Upper Limit", Climber.kUpperLimit);
-    SmartDashboard.putNumber("Lower Limit", Climber.kLowerLimit);
+    SmartDashboard.putNumber("Current Position",
+        Double.valueOf(String.valueOf(m_climberMotor.getEncoder())));
   }
 
   @Override
   public void simulationPeriodic() {
-    m_elevatorSimulation.setInput(m_climberMotor.get() * RobotController.getBatteryVoltage());
+    m_elevatorSimulation
+        .setInput(m_climberMotor.get() * RobotController.getBatteryVoltage());
     m_elevatorSimulation.update(0.020);
     m_encoderSimulation.setDistance(m_elevatorSimulation.getPositionMeters());
-    m_elevatorMech2d.setLength(Units.metersToInches(m_elevatorSimulation.getPositionMeters()));
+    m_elevatorMech2d.setLength(
+        Units.metersToInches(m_elevatorSimulation.getPositionMeters()));
 
   }
-}
\ No newline at end of file
+}