add command to robot.java to actually run the LimitSwitchTest

[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.Counter;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.command.Subsystem;

public class Arm extends Subsystem {
	private CANJaguar left, right;
	double speedOfArm = 0.0;

	DigitalInput[] limitSwitches = new DigitalInput[3];
	Counter[] counters = new Counter[3];

	public Arm() {
		left = new CANJaguar(RobotMap.ARM_LEFT);
		right = new CANJaguar(RobotMap.ARM_RIGHT);
		
		setDigitalInputValues();
		setCountersCorrespondingToSwitches();
	}

	public void setDigitalInputValues() {
		limitSwitches[0] = new DigitalInput(0);
		limitSwitches[1] = new DigitalInput(1);
		limitSwitches[2] = new DigitalInput(2);
	}

	public void setCountersCorrespondingToSwitches() {
		counters[0] = new Counter(0);
		counters[1] = new Counter(1);
		counters[3] = new Counter(2);
	}

	public double getArmSpeed() {
	  return this.speedOfArm;
  }

	public boolean isSwitch1Hit() {
		return counters[0].get() > 0;
	}

	public boolean isSwitch2Hit() {
		return counters[1].get() > 0;
	}

	public boolean isSwitch3Hit() {
		return counters[2].get() > 0;
	}

	public void initializeCounters() {
		for (int i = 0; i < counters.length; i++) {
			counters[i].reset();
		}
	}

	public void initDefaultCommand() {
	}

	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) {
		this.speedOfArm = speed;
		setLeft(speed);
		setRight(speed);
	}

	public void stop() {
		this.speedOfArm = 0.0;
		left.set(0);
		right.set(0);
	}
}