edu.wpi.first.wpilibj.smartdashboard.SmartDashboard Java Examples

@Override
public void outputTelemetry() {
    SmartDashboard.putNumber("Elevator Output %", mPeriodicIO.output_percent);
    SmartDashboard.putNumber("Elevator RPM", getRPM());
    SmartDashboard.putNumber("Elevator Current", mMaster.getOutputCurrent());
    // SmartDashboard.putNumber("Elevator Error", mMaster.getClosedLoopError(0) / kEncoderTicksPerInch);
    SmartDashboard.putNumber("Elevator Height", getInchesOffGround());
    SmartDashboard.putBoolean("Elevator Limit", mPeriodicIO.limit_switch);
    SmartDashboard.putNumber("Elevator Sensor Height", mPeriodicIO.position_ticks);

    SmartDashboard.putNumber("Elevator Last Expected Trajectory", mPeriodicIO.demand);
    SmartDashboard.putNumber("Elevator Current Trajectory Point", mPeriodicIO.active_trajectory_position);
    SmartDashboard.putNumber("Elevator Traj Vel", mPeriodicIO.active_trajectory_velocity);
    SmartDashboard.putNumber("Elevator Traj Accel", mPeriodicIO.active_trajectory_accel_g);
    SmartDashboard.putBoolean("Elevator Has Sent Trajectory", hasFinishedTrajectory());
}
 

public AutoModeSelector() {
    mStartPositionChooser = new SendableChooser<>();
    mStartPositionChooser.setDefaultOption("Left HAB 2", StartingPosition.LEFT_HAB_2);
    mStartPositionChooser.addOption("Right HAB 2", StartingPosition.RIGHT_HAB_2);
    mStartPositionChooser.addOption("Right HAB 1", StartingPosition.RIGHT_HAB_1);
    mStartPositionChooser.addOption("Left HAB 1", StartingPosition.LEFT_HAB_1);
    mStartPositionChooser.addOption("Center HAB 1", StartingPosition.CENTER_HAB_1);

    SmartDashboard.putData("Starting Position", mStartPositionChooser);

    mModeChooser = new SendableChooser<>();
    mModeChooser.setDefaultOption("Drive By Camera", DesiredMode.DRIVE_BY_CAMERA);
    mModeChooser.addOption("Do Nothing", DesiredMode.DO_NOTHING);
    mModeChooser.addOption("Rocket 1", DesiredMode.LOW_ROCKET);
    mModeChooser.addOption("Rocket 2", DesiredMode.MIDDLE_ROCKET);
    mModeChooser.addOption("Cargo Ship 2 Hatch", DesiredMode.SIDE_CARGO_SHIP_HATCH);
    mModeChooser.addOption("Front Then Side Cargo Ship",
            DesiredMode.FRONT_THEN_SIDE_CARGO_SHIP);
    mModeChooser.addOption("Test control flow", DesiredMode.TEST_CONTROL_FLOW);
    mModeChooser.addOption("Drive Characterization - Straight Line", DesiredMode.DRIVE_CHARACTERIZATION_STRAIGHT);
    mModeChooser.addOption("Drive Characterization - TUrn in Place", DesiredMode.DRIVE_CHARACTERIZATION_TURN);
    SmartDashboard.putData("Auto mode", mModeChooser);
}
 

@Override
public void outputTelemetry() {
    SmartDashboard.putNumber("Right Drive Distance", mPeriodicIO.right_distance);
    SmartDashboard.putNumber("Right Drive Ticks", mPeriodicIO.right_position_ticks);
    SmartDashboard.putNumber("Left Drive Ticks", mPeriodicIO.left_position_ticks);
    SmartDashboard.putNumber("Left Drive Distance", mPeriodicIO.left_distance);
    SmartDashboard.putNumber("Right Linear Velocity", getRightLinearVelocity());
    SmartDashboard.putNumber("Left Linear Velocity", getLeftLinearVelocity());

    SmartDashboard.putNumber("x err", mPeriodicIO.error.getTranslation().x());
    SmartDashboard.putNumber("y err", mPeriodicIO.error.getTranslation().y());
    SmartDashboard.putNumber("theta err", mPeriodicIO.error.getRotation().getDegrees());
    if (getHeading() != null) {
        SmartDashboard.putNumber("Gyro Heading", getHeading().getDegrees());
    }
    if (mCSVWriter != null) {
        mCSVWriter.write();
    }
}
 

