Example implementation of pure-pursuit path following for NXT robots

PathBot.java

import lejos.nxt.Button;
import lejos.nxt.addon.GyroSensor;
import lejos.nxt.SensorPort;
import lejos.nxt.NXTMotor;
import lejos.nxt.MotorPort;
import javax.microedition.lcdui.Graphics;

/**
 * This program makes an NXT robot drive a predetermined path. It uses the wheel tachometers (encoders) and
 * a gyro sensor to keep track of the robot's position and heading, then uses a basic "pure pursuit" algorithm
 * to follow a path laid out in real-world coordinates.
 *
 * A basic description of the pure pursuit algorithm can be found in section 4 of this paper:
 * https://ri.cmu.edu/pub_files/pub1/kelly_alonzo_1994_4/kelly_alonzo_1994_4.pdf
 *
 * A basic implementation of the pure pursuit algorithm (upon which this program is based) can be
 * found in this paper:
 * https://www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf
 *
 * To really understand how this algorithm works, a little bit of linear algebra knowledge is recommended.
 * There is a little bit of vector math and coordinate systems involved. If you want an easy and very visual
 * introduction to these concepts, I highly recommend 3blue1brown's excellent Essence of Linear Algebra series
 * on YouTube:
 * https://www.youtube.com/watch?v=fNk_zzaMoSs&list=PLZHQObOWTQDPD3MizzM2xVFitgF8hE_ab
 *
 * If you want to start with the big picture, you may want to look near the bottom of the file, where the main
 * function lives. However, I think it may be easier to start from the top, and make sure you understand the
 * smaller, simpler parts first.
 */
public class PathBot {
    /**
     * How many gyro readings to take during calibration.
     */
    public static int GYRO_SAMPLES = 100;

    /**
     * If the minimum and maximum gyro readings vary during calibration by at least this much,
     * the calibration routine will assume the robot was moving and will retry calibration.
     */
    public static int STILLNESS_THRESHOLD = 5;
    
    /**
     * An empirically measured conversion from wheel tachometer (encoder) ticks to inches.
     * In my measurements, there were an average of 1872 ticks over 3 feet.
     */
    public static int TICKS_PER_INCH = 52;


    // Graphics constants, used for drawing on the NXT screen
    public static int PIXELS_PER_INCH = 2;
    public static int CENTER_X = 10;
    public static int CENTER_Y = 40;


    /**
     * The lookahead distance used in the pure pursuit algorithm. Basically, the algorithm
     * will look down the path to find a point roughly this many inches from the robot's
     * current position.
     */
    public static double MAX_LOOKAHEAD_DISTANCE = 5;

    /**
     * A value used when calculating the proportional constant for the steering. It represents
     * the angle (in radians) at which the steering curvature should have a magnitude of 1.
     *
     * In other words, if this is set to pi/2, and the robot is pointing pi/2 radians away from
     * its goal, the resulting curvature will be 1.
     *
     * Make this bigger if you want looser steering, or smaller if you want tighter steering.
     */
    public static double KP_THETA_MAX = Math.PI / 2;

    GyroSensor gyro = new GyroSensor(SensorPort.S1);
    NXTMotor rightMotor = new NXTMotor(MotorPort.B);
    NXTMotor leftMotor = new NXTMotor(MotorPort.C);

    Graphics g = new Graphics();

    /**
     * Set by the gyro calibration routine. A fixed amount to add to gyro readings so that it
     * reads zero when the robot is sitting still.
     */
    double gyroOffset = 0;

    /**
     * A simple 2D vector (in the linear algebra sense). Used for the robot's position, path points,
     * and other x/y points we want to keep track of.
     */
    public static class Vec2 {
        public double x;
        public double y;

        public Vec2(double x, double y) {
            this.x = x;
            this.y = y;
        }

        public Vec2(Vec2 v) {
            this.x = v.x;
            this.y = v.y;
        }

        public Vec2 add(Vec2 b) {
            this.x += b.x;
            this.y += b.y;

            return this;
        }

        public Vec2 sub(Vec2 b) {
            this.x -= b.x;
            this.y -= b.y;

            return this;
        }

        public Vec2 times(double f) {
            this.x *= f;
            this.y *= f;

            return this;
        }

