Java 类edu.wpi.first.wpilibj.Relay 实例源码

/**
 * Creates the instance of VisionSubsystem.
 */
public Vision() {
  logger.trace("Initialize");

  ledRing0 = new Relay(RobotMap.RELAY_LED_RING_0);
  ledRing1 = new Relay(RobotMap.RELAY_LED_RING_1);

  gearVision.table = NetworkTable.getTable("Vision_Gear");
  boilerVision.table = NetworkTable.getTable("Vision_Boiler");

  initPhoneVars(gearVision, DEFAULT_GEAR_TARGET_WIDTH, DEFAULT_GEAR_TARGET_HEIGHT);
  initPhoneVars(boilerVision, DEFAULT_BOILER_TARGET_WIDTH, DEFAULT_BOILER_TARGET_HEIGHT);

  Thread forwardThread = new Thread(this::packetForwardingThread);
  forwardThread.setDaemon(true);
  forwardThread.setName("Packet Forwarding Thread");
  forwardThread.start();

  Thread dataThread = new Thread(this::dataThread);
  dataThread.setDaemon(true);
  dataThread.setName("Vision Data Thread");
  dataThread.start();
}

public void disabledPeriodic() {
    RobotMap.lightRing.set(Relay.Value.kOff);
    Scheduler.getInstance().run();

    recordedID = (String) (oi.index.getSelected());
    recordedAuton = SmartDashboard.getBoolean("Use Recorded Autonomous");

    Aimer.toPositionMode();
    RobotMap.winchMotor.setEncPosition(0);
    RobotMap.winchMotor.setPosition(0);
    RobotMap.winchMotor.set(0);
    DashboardOutput.putPeriodicData();

    isBlue = (DriverStation.getInstance().getAlliance() == Alliance.Blue);

    sendStateToLights(false, false);
}

ShooterControl(Encoder encoder, SpeedController pullBackSpeedController, double speedControllerMaxRpm,
               DigitalInput limitSwitch, DoubleSolenoid gearControl, Servo latchServo,
               Relay angleControl)
{
    m_latchReleaseServo = latchServo;
    m_currentState = SHOOTER_CONTROL_STATE_WAIT;
    m_encoder = encoder;
    m_pullBackSpeedController = pullBackSpeedController;
    m_angleControl = angleControl;
    m_speedControllerMaxRpm = speedControllerMaxRpm;
    m_limitSwitch = limitSwitch;
    m_pullBackEncoderRpm = new EncoderRPM();
    m_pullBackEncoderRpm.Init(m_pullBackSpeedController, m_encoder, (-1)*m_speedControllerMaxRpm, m_speedControllerMaxRpm, 0.05, 100, m_limitSwitch);
    m_releaseFromMidptEncoderRpm = new EncoderRPM();
    m_releaseFromMidptEncoderRpm.Init(m_pullBackSpeedController, m_encoder,(-1)*m_speedControllerMaxRpm/4,m_speedControllerMaxRpm,0.05, 3);
    m_gearControl = gearControl;
    m_latchReleased = false;
    m_gearReleased = false;
    m_isPulledBack = false;
}

@Override
public void tick() {
    double horizontalRotationDegrees = (double) this.getDataKey(CameraSubsystemDataKey.HORIZONTAL_ROTATION_DEGREES);
    double verticalRotationDegrees = (double) this.getDataKey(CameraSubsystemDataKey.VERTICAL_ROTATION_DEGREES);

    double horizontalValue = WarriorMath.map(horizontalDegrees[1], horizontalDegrees[0], horizontalRotationDegrees, 0.0, 1.0);
    double verticalValue = WarriorMath.map(verticalDegrees[1], verticalDegrees[0], verticalRotationDegrees, 0.0, 1.0);

    Relay.Value lightsValue = null;

    if((boolean) this.getDataKey(CameraSubsystemDataKey.LIGHTS) == true) {
        lightsValue = Relay.Value.kOn;
    } else {
        lightsValue = Relay.Value.kOff;
    }

    this._horizontal.set(horizontalValue);
    this._vertical.set(verticalValue);
    this._lights.set(lightsValue);
}