@Override
public synchronized void outputTelemetry() {
    SmartDashboard.putNumber("Wrist Angle", getAngle());
    SmartDashboard.putNumber("Wrist Position", getPosition());
    SmartDashboard.putNumber("Wrist Ticks", mPeriodicIO.position_ticks);
    SmartDashboard.putNumber("Wrist periodic demand", mPeriodicIO.demand);
    SmartDashboard.putBoolean("LIMR", mPeriodicIO.limit_switch);

    SmartDashboard.putNumber("Wrist RPM", getRPM());
    SmartDashboard.putNumber("Wrist Power %", mPeriodicIO.output_percent);
    SmartDashboard.putBoolean("Wrist Limit Switch", mPeriodicIO.limit_switch);
    SmartDashboard.putNumber("Wrist Last Expected Trajectory", getSetpoint());
    SmartDashboard.putNumber("Wrist Current Trajectory Point", mPeriodicIO.active_trajectory_position);
    SmartDashboard.putNumber("Wrist Traj Vel", mPeriodicIO.active_trajectory_velocity);
    SmartDashboard.putNumber("Wrist Traj Accel", mPeriodicIO.active_trajectory_acceleration_rad_per_s2);
    SmartDashboard.putBoolean("Wrist Has Sent Trajectory", hasFinishedTrajectory());
    SmartDashboard.putNumber("Wrist feedforward", mPeriodicIO.feedforward);

    if (mCSVWriter != null) {
        mCSVWriter.write();
    }

}
 

protected void execute() {
    SmartDashboard.putNumber("Tilt Angle", Robot.SUB_DRIVE.getYAngle());
    SmartDashboard.putBoolean("Docked", Robot.SUB_DRIVE.docking);
    SmartDashboard.putBoolean("Reversed", Robot.SUB_DRIVE.reversing);
    SmartDashboard.putNumber("Right Encoder Inches", Robot.SUB_DRIVE.pid.getRightInches());
    SmartDashboard.putNumber("Left Encoder Inches", Robot.SUB_DRIVE.pid.getLeftInches());

	switch (Robot.SUB_DRIVE.drivetrain) {
		case ROCKET_LEAGUE:
			Robot.SUB_DRIVE.driveRLTank(OI.DRIVER, Util.getAndSetDouble("Rocket Ramp", .75), Util.getAndSetDouble("Drive Inhibitor", 1));
			break;
		case FORZA: 
			Robot.SUB_DRIVE.driveForza(OI.DRIVER, Util.getAndSetDouble("Forza Ramp", .75), Util.getAndSetDouble("Radius", 1), Util.getAndSetDouble("Drive Inhibitor", 1));
			break;
	}
}
 

@Override
public void disabledPeriodic() {
    SmartDashboard.putString("Match Cycle", "DISABLED");

    try {
        outputToSmartDashboard();
        mWrist.resetIfAtLimit();
        mElevator.resetIfAtLimit();

        // Poll FMS auto mode info and update mode creator cache
        mAutoFieldState.setSides(DriverStation.getInstance().getGameSpecificMessage());
        mAutoModeSelector.updateModeCreator();

        if (mAutoFieldState.isValid()) {
            Optional<AutoModeBase> autoMode = mAutoModeSelector.getAutoMode(mAutoFieldState);
            if (autoMode.isPresent() && autoMode.get() != mAutoModeExecutor.getAutoMode()) {
                System.out.println("Set auto mode to: " + autoMode.get().getClass().toString());
                mAutoModeExecutor.setAutoMode(autoMode.get());
            }
            System.gc();
        }
    } catch (Throwable t) {
        CrashTracker.logThrowableCrash(t);
        throw t;
    }
}
 

