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
;
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
);
32 potHandAngles
= createHandPotArray();
33 potArmAngles
= createArmPotArray();
36 public double getArmPotAngle() {
37 return defenseArmPotentiometer
.get();
40 public double getHandPotAngle() {
41 return defenseHandPotentiometer
.get();
45 * This method takes an arm location as input (range of [0,2])
46 * Returns the angle of the arm corresponding to that arm location
48 * @param desiredArmLocation
49 * takes an arm location ranging from [0,2]
50 * 0 is the lowest position of arm
51 * 2 is the highest position of arm
53 * the angle of the arm corresponding to that arm location
55 public double getAngleForHandLocation(int desiredArmLocation
) {
56 return potHandAngles
[desiredArmLocation
];
59 public double getAngleForArmLocation(int desiredArmLocation
) {
60 return potArmAngles
[desiredArmLocation
];
63 public double[] createHandPotArray() {
64 double[] arr
= new double[3];
66 for (int i
= 0; i
< 3; i
++) {
72 public double[] createArmPotArray() {
73 double[] arr
= new double[3];
75 for (int i
= 0; i
< 3; i
++) {
82 * This method sets the voltage of the arm motor. The range is from [-1,1]. A
83 * negative voltage makes the direction of the motor go backwards.
86 * The voltage that you set the motor at. The range of the voltage of
87 * the arm motor is from [-1,1]. A
88 * negative voltage makes the direction of the motor go backwards.
91 public void setArmSpeed(double speed
) {
97 defenseArm
.set(speed
);
101 * This method sets the voltage of the hand motor. The range is from [-1,1]. A
102 * negative voltage makes the direction of the motor go backwards.
105 * The voltage that you set the motor at. The range of the voltage of
106 * the hand motor is from [-1,1]. A
107 * negative voltage makes the direction of the motor go backwards.
110 public void setHandSpeed(double speed
) {
116 defenseHand
.set(speed
);
120 protected void initDefaultCommand() {