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 = Robot.intakeArm.potAngles[2]; // TODO: find
  21                                                                  // min angle
  22                                                                  // the intake
  23                                                                  // arm has
  24   // to be to be over the cheval de frise
  25   private final double RAMP_DIST = 12;
  26   private final double MAX_TIMEOUT_RAMP = 1; // TODO: check that one second is
  27                                              // enough
  28   private final double LOWEST_ANGLE = Robot.intakeArm.potAngles[0];
  29   private final double DEFAULT_ARM_SPEED = 0.4;
  30
  31   public PushDownChevalDeFrise() {
  32     requires(Robot.intakeArm);
  33     /**
  34      * if the arm is high enough - above cheval de frise height (find this from
  35      * the potangle)
  36      *
  37      * go forward until arm is over cheval de frise
  38      *
  39      * move arm down to floor level -- or as much as possible
  40      * -------------------------------------------------------------------------
  41      *
  42      * if the arm is beneath the cheval de frise
  43      *
  44      * move the arm up so it's high enough
  45      *
  46      * move forward a little
  47      *
  48      * move arm down
  49      **/
  50
  51     if (Robot.intakeArm.getArmAngle() > MIN_ANGLE) {
  52       addSequential(new DriveDistance(RAMP_DIST, MAX_TIMEOUT_RAMP));
  53       addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE, DEFAULT_ARM_SPEED));
  54
  55     } else {
  56       addSequential(new MoveIntakeArmToAngle(MIN_ANGLE + 3, DEFAULT_ARM_SPEED)); // TODO:
  57       // check that adding
  58       // 3 is correct
  59
  60       addSequential(new DriveDistance(RAMP_DIST, MAX_TIMEOUT_RAMP));
  61       addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE, DEFAULT_ARM_SPEED));
  62
  63     }
  64
  65   }
  66 }