@Override
public void outputTelemetry() {
    SmartDashboard.putBoolean("Vacuum: Low sensor", getLowerPressureSensor());
    SmartDashboard.putBoolean("Vacuum: High sensor", getHigherPressureSensor());

    String state = "default";
    if (mPeriodicIO.thresholdState == ThresholdState.ABOVE) {
        state = "Above";
    } else if (mPeriodicIO.thresholdState == ThresholdState.BETWEEN) {
        state = "Between";
    } else if (mPeriodicIO.thresholdState == ThresholdState.BELOW) {
        state = "Below";
    }
    SmartDashboard.putString("Vacuum: Threshold state", state);

    if (mCSVWriter != null) {
        mCSVWriter.write();
    }
}
 

@Override
public void testInit() {
    SmartDashboard.putString("Match Cycle", "TEST");

    try {
        System.out.println("Starting check systems.");

        mDisabledLooper.stop();
        mEnabledLooper.stop();

        //mDrive.checkSystem();
        //mIntake.checkSystem();
        //mWrist.checkSystem();
        mElevator.checkSystem();

    } catch (Throwable t) {
        CrashTracker.logThrowableCrash(t);
        throw t;
    }
}
 

@Override
public void autonomousInit() {
    SmartDashboard.putString("Match Cycle", "AUTONOMOUS");

    try {
        CrashTracker.logAutoInit();
        mDisabledLooper.stop();

        RobotState.getInstance().reset(Timer.getFPGATimestamp(), Pose2d.identity());

        Drive.getInstance().zeroSensors();
        mInfrastructure.setIsDuringAuto(true);

        mWrist.setRampRate(Constants.kAutoWristRampRate);

        mAutoModeExecutor.start();

        mLED.setEnableFaults(false);
        mEnabledLooper.start();

        mSuperstructure.setKickstand(true);
    } catch (Throwable t) {
        CrashTracker.logThrowableCrash(t);
        throw t;
    }
}
 

public AutoModeSelector() {
    mModeChooser = new SendableChooser<>();
    mModeChooser.addDefault("Cross Auto Line", DesiredMode.CROSS_AUTO_LINE);
    mModeChooser.addObject("Do Nothing", DesiredMode.DO_NOTHING);
    mModeChooser.addObject("Simple switch", DesiredMode.SIMPLE_SWITCH);
    mModeChooser.addObject("Scale AND Switch", DesiredMode.SCALE_AND_SWITCH);
    mModeChooser.addObject("Only Scale", DesiredMode.ONLY_SCALE);
    SmartDashboard.putData("Auto mode", mModeChooser);

    mStartPositionChooser = new SendableChooser<>();
    mStartPositionChooser.addDefault("Right", StartingPosition.RIGHT);
    mStartPositionChooser.addObject("Center", StartingPosition.CENTER);
    mStartPositionChooser.addObject("Left", StartingPosition.LEFT);
    SmartDashboard.putData("Starting Position", mStartPositionChooser);

    mSwitchScalePositionChooser = new SendableChooser<>();
    mSwitchScalePositionChooser.addDefault("Use FMS Data", SwitchScalePosition.USE_FMS_DATA);
    mSwitchScalePositionChooser.addObject("Left Switch Left Scale", SwitchScalePosition.LEFT_SWITCH_LEFT_SCALE);
    mSwitchScalePositionChooser.addObject("Left Switch Right Scale", SwitchScalePosition.LEFT_SWITCH_RIGHT_SCALE);
    mSwitchScalePositionChooser.addObject("Right Switch Left Scale", SwitchScalePosition.RIGHT_SWITCH_LEFT_SCALE);
    mSwitchScalePositionChooser.addObject("Right Switch Right Scale", SwitchScalePosition.RIGHT_SWITCH_RIGHT_SCALE);
    SmartDashboard.putData("Switch and Scale Position", mSwitchScalePositionChooser);

}
 

@Override
public void outputTelemetry() {
    SmartDashboard.putBoolean("Connected to CheesyVision2", isConnected());
    SmartDashboard.putNumber("Desired Height (0 cubes)", getDesiredHeight(false, 0, true));
    SmartDashboard.putNumber("Desired Height (1 cube)", getDesiredHeight(false, 1, true));
    SmartDashboard.putNumber("Desired Height (2 cubes)", getDesiredHeight(false, 2, true));
}
 