public void acceptNotification(Subject subjectThatCaused)
    {
        if(((BooleanSubject) subjectThatCaused).getValue())
        {
//            if(subjectThatCaused.getType() == JoystickDPadButtonEnum.MANIPULATOR_D_PAD_BUTTON_DOWN)
//            {
//                SmartDashboard.putBoolean("Looking For Hot Goal", true);
//                SmartDashboard.putBoolean("Found Hot Goal", this.checkForHotGoal());
//                SmartDashboard.putBoolean("Looking For Hot Goal", false);
//            }
            if(subjectThatCaused.getType() == JoystickDPadButtonEnum.MANIPULATOR_D_PAD_BUTTON_RIGHT)
            {
                ledState = (ledState == Relay.Value.kOff ? Relay.Value.kOn : Relay.Value.kOff);
            }
        }
    }

/**
 * Constructor for OutputManager.
 *
 * All new output elements need to be added in the constructor as well as
 * having a key value added above.
 */

protected OutputManager() {
    //Add the facade data elements
    outputs.addToIndex(UNKNOWN_INDEX, new NoOutput());
    outputs.addToIndex(RIGHT_DRIVE_SPEED_INDEX, new WsDriveSpeed("Right Drive Speed", 3, 4));
    outputs.addToIndex(LEFT_DRIVE_SPEED_INDEX, new WsDriveSpeed("Left Drive Speed", 1, 2));
    outputs.addToIndex(SHIFTER_INDEX, new WsDoubleSolenoid("Shifter", 1, 1, 2));
    outputs.addToIndex(LIGHT_CANNON_RELAY_INDEX, new WsRelay(1, 5, Relay.Direction.kForward));
    outputs.addToIndex(WINGS_SOLENOID_INDEX, new WsDoubleSolenoid("Wings Solenoid1", 1, 3, 4));
    outputs.addToIndex(LANDING_GEAR_SOLENOID_INDEX, new WsDoubleSolenoid("Landing Gear Solenoid", 1, 7, 8));
    outputs.addToIndex(CATAPAULT_SOLENOID_INDEX, new WsSolenoid("Arm Catapult Solenoid", 1, 5));
    outputs.addToIndex(FRONT_ARM_VICTOR_INDEX, new WsVictor("Front Arm Victor", 5));       
    outputs.addToIndex(BACK_ARM_VICTOR_INDEX, new WsVictor("Back Arm Victor", 6));
    outputs.addToIndex(FRONT_ARM_ROLLER_VICTOR_INDEX, new WsVictor("Front Arm Roller Victor",7));
    outputs.addToIndex(BACK_ARM_ROLLER_VICTOR_INDEX, new WsVictor("Back Arm Roller Victor", 8));
    outputs.addToIndex(LATCH_SOLENOID_INDEX, new WsSolenoid("Latch Solenoid", 1, 6));
    outputs.addToIndex(LEFT_EAR_SERVO_INDEX, new WsServo("Left Ear Servo", 9));
    outputs.addToIndex(RIGHT_EAR_SERVO_INDEX, new WsServo("Right Ear Servo", 10));
    outputs.addToIndex(CAMERA_LED_SPIKE_INDEX, new WsRelay(1, 2, Relay.Direction.kForward));
}

public static void collectorToggle() {

    boolean buttonPressed = Driverstation.operator.getRawButton(5);
    if (buttonPressed && !collectorStatus) {
        shootMotorStatus = !shootMotorStatus;

    }
    if(shootMotorStatus) {
        Collect.set(Relay.Value.kReverse);
        collectorDashboard = true;
    }
    else {
        Collect.set(Relay.Value.kOff);
        collectorDashboard = false;
    }
    collectorStatus = buttonPressed;

}

