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