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[] potAngles
= { 0, 45, 90 };
18 // angles corresponding to pre-determined heights we will need
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
);
30 defenseArm
= new CANTalon(Constants
.DefenseArm
.ARM_PORT
);
31 defenseHand
= new CANTalon(Constants
.DefenseArm
.HAND_PORT
);
34 public double getArmPotAngle() {
35 return defenseArmPotentiometer
.get();
38 public double getHandPotAngle() {
39 return defenseHandPotentiometer
.get();
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
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
51 * the angle of the arm corresponding to that arm location
53 public double getLevelValue(int level
) {
54 if (level
>= potAngles
.length
)
55 return potAngles
[level
];
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.
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.
70 public void setArmSpeed(double speed
) {
76 defenseArm
.set(speed
);
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.
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.
89 public void setHandSpeed(double speed
) {
95 defenseHand
.set(speed
);
98 public double getArmHorizontalDist() {
99 double arm
= Constants
.DefenseArm
.ARM_LENGTH
* Math
.cos(getArmPotAngle());
100 double hand
= Constants
.DefenseArm
.HAND_LENGTH
101 * Math
.cos(getHandPotAngle());
105 public double getArmHeight() {
106 double armMounted
= Constants
.DefenseArm
.ARM_MOUNTED_HEIGHT
;
107 double arm
= Constants
.DefenseArm
.ARM_LENGTH
* Math
.sin(getArmPotAngle());
108 double hand
= Constants
.DefenseArm
.HAND_LENGTH
109 * Math
.sin(getHandPotAngle());
110 return (armMounted
+ arm
+ hand
);
113 public boolean isOutsideRange() {
114 if (getArmHorizontalDist() < 15)
120 protected void initDefaultCommand() {