diff --git a/src/main/java/com/spartronics4915/atlas/commands/IntakeDown.java b/src/main/java/com/spartronics4915/atlas/commands/IntakeDown.java index 66e78e0..301143a 100644 --- a/src/main/java/com/spartronics4915/atlas/commands/IntakeDown.java +++ b/src/main/java/com/spartronics4915/atlas/commands/IntakeDown.java @@ -1,6 +1,7 @@ package com.spartronics4915.atlas.commands; import com.spartronics4915.atlas.Logger; +import com.spartronics4915.atlas.Robot; import com.spartronics4915.atlas.RobotMap; import com.spartronics4915.atlas.subsystems.Harvester; import com.spartronics4915.atlas.subsystems.Launcher; @@ -16,6 +17,8 @@ public class IntakeDown extends Command private Harvester mHarvester; private Launcher mLauncher; + private boolean shouldStop = true; + public IntakeDown() { mHarvester = Harvester.getInstance(); @@ -26,32 +29,70 @@ public IntakeDown() @Override protected void initialize() { - // no need to check the harvester position -- just extend the intake - mHarvester.extendPneumatics(); + if (mLauncher.isBallPresent()) + { + shouldStop = true; + } + else + { + shouldStop = false; + } } @Override protected void execute() { - mHarvester.setWheelSpeed(0.0); - mLauncher.stopLauncherWindingMotor(); + + // Based on ball presence, run wheels continously or not w/ diff speeds + if (shouldStop) + { + mHarvester.setWheelSpeed(RobotMap.kHarvesterIntakeWheelSpeed / 2); + } + else + { + mHarvester.setWheelSpeed(RobotMap.kHarvesterIntakeWheelSpeed); + } + + // no need to check the harvester position -- just extend the intake + mHarvester.extendPneumatics(); } @Override protected boolean isFinished() { - return mHarvester.isHarvesterDown(); + if (shouldStop && mHarvester.isHarvesterDown()) + { + mHarvester.setWheelSpeed(0.0); + return true; + } + else if (mLauncher.isBallPresent()) + { + // return true only if intake successfully picked up the ball + Logger.info("Command: IntakeDown picked up a ball -- time to stop"); + mHarvester.setWheelSpeed(0.0); + return true; + } + + Logger.info("Command: IntakeDown is should NOT finish!"); + return false; } @Override protected void end() { - mHarvester.setWheelSpeed(0.0); + Logger.info("Command: IntakeDown is ended"); + if (mLauncher.isBallPresent()) + { + // stop wheels only if the intake successfully picked up the ball + Logger.info("Command: IntakeDown picked up a ball -- time to stop"); + mHarvester.setWheelSpeed(0.0); + } } @Override protected void interrupted() { + Logger.info("Command: IntakeDown is interrupted"); end(); } } diff --git a/src/main/java/com/spartronics4915/atlas/commands/IntakeDownWithWheels.java b/src/main/java/com/spartronics4915/atlas/commands/IntakeDownWithWheels.java index a7e3dff..9a56203 100644 --- a/src/main/java/com/spartronics4915/atlas/commands/IntakeDownWithWheels.java +++ b/src/main/java/com/spartronics4915/atlas/commands/IntakeDownWithWheels.java @@ -12,8 +12,6 @@ */ public class IntakeDownWithWheels extends Command { - private boolean interruptionFinish = false; - private Harvester mHarvester; public IntakeDownWithWheels()