edu.wpi.first.wpilibj.Timer Java Examples

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

public synchronized void autoSteer(double throttle, AimingParameters aim_params) {
    double timestamp = Timer.getFPGATimestamp();
    final double kAutosteerAlignmentPointOffset = 15.0;  // Distance from wall
    boolean reverse = throttle < 0.0;
    boolean towards_goal = reverse == (Math.abs(aim_params.getRobotToGoalRotation().getDegrees()) > 90.0);
    Pose2d field_to_vision_target = aim_params.getFieldToGoal();
    final Pose2d vision_target_to_alignment_point = Pose2d.fromTranslation(new Translation2d(Math.min(kAutosteerAlignmentPointOffset, aim_params.getRange() - kAutosteerAlignmentPointOffset), 0.0));
    Pose2d field_to_alignment_point = field_to_vision_target.transformBy(vision_target_to_alignment_point);
    Pose2d vehicle_to_alignment_point = RobotState.getInstance().getFieldToVehicle(timestamp).inverse().transformBy(field_to_alignment_point);
    Rotation2d vehicle_to_alignment_point_bearing = vehicle_to_alignment_point.getTranslation().direction();
    if (reverse) {
        vehicle_to_alignment_point_bearing = vehicle_to_alignment_point_bearing.rotateBy(Rotation2d.fromDegrees(180.0));
    }
    double heading_error_rad = vehicle_to_alignment_point_bearing.getRadians();

    final double kAutosteerKp = 0.05;
    double curvature = (towards_goal ? 1.0 : 0.0) * heading_error_rad * kAutosteerKp;
    setOpenLoop(Kinematics.inverseKinematics(new Twist2d(throttle, 0.0, curvature * throttle * (reverse ? -1.0 : 1.0))));
    setBrakeMode(true);
}
 

@Test
public void test() {
    MultiTrigger trigger = new MultiTrigger(0.1);
    trigger.update(true);
    Assert.assertTrue(trigger.wasTapped());
    Assert.assertTrue(trigger.isPressed());

    trigger.update(true);
    Assert.assertFalse(trigger.wasTapped());
    Assert.assertTrue(trigger.isPressed());

    Timer.delay(0.05);
    Assert.assertFalse(trigger.isHeld());
    Assert.assertTrue(trigger.isPressed());

    Timer.delay(0.1);
    Assert.assertTrue(trigger.isHeld());
    Assert.assertTrue(trigger.isPressed());

    trigger.update(false);
    Assert.assertFalse(trigger.wasTapped());
    Assert.assertFalse(trigger.isPressed());
    Assert.assertFalse(trigger.isHeld());
}
 

private void handleLine(String line) {
    boolean isNewScan = line.substring(line.length() - 1).equals("s");
    if (isNewScan) {
        line = line.substring(0, line.length() - 1);
    }

    long curSystemTime = System.currentTimeMillis();
    double curFPGATime = Timer.getFPGATimestamp();

    String[] parts = line.split(",");
    if (parts.length == 3) {
        try {
            long ts = Long.parseLong(parts[0]);
            long ms_ago = curSystemTime - ts;
            double normalizedTs = curFPGATime - (ms_ago / 1000.0f);
            double angle = Double.parseDouble(parts[1]);
            double distance = Double.parseDouble(parts[2]);
            if (distance != 0)
                mLidarProcessor.addPoint(new LidarPoint(normalizedTs, angle, distance), isNewScan);
        } catch (java.lang.NumberFormatException e) {
            e.printStackTrace();
        }
    }
}
 

@Override
public void update() {
    synchronized (mDrive) {
        double dt = Timer.getFPGATimestamp() - mStartTime;
        double percentPower = kRampRate * dt;
        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));
        double velocity = mDrive.getAverageDriveVelocityMagnitude() / Constants.kDriveWheelRadiusInches;  // rad/s
        if (velocity < 0.5) {
            // Small velocities tend to be untrustworthy.
            return;
        }
        mVelocityData.add(new DriveCharacterization.DataPoint(
                velocity, // rad/s
                percentPower * 12.0, // convert to volts
                dt
        ));
    }
    mCSVWriter.add(mVelocityData.get(mVelocityData.size() - 1));
}
 

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

