[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 limit1, limit2, limit3, limit4;

  public Arm() {
    left = new CANJaguar(RobotMap.ARM_LEFT);
    right = new CANJaguar(RobotMap.ARM_RIGHT);
    limit1 = new DigitalInput(RobotMap.ARM_LIMIT_1);
    limit2 = new DigitalInput(RobotMap.ARM_LIMIT_2);
    limit3 = new DigitalInput(RobotMap.ARM_LIMIT_3);
    limit4 = new DigitalInput(RobotMap.ARM_LIMIT_4);
  }

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

  public void stop() {
    left.set(0);
    right.set(0);
  }
}