Merge pull request #28 from binnur/master

Updated intakedown to continue for a pickup
Spartronics4915 · Jun 9, 2018 · 5e6f0a5 · 5e6f0a5
2 parents be1a8f7 + 1f72dcd
commit 5e6f0a5
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 8 deletions.
diff --git 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
@@ -12,8 +12,6 @@
  */
 public class IntakeDownWithWheels extends Command
 {
-    private boolean interruptionFinish = false;
-
     private Harvester mHarvester;
 
     public IntakeDownWithWheels()