Java Code Examples for edu.wpi.first.wpilibj.Timer#getFPGATimestamp()

public double calculate(double input) {
    double timestamp = Timer.getFPGATimestamp();
    double dt = timestamp - m_last_timestamp;
    m_last_timestamp = timestamp;

    return calculate(input, dt);
}
 

public synchronized Optional<AimingParameters> getAimingParameters(boolean highTarget, int prev_track_id, double max_track_age) {
    GoalTracker tracker = highTarget ? vision_target_high_ : vision_target_low_;
    List<GoalTracker.TrackReport> reports = tracker.getTracks();

    if (reports.isEmpty()) {
        return Optional.empty();
    }

    double timestamp = Timer.getFPGATimestamp();

    // Find the best track.
    TrackReportComparator comparator = new TrackReportComparator(
            Constants.kTrackStabilityWeight,
            Constants.kTrackAgeWeight,
            Constants.kTrackSwitchingWeight,
            prev_track_id, timestamp);
    reports.sort(comparator);

    GoalTracker.TrackReport report = null;
    for (GoalTracker.TrackReport track : reports) {
        if (track.latest_timestamp > timestamp - max_track_age) {
            report = track;
            break;
        }
    }
    if (report == null) {
        return Optional.empty();
    }
    Pose2d vehicleToGoal = getFieldToVehicle(timestamp).inverse().transformBy(report.field_to_target).transformBy(getVisionTargetToGoalOffset());

    AimingParameters params = new AimingParameters(vehicleToGoal,
            report.field_to_target,
            report.field_to_target.getRotation(),
            report.latest_timestamp, report.stability, report.id);
    return Optional.of(params);
}
 

@Override
public void update() {
    double percentPower = kRampRate * (Timer.getFPGATimestamp() - mStartTime);
    if (percentPower > kMaxPower) {
        isFinished = true;
        return;
    }
    mDrive.setOpenLoop(new DriveSignal((mReverse ? -1.0 : 1.0) * percentPower, (mReverse ? -1.0 : 1.0) * (mTurn ? -1.0 : 1.0) * percentPower));
    mVelocityData.add(new DriveCharacterization.VelocityDataPoint(
            (Math.abs(mDrive.getLeftVelocityNativeUnits()) + Math.abs(mDrive.getRightVelocityNativeUnits())) / 4096.0 * Math.PI * 10, //convert velocity to radians per second
            percentPower * 12.0 //convert to volts
    ));
    mCSVWriter.add(mVelocityData.get(mVelocityData.size() - 1));

}
 

public synchronized void stop() {
    if (mRunning) {
        System.out.println("Stopping loops");
        mNotifier.stop();

        synchronized (mTaskRunningLock) {
            mRunning = false;
            mTimestamp = Timer.getFPGATimestamp();
            for (Loop loop : mLoops) {
                System.out.println("Stopping " + loop);
                loop.onStop(mTimestamp);
            }
        }
    }
}
 

public synchronized void stop() {
    if (running_) {
        System.out.println("Stopping loops");
        notifier_.stop();
        synchronized (taskRunningLock_) {
            running_ = false;
            timestamp_ = Timer.getFPGATimestamp();
            for (Loop loop : loops_) {
                System.out.println("Stopping " + loop);
                loop.onStop(timestamp_);
            }
        }
    }
}
 

@Override
public void start() {
    mDrive.setHighGear(mHighGear);
    mDrive.setOpenLoop(new DriveSignal((mReverse ? -1.0 : 1.0) * kPower, (mReverse ? -1.0 : 1.0) * (mTurn ? -1.0 : 1.0) * kPower));
    mStartTime = Timer.getFPGATimestamp();
    mPrevTime = mStartTime;
}
 

@Override
public void start() {
    mDrive.setHighGear(mHighGear);
    mDrive.setOpenLoop(new DriveSignal((mReverse ? -1.0 : 1.0) * kStartPower, (mReverse ? -1.0 : 1.0) * (mTurn ? -1.0 : 1.0) * kStartPower));
    mStartTime = Timer.getFPGATimestamp();
    mPrevTime = mStartTime;
}
 

@Override
public void start() {
    mStartTime = Timer.getFPGATimestamp();
    mIntake.setState(IntakeStateMachine.WantedAction.WANT_MANUAL);
    mIntake.setPower(0.0);
    mIntake.tryOpenJaw();
}
 

/**
 * @return true if the robot is receiving data from the scale tracker
 */
public synchronized boolean isConnected() {
    return Timer.getFPGATimestamp() < mLastHeartbeatTime + Constants.kScaleTrackerTimeout &&
            !Double.isNaN(mAngle);
}
 

@Override
public void start() {
    mDrive.setHighGear(mHighGear);
    mDrive.setOpenLoop(new DriveSignal(kStartPower, kStartPower));
    mStartTime = Timer.getFPGATimestamp();
}
 

@Override
public boolean isFinished() {
    return Timer.getFPGATimestamp() - mStartTime > kTotalTime;
}
 

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

@Override
public void start() {
    mStartTime = Timer.getFPGATimestamp();
}
 

public synchronized void updateZeroed() {
    mLastZeroTime = Timer.getFPGATimestamp();
}
 

@Override
public boolean isFinished() {
    return Timer.getFPGATimestamp() - mStartTime >= mTimeToWait;
}
 

@Override
public void start() {
    mDrive.setHighGear(mHighGear);
    mStartTime = Timer.getFPGATimestamp();
}
 

@Override
public void start() {
    mDrive.setHighGear(true);
    mDrive.setOpenLoop(new DriveSignal((mReverse ? -1.0 : 1.0) * kPower, (mReverse ? -1.0 : 1.0) * kPower));
    mStartTime = Timer.getFPGATimestamp();
}
 

@Override
public void start() {
    mStartTime = Timer.getFPGATimestamp();
}
 

@Override
public boolean isFinished() {
    return Timer.getFPGATimestamp() - mStartTime > kShootTime;
}
 

@Override
public boolean isFinished() {
    return Timer.getFPGATimestamp() - mStartTime > kTime;
}