Java 类edu.wpi.first.wpilibj.smartdashboard.SmartDashboard 实例源码

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
@Override
public void robotInit() {
    chassis = new Chassis();
    intake = new Intake();
    wheelintake = new wheelIntake();

    oi = new OI();
    chooser.addDefault("Default Auto", new DriveWithJoystick());
    // chooser.addObject("My Auto", new MyAutoCommand());
    SmartDashboard.putData("Auto mode", chooser);

    chassis.publishToSmartDashboard();


}

/**
 * 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<Command>();
    chassis = new Chassis();
    intake = new Intake();
    winch = new Winch();
//  shooter = new Shooter();

    oi = new OI();
    imu = new ADIS16448_IMU(ADIS16448_IMU.Axis.kX);
    pdp = new PowerDistributionPanel();

    chooser.addDefault("Drive Straight", new DriveStraight(73, 0.5));
    //chooser.addObject("Left Gear Curve", new LeftGearCurve());
    chooser.addObject("Left Gear Turn", new LeftGearTurn());
    //chooser.addObject("Right Gear Curve", new RightGearCurve());
    chooser.addObject("Right Gear Turn", new RightGearTurn());
    chooser.addObject("Middle Gear", new StraightGear());
    chooser.addObject("Turn in place", new TurnDegrees(60, .2));
    SmartDashboard.putData("Autonomous Chooser", chooser);
}

@Override
public void sendDataToSmartDashboard() {
  // phone handles vision data for us
  SmartDashboard.putBoolean("LED_On", isLedRingOn());

  boolean gearLiftPhone = false;
  boolean boilerPhone = false;
  ConnectionInfo[] connections = NetworkTablesJNI.getConnections();
  for (ConnectionInfo connInfo : connections) {
    if (System.currentTimeMillis() - connInfo.last_update < 100) {
      if (connInfo.remote_id.equals("Android_GEAR_LIFT")) {
        gearLiftPhone = true;
      } else if (connInfo.remote_id.equals("Android_BOILER")) {
        boilerPhone = true;
      }
    }
  }

  SmartDashboard.putBoolean("VisionGearLift", gearLiftPhone);
  SmartDashboard.putBoolean("VisionGearLift_data", isGearVisionDataValid());
  SmartDashboard.putBoolean("VisionBoiler", boilerPhone);
  SmartDashboard.putBoolean("VisionBoiler_data", isBoilerVisionDataValid());
}

@Override
public void log() {
    SmartDashboard.putNumber("DriveTrain: distance", getDistance());
    SmartDashboard.putNumber("DriveTrain: left distance", leftEncoder.getDistance());
    SmartDashboard.putNumber("DriveTrain: left velocity", leftEncoder.getRate());
    SmartDashboard.putNumber("DriveTrain: right distance", rightEncoder.getDistance());
    SmartDashboard.putNumber("DriveTrain: right velocity", rightEncoder.getRate());
    SmartDashboard.putNumber("DriveTrain: front right current", frontRight.getOutputCurrent());
    SmartDashboard.putNumber("DriveTrain: front right current pdp", Robot.pdp.getCurrent(12));
    SmartDashboard.putNumber("DriveTrain: front left  current", frontLeft.getOutputCurrent());
    SmartDashboard.putNumber("DriveTrain: front left  current pdp", Robot.pdp.getCurrent(10));
    SmartDashboard.putNumber("DriveTrain: back  right current", backRight.getOutputCurrent());
    SmartDashboard.putNumber("DriveTrain: back  right current pdp", Robot.pdp.getCurrent(13));
    SmartDashboard.putNumber("DriveTrain: back  left  current", backLeft.getOutputCurrent());
    SmartDashboard.putNumber("DriveTrain: back  left  current pdp", Robot.pdp.getCurrent(11));
}

/**
 * Sends shooter data to smart dashboard.
 */
