Commit | Line | Data |
---|---|---|
574b06f3 KZ |
1 | package org.usfirst.frc3501.RiceCatRobot.subsystems; |
2 | ||
3 | import java.util.ArrayList; | |
f11ce98e KZ |
4 | |
5 | import org.usfirst.frc3501.RiceCatRobot.RobotMap; | |
6 | ||
7 | import edu.wpi.first.wpilibj.CANJaguar; | |
574b06f3 | 8 | import edu.wpi.first.wpilibj.DigitalInput; |
f11ce98e KZ |
9 | import edu.wpi.first.wpilibj.command.Subsystem; |
10 | ||
11 | public class Arm extends Subsystem { | |
e3bfff96 | 12 | private CANJaguar left, right; |
574b06f3 KZ |
13 | ArrayList<DigitalInput> limitSwitches; |
14 | //channel index numbers correspond to limit switches | |
15 | int[] channels; | |
16 | private final int NUM_OF_SWITCHES = 4; | |
e3bfff96 KZ |
17 | |
18 | public Arm() { | |
19 | left = new CANJaguar(RobotMap.ARM_LEFT); | |
20 | right = new CANJaguar(RobotMap.ARM_RIGHT); | |
574b06f3 KZ |
21 | |
22 | //channels are not known, these are random numbers | |
23 | channels = new int[]{0, 1, 2, 3}; | |
24 | ||
25 | limitSwitches = new ArrayList<DigitalInput>(); | |
26 | //loop | |
27 | for(int i = 0; i < NUM_OF_SWITCHES; i ++){ | |
28 | DigitalInput d = limitSwitches.get(i); | |
29 | d = new DigitalInput(channels[i]); | |
30 | ||
31 | ||
32 | } | |
33 | ||
34 | channels = new int[NUM_OF_SWITCHES]; | |
35 | ||
36 | ||
e3bfff96 KZ |
37 | } |
38 | ||
39 | public void initDefaultCommand() { | |
40 | } | |
41 | ||
42 | public void fineTuneControl(double d) { | |
43 | if (Math.abs(d) < 0.05) { | |
44 | d = 0; | |
45 | } else if (d > 0) { | |
46 | d *= d; | |
47 | } else { | |
48 | d *= -d; | |
f11ce98e | 49 | } |
e3bfff96 KZ |
50 | setArmSpeeds(d); |
51 | } | |
52 | ||
53 | public void setLeft(double speed) { | |
54 | left.set(-speed); | |
55 | } | |
56 | ||
57 | public void setRight(double speed) { | |
58 | right.set(-speed); | |
59 | } | |
60 | ||
61 | public void setArmSpeeds(double speed) { | |
62 | setLeft(speed); | |
63 | setRight(speed); | |
64 | } | |
65 | ||
66 | public void stop() { | |
67 | left.set(0); | |
68 | right.set(0); | |
69 | } | |
574b06f3 KZ |
70 | |
71 | public int getLevel(){ | |
72 | //return last past switch | |
73 | return 0; | |
74 | } | |
b72e169c | 75 | } |