From 4e97763c78371cf9d1dbf43a48c624791eef6521 Mon Sep 17 00:00:00 2001
From: Luhan Wang <117863446+Lu-han-wang@users.noreply.github.com>
Date: Fri, 26 Jan 2024 20:17:41 -0500
Subject: [PATCH] basic climber done TODO: tune and test PID and add
 setpoints/controls

---
 .../subsystems/climber/ClimbCommand.java      | 30 +++++++++++++
 .../subsystems/climber/ClimbSubsystem.java    | 43 +++++++++++++++++++
 2 files changed, 73 insertions(+)
 create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/climber/ClimbCommand.java
 create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/climber/ClimbSubsystem.java

diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/climber/ClimbCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/climber/ClimbCommand.java
new file mode 100644
index 00000000..66cb85bd
--- /dev/null
+++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/climber/ClimbCommand.java
@@ -0,0 +1,30 @@
+package org.jmhsrobotics.frc2024.subsystems.climber;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.SparkPIDController;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class ClimbCommand extends Command {
+	private ClimbSubsystem climbSubsystem;
+	private double length;
+	private SparkPIDController lengthPIDController;
+
+	public ClimbCommand(ClimbSubsystem climbSubsystem, double length) {
+		this.climbSubsystem = climbSubsystem;
+		this.length = length;
+		addRequirements(climbSubsystem);
+
+		lengthPIDController = climbSubsystem.getMotor().getPIDController();
+		lengthPIDController.setFeedbackDevice(climbSubsystem.getEncoder());
+
+		// TODO: tune vals
+		lengthPIDController.setP(.1);
+		lengthPIDController.setI(0);
+		lengthPIDController.setD(0);
+		lengthPIDController.setFF(0);
+		lengthPIDController.setOutputRange(-1, 1);
+		lengthPIDController.setReference(length, CANSparkMax.ControlType.kPosition);
+	}
+
+}
diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/climber/ClimbSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/climber/ClimbSubsystem.java
new file mode 100644
index 00000000..696713e5
--- /dev/null
+++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/climber/ClimbSubsystem.java
@@ -0,0 +1,43 @@
+package org.jmhsrobotics.frc2024.subsystems.climber;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+
+public class ClimbSubsystem extends SubsystemBase {
+
+	private CANSparkMax climbMotor = new CANSparkMax(11, MotorType.kBrushless);
+	private RelativeEncoder climbEncoder = climbMotor.getEncoder();
+
+	public ClimbSubsystem() {
+		climbEncoder.setPosition(0);
+		climbMotor.setIdleMode(IdleMode.kBrake);
+		climbMotor.setSmartCurrentLimit(40);
+
+	}
+
+	public CANSparkMax getMotor() {
+		return climbMotor;
+	}
+
+	public void setMotor(double amount) {
+		climbMotor.set(amount);
+	}
+
+	public double getClimbPosition() {
+		return climbEncoder.getPosition();
+	}
+
+	public void setEncoder(double amount) {
+		climbEncoder.setPosition(amount);
+	}
+
+	public RelativeEncoder getEncoder() {
+		return climbEncoder;
+	}
+
+}