public void sendDataToSmartDashboard() {
  if (RobotMap.IS_ROADKILL) {
    return;
  }
  SmartDashboard.putNumber("Shooter_Master_Talon_Power",
      shooterMaster.getOutputCurrent() * shooterMaster.getOutputVoltage());
  SmartDashboard.putNumber("Shooter_Slave_Talon_Power",
      shooterSlave.getOutputCurrent() * shooterSlave.getOutputVoltage());
  SmartDashboard.putNumber("Shooter_RPM_Requested", requestedRpm);
  SmartDashboard.putNumber("Shooter_RPM_Real", getShooterRpm());
  SmartDashboard.putNumber("Shooter_PID_error", shooterMaster.getClosedLoopError());

  SmartDashboard.putBoolean("Shooter_Fault", shooterFault);
  SmartDashboard.putBoolean("Shooter_Master_Ok", shooterMaster.getFaultHardwareFailure() == 0);
  SmartDashboard.putBoolean("Shooter_Master_Temp_Ok",
      shooterMaster.getStickyFaultOverTemp() == 0);
  SmartDashboard.putBoolean("Shooter_Slave_Ok", shooterSlave.getFaultHardwareFailure() == 0);
  SmartDashboard.putBoolean("Shooter_Slave_Temp_Ok", shooterSlave.getStickyFaultOverTemp() == 0);

  SmartDashboard.putBoolean("Shooter_Master_Present", shooterMaster.isAlive());
  SmartDashboard.putBoolean("Shooter_Slave_Present", shooterSlave.isAlive());
}

protected void execute() {
    //figure out how close is "close enough" because it's unlikely we'll ever get to 2.5 on the dot.  Figure this out through testing

    Robot.driveTrain.setTankDriveCommand(.6, .6);
 /* if (Robot.pixyCamera.transaction(sendBuffer, sendBuffer.length, buffer, 1))
    {
        pixyValue = buffer[0]  & 0xFF;
    } */
    SmartDashboard.putNumber("pixyXAutonAimGear", Robot.pixyValue);
    if ((int) Robot.pixyValue > 128 && (int) Robot.pixyValue != 255) //132
            {
        Robot.driveTrain.setTankDriveCommand(.3, .6);
        SmartDashboard.putString("PixyImage", "turning right");
    }
    else if ((int) Robot.pixyValue < 126){
Robot.driveTrain.setTankDriveCommand(.6, .3);
SmartDashboard.putString("PixyImage", "turning left");//123
    }
    else if (Robot.pixyValue == 255)
        SmartDashboard.putString("PixyImage", "no image");

 }

/**
 * Shifts the gearboxes up or down.
 * 
 * @param shiftType whether to shift up or down
 */
public void shift(ShiftType shiftType) {
  logger.info(String.format("Shifting, type=%s, shifter state=%s", shiftType.toString(),
      shiftingSolenoid.get().toString()));
  if (pcmPresent) {
    if (shiftType == ShiftType.TOGGLE) {
      if (shiftingSolenoid.get() == DoubleSolenoid.Value.kReverse) {
        shiftingSolenoid.set(DoubleSolenoid.Value.kForward);
        SmartDashboard.putBoolean("Drive_Shift", true);
      } else {
        shiftingSolenoid.set(DoubleSolenoid.Value.kReverse);
        SmartDashboard.putBoolean("Drive_Shift", false);
      }
    } else if (shiftType == ShiftType.UP) {
      shiftingSolenoid.set(DoubleSolenoid.Value.kForward);
      SmartDashboard.putBoolean("Drive_Shift", true);
    } else {
      shiftingSolenoid.set(DoubleSolenoid.Value.kReverse);
      SmartDashboard.putBoolean("Drive_Shift", false);
    }
  }
}

