add arm methods getCurrentLevel() and updateLimitStates()

[3501/2015-FRC-Spark] / src / org / usfirst / frc3501 / RiceCatRobot / subsystems / Arm.java

package org.usfirst.frc3501.RiceCatRobot.subsystems;

import org.usfirst.frc3501.RiceCatRobot.RobotMap;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.command.Subsystem;

public class Arm extends Subsystem {
  private CANJaguar left, right;
  private DigitalInput[] limits;
  private int currentLevel;

  public Arm() {
    left = new CANJaguar(RobotMap.ARM_LEFT);
    right = new CANJaguar(RobotMap.ARM_RIGHT);
    limits = new DigitalInput[RobotMap.NUM_LIMITS];
    currentLevel = 0;
  }

  public void initDefaultCommand() {
  }
  
  public void initializeArrayValues() {
	  for (int i = 0; i < RobotMap.NUM_LIMITS; i++) {
		  limits[i] = new DigitalInput(i);
	  }
  }

  public void fineTuneControl(double d) {
    if (Math.abs(d) < 0.05) {
      d = 0;
    } else if (d > 0) {
      d *= d;
    } else {
      d *= -d;
    }
    setArmSpeeds(d);
  }

  public void setLeft(double speed) {
    left.set(-speed);
  }

  public void setRight(double speed) {
    right.set(-speed);
  }

  public void setArmSpeeds(double speed) {
    setLeft(speed);
    setRight(speed);
  }

  public void stop() {
    left.set(0);
    right.set(0);
  }
  
  public void updateLimitStates() {
	  for (int i = 0; i < RobotMap.NUM_LIMITS; i++) {
		  if (limits[i].get() && i > currentLevel) currentLevel++;
		  if (limits[i].get() && i <= currentLevel) currentLevel--;
	  }
  }
  
  public int getCurrentLevel() {
	  return currentLevel;
  }
}