Initial Commit

src/main/java/frc/robot/Main.java

@@ -0,0 +1,15 @@
+// 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;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+public final class Main {
+  private Main() {}
+
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

src/main/java/frc/robot/Robot.java

@@ -0,0 +1,72 @@
+// 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;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private final RobotContainer m_robotContainer;
+
+  public Robot() {
+    m_robotContainer = new RobotContainer();
+  }
+
+  @Override
+  public void robotPeriodic() {
+    CommandScheduler.getInstance().run();
+  }
+
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  @Override
+  public void disabledExit() {}
+
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    if (m_autonomousCommand != null) {
+      CommandScheduler.getInstance().schedule(m_autonomousCommand);
+    }
+  }
+
+  @Override
+  public void autonomousPeriodic() {}
+
+  @Override
+  public void autonomousExit() {}
+
+  @Override
+  public void teleopInit() {
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void teleopExit() {}
+
+  @Override
+  public void testInit() {
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  @Override
+  public void testPeriodic() {}
+
+  @Override
+  public void testExit() {}
+}

src/main/java/frc/robot/RobotContainer.java

@@ -0,0 +1,20 @@
+// 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;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+
+public class RobotContainer {
+  public RobotContainer() {
+    configureBindings();
+  }
+
+  private void configureBindings() {}
+
+  public Command getAutonomousCommand() {
+    return Commands.print("No autonomous command configured");
+  }
+}

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -0,0 +1,36 @@
+package frc.robot.constants;
+
+public class ElevatorConstants {
+    public enum ElevatorPositions {
+        kBottom(0),
+        kL1(1),
+        kL2(2),
+        kTop(3);
+
+        private double position;
+
+        private ElevatorPositions(double position) {
+            this.position = position;
+        }
+
+        public double getPosition() {
+            return position;
+        }
+    }
+    public static final int kMotorCANID = 0;
+    
+    public static final int kEncoderChannelA = 0;
+    public static final int kEncoderChannelB = 0;
+
+    public static final double kDistancePerPulse = 1.0;
+
+    public static final double kP = 0;
+    public static final double kI = 0;
+    public static final double kD = 0;
+    public static final double kS = 0;
+    public static final double kG = 0;
+    public static final double kV = 0;
+    public static final double kA = 0;
+    public static final double kTolerance = 1;
+
+}

src/main/java/frc/robot/subsystems/ElevatorCommandOriented.java

@@ -0,0 +1,57 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.ElevatorConstants.ElevatorPositions;
+
+public class ElevatorCommandOriented extends SubsystemBase {
+    private SparkMax motor;
+
+    private Encoder encoder;
+
+    private PIDController controller;
+
+    private ElevatorFeedforward feedForward;
+
+    public ElevatorCommandOriented() {
+        motor = new SparkMax(ElevatorConstants.kMotorCANID, MotorType.kBrushless);
+
+        encoder = new Encoder(
+            ElevatorConstants.kEncoderChannelA, 
+            ElevatorConstants.kEncoderChannelB
+        );
+        encoder.setDistancePerPulse(ElevatorConstants.kDistancePerPulse);
+
+        controller = new PIDController(
+            ElevatorConstants.kP, 
+            ElevatorConstants.kI, 
+            ElevatorConstants.kD
+        );
+        controller.setTolerance(ElevatorConstants.kTolerance);
+
+        feedForward = new ElevatorFeedforward(
+            ElevatorConstants.kS, 
+            ElevatorConstants.kG, 
+            ElevatorConstants.kV,
+            ElevatorConstants.kA
+        );
+    }
+
+    public Command maintainPosition(ElevatorPositions position) {
+        return run(() -> {
+            double output = controller.calculate(
+                encoder.getDistance(), 
+                position.getPosition()
+            ) + feedForward.calculate(0);
+
+            motor.setVoltage(output);
+        });
+    }
+}

src/main/java/frc/robot/subsystems/ElevatorPeriodicOriented.java

@@ -0,0 +1,66 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.ElevatorConstants.ElevatorPositions;
+
+public class ElevatorPeriodicOriented extends SubsystemBase {
+    private SparkMax motor;
+
+    private Encoder encoder;
+
+    private PIDController controller;
+
+    private ElevatorFeedforward feedForward;
+
+    private ElevatorPositions targetPosition;
+
+    public ElevatorPeriodicOriented() {
+        motor = new SparkMax(ElevatorConstants.kMotorCANID, MotorType.kBrushless);
+
+        encoder = new Encoder(
+            ElevatorConstants.kEncoderChannelA, 
+            ElevatorConstants.kEncoderChannelB
+        );
+        encoder.setDistancePerPulse(ElevatorConstants.kDistancePerPulse);
+
+        controller = new PIDController(
+            ElevatorConstants.kP, 
+            ElevatorConstants.kI, 
+            ElevatorConstants.kD
+        );
+        controller.setTolerance(ElevatorConstants.kTolerance);
+
+        feedForward = new ElevatorFeedforward(
+            ElevatorConstants.kS, 
+            ElevatorConstants.kG, 
+            ElevatorConstants.kV,
+            ElevatorConstants.kA
+        );
+
+        targetPosition = ElevatorPositions.kBottom;
+    }
+
+    @Override
+    public void periodic() {
+        double output = controller.calculate(
+            encoder.getDistance(), 
+            targetPosition.getPosition()
+        ) + feedForward.calculate(0);
+
+        motor.setVoltage(output);
+    }
+
+    public Command setTargetPosition(ElevatorPositions position) {
+        return runOnce(() -> {
+            targetPosition = position;
+        });
+    }
+}