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