1 package org
.usfirst
.frc
.team3501
.robot
.subsystems
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
;
5 import edu
.wpi
.first
.wpilibj
.AnalogPotentiometer
;
6 import edu
.wpi
.first
.wpilibj
.CANTalon
;
7 import edu
.wpi
.first
.wpilibj
.command
.Subsystem
;
9 public class DoubleJointedDefenseArm
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
;
19 // angles corresponding to pre-determined heights we will need
21 public DoubleJointedDefenseArm() {
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
);
31 defenseArm
= new CANTalon(Constants
.DefenseArm
.ARM_PORT
);
32 defenseHand
= new CANTalon(Constants
.DefenseArm
.HAND_PORT
);
33 potHandAngles
= createHandPotArray();
34 potArmAngles
= createArmPotArray();
37 public double getArmPotAngle() {
38 return defenseArmPotentiometer
.get();
41 public double getHandPotAngle() {
42 return defenseHandPotentiometer
.get();
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
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
54 * the angle of the arm corresponding to that arm location
56 public double getAngleForHandLocation(int desiredArmLocation
) {
57 return potHandAngles
[desiredArmLocation
];
60 public double getAngleForArmLocation(int desiredArmLocation
) {
61 return potArmAngles
[desiredArmLocation
];
64 public double[] createHandPotArray() {
65 double[] arr
= new double[3];
67 for (int i
= 0; i
< 3; i
++) {
73 public double[] createArmPotArray() {
74 double[] arr
= new double[3];
76 for (int i
= 0; i
< 3; i
++) {
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.
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.
92 public void setArmSpeed(double speed
) {
98 defenseArm
.set(speed
);
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.
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.
111 public void setHandSpeed(double speed
) {
117 defenseHand
.set(speed
);
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
);
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
);
138 public boolean isOutsideRange() {
139 if (getArmHorizontalDisplacement() < 15)
145 protected void initDefaultCommand() {