public static void collectorToggle() {

    boolean buttonPressed = Driverstation.operator.getRawButton(5);
    if (buttonPressed && !collectorStatus) {
        shootMotorStatus = !shootMotorStatus;

    }
    if(shootMotorStatus) {
        Collect.set(Relay.Value.kReverse);
        collectorDashboard = true;
    }
    else {
        Collect.set(Relay.Value.kOff);
        collectorDashboard = false;
    }
    collectorStatus = buttonPressed;

}

/**
 * Set the state of the reverse channel.
 * The center pin (B) on the Relay connector is "reverse".
 * This method will not affect the state of the forward channel.
 * 
 * @param val true if on, false if off
 */
public void setReverse(boolean revVal) {
    Relay.Value currentValue = this.get(),
            newValue = currentValue;

if(currentValue == Relay.Value.kForward) {
    if(revVal)
        newValue = Relay.Value.kOn;
} else if(currentValue == Relay.Value.kOff) {
    if(revVal)
        newValue = Relay.Value.kReverse;
} else if(currentValue == Relay.Value.kOn) {
    if(!revVal)
        newValue = Relay.Value.kForward;
} else if(currentValue == Relay.Value.kReverse) {
    if(!revVal)
        newValue = Relay.Value.kOff;
}

this.set(newValue);
}

public static void Collector() {
    //Collector bring in ball
    if (Driverstation.operator.getRawButton(5)) {
        Collect.set(Relay.Value.kForward);

    } //Collector release ball
    else if (Driverstation.operator.getRawButton(3)) {
        Collect.set(Relay.Value.kReverse);

    } //Off
    else {
        Collect.set(Relay.Value.kOff);
    }
    //Switch to brake mode
    SetBrakes(!Driverstation.operator.getRawButton(2));
}

/**
 * Constructs a new Launcher, with two Talons for winding, a release relay,
 * and a limit switch for winding regulation based on input from a
 * MaxbotixUltrasonic rangefinder.
 */
public Launcher() {
    super("Launcher");

    winderL = new Talon(RobotMap.WIND_LEFT_PORT);
    winderR = new Talon(RobotMap.WIND_RIGHT_PORT);

    releaseMotor = new Relay(RobotMap.RELAY_PORT);

    camera = new AxisCameraM1101();
    rangefinder = new MaxbotixUltrasonic(RobotMap.ULTRASONIC_RANGEFINDER);

    limitSwitch = new DigitalInput(RobotMap.LIMIT_SWITCH);
    counter = new Counter(limitSwitch);

    counterInit(counter);
}

/**
 * Set the state of the forward channel.
 * The innermost pin (A) on the Relay connector is "forward".
 * This method will not affect the state of the reverse channel.
 * 
 * @param val true if on, false if off
 */
public void setForward(boolean fwdVal) {
    Relay.Value currentValue = this.get(),
                newValue = currentValue;

    if(currentValue == Relay.Value.kForward) {
        if(!fwdVal)
            newValue = Relay.Value.kOff;
    } else if(currentValue == Relay.Value.kOff) {
        if(fwdVal)
            newValue = Relay.Value.kForward;
    } else if(currentValue == Relay.Value.kOn) {
        if(!fwdVal)
            newValue = Relay.Value.kReverse;
    } else if(currentValue == Relay.Value.kReverse) {
        if(fwdVal)
            newValue = Relay.Value.kOn;
    }

    this.set(newValue);
}

public void SetBumpersColor(){

    double ColorValue = 0;
    try {
        ColorValue = DriverStation.getInstance().getEnhancedIO().getAnalogIn(6);
    } catch (DriverStationEnhancedIO.EnhancedIOException ex) {
        ex.printStackTrace();
    }

    if(ColorValue > 1.5){

        ColorLedsRelay.set(Relay.Value.kForward);
    }
    else{
        ColorLedsRelay.set(Relay.Value.kReverse);
    }

}