public void robotPeriodic() {
    SmartDashboard.putNumber("Pixy X value", pixyValue  );
    //SmartDashboard.putBoolean("IsIngesting", isIngesting);

    SmartDashboard.putNumber("Right Encoder", DriveEncoders.getRawRightValue());
    SmartDashboard.putNumber("Left Encoder", DriveEncoders.getRawLeftValue());
    SmartDashboard.putNumber("Encoder Differences", DriveEncoders.getRawEncDifference());

    SmartDashboard.putNumber("Accelerometer", NavX.ahrs.getWorldLinearAccelY());
    SmartDashboard.putBoolean("IMU_TP_Connected", NavX.ahrs.isConnected());
    SmartDashboard.putNumber("IMU_Yaw", NavX.ahrs.getYaw());

    SmartDashboard.putNumber("Left Encoder Value: ", RobotMap.driveTrainLeftFront.getEncPosition());
    SmartDashboard.putNumber("Right Encoder Value: ", RobotMap.driveTrainRightFront.getEncPosition());
    //SmartDashboard.putNumber("Compressor", RobotMap.compressor.getCompressorCurrent());   
    SmartDashboard.putNumber("Left Trigger: ", Robot.lTrigger);
    SmartDashboard.putNumber("Right Trigger: ", Robot.rTrigger);
    //SmartDashboard.putBoolean("Door Status", doorsOpen);
    SmartDashboard.putNumber("camera", Robot.camera);

}

protected void execute() {
    System.err.println("Execute turn");
    Robot.driveTrain.tankDrive(-_leftSpeed, _rightSpeed);//negative sign for turning
    SmartDashboard.putNumber("distance traveled",
            Math.max(Robot.driveTrain.getLeftDistance(), Robot.driveTrain.getRightDistance()));
    SmartDashboard.putNumber("Distance", _distanceL);

    // theoretically, this should print out current velocity of both wheels
    SmartDashboard.putNumber("LeftEncVelocity", Robot.driveTrain.getLeftSpeedEnc());
    SmartDashboard.putNumber("RightEncVelocity", Robot.driveTrain.getRightSpeedEnc());

    SmartDashboard.putNumber("LeftSpeed", Robot.driveTrain.getLeftSpeed());
    SmartDashboard.putNumber("RightSpeed", Robot.driveTrain.getRightSpeed());       

    if (Math.abs(Robot.driveTrain.getLeftDistance()) >= Math.abs(_distanceL)) {
        System.err.println("Done left!");
        _leftSpeed = 0;
    }
    if (Math.abs(Robot.driveTrain.getRightDistance()) >= Math.abs(_distanceR)) {
        System.err.println("Done right!");
        _rightSpeed = 0;
    }

}

public static bbShuffle getInstance()
{
    if (bs == null)
    {
        bs = new bbShuffle();
        if(bs == null)
        {
            SmartDashboard.putBoolean("BS Exists?", false);
            System.out.println("bbShuffle can't make itself. Fix it pls");
            return null;
        }
        SmartDashboard.putBoolean("BS Exists?", true);
        System.out.println("bbShuffle created itself");
        return bs;
    }
    SmartDashboard.putBoolean("BS Exists?", true);
    return bs;
}

public OI() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS

    logitech = new Joystick(0);

    shooterbutton = new JoystickButton(logitech, 1);
    shooterbutton.whileHeld(new shoot());


    // SmartDashboard Buttons
    SmartDashboard.putData("Autonomous Command", new AutonomousCommand());
    SmartDashboard.putData("shoot", new shoot());

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    shootBackwardsButton = new JoystickButton(logitech, 2);
    shootBackwardsButton.whileHeld(new ShootReverse());

    LiftUPButton = new JoystickButton(logitech, 3);
    LiftReservseButton = new JoystickButton(logitech, 4);

    LiftUPButton.whileHeld(new LiftUP());
    LiftReservseButton.whileHeld(new LiftReverse());
}

public OI() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS

    xBoxController = new Joystick(0);

    aButton = new JoystickButton(xBoxController, 1);
    bButton = new JoystickButton(xBoxController, 2);
    xButton = new JoystickButton(xBoxController, 3);
    aButton.whenPressed(new RelayOn());
    //b button operated by default command only?
    bButton.whenPressed(new AllForward());
    xButton.whenPressed(new MotorPositionCheck());


    // SmartDashboard Buttons
    SmartDashboard.putData("Autonomous Command", new AutonomousCommand());
    SmartDashboard.putData("RelayOn", new RelayOn());
    SmartDashboard.putData("AllForward", new AllForward());
    SmartDashboard.putData("MotorPositionCheck", new MotorPositionCheck());
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
}

