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[] potHandAngles;
  17   private double[] potArmAngles;
  18   // angles corresponding to pre-determined heights we will need
  19
  20   public DefenseArm() {
  21     defenseArmPotentiometer = new AnalogPotentiometer(
  22         Constants.DefenseArm.ARM_CHANNEL,
  23         Constants.DefenseArm.FULL_RANGE,
  24         Constants.DefenseArm.OFFSET);
  25     defenseHandPotentiometer = new AnalogPotentiometer(
  26         Constants.DefenseArm.HAND_CHANNEL,
  27         Constants.DefenseArm.FULL_RANGE,
  28         Constants.DefenseArm.OFFSET);
  29
  30     defenseArm = new CANTalon(Constants.DefenseArm.ARM_PORT);
  31     defenseHand = new CANTalon(Constants.DefenseArm.HAND_PORT);
  32     potHandAngles = createHandPotArray();
  33     potArmAngles = createArmPotArray();
  34   }
  35
  36   public double getArmPotAngle() {
  37     return defenseArmPotentiometer.get();
  38   }
  39
  40   public double getHandPotAngle() {
  41     return defenseHandPotentiometer.get();
  42   }
  43
  44   /***
  45    * This method takes an arm location as input (range of [0,2])
  46    * Returns the angle of the arm corresponding to that arm location
  47    *
  48    * @param desiredArmLocation
  49    *          takes an arm location ranging from [0,2]
  50    *          0 is the lowest position of arm
  51    *          2 is the highest position of arm
  52    * @return
  53    *         the angle of the arm corresponding to that arm location
  54    */
  55   public double getAngleForHandLocation(int desiredArmLocation) {
  56     return potHandAngles[desiredArmLocation];
  57   }
  58
  59   public double getAngleForArmLocation(int desiredArmLocation) {
  60     return potArmAngles[desiredArmLocation];
  61   }
  62
  63   public double[] createHandPotArray() {
  64     double[] arr = new double[3];
  65
  66     for (int i = 0; i < 3; i++) {
  67       arr[i] = 45 * i;
  68     }
  69     return arr;
  70   }
  71
  72   public double[] createArmPotArray() {
  73     double[] arr = new double[3];
  74
  75     for (int i = 0; i < 3; i++) {
  76       arr[i] = 45 * i;
  77     }
  78     return arr;
  79   }
  80
  81   /***
  82    * This method sets the voltage of the arm motor. The range is from [-1,1]. A
  83    * negative voltage makes the direction of the motor go backwards.
  84    *
  85    * @param speed
  86    *          The voltage that you set the motor at. The range of the voltage of
  87    *          the arm motor is from [-1,1]. A
  88    *          negative voltage makes the direction of the motor go backwards.
  89    */
  90
  91   public void setArmSpeed(double speed) {
  92     if (speed > 1)
  93       speed = 1;
  94     else if (speed < -1)
  95       speed = -1;
  96
  97     defenseArm.set(speed);
  98   }
  99
 100   /***
 101    * This method sets the voltage of the hand motor. The range is from [-1,1]. A
 102    * negative voltage makes the direction of the motor go backwards.
 103    *
 104    * @param speed
 105    *          The voltage that you set the motor at. The range of the voltage of
 106    *          the hand motor is from [-1,1]. A
 107    *          negative voltage makes the direction of the motor go backwards.
 108    */
 109
 110   public void setHandSpeed(double speed) {
 111     if (speed > 1)
 112       speed = 1;
 113     else if (speed < -1)
 114       speed = -1;
 115
 116     defenseHand.set(speed);
 117   }
 118
 119   @Override
 120   protected void initDefaultCommand() {
 121   }
 122 }