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; | |
72fc3c9b | 16 | |
3fb3f225 SC |
17 | private double[] potHandAngles; |
18 | private double[] potArmAngles; | |
72fc3c9b | 19 | private double[] potAngles; |
28ad5f35 | 20 | |
1d110d2f | 21 | // angles corresponding to pre-determined heights we will need |
d6c9c0f4 KZ |
22 | |
23 | public DefenseArm() { | |
24 | defenseArmPotentiometer = new AnalogPotentiometer( | |
72fc3c9b | 25 | Constants.DefenseArm.ARM_CHANNEL, Constants.DefenseArm.FULL_RANGE, |
d6c9c0f4 | 26 | Constants.DefenseArm.OFFSET); |
1d110d2f | 27 | defenseHandPotentiometer = new AnalogPotentiometer( |
72fc3c9b | 28 | Constants.DefenseArm.HAND_CHANNEL, Constants.DefenseArm.FULL_RANGE, |
1d110d2f | 29 | Constants.DefenseArm.OFFSET); |
d6c9c0f4 KZ |
30 | |
31 | defenseArm = new CANTalon(Constants.DefenseArm.ARM_PORT); | |
32 | defenseHand = new CANTalon(Constants.DefenseArm.HAND_PORT); | |
3fb3f225 SC |
33 | potHandAngles = createHandPotArray(); |
34 | potArmAngles = createArmPotArray(); | |
d6c9c0f4 KZ |
35 | } |
36 | ||
37 | public double getArmPotAngle() { | |
38 | return defenseArmPotentiometer.get(); | |
39 | } | |
40 | ||
41 | public double getHandPotAngle() { | |
42 | return defenseHandPotentiometer.get(); | |
43 | } | |
44 | ||
64f47e4f | 45 | /*** |
72fc3c9b CZ |
46 | * This method takes an arm location as input (range of [0,2]) Returns the |
47 | * angle of the arm corresponding to that arm location | |
64f47e4f SC |
48 | * |
49 | * @param desiredArmLocation | |
72fc3c9b CZ |
50 | * takes an arm location ranging from [0,2] 0 is the lowest position |
51 | * of arm 2 is the highest position of arm | |
52 | * @return the angle of the arm corresponding to that arm location | |
64f47e4f | 53 | */ |
3fb3f225 SC |
54 | public double getAngleForHandLocation(int desiredArmLocation) { |
55 | return potHandAngles[desiredArmLocation]; | |
90784ff2 YN |
56 | } |
57 | ||
3fb3f225 SC |
58 | public double getAngleForArmLocation(int desiredArmLocation) { |
59 | return potArmAngles[desiredArmLocation]; | |
60 | } | |
61 | ||
62 | public double[] createHandPotArray() { | |
63 | double[] arr = new double[3]; | |
64 | ||
65 | for (int i = 0; i < 3; i++) { | |
66 | arr[i] = 45 * i; | |
67 | } | |
68 | return arr; | |
69 | } | |
70 | ||
71 | public double[] createArmPotArray() { | |
1d110d2f SC |
72 | double[] arr = new double[3]; |
73 | ||
90784ff2 | 74 | for (int i = 0; i < 3; i++) { |
1d110d2f | 75 | arr[i] = 45 * i; |
90784ff2 | 76 | } |
1d110d2f | 77 | return arr; |
90784ff2 YN |
78 | } |
79 | ||
d6c9c0f4 KZ |
80 | /*** |
81 | * This method sets the voltage of the arm motor. The range is from [-1,1]. A | |
82 | * negative voltage makes the direction of the motor go backwards. | |
83 | * | |
84 | * @param speed | |
85 | * The voltage that you set the motor at. The range of the voltage of | |
72fc3c9b CZ |
86 | * the arm motor is from [-1,1]. A negative voltage makes the |
87 | * direction of the motor go backwards. | |
d6c9c0f4 KZ |
88 | */ |
89 | ||
90 | public void setArmSpeed(double speed) { | |
91 | if (speed > 1) | |
92 | speed = 1; | |
93 | else if (speed < -1) | |
94 | speed = -1; | |
95 | ||
96 | defenseArm.set(speed); | |
97 | } | |
98 | ||
99 | /*** | |
100 | * This method sets the voltage of the hand motor. The range is from [-1,1]. A | |
101 | * negative voltage makes the direction of the motor go backwards. | |
102 | * | |
103 | * @param speed | |
104 | * The voltage that you set the motor at. The range of the voltage of | |
72fc3c9b CZ |
105 | * the hand motor is from [-1,1]. A negative voltage makes the |
106 | * direction of the motor go backwards. | |
d6c9c0f4 KZ |
107 | */ |
108 | ||
109 | public void setHandSpeed(double speed) { | |
110 | if (speed > 1) | |
111 | speed = 1; | |
112 | else if (speed < -1) | |
113 | speed = -1; | |
114 | ||
115 | defenseHand.set(speed); | |
116 | } | |
117 | ||
59a85209 | 118 | // TODO: figure out if measurements are all in inches |
4927eb6c | 119 | public double getArmHorizontalDisplacement() { |
bf83e12d CZ |
120 | double armHorizontalDisplacement = Constants.DefenseArm.ARM_LENGTH |
121 | * Math.cos(getArmPotAngle()); | |
122 | double handHorizontalDisplacement = Constants.DefenseArm.HAND_LENGTH | |
01654890 | 123 | * Math.cos(getHandPotAngle()); |
bf83e12d | 124 | return (armHorizontalDisplacement + handHorizontalDisplacement); |
01654890 CZ |
125 | } |
126 | ||
56c148bd | 127 | public double getArmVerticalDisplacement() { |
01654890 | 128 | double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT; |
bf83e12d CZ |
129 | double armVerticalDisplacement = Constants.DefenseArm.ARM_LENGTH |
130 | * Math.sin(getArmPotAngle()); | |
131 | double handVerticalDisplacement = Constants.DefenseArm.HAND_LENGTH | |
01654890 | 132 | * Math.sin(getHandPotAngle()); |
bf83e12d | 133 | return (armMounted + armVerticalDisplacement + handVerticalDisplacement); |
01654890 CZ |
134 | } |
135 | ||
72fc3c9b CZ |
136 | public double getArmHorizontalDist() { |
137 | double arm = Constants.DefenseArm.ARM_LENGTH * Math.cos(getArmPotAngle()); | |
138 | double hand = Constants.DefenseArm.HAND_LENGTH | |
139 | * Math.cos(getHandPotAngle()); | |
140 | return (arm + hand); | |
141 | } | |
142 | ||
143 | public double getArmHeight() { | |
144 | double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT; | |
145 | double arm = Constants.DefenseArm.ARM_LENGTH * Math.sin(getArmPotAngle()); | |
146 | double hand = Constants.DefenseArm.HAND_LENGTH | |
147 | * Math.sin(getHandPotAngle()); | |
148 | return (armMounted + arm + hand); | |
149 | } | |
150 | ||
01654890 | 151 | public boolean isOutsideRange() { |
72fc3c9b | 152 | if (getArmHorizontalDist() < 15) |
01654890 CZ |
153 | return false; |
154 | return true; | |
155 | } | |
156 | ||
d6c9c0f4 KZ |
157 | @Override |
158 | protected void initDefaultCommand() { | |
159 | } | |
160 | } |