        /**
         * Rotates the vector about the origin by the specified number of radians. Because this
         * is math, positive radians are counter-clockwise.
         */
        public Vec2 rotate(double rad) {
            double cosAngle = Math.cos(rad);
            double sinAngle = Math.sin(rad);

            double newX = this.x * cosAngle + this.y * sinAngle;
            double newY = -this.x * sinAngle + this.y * cosAngle;

            this.x = newX;
            this.y = newY;

            return this;
        }
    }

    /**
     * Makes the robot pause for the specified number of milliseconds. This is really only here
     * because Thread.sleep throws an exception that we don't care to handle elsewhere.
     */
    public static void sleep(int ms) {
        try {
            Thread.sleep(ms);
        } catch (InterruptedException e) {
            System.out.println("Something is deeply wrong! Goodbye.");
            System.exit(1);
        }
    }

    /**
     * Rounds a double to two decimal places. Used to make things fit on the NXT screen.
     */
    public static double round2(double n) {
        return Math.round(n * 100.0) / 100.0;
    }

    /**
     * Forces the given value into a range. For example, clamp(-10, 0, 1) is 0, clamp(15, 0, 1) is 1,
     * and clamp(0.5, 0, 1) is 0.5.
     */
    public static double clamp(double n, double min, double max) {
        if (n < min) {
            return min;
        } else if (n > max) {
            return max;
        } else {
            return n;
        }
    }

    /**
     * Calibrates the robot's gyro sensor by averaging the reading over many samples.
     */
	void calibrateGyro() {
        System.out.println("Calibrating. Please do not touch the NXT.");

        while (true) {
            int rateSum = 0;
            int minRate = 90909;
            int maxRate = 90909;
            int[] rates = new int[GYRO_SAMPLES];
            for (int i = 0; i < GYRO_SAMPLES; i++) {
                int rate = gyro.readValue();

                if (minRate == 90909 || maxRate == 90909) {
                    minRate = rate;
                    maxRate = rate;
                } else if (rate < minRate) {
                    minRate = rate;
                } else if (maxRate < rate) {
                    maxRate = rate;
                }

                rateSum += rate;
                rates[i] = rate;

                sleep(20);
            }

            if (maxRate - minRate > STILLNESS_THRESHOLD) {
                System.out.println("Trying again. Please do not touch the NXT.");
                continue;
            }

            gyroOffset = rateSum / (double) GYRO_SAMPLES;

            return;
        }
    }

    /**
     * Draws the x and y axes on the NXT's screen. 
     */
    void drawAxes() {
        g.drawLine(CENTER_X, 0, CENTER_X, 63);
        g.drawLine(0, CENTER_Y, 99, CENTER_Y);

        // for (int i = 0; i < 50; i++) {
        //     int tickX = 50 + i * PIXELS_PER_INCH;
        //     g.drawLine(tickX, 31, tickX, 33);
        //     g.drawLine(100 - tickX, 31, 100 - tickX, 33);
        // }

        // for (int i = 0; i < 32; i++) {
        //     int tickY = 32 + i * PIXELS_PER_INCH;
        //     g.drawLine(49, tickY, 51, tickY);
        //     g.drawLine(49, 64 - tickY, 51, 64 - tickY);
        // }
    }

    /**
     * Draws a point in real-world coordinates on the NXT's screen.
     */
    void drawPoint(Vec2 p) {
        int screenX = (int)(CENTER_X + p.x * PIXELS_PER_INCH);
        int screenY = (int)(CENTER_Y + -p.y * PIXELS_PER_INCH);
        g.drawLine(screenX, screenY, screenX, screenY);
    }

    /**
     * Sets the left and right motors to the given powers, where each ranges from
     * -1 (full reverse) to 1 (full forward).
     */
    void tankDrive(double left, double right) {
        leftMotor.setPower((int)(left * 100));
        rightMotor.setPower((int)(right * 100));
    }

    /**
     * Sets the robot to drive a given curvature at a given power, by maintaining a
     * fixed ratio of wheel speeds. Positive curvature means a curve to the right;
     * negative curvature means a curve to the left.
     */
    void curvatureDrive(double power, double curvature) {
        leftMotor.setPower((int)(clamp(curvature < 0 ? 1 + curvature : 1, 0, 1) * power * 100));
        rightMotor.setPower((int)(clamp(curvature < 0 ? 1 : 1 - curvature, 0, 1) * power * 100));
    }

