///////////////////////////////////////////////////////////////////////
// RobotC+.h - damit's ein bisserl einfacher wird...    ;-)          //
// Version 0.5.8
///////////////////////////////////////////////////////////////////////

//===================================================================
// mathematische Konstanten und Funktionen
//===================================================================

// Eulersche Zahl E:
const float E=2.718281828450945235360287471352662497757247093699959574966967627724076630353547594571;

//********************************************

int  min(int a, int b)
{
   int m;
   m=(a<b ? a:b);
   return m;
}

//********************************************

float min(float a, float b)
{
   float m;
   m=(a<b ? a:b);
   return m;
}

//********************************************

int  max(int a, int b)
{
   int m;
   m=(a>b ? a:b);
   return m;
}

//********************************************

float max(float a, float b)
{
   float m;
   m=(a>b ? a:b);
   return m;
}

//********************************************

int round(float f)
{
  if(f>0) return (int)(f + 0.5);
  else    return (int)(f - 0.5);
}

//********************************************

// ArcusTangens mit Sonderfaellen; Angabe in Grad!
// x=Ankathete y=Gegenkathete Tangens=y/x

float atan2Degrees(float x, float y)
{
  float phi, alpha; //phi=Bogenmass, alpha=Grad;

   if (x>0) {phi=atan(y/x);}
   else
   if ((x<0)&&(y>=0))  {phi=PI+atan(y/x);}
   else
   if ((x<0)&&(y<0))   {phi=-PI+atan(y/x);}
   else
   if ((x==0)&&(y>0))  {phi=PI/2;}
   else
   if ((x==0)&&(y<0))  {phi=-PI/2;}
   else
   if ((x==0)&&(y==0)) {phi=0;}

   alpha=radiansToDegrees(phi);
   return alpha;
}

//********************************************

bool IsInRange(int Wert, int Basis, int Toleranz)
{
   return((Wert>=Basis-Toleranz) && (Wert<=Basis+Toleranz));
}


//===================================================================
// Prozeduren fuer Sensoren
//===================================================================

bool SMuxPressed(int port, byte ch) // MUX-Port an NXT (S1, S2,... ) // ch: 0=direkt, 1=ch1, 2=ch2, 3=ch3

{
  int v;
   bool pressed;

   v=SensorValue(port);
   if (ch==0)
   { pressed=(v<100 ); }
   else
  if (ch==1)
   { pressed=( IsInRange(v,775,30) || IsInRange(v,523,25) || IsInRange(v,343,8)   || IsInRange(v,275,8)) ; }
  else
   if (ch==2)
   { pressed=( IsInRange(v,620,40) || IsInRange(v,523,25) || IsInRange(v,295,8)   || IsInRange(v,275,8)) ; }
  else
   if (ch==3)
   { pressed=( IsInRange(v,363,15) || IsInRange(v,343,8)  || IsInRange(v,295,8)   || IsInRange(v,275,8)) ; }
  return pressed;

// Wertetabelle fuer Sensor-Muxer (natuerlich Bauform-abhaengig)
//*************************************************************
//
//  rcx ch3 ch2 ch1     raw       // RCX-WiderstMux als sensorRawValue definiert
//   0   0   0   0     1023       // an 3 Mux-Einaenge UND direktan rcx-Eingang
//   0   0   0   1      775       // je 1 Taster angeschlossen
//   0   0   1   0      620
//   0   0   1   1      523
//   0   1   0   0      363
//   0   1   0   1      343
//   0   1   1   0      295
//   0   1   1   1      275
//   1   0   0   0       79        // ab hier die Werte, wenn der
//   1   0   0   1       64        // 4. Taster direkt am rcx-Sensor-Eingang
//   1   0   1   0       74        // gedrueckt wird.
//   1   0   1   1       70        // Eine Unterscheidung, ob dann auch
//   1   1   0   0       69        // gleichzeitig  noch andere Taster
//   1   1   0   1       55        // zusaetzlich gedrueckt sind,
//   1   1   1   0       55        // ist dann kaum mehr moeglich.
//   1   1   1   1       55        //

}