@Test
public void test() {

    // pretext
    Assert.assertEquals(doubleSolenoide.isDefaultState(), true);

    // test toggle
    doubleSolenoide.toggle();
    Assert.assertEquals(relay1, Relay.Value.kForward);
    Assert.assertEquals(relay2, Relay.Value.kOff);

    doubleSolenoide.toggle();
    Assert.assertEquals(relay1, Relay.Value.kOff);
    Assert.assertEquals(relay2, Relay.Value.kForward);

}

public static void init() {
      // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    compressor = new Compressor();

      driveTrainLeftFront = new CANTalon(1);
      LiveWindow.addActuator("DriveTrain", "LeftFront", driveTrainLeftFront);
      driveTrainRightFront = new CANTalon(3);
      LiveWindow.addActuator("DriveTrain", "RightFront", driveTrainRightFront);
      driveTrainLeftRear = new CANTalon(2);
      driveTrainLeftRear.changeControlMode(TalonControlMode.Follower);
      driveTrainLeftRear.set(driveTrainLeftFront.getDeviceID());
      LiveWindow.addActuator("DriveTrain", "LeftRear", driveTrainLeftRear);
      driveTrainRightRear = new CANTalon(4);
      driveTrainRightRear.changeControlMode(TalonControlMode.Follower);
      driveTrainRightRear.set(driveTrainRightFront.getDeviceID());
      driveTrainRightRear.reverseOutput(false);
      LiveWindow.addActuator("DriveTrain", "RightRear", driveTrainRightRear);

      driveTrainLeftFront.setInverted(true);
      driveTrainRightFront.setInverted(true);
      driveTrainLeftRear.setInverted(true);
      driveTrainRightRear.setInverted(true);

      driveTrainRobotDrive41 = new RobotDrive(driveTrainRightFront, driveTrainLeftFront);
driveTrainRobotDrive41.setSafetyEnabled(true);
driveTrainRobotDrive41.setExpiration(0.1);
driveTrainRobotDrive41.setSensitivity(0.5);
driveTrainRobotDrive41.setMaxOutput(1.0);

      climbMotor = new CANTalon(5);
      LiveWindow.addActuator("Climbing", "Motor", climbMotor);

      lightsLightEnable = new Relay(0);
      LiveWindow.addActuator("Lights", "LightEnable", lightsLightEnable);

      gearIntakeRamp = new DoubleSolenoid(1, 0);
      LiveWindow.addActuator("GearIntake", "IntakeRamp", gearIntakeRamp);

      // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
  }

public static void init() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    motorRelayMotorRelay1 = new Relay(0);
    LiveWindow.addActuator("MotorRelay", "MotorRelay1", motorRelayMotorRelay1);

    lFront = new CANTalon(1);
    LiveWindow.addActuator("Wheels", "Speed Controller 1", (CANTalon) lFront);

    rFront = new CANTalon(3);
    LiveWindow.addActuator("Wheels", "Speed Controller 2", (CANTalon) rFront);

    lRear = new CANTalon(2);
    LiveWindow.addActuator("Wheels", "Speed Controller 3", (CANTalon) lRear);

    rRear = new CANTalon(4);
    LiveWindow.addActuator("Wheels", "Speed Controller 4", (CANTalon) rRear);

    driveSystem = new RobotDrive(lFront, lRear,
          rFront, rRear);

    driveSystem.setSafetyEnabled(true);
    driveSystem.setExpiration(0.1);
    driveSystem.setSensitivity(0.5);
    driveSystem.setMaxOutput(1.0);

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
}

/**
 * Turns the LED ring for the retroreflective tape on or off.
 * 
 * @param desiredState Whether the LED ring should be on or not.
 */
public void setLedRingOn(LedState desiredState) {
  logger.trace(String.format("Setting LED ring %s, current state %b", desiredState.toString(),
      isLedRingOn()));
  boolean on;
  if (desiredState == LedState.TOGGLE) {
    on = !isLedRingOn();
  } else if (desiredState == LedState.ON) {
    on = true;
  } else {
    on = false;
  }
  ledRing0.set(on ? Relay.Value.kReverse : Relay.Value.kOff);
  ledRing1.set(on ? Relay.Value.kForward : Relay.Value.kOff);
}

