Package edu.stuy

Source Code of edu.stuy.OI

package edu.stuy;

import edu.stuy.commands.*;
import edu.stuy.subsystems.Flywheel;
import edu.stuy.util.InverseDigitalIOButton;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO.EnhancedIOException;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.JoystickButton;

public class OI {
    private Joystick leftStick;
    private Joystick rightStick;
    private Joystick shooterStick;
    private Joystick debugBox;
   
    public static final int DISTANCE_BUTTON_KEY = 7;
    public static final int DISTANCE_BUTTON_FAR = 6;
    public static final int DISTANCE_BUTTON_FENDER_LENGTH = 5;
    public static final int DISTANCE_BUTTON_FENDER_WIDTH = 3;
    public static final int DISTANCE_BUTTON_REVERSE = 4;
    public static final int DISTANCE_BUTTON_FENDER = 2;
    public static final int DISTANCE_BUTTON_STOP = 1;
   
    private DriverStationEnhancedIO enhancedIO;
   
    // EnhancedIO digital input
   
    public static final int CONVEYOR_DOWN_SWITCH_CHANNEL = 1;
    public static final int CONVEYOR_UP_SWITCH_CHANNEL = 2;
    public static final int BIT_1_CHANNEL = 5;
    public static final int BIT_2_CHANNEL = 4;
    public static final int BIT_3_CHANNEL = 3;
    public static final int SHOOTER_BUTTON_CHANNEL = 7;
    public static final int STINGER_SWITCH_CHANNEL = 6;
    public static final int ACQUIRER_IN_SWITCH_CHANNEL = 9;
    public static final int ACQUIRER_OUT_SWITCH_CHANNEL = 8;
   
    public int distanceButton;
    public double distanceInches;
    public boolean topHoop = true;
   
    // EnhancedIO digital output
    private static final int DISTANCE_BUTTON_KEY_LIGHT_CHANNEL = 10;
    private static final int DISTANCE_BUTTON_FAR_LIGHT_CHANNEL = 11;
    private static final int DISTANCE_BUTTON_FENDER_2PT_LIGHT_CHANNEL = 15;
    private static final int DISTANCE_BUTTON_FENDER_NARROW_LIGHT_CHANNEL = 13;
    private static final int DISTANCE_BUTTON_REVERSE_LIGHT_CHANNEL = 12;
    private static final int DISTANCE_BUTTON_FENDER_LIGHT_CHANNEL = 14;
    private static final int DISTANCE_BUTTON_STOP_LIGHT_CHANNEL = 16;
   
    // EnhancedIO analog input
    public static final int DISTANCE_BUTTONS_CHANNEL = 1;
    public static final int SPEED_TRIM_POT_CHANNEL = 2;
    public static final int DELAY_POT_CHANNEL = 3;
    public static final int MAX_ANALOG_CHANNEL = 4;