@Override
public void autonomousPeriodic() {
    SmartDashboard.putString("Match Cycle", "AUTONOMOUS");

    outputToSmartDashboard();
    try {

    } catch (Throwable t) {
        CrashTracker.logThrowableCrash(t);
        throw t;
    }
}
 

@Override
public void teleopInit() {
    SmartDashboard.putString("Match Cycle", "TELEOP");

    try {
        CrashTracker.logTeleopInit();
        mDisabledLooper.stop();
        if (mAutoModeExecutor != null) {
            mAutoModeExecutor.stop();
        }

        mAutoFieldState.disableOverride();

        mInfrastructure.setIsDuringAuto(false);
        mWrist.setRampRate(Constants.kWristRampRate);

        RobotState.getInstance().reset(Timer.getFPGATimestamp(), Pose2d.identity());
        mEnabledLooper.start();
        mLED.setEnableFaults(false);
        mInHangMode = false;
        mForklift.retract();

        mShootDelayed.update(false, Double.POSITIVE_INFINITY);
        mPoopyShootDelayed.update(false, Double.POSITIVE_INFINITY);
        mDrive.setVelocity(DriveSignal.NEUTRAL, DriveSignal.NEUTRAL);
        mDrive.setOpenLoop(new DriveSignal(0.05, 0.05));

        mKickStandEngaged = true;
        mKickStandReleased.update(true);
    } catch (Throwable t) {
        CrashTracker.logThrowableCrash(t);
        throw t;
    }
}
 

@Override
public void disabledInit() {
    SmartDashboard.putString("Match Cycle", "DISABLED");

    try {
        CrashTracker.logDisabledInit();
        mEnabledLooper.stop();
        if (mAutoModeExecutor != null) {
            mAutoModeExecutor.stop();
        }

        mInfrastructure.setIsDuringAuto(true);
        Drive.getInstance().zeroSensors();
        RobotState.getInstance().reset(Timer.getFPGATimestamp(), Pose2d.identity());

        // Reset all auto mode state.
        mAutoModeSelector.reset();
        mAutoModeSelector.updateModeCreator();
        mAutoModeExecutor = new AutoModeExecutor();

        mDisabledLooper.start();

        mLED.setEnableFaults(true);
    } catch (Throwable t) {
        CrashTracker.logThrowableCrash(t);
        throw t;
    }
}
 

public static void deletePersistentKeys() {
    SmartDashboard smartDashboard = new SmartDashboard();
    for (String key : smartDashboard.getKeys()) {
        if (smartDashboard.isPersistent(key)) {
            smartDashboard.delete(key);
        }
    }
}
 

public void outputToSmartDashboard() {
    Pose2d odometry = getLatestFieldToVehicle().getValue();
    SmartDashboard.putNumber("Robot Pose X", odometry.getTranslation().x());
    SmartDashboard.putNumber("Robot Pose Y", odometry.getTranslation().y());
    SmartDashboard.putNumber("Robot Pose Theta", odometry.getRotation().getDegrees());
    SmartDashboard.putNumber("Robot Linear Velocity", vehicle_velocity_measured_.dx);
}
 

public void addPoint(LidarPoint point, boolean newScan) {
    SmartDashboard.putNumber("LIDAR last_angle", point.angle);

    Translation2d cartesian = point.toCartesian();
    logPoint(point.angle, point.distance, cartesian.x(), cartesian.y());

    lock.writeLock().lock();
    try {
        if (newScan) { // crosses the 360-0 threshold. start a new scan
            prev_timestamp = Timer.getFPGATimestamp();

            // long start = System.nanoTime();
            // Translation2d towerPos = getTowerPosition();
            // long end = System.nanoTime();
            // SmartDashboard.putNumber("towerPos_ms", (end-start)/1000000);
            // SmartDashboard.putNumber("towerPosX", towerPos.x());
            // SmartDashboard.putNumber("towerPosY", towerPos.y());

            mScans.add(new LidarScan());
            if (mScans.size() > Constants.kChezyLidarNumScansToStore) {
                mScans.removeFirst();
            }
        }

        if (!excludePoint(cartesian.x(), cartesian.y())) {
            getCurrentScan().addPoint(new Point(cartesian), point.timestamp);
        }
    } finally {
        lock.writeLock().unlock();
    }
}
 

