#pragma config(Sensor, S2, COMPASS, sensorI2CCustom9V)
#pragma config(Motor, motorB, LEFT, tmotorNormal, PIDControl, encoder)
#pragma config(Motor, motorC, RIGHT, tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

// Xander's driver suite
#include "hitechnic-compass.h"

//**********************************************************//
//* *//
//* PoseNXC - LEGO NXT Robotics with Pose *//
//* *//
//**********************************************************//

#define __POSE_DEBUG__
#include "pose.h"

// shorthand for the typing lazy
intrinsic void _T(const string sFormatString, ...)
asm(opcdStringOps, strOpDebugStreamFormat, variableRefString(sFormatString), varArgList);

intrinsic void _TL(const string sFormatString, ...)
asm(opcdStringOps, strOpDebugStreamFormatWithNewLine, variableRefString(sFormatString), varArgList);

task DisplayPose()
{
while (true)
{
nxtDisplayTextLine(3, "# Pose@ %d", PoseClock);
nxtDisplayTextLine(4, "#-x,y %1.1f, %1.1f", PoseX, PoseY);
nxtDisplayTextLine(5, "#-hdg %d", PoseHdg);
nxtDisplayTextLine(6, "#-spd %1.1f", PoseVelocity);
wait1Msec(500);
}
}

void ShowPose(string t)
{
#ifdef _DEBUG
_TL("-%s", t);
_TL("--x,y %1.1f, %1.1f", PoseX, PoseY);
_TL("--hdg %d, %d", PoseHdg, HTMCreadRelativeHeading(COMPASS));
#endif
}

//**********************************************************//
//* *//
//* Main task *//
//* *//
//**********************************************************//

task main()
{
eraseDisplay();
nxtDisplayCenteredBigTextLine(0, "PoseNXT1");
_TL("");
_TL("***PoseNXT1");

time1[T1] = 0;

// should help to actually measure the tire, LEGO specs it at 81.6
// (float wheelDiameter, float wheelBase, float finalGearRatio, tMotor left, tMotor right, TSynchedMotors useForSynch, int movePower, int turnPower, int updateRate = 5)
PoseStartTask(81.6, 120.0, (float)(16/16), LEFT, RIGHT, synchBC, 50, 30);

StartTask(DisplayPose);
HTMCsetTarget(COMPASS);
PlaySound(soundBeepBeep);

// PilotMove(250.0);
// wait1Msec(500);
// PilotMove(-250.0);

// a lap done with moves and turns
for (int i = 0; i < 4; i++)
{
ShowPose("*move");
PilotMove(200.0);
PilotWaitDrivesIdle();
ShowPose("#move");

// simple
// PilotTurn(90);

// using corrected Turn
// PilotTurn(PilotQuickestTurnTo(PoseHdg, (i+1)*90));

// using TurnTo (which will internally correct)
// correct PoseHdg before we turn if compass available (will work with or without)
PoseHdg = PilotHdg360(HTMCreadRelativeHeading(COMPASS));
PilotTurnTo((i+1)*90);

wait1Msec(50); // give it as little time to update Pose
ShowPose("#turn");
}

// another lap done with GoTo
// PilotGoTo(0.0, 200.0);
// PilotGoTo(200.0, 200.0);
// PilotGoTo(200.0, 0.0);
// PilotGoTo(0.0, 0.0);
// PilotTurnTo(0);

PlaySound(soundBeepBeep);

bFloatDuringInactiveMotorPWM = true;
motor[LEFT] = 0;
motor[RIGHT] = 0;

ShowPose("final");
StopTask(PoseUpdate);

while (bSoundActive)
wait1Msec(100);

_TL("###PoseNXT");

while(true)
EndTimeSlice(); // use dark grey button to exit
}

The program was written for a previous version of ROBOTC and mikep kindly modified the library pose.h in this way to work under 3.5 version:

/changes
// 9/12/12 - update to work with robotc 3.5

bool _hasBeenInitialized = false;

// geometry info

float _wheelDiameter; // in some global unit
float _finalGearRatio; // generally spur / pinion
float _wheelBase;
int _PoseUpdateRate; // times per second
int _movePower, _turnPower;
tMotor _left, _right;
TSynchedMotors _useForSynch;

float _wheelTravelPerRevolution;
int _ticksPerWheelRevolution;
float _wheelBaseDiameter;
int _ticksForFullPilotTurn;

// Global Pose Variables
long PoseClock = 0; /*!< time (in ticks) the last update occurred */
short PoseHdg = 0; /*!< current heading in degrees */
float PoseX = 0.0, /*!< current X location */
PoseY = 0.0, /*!< current Y location */
PoseVelocity; /*!< current velocity */

task PoseUpdate();

