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
  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     potAngles = createPotArray();
  33   }
  34
  35   public double getArmPotAngle() {
  36     return defenseArmPotentiometer.get();
  37   }
  38
  39   public double getHandPotAngle() {
  40     return defenseHandPotentiometer.get();
  41   }
  42
  43   /***
  44    * This method takes an arm location as input (range of [0,2])
  45    * Returns the angle of the arm corresponding to that arm location
  46    *
  47    * @param desiredArmLocation
  48    *          takes an arm location ranging from [0,2]
  49    *          0 is the lowest position of arm
  50    *          2 is the highest position of arm
  51    * @return
  52    *         the angle of the arm corresponding to that arm location
  53    */
  54   public double getAngleForLocation(int desiredArmLocation) {
  55     return potAngles[desiredArmLocation];
  56   }
  57
  58   public double[] createPotArray() {
  59     double[] arr = new double[3];
  60
  61     for (int i = 0; i < 3; i++) {
  62       arr[i] = 45 * i;
  63     }
  64     return arr;
  65   }
  66
  67   /***
  68    * This method sets the voltage of the arm motor. The range is from [-1,1]. A
  69    * negative voltage makes the direction of the motor go backwards.
  70    *
  71    * @param speed
  72    *          The voltage that you set the motor at. The range of the voltage of
  73    *          the arm motor is from [-1,1]. A
  74    *          negative voltage makes the direction of the motor go backwards.
  75    */
  76
  77   public void setArmSpeed(double speed) {
  78     if (speed > 1)
  79       speed = 1;
  80     else if (speed < -1)
  81       speed = -1;
  82
  83     defenseArm.set(speed);
  84   }
  85
  86   /***
  87    * This method sets the voltage of the hand motor. The range is from [-1,1]. A
  88    * negative voltage makes the direction of the motor go backwards.
  89    *
  90    * @param speed
  91    *          The voltage that you set the motor at. The range of the voltage of
  92    *          the hand motor is from [-1,1]. A
  93    *          negative voltage makes the direction of the motor go backwards.
  94    */
  95
  96   public void setHandSpeed(double speed) {
  97     if (speed > 1)
  98       speed = 1;
  99     else if (speed < -1)
 100       speed = -1;
 101
 102     defenseHand.set(speed);
 103   }
 104
 105   public double getArmHorizontalDist() {
 106     double arm = Constants.DefenseArm.ARM_LENGTH * Math.cos(getArmPotAngle());
 107     double hand = Constants.DefenseArm.HAND_LENGTH
 108         * Math.cos(getHandPotAngle());
 109     return (arm + hand);
 110   }
 111
 112   public double getArmHeight() {
 113     double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT;
 114     double arm = Constants.DefenseArm.ARM_LENGTH * Math.sin(getArmPotAngle());
 115     double hand = Constants.DefenseArm.HAND_LENGTH
 116         * Math.sin(getHandPotAngle());
 117     return (armMounted + arm + hand);
 118   }
 119
 120   public boolean isOutsideRange() {
 121     if (getArmHorizontalDist() < 15)
 122       return false;
 123     return true;
 124   }
 125
 126   @Override
 127   protected void initDefaultCommand() {
 128   }
 129 }