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