change name of LowerChevalDeFrise to PassChevalDeFrise and add pre and post condition...

[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) {

    switch (defense) {

    case PORTCULLIS:

      addSequential(new LiftPortcullis());

    case SALLY_PORT:

      addSequential(new PassSallyPort());

    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 PassChevalDeFrise());

    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;
    }

    // TODO: put in the argument that goes inside of aim, based on the position
    // variable that was passed into the command group
    addSequential(new Aim());
    addSequential(new Shoot());

  }

}