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

        package org.usfirst.frc3501.RiceCatRobot.subsystems;

import java.util.ArrayList;

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;
  ArrayList<DigitalInput> limitSwitches;
  //channel index numbers correspond to limit switches
  int[] channels;
  private final int NUM_OF_SWITCHES = 4;

  public Arm() {
    left = new CANJaguar(RobotMap.ARM_LEFT);
    right = new CANJaguar(RobotMap.ARM_RIGHT);
    
    //channels are not known, these are random numbers 
    channels = new int[]{0, 1, 2, 3};
   
    limitSwitches = new ArrayList<DigitalInput>();
    //loop
    for(int i = 0; i < NUM_OF_SWITCHES; i ++){
        DigitalInput d = limitSwitches.get(i);
        d = new DigitalInput(channels[i]);
        
        
    }
    
    channels = new int[NUM_OF_SWITCHES];
    
    
  }

  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);
  }
  
  public int getLevel(){
          //return last past switch 
          return 0;
  }
}