Add commands

Co-authored-by: ambers7 <[email protected]>
Co-authored-by: ilovemarmot <[email protected]>

src/main/java/com/stuypulse/robot/commands/ElevatorDrive.java

@@ -0,0 +1,30 @@
+package com.stuypulse.robot.commands;
+
+import com.stuypulse.robot.subsystems.Elevator;
+import com.stuypulse.stuylib.input.Gamepad;
+import com.stuypulse.stuylib.streams.IStream;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class ElevatorDrive extends CommandBase {
+    private final Elevator elevator;
+    private IStream velocity;
+
+    public ElevatorDrive(Elevator elevator, Gamepad gamepad) {
+        this.elevator = elevator;
+
+        velocity = IStream.create(gamepad::getLeftY); // left Y is elevator in controls 
+
+        addRequirements(elevator);
+    }
+
+    @Override
+    public void execute() {
+        elevator.addTargetHeight(velocity.get());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

src/main/java/com/stuypulse/robot/commands/ElevatorToBottom.java

@@ -0,0 +1,11 @@
+package com.stuypulse.robot.commands;
+
+import com.stuypulse.robot.subsystems.Elevator;
+
+import static com.stuypulse.robot.constants.Settings.Elevator.*;
+
+public class ElevatorToBottom extends ElevatorToHeight {
+    public ElevatorToBottom(Elevator elevator) {
+        super(elevator, MIN_HEIGHT);
+    }
+}

src/main/java/com/stuypulse/robot/commands/ElevatorToHeight.java

@@ -0,0 +1,41 @@
+package com.stuypulse.robot.commands;
+
+import com.stuypulse.robot.subsystems.Elevator;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import static com.stuypulse.robot.constants.Settings.Elevator.*;
+
+public class ElevatorToHeight extends CommandBase {
+    private final Elevator elevator;
+    private final double height; 
+
+    private boolean instant;
+
+    public ElevatorToHeight(Elevator elevator, double height) {
+        this.elevator = elevator;
+        this.height = height;
+
+        instant = true;
+
+        addRequirements(elevator);
+    }
+
+    public final ElevatorToHeight untilReady() {
+        instant = false;
+        return this;
+    }
+
+    @Override
+    public void initialize() {
+        elevator.setTargetHeight(height);
+    }
+
+    @Override
+    public boolean isFinished() {
+        if (!instant) {
+            elevator.isReady(MAX_HEIGHT_ERROR);
+        }
+        return true;
+    }
+}

src/main/java/com/stuypulse/robot/commands/ElevatorToTop.java

@@ -0,0 +1,11 @@
+package com.stuypulse.robot.commands;
+
+import com.stuypulse.robot.subsystems.Elevator;
+
+import static com.stuypulse.robot.constants.Settings.Elevator.*;
+
+public class ElevatorToTop extends ElevatorToHeight {
+    public ElevatorToTop(Elevator elevator) {
+        super(elevator, MAX_HEIGHT);
+    }
+}

src/main/java/com/stuypulse/robot/subsystems/Elevator.java

@@ -15,14 +15,15 @@
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import static com.stuypulse.robot.constants.Settings.Elevator.*;
 import static com.stuypulse.robot.constants.Settings.Elevator.Encoder.*;
 import static com.stuypulse.robot.constants.Settings.PID.*;
 
 import static com.stuypulse.robot.constants.Settings.FeedForward.*;
 
-public class Elevator {
+public class Elevator extends SubsystemBase {
     // hardware
     public CANSparkMax leftMotor;
     public CANSparkMax rightMotor;
@@ -104,6 +105,14 @@ public void setVoltage(double voltage) {
         leftMotor.setVoltage(voltage);
     }
 
+    public final boolean isReady(double error) {
+        return Math.abs(getTargetHeight() - getHeight()) < error;
+    }
+
+    public void addTargetHeight(double delta) {
+        setTargetHeight(getTargetHeight() + delta);
+    }
+
     public void periodic() {
         setVoltage(position.update(getHeight(), getTargetHeight()));