Prepare parallel bug (#12)

* refactoring coraller into composable parts

* refactoring commands

* parallelize the two components

* adding some constants for config

* cleaning up config values

* reordering

* hiding internal classes

* whoops

* cleanup

* fixing parallel contention

* reordering deps

* setting angler setpoint

src/main/java/frc/robot/subsystems/coraller/Angler.java

@@ -11,10 +11,13 @@
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
 import frc.robot.Constants.CorallerConfig;
 import frc.robot.RobotMap;
 
-class Angler {
+class Angler extends SubsystemBase {
   private final SparkMax motor;
   private final AbsoluteEncoder encoder;
   private final SparkClosedLoopController pid;
@@ -41,11 +44,12 @@ public double getAngle() {
     return encoder.getPosition();
   }
 
-  public void setPosition(double pos) {
-    changeSetPoint(pos);
+  public Command setPosition(double pos) {
+    return this.runOnce(() -> changeSetPoint(pos));
   }
 
   private void changeSetPoint(double setpoint) {
+    setPointAngle = setpoint;
     pid.setReference(setpoint, ControlType.kPosition);
   }
 

src/main/java/frc/robot/subsystems/coraller/Coraller.java

@@ -6,28 +6,18 @@
 import frc.robot.Constants.CorallerConfig;
 
 public class Coraller extends SubsystemBase {
-
-  private final Elevator elevator;
-  private final Angler angler;
-  private final Intake intake;
-
-  public Coraller() {
-    elevator = new Elevator(CorallerConfig.kRobotElevatorStowHeightInches);
-    angler = new Angler();
-    intake = new Intake();
-  }
-
-
+  private final Elevator elevator = new Elevator();
+  private final Angler angler = new Angler();
+  private final Intake intake = new Intake();
 
   public Command prepareToScore(Configuration cfg) {
     return Commands.parallel(
-      this.runOnce(() -> elevator.setPosition(cfg.elevatorPosition)),
-      this.runOnce(() -> angler.setPosition(cfg.anglerPosition))
+      elevator.setPosition(cfg.elevatorPosition),
+      angler.setPosition(cfg.anglerPosition)
     );
   }
 
   public Command intakeCoral() {
-
     return this.startEnd(
       intake::start, // can also be written as () -> intake.start()
       intake::stop
@@ -43,7 +33,6 @@ public Command releaseCoral() {
       intake::stop
     )
     .withTimeout(.5);
-
   }
 
   @Override

src/main/java/frc/robot/subsystems/coraller/Elevator.java

@@ -12,20 +12,20 @@
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
 import frc.robot.RobotMap;
 import frc.robot.Constants.CorallerConfig;
 
-class Elevator {
+class Elevator extends SubsystemBase {
   private final SparkMax motor;
   private final RelativeEncoder encoder;
   private final SparkClosedLoopController pid;
 
   private double setPointHeight;
 
-  private double baseHeight;
-
-  public Elevator(double base) {
-
+  public Elevator() {
     var elevatorEncoderConfig = new EncoderConfig()
         .positionConversionFactor(CorallerConfig.kElevatorEncoderPositionConversionFactor)
         .velocityConversionFactor(CorallerConfig.kElevatorEncoderVelocityConversionFactor);
@@ -44,12 +44,14 @@ public Elevator(double base) {
     encoder = motor.getEncoder();
   }
 
+  public Command setPosition(double position) {
+    return this.runOnce(() -> setPositionInternal(setPointHeight));
+  }
+
     // sets height relative to the floor
-  public void setPosition(double position) {
+  private void setPositionInternal(double position) {
     setPointHeight = position;
-
-    double setpoint = position - baseHeight;
-
+    double setpoint = position - CorallerConfig.kRobotElevatorStowHeightInches;
     changeSetPoint(setpoint);
   }
 
@@ -58,9 +60,7 @@ private void changeSetPoint(double setpoint) {
   }
 
   public boolean isElevatorAtBottom() {
-
-    return getHeightFromGround() == baseHeight;
-
+    return getHeightFromGround() == CorallerConfig.kRobotElevatorStowHeightInches;
   }
 
   public double getSetPoint() {
@@ -78,8 +78,6 @@ public double getRelativePosition() {
   }
 
   private double getHeightFromGround() {
-
-    return encoder.getPosition() + baseHeight;
-
+    return encoder.getPosition() + CorallerConfig.kRobotElevatorStowHeightInches;
   }
 }

src/main/java/frc/robot/subsystems/coraller/Intake.java

@@ -22,7 +22,6 @@ public Intake() {
   }
 
   // TODO() find out volts and which are inversed
-
   public void start() {
     intake.setVoltage(6);
   }
@@ -34,6 +33,4 @@ public void reverse() {
   public void stop() {
     intake.setVoltage(0);
   }
-
-
 }