edu.wpi.first.wpilibj.DriverStation Java Examples

private void updatePathFollower(double timestamp) {
    if (mDriveControlState == DriveControlState.PATH_FOLLOWING) {
        RobotState robot_state = RobotState.getInstance();
        Pose2d field_to_vehicle = robot_state.getLatestFieldToVehicle().getValue();
        Twist2d command = mPathFollower.update(timestamp, field_to_vehicle, robot_state.getDistanceDriven(),
                robot_state.getPredictedVelocity().dx);
        if (!mPathFollower.isFinished()) {
            DriveSignal setpoint = Kinematics.inverseKinematics(command);
            setVelocity(setpoint, new DriveSignal(0, 0));
        } else {
            if (!mPathFollower.isForceFinished()) {
                setVelocity(new DriveSignal(0, 0), new DriveSignal(0, 0));
            }
        }
    } else {
        DriverStation.reportError("drive is not in path following state", false);
    }
}
 

@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;
    }
}
 

private void frameCameraStream(){
	cameraFrame = CameraServer.getInstance().startAutomaticCapture("Frame", VisionConstants.HOOK_ID);
	
	CvSink cvsinkFrame = new CvSink("frameSink");
	cvsinkFrame.setSource(cameraFrame);
	cvsinkFrame.setEnabled(true);
	
	Mat streamImages = new Mat();

	CvSource outputFrame = CameraServer.getInstance().putVideo("Frame", VisionConstants.CAM_WIDTH, VisionConstants.CAM_HEIGHT);
	 while (!Thread.interrupted()){
         try {
             cvsinkFrame.grabFrame(streamImages);
             if ((Robot.bot == Bot.TEUFELSKIND && Constants.TEUFELSKIND.FRAME_CAM_FLIP)
                     || (Robot.bot == Bot.OOF && Constants.OOF.FRAME_CAM_FLIP)) {
                 Core.rotate(streamImages, streamImages, Core.ROTATE_180);
             }
             outputFrame.putFrame(streamImages);
         } catch (CvException cameraFail){
             DriverStation.reportWarning("Frame Camera: " + cameraFail.toString(), false);
             outputFrame.putFrame(failImage);
         }
	 }
}
 

private void configureMaster(TalonSRX talon, boolean left) {
    talon.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 5, 100);
    final ErrorCode sensorPresent = talon.configSelectedFeedbackSensor(FeedbackDevice
            .CTRE_MagEncoder_Relative, 0, 100); //primary closed-loop, 100 ms timeout
    if (sensorPresent != ErrorCode.OK) {
        DriverStation.reportError("Could not detect " + (left ? "left" : "right") + " encoder: " + sensorPresent, false);
    }
    talon.setInverted(!left);
    talon.setSensorPhase(true);
    talon.enableVoltageCompensation(true);
    talon.configVoltageCompSaturation(12.0, Constants.kLongCANTimeoutMs);
    talon.configVelocityMeasurementPeriod(VelocityMeasPeriod.Period_50Ms, Constants.kLongCANTimeoutMs);
    talon.configVelocityMeasurementWindow(1, Constants.kLongCANTimeoutMs);
    talon.configClosedloopRamp(Constants.kDriveVoltageRampRate, Constants.kLongCANTimeoutMs);
    talon.configNeutralDeadband(0.04, 0);
}
 

private void updatePathFollower() {
    if (mDriveControlState == DriveControlState.PATH_FOLLOWING) {
        final double now = Timer.getFPGATimestamp();

        DriveMotionPlanner.Output output = mMotionPlanner.update(now, RobotState.getInstance().getFieldToVehicle(now));

        // DriveSignal signal = new DriveSignal(demand.left_feedforward_voltage / 12.0, demand.right_feedforward_voltage / 12.0);

        mPeriodicIO.error = mMotionPlanner.error();
        mPeriodicIO.path_setpoint = mMotionPlanner.setpoint();

        if (!mOverrideTrajectory) {
            setVelocity(new DriveSignal(radiansPerSecondToTicksPer100ms(output.left_velocity), radiansPerSecondToTicksPer100ms(output.right_velocity)),
                    new DriveSignal(output.left_feedforward_voltage / 12.0, output.right_feedforward_voltage / 12.0));

            mPeriodicIO.left_accel = radiansPerSecondToTicksPer100ms(output.left_accel) / 1000.0;
            mPeriodicIO.right_accel = radiansPerSecondToTicksPer100ms(output.right_accel) / 1000.0;
        } else {
            setVelocity(DriveSignal.BRAKE, DriveSignal.BRAKE);
            mPeriodicIO.left_accel = mPeriodicIO.right_accel = 0.0;
        }
    } else {
        DriverStation.reportError("Drive is not in path following state", false);
    }
}
 