    public static final int MAX_WAIT_TIME = 10;
   
    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());
        }
    }

   
    // Copied from last year's DesDroid code.
   
    public double getRawAnalogVoltage() {
        try {
            return enhancedIO.getAnalogIn(DISTANCE_BUTTONS_CHANNEL);
        }
        catch (EnhancedIOException e) {
            return 0;
        }
    }
   
    public double getMaxVoltage() {
        try {
            return enhancedIO.getAnalogIn(MAX_ANALOG_CHANNEL);
        }
        catch (EnhancedIOException e) {
            return 2.2;
        }
    }
   
    /**
     * Determines which height button is pressed. All (7 logically because
     * side buttons are wired together) buttons are wired by means of
     * resistors to one analog input. Depending on the button that is pressed, a
     * different voltage is read by the analog input. Each resistor reduces the
     * voltage by about 1/7 the maximum voltage.
     *
     * @return An integer value representing the distance button that was pressed.
     * If a Joystick button is being used, that will returned. Otherwise, the
     * button will be returned from the voltage (if it returns 0, no button is pressed).
     */
    public int getDistanceButton() {
       if (shooterStick.getRawButton(9)) {
           distanceButton = DISTANCE_BUTTON_STOP;
       }
       else if(shooterStick.getRawButton(10)) {
           distanceButton = DISTANCE_BUTTON_FENDER;
       }
       else if(shooterStick.getRawButton(11)) {
           distanceButton = DISTANCE_BUTTON_FAR;
       }
       int preValue = (int) ((getRawAnalogVoltage() / (getMaxVoltage() / 8)) + 0.5);
       // If no buttons are pressed, it does not update the distance.
       if(preValue != 0){
           distanceButton = preValue;
       }
       return distanceButton;
    }
   
    /**
     * Takes the distance button that has been pressed, and finds the distance for
     * the shooter to use.
     * @return distance for the shooter.
     */
    public double getDistanceFromDistanceButton(){
        switch(distanceButton){
            case DISTANCE_BUTTON_KEY:
                distanceInches = Flywheel.distances[Flywheel.CLOSE_KEY_INDEX];
                break;
            case DISTANCE_BUTTON_FAR:
                distanceInches = Flywheel.distances[Flywheel.MAX_DIST];
                break;
            case DISTANCE_BUTTON_FENDER_LENGTH:
                distanceInches = Flywheel.distances[Flywheel.FENDER_LONG_INDEX];
                break;
            case DISTANCE_BUTTON_FENDER_WIDTH:
                distanceInches = Flywheel.distances[Flywheel.FENDER_WIDE_INDEX];
                break;
            case DISTANCE_BUTTON_REVERSE:
                distanceInches = Flywheel.distances[Flywheel.REVERSE_INDEX];
                break;
            case DISTANCE_BUTTON_FENDER:
                distanceInches = Flywheel.distances[Flywheel.FENDER_INDEX];
                break;
            case DISTANCE_BUTTON_STOP:
                distanceInches = 0;
                break;
            default:
                break;
        }
        return distanceInches;
    }
   
    // Copied from last year's DesDroid code.
   
    public Joystick getLeftStick() {
        return leftStick;
    }
   
    public Joystick getRightStick() {
        return rightStick;
    }
   
    public Joystick getDebugBox() {
        return debugBox;
    }

    /**
     * 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;
        }
    }
   
    /**
     * 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 getDebugBoxBinaryAutonSetting() {
        int switchNum = 0;
        int[] binaryValue = new int[4];

        //boolean[] dIO = {debugBox.getRawButton(1), debugBox.getRawButton(2), debugBox.getRawButton(3), debugBox.getRawButton(4)};

        /*for (int i = 0; i < 4; i++) {
            if (dIO[i]) {
                binaryValue[i] = 1;
            }
            else {
                binaryValue[i] = 0;
            }
        }*/

        binaryValue[0] *= 8; // convert all binaryValues to decimal values
        binaryValue[1] *= 4;
        binaryValue[2] *= 2;

        for (int i = 0; i < 4; i++) { // finish binary -> decimal conversion
            switchNum += binaryValue[i];
        }

        return switchNum;
    }

    public double getSpeedPot() {
        try {
            return getMaxVoltage() - enhancedIO.getAnalogIn(SPEED_TRIM_POT_CHANNEL);
        } catch (EnhancedIOException ex) {
            return 0.0;
        }
    }

    public double getSpeedTrim() {
        double trim = getSpeedPot(); // 0 to max voltage (the 0 might be a little negative)
        double halfMax = getMaxVoltage() / 2;
        double speed = 0;
        speed = (trim - halfMax) / halfMax;
        //deadband
        if(speed < -0.1) {
            speed = speed * 1.0/0.9;
        }
        else if (speed < 0.1) {
            speed = 0;
        }
        else {
            speed = speed * 1.0 / 0.9;
        }
        return Flywheel.MAX_TRIM_RPM * speed;
    }
   
    public double getDelayPot() {
        try {
            return getMaxVoltage() - enhancedIO.getAnalogIn(DELAY_POT_CHANNEL);
        } catch (EnhancedIOException ex) {
            return 0.0;
        }
    }
   
    public double getDelayTime() {
        double delay = getDelayPot();

        if (delay > 0) {        //Just in case the value from the pot is negative. We observed a -.2.
            return MAX_WAIT_TIME * delay / getMaxVoltage();
        } else {
            return 0;
        }
    }
   
    /**
     * Turns on specified light on OI.
     * @param lightNum
     */
    public void setLight(int lightNum) {
        turnOffLights();
        try {
            enhancedIO.setDigitalOutput(lightNum, true);
        }
        catch (EnhancedIOException e) {
        }
    }
   
    /**
     * Turns all lights off.
     */
    public void turnOffLights(){
        try {
            enhancedIO.setDigitalOutput(DISTANCE_BUTTON_KEY_LIGHT_CHANNEL, false);
            enhancedIO.setDigitalOutput(DISTANCE_BUTTON_FAR_LIGHT_CHANNEL, false);
            enhancedIO.setDigitalOutput(DISTANCE_BUTTON_FENDER_2PT_LIGHT_CHANNEL, false);
            enhancedIO.setDigitalOutput(DISTANCE_BUTTON_FENDER_NARROW_LIGHT_CHANNEL, false);
            enhancedIO.setDigitalOutput(DISTANCE_BUTTON_REVERSE_LIGHT_CHANNEL, false);
            enhancedIO.setDigitalOutput(DISTANCE_BUTTON_FENDER_LIGHT_CHANNEL, false);
            enhancedIO.setDigitalOutput(DISTANCE_BUTTON_STOP_LIGHT_CHANNEL, false);
        }
        catch (EnhancedIOException e) {
        }
    }
   
    /**
     * 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) {
        }
    }
   
    /**
     * Meant to be called continuously to update the lights on the OI board.
     * Depending on which button has been pressed last (which distance is
     * currently set), that button will be lit.
     */
    public void updateLights(){
        switch(distanceButton){
            case DISTANCE_BUTTON_KEY:
                setLight(DISTANCE_BUTTON_KEY_LIGHT_CHANNEL);
                break;
            case DISTANCE_BUTTON_FAR:
                setLight(DISTANCE_BUTTON_FAR_LIGHT_CHANNEL);
                break;
            case DISTANCE_BUTTON_FENDER_LENGTH:
                setLight(DISTANCE_BUTTON_FENDER_2PT_LIGHT_CHANNEL);
                break;
            case DISTANCE_BUTTON_FENDER_WIDTH:
                setLight(DISTANCE_BUTTON_FENDER_NARROW_LIGHT_CHANNEL);
                break;
            case DISTANCE_BUTTON_REVERSE:
                setLight(DISTANCE_BUTTON_REVERSE_LIGHT_CHANNEL);
                break;
            case DISTANCE_BUTTON_FENDER:
                setLight(DISTANCE_BUTTON_FENDER_LIGHT_CHANNEL);
                break;
            case DISTANCE_BUTTON_STOP:
                setLight(DISTANCE_BUTTON_STOP_LIGHT_CHANNEL);
                break;
            default:
                turnOffLights();
                break;
        }
    }
   
    // For debugging purposes.
    public boolean getDigitalValue(int channel) {
        boolean b = false;
        try{
            b = !enhancedIO.getDigital(channel);
        }
        catch (EnhancedIOException e) {
        }
        return b;
    }
   
    // For debugging purposes.
    public double getAnalogValue(int channel) {
        double b = 0;
        try{
            b = enhancedIO.getAnalogOut(channel);
        }
        catch (EnhancedIOException e) {
        }
        return b;
    }

    public void resetBox(){
        turnOffLights();
        distanceButton = DISTANCE_BUTTON_STOP;
    }
}
TOP

Related Classes of edu.stuy.OI

TOP
Copyright © 2018 www.massapi.com. All rights reserved.
All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.