    /**
     * Uses the pure pursuit algorithm to calculate the robot's next drive curvature. Make
     * sure to read the papers linked in the top of this file for a broader overview of
     * the pure pursuit algorithm.
     * 
     * @param  path     The path for the robot to follow.
     * @param  position The current position of the robot.
     * @param  heading  The current heading of the robot.
     * @return          A curvature value to apply to the robot's driving. If the error is very
     *                  extreme, this value will be a large negative number (for an extreme left
     *                  correction) or a large positive number (for an extreme right correction).
     *                  In both extreme cases, the value will have a magnitude greater than 100,
     *                  so you can check if (value > 100) or (value < -100).
     */
    double purePursuitCurvature(Vec2[] path, Vec2 position, double heading) {
        // Find the point on the path closest to the robot. This will
        // be the point at which we start looking our goal point.
        int closestPointIndex = -1;
        Vec2 closestPoint = null;
        double minSquaredDistance = Double.POSITIVE_INFINITY;
        for (int i = 0; i < path.length; i++) {
            Vec2 point = path[i];

            double dx = position.x - point.x;
            double dy = position.y - point.y;
            double distanceSquared = dx * dx + dy * dy;

            if (distanceSquared < minSquaredDistance) {
                closestPointIndex = i;
                closestPoint = point;
                minSquaredDistance = distanceSquared;
            }
        }

        if (closestPoint == null) {
            // all is lost
            System.out.println("Closest point was null!");
            sleep(5000);
            System.exit(1);
        }
        
        // Starting at the closest point to the robot, look down the path until you encounter the
        // first point that is too far away from the robot (according to MAX_LOOKAHEAD_DISTANCE).
        // The last point that was still within our lookahead distance will be our goal point.
        Vec2 goalPoint = closestPoint;
        double lookaheadSquared = MAX_LOOKAHEAD_DISTANCE * MAX_LOOKAHEAD_DISTANCE;
        for (int i = closestPointIndex + 1; i < path.length; i++) {
            Vec2 point = path[i];

            double dx = position.x - point.x;
            double dy = position.y - point.y;
            double distanceSquared = dx * dx + dy * dy;

            if (distanceSquared < lookaheadSquared) {
                goalPoint = point;
            } else {
                break;
            }
        }

        // Transform the goal point into robot (vehicle) coordinates. Basically, we want to shift things
        // so that the robot is at the origin (0, 0), and the robot is pointing along the x axis. Doing
        // this will make it much easier to calculate the angle of the goal point relative to the robot.
        Vec2 goalPointV = new Vec2(goalPoint);
        goalPointV.sub(position).rotate(Math.toRadians(heading));

        // g.drawString("gx: " + goalPointV.x, 0, 27, 0);
        // g.drawString("gy: " + goalPointV.y, 0, 36, 0);

        // Calculate the angle of the goal point relative to the robot. This is mostly just some simple
        // trigonometry, but if the goal point is ever behind the robot, it will return extreme values
        // rather than using any trig.
        double goalTheta = Math.atan(goalPointV.y / goalPointV.x);
        if (goalPointV.x < 0 && goalPointV.y > 0) {
            return -101;
        } else if (goalPointV.x < 0 && goalPointV.y < 0) {
            return 101;
        }

        // Calculate our proportional constant and return the final curvature!
        double kp = -1.0 / KP_THETA_MAX;
        // g.drawString("th: " + Math.toDegrees(goalTheta), 0, 45, 0);
        // g.drawString(" c: " + round2(kp * goalTheta), 0, 54, 0);
        return kp * goalTheta;
    }

