Coral elevator

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

@@ -4,16 +4,18 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.subsystems.AlgaeArm;
 import frc.robot.subsystems.AlgaeGripper;
+import frc.robot.subsystems.CoralElevator;
 
 public class Robot extends TimedRobot {
     private AlgaeArm algaeArm;
     private AlgaeGripper algaeGripper;
+    private CoralElevator coralElevator;
 
     @Override
     public void robotInit() {
         algaeArm = new AlgaeArm();
         algaeGripper = new AlgaeGripper();
-
+        coralElevator = new CoralElevator();
     }
 
     @Override

src/main/java/frc/robot/RobotMap.java

@@ -1,16 +1,23 @@
 package frc.robot;
 
 public class RobotMap {
-
     private RobotMap() {}
+
+    // coral elevator
+    public static final int CORAL_ELEVATOR_UPPER_LIMIT_SWITCH = 1;
+    public static final int CORAL_ELEVATOR_LOWER_LIMIT_SWITCH = 2;
+    public static final int CORAL_ELEVATOR_PISTON_FORWARD_CHANNEL = 1;
+    public static final int CORAL_ELEVATOR_PISTON_REVERSE_CHANNEL = 1;
+
+    // algae gripper
+    public static final int ALGAE_GRIPPER_MOTOR = 0;
     public static final int ALGAE_GRIPPER_DIGITALINPUT = 0;
+
+    // algae arm
     public static final int ALGAE_ARM_SWITCH_TOP = 0;
     public static final int ALGAE_ARM_SWITCH_BOTTOM = 0;
-    public static final int ALGAE_GRIPPER_MOTOR = 0;
     public static final int ALGEA_ARM_FORWARD_PISTON_FORWARD_CHANNEL = 0;
     public static final int ALGEA_ARM_FORWARD_PISTON_REVERSE_CHANI = 0;
     public static final int ALGEA_ARM_BACKWARD_PISTON_FORWARD_CHANNEL = 0;
     public static final int ALGEA_ARM_BACKWARD_PISTON_REVERSE_CHANI = 0;
-    // add constants here
-    // public static final type NAME = value;
 }

src/main/java/frc/robot/commands/LowerCoralElevator.java

@@ -0,0 +1,35 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.CoralElevator;
+
+public class LowerCoralElevator extends Command {
+
+    private final CoralElevator coralElevator;
+
+    public LowerCoralElevator(CoralElevator coralElevator) {
+        this.coralElevator = coralElevator;
+        addRequirements(coralElevator);
+    }
+
+    @Override
+    public void initialize() {
+        coralElevator.lower();
+    }
+
+    @Override
+    public void execute() {
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return coralElevator.isLowered();
+    }
+
+
+}

src/main/java/frc/robot/commands/RaiseCoralElevator.java

@@ -0,0 +1,34 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.CoralElevator;
+
+public class RaiseCoralElevator extends Command {
+
+    private final CoralElevator coralElevator;
+
+    public RaiseCoralElevator(CoralElevator coralElevator) {
+        this.coralElevator = coralElevator;
+
+        addRequirements(coralElevator);
+    }
+
+    @Override
+    public void initialize() {
+        coralElevator.raise();
+    }
+
+    @Override
+    public void execute() {
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    @Override
+    public boolean isFinished() {
+        return coralElevator.isRaised();
+    }
+
+}

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

@@ -0,0 +1,53 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.RobotMap;
+
+public class CoralElevator extends SubsystemBase {
+
+    private final DoubleSolenoid piston1;
+    private final DoubleSolenoid piston2;
+    private final DigitalInput UpperLimitSwitch;
+    private final DigitalInput LowerLimitSwitch;
+
+    public CoralElevator() {
+        piston1 = new DoubleSolenoid(PneumaticsModuleType.REVPH, RobotMap.CORAL_ELEVATOR_PISTON_FORWARD_CHANNEL, RobotMap.CORAL_ELEVATOR_PISTON_REVERSE_CHANNEL);
+        piston2 = new DoubleSolenoid(PneumaticsModuleType.REVPH, RobotMap.CORAL_ELEVATOR_PISTON_FORWARD_CHANNEL, RobotMap.CORAL_ELEVATOR_PISTON_REVERSE_CHANNEL);
+        UpperLimitSwitch = new DigitalInput(RobotMap.CORAL_ELEVATOR_UPPER_LIMIT_SWITCH);
+        LowerLimitSwitch = new DigitalInput(RobotMap.CORAL_ELEVATOR_LOWER_LIMIT_SWITCH);
+    }
+
+    public boolean isRaised() {
+        return !UpperLimitSwitch.get();
+    }
+
+    public boolean isLowered() {
+        return !LowerLimitSwitch.get();
+    }
+
+    public void raise() {
+        piston1.set(DoubleSolenoid.Value.kForward);
+        piston2.set(DoubleSolenoid.Value.kForward);
+    }
+
+    public void lower() {
+        piston1.set(DoubleSolenoid.Value.kReverse);
+        piston2.set(DoubleSolenoid.Value.kReverse);
+    }
+
+    public void stop() {
+        piston1.set(DoubleSolenoid.Value.kOff);
+        piston2.set(DoubleSolenoid.Value.kOff);
+    }
+
+
+    @Override
+    public void periodic() {
+        SmartDashboard.putBoolean("Elevator Raised: " , isRaised());
+        SmartDashboard.putBoolean("Elevator Lowered: " , isLowered());
+    }
+}