@Override
public void outputTelemetry() {
    SmartDashboard.putNumber("Right Drive Distance", mPeriodicIO.right_distance);
    SmartDashboard.putNumber("Right Drive Ticks", mPeriodicIO.right_position_ticks);
    SmartDashboard.putNumber("Left Drive Ticks", mPeriodicIO.left_position_ticks);
    SmartDashboard.putNumber("Left Drive Distance", mPeriodicIO.left_distance);
    // SmartDashboard.putNumber("Right Linear Velocity", getRightLinearVelocity());
    // SmartDashboard.putNumber("Left Linear Velocity", getLeftLinearVelocity());

    // SmartDashboard.putNumber("X Error", mPeriodicIO.error.getTranslation().x());
    // SmartDashboard.putNumber("Y error", mPeriodicIO.error.getTranslation().y());
    // SmartDashboard.putNumber("Theta Error", mPeriodicIO.error.getRotation().getDegrees());

    // SmartDashboard.putNumber("Left Voltage Kf", mPeriodicIO.left_voltage / getLeftLinearVelocity());
    // SmartDashboard.putNumber("Right Voltage Kf", mPeriodicIO.right_voltage / getRightLinearVelocity());

    // if (mPathFollower != null) {
    //     SmartDashboard.putNumber("Drive LTE", mPathFollower.getAlongTrackError());
    //     SmartDashboard.putNumber("Drive CTE", mPathFollower.getCrossTrackError());
    // } else {
    //     SmartDashboard.putNumber("Drive LTE", 0.0);
    //     SmartDashboard.putNumber("Drive CTE", 0.0);
    // }

    if (getHeading() != null) {
        SmartDashboard.putNumber("Gyro Heading", getHeading().getDegrees());
    }

    if (mCSVWriter != null) {
        mCSVWriter.write();
    }
}
 

protected void execute() {
	DriverStation.reportWarning("DRIVING " + inches + " INCHES", false);
	SmartDashboard.putNumber("Left Encoder Inches", Robot.SUB_DRIVE.pid.getLeftInches());
	SmartDashboard.putNumber("Right Encoder Inches", Robot.SUB_DRIVE.pid.getRightInches());

    SmartDashboard.putNumber("Error", Robot.SUB_DRIVE.pid.getError());
}
 

@Override
public void outputTelemetry() {
    SmartDashboard.putNumber(mConstants.kName + ": Position (units)", mPeriodicIO.position_units);
    SmartDashboard.putBoolean(mConstants.kName + ": Homing Location", atHomingLocation());
    // synchronized (this) {
    //     if (mCSVWriter != null) {
    //         mCSVWriter.write();
    //     }
    // }
}
 

