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 { | |
10 | // Defense arm related objects | |
11 | private AnalogPotentiometer defenseArmPotentiometer; | |
12 | private AnalogPotentiometer defenseHandPotentiometer; | |
13 | private CANTalon defenseArm; | |
14 | private CANTalon defenseHand; | |
15 | private double hookHeight; | |
16 | private double footHeight; | |
90784ff2 | 17 | private Double[] potAngles; // change name, 5 to another # |
d6c9c0f4 KZ |
18 | // array; |
19 | ||
20 | public DefenseArm() { | |
21 | defenseArmPotentiometer = new AnalogPotentiometer( | |
22 | Constants.DefenseArm.ARM_CHANNEL, | |
23 | Constants.DefenseArm.FULL_RANGE, | |
24 | Constants.DefenseArm.OFFSET); | |
25 | ||
26 | defenseArm = new CANTalon(Constants.DefenseArm.ARM_PORT); | |
27 | defenseHand = new CANTalon(Constants.DefenseArm.HAND_PORT); | |
28 | } | |
29 | ||
30 | public double getArmPotAngle() { | |
31 | return defenseArmPotentiometer.get(); | |
32 | } | |
33 | ||
34 | public double getHandPotAngle() { | |
35 | return defenseHandPotentiometer.get(); | |
36 | } | |
37 | ||
90784ff2 YN |
38 | public double getDistance(int desiredArmLocation) { |
39 | return potAngles[desiredArmLocation]; | |
40 | } | |
41 | ||
42 | public Double[] putInValues() { | |
43 | for (int i = 0; i < 3; i++) { | |
44 | potAngles[i] = (double) (45 * i); | |
45 | } | |
46 | return potAngles; | |
47 | } | |
48 | ||
d6c9c0f4 KZ |
49 | /*** |
50 | * This method sets the voltage of the arm motor. The range is from [-1,1]. A | |
51 | * negative voltage makes the direction of the motor go backwards. | |
52 | * | |
53 | * @param speed | |
54 | * The voltage that you set the motor at. The range of the voltage of | |
55 | * the arm motor is from [-1,1]. A | |
56 | * negative voltage makes the direction of the motor go backwards. | |
57 | */ | |
58 | ||
59 | public void setArmSpeed(double speed) { | |
60 | if (speed > 1) | |
61 | speed = 1; | |
62 | else if (speed < -1) | |
63 | speed = -1; | |
64 | ||
65 | defenseArm.set(speed); | |
66 | } | |
67 | ||
68 | /*** | |
69 | * This method sets the voltage of the hand motor. The range is from [-1,1]. A | |
70 | * negative voltage makes the direction of the motor go backwards. | |
71 | * | |
72 | * @param speed | |
73 | * The voltage that you set the motor at. The range of the voltage of | |
74 | * the hand motor is from [-1,1]. A | |
75 | * negative voltage makes the direction of the motor go backwards. | |
76 | */ | |
77 | ||
78 | public void setHandSpeed(double speed) { | |
79 | if (speed > 1) | |
80 | speed = 1; | |
81 | else if (speed < -1) | |
82 | speed = -1; | |
83 | ||
84 | defenseHand.set(speed); | |
85 | } | |
86 | ||
3c61311b YA |
87 | public void moveArmDown(int levelsToDegrees) { |
88 | // to move arm down levels | |
89 | if (levelsToDegrees > 0 & levelsToDegrees < 45) { | |
90 | levelsToDegrees = 0; | |
91 | if (levelsToDegrees > 45 & levelsToDegrees < 90) { | |
92 | levelsToDegrees = 45; | |
93 | ||
94 | } | |
95 | } | |
96 | } | |
97 | ||
98 | public void moveArmTo(int levelsToDegrees) { | |
99 | ||
100 | } | |
101 | ||
102 | public void moveArmUp(int levelsToDegrees) { | |
103 | // to move arm up levels | |
104 | if (levelsToDegrees < 45 & levelsToDegrees > 0) { | |
105 | levelsToDegrees = 45; | |
106 | if (levelsToDegrees < 90 & levelsToDegrees > 45) { | |
107 | levelsToDegrees = 90; | |
108 | ||
109 | } | |
110 | ||
111 | } | |
112 | } | |
113 | ||
d6c9c0f4 KZ |
114 | @Override |
115 | protected void initDefaultCommand() { | |
116 | } | |
117 | } |