private void screwCameraStream(){

    	cameraScrew = CameraServer.getInstance().startAutomaticCapture("Screw", VisionConstants.SCREW_ID);
    	
    	CvSink cvsinkScrew = new CvSink("screwSink");
    	cvsinkScrew.setSource(cameraScrew);
    	cvsinkScrew.setEnabled(true);
    	
    	Mat streamImages = new Mat();
    	
    	CvSource outputScrew = CameraServer.getInstance().putVideo("Screw", VisionConstants.CAM_WIDTH, VisionConstants.CAM_HEIGHT);
    	 while (!Thread.interrupted()){
    	     try {
                 cvsinkScrew.grabFrame(streamImages);
                 if ((Robot.bot == Bot.TEUFELSKIND && Constants.TEUFELSKIND.SCREW_CAM_FLIP)
                         || (Robot.bot == Bot.OOF && Constants.OOF.SCREW_CAM_FLIP)) {
                     Core.rotate(streamImages, streamImages, Core.ROTATE_180);
                 }
                 outputScrew.putFrame(streamImages);
             } catch (CvException cameraFail){
    	         DriverStation.reportWarning("Screw Camera: " + cameraFail.toString(), false);
    	         outputScrew.putFrame(failImage);
             }
    	 }
    }
 

protected boolean isFinished() {
	DriverStation.reportWarning("DONE ROTATING", false);
    if(!inRange) {
        time = System.currentTimeMillis() + TIME_WAIT;
    }
    return time < System.currentTimeMillis();
}
 

public static Position getSwitch() {
	if (DriverStation.getInstance().getGameSpecificMessage().charAt(1) == 'L') {
		return Position.LEFT;
	} else {
		return Position.RIGHT;
	}
}
 

public static Position getThierScale() {
	if (DriverStation.getInstance().getGameSpecificMessage().charAt(2) == 'L') {
		return Position.LEFT;
	} else {
		return Position.RIGHT;
	}
}
 

public static Position getOurScale() {
	if (DriverStation.getInstance().getGameSpecificMessage().charAt(0) == 'L') {
		return Position.LEFT;
	} else {
		return Position.RIGHT;
	}
}
 

/** runs when autonomous start */
public void autonomousInit() {
	DriverStation.reportWarning("AUTONOMOUS IS STARTING...", false);
	if(goalChooser.getSelected() != null) {
		auto = new CommandGroupAuto(positionChooser.getSelected(), goalChooser.getSelected());
		auto.start(); 
	} 
}
 

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());
}
 

protected void initialize() {
	DriverStation.reportWarning("ROTATING " + (inches / SCALAR) + " DEGREES" + ((inches > 0) ? "CW" : "CCW"), false);
    time = System.currentTimeMillis() + TIME_WAIT;
    inRange = false;
    Robot.SUB_DRIVE.pid.reset();
    time = System.currentTimeMillis() + TIME_WAIT;
    inches = Util.getAndSetDouble("Rotate Degrees", 0) * SCALAR;
    Robot.SUB_DRIVE.pid.setPIDF(Util.getAndSetDouble("P", .11),
            Util.getAndSetDouble("I", 0),
            Util.getAndSetDouble("D", 0),
            Util.getAndSetDouble("F", 0));
    inRange = Robot.SUB_DRIVE.driveDistance(inches, -1* inches);
}
 

