Java Code Examples for edu.wpi.first.wpilibj.DriverStation#reportError()

The following examples show how to use edu.wpi.first.wpilibj.DriverStation#reportError() . You can vote up the ones you like or vote down the ones you don't like, and go to the original project or source file by following the links above each example. You may check out the related API usage on the sidebar.
Example 1
Source File: Drive.java    From FRC-2019-Public with MIT License 6 votes vote down vote up 
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);
    }
}
Example 2
Source File: Drive.java    From FRC-2018-Public with MIT License 6 votes vote down vote up 
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);
}
Example 3
Source File: Drive.java    From FRC-2018-Public with MIT License 6 votes vote down vote up 
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);
    }
}
Example 4
Source File: AutoModeBase.java    From FRC-2019-Public with MIT License 5 votes vote down vote up 
public void run() {
    mActive = true;

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

    done();
}
Example 5
Source File: AutoModeBase.java    From FRC-2018-Public with MIT License 5 votes vote down vote up 
public void run() {
    mActive = true;

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

    done();
}
Example 6
Source File: SparkMaxUtil.java    From FRC-2019-Public with MIT License 4 votes vote down vote up 
public static void checkError(CANError errorCode, String message) {
    if (errorCode != CANError.kOK) {
        DriverStation.reportError(message + errorCode, false);
    }
}
Example 7
Source File: SparkMaxFactory.java    From FRC-2019-Public with MIT License 4 votes vote down vote up 
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);
    }
}
Example 8
Source File: ServoMotorSubsystem.java    From FRC-2019-Public with MIT License 4 votes vote down vote up 
@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);
    }
}
Example 9
Source File: Wrist.java    From FRC-2018-Public with MIT License 4 votes vote down vote up 
@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);
    }
}
Example 10
Source File: TalonSRXUtil.java    From FRC-2018-Public with MIT License 4 votes vote down vote up 
public static void checkError(ErrorCode errorCode, String message) {
    if (errorCode != ErrorCode.OK) {
        DriverStation.reportError(message + errorCode, false);
    }
}
Example 11
Source File: TalonSRXUtil.java    From FRC-2019-Public with MIT License 2 votes vote down vote up 
/**
 * checks the specified error code for issues
 *
 * @param errorCode error code
 * @param message   message to print if error happens
 */
public static void checkError(ErrorCode errorCode, String message) {
    if (errorCode != ErrorCode.OK) {
        DriverStation.reportError(message + errorCode, false);
    }
}