@Override
public void update() {
    double t = Timer.getFPGATimestamp();
    Superstructure superstructure = Superstructure.getInstance();
    Optional<AimingParameters> aimParams = superstructure.getLatestAimingParameters();
    if (!aimParams.isEmpty() && !mFinishedExtending) {
        if (superstructure.isOnTarget()) {
            SuperstructureCommands.goToAutoScore();
            if (superstructure.isAtDesiredState() || mTimedOutExtending.update(t, true)) {
                EndEffector.getInstance().setWantedAction(EndEffectorStateMachine.WantedAction.EXHAUST);
                mHasShot = true;
            }
        }
    }
    mFinishedExtending = mHasExtendedLongEnough.update(t, mHasShot);
    if (mFinishedExtending) {
        SuperstructureCommands.goToPreAutoThrust();
        mFinished = mHasShotLongEnough.update(t, true);
    }
}
 

@Override
public void update() {
    double t = Timer.getFPGATimestamp() - mStartTime;
    if (t < kStartTime) { //give the robot some time to accelerate before recording data
        return;
    }
    double rightPower = kStartPower + (t - kStartTime) * kRampRate;
    if (rightPower > kMaxPower) {
        isFinished = true;
        return;
    }
    mDrive.setOpenLoop(new DriveSignal((mReverse ? -1.0 : 1.0) * kStartPower, (mReverse ? -1.0 : 1.0) * rightPower));
    mCurvatureData.add(new DriveCharacterization.CurvatureDataPoint(
            mRobotState.getPredictedVelocity().dx,
            mRobotState.getPredictedVelocity().dtheta,
            kStartPower,
            rightPower
    ));
    mCSVWriter.add(mCurvatureData.get(mCurvatureData.size() - 1));
}
 

@Override
public TurretCardinal getTurretCardinal() {
    int dPad = mController.getDPad();
    TurretCardinal newCardinal = dPad == -1 ? TurretCardinal.NONE : TurretCardinal.findClosest(Rotation2d.fromDegrees(-dPad));
    if (newCardinal != TurretCardinal.NONE && TurretCardinal.isDiagonal(newCardinal)) {
        // Latch previous direction on diagonal presses, because the D-pad sucks at diagonals.
        newCardinal = mLastCardinal;
    }
    boolean valid = mDPadValid.update(Timer.getFPGATimestamp(), newCardinal != TurretCardinal.NONE && (mLastCardinal == TurretCardinal.NONE || newCardinal == mLastCardinal));
    if (valid) {
        if (mLastCardinal == TurretCardinal.NONE) {
            mLastCardinal = newCardinal;
        }
        return mLastCardinal;
    } else {
        mLastCardinal = newCardinal;
    }
    return TurretCardinal.NONE;
}
 

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) * kStartPower, (mReverse ? -1.0 : 1.0) * (mTurn ? -1.0 : 1.0) * kStartPower));
    mStartTime = Timer.getFPGATimestamp();
    mPrevTime = mStartTime;
}
 

@Override
public void runCrashTracked() {
    synchronized (mTaskRunningLock) {
        if (mRunning) {
            double now = Timer.getFPGATimestamp();

            for (Loop loop : mLoops) {
                loop.onLoop(now);
            }

            mDT = now - mTimestamp;
            mTimestamp = now;
        }
    }
}
 

