Add control

StuyPulse · Jan 24, 2024 · 48f79bc · 48f79bc
1 parent 185903c
commit 48f79bc
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 1 deletion.
diff --git a/src/main/java/com/stuypulse/robot/subsystems/climber/AbstractClimber.java b/src/main/java/com/stuypulse/robot/subsystems/climber/AbstractClimber.java
@@ -15,12 +15,14 @@
 
 import com.stuypulse.stuylib.network.SmartNumber;
 
+import edu.wpi.first.math.controller.BangBangController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public abstract class AbstractClimber extends SubsystemBase {
     public static final AbstractClimber instance;
 
-    private SmartNumber targetHeight;
+    public final SmartNumber targetHeight;
+    public final BangBangController controller;
 
     static {
         instance = new Climber();
@@ -30,6 +32,11 @@ public static AbstractClimber getInstance() {
         return instance;
     }
 
+    public AbstractClimber() {
+        targetHeight = new SmartNumber("Climber/Target Height", getTargetHeight());
+        controller = new BangBangController();
+    }
+
     public void setTargetHeight(double height) {
         targetHeight.set(height);
     }

diff --git a/src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java b/src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java
@@ -6,9 +6,11 @@
 import com.stuypulse.robot.constants.Motors;
 
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import static com.stuypulse.robot.constants.Settings.Climber.Encoder.*;
+import static com.stuypulse.robot.constants.Settings.Climber.*;
 import static com.stuypulse.robot.constants.Ports.Climber.*;
 
 public class Climber extends AbstractClimber {
@@ -54,6 +56,20 @@ public double getVelocity() {
 
     @Override
     public void setVoltage(double voltage) {
+        if (atTop() && voltage > 0) {
+            DriverStation.reportWarning("Top Limit Reached", false);
+            voltage = 0.0;
+
+            leftEncoder.setPosition(MAX_HEIGHT);
+            rightEncoder.setPosition(MAX_HEIGHT);
+        } else if (atBottom() && voltage > 0) {
+            DriverStation.reportWarning("Bottom Limit Reached", false);
+            voltage = 0.0;
+
+            leftEncoder.setPosition(MIN_HEIGHT);
+            rightEncoder.setPosition(MIN_HEIGHT);
+        }
+
         rightMotor.setVoltage(voltage);
         leftMotor.setVoltage(voltage);
     }
@@ -70,6 +86,8 @@ public boolean atBottom() {
 
 	@Override
 	public void periodic() {
+        setVoltage(controller.calculate(getTargetHeight(), getHeight()));
+
         SmartDashboard.putNumber("Climber/Target Height", getTargetHeight());
         SmartDashboard.putNumber("Climber/Height", getHeight());