@Override
public void teleopInit() {
    VisionData.getNt().putBoolean("running", false);

    bumper.setTeam();

    gyroPID.resetGyro();

    // Zeroes joysticks at the beginning
    stickCalLeft = controls.getLeftDrive();
    stickCalRight = controls.getRightDrive();

    // Test component speeds with SmartDashboard
    SmartDashboard.putNumber("Shooter Speed", shooterSpeed);
    SmartDashboard.putNumber("Indexer Speed", indexerSpeed);
    SmartDashboard.putNumber("Intake Speed", intakeSpeed);
}

@Override
public void robotPeriodic() {
  try {
    // measure total cycle time, time we take during robotPeriodic, and WPIlib overhead
    final long start = System.nanoTime();
    logger.trace("robotPeriodic()");
    Scheduler.getInstance().run();

    long currentNanos = System.nanoTime();

    if (currentNanos - nanosAtLastUpdate > RobotMap.SMARTDASHBOARD_UPDATE_RATE * 1000000000) {
      allSubsystems.forEach(this::tryToSendDataToSmartDashboard);
      nanosAtLastUpdate = currentNanos;
    }

    SmartDashboard.putNumber("cycleMillis", (currentNanos - prevNanos) / 1000000.0);
    SmartDashboard.putNumber("ourTime", (currentNanos - start) / 1000000.0);
    prevNanos = currentNanos;
  } catch (Throwable ex) {
    logger.error("robotPeriodic error", ex);
    ex.printStackTrace();
  }
}

public void publishToSmartDashboard(){
    chassis.publishToSmartDashboard();
    winch.publishToSmartDashboard();
    //shooter.publishToSmartDashboard();
    intake.publishToSmartDashboard();
    SmartDashboard.putNumber("Robot Angle", imu.getAngleX()/4);
    imu.updateTable();

}

public void disabledPeriodic() {
    Scheduler.getInstance().run();

    SmartDashboard.putString("Selected Auto", chooser.getSelected().getName());
    publishToSmartDashboard();
    //SmartDashboard.putString("Selected Autonomous", chooser.getSelected().getName());

}

public static void stri(String label, String data, boolean log)
{
    SmartDashboard.putString(label, data);
    if(log)
    {
        BulldogLogger.getInstance().log(label, data);
    }
}

/**
 * Sends diagnostics to SmartDashboard.
 */
public void sendDataToSmartDashboard() {
  SmartDashboard.putBoolean("Feeder_Present", feederTalon != null && feederTalon.isAlive());
  if (feederTalon != null) {
    SmartDashboard.putNumber("Feeder_Power",
        feederTalon.getOutputCurrent() * feederTalon.getOutputVoltage());

    SmartDashboard.putBoolean("Feeder_Ok", feederTalon.getFaultHardwareFailure() == 0);
    SmartDashboard.putBoolean("Feeder_Temp_Ok", feederTalon.getStickyFaultOverTemp() == 0);
  }
}

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
// @Override
public void robotInit() {

    System.out.println("1");

    RobotMap.init();

    drivetrain = new Drivetrain();
    climber = new Climber();
    // fuel = new Fuel();
    gear = new Gear();

    oi = new OI();

    // initializes camera server
    server = CameraServer.getInstance();
    cameraInit();

    // initializes auto chooser
    autoChooser = new SendableChooser();
    autoChooser.addDefault("Middle Hook", new AutoMiddleHook());
    autoChooser.addObject("Loading Station Hook", new AutoLeftHook());
    autoChooser.addObject("Boiler Side Hook", new AutoRightHook());
    SmartDashboard.putData("Auto mode", autoChooser);
    // auto delay
    SmartDashboard.putNumber("Auto delay", 0);

    // resets all sensors
    resetAllSensors();
}