//===================================================================
// globale Prozeduren fue Motor-Steuerung
//===================================================================

void  motSync (int m, int s) // setzt Sync-Master & Slave
{
   if ((m==0) && (s==0))
   { nSyncedMotors = synchNone;}
   else
   if ((m==0) && (s==1))
   { nSyncedMotors = synchAB;}
   else
   if ((m==0) && (s==2))
   { nSyncedMotors = synchAC;}
   else
   if ((m==1) && (s==0))
   { nSyncedMotors = synchBA;}
   else
  if ((m==1) && (s==2))
   { nSyncedMotors = synchBC;}
   else
  if ((m==2) && (s==0))
   { nSyncedMotors = synchCA;}
   else
  if ((m==2) && (s==1))
   { nSyncedMotors = synchCB;}
   else nSyncedMotors = synchNone;
}

//===================================================================
//===================================================================

//================================================================//
//                          navigator.h                           //
//                 Navigations-Routinen fuer RobotC               //
//================================================================//
//                         Version 0.7.4.
//
//  0.7.0.: Kompass-Korrektur bei Zwischenstopp
//  0.6.0.: User-Interface Routinen neu
// (c) Helmut W. 2008
//=================================================================//

#include "RobotC+.h";
#include "HTCompassDrv.h"

//=============================================================================================//
// Interface:
//=============================================================================================//

//***************************************************************//
// Initialisierung:
// Uebergabe der Groessenverhaeltnisse des Robots und Motor-Ports
// beim Start des eigenen Hauptprogramms an diesen Navigator:
//***************************************************************//

void InitNavBot (float Raddurchmesser,
                 float _RadAbstand,
                 float LaengeVoraus,
                 short MotorPortRechts,
                 short MotorPortLinks);


//***********************//
// Globale Variablen     //
// (Benutzer-definiert)  //
//***********************//

bool CompassExists;      // soll im Hauptprogramm gesetzt werden bei Kompass-Initialisierung

//**************************//
// globale Motor-Parameter  //
// (Benutzer-definiert)     //
//**************************//

const short mVSlow=15;         // MotorSpeed, haengt vom Getriebe ab
const short mSlow=25;
const short mNorm=40;
const short mFast=60;
const short mMax=100;



//*****************************//
// Antriebs-Steuerung          //
//*****************************//


void StopMotors ();               // muss nach jedem Einzelmanoever aufgerufen werden!
                                  // stoppt + speichert augenblickliche Positionsdaten

void move_cm(float s);            // geradeaus Anzahl cm vor o. zurueck
void forward();                   // vorwaerts bis StopMotors() aufgerufen wird
void reverse();                   // rueckwaerts bis StopMotors() aufgerufen wird

void turn_degr(float Winkel);     // drehen um einen best. Winkel (+) links, (-) rechts herum
void turn_left();                 // links herum bis StopMotors() aufgerufen wird
void turn_right();                // rechts herum bis StopMotors() aufgerufen wird

void GotoXY(float x, float y);       // geht von der aktuellen zur angegeb. Zielposition
void GotoField(short xF, short yF);  // geht ins Zentrum des betr. Feldes
void turn_toXY(float x, float y);    // dreht aus der aktuellen zur angegeb. Zielposition
float GetDistToXY(float x, float y); // berechnet die Weglaenge zur angegeb. Zielposition



//***********************//
// Landkarte             //
//***********************//


short GetMap(int x, int y);             // liest den Zustand des Feldes: 0=frei, 1= Hindernis
void  SetMap(int x, int y, int wert);   // schreibt den Zustand des Feldes

