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

   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 <<<<<<< HEAD
  57 =======
  58
  59 >>>>>>> create separate arrays to hold potentiometer angles for hand and arm and add separate methods for both angle arrays
  60   public double getAngleForHandLocation(int desiredArmLocation) {
  61     return potHandAngles[desiredArmLocation];
  62   }
  63
  64   public double getAngleForArmLocation(int desiredArmLocation) {
  65     return potArmAngles[desiredArmLocation];
  66   }
  67
  68   public double[] createHandPotArray() {
  69     double[] arr = new double[3];
  70
  71     for (int i = 0; i < 3; i++) {
  72       arr[i] = 45 * i;
  73     }
  74     return arr;
  75   }
  76
  77   public double[] createArmPotArray() {
  78     double[] arr = new double[3];
  79
  80     for (int i = 0; i < 3; i++) {
  81       arr[i] = 45 * i;
  82     }
  83     return arr;
  84   }
  85
  86   /***
  87    * This method sets the voltage of the arm 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 arm motor is from [-1,1]. A
  93    *          negative voltage makes the direction of the motor go backwards.
  94    */
  95
  96   public void setArmSpeed(double speed) {
  97     if (speed > 1)
  98       speed = 1;
  99     else if (speed < -1)
 100       speed = -1;
 101
 102     defenseArm.set(speed);
 103   }
 104
 105   /***
 106    * This method sets the voltage of the hand motor. The range is from [-1,1]. A
 107    * negative voltage makes the direction of the motor go backwards.
 108    *
 109    * @param speed
 110    *          The voltage that you set the motor at. The range of the voltage of
 111    *          the hand motor is from [-1,1]. A
 112    *          negative voltage makes the direction of the motor go backwards.
 113    */
 114
 115   public void setHandSpeed(double speed) {
 116     if (speed > 1)
 117       speed = 1;
 118     else if (speed < -1)
 119       speed = -1;
 120
 121     defenseHand.set(speed);
 122   }
 123
 124   // TODO: figure out if measurements are all in inches
 125   public double getArmHorizontalDisplacement() {
 126     double armHorizontalDisplacement = Constants.DefenseArm.ARM_LENGTH
 127         * Math.cos(getArmPotAngle());
 128     double handHorizontalDisplacement = Constants.DefenseArm.HAND_LENGTH
 129         * Math.cos(getHandPotAngle());
 130     return (armHorizontalDisplacement + handHorizontalDisplacement);
 131   }
 132
 133   public double getArmVerticalDisplacement() {
 134     double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT;
 135     double armVerticalDisplacement = Constants.DefenseArm.ARM_LENGTH
 136         * Math.sin(getArmPotAngle());
 137     double handVerticalDisplacement = Constants.DefenseArm.HAND_LENGTH
 138         * Math.sin(getHandPotAngle());
 139     return (armMounted + armVerticalDisplacement + handVerticalDisplacement);
 140   }
 141
 142   public boolean isOutsideRange() {
 143     if (getArmHorizontalDisplacement() < 15)
 144       return false;
 145     return true;
 146   }
 147
 148   @Override
 149   protected void initDefaultCommand() {
 150   }
 151 }