public VisionTest() {
        UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(); // cam0 by default
        camera.setResolution(IMG_WIDTH, IMG_HEIGHT);
        camera.setBrightness(0);
//      camera.setExposureManual(100);
        camera.setExposureAuto();

        CvSource cs= CameraServer.getInstance().putVideo("name", IMG_WIDTH, IMG_HEIGHT);

        visionThread = new VisionThread(camera, new GripPipeline(), pipeline -> {
            Mat IMG_MOD = pipeline.hslThresholdOutput();
            if (!pipeline.filterContoursOutput().isEmpty()) {
                //Rect recCombine = Imgproc.boundingRect(pipeline.filterContoursOutput().get(0));
                Rect recCombine = computeBoundingRectangle(pipeline.filterContoursOutput());
                if (recCombine == null) {
                    targetDetected = false;
                    return;
                }
                targetDetected = true;
                computeCoords(recCombine);
                synchronized (imgLock) {
                    centerX = recCombine.x + (recCombine.width / 2);
                }

                Imgproc.rectangle(IMG_MOD, new Point(recCombine.x, recCombine.y),new Point(recCombine.x + recCombine.width,recCombine.y + recCombine.height), new Scalar(255, 20, 0), 5);

            } else {
                targetDetected = false;
            }

            cs.putFrame(IMG_MOD);
        });

        visionThread.start();
        Relay relay = new Relay(RobotMap.RELAY_CHANNEL, Direction.kForward);
        relay.set(Relay.Value.kOn);
        //this.processImage();
    }

public void autonomousInit() {
     RobotMap.lightRing.set(Relay.Value.kOn);
    RobotMap.winchMotor.enableBrakeMode(true);
    if (recordedAuton) {
        oi.gamepad.loadVirtualGamepad(recordedID);
        oi.gamepad.startVirtualGamepad();
    } else {
   // schedule the autonomous command (example) 
autonomousCommand = (CommandGroup) new ConstructedAutonomous(ParseInput.takeInput((String)auto_Movement.getSelected(), 
        (boolean)auto_Reverse.getSelected(), (int)auto_isHighGoal.getSelected()));
if(autonomousCommand != null)
    autonomousCommand.start();
    }
 }

public void teleopInit() {
    RobotMap.winchMotor.enableBrakeMode(true);
    RobotMap.lightRing.set(Relay.Value.kOn);

    // This makes sure that the autonomous stops running when
    // teleop starts running. If you want the autonomous to 
    // continue until interrupted by another command, remove
    // this line or comment it out.
    //if (autonomousCommand != null) autonomousCommand.cancel();
}

public void reset() {
    for (int i = 0; i < motors.length; i++) {
        if (motors[i] == null) {
            continue;
        }
        if (motors[i] instanceof CANTalon) {
            ((CANTalon) motors[i]).delete();
        } else if (motors[i] instanceof Victor) {
            ((Victor) motors[i]).free();
        }
    }
    motors = new SpeedController[64];

    for (int i = 0; i < solenoids.length; i++) {
        if (solenoids[i] == null) {
            continue;
        }
        solenoids[i].free();
    }
    solenoids = new Solenoid[64];

    for (int i = 0; i < relays.length; i++) {
        if (relays[i] == null) {
            continue;
        }
        relays[i].free();
    }
    relays = new Relay[64];

    for (int i = 0; i < analogInputs.length; i++) {
        if (analogInputs[i] == null) {
            continue;
        }
        analogInputs[i].free();
    }
    analogInputs = new AnalogInput[64];

    if (compressor != null)
    compressor.free();
}

/**
 * Stop everything.
 */