@Override
public synchronized void readPeriodicInputs() {
    mPeriodicIO.timestamp = Timer.getFPGATimestamp();
    double prevLeftTicks = mPeriodicIO.left_position_ticks;
    double prevRightTicks = mPeriodicIO.right_position_ticks;
    mPeriodicIO.left_voltage = mLeftMaster.getAppliedOutput() * mLeftMaster.getBusVoltage();
    mPeriodicIO.right_voltage = mRightMaster.getAppliedOutput() * mRightMaster.getBusVoltage();

    mPeriodicIO.left_position_ticks = mLeftEncoder.get();
    mPeriodicIO.right_position_ticks = mRightEncoder.get();
    mPeriodicIO.gyro_heading = Rotation2d.fromDegrees(mPigeon.getFusedHeading()).rotateBy(mGyroOffset);

    double deltaLeftTicks = ((mPeriodicIO.left_position_ticks - prevLeftTicks) / Constants.kDriveEncoderPPR)
            * Math.PI;
    mPeriodicIO.left_distance += deltaLeftTicks * Constants.kDriveWheelDiameterInches;

    double deltaRightTicks = ((mPeriodicIO.right_position_ticks - prevRightTicks) / Constants.kDriveEncoderPPR)
            * Math.PI;
    mPeriodicIO.right_distance += deltaRightTicks * Constants.kDriveWheelDiameterInches;

    mPeriodicIO.left_velocity_ticks_per_100ms = (int) (mLeftEncoder.getRate()
            / (10 * mLeftEncoder.getDistancePerPulse()));
    mPeriodicIO.right_velocity_ticks_per_100ms = (int) (mRightEncoder.getRate()
            / (10 * mRightEncoder.getDistancePerPulse()));

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

/**
 * Removes the track if it is older than the set "age" described in the Constants file.
 *
 * @see Constants
 */
synchronized void pruneByTime() {
    double delete_before = Timer.getFPGATimestamp() - Constants.kMaxGoalTrackAge;
    mObservedPositions.entrySet().removeIf(entry -> entry.getKey() < delete_before);
    if (mObservedPositions.isEmpty()) {
        mSmoothedPosition = null;
    } else {
        smooth();
    }
}
 

@Override
public synchronized void readPeriodicInputs() {
    final double t = Timer.getFPGATimestamp();
    mPeriodicIO.position_ticks = mMaster.getSelectedSensorPosition(0);
    mPeriodicIO.velocity_ticks_per_100ms = mMaster.getSelectedSensorVelocity(0);
    if (mMaster.getControlMode() == ControlMode.MotionMagic) {
        mPeriodicIO.active_trajectory_position = mMaster.getActiveTrajectoryPosition();
        final int newVel = mMaster.getActiveTrajectoryVelocity();
        // TODO check sign of elevator accel
        if (Util.epsilonEquals(newVel, Constants.kElevatorHighGearCruiseVelocity, 5) ||
                Util.epsilonEquals(newVel, mPeriodicIO.active_trajectory_velocity, 5)) {
            // Elevator is ~constant velocity.
            mPeriodicIO.active_trajectory_accel_g = 0.0;
        } else if (newVel > mPeriodicIO.active_trajectory_velocity) {
            // Elevator is accelerating downwards.
            mPeriodicIO.active_trajectory_accel_g = -Constants.kElevatorHighGearAcceleration * 10.0 /
                    (kEncoderTicksPerInch * 386.09);
        } else {
            // Elevator is accelerating upwards.
            mPeriodicIO.active_trajectory_accel_g = Constants.kElevatorHighGearAcceleration * 10.0 /
                    (kEncoderTicksPerInch * 386.09);
        }
        mPeriodicIO.active_trajectory_velocity = newVel;
    } else {
        mPeriodicIO.active_trajectory_position = Integer.MIN_VALUE;
        mPeriodicIO.active_trajectory_velocity = 0;
        mPeriodicIO.active_trajectory_accel_g = 0.0;
    }
    mPeriodicIO.output_percent = mMaster.getMotorOutputPercent();
    mPeriodicIO.limit_switch = mMaster.getSensorCollection().isFwdLimitSwitchClosed();
    mPeriodicIO.t = t;

    if (getInchesOffGround() > Constants.kElevatorEpsilon && mShifter.get()) {
        mPeriodicIO.feedforward = mIntake.hasCube() ? Constants.kElevatorFeedforwardWithCube : Constants
                .kElevatorFeedforwardNoCube;
    } else {
        mPeriodicIO.feedforward = 0.0;
    }
}
 

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 boolean isFinished() {
    if (mWaitUntilHasCube) {
        boolean timedOut = Timer.getFPGATimestamp() - mStartTime > 0.5;
        if (timedOut) {
            System.out.println("Timed out!!!!!");
        }
        return mDebounce.update(mIntake.definitelyHasCube(), .1) || timedOut;
    } else {
        return true;
    }
}
 

@Override
public void runCrashTracked() {
    synchronized (taskRunningLock_) {
        if (running_) {
            double now = Timer.getFPGATimestamp();

            for (Loop loop : loops_) {
                loop.onLoop(now);
            }

            dt_ = now - timestamp_;
            timestamp_ = now;
        }
    }
}
 

@Override
public void start() {
    if (mMoveToIntakingPosition) {
        mSuperstructure.setDesiredAngle(SuperstructureConstants.kIntakePositionAngle);
        mSuperstructure.setDesiredHeight(SuperstructureConstants.kIntakeFloorLevelHeight);
    }
    mIntake.getOrKeepCube();
    mStartTime = Timer.getFPGATimestamp();
}
 

public synchronized void start() {
    if (!running_) {
        System.out.println("Starting loops");
        synchronized (taskRunningLock_) {
            timestamp_ = Timer.getFPGATimestamp();
            for (Loop loop : loops_) {
                loop.onStart(timestamp_);
            }
            running_ = true;
        }
        notifier_.startPeriodic(kPeriod);
    }
}
 

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

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

@Override
public void update() {
    double currentVelocity = (Math.abs(mDrive.getLeftVelocityNativeUnits()) + Math.abs(mDrive.getRightVelocityNativeUnits())) / 4096.0 * Math.PI * 10;
    double currentTime = Timer.getFPGATimestamp();

    //don't calculate acceleration until we've populated prevTime and prevVelocity
    if (mPrevTime == mStartTime) {
        mPrevTime = currentTime;
        mPrevVelocity = currentVelocity;
        return;
    }

    double acceleration = (currentVelocity - mPrevVelocity) / (currentTime - mPrevTime);

    //ignore accelerations that are too small
    if (acceleration < Util.kEpsilon) {
        mPrevTime = currentTime;
        mPrevVelocity = currentVelocity;
        return;
    }

    mAccelerationData.add(new DriveCharacterization.AccelerationDataPoint(
            currentVelocity, //convert to radians per second
            kPower * 12.0, //convert to volts
            acceleration
    ));

    mCSVWriter.add(mAccelerationData.get(mAccelerationData.size() - 1));

    mPrevTime = currentTime;
    mPrevVelocity = currentVelocity;
}
 

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

}
 

