*/
public class Constants {
- public static class OI {
- // Computer Ports
- public final static int LEFT_STICK_PORT = 0;
- public final static int RIGHT_STICK_PORT = 1;
- // Ports on the Joystick
- public final static int TRIGGER_PORT = 1;
- public final static int DECREMENT_SHOOTER_SPEED_PORT = 2;
- public final static int INCREMENT_SHOOTER_SPEED_PORT = 3;
- public final static int SHOOT_PORT = 4;
- public final static int LOG_PORT = 5;
- }
+ public static class OI {
+ // Computer Ports
+ public final static int LEFT_STICK_PORT = 0;
+ public final static int RIGHT_STICK_PORT = 1;
+ // Ports on the Joystick
+ public final static int TRIGGER_PORT = 1;
+ public final static int DECREMENT_SHOOTER_SPEED_PORT = 2;
+ public final static int INCREMENT_SHOOTER_SPEED_PORT = 3;
+ public final static int SHOOT_PORT = 4;
+ public final static int LOG_PORT = 5;
+ }
- public static class DriveTrain {
- // Drivetrain Motor Related Ports
- public static final int FRONT_LEFT = 0;
- public static final int FRONT_RIGHT = 0;
- public static final int REAR_LEFT = 0;
- public static final int REAR_RIGHT = 0;
+ public static class DriveTrain {
+ // Drivetrain Motor Related Ports
+ public static final int FRONT_LEFT = 0;
+ public static final int FRONT_RIGHT = 0;
+ public static final int REAR_LEFT = 0;
+ public static final int REAR_RIGHT = 0;
- // Encoder related ports
- public final static int ENCODER_LEFT_A = 3;
- public final static int ENCODER_LEFT_B = 4;
- public final static int ENCODER_RIGHT_A = 2;
- public final static int ENCODER_RIGHT_B = 1;
+ // Encoder related ports
+ public final static int ENCODER_LEFT_A = 3;
+ public final static int ENCODER_LEFT_B = 4;
+ public final static int ENCODER_RIGHT_A = 2;
+ public final static int ENCODER_RIGHT_B = 1;
- private final static double WHEEL_DIAMETER = 6.0; // in inches
- private final static double PULSES_PER_ROTATION = 256; // in pulses
- private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
- private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
- public final static double INCHES_PER_PULSE = (((Math.PI)
- * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) / WHEEL_SPROCKET_DIAMETER)
- * WHEEL_DIAMETER;
- }
+ private final static double WHEEL_DIAMETER = 6.0; // in inches
+ private final static double PULSES_PER_ROTATION = 256; // in pulses
+ private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
+ private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
+ public final static double INCHES_PER_PULSE = (((Math.PI) * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
+ / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
+ }
- public static class Scaler {
- // Piston channels
- public final static int FORWARD_CHANNEL = 0;
- public final static int REVERSE_CHANNEL = 0;
+ public static class Scaler {
+ // Piston channels
+ public final static int FORWARD_CHANNEL = 0;
+ public final static int REVERSE_CHANNEL = 0;
- // Winch port
- public final static int WINCH_MOTOR = 0;
- }
+ // Winch port
+ public final static int WINCH_MOTOR = 0;
+ }
- public static class Shooter {
- public static final int PORT = 0;
- public static final int PUNCH_FORWARD_PORT = 0;
- public static final int PUNCH_REVERSE_PORT = 1;
- public static final int ANGLE_ADJUSTER_PORT = 0;
+ public static class Shooter {
+ public static final int PORT = 0;
+ public static final int PUNCH_FORWARD_PORT = 0;
+ public static final int PUNCH_REVERSE_PORT = 1;
+ public static final int ANGLE_ADJUSTER_PORT = 0;
- public static final DoubleSolenoid.Value punch = DoubleSolenoid.Value.kForward;
- public static final DoubleSolenoid.Value retract = DoubleSolenoid.Value.kReverse;
+ public static final DoubleSolenoid.Value punch = DoubleSolenoid.Value.kForward;
+ public static final DoubleSolenoid.Value retract = DoubleSolenoid.Value.kReverse;
- // Encoder port
- public static final int ENCODER_PORT_A = 0;
- public static final int ENCODER_PORT_B = 0;
+ // Encoder port
+ public static final int ENCODER_PORT_A = 0;
+ public static final int ENCODER_PORT_B = 0;
- public static enum State {
- RUNNING, STOPPED;
- }
- }
+ public static enum State {
+ RUNNING, STOPPED;
+ }
+ }
- public static class IntakeArm {
- public static final int ROLLER_PORT = 0;
- public static final int INTAKE_PORT = 1;
- public static final int INTAKE_CHANNEL = 0;
- public static final double INTAKE_SPEED = 0.5;
- public static final double OUTPUT_SPEED = -0.5;
- public final static double FULL_RANGE = 270.0; // in degrees
- public final static double OFFSET = -135.0; // in degrees
- }
+ public static class IntakeArm {
+ public static final int ROLLER_PORT = 0;
+ public static final int ARM_PORT = 1;
+ public static final int POT_CHANNEL = 0;
+ public static final double INTAKE_SPEED = 0.5;
+ public static final double OUTPUT_SPEED = -0.5;
+ public final static double FULL_RANGE = 270.0; // in degrees
+ public final static double OFFSET = -135.0; // in degrees
+ }
- public static class DefenseArm {
- // Potentiometer related ports
- public static final int ARM_CHANNEL = 0;
- public static final int ARM_PORT = 0;
- public static final int HAND_PORT = 1;
- public static final int HAND_CHANNEL = 1;
- public final static double FULL_RANGE = 270.0; // in degrees
- public final static double OFFSET = -135.0; // in degrees
- public final static double[] armPotValue = { 0.0, 45.0, 90.0 }; // 3 level
- public final static double ARM_LENGTH = 0; // TODO: find actual length
- public final static double HAND_LENGTH = 0; // TODO: find actual length
- public final static double ARM_MOUNTED_HEIGHT = 0; // TODO: find actual
- // height
- }
+ public static class DefenseArm {
+ // Potentiometer related ports
+ public static final int ARM_CHANNEL = 0;
+ public static final int ARM_PORT = 0;
+ public static final int HAND_PORT = 1;
+ public static final int HAND_CHANNEL = 1;
+ public final static double FULL_RANGE = 270.0; // in degrees
+ public final static double OFFSET = -135.0; // in degrees
+ public final static double[] armPotValue = { 0.0, 45.0, 90.0 }; // 3
+ // level
+ public final static double ARM_LENGTH = 0; // TODO: find actual length
+ public final static double HAND_LENGTH = 0; // TODO: find actual length
+ public final static double ARM_MOUNTED_HEIGHT = 0; // TODO: find actual
+ // height
+ }
- public enum Defense {
- PORTCULLIS, SALLY_PORT, ROUGH_TERRAIN, LOW_BAR, CHEVAL_DE_FRISE, DRAWBRIDGE, MOAT, ROCK_WALL, RAMPART;
- }
+ public enum Defense {
+ PORTCULLIS, SALLY_PORT, ROUGH_TERRAIN, LOW_BAR, CHEVAL_DE_FRISE, DRAWBRIDGE, MOAT, ROCK_WALL, RAMPART;
+ }
}
* The IntakeArm consists of two rollers that are controlled by one motor, with
* a potentiometer on it.
*
- * The motor controls the rollers, making them roll forwards and backwards.
- * The Intake rollers are on the back of the robot. As the rollers run, they
- * intake the ball.
+ * The motor controls the rollers, making them roll forwards and backwards. The
+ * Intake rollers are on the back of the robot. As the rollers run, they intake
+ * the ball.
*
* @author superuser
*
*/
public class IntakeArm extends Subsystem {
- private CANTalon intakeRoller;
- private CANTalon intakeArm;
- private AnalogPotentiometer intakePot;
-
- public IntakeArm() {
- intakeRoller = new CANTalon(Constants.IntakeArm.ROLLER_PORT);
- intakeArm = new CANTalon(Constants.IntakeArm.INTAKE_PORT);
- intakePot = new AnalogPotentiometer(
- Constants.IntakeArm.INTAKE_CHANNEL,
- Constants.IntakeArm.FULL_RANGE,
- Constants.IntakeArm.OFFSET);
- }
-
- /***
- * This method sets the voltage of the motor to intake the ball.
- * The voltage values are constants in Constants class
- */
- public void intakeBall() {
- intakeRoller.set(Constants.IntakeArm.INTAKE_SPEED);
- }
-
- /***
- * This method sets the voltage of the motor to output the ball.
- * The voltage values are constants in Constants class
- */
- public void outputBall() {
- intakeRoller.set(Constants.IntakeArm.OUTPUT_SPEED);
- }
-
- /***
- * This method gets you the current voltage of the motor that controls the
- * intake arm roller. The range of voltage is from [-1,1].
- * A negative voltage makes the motor run backwards.
- *
- * @return Returns the voltage of the motor that controls the roller. The
- * range of the voltage goes from [-1,1].
- * A negative voltage indicates that the motor is running backwards.
- */
-
- public double getRollerVoltage() {
- return intakeRoller.get();
- }
-
- /***
- * This method sets the voltage of the arm motor. The range is from [-1,1]. A
- * negative voltage makes the direction of the motor go backwards.
- *
- * @param voltage
- * The voltage that you set the motor at. The range of the voltage of
- * the arm motor is from [-1,1]. A
- * negative voltage makes the direction of the motor go backwards.
- */
-
- public void setArmVoltage(double voltage) {
- if (voltage > 1)
- voltage = 1;
- else if (voltage < -1)
- voltage = -1;
-
- intakeArm.set(voltage);
- }
-
- /***
- * This method gets you the current voltage of the motor that controls the
- * intake arm. The range of voltage is from [-1,1].
- * A negative voltage makes the motor run backwards.
- *
- * @return Returns the voltage of the motor that controls the arm. The
- * range of the voltage goes from [-1,1].
- * A negative voltage indicates that the motor is running backwards.
- */
-
- public double getArmVoltage() {
- return intakeArm.get();
- }
-
- /***
- * This method checks to see if the presence of the ball inside is true or
- * false.
- *
- * @return Returns whether the ball is inside as true or false
- */
-
- public boolean isBallInside() {
- return true;
- }
-
- /***
- * This method checks to see if the motors controlling the rollers are
- * currently running.
- *
- * @return Returns whether the motors are currently running, and returns the
- * state of the condition (true or false).
- *
- */
-
- public boolean areRollersRolling() {
- return true;
- }
-
- /***
- * This method gets the angle of the potentiometer on the Intake Arm.
- *
- * @return angle of potentiometer
- */
- public double getArmAngle() {
- return intakePot.get();
- }
-
- @Override
- protected void initDefaultCommand() {
-
- }
+ private CANTalon intakeRoller;
+ private CANTalon intakeArm;
+ private AnalogPotentiometer intakePot;
+
+ public IntakeArm() {
+ intakeRoller = new CANTalon(Constants.IntakeArm.ROLLER_PORT);
+ intakeArm = new CANTalon(Constants.IntakeArm.ARM_PORT);
+ intakePot = new AnalogPotentiometer(Constants.IntakeArm.POT_CHANNEL, Constants.IntakeArm.FULL_RANGE,
+ Constants.IntakeArm.OFFSET);
+ }
+
+ /***
+ * This method sets the voltage of the motor to intake the ball. The voltage
+ * values are constants in Constants class
+ */
+ public void intakeBall() {
+ intakeRoller.set(Constants.IntakeArm.INTAKE_SPEED);
+ }
+
+ /***
+ * This method sets the voltage of the motor to output the ball. The voltage
+ * values are constants in Constants class
+ */
+ public void outputBall() {
+ intakeRoller.set(Constants.IntakeArm.OUTPUT_SPEED);
+ }
+
+ /***
+ * This method gets you the current voltage of the motor that controls the
+ * intake arm roller. The range of voltage is from [-1,1]. A negative
+ * voltage makes the motor run backwards.
+ *
+ * @return Returns the voltage of the motor that controls the roller. The
+ * range of the voltage goes from [-1,1]. A negative voltage
+ * indicates that the motor is running backwards.
+ */
+
+ public double getRollerVoltage() {
+ return intakeRoller.get();
+ }
+
+ /***
+ * This method sets the voltage of the arm motor. The range is from [-1,1].
+ * A negative voltage makes the direction of the motor go backwards.
+ *
+ * @param voltage
+ * The voltage that you set the motor at. The range of the
+ * voltage of the arm motor is from [-1,1]. A negative voltage
+ * makes the direction of the motor go backwards.
+ */
+
+ public void setArmVoltage(double voltage) {
+ if (voltage > 1)
+ voltage = 1;
+ else if (voltage < -1)
+ voltage = -1;
+
+ intakeArm.set(voltage);
+ }
+
+ /***
+ * This method gets you the current voltage of the motor that controls the
+ * intake arm. The range of voltage is from [-1,1]. A negative voltage makes
+ * the motor run backwards.
+ *
+ * @return Returns the voltage of the motor that controls the arm. The range
+ * of the voltage goes from [-1,1]. A negative voltage indicates
+ * that the motor is running backwards.
+ */
+
+ public double getArmVoltage() {
+ return intakeArm.get();
+ }
+
+ /***
+ * This method checks to see if the presence of the ball inside is true or
+ * false.
+ *
+ * @return Returns whether the ball is inside as true or false
+ */
+
+ public boolean isBallInside() {
+ return true;
+ }
+
+ /***
+ * This method checks to see if the motors controlling the rollers are
+ * currently running.
+ *
+ * @return Returns whether the motors are currently running, and returns the
+ * state of the condition (true or false).
+ *
+ */
+
+ public boolean areRollersRolling() {
+ return true;
+ }
+
+ /***
+ * This method gets the angle of the potentiometer on the Intake Arm.
+ *
+ * @return angle of potentiometer
+ */
+ public double getArmAngle() {
+ return intakePot.get();
+ }
+
+ @Override
+ protected void initDefaultCommand() {
+
+ }
}