private static void done ()
{
    // after autonomous is disabled, the state machine will stop.
    // this code will run but once.
    autonomousEnabled = false;

    // stop printing debug statements.
    debug = false;

    // turn off all motors.
    Hardware.transmission.controls(0.0, 0.0);

    // including the arm.
    // end any surviving arm motions.
    armState = ArmState.DONE;
    Hardware.armMotor.set(0.0);

    // reset delay timer
    Hardware.delayTimer.stop();
    Hardware.delayTimer.reset();

    // turn off ringlight.
    Hardware.ringLightRelay.set(Relay.Value.kOff);

    Hardware.transmission
            .setDebugState(debugStateValues.DEBUG_NONE);
}

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
public void robotInit()
{
    // PWM's
    mTestMotor1 = new Talon(0);
    mTestMotor2 = new Jaguar(1);
    mServo = new Servo(2);

    // Digital IO
    mDigitalOutput = new DigitalOutput(0);
    mRightEncoder = new Encoder(4, 5);
    mLeftEncoder = new Encoder(1, 2);
    mUltrasonic = new Ultrasonic(7, 6);

    // Analog IO
    mAnalogGryo = new AnalogGyro(0);
    mPotentiometer = new AnalogPotentiometer(1);

    // Solenoid
    mSolenoid = new Solenoid(0);

    // Relay
    mRelay = new Relay(0);

    // Joysticks
    mJoystick1 = new Joystick(0);
    mJoystick2 = new XboxController(1);

    // SPI
    mSpiGryo = new ADXRS450_Gyro();

    // Utilities
    mTimer = new Timer();
    mPDP = new PowerDistributionPanel();

    Preferences.getInstance().putDouble("Motor One Speed", .5);
}

public LightManager(IOperatorJoystick aOperatorJoystick, ISnobotActor aSnobotActor, VisionManager aVisionManager, Relay aRelay, Relay anotherRelay)
{
    mGreenRelay = aRelay;
    mBlueRelay = anotherRelay;

    mOperatorJoystick = aOperatorJoystick;
    mSnobotActor = aSnobotActor;
    mVisionManager = aVisionManager;
    mFlashCounter = 0;
}

public VisionTarget(FrcVision.ImageProvider imageProvider)
{
    if (debugEnabled)
    {
        dbgTrace = new TrcDbgTrace(
                moduleName,
                false,
                TrcDbgTrace.TraceLevel.API,
                TrcDbgTrace.MsgLevel.INFO);
    }

    ringLightPower = new Relay(
            RobotInfo.RELAY_RINGLIGHT_POWER, Relay.Direction.kForward);
    colorThresholds = new Range[3];
    colorThresholds[0] = new Range(101, 64);
    colorThresholds[1] = new Range(88, 255);
    colorThresholds[2] = new Range(134, 255);
    filterCriteria = new ParticleFilterCriteria2[1];
    filterCriteria[0] = new ParticleFilterCriteria2(
            MeasurementType.MT_AREA_BY_IMAGE_AREA,
            AREA_MINIMUM, 100.0,
            0, 0);
    filterOptions = new ParticleFilterOptions2(0, 0, 1, 1);
    targetReport = new TargetReport();
    targetReport.rect = new NIVision.Rect();
    visionTask = new FrcVision(
            imageProvider,
            ImageType.IMAGE_RGB,
            ColorMode.HSV,
            colorThresholds,
            false,
            filterCriteria,
            filterOptions);
}

public void setRingLightPowerOn(boolean on)
{
    final String funcName = "setRightLightPowerOn";
    if (debugEnabled)
    {
        dbgTrace.traceEnter(
                funcName, TrcDbgTrace.TraceLevel.API,
                "on=%s", Boolean.toString(on));
        dbgTrace.traceExit(
                funcName, TrcDbgTrace.TraceLevel.API);
    }
    ringLightPower.set(on? Relay.Value.kOn: Relay.Value.kOff);
}

public void ChangeAngle(boolean forward)
{
    if (forward)
    {
       m_angleControl.set(Relay.Value.kForward);
    }
    else
    {
        m_angleControl.set(Relay.Value.kReverse);
    }
}