const short MapSize=30;             // Karte hat 30*30 Felder (soll durch 2 teilbar sein)...
const short Kante=30;               // Kantenlaenge von je 30cm (angepasst an Roboter-Groesse!)
                                    // ergibt eine Landkate von ca. 100 Quadratmeter Groesse

// spezielle globale Variablen fuer die Landkarte

short maxXYMap;         // bewegt er sich darueber hinaus, ist das erlaubt,
float maxXYpos;         // die echte Postion wird weiter berechnet
                        // aber nichts mehr in die Landkarte eingetragen
                        // Startpunkt ist (0,0) mit je 18 Feldern
                        // nach links, rechts, oben, unten


//=============================================================================================//
// Private Deklarationen, NICHT AENDERN!
//=============================================================================================//
//***********************//
// PRIVAT:
// Robot-Bewegungsformen //
//***********************//


#define _navMoveStop         0      // beide Mot. synchron stopp
#define _navMoveStraight   100      // beide Mot. synchron in dieselbe Richtung
#define _navMoveTurn      -100      // Mot. synchron gegenläufig links/rechts

int navMoveType=-1;


//************************************//
// Roboter-Messwerte und Anschluesse,
// ueber InitRobot initialisiert
//************************************//

short Mre=1, Mli=2;   // Motor-Ports fuer synchronisierte Motoren rechts und links;

float _DRad=0;        // Durchmesser Antriebsrad in cm, wird im task main initialisiert
float _RadAbst=0;     // Abstand der Antriebraeder in cm (_RadAbstand), muss im Hauptprogramm gesetzt werden
float _LVorn=0;       // Roboter-Laenge von Achse bis zur Spitze (in cm),
                      // wo Hindernisse per Stoss-Sensor oder Ultraschall erkannt werden)

//*******************************//
// Roboter-Messwerte,
// von InitRobot errechnet
//*******************************//


float _cmPerTick=0;     // Weg pro Tick (360 Ticks = 360 Grad = 1 Umdrehung)
float _AnglePerTick=0;  // gedrehter Bogen in Grad pro Tick
int   _TVorn=0;         // Roboter-Laenge (in Motor-Ticks) von Achsmitte bis zur Spitze,
                       // wo Hindernisse per Stoss-Sensor oder Ultraschall erkannt werden)



//*******************************//
// waehrend der Laufzeit
// immer neu errechnete Werte
//*******************************//


float Stretch=0;       // gefahrenes Wegstueck in cm (seit letztem Zwischenstopp)
float DrehWinkel=0;    // gedrehter Winkel in Grad  (seit letztem Zwischenstopp)


float xpos=0, ypos=0;           // errechnete Position in rechtw. Koordinaten
float xAlt=0, yAlt=0;           // Alte Position seit dem letzten Manoever in rechtw. Koordinaten

float xVorn=0, yVorn=0;         // Position der Roboter-Spitze, die ein Hindernis erkennen kann

short Map [MapSize][MapSize];   // Planquadrat-Feld, enthaelt Angaben ueber frei oder besetzt
short xMap=0, yMap=0;           // errechnete aktuelle Planquadrat-Position der Achsmitte
short xFNext=0, yFNext=0;       // errechnete aktuelle Planquadrat-Position der Roboterspitze

float Kurs;                     // errechneter Kurs (bezogen auf x-Achse)
float KKurs;                    // gemessener Kompass-Kurs (Kompass-Sensor, bezogen auf magnet. Nord)
float MKurs;                    // magnetisch (durch Kompass) korrigierter Kurs (bezogen auf x-Achse)
float oKurs;                    // offset Winkel zw. Kurssystem und magnetischem Koordinatensystem

bool TiltFront=false;           // vorne Hindernis erreicht bzw. angestossen



//===============================================================================================//
// Prozeduren
//===============================================================================================//

short GetMap(short x, short y)            // gibt den Zustand des Feldes zurueck: 0=frei, 1= Hindernis
{   return Map[x+(MapSize/2)][y+(MapSize/2)];}

