Commit | Line | Data |
---|---|---|
f11ce98e KZ |
1 | package org.usfirst.frc3501.RiceCatRobot.subsystems; |
2 | ||
3 | import org.usfirst.frc3501.RiceCatRobot.RobotMap; | |
4 | ||
5 | import edu.wpi.first.wpilibj.CANJaguar; | |
7a25a687 E |
6 | import edu.wpi.first.wpilibj.Counter; |
7 | import edu.wpi.first.wpilibj.DigitalInput; | |
f11ce98e KZ |
8 | import edu.wpi.first.wpilibj.command.Subsystem; |
9 | ||
10 | public class Arm extends Subsystem { | |
7a25a687 E |
11 | private CANJaguar left, right; |
12 | double speedOfArm = 0.0; | |
e3bfff96 | 13 | |
7a25a687 E |
14 | DigitalInput[] limitSwitches = new DigitalInput[3]; |
15 | Counter[] counters = new Counter[3]; | |
e3bfff96 | 16 | |
7a25a687 E |
17 | public Arm() { |
18 | left = new CANJaguar(RobotMap.ARM_LEFT); | |
19 | right = new CANJaguar(RobotMap.ARM_RIGHT); | |
20 | } | |
e3bfff96 | 21 | |
7a25a687 E |
22 | public void setDigitalInputValues() { |
23 | limitSwitches[0] = new DigitalInput(0); | |
24 | limitSwitches[1] = new DigitalInput(1); | |
25 | limitSwitches[2] = new DigitalInput(2); | |
26 | } | |
e3bfff96 | 27 | |
7a25a687 E |
28 | public void setCountersCorrespondingToSwitches() { |
29 | counters[0] = new Counter(0); | |
30 | counters[1] = new Counter(1); | |
31 | counters[3] = new Counter(2); | |
32 | } | |
e3bfff96 | 33 | |
7a25a687 E |
34 | public double getArmSpeed() { |
35 | return this.speedOfArm; | |
e3bfff96 KZ |
36 | } |
37 | ||
7a25a687 E |
38 | public boolean isSwitch1Hit() { |
39 | return counters[0].get() > 0; | |
40 | } | |
e3bfff96 | 41 | |
7a25a687 E |
42 | public boolean isSwitch2Hit() { |
43 | return counters[1].get() > 0; | |
44 | } | |
45 | ||
46 | public boolean isSwitch3Hit() { | |
47 | return counters[2].get() > 0; | |
48 | } | |
49 | ||
50 | public void initializeCounters() { | |
51 | for (int i = 0; i < counters.length; i++) { | |
52 | counters[i].reset(); | |
53 | } | |
54 | } | |
55 | ||
56 | public void initDefaultCommand() { | |
57 | } | |
58 | ||
59 | public void fineTuneControl(double d) { | |
60 | if (Math.abs(d) < 0.05) { | |
61 | d = 0; | |
62 | } else if (d > 0) { | |
63 | d *= d; | |
64 | } else { | |
65 | d *= -d; | |
66 | } | |
67 | setArmSpeeds(d); | |
68 | } | |
69 | ||
70 | public void setLeft(double speed) { | |
71 | left.set(-speed); | |
72 | } | |
73 | ||
74 | public void setRight(double speed) { | |
75 | right.set(-speed); | |
76 | } | |
77 | ||
78 | public void setArmSpeeds(double speed) { | |
79 | this.speedOfArm = speed; | |
80 | setLeft(speed); | |
81 | setRight(speed); | |
82 | } | |
83 | ||
84 | public void stop() { | |
85 | this.speedOfArm = 0.0; | |
86 | left.set(0); | |
87 | right.set(0); | |
88 | } | |
b72e169c | 89 | } |