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; | |
baf4f2ba | 6 | import edu.wpi.first.wpilibj.DigitalInput; |
f11ce98e KZ |
7 | import edu.wpi.first.wpilibj.command.Subsystem; |
8 | ||
9 | public class Arm extends Subsystem { | |
e3bfff96 | 10 | private CANJaguar left, right; |
0d42142e | 11 | private DigitalInput limit1, limit2, limit3, limit4; |
f1068f7c | 12 | private boolean limitSet1, limitSet2, limitSet3, limitSet4; |
e3bfff96 KZ |
13 | |
14 | public Arm() { | |
15 | left = new CANJaguar(RobotMap.ARM_LEFT); | |
16 | right = new CANJaguar(RobotMap.ARM_RIGHT); | |
0d42142e LM |
17 | limit1 = new DigitalInput(RobotMap.ARM_LIMIT_1); |
18 | limit2 = new DigitalInput(RobotMap.ARM_LIMIT_2); | |
19 | limit3 = new DigitalInput(RobotMap.ARM_LIMIT_3); | |
20 | limit4 = new DigitalInput(RobotMap.ARM_LIMIT_4); | |
f1068f7c LM |
21 | limitSet1 = false; |
22 | limitSet2 = false; | |
23 | limitSet3 = false; | |
24 | limitSet4 = false; | |
e3bfff96 KZ |
25 | } |
26 | ||
27 | public void initDefaultCommand() { | |
28 | } | |
29 | ||
30 | public void fineTuneControl(double d) { | |
31 | if (Math.abs(d) < 0.05) { | |
32 | d = 0; | |
33 | } else if (d > 0) { | |
34 | d *= d; | |
35 | } else { | |
36 | d *= -d; | |
f11ce98e | 37 | } |
e3bfff96 KZ |
38 | setArmSpeeds(d); |
39 | } | |
40 | ||
41 | public void setLeft(double speed) { | |
42 | left.set(-speed); | |
43 | } | |
44 | ||
45 | public void setRight(double speed) { | |
46 | right.set(-speed); | |
47 | } | |
48 | ||
49 | public void setArmSpeeds(double speed) { | |
50 | setLeft(speed); | |
51 | setRight(speed); | |
52 | } | |
53 | ||
54 | public void stop() { | |
55 | left.set(0); | |
56 | right.set(0); | |
57 | } | |
b72e169c | 58 | } |