void SetMap(short x, short y, short wert) // schreibt den Zustand des Feldes: 0=frei, 1= Hindernis
{   Map[x+(MapSize/2)][y+(MapSize/2)]=wert;}

//=============================================================================================//
// Kompass-Navigation
//===============================================================================================//

float Compass2MathKurs(float KWert)
{
   float k;

   k= oKurs-KWert;
   if (k<0) k+=360;
   if (k>=360) k-=360;
   return k;
}


//=============================================================================================//

void turn_degr(float Winkel)
{
   int SollTicks, speed;

   navMoveType=_navMoveTurn;

   nSyncedTurnRatio=-100;
   nMotorEncoder[Mre]=0;
   wait1Msec(10);

   SollTicks=round(Winkel/_AnglePerTick)+1;

   while(nMotorEncoder[Mre]!=SollTicks)
   {
     if (abs(nMotorEncoder[Mre]-SollTicks)>20){speed=mSlow;} // anpassen fuer optimales Dreh-
     else speed=mVSlow;                                      // und Bremsverhalten

      if (nMotorEncoder[Mre]<SollTicks)
     {   motor[Mre]=speed;  }
     else
     if (nMotorEncoder[Mre]>SollTicks)
     {   motor[Mre]=-speed; }
   }
   motor[Mre]=0;

}

//=============================================================================================//

void turn_left()   // links herum bis StopMotors() aufgerufen wird
{
   navMoveType=_navMoveTurn;

   nSyncedTurnRatio=-100;
   nMotorEncoder[Mre]=0;
   wait1Msec(10);

  motor[Mre]=mSlow;
}

//=============================================================================================//

void turn_right()   // rechts herum bis StopMotors() aufgerufen wird
{
   navMoveType=_navMoveTurn;

   nSyncedTurnRatio=-100;
   nMotorEncoder[Mre]=0;
   wait1Msec(10);

  motor[Mre]=-mSlow;
}

//=============================================================================================//

void move_cm(float s)
{
  int target, speed;

  nSyncedTurnRatio=100;
  nMotorEncoder[Mre]=0;
  wait1Msec(10);

  navMoveType=_navMoveStraight;

  target=round(s/_cmPerTick);

  while (target!=nMotorEncoder[Mre])
  {
    if (abs(nMotorEncoder[Mre]-target)>20) {speed=mNorm;}  // anpassen fuer optimales Fahr-
    else speed=mVSlow;                                         // und Bremsverhalten
    if (target > nMotorEncoder[Mre]) {motor[Mre]=speed; }
    else
    if (target < nMotorEncoder[Mre]) {motor[Mre]=-speed; }
  }
  motor[Mre]=0;
}


//=============================================================================================//

void forward()   // vorwaerts bis StopMotors() aufgerufen wird
{
  nSyncedTurnRatio=100;
  nMotorEncoder[Mre]=0;
  wait1Msec(10);

  navMoveType=_navMoveStraight;
  motor[Mre]=mNorm;
}

//=============================================================================================//

void reverse()   // rueckwaerts bis StopMotors() aufgerufen wird
{
  nSyncedTurnRatio=100;
  wait1Msec(10);

  nMotorEncoder[Mre]=0;
  navMoveType=_navMoveStraight;
  motor[Mre]=-mNorm;
}

//=============================================================================================//


void StopMotors ()
{
    navMoveType=_navMoveStop;
    motor[Mre]=0;
    wait1Msec(100);  // Zeit zum Bremsen

    nSyncedTurnRatio=100;
    nMotorEncoder[Mre]=0;
    wait1Msec(10);

    xAlt=xpos;
    yAlt=ypos;

    Stretch=0;

    Kurs=Kurs+DrehWinkel;
    DrehWinkel=0;



    if ((CompassExists))
    {
       KKurs=(float)HTCompassValue(CompassPort);
       MKurs=Compass2MathKurs(KKurs);
       Kurs=MKurs;
    }

    while (Kurs>360) {Kurs=Kurs-360;}
    while (Kurs<0) {Kurs=Kurs+360;}
}

