diff --git a/src/main/java/frc/robot/subsystems/coraller/Angler.java b/src/main/java/frc/robot/subsystems/coraller/Angler.java index 67903da..32805e3 100644 --- a/src/main/java/frc/robot/subsystems/coraller/Angler.java +++ b/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); } diff --git a/src/main/java/frc/robot/subsystems/coraller/Coraller.java b/src/main/java/frc/robot/subsystems/coraller/Coraller.java index b54597e..e7803e1 100644 --- a/src/main/java/frc/robot/subsystems/coraller/Coraller.java +++ b/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 diff --git a/src/main/java/frc/robot/subsystems/coraller/Elevator.java b/src/main/java/frc/robot/subsystems/coraller/Elevator.java index df2e97b..8d72cb7 100644 --- a/src/main/java/frc/robot/subsystems/coraller/Elevator.java +++ b/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; } } diff --git a/src/main/java/frc/robot/subsystems/coraller/Intake.java b/src/main/java/frc/robot/subsystems/coraller/Intake.java index ac110a6..14cb738 100644 --- a/src/main/java/frc/robot/subsystems/coraller/Intake.java +++ b/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); } - - }