Commit | Line | Data |
---|---|---|
d6c9c0f4 KZ |
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 { | |
d6c9c0f4 KZ |
10 | private AnalogPotentiometer defenseArmPotentiometer; |
11 | private AnalogPotentiometer defenseHandPotentiometer; | |
12 | private CANTalon defenseArm; | |
13 | private CANTalon defenseHand; | |
14 | private double hookHeight; | |
15 | private double footHeight; | |
1d110d2f SC |
16 | private double[] potAngles; |
17 | // angles corresponding to pre-determined heights we will need | |
d6c9c0f4 KZ |
18 | |
19 | public DefenseArm() { | |
20 | defenseArmPotentiometer = new AnalogPotentiometer( | |
21 | Constants.DefenseArm.ARM_CHANNEL, | |
22 | Constants.DefenseArm.FULL_RANGE, | |
23 | Constants.DefenseArm.OFFSET); | |
1d110d2f SC |
24 | defenseHandPotentiometer = new AnalogPotentiometer( |
25 | Constants.DefenseArm.HAND_CHANNEL, | |
26 | Constants.DefenseArm.FULL_RANGE, | |
27 | Constants.DefenseArm.OFFSET); | |
d6c9c0f4 KZ |
28 | |
29 | defenseArm = new CANTalon(Constants.DefenseArm.ARM_PORT); | |
30 | defenseHand = new CANTalon(Constants.DefenseArm.HAND_PORT); | |
1d110d2f | 31 | potAngles = createPotArray(); |
d6c9c0f4 KZ |
32 | } |
33 | ||
34 | public double getArmPotAngle() { | |
35 | return defenseArmPotentiometer.get(); | |
36 | } | |
37 | ||
38 | public double getHandPotAngle() { | |
39 | return defenseHandPotentiometer.get(); | |
40 | } | |
41 | ||
90784ff2 YN |
42 | public double getDistance(int desiredArmLocation) { |
43 | return potAngles[desiredArmLocation]; | |
44 | } | |
45 | ||
1d110d2f SC |
46 | public double[] createPotArray() { |
47 | double[] arr = new double[3]; | |
48 | ||
90784ff2 | 49 | for (int i = 0; i < 3; i++) { |
1d110d2f | 50 | arr[i] = 45 * i; |
90784ff2 | 51 | } |
1d110d2f | 52 | return arr; |
90784ff2 YN |
53 | } |
54 | ||
d6c9c0f4 KZ |
55 | /*** |
56 | * This method sets the voltage of the arm motor. The range is from [-1,1]. A | |
57 | * negative voltage makes the direction of the motor go backwards. | |
58 | * | |
59 | * @param speed | |
60 | * The voltage that you set the motor at. The range of the voltage of | |
61 | * the arm motor is from [-1,1]. A | |
62 | * negative voltage makes the direction of the motor go backwards. | |
63 | */ | |
64 | ||
65 | public void setArmSpeed(double speed) { | |
66 | if (speed > 1) | |
67 | speed = 1; | |
68 | else if (speed < -1) | |
69 | speed = -1; | |
70 | ||
71 | defenseArm.set(speed); | |
72 | } | |
73 | ||
74 | /*** | |
75 | * This method sets the voltage of the hand motor. The range is from [-1,1]. A | |
76 | * negative voltage makes the direction of the motor go backwards. | |
77 | * | |
78 | * @param speed | |
79 | * The voltage that you set the motor at. The range of the voltage of | |
80 | * the hand motor is from [-1,1]. A | |
81 | * negative voltage makes the direction of the motor go backwards. | |
82 | */ | |
83 | ||
84 | public void setHandSpeed(double speed) { | |
85 | if (speed > 1) | |
86 | speed = 1; | |
87 | else if (speed < -1) | |
88 | speed = -1; | |
89 | ||
90 | defenseHand.set(speed); | |
91 | } | |
92 | ||
93 | @Override | |
94 | protected void initDefaultCommand() { | |
95 | } | |
96 | } |