//=============================================================================================//

void GotoXY(float x, float y)
{
   float xDelta, yDelta, savKurs, ZielWinkel, ZielStrecke;

   savKurs=Kurs;       // augenblicklichen Kurs speichern

   xDelta=x-xpos;      // relative Position zum Ziel in rechtw. Koordinaten
   yDelta=y-ypos;

   ZielWinkel=atan2Degrees(xDelta, yDelta);  // relative Zielrichtung zwischen beiden Punkten
   turn_degr(ZielWinkel-savKurs);            // drehen auf neuen Kurs!
   StopMotors();                             // Positionsdaten zwischenspeichern

   ZielStrecke =sqrt((xDelta*xDelta)+(yDelta*yDelta));  // Laenge des rel. Vektors zum Ziel
   move_cm(ZielStrecke);                     // ...und losfahren!

}

//=============================================================================================//

void GotoField(short xF, short yF)    // geht ins Zentrum des betr. Feldes
{
   float xc, yc;
   xc=(float)(xF*Kante);
   yc=(float)(yF*Kante);
   GotoXY(xc, yc);
}

//=============================================================================================//


float GetDistToXY(float x, float y) // berechnet Weglaenge (ohne loszufahren!)
{
   float xDelta, yDelta, ZVLaenge;

   xDelta=x-xpos;
   yDelta=y-ypos;

   ZVLaenge =sqrt((xDelta*xDelta)+(yDelta*yDelta));  // Laenge des rel. Vektors zum Ziel

   return(ZVLaenge);    //

}

//=============================================================================================//

void turn_toXY(float x, float y)  // dreht auf die angegebene Zielposition
{
   float xDelta, yDelta, savKurs, ZVWinkel;

   savKurs=Kurs;         // augenblicklichen Kurs speichern

   xDelta=x-xpos;        // relative Position zum Ziel in rechtw. Koordinaten
   yDelta=y-ypos;

   ZVWinkel=atan2Degrees(xDelta, yDelta);  // relative Zielrichtung zwischen beiden Punkten

   turn_degr(ZVWinkel-savKurs);   // ...und losdrehen auf neuen Kurs!

}



//=============================================================================================//


task odometrics ()
{
  int ticks;

  while(true)
  {
      if (navMoveType==_navMoveStraight) // geradeaus
      {
          // zeichnet durchgefuehrte Geradeaus-Fahrten auf
          // Umrechnung der Polar-Koordinaten in rechtwinklige (kartesische)
          // x=r*cos(Kurs); y=r*sin(Kurs); r=Fahrstrecke=Ticks*_cmPerTick

          if ((CompassExists))
          {
            KKurs=(float)HTCompassValue(CompassPort);
            MKurs=Compass2MathKurs(KKurs);
            //Kurs=MKurs;
          }

           ticks=nMotorEncoder[Mre];
          xpos=xAlt + ticks*_cmPerTick*cosDegrees(Kurs);  // ermittelt die Postion der Roboter-Mitte (Achse)
          ypos=yAlt + ticks*_cmPerTick*sinDegrees(Kurs);  //

          if ( (abs(xpos)<maxXYpos) && (abs(ypos)<maxXYpos))
          {
            xMap=round((xpos/Kante));  // (xpos,ypos) umgerechnet in (xMap,yMap)
            yMap=round((ypos/Kante));  // Feld, in dem sich die Roboter-Achsenmitte befindet
            SetMap(xMap, yMap, 0);     // "0" schreiben kann entfallen, wenn bereits mit "0" initialisiert
          }

          xVorn=xAlt + (ticks+_TVorn)*_cmPerTick*cosDegrees(Kurs);  // ermittelt die Postion der Roboter-Spitze
          yVorn=yAlt + (ticks+_TVorn)*_cmPerTick*sinDegrees(Kurs);  // (wo Hindernisse erkannt werden koennen)

          if ( (abs(xVorn)<maxXYpos) && (abs(yVorn)<maxXYpos))
          {
            xFNext=round((xpos/Kante)); // (xVorn,yVorn) umgerechnet in (xFNext,yFNext)
            yFNext=round((ypos/Kante));
            if (TiltFront) SetMap(xFNext, yFNext, 0);   // Feld-Wert vorn an der Spitze =0 fuer "frei";
            else SetMap(xFNext, yFNext, 1);             // falls man vorn anstoesst, wird vorne =1 gesetzt
          }
          Stretch=ticks*_cmPerTick;          // gefahrenes Stretch seit letztem Stopp / Manoever

      }
      else
        if (navMoveType==_navMoveTurn)     //  drehen
        {  DrehWinkel=_AnglePerTick*nMotorEncoder[Mre];    }
        else
        if (navMoveType==_navMoveStop)
        {  wait1Msec(20) ;            }    // warten
  }
  return;
}


