diff --git a/src/main/java/frc/robot/subsystems/coraller/Coraller.java b/src/main/java/frc/robot/subsystems/coraller/Coraller.java index b366a93..fa1813e 100644 --- a/src/main/java/frc/robot/subsystems/coraller/Coraller.java +++ b/src/main/java/frc/robot/subsystems/coraller/Coraller.java @@ -24,11 +24,21 @@ public Command prepareToScore(Configuration cfg) { } public Command intakeCoral() { - return this.runOnce(() -> intake.intake()); + return this.startEnd( + intake::start, // can also be written as () -> intake.start() + intake::stop + ) + // TODO: We need a sensor to tell us if we have coral + .until(() -> { return false; }) + .withTimeout(3); } public Command releaseCoral() { - return this.runOnce(() -> intake.release()); + return this.startEnd( + intake::reverse, + intake::stop + ) + .withTimeout(.5); } @Override diff --git a/src/main/java/frc/robot/subsystems/coraller/Intake.java b/src/main/java/frc/robot/subsystems/coraller/Intake.java index f55c86a..d8055e0 100644 --- a/src/main/java/frc/robot/subsystems/coraller/Intake.java +++ b/src/main/java/frc/robot/subsystems/coraller/Intake.java @@ -22,11 +22,16 @@ public Intake() { } // TODO() find out volts and which are inversed - public void intake() { + public void start() { intake.setVoltage(6); } - public void release() { + public void reverse() { intake.setVoltage(-6); } + + public void stop() { + intake.setVoltage(0); + } + }