1 package org
.usfirst
.frc
.team3501
.robot
.subsystems
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
;
5 import edu
.wpi
.first
.wpilibj
.AnalogPotentiometer
;
6 import edu
.wpi
.first
.wpilibj
.CANTalon
;
7 import edu
.wpi
.first
.wpilibj
.command
.Subsystem
;
10 * The IntakeArm consists of two rollers that are controlled by one motor, with
11 * a potentiometer on it.
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
21 public class IntakeArm
extends Subsystem
{
23 private CANTalon intakeRoller
;
24 private CANTalon leftIntakeArm
;
25 private CANTalon rightIntakeArm
;
26 private AnalogPotentiometer intakePot
;
27 public static double[] potAngles
= { 0, 30, 45, 90 }; // TODO: correct angles
28 public static double moveIntakeArmSpeed
= 0;
31 intakeRoller
= new CANTalon(Constants
.IntakeArm
.ROLLER_PORT
);
32 leftIntakeArm
= new CANTalon(Constants
.IntakeArm
.LEFT_ARM_PORT
);
33 rightIntakeArm
= new CANTalon(Constants
.IntakeArm
.RIGHT_ARM_PORT
);
34 intakePot
= new AnalogPotentiometer(Constants
.IntakeArm
.POT_CHANNEL
,
35 Constants
.IntakeArm
.FULL_RANGE
, Constants
.IntakeArm
.OFFSET
);
39 * This method sets the voltage of the motor to intake the ball. The voltage
40 * values are constants in Constants class
42 public void intakeBall() {
43 intakeRoller
.set(Constants
.IntakeArm
.INTAKE_SPEED
);
47 * This method sets the voltage of the motor to output the ball. The voltage
48 * values are constants in Constants class
50 public void outputBall() {
51 intakeRoller
.set(Constants
.IntakeArm
.OUTPUT_SPEED
);
54 public void stopRollers() {
59 * This method gets you the current voltage of the motor that controls the
60 * intake arm roller. The range of voltage is from [-1,1]. A negative voltage
61 * makes the motor run backwards.
63 * @return Returns the voltage of the motor that controls the roller. The
64 * range of the voltage goes from [-1,1]. A negative voltage indicates
65 * that the motor is running backwards.
68 public double getRollerVoltage() {
69 return intakeRoller
.get();
73 * This method sets the voltages of the arm motors. The range is from [-1,1].
74 * A negative voltage makes the direction of the motor go backwards.
77 * The voltage that you set the motors at. The range of the voltage
78 * of the arm motors is from [-1,1]. A negative voltage makes the
79 * direction of the motors go backwards.
82 public void setArmSpeeds(double voltage
) {
85 else if (voltage
< -1)
88 leftIntakeArm
.set(voltage
);
89 rightIntakeArm
.set(voltage
);
93 * This method gets you the current voltage of the left motor that controls
94 * the intake arm. The range of voltage is from [-1,1]. A negative voltage
95 * makes the motor run backwards.
97 * @return Returns the voltage of the left motor that controls the arm. The
98 * range of the voltage goes from [-1,1]. A negative voltage indicates
99 * that the motor is running backwards.
102 public double getLeftArmSpeed() {
103 return leftIntakeArm
.get();
107 * This method gets you the current voltage of the right motor that controls
108 * the intake arm. The range of voltage is from [-1,1]. A negative voltage
109 * makes the motor run backwards.
111 * @return Returns the voltage of the right motor that controls the arm. The
112 * range of the voltage goes from [-1,1]. A negative voltage indicates
113 * that the motor is running backwards.
115 public double getRightArmSpeed() {
116 return rightIntakeArm
.get();
119 public CANTalon
getLeftIntakeArmMotor() {
120 return leftIntakeArm
;
123 public CANTalon
getRightIntakeArmMotor() {
124 return rightIntakeArm
;
128 * This method checks to see if the presence of the ball inside is true or
131 * @return Returns whether the ball is inside as true or false
134 public boolean isBallInside() {
139 * This method checks to see if the motors controlling the rollers are
142 * @return Returns whether the motors are currently running, and returns the
143 * state of the condition (true or false).
147 public boolean areRollersRolling() {
148 if (Math
.abs(getRollerVoltage()) < 0.02)
154 * This method gets the angle of the potentiometer on the Intake Arm.
156 * @return angle of potentiometer
159 public double getArmAngle() {
160 return intakePot
.get() + Constants
.IntakeArm
.ZERO_ANGLE
;
167 public double getAngleForLevel(double targetLevel
) {
168 return potAngles
[(int) (targetLevel
- 1)];
172 protected void initDefaultCommand() {