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