implement ShootAtHighGoal and add some helper methods

diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/ResetCatapult.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/ResetCatapult.java
index ecdfce6ebf762120c56b4927d3a62337fadd937b..3c46bf4dfcf757e1e651bdf7a665d49822e6a458 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/shooter/ResetCatapult.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/ResetCatapult.java
@@ -1,5 +1,6 @@
 package org.usfirst.frc.team3501.robot.commands.shooter;
 
+import org.usfirst.frc.team3501.robot.Constants;
 import org.usfirst.frc.team3501.robot.Robot;
 
 import edu.wpi.first.wpilibj.command.Command;
@@ -12,7 +13,9 @@ public class ResetCatapult extends Command {
 
   @Override
   protected void initialize() {
-    Robot.shooter.resetCatapult();
+    if (Robot.shooter.getCatapult1State() == Constants.Shooter.SHOOT
+        || Robot.shooter.getCatapult2State() == Constants.Shooter.SHOOT)
+      Robot.shooter.resetCatapult();
   }
 
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/ShootAtHighGoal.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/ShootAtHighGoal.java
index 5ecc8091a64de82af8b18863ca9cdc729840c25c..54e4d94fc43faa600b93655f86b15b822cdb303f 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/shooter/ShootAtHighGoal.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/ShootAtHighGoal.java
@@ -1,29 +1,35 @@
 package org.usfirst.frc.team3501.robot.commands.shooter;
 
+import org.usfirst.frc.team3501.robot.Constants.IntakeArm;
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.commands.intakearm.MoveIntakeArm;
+
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 /**
  * This command group performs the sequence of steps to shoot at the high goal
  *
- * pre-conditions: the catapult is down, a ball is in the intake, and the intake
- * is in the up position
+ * pre-conditions: a ball is in the intake, and the intake is in the up position
  *
  * post-conditions: catapult is retracted, intake is extended
  */
 public class ShootAtHighGoal extends CommandGroup {
 
   public ShootAtHighGoal() {
-    // check that the catapult is down and change accordingly
-
     // (if photogate) check if ball is in intake
 
     // make sure intake is in up position and change accordingly
+    if (!Robot.intakeArm.isExtended())
+      addSequential(new MoveIntakeArm(IntakeArm.RETRACT));
 
     // shoot catapult pistons
+    addSequential(new FireCatapult());
 
     // extend intake (ball actually shoots here)
+    addSequential(new MoveIntakeArm(IntakeArm.EXTEND));
 
     // retract catapult pistons
+    addSequential(new ResetCatapult());
 
   }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
index c9c29dd27ec45e88047e5c97697df44aa92070aa..cbd0f3ca1e83e9ea8694f63544c102fdfb8d5d22 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
@@ -9,13 +9,13 @@ import edu.wpi.first.wpilibj.command.Subsystem;
 /***
  * The IntakeArm consists of two rollers that are controlled by one motor, with
  * a potentiometer on it.
- * 
+ *
  * The motor controls the rollers, making them roll forwards and backwards. The
  * Intake rollers are on the back of the robot. As the rollers run, they intake
  * the ball.
- * 
+ *
  * @author superuser
- * 
+ *
  */
 
 public class IntakeArm extends Subsystem {
@@ -46,6 +46,11 @@ public class IntakeArm extends Subsystem {
     rightIntake.set(Constants.IntakeArm.EXTEND);
   }
 
+  public boolean isExtended() {
+    return (leftIntake.get() == Constants.IntakeArm.EXTEND
+        && rightIntake.get() == Constants.IntakeArm.EXTEND);
+  }
+
   /***
    * This method sets the voltage of the motor to intake the ball. The voltage
    * values are constants in Constants class
@@ -70,7 +75,7 @@ public class IntakeArm extends Subsystem {
    * This method gets you the current voltage of the motor that controls the
    * intake arm roller. The range of voltage is from [-1,1]. A negative voltage
    * makes the motor run backwards.
-   * 
+   *
    * @return Returns the voltage of the motor that controls the roller. The
    *         range of the voltage goes from [-1,1]. A negative voltage indicates
    *         that the motor is running backwards.
@@ -83,7 +88,7 @@ public class IntakeArm extends Subsystem {
   /***
    * This method checks to see if the presence of the ball inside is true or
    * false.
-   * 
+   *
    * @return Returns whether the ball is inside as true or false
    */
 
@@ -94,10 +99,10 @@ public class IntakeArm extends Subsystem {
   /***
    * This method checks to see if the motors controlling the rollers are
    * currently running.
-   * 
+   *
    * @return Returns whether the motors are currently running, and returns the
    *         state of the condition (true or false).
-   * 
+   *
    */
 
   public boolean areRollersRolling() {

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
index 95d69af9358eb7f59041255c0a993272400ddb38..c3500e2ba27b644e309823556e7a45cfbdbddd1b 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
@@ -3,6 +3,7 @@ package org.usfirst.frc.team3501.robot.subsystems;
 import org.usfirst.frc.team3501.robot.Constants;
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
 import edu.wpi.first.wpilibj.command.Subsystem;
 
 /***
@@ -10,9 +11,9 @@ import edu.wpi.first.wpilibj.command.Subsystem;
  * motors. The piston controlling the platform pushes the ball onto the wheel.
  * The wheel is controlled by a motor, which is running before the ball is
  * pushed onto the wheel. The spinning wheel propels the ball.
- * 
+ *
  * @author superuser
- * 
+ *
  */
 
 public class Shooter extends Subsystem {
@@ -38,6 +39,14 @@ public class Shooter extends Subsystem {
     catapult2.set(Constants.Shooter.RESET);
   }
 
+  public Value getCatapult1State() {
+    return catapult1.get();
+  }
+
+  public Value getCatapult2State() {
+    return catapult2.get();
+  }
+
   @Override
   protected void initDefaultCommand() {
   }