challenge-bot.com Git - 3501/2015-FRC-Spark/blob - src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java

   1         package org.usfirst.frc3501.RiceCatRobot.subsystems;
   2
   3 import java.util.ArrayList;
   4
   5 import org.usfirst.frc3501.RiceCatRobot.RobotMap;
   6
   7 import edu.wpi.first.wpilibj.CANJaguar;
   8 import edu.wpi.first.wpilibj.DigitalInput;
   9 import edu.wpi.first.wpilibj.command.Subsystem;
  10
  11 public class Arm extends Subsystem {
  12   private CANJaguar left, right;
  13   private ArrayList<DigitalInput> limitSwitches;
  14   //channel index numbers correspond to limit switches
  15   private int[] channels;
  16   private int currentLevel;
  17   private final int NUM_OF_SWITCHES = 4;
  18
  19   public Arm() {
  20     left = new CANJaguar(RobotMap.ARM_LEFT);
  21     right = new CANJaguar(RobotMap.ARM_RIGHT);
  22
  23     //channels are not known, these are random numbers
  24     channels = new int[]{0, 1, 2, 3};
  25
  26     limitSwitches = new ArrayList<DigitalInput>();
  27     //loop
  28     for(int i = 0; i < NUM_OF_SWITCHES; i ++){
  29         DigitalInput d = limitSwitches.get(i);
  30         d = new DigitalInput(channels[i]);
  31
  32
  33     }
  34
  35     channels = new int[NUM_OF_SWITCHES];
  36
  37
  38   }
  39
  40   public void initDefaultCommand() {
  41   }
  42
  43   public void fineTuneControl(double d) {
  44     if (Math.abs(d) < 0.05) {
  45       d = 0;
  46     } else if (d > 0) {
  47       d *= d;
  48     } else {
  49       d *= -d;
  50     }
  51     setArmSpeeds(d);
  52   }
  53
  54   public void setLeft(double speed) {
  55     left.set(-speed);
  56   }
  57
  58   public void setRight(double speed) {
  59     right.set(-speed);
  60   }
  61
  62   public void setArmSpeeds(double speed) {
  63     setLeft(speed);
  64     setRight(speed);
  65   }
  66
  67   public void stop() {
  68     left.set(0);
  69     right.set(0);
  70   }
  71
  72   //should run in robot.java teleop periodic (autonomous periodic too maybe?)
  73   public void updateLimitSwitches(){
  74           for(int i = 0; i < NUM_OF_SWITCHES; i ++){
  75                   DigitalInput d = limitSwitches.get(i);
  76                   if(d.get()){
  77                           currentLevel = i;
  78                   }
  79           }
  80   }
  81
  82   public int getLevel(){
  83           //return last past switch
  84           return 0;
  85   }
  86 }