/**
  * This function is run when the robot is first started up and should be
  * used for any initialization code.
  */
 public void robotInit() {
     chooser = new SendableChooser();
     chooser.addDefault("Default Auto", defaultAuto);
     chooser.addObject("My Auto", customAuto);
     SmartDashboard.putData("Auto choices", chooser);

     // create and reset relay
     myRelay = new Relay(RELAY_CHANNEL,Relay.Direction.kForward);
    myRelay.set(Relay.Value.kOff);
}

public static void setCameraLed(boolean state) {

    if (state == true) {
        cameraLedRelay.set(Relay.Value.kOn);
        ledState = true;
    }
    else {
        cameraLedRelay.set(Relay.Value.kOff);
        ledState = false;
    }

}

public static void setCameraLed(boolean state) {

    if (state == true) {
        cameraLedRelay.set(Relay.Value.kOn);
        ledState = true;
    }
    else {
        cameraLedRelay.set(Relay.Value.kOff);
        ledState = false;
    }

}

public CameraSubsystem(Servo _horizontal, Servo _vertical, Relay _lights) {
    this._horizontal = _horizontal;
    this._vertical = _vertical;
    this._lights = _lights;

    this.setDataKey(CameraSubsystemDataKey.HORIZONTAL_ROTATION_DEGREES, 90.0d);
    this.setDataKey(CameraSubsystemDataKey.VERTICAL_ROTATION_DEGREES, 0.0d);
    this.setDataKey(CameraSubsystemDataKey.LIGHTS, false);
}

public IntakeSubsystem(SpeedController _controller, LimitSwitch limitSwitch, Relay indicatorRelay) {
    super(_controller);

    this._limitSwitch = limitSwitch;
    this._indicatorRelay = indicatorRelay;

    this._mode = IntakeSubsystemMode.STOPPED;
}

public IntakeSubsystem(SpeedControllerSubsystemType type, final int speedControllerChannel, final int limitSwitchChannel, final int indicatorRelayChannel) {
    super(type, speedControllerChannel);

    this._limitSwitch = new LimitSwitch(limitSwitchChannel);
    this._indicatorRelay = new Relay(indicatorRelayChannel);

    this._mode = IntakeSubsystemMode.STOPPED;
}

public void toggle() {
    if (isUp) {
        isUp = false;
        relay.set(Relay.Value.kReverse);
    } else {
        isUp = true;
        relay.set(Relay.Value.kForward);
    }
}

public void toggle() {
    if (isOn) {
        relay.set(Relay.Value.kOff);
        isOn = false;
    } else {
        relay.set(Relay.Value.kReverse);
        isOn = true;
    }
}

public void update() {
    if(!pressureSwitch.get() && enabled) {
        spike.set(Relay.Value.kForward);
    } else {
        spike.set(Relay.Value.kOff);
    }
    SmartDashboard.putBoolean("Pressure Input Switch" , pressureSwitch.get());
    SmartDashboard.putBoolean("Enabled" , enabled);
}

public void tickTeleop() {
    compressor.set(cutoff.get() ? Relay.Value.kOff : Relay.Value.kForward);
    pressure = (analogPressure.getAverageVoltage() - .854) * 40.9276;
    SmartDashboard.putNumber("Pressure", pressure);

    SmartDashboard.putBoolean("Ready to Fire", pressure > 80);
}

CompressorControl(int chanCompressor, int modComp, int modPS, int chanPS) {
    compressor = new Relay(modComp, chanCompressor, Relay.Direction.kForward);
    pressureSwitch = new DigitalInput(modPS, chanPS);

    // Start background task
    Thread t = new Thread(this);
    t.start();
}

public void runner() {
   // while(true) {
        if(pressureSwitch.get() == false) {
            // low pressure, turn on compressor
            //System.out.println("Compressor on");
            compressor.set(Relay.Value.kOn);
        }
        else {
            // high pressure, turn off compressor
            //System.out.println("Compressor off");
            compressor.set(Relay.Value.kOff);
        }
  //  }
}

public void init()
{
    cc = new CriteriaCollection();
    cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_AREA, AREA_MINIMUM, 65535, false);

    ledState = Relay.Value.kOff;
}