add pre and post conditions for the LiftPortcullis command

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / commands / DefaultAutonStrategy.java

package org.usfirst.frc.team3501.robot.commands;

import org.usfirst.frc.team3501.robot.Constants.Defense;

import edu.wpi.first.wpilibj.command.CommandGroup;

/**
 * The default autonomous strategy involves passing the defense that is in front
 * of it, aiming the robot/ shooter towards the goal, and shooting.
 */
public class DefaultAutonStrategy extends CommandGroup {

  // in feet
  // distance along a platform
  final double DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD = 4.0;
  // number from -1 to 1
  final double SPEED_FOR_PASSING_DEFENSE = 0.5;

  public DefaultAutonStrategy(int position, Defense defense) {

    // TODO: any variable that is not declared/instantiated are variables that
    // need
    // to be tested for their value

    /*
     * Portcullis: Robot is in front of the portcullis in whatever position 1-5.
     * addSequential(new LiftPortcullis()); lifting the portcullis.
     * addSequential(new DriveForDistance(distance, speed)); drive through the
     * portcullis however neccesary. change direction to drive towards the
     * designated shooting spot, go to that spot. shoot
     */

    /*
     * Sally Port: Robot is in front of the sally port in whatever position 1-5.
     * Do whatever is needed in order to cross sally port. addSequential(new
     * DriveForDistance(distance, speed));//drive through the sally port. change
     * direction to drive towards the designated shooting spot, go to that spot.
     * shoot
     */

    /*
     * Rough Terrain: Robot is in front of the sally port in whatever postion
     * 1-5. Do whatever adaptive technique is required to pass rough terrain
     * addSequential(new DriveForDistance(distance, speed)); go through the
     * rough terrain change direction to drive towards the designated shooting
     * spot, go to that spot. shoot
     */

    /*
     * Low Bar: Robot is in front of low bar in whatever position 1-5.
     * addSequential(new DriveForDistance(distance, speed));//drive through the
     * low bar change direction to drive towards the designated shooting spot,
     * go to that spot. shoot
     */

    /*
     * Cheval de Frise: Robot is in front of cheval de frise in whatever
     * position 1-5. do whatever to make all pieces facing down toward robot
     * addSequential(new DriveForDistance(distance, speed));//drive to pass
     * cheval de frise change direction to drive towards the designated shooting
     * spot, go to that spot. shoot
     */

    /*
     * Drawbridge: Robot is in front of drawbridge in whatever position 1-5.
     * lift the drawbridge in whatever way it is to be lifted. addSequential(new
     * DriveForDistance(distance, speed));//pass the drawbridge change direction
     * to drive towards the designated shooting spot, go to that spot. shoot
     */

    /*
     * Moat: Robot is in front of moat in whatever position 1-5. do whatever is
     * adaptable to cross the moat. addSequential(new DriveForDistance(distance,
     * speed));//cross the moat change direction to drive towards the designated
     * shooting spot, go to that spot. shoot
     */

    /*
     * Rockwall: Robot is in front of moat in whatever position 1-5. do whatever
     * function is neccesary to cross the rock wall. addSequential(new
     * DriveForDistance(distance, speed));//cross the rockwall change direction
     * to drive towards the designated shooting spot, go to that spot. shoot
     */

    /*
     * Rampart: Robot is in front of moat in whatever position 1-5. do whatever
     * function is neccesary to cross the rockwall addSequential(new
     * DriveForDistance(distance, speed));//cross the rockwall change direction
     * to drive towards the designated shooting spot, go to that spot. shoot
     */

    switch (defense) {

    case PORTCULLIS:

      addSequential(new LiftPortcullis());

    case SALLY_PORT:

      addSequential(
          new DriveForDistance(DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD + 2,
              DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD + 2));

    case ROUGH_TERRAIN:

      addSequential(
          new DriveForDistance(DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD + 2,
              DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD + 2));

    case LOW_BAR:

      addSequential(
          new DriveForDistance(DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD + 2,
              SPEED_FOR_PASSING_DEFENSE));

    case CHEVAL_DE_FRISE:

      addSequential(new LowerChevalDeFrise());

    case DRAWBRIDGE:

      addSequential(new LowerDrawBridge());

    case MOAT:

      addSequential(new DriveForDistance(
          DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD, SPEED_FOR_PASSING_DEFENSE));

    case ROCK_WALL:

      addSequential(new DriveForDistance(
          DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD, SPEED_FOR_PASSING_DEFENSE));

    case RAMPART:

      addSequential(new DriveForDistance(
          DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD, SPEED_FOR_PASSING_DEFENSE));

    default:
      break;
    }

    // aim towards goal
    // later put in whatever argument goes inside of aim based on the position
    // variable that was passed onto the command
    // group
    addSequential(new Aim());
    addSequential(new Shoot());

  }

}