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; | |
e1a27621 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 { | |
e3bfff96 KZ |
11 | private CANJaguar left, right; |
12 | ||
e1a27621 E |
13 | DigitalInput limitSwitch = new DigitalInput(1); |
14 | Counter counter = new Counter(limitSwitch); | |
15 | ||
e3bfff96 KZ |
16 | public Arm() { |
17 | left = new CANJaguar(RobotMap.ARM_LEFT); | |
18 | right = new CANJaguar(RobotMap.ARM_RIGHT); | |
19 | } | |
20 | ||
21 | public void initDefaultCommand() { | |
22 | } | |
e1a27621 E |
23 | |
24 | public boolean isSwitchHit() { | |
b3b77d1a E |
25 | return counter.get() > 0; //detects if switch is hit |
26 | } | |
e1a27621 E |
27 | |
28 | public void initializeCounter() { | |
b3b77d1a | 29 | counter.reset(); //resets the counter back to 0 |
e1a27621 | 30 | } |
e3bfff96 KZ |
31 | |
32 | public void fineTuneControl(double d) { | |
33 | if (Math.abs(d) < 0.05) { | |
34 | d = 0; | |
35 | } else if (d > 0) { | |
36 | d *= d; | |
37 | } else { | |
38 | d *= -d; | |
f11ce98e | 39 | } |
e3bfff96 KZ |
40 | setArmSpeeds(d); |
41 | } | |
42 | ||
43 | public void setLeft(double speed) { | |
44 | left.set(-speed); | |
45 | } | |
46 | ||
47 | public void setRight(double speed) { | |
48 | right.set(-speed); | |
49 | } | |
50 | ||
51 | public void setArmSpeeds(double speed) { | |
52 | setLeft(speed); | |
53 | setRight(speed); | |
54 | } | |
55 | ||
56 | public void stop() { | |
57 | left.set(0); | |
58 | right.set(0); | |
59 | } | |
b72e169c | 60 | } |