/*!
* Initialize geomtery variables, Performs one time calculations, and starts the task PoseUpdate().
* All measurements must be in the same units, angles are in degrees. The example uses MM.
* @param wheelDiameter wheel diameter
* @param wheelBase wheel base
* @param finalGearRatio usually calculated as spur (the gear on the wheel) / pinion (the gear on the motor)
* @param left the motor that is driving the left wheel
* @param right the motor that is driving the right wheel
* @param useForSynch the syncMode to use for robotc, typically synchBC
* @param movePower the power to apply for a straight line move
* @param turnPower the power to apply when rotating to a new heading
* @param updateRate times per second the pose should be updated, the default is 5
*/
void PoseStartTask(float wheelDiameter, float wheelBase, float finalGearRatio, tMotor left, tMotor right, TSynchedMotors useForSynch, int movePower, int turnPower, int updateRate = 5)
{
_wheelDiameter = wheelDiameter;
_wheelBase = wheelBase;
_finalGearRatio = finalGearRatio;
_useForSynch = useForSynch;
_movePower = movePower;
_turnPower = turnPower;
_PoseUpdateRate = updateRate; // times per second
//_CompassInterval = compassInterval;
_left = left;
_right = right;

// one time calculated values
_wheelTravelPerRevolution = _wheelDiameter * PI;
_ticksPerWheelRevolution = 360 * _finalGearRatio;
_wheelBaseDiameter = _wheelBase * PI;
_ticksForFullPilotTurn = _wheelBaseDiameter * _ticksPerWheelRevolution / _wheelTravelPerRevolution;

#ifdef __POSE_DEBUG__
writeDebugStreamLine("PoseStartTask");
writeDebugStreamLine("wd %1.1f", _wheelDiameter);
writeDebugStreamLine("wb %1.1f", _wheelBase);
writeDebugStreamLine("fgr %1.1f", _finalGearRatio);

writeDebugStreamLine("wtpr %1.1f", _wheelTravelPerRevolution);
writeDebugStreamLine("tpwr %d", _ticksPerWheelRevolution);
writeDebugStreamLine("wbd %1.1f", _wheelBaseDiameter);
writeDebugStreamLine("tfft %d", _ticksForFullPilotTurn);
#endif

_hasBeenInitialized = true;
StartTask(PoseUpdate);
}

void PoseException()
{
hogCPU();
eraseDisplay();
PlaySound(soundException);
nxtDisplayCenteredTextLine(0, "Missing call to");
nxtDisplayCenteredTextLine(1, "PoseStartTask");
nxtDisplayCenteredTextLine(2, "Program aborted");
writeDebugStreamLine("Missing call to");
writeDebugStreamLine("PoseStartTask");
writeDebugStreamLine("Program aborted");
wait1Msec(10000);
StopAllTasks();
}

/*!
* This task is called by the NXT OS and will update the global pose variables to their current value.
* PoseStartTask() must be called before this task is started, and is automatically initially started by it.
* This task may be stopped and restarted by the user, and it will re-initialize Pose information.
*/
task PoseUpdate()
{
long lastEncoderLeft = 0;
long lastEncoderRight = 0;
long nowEncoderLeft;
long nowEncoderRight;
float leftDistance;
float rightDistance;
float poseTheta = 0.0;
float distance;
float xChange, yChange;
long deltaLeft, deltaRight;

if (!_hasBeenInitialized)
PoseException();

nSchedulePriority = 10; // we are pretty important

PoseX = 0.0;
PoseY = 0.0;
PoseHdg = 0;
PoseVelocity = 0.0;
nSyncedMotors = synchNone; // so we can reset both encoders
nMotorEncoder[_left] = 0;
nMotorEncoder[_right] = 0;
nSyncedMotors = _useForSynch; // back to 'normal' mode

while (true)
{
long elapsedTime;

hogCPU();
long nowTime = time1[T1];
elapsedTime = nowTime - PoseClock;
PoseClock = nowTime;

nowEncoderLeft = nMotorEncoder[_left];
nowEncoderRight = nMotorEncoder[_right];

deltaLeft = nowEncoderLeft - lastEncoderLeft;
deltaRight = nowEncoderRight - lastEncoderRight;

leftDistance = deltaLeft * _wheelTravelPerRevolution / (float)_ticksPerWheelRevolution;
rightDistance = deltaRight * _wheelTravelPerRevolution / (float)_ticksPerWheelRevolution;

distance = (leftDistance + rightDistance) / 2.0;
PoseVelocity = distance / elapsedTime / 1000;

//float poseTheta1 = MSIMUreadGyroZAxis(AIMU)/ 100.0; // using IMU gyro in radians
float poseTheta2 = ((leftDistance - rightDistance ) / _wheelBase); // using odemetry, in radians
poseTheta += poseTheta2;

// these (trig functions) are reversed from normal thinking,
// compass direction of 0 indicates North = a movement up the Y axis
xChange = distance * sin(poseTheta);
yChange = distance * cos(poseTheta);

PoseX += xChange;
PoseY += yChange;
PoseHdg = poseTheta * 180.0 / PI; // in degrees

lastEncoderLeft = nowEncoderLeft;
lastEncoderRight = nowEncoderRight;

releaseCPU();
wait1Msec(1000 / _PoseUpdateRate);
}
}

void PilotChangePower(int movePower, int turnPower)
{
_movePower = movePower;
_turnPower = turnPower;
}

