delete anything to do with pots and motors in IntakeArm and Constants, add the left...

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

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

import org.usfirst.frc.team3501.robot.Robot;
import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;

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

/**
 * This command group uses the intake arm to push down the moving parts of the
 * cheval de frise so that the robot can drive over it
 *
 * pre-condition: the robot is flush against the border of the outerworks (flush
 * against the ramp)
 *
 * post condition: intake arm has pushed the cheval de frise down
 *
 */
public class PushDownChevalDeFrise extends CommandGroup {
  // distances are in inches
  private final double MIN_ANGLE = Robot.intakeArm.potAngles[2]; // TODO: find
                                                                 // min angle
                                                                 // the intake
                                                                 // arm has
  // to be to be over the cheval de frise
  private final double RAMP_DIST = 12;
  private final double MAX_TIMEOUT_RAMP = 1; // TODO: check that one second is
                                             // enough
  private final double LOWEST_ANGLE = Robot.intakeArm.potAngles[0];
  private final double DEFAULT_ARM_SPEED = 0.4;

  public PushDownChevalDeFrise() {
    requires(Robot.intakeArm);
    /**
     * if the arm is high enough - above cheval de frise height (find this from
     * the potangle)
     *
     * go forward until arm is over cheval de frise
     *
     * move arm down to floor level -- or as much as possible
     * -------------------------------------------------------------------------
     *
     * if the arm is beneath the cheval de frise
     *
     * move the arm up so it's high enough
     *
     * move forward a little
     *
     * move arm down
     **/

    if (Robot.intakeArm.getArmAngle() > MIN_ANGLE) {
      addSequential(new DriveDistance(RAMP_DIST, MAX_TIMEOUT_RAMP));
      addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE, DEFAULT_ARM_SPEED));

    } else {
      addSequential(new MoveIntakeArmToAngle(MIN_ANGLE + 3, DEFAULT_ARM_SPEED)); // TODO:
      // check that adding
      // 3 is correct

      addSequential(new DriveDistance(RAMP_DIST, MAX_TIMEOUT_RAMP));
      addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE, DEFAULT_ARM_SPEED));

    }

  }
}