Elevator

src/main/java/frc/team3128/Constants.java

@@ -515,8 +515,29 @@ public static class ManipulatorConstants{
         public static final double ROLLER_VOLTAGE = 8;
     }
 
+    public static class ElevatorConstants {
+        public static final int ELV1_ID = 11;
+        public static final int ELV2_ID = 12;
+
+        public static final double kP = 6;
+        public static final double kI = 0;
+        public static final double kD = 0;
+
+        public static final double kS = 0.475;
+        public static final double kV = 0;
+        public static final double kG = 0.625;
+
+        public static final double MIN_DIST = 5; //Ask Charlie
+        public static final double MAX_DIST = 55; //Ask Charlie
+
+        public static final double GEAR_RATIO = 12.5;
+        public static final double SPOOL_CIRCUMFERENCE = 3 * Math.PI;
+
+        public static final int CURRENT_LIMIT = 80;
+    }
+
     public static class BalanceConstants{
-        public static final double turnKP = 0.05;
+        public static final double turnKP = 6;
         public static final double turnKI = 0;
         public static final double turnKD = .005;
         public static final double TURN_TOLERANCE = 1.5;

src/main/java/frc/team3128/RobotContainer.java

@@ -27,6 +27,7 @@
 import frc.team3128.common.hardware.input.NAR_XboxController;
 import frc.team3128.common.narwhaldashboard.NarwhalDashboard;
 import frc.team3128.common.utility.Log;
+import frc.team3128.subsystems.Elevator;
 import frc.team3128.subsystems.Led;
 import frc.team3128.common.utility.NAR_Shuffleboard;
 import frc.team3128.subsystems.Swerve;
@@ -98,7 +99,11 @@ private void configureButtonBindings() {
         controller.getButton("B").onTrue(new InstantCommand(()-> swerve.resetEncoders()));
 
         rightStick.getButton(1).onTrue(new InstantCommand(()->swerve.zeroGyro()));
-
+        rightStick.getButton(2).onTrue(new InstantCommand(()-> Elevator.getInstance().set(0.4))).onFalse(new InstantCommand(()-> Elevator.getInstance().set(0)));
+        rightStick.getButton(3).onTrue(new InstantCommand(()-> Elevator.getInstance().set(-0.4))).onFalse(new InstantCommand(()-> Elevator.getInstance().set(0)));
+        rightStick.getButton(4).onTrue(moveElevator(30));
+        rightStick.getButton(5).onTrue(new InstantCommand(()-> Elevator.getInstance().resetEncoder()));
+
         rightStick.getButton(7).onTrue(Commands.sequence(
                                             Commands.deadline(Commands.sequence(new WaitUntilCommand(()-> Math.abs(swerve.getPitch()) > 6), new CmdBangBangBalance()), new CmdBalance()), 
                                             //new RunCommand(()-> swerve.drive(new Translation2d(CmdBalance.DIRECTION ? -0.25 : 0.25,0),0,true)).withTimeout(0.5), 

src/main/java/frc/team3128/commands/CmdManager.java

@@ -1,19 +1,50 @@
 package frc.team3128.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import static edu.wpi.first.wpilibj2.command.Commands.*;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ScheduleCommand;
 import edu.wpi.first.wpilibj2.command.StartEndCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.team3128.RobotContainer;
 import frc.team3128.common.hardware.input.NAR_XboxController;
+import frc.team3128.subsystems.Elevator;
+
 
 public class CmdManager {
     private static NAR_XboxController controller = RobotContainer.controller;
+    private static Elevator elevator = Elevator.getInstance();
+
 
 
     private CmdManager() {}
 
     public static CommandBase vibrateController() {
         return new ScheduleCommand(new WaitCommand(0.5).deadlineWith(new StartEndCommand(() -> controller.startVibrate(), () -> RobotContainer.controller.stopVibrate())));
     }
+
+    public static CommandBase moveElevator(double setpoint) {
+        return sequence(
+            runOnce(() -> elevator.startPID(setpoint), elevator),
+            waitUntil(() -> elevator.atSetpoint())
+        );
+        // return run(() -> elevator.startPID(setpoint), elevator);
+    }
+
+    public static CommandBase moveElevator(Elevator.States setpoint) {
+        return sequence(
+            runOnce(() -> elevator.startPID(setpoint.height), elevator),
+            waitUntil(() -> elevator.atSetpoint())
+        );
+    }
+
+    public static CommandBase moveElv(double speed) {
+        return runOnce(()-> elevator.set(speed), elevator);
+    }
+
+
+
+
+
+
 }

src/main/java/frc/team3128/subsystems/Elevator.java

@@ -0,0 +1,81 @@
+package frc.team3128.subsystems;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax;
+import static frc.team3128.Constants.ElevatorConstants.*;
+
+public class Elevator extends NAR_PIDSubsystem {
+
+    private NAR_CANSparkMax m_elv1, m_elv2;
+
+    private static Elevator instance;
+
+    public static Elevator getInstance() {
+        if (instance == null){
+            instance = new Elevator();  
+        }
+        return instance;
+    }
+
+    public enum States {
+        LOW(0),
+        MID(0),
+        HIGH(0),
+        GROUND(0),
+        SHELF(0),
+        DROP(0);
+
+        public double height;
+        private States(double height) {
+            this.height = height;
+        }
+    }
+
+    public Elevator() {
+        super(new PIDController(kP, kI, kD), kS, kV, kG);
+        setConstraints(MIN_DIST, MAX_DIST);
+        configMotors();
+        initShuffleboard(kS, kV, kG);
+        resetEncoder();
+    }
+
+    private void configMotors() {
+        m_elv1 = new NAR_CANSparkMax(ELV1_ID);
+        m_elv2 = new NAR_CANSparkMax(ELV2_ID);
+
+        m_elv1.setInverted(false);
+        m_elv2.setInverted(true);
+
+        m_elv1.setSmartCurrentLimit(CURRENT_LIMIT); 
+        m_elv2.setSmartCurrentLimit(CURRENT_LIMIT);
+    }
+
+    @Override
+    protected void useOutput(double output, double setpoint) {
+        final double power = MathUtil.clamp(output / 12.0, -1, 1);
+        m_elv1.set(power);
+        m_elv2.set(power);
+    }
+
+    public void set(double power) {
+        disable();
+        m_elv1.set(power);
+        m_elv2.set(power);
+    }
+
+    public void stop() {
+        set(0);
+    }
+
+    public void resetEncoder() {
+        m_elv1.setSelectedSensorPosition(0);
+    }
+
+    @Override
+    public double getMeasurement() {
+        return m_elv1.getSelectedSensorPosition() / GEAR_RATIO * SPOOL_CIRCUMFERENCE;
+    }
+
+
+}

src/main/java/frc/team3128/subsystems/NAR_PIDSubsystem.java

@@ -35,7 +35,7 @@ public abstract class NAR_PIDSubsystem extends SubsystemBase {
     public NAR_PIDSubsystem(PIDController controller, double kS, double kV, double kG) {
         m_controller = controller;
         this.kG_Function = () -> 1;
-        initShuffleboard(kS, kV, kG);
+        // initShuffleboard(kS, kV, kG);
         min = Double.NEGATIVE_INFINITY;
         max = Double.POSITIVE_INFINITY;
     }
@@ -60,7 +60,7 @@ public void periodic() {
         }
     }
 
-    private void initShuffleboard(double kS, double kV, double kG) {
+    public void initShuffleboard(double kS, double kV, double kG) {
         NAR_Shuffleboard.addComplex(getName(), "PID_Controller", m_controller, 0, 0);
 
         NAR_Shuffleboard.addData(getName(), "Enabled", ()-> isEnabled(), 1, 0);