@Override
public void start() {
    System.out.println("Starting trajectory! (length=" + mTrajectory.getRemainingProgress() + ")");
    if (mResetPose) {
        mRobotState.reset(Timer.getFPGATimestamp(), mTrajectory.getState().state().getPose());
    }
    mDrive.setTrajectory(mTrajectory);
}
 

@Override
public boolean isFinished() {
    if (Timer.getFPGATimestamp() - mStartTime > kTimeout) {
        System.out.println("Auto Superstructure Position timed out!!!");
        return true;
    }
    if (mWaitForCompletion) {
        SuperstructureState state = mSuperstructure.getObservedState();
        return Util.epsilonEquals(state.height, mHeight, kHeightEpsilon) &&
                Util.epsilonEquals(state.angle, mAngle, kAngleEpsilon);
    } else {
        return true;
    }
}
 

@Override
public void start() {
    double cheesyVisionHeight = mCheesyVision2.getDesiredHeight((mAngle == SuperstructureConstants.kScoreBackwardsAngle), mNumCubes, mUseKickstand);
    if (cheesyVisionHeight < mMinHeight) {
        mHeight = mMinHeight;
    } else {
        mHeight = cheesyVisionHeight;
    }
    mSuperstructure.setDesiredHeight(mHeight);
    mSuperstructure.setDesiredAngle(mAngle);
    mStartTime = Timer.getFPGATimestamp();
}
 

@Override
public boolean isFinished() {
    if (Timer.getFPGATimestamp() - mStartTime > kTimeout) {
        System.out.println("Set Superstructure Position timed out!!!");
        return true;
    }
    if (mWaitForCompletion) {
        SuperstructureState state = mSuperstructure.getObservedState();
        return Util.epsilonEquals(state.height, mHeight, kHeightEpsilon) &&
                Util.epsilonEquals(state.angle, mAngle, kAngleEpsilon);
    } else {
        return true;
    }
}