Commit | Line | Data |
---|---|---|
7b11350e KZ |
1 | package org.usfirst.frc.team3501.robot.subsystems; |
2 | ||
3 | import org.usfirst.frc.team3501.robot.Constants; | |
4 | ||
6fd4f44e | 5 | import edu.wpi.first.wpilibj.AnalogPotentiometer; |
7b11350e KZ |
6 | import edu.wpi.first.wpilibj.CANTalon; |
7 | import edu.wpi.first.wpilibj.command.Subsystem; | |
8 | ||
add57fee | 9 | /*** |
1eb84222 SC |
10 | * The IntakeArm consists of two rollers that are controlled by one motor, with |
11 | * a potentiometer on it. | |
add57fee | 12 | * |
dc394eb0 SG |
13 | * The motor controls the rollers, making them roll forwards and backwards. The |
14 | * Intake rollers are on the back of the robot. As the rollers run, they intake | |
15 | * the ball. | |
add57fee YA |
16 | * |
17 | * @author superuser | |
18 | * | |
19 | */ | |
20 | ||
7b11350e | 21 | public class IntakeArm extends Subsystem { |
4e0d6389 LM |
22 | |
23 | private CANTalon intakeRoller; | |
24 | private CANTalon intakeArm; | |
25 | private AnalogPotentiometer intakePot; | |
54c588d9 CZ |
26 | public static double[] potAngles = { 0, 30, 45, 90 }; // TODO: correct angles |
27 | public static double moveIntakeArmSpeed = 0; | |
4e0d6389 LM |
28 | |
29 | public IntakeArm() { | |
30 | intakeRoller = new CANTalon(Constants.IntakeArm.ROLLER_PORT); | |
31 | intakeArm = new CANTalon(Constants.IntakeArm.ARM_PORT); | |
32 | intakePot = new AnalogPotentiometer(Constants.IntakeArm.POT_CHANNEL, | |
33 | Constants.IntakeArm.FULL_RANGE, Constants.IntakeArm.OFFSET); | |
4e0d6389 LM |
34 | } |
35 | ||
36 | /*** | |
37 | * This method sets the voltage of the motor to intake the ball. The voltage | |
38 | * values are constants in Constants class | |
39 | */ | |
40 | public void intakeBall() { | |
41 | intakeRoller.set(Constants.IntakeArm.INTAKE_SPEED); | |
42 | } | |
43 | ||
44 | /*** | |
45 | * This method sets the voltage of the motor to output the ball. The voltage | |
46 | * values are constants in Constants class | |
47 | */ | |
48 | public void outputBall() { | |
49 | intakeRoller.set(Constants.IntakeArm.OUTPUT_SPEED); | |
50 | } | |
51 | ||
2b65c91d | 52 | public void stopRollers() { |
33141cdd | 53 | intakeRoller.set(0); |
2b65c91d LM |
54 | } |
55 | ||
4e0d6389 LM |
56 | /*** |
57 | * This method gets you the current voltage of the motor that controls the | |
58 | * intake arm roller. The range of voltage is from [-1,1]. A negative voltage | |
59 | * makes the motor run backwards. | |
60 | * | |
61 | * @return Returns the voltage of the motor that controls the roller. The | |
62 | * range of the voltage goes from [-1,1]. A negative voltage indicates | |
63 | * that the motor is running backwards. | |
64 | */ | |
65 | ||
66 | public double getRollerVoltage() { | |
67 | return intakeRoller.get(); | |
68 | } | |
69 | ||
70 | /*** | |
71 | * This method sets the voltage of the arm motor. The range is from [-1,1]. A | |
72 | * negative voltage makes the direction of the motor go backwards. | |
73 | * | |
74 | * @param voltage | |
75 | * The voltage that you set the motor at. The range of the voltage of | |
76 | * the arm motor is from [-1,1]. A negative voltage makes the | |
77 | * direction of the motor go backwards. | |
78 | */ | |
79 | ||
80 | public void setArmSpeed(double voltage) { | |
81 | if (voltage > 1) | |
82 | voltage = 1; | |
83 | else if (voltage < -1) | |
84 | voltage = -1; | |
85 | ||
86 | intakeArm.set(voltage); | |
87 | } | |
88 | ||
89 | /*** | |
90 | * This method gets you the current voltage of the motor that controls the | |
91 | * intake arm. The range of voltage is from [-1,1]. A negative voltage makes | |
92 | * the motor run backwards. | |
93 | * | |
94 | * @return Returns the voltage of the motor that controls the arm. The range | |
95 | * of the voltage goes from [-1,1]. A negative voltage indicates that | |
96 | * the motor is running backwards. | |
97 | */ | |
98 | ||
99 | public double getArmSpeed() { | |
100 | return intakeArm.get(); | |
101 | } | |
102 | ||
103 | /*** | |
104 | * This method checks to see if the presence of the ball inside is true or | |
105 | * false. | |
106 | * | |
107 | * @return Returns whether the ball is inside as true or false | |
108 | */ | |
109 | ||
110 | public boolean isBallInside() { | |
111 | return true; | |
112 | } | |
113 | ||
114 | /*** | |
115 | * This method checks to see if the motors controlling the rollers are | |
116 | * currently running. | |
117 | * | |
118 | * @return Returns whether the motors are currently running, and returns the | |
119 | * state of the condition (true or false). | |
120 | * | |
121 | */ | |
122 | ||
123 | public boolean areRollersRolling() { | |
42c4c734 LM |
124 | if (Math.abs(getRollerVoltage()) < 0.02) |
125 | return false; | |
4e0d6389 LM |
126 | return true; |
127 | } | |
128 | ||
129 | /*** | |
130 | * This method gets the angle of the potentiometer on the Intake Arm. | |
131 | * | |
132 | * @return angle of potentiometer | |
133 | */ | |
134 | ||
135 | public double getArmAngle() { | |
136 | return intakePot.get() + Constants.IntakeArm.ZERO_ANGLE; | |
137 | } | |
138 | ||
139 | public void stop() { | |
140 | setArmSpeed(0); | |
141 | } | |
142 | ||
143 | public double getAngleForLevel(double targetLevel) { | |
144 | return potAngles[(int) (targetLevel - 1)]; | |
145 | } | |
146 | ||
147 | @Override | |
148 | protected void initDefaultCommand() { | |
149 | ||
150 | } | |
7b11350e | 151 | } |