public void run() {
    mActive = true;

    try {
        routine();
    } catch (AutoModeEndedException e) {
        DriverStation.reportError("AUTO MODE DONE!!!! ENDED EARLY!!!!", false);
        return;
    }

    done();
}
 

public void run() {
    mActive = true;

    try {
        routine();
    } catch (AutoModeEndedException e) {
        DriverStation.reportError("AUTO MODE DONE!!!! ENDED EARLY!!!!", false);
        return;
    }

    done();
}
 

protected void end() {
    DriverStation.reportWarning("CyborgCommandDriveUntilError finished", false);
    Robot.SUB_DRIVE.driveDirect(0, 0);
}
 

private static void handleCANError(int id, CANError error, String message) {
    if (error != CANError.kOK) {
        DriverStation.reportError(
                "Could not configure spark id: " + id + " error: " + error.toString() + " " + message, false);
    }
}
 

/** runs when teleop starts*/
public void teleopInit() {
	DriverStation.reportWarning("TELEOP IS ENABLED", false);
	if (auto != null)
		auto.cancel(); 
}
 

@Override
public synchronized void readPeriodicInputs() {
    mPeriodicIO.timestamp = Timer.getFPGATimestamp();

    if (mMaster.hasResetOccurred()) {
        DriverStation.reportError(mConstants.kName + ": Talon Reset! ", false);
        mPeriodicIO.reset_occured = true;
        return;
    } else {
        mPeriodicIO.reset_occured = false;
    }

    mMaster.getStickyFaults(mFaults);
    if (mFaults.hasAnyFault()) {
        DriverStation.reportError(mConstants.kName + ": Talon Fault! " + mFaults.toString(), false);
        mMaster.clearStickyFaults(0);
    }
    if (mMaster.getControlMode() == ControlMode.MotionMagic) {
        mPeriodicIO.active_trajectory_position = mMaster.getActiveTrajectoryPosition();

        if (mPeriodicIO.active_trajectory_position < mReverseSoftLimitTicks) {
            DriverStation.reportError(mConstants.kName + ": Active trajectory past reverse soft limit!", false);
        } else if (mPeriodicIO.active_trajectory_position > mForwardSoftLimitTicks) {
            DriverStation.reportError(mConstants.kName + ": Active trajectory past forward soft limit!", false);
        }
        final int newVel = mMaster.getActiveTrajectoryVelocity();
        if (Util.epsilonEquals(newVel, mConstants.kCruiseVelocity, Math.max(1, mConstants.kDeadband)) || Util
                .epsilonEquals(newVel, mPeriodicIO.active_trajectory_velocity, Math.max(1, mConstants.kDeadband))) {
            // Mechanism is ~constant velocity.
            mPeriodicIO.active_trajectory_acceleration = 0.0;
        } else {
            // Mechanism is accelerating.
            mPeriodicIO.active_trajectory_acceleration = Math
                    .signum(newVel - mPeriodicIO.active_trajectory_velocity) * mConstants.kAcceleration;
        }
        mPeriodicIO.active_trajectory_velocity = newVel;
    } else {
        mPeriodicIO.active_trajectory_position = Integer.MIN_VALUE;
        mPeriodicIO.active_trajectory_velocity = 0;
        mPeriodicIO.active_trajectory_acceleration = 0.0;
    }
    if (mMaster.getControlMode() == ControlMode.Position) {
        mPeriodicIO.error_ticks = mMaster.getClosedLoopError(0);
    } else {
        mPeriodicIO.error_ticks = 0;
    }
    mPeriodicIO.master_current = mMaster.getOutputCurrent();
    mPeriodicIO.output_voltage = mMaster.getMotorOutputVoltage();
    mPeriodicIO.output_percent = mMaster.getMotorOutputPercent();
    mPeriodicIO.position_ticks = mMaster.getSelectedSensorPosition(0);
    mPeriodicIO.position_units = ticksToHomedUnits(mPeriodicIO.position_ticks);
    mPeriodicIO.velocity_ticks_per_100ms = mMaster.getSelectedSensorVelocity(0);

    if (mConstants.kRecoverPositionOnReset) {
        mPeriodicIO.absolute_pulse_position = mMaster.getSensorCollection().getPulseWidthPosition();
        mPeriodicIO.absolute_pulse_position_modded = mPeriodicIO.absolute_pulse_position % 4096;
        if (mPeriodicIO.absolute_pulse_position_modded < 0) {
            mPeriodicIO.absolute_pulse_position_modded += 4096;
        }

        int estimated_pulse_pos = ((mConstants.kMasterConstants.invert_sensor_phase ? -1 : 1) * mPeriodicIO.position_ticks) + mPeriodicIO.absolute_pulse_offset;
        int new_wraps = (int) Math.floor(estimated_pulse_pos / 4096.0);
        // Only set this when we are really sure its a valid change
        if (Math.abs(mPeriodicIO.encoder_wraps - new_wraps) <= 1) {
            mPeriodicIO.encoder_wraps = new_wraps;
        }
    }

    if (mCSVWriter != null) {
        mCSVWriter.add(mPeriodicIO);
    }
}
 