//=============================================================================================//

void InitVariables()
{
  int x,y;
  int kh;

  _cmPerTick=(PI*_DRad)/360;      // Wegstrecke pro Tick
  _AnglePerTick=_DRad/_RadAbst;   // Drehwinkel pro Tick
  _TVorn=_LVorn/_cmPerTick;        // Roboter-Laenge (in Motor-Ticks) von Achsmitte bis zur Spitze

  for(x=0;x<MapSize;x++)
  {
      for (y=0;y<MapSize;y++)      // initialisiert alle Kartenfelder
      {Map[x][y]=0;}             // mit Wert =0 fuer "frei"
  }

  maxXYMap=(int)((MapSize/2)-1);
  maxXYpos=(float)maxXYMap*Kante;

  Kurs=0;
  KKurs=0;
  oKurs=0;
  MKurs=0;
  DrehWinkel=0;
  Stretch=0;
  xAlt=xpos=0;
  yAlt=ypos=0;
  nMotorEncoder[Mre]=0;

  if (CompassExists)
  {
     KKurs=(float)HTCompassValue(CompassPort);
     oKurs=KKurs;
     MKurs=Compass2MathKurs(KKurs);
  }

}


//=============================================================================================//
//       InitRobot muss im Hauptprogramm vom Benutzer mit 5 Parametern aufgerufen werden:       //
//=============================================================================================//

void InitNavBot (float Raddurchmesser,
                 float _RadAbstand,
                 float LaengeVoraus,
                 short MotorPortRechts,
                 short MotorPortLinks)
{

   _DRad=Raddurchmesser;       // Zuweisung der uebergebenen Parameter auf die globalen Variablen
   _RadAbst= _RadAbstand;
   _LVorn=LaengeVoraus;

   Mre=MotorPortRechts;
   Mli=MotorPortLinks;
   motSync(Mre,Mli);

   InitVariables();

   StartTask (odometrics);

}
//=============================================================================================//
//=============================================================================================//

// =================================================================================
// Demo-Programm fuer  Navigations-Routinen
// Version 0.6.0
// notwendige Roboter-Bauform: beliebiger "Tribot" oder "RoverBot"
// (Antriebsachse mit 2 unabhaengigen Antriebsmotoren fuer Raeder oder Raupen)
// =================================================================================

#include "navigator.h"
#define println nxtDisplayString

//===================================================================

task displayValues()
{
   eraseDisplay();
   while (true)
  {

    println(0, "Wegst %5.1f", Stretch);               // Weg seit letzter Kursaenderung (xAlt,yAlt)

    println(2, "Pos %5.1f %5.1f", xpos,  ypos);       // aktuelle Position (xpos,ypos)
    println(3, "Feld%5d%5d", (int)xMap, (int)yMap );  // aktuelles Planfeld (xFeld,yFeld)

    println(5, "Drehw= %5.1f",  DrehWinkel );         // Drehwinkel
    println(6, "Kurs = %5.1f",  Kurs );               // Kurs in Bezug auf x-Achse

  }
  return;
}