/**
     * drive code where rotation is dependent on acceleration
     * @param radius 0.00-1.00, 1 being zero radius and 0 being driving in a line
     */
    public void driveForza(Joystick joy, double ramp, double radius, double inhibitor) {
        double left = 0,
                right = 0;
        double acceleration = Xbox.RT(joy) - Xbox.LT(joy);

        setRamps(ramp);
//        if (getYAngle() > Constants.TILT_ANGLE ) {
//            leftMaster.set(ControlMode.PercentOutput, -1 * Constants.RECOVERY_SPEED);
//            rightMaster.set(ControlMode.PercentOutput, -1 * Constants.RECOVERY_SPEED);
//        } else if (getYAngle() < -1 * Constants.TILT_ANGLE){
//            leftMaster.set(ControlMode.PercentOutput, Constants.RECOVERY_SPEED);
//            rightMaster.set(ControlMode.PercentOutput, Constants.RECOVERY_SPEED);
//        } else {

            if (!reversing ? Xbox.LEFT_X(joy) < 0 : Xbox.LEFT_X(joy) > 0) {
                right = acceleration;
                left = (acceleration * ((2 * (1 - Math.abs(Xbox.LEFT_X(joy)))) - 1)) / radius;
            } else if (!reversing ? Xbox.LEFT_X(joy) > 0 : Xbox.LEFT_X(joy) < 0) {
                left = acceleration;
                right = (acceleration * ((2 * (1 - Math.abs(Xbox.LEFT_X(joy)))) - 1)) / radius;
            } else {
                left = acceleration;
                right = acceleration;
            }
//        }][\
        left = (left > 1.0 ? 1.0 : (left < -1.0 ? -1.0 : left));
        right = (right > 1.0 ? 1.0 : (right < -1.0 ? -1.0 : right));
        leftMaster.set(ControlMode.PercentOutput, leftify(left) * inhibitor * (reversing ? -1.0 : 1.0));
        rightMaster.set(ControlMode.PercentOutput, rightify(right) * inhibitor * (reversing ? -1.0 : 1.0));

        SmartDashboard.putString("Left Master", "Left Master Voltage: " + leftMaster.getBusVoltage());
        SmartDashboard.putString("Right Master", "Right Master Voltage: " + rightMaster.getBusVoltage());
    }
 

public static void deleteAllKeys() {
    SmartDashboard smartDashboard = new SmartDashboard();
    for (String key : smartDashboard.getKeys()) {
        smartDashboard.delete(key);
    }
}
 

/** assigns what every SmartDash and controller button does */
public OI() {
	/// manipulator wheels
		Button spinIn = new JoystickButton(OPERATOR, Xbox.RB);
				spinIn.whileHeld(new ButtonCommandEat());
		Button spinOut = new JoystickButton(OPERATOR, Xbox.LB);
				spinOut.whileHeld(new ButtonCommandSpit());
	/// manipulator clamp
		Button toggleClamp = new JoystickButton(OPERATOR, Xbox.A);
			toggleClamp.toggleWhenActive(new ToggleCommandClamp());
	/// candy cane
		Button toggleHook = new JoystickButton(OPERATOR, Xbox.B);
			toggleHook.toggleWhenActive(new ToggleCommandHook());
	/// drop the mast
		Button dropIt = new JoystickButton(OPERATOR, Xbox.X);
			dropIt.toggleWhenPressed(new ButtonCommandHitTheDeck());
	/// Reversing mode
		Button toggleReverse = new JoystickButton(DRIVER, Xbox.Y);
			toggleReverse.toggleWhenPressed(new ToggleCommandReverse());
	/// Docking mode
		Button toggleDock = new JoystickButton(DRIVER, Xbox.X);
			toggleDock.toggleWhenPressed(new ToggleCommandDock());
	/// To Compress, or Not To Compress. It is now an option.
		SmartDashboard.putData("Disable Compressor", new ToggleCommandKillCompressor());
		

	/// PID
		SmartDashboard.putData("Kill PID", new ToggleCommandKillPID());
		SmartDashboard.putNumber("Right Encoder Position", 0);
		SmartDashboard.putNumber("Left Encoder Position", 0);
		
	/// limit switch displays
		SmartDashboard.putBoolean("Lower Screw", true);
    	SmartDashboard.putBoolean("Upper Screw", false);
    	SmartDashboard.putBoolean("Lower Pinion", true);
    	SmartDashboard.putBoolean("Upper Pinion", false);
    	
    	SmartDashboard.putNumber("Left inches", 0);
    	SmartDashboard.putNumber("Right inches", 0);
    	
    	DriverStation.reportWarning("OI IS INSTANTIATED", false);

    /// Cyborg command testers
		SmartDashboard.putData("Drive Direct", new CyborgCommandDriveDirect(Util.getAndSetDouble("Drive Direct Power", 0)));
		SmartDashboard.putData("Drive Distance", new CyborgCommandDriveDistance(Util.getAndSetDouble("Drive Distance Inches", 0)));
		SmartDashboard.putData("Drive Until Error", new CyborgCommandDriveUntilError());
		SmartDashboard.putData("Rotate Degree", new CyborgCommandRotateDegrees(Util.getAndSetDouble("Rotate Degrees", 0)));
		SmartDashboard.putData("Spit", new CyborgCommandSpit((long)Util.getAndSetDouble("Spit Time", 500)));
		SmartDashboard.putData("Raise to Position: Pinion Up", new CyborgCommandGrow(Mast.PINION_UP));
		SmartDashboard.putData("Raise to Position: Pinion Down", new CyborgCommandGrow(Mast.PINION_DOWN));
		SmartDashboard.putData("Raise to Position: Screw Up", new CyborgCommandGrow(Mast.SCREW_UP));
		SmartDashboard.putData("Raise to Position: Screw Down", new CyborgCommandGrow(Mast.SCREW_DOWN));
}
 