public Shooter() {
    super();
    System.out.println("shooter sub system init");
    threadInit(() -> {
        SmartDashboard.putNumber("shooter l speed:", leftShooter.get());
        SmartDashboard.putNumber("shooter r speed:", rightShooter.get());
        SmartDashboard.putNumber("shooter set @", ((-Robot.oi.getThrottle() + 1) / 2) * 0.7 + 0.3);
    });
}

public DriveToPeg(){        
    double distance = .2;

    requires(Robot.driveBase);
    double kP = -.4;
    double kI = 1;
    double kD = 5;

    pid = new PIDController(kP, kI, kD, new PIDSource() {
        PIDSourceType m_sourceType = PIDSourceType.kDisplacement;

        @Override
        public double pidGet() {
            return Robot.driveBase.getDistanceAhead();
        }

        @Override
        public void setPIDSourceType(PIDSourceType pidSource) {
            m_sourceType = pidSource;
        }

        @Override
        public PIDSourceType getPIDSourceType() {
            return m_sourceType;
        }
    }, new PIDOutput() {
        @Override
        public void pidWrite(double d) {
            Robot.driveBase.driveForward(d);
            System.out.println(d);
        }
    });
    pid.setAbsoluteTolerance(0.01);
    pid.setSetpoint(distance);
    pid.setOutputRange(0, .35);

    SmartDashboard.putData("driveToPeg", pid);
}