    /**
     * The main loop of the robot. This would be pretty much like AutonomousPeriodic, in FRC terms.
     * This is where the main logic of the robot happens.
     */
    void mainLoop() {
        // Initialize things. Setting the motors to zero here is important because it messes with voltages
        // on the robot and changes the gyro readings.
        leftMotor.setPower(0);
        rightMotor.setPower(0);

        calibrateGyro();

        long previousTime = System.currentTimeMillis();

        int previousLeft = 0;
        int previousRight = 0;

        double gyroHeading = 0;
        Vec2 position = new Vec2(0, 0);

        // Define the path the robot will follow. A path in this program is just an array of Vec2's.
        Vec2[] cosine = new Vec2[]{
            new Vec2(0, 0),
            new Vec2(2.5, -Math.cos(2.5/Math.PI) * 5.0 + 5.0),
            new Vec2(5, -Math.cos(5/Math.PI) * 5.0 + 5.0),
            new Vec2(7.5, -Math.cos(7.5/Math.PI) * 5.0 + 5.0),
            new Vec2(10, -Math.cos(10/Math.PI) * 5.0 + 5.0),
            new Vec2(12.5, -Math.cos(12.5/Math.PI) * 5.0 + 5.0),
            new Vec2(15, -Math.cos(15/Math.PI) * 5.0 + 5.0),
            new Vec2(17.5, -Math.cos(17.5/Math.PI) * 5.0 + 5.0),
            new Vec2(20, -Math.cos(20/Math.PI) * 5.0 + 5.0)
        };
        // define other paths here if you like

        Vec2[] path = cosine;

        boolean stopDriving = false; // weird hack plz ignore

        while (true) {
            // Calculate the elapsed time since the last time the loop ran. We call that "deltaTime", because
            // it represents change in time. This is pretty common naming, and you'll recognize it if you've
            // taken a calculus course.
            long newTime = System.currentTimeMillis();
            long deltaTime = newTime - previousTime;
            previousTime = newTime;

            // The NXT gyro sensor actually returns how quickly the robot is rotating, in degrees per second.
            // This means that to calculate our actual heading, we need to add up our degrees per second over time.
            // (If you're familiar with calculus - we're integrating our gyro rate over time.) Basically, each
            // time the loop runs, we'll multiply our gyro rate by the elapsed time to get the total number of
            // degrees we moved. The units check out:
            // 
            // (degrees / second) * seconds = degrees
            // 
            // Note that we use -= instead of += because the gyro sensor uses positive for clockwise rotation.
            // We're doing math with these angles, so we want positive to be counterclockwise rotation.
            double gyroRate = gyro.readValue() - gyroOffset;
            gyroHeading -= gyroRate * (deltaTime / 1000.0);

            // In this section, we figure out the robot's new position by looking at how far we've traveled, and
            // in what direction. To figure out how far we've traveled, we just average the changes in each wheel
            // tachometer. To figure out the heading, we just rely on the gyro value we calculated before. We're
            // not using the tachometers for direction at all, just for distance.
            int newLeft = leftMotor.getTachoCount();
            int newRight = rightMotor.getTachoCount();

            int deltaLeft = newLeft - previousLeft;
            int deltaRight = newRight - previousRight;
            previousLeft = newLeft;
            previousRight = newRight;

            double inchesTraveled = (deltaLeft + deltaRight) / 2.0 / TICKS_PER_INCH; // the (l+r)/2 is to average the left and right sides, then we convert to inches

            double gyroHeadingRad = Math.toRadians(gyroHeading);
            position.add(new Vec2(inchesTraveled * Math.cos(gyroHeadingRad), inchesTraveled * Math.sin(gyroHeadingRad)));

            // Draw some things, if we so desire. This is kinda slow though.
            g.clear();
            // drawAxes();

            // drawPoint(position);
            // for (Vec2 point : path) {
            //     drawPoint(point);
            // }

            // g.drawString("g: " + round2(gyroHeading), 0, 0, 0);
            // g.drawString("x: " + round2(position.x), 0, 9, 0);
            // g.drawString("y: " + round2(position.y), 0, 18, 0);

            // Calculate the power and curvature using the pure pursuit algorithm, then use that to drive!
            double power = 0.6;
            double curvature = purePursuitCurvature(path, position, gyroHeading);

            if (stopDriving) {
                leftMotor.setPower(0);
                rightMotor.setPower(0);
            } else {
                if (curvature > 100) {
                    tankDrive(power, -power);
                } else if (curvature < -100) {
                    tankDrive(-power, power);
                } else {
                    curvatureDrive(power, curvature);
                }
            }

            // Stop driving or break out of this loop if a button is pressed.
            if (Button.ENTER.isDown() || Button.ESCAPE.isDown()) {
                if (!stopDriving) {
                    stopDriving = true;
                    sleep(300);
                } else {
                    break;
                }
            }

            // Just a slight pause to space things out. This might not even be necessary, since
            // Java runs so slowly on the NXT.
            sleep(10);
        }
    }

    // Finally - the main function! As you can see, it simply makes a PathBot instance and
    // starts the main loop.
    public static void main(String[] args) {
        PathBot bot = new PathBot();
        bot.mainLoop();
	}
}