protected void execute() {
    DriverStation.reportWarning("ROTATING " + (inches / SCALAR) + " DEGREES" + ((inches > 0) ? "CW" : "CCW"), false);
    SmartDashboard.putNumber("Left Encoder Inches", Robot.SUB_DRIVE.pid.getLeftInches());
    SmartDashboard.putNumber("Right Encoder Inches", Robot.SUB_DRIVE.pid.getRightInches());
    SmartDashboard.putNumber("Error", Robot.SUB_DRIVE.pid.getError());
}
 

/** runs when robot is turned on */
	public void robotInit() {
		/// instantiate bot chooser
		botChooser = new SendableChooser<>();
		botChooser.addDefault(Bot.TEUFELSKIND.toString(), Bot.TEUFELSKIND);
		botChooser.addObject(Bot.OOF.toString(), Bot.OOF);
		SmartDashboard.putData("Bot", botChooser);

		if (botChooser.getSelected() != null){
			bot = botChooser.getSelected();
		} else {
			bot = Bot.OOF;
		}

			DriverStation.reportWarning("ROBOT STARTED; GOOD LUCK", false);
		/// instantiate subsystems
//			SUB_ARDUINO = new SubsystemArduino();
			SUB_MANIPULATOR = new SubsystemManipulator();
			SUB_CLAMP = new SubsystemClamp();
			SUB_COMPRESSOR = new SubsystemCompressor();
			SUB_DRIVE = new SubsystemDrive();
			
			SUB_DRIVE.pid.setPIDF(.5, 0, 0, 0);
			
			SUB_HOOK = new SubsystemHook();
			SUB_MAST = new SubsystemMast();
			vision = new Vision();

		/// instantiate operator interface
			oi = new OI();

			
			
		/// instantiate drivetrain chooser
				driveChooser = new SendableChooser<>();
				driveChooser.addDefault(Drivetrain.ROCKET_LEAGUE.toString(), Drivetrain.ROCKET_LEAGUE); // set default to RL drive
				for(int i = 1; i < Drivetrain.values().length; i++) { 
					driveChooser.addObject(Drivetrain.values()[i].toString(), Drivetrain.values()[i]); } // add each drivetrain enum value to chooser
				SmartDashboard.putData("Drivetrain", driveChooser); //display the chooser on the dash
			
		/// instantiate position chooser
				positionChooser = new SendableChooser<>();
				positionChooser.addDefault(Position.CENTER.toString(), Position.CENTER); // set default to center
				for(int i = 1; i < Position.values().length; i++) { 
					positionChooser.addObject(Position.values()[i].toString(), Position.values()[i]); } // add each position enum value to chooser
				SmartDashboard.putData("Position", positionChooser); //display the chooser on the dash

		/// instantiate goal chooser
				goalChooser = new SendableChooser<>();
				goalChooser.addDefault(Goal.NOTHING.toString(), Goal.NOTHING); // set default to nothing
				for(int i = 1; i < Goal.values().length; i++) { 
					goalChooser.addObject(Goal.values()[i].toString(), Goal.values()[i]); } // add each autonomous goal to chooser
				SmartDashboard.putData("Goal", goalChooser); //display the chooser on the dash
			
		/// instantiate bot chooser
				botChooser = new SendableChooser<>();
				botChooser.addDefault(Bot.TEUFELSKIND.toString(), Bot.TEUFELSKIND);
					botChooser.addObject(Bot.OOF.toString(), Bot.OOF); 
				SmartDashboard.putData("Bot", botChooser);
				
				
				
		/// instantiate cameras
			vision.startScrewCameraThread();
			vision.startFrameCameraThread();

		SmartDashboard.putData("Sub_Drive", SUB_DRIVE);
		DriverStation.reportWarning("SUBSYSTEMS, CHOOSERS INSTANTIATED", false);
	}
 

