Commit | Line | Data |
---|---|---|
d6c9c0f4 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 { | |
d6c9c0f4 KZ |
10 | private AnalogPotentiometer defenseArmPotentiometer; |
11 | private AnalogPotentiometer defenseHandPotentiometer; | |
12 | private CANTalon defenseArm; | |
13 | private CANTalon defenseHand; | |
14 | private double hookHeight; | |
15 | private double footHeight; | |
28ad5f35 KZ |
16 | private double[] potAngles = { 0, 45, 90 }; |
17 | ||
1d110d2f | 18 | // angles corresponding to pre-determined heights we will need |
d6c9c0f4 KZ |
19 | |
20 | public DefenseArm() { | |
21 | defenseArmPotentiometer = new AnalogPotentiometer( | |
22 | Constants.DefenseArm.ARM_CHANNEL, | |
23 | Constants.DefenseArm.FULL_RANGE, | |
24 | Constants.DefenseArm.OFFSET); | |
1d110d2f SC |
25 | defenseHandPotentiometer = new AnalogPotentiometer( |
26 | Constants.DefenseArm.HAND_CHANNEL, | |
27 | Constants.DefenseArm.FULL_RANGE, | |
28 | Constants.DefenseArm.OFFSET); | |
d6c9c0f4 KZ |
29 | |
30 | defenseArm = new CANTalon(Constants.DefenseArm.ARM_PORT); | |
31 | defenseHand = new CANTalon(Constants.DefenseArm.HAND_PORT); | |
32 | } | |
33 | ||
34 | public double getArmPotAngle() { | |
35 | return defenseArmPotentiometer.get(); | |
36 | } | |
37 | ||
38 | public double getHandPotAngle() { | |
39 | return defenseHandPotentiometer.get(); | |
40 | } | |
41 | ||
64f47e4f SC |
42 | /*** |
43 | * This method takes an arm location as input (range of [0,2]) | |
44 | * Returns the angle of the arm corresponding to that arm location | |
45 | * | |
46 | * @param desiredArmLocation | |
47 | * takes an arm location ranging from [0,2] | |
48 | * 0 is the lowest position of arm | |
49 | * 2 is the highest position of arm | |
50 | * @return | |
51 | * the angle of the arm corresponding to that arm location | |
52 | */ | |
28ad5f35 KZ |
53 | public double getLevelValue(int level) { |
54 | if (level >= potAngles.length) | |
55 | return potAngles[level]; | |
56 | else | |
57 | return 0; | |
90784ff2 YN |
58 | } |
59 | ||
d6c9c0f4 KZ |
60 | /*** |
61 | * This method sets the voltage of the arm motor. The range is from [-1,1]. A | |
62 | * negative voltage makes the direction of the motor go backwards. | |
63 | * | |
64 | * @param speed | |
65 | * The voltage that you set the motor at. The range of the voltage of | |
66 | * the arm motor is from [-1,1]. A | |
67 | * negative voltage makes the direction of the motor go backwards. | |
68 | */ | |
69 | ||
70 | public void setArmSpeed(double speed) { | |
71 | if (speed > 1) | |
72 | speed = 1; | |
73 | else if (speed < -1) | |
74 | speed = -1; | |
75 | ||
76 | defenseArm.set(speed); | |
77 | } | |
78 | ||
79 | /*** | |
80 | * This method sets the voltage of the hand motor. The range is from [-1,1]. A | |
81 | * negative voltage makes the direction of the motor go backwards. | |
82 | * | |
83 | * @param speed | |
84 | * The voltage that you set the motor at. The range of the voltage of | |
85 | * the hand motor is from [-1,1]. A | |
86 | * negative voltage makes the direction of the motor go backwards. | |
87 | */ | |
88 | ||
89 | public void setHandSpeed(double speed) { | |
90 | if (speed > 1) | |
91 | speed = 1; | |
92 | else if (speed < -1) | |
93 | speed = -1; | |
94 | ||
95 | defenseHand.set(speed); | |
96 | } | |
97 | ||
59a85209 | 98 | // TODO: figure out if measurements are all in inches |
4927eb6c | 99 | public double getArmHorizontalDisplacement() { |
bf83e12d CZ |
100 | double armHorizontalDisplacement = Constants.DefenseArm.ARM_LENGTH |
101 | * Math.cos(getArmPotAngle()); | |
102 | double handHorizontalDisplacement = Constants.DefenseArm.HAND_LENGTH | |
01654890 | 103 | * Math.cos(getHandPotAngle()); |
bf83e12d | 104 | return (armHorizontalDisplacement + handHorizontalDisplacement); |
01654890 CZ |
105 | } |
106 | ||
107 | public double getArmHeight() { | |
108 | double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT; | |
bf83e12d CZ |
109 | double armVerticalDisplacement = Constants.DefenseArm.ARM_LENGTH |
110 | * Math.sin(getArmPotAngle()); | |
111 | double handVerticalDisplacement = Constants.DefenseArm.HAND_LENGTH | |
01654890 | 112 | * Math.sin(getHandPotAngle()); |
bf83e12d | 113 | return (armMounted + armVerticalDisplacement + handVerticalDisplacement); |
01654890 CZ |
114 | } |
115 | ||
116 | public boolean isOutsideRange() { | |
4927eb6c | 117 | if (getArmHorizontalDisplacement() < 15) |
01654890 CZ |
118 | return false; |
119 | return true; | |
120 | } | |
121 | ||
d6c9c0f4 KZ |
122 | @Override |
123 | protected void initDefaultCommand() { | |
124 | } | |
125 | } |