public USensor() {
    leftPulse = new DigitalOutput(RobotMap.LEFT_PULSE);
    left = new Counter(RobotMap.LEFT_ECHO);
    left.setMaxPeriod(1);
    left.setSemiPeriodMode(true);
    left.reset();

    rightPulse = new DigitalOutput(RobotMap.RIGHT_PULSE);
    right = new Counter(RobotMap.RIGHT_ECHO);
    right.setMaxPeriod(1);
    right.setSemiPeriodMode(true);
    right.reset();

    threadInit(() -> {
        leftPulse.pulse(RobotMap.US_PULSE);
        rightPulse.pulse(RobotMap.US_PULSE);

        do {
            try {
                Thread.sleep(1);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
        } while (left.get() < 2 || right.get() < 2);

        leftDistant = left.getPeriod() * 0.5 * RobotMap.SPEED_OF_SOUND;
        rightDistant = right.getPeriod() * 0.5 * RobotMap.SPEED_OF_SOUND;

        SmartDashboard.putNumber("left dis", leftDistant);
        SmartDashboard.putNumber("right dis", rightDistant);

        left.reset();
        right.reset();
    });
}

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
@Override
public void robotInit() {
    oi = new OI();
    chooser.addDefault("Default Auto", new ExampleCommand());
    // chooser.addObject("My Auto", new MyAutoCommand());
    SmartDashboard.putData("Auto mode", chooser);
}

/**
 * This function is run when the robot is first started up and should be used for any initialization code.
 */
@Override
public void robotInit() {
    drive = new Drive();
    gearManipulator = new GearManipulator();
    intake = new Intake();
    shooter = new Shooter();
    storage = new Storage();
    climber = new Climber();
    elevator = new Elevator();
    vision = new Vision();
    visionProcessing = new LiveUsbCamera();
    oi = new OI();
    Robot.gearManipulator.gearManipulatorUp();
    chooser.addDefault("Center Gear Auto", new CenterGearAutonomous());
    chooser.addObject("Base Line Autonomous", new BaseLineAutonomous());
    chooser.addObject("Boiler side Blue auto", new BoilerSideBlueAuto());
    chooser.addObject("Boiler side Red auto", new BoilerSideRedAuto());
    chooser.addObject("Do Nothing Autonomous", null);

    double speed = Preferences.getInstance().getDouble(RobotMap.centerGearAutoSpeed, 0);
    double distance = Preferences.getInstance().getDouble(RobotMap.centerGearAutoDistance, 0);

    SmartDashboard.putData("Auto Mode", chooser);
    // xboxLeftTrigger.whileActive(new ClimberTriggerOn());
    xboxRightTrigger.whileActive(new RunShooter());

    visionProcessing.runUsbCamera();
}

/**
 * This function is called periodically during test mode
 */
public void testPeriodic() {
    LiveWindow.run();
    Robot.driveBase.EnableDriveBase();
    Robot.driveBase.DriveAutonomous();
    SmartDashboard.putNumber("AV Distance", RobotMap.AverageDistance);
}

/**
     * calculates amount to turn and move forward to align to target
     * STOPS ROBOT IF TARGET LOST OR IN RANGE
     *
     * @return true if pids ran successfully, false if vision done or interrupted
     */
    public boolean autoAlign() {
        System.out.println("Lost Target " + tracker.lostTarget());
        System.out.println("VisionData running " + VisionData.visionRunning());
        System.out.println("Too close = " + (areaPID.getInput() > areaCap));
        System.out.println("On target " + areaPID.getController().onTarget());
        System.out.println("------------");

        // stop if no vision, no target, too close, or at destination
        tracker.addTargetFound(VisionData.foundTarget());

        if (tracker.lostTarget() || !VisionData.visionRunning() || areaPID.getInput() > areaCap || areaPID.getController().onTarget()) {
            driveTrain.stop();
            SmartDashboard.putString("Mode", "FINISHED");
            return false;
        }

        double div = 0.85 * (1 + Math.abs(offsetPID.getOutput()) / 30.0);
        double speedLeft = offsetPID.getOutput() + areaPID.getOutput() / div;
        double speedRight = -offsetPID.getOutput() + areaPID.getOutput() / div;

//        double angle = VisionData.getAngle();
//        gyroPID.getController().setPID(0.005, 0.0, 0.0);
//        gyroPID.getController().setSetpoint(gyroPID.getInput() + angle);
//
//        // slow down when turning
//        double div = (1 + Math.abs(gyroPID.getOutput()));
//        double speedLeft = gyroPID.getOutput() - areaPID.getOutput() / div;
//        double speedRight = -gyroPID.getOutput() - areaPID.getOutput() / div;

        driveTrain.setLeftMotors(speedLeft);
        driveTrain.setRightMotors(speedRight);

        return true;
    }

@Override
public void sendDataToSmartDashboard() {
  if (pdp != null) {
    SmartDashboard.putNumber("Temperature", pdp.getTemperature());
    //SmartDashboard.putNumber("TotalPower", pdp.getTotalPower());

    // in the past, this has tended to screw up the CAN bus
    // for (int i = 0; i < 16; i++) {
    // SmartDashboard.putNumber("Current_" + i, pdp.getCurrent(i));
    // }
  }
}

public void robotPeriodic(){
    lights.set(1);
    SmartDashboard.putBoolean("Pressurized", !compressor.getPressureSwitchValue());
    SmartDashboard.putBoolean("PressureCharging", compressor.enabled());
    SmartDashboard.putBoolean("PressureControlled", compressor.getClosedLoopControl());
    SmartDashboard.putNumber("PressureCurrent", compressor.getCompressorCurrent());
}

/**
 * Stop rotating the winch
 */
@Override
protected void end() {
    Robot.winch.off();
    logger.info("Winch reset");
    SmartDashboard.putBoolean("Winch Ready", true);

}

@Override
protected void execute() {
    double targetSpeed = .5* 1500;
    Robot.shooter.setSetpoint(targetSpeed);
    SmartDashboard.putNumber("Setpoint Set",targetSpeed);
    Robot.shooter.agitate();


}

protected void initialize() {
    // Reset for delta displacement
    Robot.driveTrain.resetAccel();

    Robot.driveTrain.enableAccelPIDY();
    Robot.driveTrain.setTargetDistanceY(SmartDashboard.getNumber("distanceDeltaY", 0));
}

@Override
public void log() {
    double cx = centerX;
    SmartDashboard.putNumber("Vision: Center Value", cx);
    SmartDashboard.putNumber("Vision: Target Distance", targetDistance);
    SmartDashboard.putNumber("Vision: Target Azimuth", targetAzimuth);
    SmartDashboard.putNumber("Vision: Target X Offset", targetOffsetX);
    SmartDashboard.putBoolean("Vision: Target Detected", targetDetected);
}

protected void initialize() {
    // Reset for delta displacement
    Robot.driveTrain.resetAccel();

    Robot.driveTrain.enableAccelPIDX();
    Robot.driveTrain.setTargetDistanceX(SmartDashboard.getNumber("distanceDeltaX", 0));
}

public GyroPID() {
    gyroSource = new GyroSource();
    defaultOutput = new DefaultOutput();

    gyroPID = new PIDController(Constants.GYRO_P, Constants.GYRO_I, Constants.GYRO_D, gyroSource, defaultOutput);
    gyroPID.setContinuous();
    gyroPID.setOutputRange(-Constants.GYRO_CAP, Constants.GYRO_CAP);
    gyroPID.enable();

    SmartDashboard.putData("GryoPID", gyroPID);
}

protected void execute(){
    if(raise && !ignoreUp){
        clawLift.clawRaise(1);
    }
    else if(!raise && !ignoreDown){
        clawLift.clawRaise(-1);
    }
    SmartDashboard.putBoolean("raise", raise);
    SmartDashboard.putBoolean("ignoreUp", ignoreUp);
    SmartDashboard.putBoolean("ignoreDown", ignoreDown);
    SmartDashboard.putBoolean("up", clawLift.getLimitTop());
    SmartDashboard.putBoolean("down", clawLift.getLimitBottom());
}

public void update ()
{
    double P = SmartDashboard.getNumber("DB/Slider 0", 0.0);
    double I = SmartDashboard.getNumber("DB/Slider 1", 0.0);
    double D = SmartDashboard.getNumber("DB/Slider 2", 0.0);
    double setPoint = SmartDashboard.getNumber("DB/Slider 3", 0.0);
    this.tuner.setP(P);
    this.tuner.setI(I);
    this.tuner.setD(D);
    this.tuner.setSetpoint(setPoint);
}

/**
 * Sends all diagnostics.
 */
public void sendDataToSmartDashboard() {
  SmartDashboard.putString("Gear_Mechanism_Position", position.toString());

  SmartDashboard.putNumber("Light_Spoke_Down", lsSpokeDown.getAverageVoltage());
  SmartDashboard.putNumber("Light_Wedge_Down", lsWedgeDown.getAverageVoltage());
  SmartDashboard.putString("Gear_Orientation", getGearOrientation().toString());
  SmartDashboard.putBoolean("Pressure_Plate", isPressurePlatePressed());
}

protected void execute() {      
    //Put any code here needed to handle readings from sensors.
    SmartDashboard.putNumber("Gyro", sensors.gyro.getAngle());

    SmartDashboard.putNumber("Drive Encoder L", sensors.driveEncoderL.get());
    SmartDashboard.putNumber("Drive Encoder R", sensors.driveEncoderR.get());
}

protected void initialize() {
    Robot.driveTrain.enableGyroPID();
    // Angle from SmartDash, default is the robots current angle
    double targetAngle = SmartDashboard.getNumber("targetAngle", Robot.driveTrain.getCurrentBoundedAngle());
    System.out.println(targetAngle);
    Robot.driveTrain.setTargetAngle(targetAngle);
}

public void startDrivingStraight(double speed) {
    controller.setPID(
            SmartDashboard.getNumber("kP", 0.0),
            SmartDashboard.getNumber("kI", 0.0),
            SmartDashboard.getNumber("kD", 0.0)
            );
    autoMoveSpeed = speed;
    if (!controller.isEnabled()) {
        gyro.reset();
        controller.reset();
        controller.enable();
    }
}