challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/commands/intakearm/PushDownChevalDeFrise.java

   1 package org.usfirst.frc.team3501.robot.commands.intakearm;
   2
   3 import org.usfirst.frc.team3501.robot.Robot;
   4 import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
   5
   6 import edu.wpi.first.wpilibj.command.CommandGroup;
   7
   8 /**
   9  * This command group uses the intake arm to push down the moving parts of the
  10  * cheval de frise so that the robot can drive over it
  11  *
  12  * pre-condition: the robot is flush against the border of the outerworks (flush
  13  * against the ramp)
  14  *
  15  * post condition: intake arm has pushed the cheval de frise down
  16  *
  17  */
  18 public class PushDownChevalDeFrise extends CommandGroup {
  19   // distances are in inches
  20   private final double MIN_ANGLE = 0; // TODO: find min angle the intake arm has
  21                                       // to be to be over the cheval de frise
  22   private final double RAMP_DIST = 12;
  23   private final double MAX_TIMEOUT_RAMP = 1; // TODO: check that one second is
  24                                              // enough
  25   private final double LOWEST_ANGLE = 0; // TODO: find angle
  26   private final double DEFAULT_ARM_SPEED = 0.4;
  27
  28   public PushDownChevalDeFrise() {
  29     requires(Robot.intakeArm);
  30     /**
  31      * if the arm is high enough - above cheval de frise height (find this from
  32      * the potangle)
  33      *
  34      * go forward until arm is over cheval de frise
  35      *
  36      * move arm down to floor level -- or as much as possible
  37      * -------------------------------------------------------------------------
  38      *
  39      * if the arm is beneath the cheval de frise
  40      *
  41      * move the arm up so it's high enough
  42      *
  43      * move forward a little
  44      *
  45      * move arm down
  46      */
  47
  48     if (Robot.intakeArm.getArmAngle() > MIN_ANGLE) {
  49       addSequential(new DriveDistance(RAMP_DIST, MAX_TIMEOUT_RAMP));
  50       addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE, DEFAULT_ARM_SPEED));
  51
  52     } else {
  53       addSequential(new MoveIntakeArmToAngle(MIN_ANGLE + 3, DEFAULT_ARM_SPEED)); // TODO:
  54       // check that adding
  55       // 3 is correct
  56
  57       addSequential(new DriveDistance(RAMP_DIST, MAX_TIMEOUT_RAMP));
  58       addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE, DEFAULT_ARM_SPEED));
  59
  60     }
  61
  62   }
  63 }