From 9e65adb829cd4b087e90f741b14081a6a03f0c65 Mon Sep 17 00:00:00 2001
From: Ethan Li <ethanchili224@gmail.com>
Date: Thu, 30 Nov 2023 17:39:42 -0800
Subject: [PATCH] Hopefully Brandon fixed it

(I have no idea what he did)
---
 .../frc/Subsystems/ElevatorSubsystem.java     | 64 +++++++------------
 src/main/java/frc/robot/RobotContainer.java   |  8 +--
 2 files changed, 23 insertions(+), 49 deletions(-)

diff --git a/src/main/java/frc/Subsystems/ElevatorSubsystem.java b/src/main/java/frc/Subsystems/ElevatorSubsystem.java
index 69a8ffa..a21fd46 100644
--- a/src/main/java/frc/Subsystems/ElevatorSubsystem.java
+++ b/src/main/java/frc/Subsystems/ElevatorSubsystem.java
@@ -2,17 +2,12 @@
 // 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.Subsystems;
 
-
-
-
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 import com.ctre.phoenix.motorcontrol.can.TalonFX;
 
-
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
@@ -22,38 +17,36 @@
 import frc.robot.Constants.Elevator;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-
 public class ElevatorSubsystem extends SubsystemBase {
  /** Creates a new ExampleSubsystem. */
- private TalonFX left_motor;
- /** leader */
- private TalonFX right_motor;
+ private TalonFX left_motor; /** leader */
+ private TalonFX right_motor; /** follower */
+ private TalonFX wrist_motor;
  private PIDController pidController;
  private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator");
 
-
+ private double currentHeight;
  private double targetHeight;
  private double motorPower;
   public ElevatorSubsystem() {
    left_motor = new TalonFX(14);
    right_motor = new TalonFX(15);
+   wrist_motor = new TalonFX(16);
    right_motor.configFactoryDefault();
    left_motor.configFactoryDefault();
     right_motor.clearStickyFaults();
    left_motor.clearStickyFaults();
 
-
    right_motor.configForwardSoftLimitThreshold(
        0, 0); // this is the bottom limit, we stop AT the bottom
+
    // right_motor.configReverseSoftLimitThreshold(
    //     -heightToTicks(24), 0); // this is the top limit, we stop at the very top
    right_motor.configForwardSoftLimitEnable(true, 0);
    right_motor.configReverseSoftLimitEnable(true, 0);
 
-
    right_motor.configOpenloopRamp(.5);
 
-
    left_motor.setSelectedSensorPosition(0);
    right_motor.setSelectedSensorPosition(0);
 
@@ -61,18 +54,21 @@ public ElevatorSubsystem() {
    // make sure we hold our height when we get disabled
    right_motor.setNeutralMode(NeutralMode.Coast);
    left_motor.setNeutralMode(NeutralMode.Coast);
+   wrist_motor.setNeutralMode(NeutralMode.Brake);
 
 
    right_motor.follow(left_motor);
   
    targetHeight = 0;
 
-
    motorPower = 0;
+
  pidController = new PIDController(0.34, 0, 0.02);
+
  //pidController.setTolerance(0.7,0.001);
    ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower);
    ElevatorTab.addNumber("Target Height", () -> this.targetHeight);
+   ElevatorTab.addNumber("Current Height", () -> currentHeight);
    ElevatorTab.add(pidController);
 
 
@@ -84,16 +80,6 @@ public ElevatorSubsystem() {
    // ElevatorTab.addNumber("height", () -> this.currentHeight);
    // ElevatorTab.addNumber("target height", () -> this.targetHeight);
    // ElevatorTab.addNumber("right motor sensor value", this::getHeight);
-
-
-
-
-
-
-
-
-
-
  }
  public void setMotorPower(double x){
    System.out.println("hello");
@@ -104,37 +90,31 @@ public static double inchesToTicks(double height) {
    public static double ticksToInches(double ticks) {
      return (ticks * Elevator.GEAR_CIRCUMFERENCE) / (Elevator.TICKS_PER_REVOLUTION * Elevator.GEAR_RATIO);
    }
-                      
-                                                                               
+                                                                                              
    public void setTargetHeight(double targetHeight) {    
      this.targetHeight = targetHeight;                                  
-     pidController.setSetpoint(this.targetHeight); } 
+   } 
           
-   public double getCurrentHeight(){
-     return ticksToInches(-left_motor.getSelectedSensorPosition());
-   }
 public boolean nearTargetHeight(){
-   if(targetHeight - 0.5 <= getCurrentHeight() && getCurrentHeight() <= targetHeight + 0.5)return true;
-   return false;
+   if(targetHeight -0.5 <= currentHeight && currentHeight <= targetHeight + 0.5){
+    return true;
+   }else{
+    return false;
+   }
  }
+
  @Override
  public void periodic() {
    // This method will be called once per scheduler run
-   motorPower = pidController.calculate(getCurrentHeight());
-   if (!pidController.atSetpoint()){
-     if (getCurrentHeight()<5){
-       left_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower + 0.02, -0.2,0.2)));
-     }
-     else{
+   motorPower = pidController.calculate(currentHeight, targetHeight);
        left_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower + 0.02, -0.5,0.5)));
-     }
-   }
+       currentHeight = ticksToInches(-left_motor.getSelectedSensorPosition());
+      }
    // left_motor.set(TalonFXControlMode.PercentOutput, -(0.1));
- }
 
 
  @Override
  public void simulationPeriodic() {
    // This method will be called once per scheduler run during simulation
    }
- }
\ No newline at end of file
+  }
\ 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 2ca7234..f0003a1 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -29,13 +29,7 @@ public class RobotContainer {
  public RobotContainer() {
 
 
-   double[] levels = {5d,10d,17d,8d,4d,7d,1d};
-
-
-   for(double i :levels){
-     targetHeight = i;
-   }
-
+  
 
    // Configure the button bindings
    configureButtonBindings();