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 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
;
19 // angles corresponding to pre-determined heights we will need
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
57 public double getAngleForHandLocation(int desiredArmLocation
) {
58 return potHandAngles
[desiredArmLocation
];
61 public double getAngleForArmLocation(int desiredArmLocation
) {
62 return potArmAngles
[desiredArmLocation
];
65 public double[] createHandPotArray() {
66 double[] arr
= new double[3];
68 for (int i
= 0; i
< 3; i
++) {
74 public double[] createArmPotArray() {
75 double[] arr
= new double[3];
77 for (int i
= 0; i
< 3; i
++) {
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.
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.
93 public void setArmSpeed(double speed
) {
99 defenseArm
.set(speed
);
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.
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.
112 public void setHandSpeed(double speed
) {
118 defenseHand
.set(speed
);
121 public double getArmHorizontalDist() {
122 double arm
= Constants
.DefenseArm
.ARM_LENGTH
* Math
.cos(getArmPotAngle());
123 double hand
= Constants
.DefenseArm
.HAND_LENGTH
124 * Math
.cos(getHandPotAngle());
128 public double getArmHeight() {
129 double armMounted
= Constants
.DefenseArm
.ARM_MOUNTED_HEIGHT
;
130 double arm
= Constants
.DefenseArm
.ARM_LENGTH
* Math
.sin(getArmPotAngle());
131 double hand
= Constants
.DefenseArm
.HAND_LENGTH
132 * Math
.sin(getHandPotAngle());
133 return (armMounted
+ arm
+ hand
);
136 public boolean isOutsideRange() {
137 if (getArmHorizontalDist() < 15)
143 protected void initDefaultCommand() {