Added Elevator Code

SwerveSpecialties2022/src/main/java/frc/robot/Constants.java

@@ -47,6 +47,26 @@ public final class Constants {
           new Translation2d(-DRIVETRAIN_TRACKWIDTH_METERS / 2.0, -DRIVETRAIN_WHEELBASE_METERS / 2.0)
   );
 
+  public static final double ELEVATOR_MAX_RPM = 100;
+
+  public static final double ELEVATOR_MAX_HEIGHT = 10.0;
+  public static final double ELEVATOR_MIN_HEIGHT = 0.0;
+
+  public static final double ELEVATOR_kP = 0.0;
+  public static final double ELEVATOR_kI = 0.0;
+  public static final double ELEVATOR_kD = 0.0;
+  public static final double ELEVATOR_kS = 0.0;
+  public static final double ELEVATOR_kV = 0.0;
+  public static final double ELEVATOR_kA = 0.0;
+  public static final double ELEVATOR_kG = 0.0;
+
+  public static final double ELEVATOR_Min = 0.0;
+  public static final double ELEVATOR_Max = 10.0;
+
+  //public static final double ELEVATOR_FEEDFORWARD = 0.0;
+
+  public static final double ELEVATOR_SPROKET_RADIUS = 1.0;
+
 
     public static final int DRIVETRAIN_PIGEON_ID = 0;
 
@@ -69,4 +89,7 @@ public final class Constants {
     public static final int BACK_RIGHT_MODULE_STEER_MOTOR = 8;
     public static final int BACK_RIGHT_MODULE_STEER_ENCODER = 9;
     public static final double BACK_RIGHT_MODULE_STEER_OFFSET = -Math.toRadians(89.29); //
+
+    public static final int LEFT_ELEVATOR_MOTOR = 13;
+    public static final int RIGHT_ELEVATOR_MOTOR = 14;
 }

SwerveSpecialties2022/src/main/java/frc/robot/commands/ElevatorPositionCommand.java

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class ElevatorPositionCommand extends CommandBase {
+  /** Creates a new ElevatorPositionCommand. */
+  public ElevatorPositionCommand() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

SwerveSpecialties2022/src/main/java/frc/robot/commands/ElevatorVelocityCommand.java

@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.Controller;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants;
+import frc.robot.subsystems.ElevatorSubsystem;
+
+public class ElevatorVelocityCommand extends CommandBase {
+  private XboxController coDriver;
+  private ElevatorSubsystem elevatorSubsystem;
+  /** Creates a new ElevatorVelocityCommand. */
+  public ElevatorVelocityCommand(ElevatorSubsystem elevatorSubsystem, XboxController coDriver) {
+    this.elevatorSubsystem = elevatorSubsystem;
+    this.coDriver = coDriver;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    double leftVelocity = coDriver.getLeftY();
+    elevatorSubsystem.setTargetVelocity(leftVelocity*Constants.ELEVATOR_MAX_RPM);
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

SwerveSpecialties2022/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxPIDController;
+import com.revrobotics.CANSparkMax.ControlType;
+
+import edu.wpi.first.wpilibj.CAN;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class ElevatorSubsystem extends SubsystemBase {
+
+  private SparkMaxPIDController PIDController;
+  private RelativeEncoder encoder;
+
+  private CANSparkMax motorLeft;
+  private CANSparkMax motorRight;
+
+  /** Creates a new ElevatorSubsystem. */
+  public ElevatorSubsystem(CANSparkMax motorLeft, CANSparkMax motorRight) {
+    this.motorLeft = motorLeft;
+    this.motorRight = motorRight;
+
+    this.motorLeft.setInverted(false);
+    this.motorRight.setInverted(true);
+
+    this.motorRight.follow(this.motorLeft);
+
+    PIDController = motorLeft.getPIDController();
+    PIDController.setP(Constants.ELEVATOR_kP);
+    PIDController.setI(Constants.ELEVATOR_kI);
+    PIDController.setD(Constants.ELEVATOR_kD);
+
+    PIDController.setOutputRange(Constants.ELEVATOR_Min, Constants.ELEVATOR_Max);
+
+    encoder = motorLeft.getEncoder();
+    encoder.setPosition(0.0);
+
+  }
+
+  public double getElevatorHeight() {
+    double circumference = Constants.ELEVATOR_SPROKET_RADIUS*2*Math.PI;
+    return circumference*encoder.getPosition();
+  }
+
+  public void setTargetPosition(double point) { // Sets Target to Position in _
+    PIDController.setReference(point, CANSparkMax.ControlType.kPosition);
+  }
+
+  public void setTargetVelocity(double point) { // Sets Target to Velocity in RPM
+    PIDController.setReference(point, CANSparkMax.ControlType.kVelocity);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}