From 111de827dcd49e82730e9ba43e83c41799c0f6fe Mon Sep 17 00:00:00 2001
From: Samantha <27nguyens28@stu.smuhsd.org>
Date: Tue, 28 Nov 2023 17:00:29 -0800
Subject: [PATCH] here eng my b

---
 .../robot/Subsystems/ElevatorSubsystem.java   | 25 +++++++++----------
 1 file changed, 12 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java
index baa3f78..3d90dbc 100644
--- a/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java
@@ -3,23 +3,12 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot.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;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Elevator;
 
-// import frc.robot.Constants.Elevator;
-import java.util.Timer;
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 // private PIDController controller;
 
 public class ElevatorSubsystem extends SubsystemBase {
@@ -29,6 +18,7 @@ public class ElevatorSubsystem extends SubsystemBase {
   private PIDController controller;
   private double targetHeight;
   private double motorPower;
+  private double currentHeight;
   
 
   
@@ -51,14 +41,23 @@ public void setTargetHeight(double targetHeight){
   
 
 
+  
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
     motorPower = controller.calculate(targetHeight);
+
+    leftMotor.set(TalonFXControlMode.PercentOutput, motorPower);
+    currentHeight = ticksToInches(-leftMotor.getSelectedSensorPosition());
   }
 
 
-public boolean nearTargetHeight() {
+
+  public boolean nearTargetHeight(){
+    if(targetHeight -0.5 <= currentHeight && currentHeight <= targetHeight +0.5){
+      return true;
+    }
     return false;
-}
+  }
+    
 }