Examples with MotionState com.team254.lib.motion.MotionState used on opensource projects

Example 1 with MotionState

use of com.team254.lib.motion.MotionState in project K-9 by TheGreenMachine.

the class PathFollower method update.

/**
 * Get new velocity commands to follow the path.
 *
 * @param t            The current timestamp
 * @param pose         The current robot pose
 * @param displacement The current robot displacement (total distance driven).
 * @param velocity     The current robot velocity.
 * @return The velocity command to apply
 */
public synchronized Twist2d update(double t, Pose2d pose, double displacement, double velocity) {
    if (!mSteeringController.isFinished()) {
        final AdaptivePurePursuitController.Command steering_command = mSteeringController.update(pose);
        mDebugOutput.lookahead_point_x = steering_command.lookahead_point.x();
        mDebugOutput.lookahead_point_y = steering_command.lookahead_point.y();
        mDebugOutput.lookahead_point_velocity = steering_command.end_velocity;
        mDebugOutput.steering_command_dx = steering_command.delta.dx;
        mDebugOutput.steering_command_dy = steering_command.delta.dy;
        mDebugOutput.steering_command_dtheta = steering_command.delta.dtheta;
        mCrossTrackError = steering_command.cross_track_error;
        mLastSteeringDelta = steering_command.delta;
        mVelocityController.setGoalAndConstraints(new MotionProfileGoal(displacement + steering_command.delta.dx, Math.abs(steering_command.end_velocity), CompletionBehavior.VIOLATE_MAX_ACCEL, mGoalPosTolerance, mGoalVelTolerance), new MotionProfileConstraints(Math.min(mMaxProfileVel, steering_command.max_velocity), mMaxProfileAcc));
        if (steering_command.remaining_path_length < mStopSteeringDistance) {
            doneSteering = true;
        }
    }
    final double velocity_command = mVelocityController.update(new MotionState(t, displacement, velocity, 0.0), t);
    mAlongTrackError = mVelocityController.getPosError();
    final double curvature = mLastSteeringDelta.dtheta / mLastSteeringDelta.dx;
    double dtheta = mLastSteeringDelta.dtheta;
    if (!Double.isNaN(curvature) && Math.abs(curvature) < kReallyBigNumber) {
        // Regenerate angular velocity command from adjusted curvature.
        final double abs_velocity_setpoint = Math.abs(mVelocityController.getSetpoint().vel());
        dtheta = mLastSteeringDelta.dx * curvature * (1.0 + mInertiaGain * abs_velocity_setpoint);
    }
    final double scale = velocity_command / mLastSteeringDelta.dx;
    final Twist2d rv = new Twist2d(mLastSteeringDelta.dx * scale, 0.0, dtheta * scale);
    // Fill out debug.
    mDebugOutput.t = t;
    mDebugOutput.pose_x = pose.getTranslation().x();
    mDebugOutput.pose_y = pose.getTranslation().y();
    mDebugOutput.pose_theta = pose.getRotation().getRadians();
    mDebugOutput.linear_displacement = displacement;
    mDebugOutput.linear_velocity = velocity;
    mDebugOutput.profile_displacement = mVelocityController.getSetpoint().pos();
    mDebugOutput.profile_velocity = mVelocityController.getSetpoint().vel();
    mDebugOutput.velocity_command_dx = rv.dx;
    mDebugOutput.velocity_command_dy = rv.dy;
    mDebugOutput.velocity_command_dtheta = rv.dtheta;
    mDebugOutput.cross_track_error = mCrossTrackError;
    mDebugOutput.along_track_error = mAlongTrackError;
    return rv;
}

Example 2 with MotionState

use of com.team254.lib.motion.MotionState in project K-9 by TheGreenMachine.

the class Path method verifySpeeds.

/**
 * Ensures that all speeds in the path are attainable and robot can slow down in time
 */
public void verifySpeeds() {
    double maxStartSpeed = 0.0;
    double[] startSpeeds = new double[segments.size() + 1];
    startSpeeds[segments.size()] = 0.0;
    for (int i = segments.size() - 1; i >= 0; i--) {
        PathSegment segment = segments.get(i);
        maxStartSpeed += Math.sqrt(maxStartSpeed * maxStartSpeed + 2 * Constants.kPathFollowingMaxAccel * segment.getLength());
        startSpeeds[i] = segment.getStartState().vel();
        // System.out.println(maxStartSpeed + ", " + startSpeeds[i]);
        if (startSpeeds[i] > maxStartSpeed) {
            startSpeeds[i] = maxStartSpeed;
        // System.out.println("Segment starting speed is too high!");
        }
        maxStartSpeed = startSpeeds[i];
    }
    for (int i = 0; i < segments.size(); i++) {
        PathSegment segment = segments.get(i);
        double endSpeed = startSpeeds[i + 1];
        MotionState startState = (i > 0) ? segments.get(i - 1).getEndState() : new MotionState(0, 0, 0, 0);
        startState = new MotionState(0, 0, startState.vel(), startState.vel());
        segment.createMotionProfiler(startState, endSpeed);
    }
}