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

Example 1 with MotionProfileGoal

use of com.team254.lib.motion.MotionProfileGoal 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;
}