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
  19   // angles corresponding to pre-determined heights we will need
  20
  21   public DefenseArm() {
  22     defenseArmPotentiometer = new AnalogPotentiometer(
  23         Constants.DefenseArm.ARM_CHANNEL,
  24         Constants.DefenseArm.FULL_RANGE,
  25         Constants.DefenseArm.OFFSET);
  26     defenseHandPotentiometer = new AnalogPotentiometer(
  27         Constants.DefenseArm.HAND_CHANNEL,
  28         Constants.DefenseArm.FULL_RANGE,
  29         Constants.DefenseArm.OFFSET);
  30
  31     defenseArm = new CANTalon(Constants.DefenseArm.ARM_PORT);
  32     defenseHand = new CANTalon(Constants.DefenseArm.HAND_PORT);
  33     potHandAngles = createHandPotArray();
  34     potArmAngles = createArmPotArray();
  35   }
  36
  37   public double getArmPotAngle() {
  38     return defenseArmPotentiometer.get();
  39   }
  40
  41   public double getHandPotAngle() {
  42     return defenseHandPotentiometer.get();
  43   }
  44
  45   /***
  46    * This method takes an arm location as input (range of [0,2])
  47    * Returns the angle of the arm corresponding to that arm location
  48    *
  49    * @param desiredArmLocation
  50    *          takes an arm location ranging from [0,2]
  51    *          0 is the lowest position of arm
  52    *          2 is the highest position of arm
  53    * @return
  54    *         the angle of the arm corresponding to that arm location
  55    */
  56   public double getAngleForHandLocation(int desiredArmLocation) {
  57     return potHandAngles[desiredArmLocation];
  58   }
  59
  60   public double getAngleForArmLocation(int desiredArmLocation) {
  61     return potArmAngles[desiredArmLocation];
  62   }
  63
  64   public double[] createHandPotArray() {
  65     double[] arr = new double[3];
  66
  67     for (int i = 0; i < 3; i++) {
  68       arr[i] = 45 * i;
  69     }
  70     return arr;
  71   }
  72
  73   public double[] createArmPotArray() {
  74     double[] arr = new double[3];
  75
  76     for (int i = 0; i < 3; i++) {
  77       arr[i] = 45 * i;
  78     }
  79     return arr;
  80   }
  81
  82   /***
  83    * This method sets the voltage of the arm motor. The range is from [-1,1]. A
  84    * negative voltage makes the direction of the motor go backwards.
  85    *
  86    * @param speed
  87    *          The voltage that you set the motor at. The range of the voltage of
  88    *          the arm motor is from [-1,1]. A
  89    *          negative voltage makes the direction of the motor go backwards.
  90    */
  91
  92   public void setArmSpeed(double speed) {
  93     if (speed > 1)
  94       speed = 1;
  95     else if (speed < -1)
  96       speed = -1;
  97
  98     defenseArm.set(speed);
  99   }
 100
 101   /***
 102    * This method sets the voltage of the hand motor. The range is from [-1,1]. A
 103    * negative voltage makes the direction of the motor go backwards.
 104    *
 105    * @param speed
 106    *          The voltage that you set the motor at. The range of the voltage of
 107    *          the hand motor is from [-1,1]. A
 108    *          negative voltage makes the direction of the motor go backwards.
 109    */
 110
 111   public void setHandSpeed(double speed) {
 112     if (speed > 1)
 113       speed = 1;
 114     else if (speed < -1)
 115       speed = -1;
 116
 117     defenseHand.set(speed);
 118   }
 119
 120   // TODO: figure out if measurements are all in inches
 121   public double getArmHorizontalDisplacement() {
 122     double armHorizontalDisplacement = Constants.DefenseArm.ARM_LENGTH
 123         * Math.cos(getArmPotAngle());
 124     double handHorizontalDisplacement = Constants.DefenseArm.HAND_LENGTH
 125         * Math.cos(getHandPotAngle());
 126     return (armHorizontalDisplacement + handHorizontalDisplacement);
 127   }
 128
 129   public double getArmVerticalDisplacement() {
 130     double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT;
 131     double armVerticalDisplacement = Constants.DefenseArm.ARM_LENGTH
 132         * Math.sin(getArmPotAngle());
 133     double handVerticalDisplacement = Constants.DefenseArm.HAND_LENGTH
 134         * Math.sin(getHandPotAngle());
 135     return (armMounted + armVerticalDisplacement + handVerticalDisplacement);
 136   }
 137
 138   public boolean isOutsideRange() {
 139     if (getArmHorizontalDisplacement() < 15)
 140       return false;
 141     return true;
 142   }
 143
 144   @Override
 145   protected void initDefaultCommand() {
 146   }
 147 }