protected void loopFunc() {
    m_watchdog.reset();

    // Call the appropriate function depending upon the current robot mode
    if (isDisabled()) {
        // Call DisabledInit() if we are now just entering disabled mode from either a different mode
        // or from power-on.
        if (m_lastMode != Mode.kDisabled) {
            Shuffleboard.disableActuatorWidgets();
            disabledInit();
            m_watchdog.addEpoch("disabledInit()");
            m_lastMode = Mode.kDisabled;
        }

        HAL.observeUserProgramDisabled();
        disabledPeriodic();
        m_watchdog.addEpoch("disablePeriodic()");
    } else if (isAutonomous()) {
        // Call AutonomousInit() if we are now just entering autonomous mode from either a different
        // mode or from power-on.
        if (m_lastMode != Mode.kAutonomous) {
            Shuffleboard.disableActuatorWidgets();
            autonomousInit();
            m_watchdog.addEpoch("autonomousInit()");
            m_lastMode = Mode.kAutonomous;
        }

        HAL.observeUserProgramAutonomous();
        autonomousPeriodic();
        m_watchdog.addEpoch("autonomousPeriodic()");
    } else if (isOperatorControl()) {
        // Call TeleopInit() if we are now just entering teleop mode from either a different mode or
        // from power-on.
        if (m_lastMode != Mode.kTeleop) {
            Shuffleboard.disableActuatorWidgets();
            teleopInit();
            m_watchdog.addEpoch("teleopInit()");
            m_lastMode = Mode.kTeleop;
        }

        HAL.observeUserProgramTeleop();
        teleopPeriodic();
        m_watchdog.addEpoch("teleopPeriodic()");
    } else {
        // Call TestInit() if we are now just entering test mode from either a different mode or from
        // power-on.
        if (m_lastMode != Mode.kTest) {
            Shuffleboard.enableActuatorWidgets();
            testInit();
            m_watchdog.addEpoch("testInit()");
            m_lastMode = Mode.kTest;
        }

        HAL.observeUserProgramTest();
        testPeriodic();
        m_watchdog.addEpoch("testPeriodic()");
    }

    robotPeriodic();
    m_watchdog.addEpoch("robotPeriodic()");
    m_watchdog.disable();
    SmartDashboard.updateValues();

    Shuffleboard.update();

    // Warn on loop time overruns
    if (m_watchdog.isExpired()) {
        //m_watchdog.printEpochs();
    }
}
 

public void publishSwitches() {
	SmartDashboard.putBoolean("Lower Screw", !lowerScrewLimit.get());
	SmartDashboard.putBoolean("Upper Screw", !upperScrewLimit.get());
	SmartDashboard.putBoolean("Lower Pinion", !lowerPinionLimit.get());
	SmartDashboard.putBoolean("Upper Pinion", !upperPinionLimit.get());
}
 

@Override
public void outputTelemetry() {
    SmartDashboard.putBoolean("LeftBanner", getLeftBannerSensor());
    SmartDashboard.putBoolean("RightBanner", getRightBannerSensor());
}
 

public void outputToSmartDashboard() {
    SmartDashboard.putString("FieldState OurSwitch", getOurSwitchSide() == null ? "NULL" : getOurSwitchSide().toString());
    SmartDashboard.putString("FieldState Scale", getScaleSide() == null ? "NULL" : getScaleSide().toString());
    SmartDashboard.putString("FieldState TheirSwitch", getOpponentSwitchSide() == null ? "NULL" : getOpponentSwitchSide().toString());
}
 

@Override
public void testPeriodic() {
    SmartDashboard.putString("Match Cycle", "TEST");
}