Pure Pursuit Algorythm

int coordsX [] = {};
int coordsY [] = {};
int counter = 0;
int setLookAhead = 60;
float curveVelocityPenaltyCoef = 0.0;
float driveVelocityTurnPenaltyCoef = 0.0;
int width = 180;
int trackWidth = 210;
float lookAheadGain = 1.0;

while (true){
    //psudo code for defining right and left x and y for the drive

    float lookAhead = setLookAhead * (curveVelocity * lookAheadGain);

    int l = (coordsX[counter] - robotX) ^ 2 + (coordsY[counter] - robotY) ^ 2;
    float curvature = ( 2 * coordsX[counter] + .00000000001) / l;

    float side = sgn(sin(robotAngle) * (xLPos - xRPos) - cos(robotAngle) * (yLPos - yRPos));

    //to find the distance to the next point in the path, if less, then use it, if more, don't, keep the current one
    if((sqrt((coordsX[counter + 1] - robotX) ^ 2 + (coordsY[counter + 1] - robotY) ^ 2)) < lookAhead){
        counter++;
    }

    float curveVelocity = curvature / (curveVelocityPenaltyCoef + 1);

    float leftDrive = (curveVelocity * ((2 + (curvature * trackWidth)) / 2));
    float rightDrive = (curveVelocity * ((2 - (curvature * trackWidth)) / 2));

    //add PID to control motors

}