/** runs when robot gets disabled */
public void disabledInit() { 
	DriverStation.reportWarning("TELEOP IS DISABLED", false);
	Scheduler.getInstance().removeAll();
}
 

/** 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);
	}
 

/**
 * Start both the left and right camera streams and combine them into a single one which is then pushed
 * to an output stream titled Concat.
 * This method should only be used for starting the camera stream.
 */
private void concatCameraStream() {
    cameraLeft = CameraServer.getInstance().startAutomaticCapture("Left", VisionConstants.LEFT_ID);
    cameraRight = CameraServer.getInstance().startAutomaticCapture("Right", VisionConstants.RIGHT_ID);

    /// Dummy sinks to keep camera connections open.
    CvSink cvsinkLeft = new CvSink("leftSink");
    cvsinkLeft.setSource(cameraLeft);
    cvsinkLeft.setEnabled(true);
    CvSink cvsinkRight = new CvSink("rightSink");
    cvsinkRight.setSource(cameraRight);
    cvsinkRight.setEnabled(true);

    /// Matrices to store each image from the cameras.
    Mat leftSource = new Mat();
    Mat rightSource = new Mat();

    /// The ArrayList of left and right sources is needed for the hconcat method used to combine the streams
    ArrayList<Mat> sources = new ArrayList<>();
    sources.add(leftSource);
    sources.add(rightSource);

    /// Concatenation of both matrices
    Mat concat = new Mat();

    /// Puts the combined video on the SmartDashboard (I think)
    /// The width is multiplied by 2 as the dimensions of the stream will have a width two times that of a single webcam
    CvSource outputStream = CameraServer.getInstance().putVideo("Concat", 2*VisionConstants.CAM_WIDTH, VisionConstants.CAM_HEIGHT);

    while (!Thread.interrupted()) {
        try {
            /// Provide each mat with the current frame
            cvsinkLeft.grabFrame(leftSource);
            cvsinkRight.grabFrame(rightSource);
            /// Combine the frames into a single mat in the Output and stream the image.
            Core.hconcat(sources, concat);
            outputStream.putFrame(concat);
        } catch (CvException cameraFail){
            DriverStation.reportWarning("Concat Cameras: " + cameraFail.toString(), false);
            outputStream.putFrame(failImage);
        }
    }
}
 