//===================================================================

task main ()
{
   InitNavBot (8.2, 13.5, 22.0, 1, 2);  // einmalig aufrufen zum Start des Navigators!
               // Raddurchmesser=8.2cm,
                    // Abstand der Antriebsraeder=13.5cm,
                         // LaengeVoraus (Achse bis Spitze)=22cm
                                // reMotor=SyncMaster=port1=motorB,
                                  // liMotor=SyncSlave=port2=motorC

  StartTask (displayValues);          // optional, kann man natuerlich weglassen

  move_cm(50);           // gehe 50 cm vorwaerts => Position (50, 0)
  StopMotors();          // schreibt die momentane Position und den Kurs
  wait1Msec(4000);       // nur zur besseren Beobachtung der Messwerte (kann wegfallen)

  turn_degr(90);         // drehe linksrum um 90 Grad
  StopMotors();          // schreibt  momentane Position und Kurs
  wait1Msec(4000);       // (s.o., kann wegfallen)

  move_cm(50);           // gehe 50 cm vorwaerts => Position (50, 50) (angenaehert)
  StopMotors();          // schreibt  momentane Position und  Kurs
  wait1Msec(4000);       // (s.o., kann wegfallen)

  GotoXY(10,10);         // Nachhauseweg grob anpeilen
  StopMotors();          // schreibt  momentane Position und Kurs
  wait1Msec(4000);       // (s.o., kann wegfallen)

  GotoXY(0,0);           // ...und jetzt noch das letzte Stueck nach Hause
  StopMotors();          // schreibt  momentane Position und Kurs
  wait1Msec(4000);       // (s.o., kann wegfallen)

  turn_toXY(100,0);      // zurueck in die Ausgangs-Richtung 0 Grad
  StopMotors();          // steht wieder fast wie am Anfang

  while (true)
  { wait1Msec(50);}      // zum Ablesen der Messwerte
}
//===================================================================
//===================================================================

/**************************************************************************
*                       HiTechnic Compass Sensor                          *
*                            HTCompassDrv.h                               *
*                             version 0.72                                *
***************************************************************************/


tSensors CompassPort;

void SetHTCompassSensor(tSensors port)
{
  SensorType[port] = sensorI2CCustomStd;
  wait10Msec(5);
  nI2CBytesReady[port]=0;
  while(nI2CStatus[port]== STAT_COMM_PENDING)
  wait1Msec(2);
  CompassPort=port;
}

//**************************************************************************

int HTCompassValue(tSensors port)
{
   typedef struct{
  byte nMsgSize;
  byte nDeviceAddress;
  byte nLocationPtr;
  byte nSensorMode;
  } TI2C_Output;


  TI2C_Output sOutput;
  int hUpper, hLower, heading;
  byte rMessage[6];

  nI2CBytesReady[port] = 0;
   sOutput.nMsgSize = 2;                   //read the value from the sensor
  sOutput.nDeviceAddress = 0x02;
  sOutput.nLocationPtr = 0x42;
  sendI2CMsg(port, sOutput.nMsgSize, 6);

  while (nI2CStatus[port] == STAT_COMM_PENDING)
  {  wait1Msec(2); }

  if (nI2CStatus[port] == NO_ERR)
  {
    readI2CReply(port, rMessage[0], 6);      //get the compass heading

    hUpper = (rMessage[0] & 0x00FF);
    hLower = (rMessage[1] & 0x00FF);
    heading = (hUpper << 1) | hLower ;    //convert contents of 0x44 0x45
  }
  else
  { heading=nI2CStatus[port]; }

  return heading;
}
//**************************************************************************
//**************************************************************************