/**
* Use a thumb wheel switch to set the autonomous mode setting.
*
* @return Autonomous setting to run.
*/
public int getAutonSetting() {
try {
int switchNum = 0;
int[] binaryValue = new int[3];
boolean[] dIO = {
!enhancedIO.getDigital(BIT_1_CHANNEL),
!enhancedIO.getDigital(BIT_2_CHANNEL),
!enhancedIO.getDigital(BIT_3_CHANNEL)
};
for (int i = 0; i < 3; i++) {
if (dIO[i]) {
binaryValue[i] = 1;
} else {
binaryValue[i] = 0;
}
}
binaryValue[0] *= 4; // convert all binaryValues to decimal values
binaryValue[1] *= 2;
for (int i = 0; i < 3; i++) { // finish binary -> decimal conversion
switchNum += binaryValue[i];
}
return switchNum;
} catch (EnhancedIOException e) {
return -1; // Do nothing in case of failure
}
}
public int getPostion() throws DriverStationEnhancedIO.EnhancedIOException {
if (ds.getAnalogIn(1) < RobotMap.shooterVDposition0) {
position = 1;
}
// If Station Knob is at
else if (ds.getAnalogIn(1) >= RobotMap.shooterVDposition1
&& ds.getAnalogIn(1) < RobotMap.shooterVDposition1 + .3) {
position = 2;
}
// If Station Knob is at
else if (ds.getAnalogIn(1) >= RobotMap.shooterVDposition2
&& ds.getAnalogIn(1) < RobotMap.shooterVDposition2 + .2) {
position = 3;
}
// If Station Knob is at
else if (ds.getAnalogIn(1) >= RobotMap.shooterVDposition3
&& ds.getAnalogIn(1) < RobotMap.shooterVDposition3 + .3) {
position = 4;
}
// If Station Knob is at
else if (ds.getAnalogIn(1) >= RobotMap.shooterVDposition4
&& ds.getAnalogIn(1) < RobotMap.shooterVDposition4 + .3) {
position = 5;
}
// If Station Knob is at
else if (ds.getAnalogIn(1) >= RobotMap.shooterVDposition5) {
position = 6;
}
return position;
}
public int getLiftPoition() throws DriverStationEnhancedIO.EnhancedIOException {
if (ds.getAnalogIn(2) < RobotMap.liftVDposition1) {
liftPosition = 1;
} else if (ds.getAnalogIn(2) < RobotMap.liftVDposition2) {
liftPosition = 2;
} else if (ds.getAnalogIn(2) < RobotMap.liftVDposition3) {
liftPosition = 3;
} else {
liftPosition = 4;
}
return liftPosition;
}
public double getMaxVoltage() {
try {
return enhancedIO.getAnalogIn(MAX_ANALOG_CHANNEL);
} catch (EnhancedIOException e) {
return 2.2;
}
}
public double getDelayPot() {
try {
return getMaxVoltage() - enhancedIO.getAnalogIn(DELAY_POT_CHANNEL);
} catch (EnhancedIOException ex) {
return 0.0;
}
}
/**
* Turns on specified light on OI.
*
* @param lightNum
*/
public void setLight(int lightNum) {
turnOffLights();
try {
enhancedIO.setDigitalOutput(lightNum, true);
} catch (EnhancedIOException e) {
}
}
public double getRawAnalogVoltage() {
try {
return enhancedIO.getAnalogIn(DISTANCE_BUTTONS_CHANNEL);
} catch (EnhancedIOException e) {
return 0;
}
}
public double getSpeedPot() {
try {
return getMaxVoltage() - enhancedIO.getAnalogIn(SPEED_TRIM_POT_CHANNEL);
} catch (EnhancedIOException ex) {
return 0.0;
}
}
/**
* Gets value of hoop height toggle switch.
*
* @return true if high setting, false if middle
*/
public boolean getStingerSwitch() {
try {
return !enhancedIO.getDigital(STINGER_SWITCH_CHANNEL);
} catch (EnhancedIOException ex) {
return true;
}
}
// For debugging purposes.
public double getAnalogValue(int channel) {
double b = 0;
try {
b = enhancedIO.getAnalogOut(channel);
} catch (EnhancedIOException e) {
}
return b;
}
// For debugging purposes.
public boolean getDigitalValue(int channel) {
boolean b = false;
try {
b = !enhancedIO.getDigital(channel);
} catch (EnhancedIOException e) {
}
return b;
}
/** Turns all lights on. */
public void turnOnLights() {
try {
enhancedIO.setDigitalOutput(DISTANCE_BUTTON_KEY_LIGHT_CHANNEL, true);
enhancedIO.setDigitalOutput(DISTANCE_BUTTON_FAR_LIGHT_CHANNEL, true);
enhancedIO.setDigitalOutput(DISTANCE_BUTTON_FENDER_2PT_LIGHT_CHANNEL, true);
enhancedIO.setDigitalOutput(DISTANCE_BUTTON_FENDER_NARROW_LIGHT_CHANNEL, true);
enhancedIO.setDigitalOutput(DISTANCE_BUTTON_REVERSE_LIGHT_CHANNEL, true);
enhancedIO.setDigitalOutput(DISTANCE_BUTTON_FENDER_LIGHT_CHANNEL, true);
enhancedIO.setDigitalOutput(DISTANCE_BUTTON_STOP_LIGHT_CHANNEL, true);
} catch (EnhancedIOException e) {
}
}
public OI() {
enhancedIO = DriverStation.getInstance().getEnhancedIO();
leftStick = new Joystick(RobotMap.LEFT_JOYSTICK_PORT);
rightStick = new Joystick(RobotMap.RIGHT_JOYSTICK_PORT);
shooterStick = new Joystick(RobotMap.SHOOTER_JOYSTICK_PORT);
debugBox = new Joystick(RobotMap.DEBUG_BOX_PORT);
distanceButton = DISTANCE_BUTTON_STOP;
distanceInches = Flywheel.distances[Flywheel.STOP_INDEX];
try {
if (!Devmode.DEV_MODE) {
enhancedIO.setDigitalConfig(
BIT_1_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
BIT_2_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
BIT_3_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
ACQUIRER_IN_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
ACQUIRER_OUT_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
CONVEYOR_UP_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
CONVEYOR_DOWN_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
SHOOTER_BUTTON_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
STINGER_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_KEY_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_FAR_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_FENDER_2PT_LIGHT_CHANNEL,
DriverStationEnhancedIO.tDigitalConfig.kOutput);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_FENDER_NARROW_LIGHT_CHANNEL,
DriverStationEnhancedIO.tDigitalConfig.kOutput);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_REVERSE_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_FENDER_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_STOP_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
}
} catch (EnhancedIOException e) {
}
if (!Devmode.DEV_MODE) {
new JoystickButton(leftStick, 1).whenPressed(new DrivetrainSetGear(false));
new JoystickButton(leftStick, 2).whenPressed(new DrivetrainSetGear(true));
new JoystickButton(rightStick, 1).whenPressed(new TusksExtend());
new JoystickButton(rightStick, 2).whenPressed(new TusksRetract());
// OI box switches
new InverseDigitalIOButton(ACQUIRER_IN_SWITCH_CHANNEL).whileHeld(new AcquirerAcquire());
new InverseDigitalIOButton(ACQUIRER_OUT_SWITCH_CHANNEL).whileHeld(new AcquirerReverse());
new InverseDigitalIOButton(CONVEYOR_UP_SWITCH_CHANNEL).whileHeld(new ConveyManual());
new InverseDigitalIOButton(CONVEYOR_DOWN_SWITCH_CHANNEL).whileHeld(new ConveyReverseManual());
new InverseDigitalIOButton(SHOOTER_BUTTON_CHANNEL).whileHeld(new ConveyAutomatic());
new InverseDigitalIOButton(STINGER_SWITCH_CHANNEL).whileHeld(new StingerExtend());
new JoystickButton(shooterStick, 1).whileHeld(new ConveyManual());
new JoystickButton(shooterStick, 4).whenPressed(new FlywheelStop());
new JoystickButton(shooterStick, 5).whileHeld(new AcquirerReverse());
new JoystickButton(shooterStick, 6).whileHeld(new ConveyReverseManual());
new JoystickButton(shooterStick, 7).whileHeld(new AcquirerAcquire());
new JoystickButton(shooterStick, 8).whileHeld(new ConveyAutomatic());
// see getDistanceButton()
// Debug box switches
new JoystickButton(debugBox, 1)
.whileHeld(
new FlywheelRun(Flywheel.distances[Flywheel.FENDER_INDEX], Flywheel.speedsTopHoop));
new JoystickButton(debugBox, 2).whileHeld(new AcquirerAcquire());
new JoystickButton(debugBox, 3).whileHeld(new ConveyAutomatic());
new JoystickButton(debugBox, 4).whileHeld(new ConveyManual());
// Debug box buttons
new JoystickButton(debugBox, 5).whileHeld(new DrivetrainSetGear(false)); // low gear
new JoystickButton(debugBox, 6).whileHeld(new DrivetrainSetGear(true)); // high gear
new JoystickButton(debugBox, 9).whileHeld(new TusksExtend());
new JoystickButton(debugBox, 10).whileHeld(new TusksRetract());
}
}
public boolean getAutoLeft() throws EnhancedIOException {
return ds.getDigital(3);
}
public boolean getDisckLockButton() throws EnhancedIOException {
return ds.getDigital(8);
}
public boolean getAutoRight() throws EnhancedIOException {
return ds.getDigital(7);
}
public boolean getNandoSwitch() throws EnhancedIOException {
return ds.getDigital(5);
}
public boolean getAngleSwitch() throws EnhancedIOException {
return ds.getDigital(2);
}
public boolean getCompressorSwitch() throws EnhancedIOException {
return ds.getDigital(1);
}