@Override
public synchronized void readPeriodicInputs() {
    if (mMaster.hasResetOccurred()) {
        DriverStation.reportError("Wrist Talon Reset! ", false);
    }
    StickyFaults faults = new StickyFaults();
    mMaster.getStickyFaults(faults);
    if (faults.hasAnyFault()) {
        DriverStation.reportError("Wrist Talon Fault! " + faults.toString(), false);
        mMaster.clearStickyFaults(0);
    }
    if (mMaster.getControlMode() == ControlMode.MotionMagic) {
        mPeriodicIO.active_trajectory_position = mMaster.getActiveTrajectoryPosition();

        if (mPeriodicIO.active_trajectory_position < kReverseSoftLimit) {
            DriverStation.reportError("Active trajectory past reverse soft limit!", false);
        } else if (mPeriodicIO.active_trajectory_position > kForwardSoftLimit) {
            DriverStation.reportError("Active trajectory past forward soft limit!", false);
        }
        final int newVel = mMaster.getActiveTrajectoryVelocity();
        // TODO check sign of accel
        if (Util.epsilonEquals(newVel, Constants.kWristCruiseVelocity, 5) ||
                Util.epsilonEquals(newVel, mPeriodicIO.active_trajectory_velocity, 5)) {
            // Wrist is ~constant velocity.
            mPeriodicIO.active_trajectory_acceleration_rad_per_s2 = 0.0;
        } else {
            // Wrist is accelerating.
            mPeriodicIO.active_trajectory_acceleration_rad_per_s2 = Math.signum(newVel - mPeriodicIO
                    .active_trajectory_velocity) * Constants.kWristAcceleration * 20.0 * Math.PI /
                    4096;
        }
        mPeriodicIO.active_trajectory_velocity = newVel;
    } else {
        mPeriodicIO.active_trajectory_position = Integer.MIN_VALUE;
        mPeriodicIO.active_trajectory_velocity = 0;
        mPeriodicIO.active_trajectory_acceleration_rad_per_s2 = 0.0;
    }
    mPeriodicIO.limit_switch = mCanifier.getLimR();
    mPeriodicIO.output_voltage = mMaster.getMotorOutputVoltage();
    mPeriodicIO.output_percent = mMaster.getMotorOutputPercent();
    mPeriodicIO.position_ticks = mMaster.getSelectedSensorPosition(0);
    mPeriodicIO.velocity_ticks_per_100ms = mMaster.getSelectedSensorVelocity(0);

    if (getAngle() > Constants.kWristEpsilon ||
            sensorUnitsToDegrees(mPeriodicIO.active_trajectory_position) > Constants.kWristEpsilon) {
        double wristGravityComponent = Math.cos(Math.toRadians(getAngle())) * (mIntake.hasCube() ? Constants
                .kWristKfMultiplierWithCube : Constants.kWristKfMultiplierWithoutCube);
        double elevatorAccelerationComponent = mElevator.getActiveTrajectoryAccelG() * Constants
                .kWristElevatorAccelerationMultiplier;
        double wristAccelerationComponent = mPeriodicIO.active_trajectory_acceleration_rad_per_s2 *
                (mIntake.hasCube() ? Constants.kWristKaWithCube : Constants.kWristKaWithoutCube);
        mPeriodicIO.feedforward = (elevatorAccelerationComponent) * wristGravityComponent + wristAccelerationComponent;
    } else {
        if (getSetpoint() < Util.kEpsilon) {
            mPeriodicIO.feedforward = -0.1;
        } else {
            mPeriodicIO.feedforward = 0.0;
        }
    }
    if (mCSVWriter != null) {
        mCSVWriter.add(mPeriodicIO);
    }
}
 

protected void initialize() {
	DriverStation.reportWarning("DRIVING BY POWER", false);
    Robot.SUB_DRIVE.pid.reset();
    time = System.currentTimeMillis() + TIME_WAIT;
}
 

protected void end() {
    DriverStation.reportWarning("CyborgCommandDriveDistance finished", false);
    Robot.SUB_DRIVE.driveDirect(0, 0);
}
 

public static void checkError(ErrorCode errorCode, String message) {
    if (errorCode != ErrorCode.OK) {
        DriverStation.reportError(message + errorCode, false);
    }
}
 

protected void end() {
	DriverStation.reportWarning("DONE WAITING", false);
}
 

protected void initialize() {
	DriverStation.reportWarning("WAITING " + wait + " MILISECONDS", false);
	startTime = System.currentTimeMillis();
}
 

protected void end() {
    DriverStation.reportWarning("CyborgCommandRotateDegrees finished", false);
    Robot.SUB_DRIVE.driveDirect(0, 0);
}
 

public static void checkError(CANError errorCode, String message) {
    if (errorCode != CANError.kOK) {
        DriverStation.reportError(message + errorCode, false);
    }
}