void PilotChangePower(int movePower)
{
_movePower = movePower;
}

int PilotHdg360(int degrees)
{
while (degrees < 0)
degrees += 360;
return degrees % 360;
}

int PilotHdg180(int degrees)
{
degrees = PilotHdg360(degrees);
return degrees > 180 ? degrees - 360: degrees;
}

int PilotQuickestTurnTo(int hdgNow, int hdgNew)
{
hdgNow = PilotHdg360(hdgNow);
hdgNew = PilotHdg360(hdgNew);
if (hdgNow < hdgNew)
hdgNow += 360;
int left = hdgNow - hdgNew;
return (left < 181 ? -left : 360 - left);
}

void PilotWaitDrivesIdle()
{
if (!_hasBeenInitialized)
PoseException();

wait1Msec(20);
bool done = false;
while(!done)
{
done = (nMotorRunState[_left] == runStateIdle && nMotorRunState[_right] == runStateIdle);
EndTimeSlice();
}
wait1Msec(20);
}

void PilotMoveAsync(float distance)
{
long ticks;
if (!_hasBeenInitialized)
PoseException();

ticks = abs(distance * (float)_ticksPerWheelRevolution / _wheelTravelPerRevolution);

#ifdef __POSE_DEBUG__
writeDebugStreamLine("PilotMove:ticks %d", ticks);
#endif

nSyncedTurnRatio = 100;
nSyncedMotors = _useForSynch;
nMotorEncoderTarget[_left] = ticks;
motor[_left] = _movePower * sgn(distance);
}

void PilotTurnAsync(int degrees)
{
// this function is sensitive to which motor pairs we are using and in their order
// degrees may be flipped for BC or BA

long delta;

if (!_hasBeenInitialized)
PoseException();

// test this before uncommenting
// if (_useForSynch == synchBA || _useForSynch == synchCA || _useForSynch == synchCB)
// degrees = -degrees; ??

delta = (degrees * _ticksForFullPilotTurn / 360.0);

#ifdef __POSE_DEBUG__
writeDebugStreamLine("PilotTurn:%d", degrees);
writeDebugStreamLine(" LRe %d %d", nMotorEncoder[_left], nMotorEncoder[_right]);
writeDebugStreamLine(" ticks %d", delta);
#endif

nSyncedMotors = _useForSynch;
nSyncedTurnRatio = -100; // always turn opposite

nMotorEncoderTarget[_left] = abs(delta);
motor[_left] = _turnPower * sgn(delta);
}

void PilotTurnToAsync(int heading)
{
if (!_hasBeenInitialized)
PoseException();
PilotTurnAsync(PilotQuickestTurnTo(PoseHdg, heading));
}

void PilotGoToAsync(float x, float y)
{
float distance, hdgRadians;
int hdg;

distance = sqrt( (PoseX - x) * (PoseX - x) + (PoseY - y) * (PoseY - y) );
hdgRadians = atan2((y - PoseY), (x - PoseX)); // relative to 0,0
hdg = (int)(hdgRadians * 180.0 / PI);

#ifdef __POSE_DEBUG__
writeDebugStreamLine("GoTo: %1.1f, %1.1f", x, y);
writeDebugStreamLine(" frm: %1.1f, %1.1f", PoseX, PoseY);
writeDebugStreamLine(" hdgR: %1.1f", hdgRadians);
writeDebugStreamLine("dst hdg: %1.1f, %d", distance, hdg);
#endif

PilotTurnToAsync(hdg);
PilotWaitDrivesIdle(); //go to wait before we start moving
PilotMoveAsync(distance);
}

void PilotMove(float distance)
{
PilotMoveAsync(distance);
PilotWaitDrivesIdle();
}

void PilotTurn(int degrees)
{
PilotTurnAsync(degrees);
PilotWaitDrivesIdle();
}

void PilotTurnTo(int heading)
{
PilotTurnToAsync(heading);
PilotWaitDrivesIdle();
}

void PilotGoTo(float x, float y)
{
PilotGoToAsync(x,y);
PilotWaitDrivesIdle();
}

**Error**:Undefined variable 'strOpDebugStreamFormat'. 'short' assumed.
**Error**:Undefined variable 'varArgList'. 'short' assumed.
**Error**:Undefined variable 'strOpDebugStreamFormatWithNewLine'. 'short' assumed.
**Error**:Calling procedure 'ShowPose'. 'const' expressions does not fit. Parameter: 'string & t'. Expression: '"*move"'. Type: 'char *'
**Error**:Calling procedure 'ShowPose'. 'const' expressions does not fit. Parameter: 'string & t'. Expression: '"#move"'. Type: 'char *'
**Error**:Calling procedure 'ShowPose'. 'const' expressions does not fit. Parameter: 'string & t'. Expression: '"#turn"'. Type: 'char *'
**Error**:Calling procedure 'ShowPose'. 'const' expressions does not fit. Parameter: 'string & t'. Expression: '"final"'. Type: 'char *'

void ShowPose(string t)  <- replace this with void ShowPose(char *t)
{
...
}