challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java

   1 package org.usfirst.frc.team3501.robot.subsystems;
   2
   3 import org.usfirst.frc.team3501.robot.Constants;
   4
   5 import edu.wpi.first.wpilibj.AnalogPotentiometer;
   6 import edu.wpi.first.wpilibj.CANTalon;
   7 import edu.wpi.first.wpilibj.command.Subsystem;
   8
   9 public class DefenseArm extends Subsystem {
  10   private AnalogPotentiometer defenseArmPotentiometer;
  11   private AnalogPotentiometer defenseHandPotentiometer;
  12   private CANTalon defenseArm;
  13   private CANTalon defenseHand;
  14   private double hookHeight;
  15   private double footHeight;
  16   private double[] potAngles;
  17   // angles corresponding to pre-determined heights we will need
  18
  19   public DefenseArm() {
  20     defenseArmPotentiometer = new AnalogPotentiometer(
  21         Constants.DefenseArm.ARM_CHANNEL,
  22         Constants.DefenseArm.FULL_RANGE,
  23         Constants.DefenseArm.OFFSET);
  24     defenseHandPotentiometer = new AnalogPotentiometer(
  25         Constants.DefenseArm.HAND_CHANNEL,
  26         Constants.DefenseArm.FULL_RANGE,
  27         Constants.DefenseArm.OFFSET);
  28
  29     defenseArm = new CANTalon(Constants.DefenseArm.ARM_PORT);
  30     defenseHand = new CANTalon(Constants.DefenseArm.HAND_PORT);
  31     potAngles = createPotArray();
  32   }
  33
  34   public double getArmPotAngle() {
  35     return defenseArmPotentiometer.get();
  36   }
  37
  38   public double getHandPotAngle() {
  39     return defenseHandPotentiometer.get();
  40   }
  41
  42   public double getDistance(int desiredArmLocation) {
  43     return potAngles[desiredArmLocation];
  44   }
  45
  46   public double[] createPotArray() {
  47     double[] arr = new double[3];
  48
  49     for (int i = 0; i < 3; i++) {
  50       arr[i] = 45 * i;
  51     }
  52     return arr;
  53   }
  54
  55   /***
  56    * This method sets the voltage of the arm motor. The range is from [-1,1]. A
  57    * negative voltage makes the direction of the motor go backwards.
  58    *
  59    * @param speed
  60    *          The voltage that you set the motor at. The range of the voltage of
  61    *          the arm motor is from [-1,1]. A
  62    *          negative voltage makes the direction of the motor go backwards.
  63    */
  64
  65   public void setArmSpeed(double speed) {
  66     if (speed > 1)
  67       speed = 1;
  68     else if (speed < -1)
  69       speed = -1;
  70
  71     defenseArm.set(speed);
  72   }
  73
  74   /***
  75    * This method sets the voltage of the hand motor. The range is from [-1,1]. A
  76    * negative voltage makes the direction of the motor go backwards.
  77    *
  78    * @param speed
  79    *          The voltage that you set the motor at. The range of the voltage of
  80    *          the hand motor is from [-1,1]. A
  81    *          negative voltage makes the direction of the motor go backwards.
  82    */
  83
  84   public void setHandSpeed(double speed) {
  85     if (speed > 1)
  86       speed = 1;
  87     else if (speed < -1)
  88       speed = -1;
  89
  90     defenseHand.set(speed);
  91   }
  92
  93   @Override
  94   protected void initDefaultCommand() {
  95   }
  96 }