/*

4WD_CARmod11 = 1.1 version - Auto Steering Initialisation added. (Front Wheels automatically point forward on power-up)
4WD_CARmod10 = 1.0 version - Motors reversed (changed wiring), first complete version.
4WD_CARmod08 = 0.8 version - Sound indication on ADC/CPA status
                             Ability to drive on slowly when ADC is active
                             CPA Distance increased for earlier automatic emergency braking
4WD_CARmod07 = 0.7 version - Compressed 4 TX and 4 RX Mailboxes into 1+1 to improve speed of communications
4WD_CARmod06 = 0.6 version - First version of Auto Distance Control and Collision Prevention Assistant (UltraSonic)
                             Resolved failure of 3rd & 4th MailBox TX to Master NXT (BlueTooth)
4WD_CARmod05 = 0.5 version - Activate Head Lights when driving (OUT_C)
4WD_CARmod04 = 0.4 version - Monitor Speed of Rear Motor
4WD_CARmod03 = 0.3 version - Driving Motor Control (Motor B)
4WD_CARmod02 = 0.2 version - Add SteeringMotor Management (Motor A)
                             Add Simple Driving Motor Control (Motor B)
4WD_CARmod01 = 0.1 version - CAR Module Based on RCmodule version 0.5

*/

#include "NXCDefs.h"

// Bluetooth communication constants
#define BT_CONN 0                 // Use Bluetooth Channel 0 (Will automaticly be connected to other Master NXT (=RC))

// Display Positioning constants per object
#define XposDash            0      // Overall Dashbord Picture
#define YposDash            0      //
#define XposNoRC           16      // NO CAR FOUND Message window
#define YposNoRC           19      //
#define XposLeftArrow      34      // Richtingaanwijzer Links
#define XposRightArrow     52      // Richtingaanwijzer Rechts
#define YposArrow          52      // Richtingaanwijzers
#define XposADC1            8      // AutoDistanceControl Indicator 1
#define YposADC            45      //
#define XposADC2           78      // AutoDistanceControl Indicator 2
#define XposCPA1            6      // CollisionPreventionAssistant Ind.1
#define YposCPA            45      //
#define XposCPA2           76      // CollisionPreventionAssistant Ind.2
#define XposPRND           40      // PRND Ind. Automatic Gearing
#define YposPRND            8      //
#define XposReFuel         69      // Please Refuel indicator
#define YposReFuel          1      //
#define XposPower          14      // Power meter (Toerenteller) center
#define YposPower          23      //
#define XposSpeed          49      // Speedo meter center
#define YposSpeed          23      //
#define XposFuel           88      // Fuel (Battery) meter center
#define YposFuel           22      //

// Declare X and Y Coordinate Arrays for Analog Meters on NXT Display
// These points are calculated as being the End of the Finger in the meter.
// These points are the absolute position related to the center of the meter as positioned in the programm
// Replacing the center of the meter will require an offset caculation to these poits

#define PowerMeterPoints 20        // Number of prepared coördinates in RPM Meter (See array below)
int XcoordPower[] = { 7,  5,  4,  4,  3,  3,  3,  3,  4,  5,  6,  8,  9, 11, 13, 14, 16, 18, 20, 21, 22}; // 20 points
int YcoordPower[] = {15, 16, 18, 19, 21, 23, 24, 26, 28, 29, 31, 32, 33, 34, 34, 34, 34, 33, 32, 31, 30};

#define SpeedMeterPoints 33        // Number of prepared coördinates in Speed Meter (See array below)
int XcoordSpeed[] = {32, 31, 30, 29, 29, 29, 29, 30, 31, 33, 34, 36, 38, 41, 43, 46, 48, 51, 54, 56, 59, 61, 63, 65, 66, 67, 68, 69, 69, 69, 68, 68, 66, 65}; // 33 x-points
int YcoordSpeed[] = {12, 15, 17, 20, 22, 25, 27, 30, 32, 35, 37, 38, 40, 41, 42, 43, 43, 43, 42, 42, 41, 39, 37, 36, 33, 31, 29, 26, 23, 21, 18, 16, 13, 11}; // 33 y-points

#define FuelMeterPoints 16         // Number of prepared coördinates in Fuel Meter (See array below)
int XcoordFuel[] = {81, 79, 78, 78, 79, 80, 82, 85, 88, 91, 94, 96, 97, 98, 98, 97, 95}; // 16 points
int YcoordFuel[] = {15, 17, 20, 23, 26, 28, 30, 32, 32, 32, 30, 28, 26, 23, 20, 17, 15};

// State threshold constants
#define SteerEdgeMax  45           // Plus or Minu xx Degrees
#define FlashLight    30           // Start Left/Right Flashlights when making a turn of xx Degrees or more
#define BattEmpty     7000         // mVolt Minimum Batt Level before failure (Fuel = 0)
#define BattCharge    15           // xx% of Full Battery to Light Up "Refuel Indicator" on Batt Level check
#define BattFull      8100         // mVolt Maximum Voltage after recharge that Equals Fuel = 100%
#define Park           1           // Used for PRND variable settings (0=Off)
#define Reverse        2           //
#define Neutral        3           //
#define Drive          4           //
#define CoastDist     30           // Enable Auto Distance Control at xx cm Distance
#define BreakDist     15           // Enable Colision Prevention Assistance at xx cm Distance
#define BT_TimeOut    200          // Terminate Bluetooth communication after xx mSec of continuous Errors

// Sensor conversion ratio's
#define TachoRatio     5           // One degree SteerEdge = xx Ticks in TachoCount of NXT Motor
#define SteerTick      1           // One Sensor Tick = xx Degrees Steering
#define SpeedRatio     1923        // One SensorTick per milli second is xx% of Maximum Speed (=100)


// Global Variables
bool ShowDebugInfo=0;              // Set when you want to display extra programstate info during execution in NXT
bool NoRC=0;                       // Set when there is no Bluetooth connection with the Car
bool NoRC_OLD=0;                   // Indicates previous diplay cycle of NoRC status
bool GoLeft=0;                     // Set Left BlinkingLight when true
bool GoRight=0;                    // Set Right BlinkingLight when true
bool B_GoLeft=0;                   // Blinking for GoLeft
bool B_GoRight=0;                  // Blinking for GoRight
bool ADC=0;                        // Automatic Distance Control is ACTIVE when true
bool CPA=0;                        // Collision Prevention Assistant is ACTIVE when true
short PRND=0;                      // PRND Gearing Indicator 0=Off 1=P 2=R 3=N 4=D
bool Brake=0;                      // True when brakepedal is pressed for braking while riding
bool ReFuel=0;                     // Battery is almost Empty (Show indicator)
bool B_ReFuel=0;                   // Blinking for Refuel
short MotPwr=0;                    // Powerlevel 0..100% to Driving Motors
short PwrPedal=0;                  // Gas Pedal position measured with Rotation Sensor
short PwrCorrection=0;             // Correctionvalue when Gas Pedal is pushed too far
short PwrCorrection_OLD=0;         // Chached for comparisson in next measurement
short Speed=0;                     // Speedlevel 0..100% measured from
short Fuel=0;                      // Fuel level measured from Battery (Converted to 0..100%)
short Fuel_OLD=30;                 // Previous FuelLevel (used to make an avarage because of instable voltages)
short SteerWheel=0;                // Steering Wheel position read from the RC Unit Sensor (in degrees)
short SteerCorrection=0;           // Correctionvalue when driver is turning steering wheel too far
short SteerCorrection_OLD=0;       // Chached for comparisson in next measurement
short SteerEdge=0;                 // Steering edge value for frontwheels (in degrees)
short SteerEdge_OLD=0;             // Last used Edge for SteeringMotor (0=Straight driving)
short SteerEdge_Now=0;             // Used for temporary calculations
short Distance=0;                  // Distance in Centimeters from car to object in front (Ultrasonic)
bool Blink=0;                      // False = dont show, True= show indicator
long B_Time=0;                     // Measured System Timer for Blinking functions
long Disp_Time=0;                  // Measures Dispay Time functions
short Tmp=0;                       // Scratch Variable
long DebugVal=0;                   // Scratch Variable shown on Displa (Left) when ShowDebugInfo = True



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

sub ShowDashboard ()   // Display All meters & indicators in dashboard
{
    byte FingerPoint=0;                                                         // For calculating meterfinger coordinates
    
    if (NoRC) {
       Disp_Time=CurrentTick();
       NoRC_OLD=true;
       }
    else
       if (NoRC_OLD)
          if (CurrentTick()>Disp_Time+100) NoRC_OLD=0;

    GraphicOut(0, 0, "My_DashBasic.ric", false);                                 // Init screen (clearscreen = false because basic ric covers total screen)
    if ((NoRC) || (NoRC_OLD)) {
       // ALWAYS ON TOP - MUST BE DIPLAYED AS LAST ITEM
       GraphicOut(XposNoRC, YposNoRC,"My_DashNRF.ric", false);                // No Car Found warning
       }
    else
    {
    switch (PRND) {
      case 0 :
          GraphicOut(XposPRND, YposPRND, "My_DashPRND_Off.ric", false);         // Show Automatic Gearing state
          break;
      case 1 :
          GraphicOut(XposPRND, YposPRND, "My_DashPRND_P.ric", false);
          break;
      case 2 :
          GraphicOut(XposPRND, YposPRND, "My_DashPRND_R.ric", false);
          break;
      case 3 :
          GraphicOut(XposPRND, YposPRND, "My_DashPRND_N.ric", false);
          break;
      case 4 :
          GraphicOut(XposPRND, YposPRND, "My_DashPRND_D.ric", false);
          break;
      default :
          GraphicOut(XposPRND, YposPRND, "My_DashPRND_Off.ric", false);
      }

    if (CPA) {
       GraphicOut(XposCPA1, YposCPA, "My_DashCPA1.ric", false);                 // Collision Prevention Assistance
       GraphicOut(XposCPA2, YposCPA, "My_DashCPA1.ric", false);
       }
    else
       if (ADC) {
          GraphicOut(XposADC1, YposADC, "My_DashADC1.ric", false);              // Automatic Distance Control
          GraphicOut(XposADC2, YposADC, "My_DashADC1.ric", false);
          }
    if (B_ReFuel)  GraphicOut(XposReFuel,     YposReFuel,"My_DashReFuel.ric",  false);  // ReFuel sign
    if (B_GoLeft)  GraphicOut(XposLeftArrow,  YposArrow, "My_DashLAopen1.ric", false);  // Go Left sign
    if (B_GoRight) GraphicOut(XposRightArrow, YposArrow, "My_DashRAopen1.ric", false);  // Go Right sign


    // Display ANALOG METERS
    FingerPoint = MotPwr * PowerMeterPoints / 100;
    if (FingerPoint>PowerMeterPoints) FingerPoint=PowerMeterPoints;             // Protect against > 100 value
    LineOut(XposPower-1, YposPower, XcoordPower[FingerPoint], YcoordPower[FingerPoint], false);
    LineOut(XposPower, YposPower-1, XcoordPower[FingerPoint], YcoordPower[FingerPoint], false);
    LineOut(XposPower+1, YposPower, XcoordPower[FingerPoint], YcoordPower[FingerPoint], false);
    LineOut(XposPower, YposPower+1, XcoordPower[FingerPoint], YcoordPower[FingerPoint], false);

    FingerPoint = Speed * SpeedMeterPoints / 100;
    if (FingerPoint>SpeedMeterPoints) FingerPoint=SpeedMeterPoints;             // Protect against > 100 value
    LineOut(XposSpeed, YposSpeed, XcoordSpeed[FingerPoint], YcoordSpeed[FingerPoint], false);
    LineOut(XposSpeed, YposSpeed+1, XcoordSpeed[FingerPoint], YcoordSpeed[FingerPoint], false);
    LineOut(XposSpeed+1, YposSpeed, XcoordSpeed[FingerPoint], YcoordSpeed[FingerPoint], false);
    LineOut(XposSpeed+1, YposSpeed+1, XcoordSpeed[FingerPoint], YcoordSpeed[FingerPoint], false);

    FingerPoint = Fuel * FuelMeterPoints / 100;
    if (FingerPoint>FuelMeterPoints) FingerPoint=FuelMeterPoints;               // Protect against > 100 value
    LineOut(XposFuel-1, YposFuel, XcoordFuel[FingerPoint], YcoordFuel[FingerPoint], false);
    LineOut(XposFuel, YposFuel-1, XcoordFuel[FingerPoint], YcoordFuel[FingerPoint], false);
    LineOut(XposFuel+1, YposFuel, XcoordFuel[FingerPoint], YcoordFuel[FingerPoint], false);
    LineOut(XposFuel, YposFuel+1, XcoordFuel[FingerPoint], YcoordFuel[FingerPoint], false);

    } // End of If (NoRC) Else body

    if (ShowDebugInfo) NumOut(1,55,DebugVal,false);
}

sub WaitBT_Ready() // Wait for Bluetooth communication in Mailbox is ready
{
    long BT_Tick;                                                                      // TimeStamp for Bluetooth communications TimeOuts

    BT_Tick=CurrentTick();
    while ((BluetoothStatus(BT_CONN)!=NO_ERR) && (CurrentTick()<BT_Tick+BT_TimeOut));  // Wait for BT Ready or TimeOut
      if (BluetoothStatus(BT_CONN)!=NO_ERR) {NoRC=1; Wait(1000);} else NoRC=0;         // Still <>NO_ERR Status means trouble.

    // Suggestion: Insert routine to repare BT_Connection while driving here
}

sub SendToRC() // Send monitored values to RC
{
    long TxValue=0;
    
/*  // Previous TxSequence (Causing much BT-Communication Delays at RC because of wait-for-value sync loops per inbox)
    WaitBT_Ready();
    SendResponseNumber(1,Speed);
    WaitBT_Ready();
    SendResponseNumber(2,Fuel);
    WaitBT_Ready();
    SendResponseBool(3,ADC);
    WaitBT_Ready();
    SendResponseBool(4,CPA);
*/
      // Improved TxSequence for Slave-2-Master messaging.
      // Prepare TxValue to send 4 parameters as 1 number (to reduce Bluetooth communication delays)
      //
      // Binary Format: 00 0000 0000 0000 0000
      //                || |       | |_______|__ 8 bits for Speed 0..100
      //                || |_______|____________ 8 bits for Fuel 0..100
      //                ||______________________ 1 bit for ADC Flag
      //                |_______________________ 1 bit for CPA Flag
      //

      TxValue=CPA;   TxValue<<=1;     // Add CPA Flag and shift Left one Bit
      TxValue+=ADC;  TxValue<<=8;     // Add ADC Flag and Shift Left 8 Bits further
      TxValue+=Fuel; TxValue<<=8;     // Add Fuel level and Shift Left 8 Bits further
      TxValue+=Speed;                 // Add Speed level
      WaitBT_Ready();
      SendResponseNumber(1,TxValue);

}

sub ReceiveFromRC() // Receive driving information from RC
{
    short BT_Nr_Input;
    bool  BT_Bo_Input;
    byte  BT_State;
    short BitTest;
    long  RxValue=0;

    BT_State=BluetoothStatus(BT_CONN);
    if (BT_State==NO_ERR) {
//      Old sequence, using 4 separate mailboxes to Receive commands from RC
//      This worked okay but it can be done faster!
//
//      BT_State=ReceiveRemoteNumber(1, true, BT_Nr_Input);
//      if (BT_State==NO_ERR)  {MotPwr=BT_Nr_Input;}                 // Only accept new info when mailbox was NOT empty.
//      BT_State=ReceiveRemoteNumber(2, true, BT_Nr_Input);
//      if (BT_State==NO_ERR)  {SteerEdge=BT_Nr_Input; }             // Only accept new info when mailbox was NOT empty.
//      BT_State=ReceiveRemoteBool(  3, true, BT_Bo_Input);
//      if (BT_State==NO_ERR)  {Brake=BT_Bo_Input;}                  // Only accept new info when mailbox was NOT empty.
//      BT_State=ReceiveRemoteNumber(4, true, BT_Nr_Input);
//      if (BT_State==NO_ERR)  {PRND=BT_Nr_Input;}                   // Only accept new info when mailbox was NOT empty.

      // Communicating 4 parameters in 1 mailbox at the same time
      // Binary Format: 0000 0000 0000 0000 0000
      //                | || |       | |_______|__ 8 bits for MotPwr 0..100
      //                | || |_______|____________ 8 bits for SteerEdge 0..100
      //                | ||______________________ 1 bit  for Brake Flag
      //                |_|_______________________ 3 bits for PRND 0..4 (with extra bit for future extensions)
      //
      BT_State=ReceiveRemoteNumber(1, true, RxValue);
      if (BT_State==NO_ERR)  {       // Only accept new info when mailbox was NOT empty.
         MotPwr=RxValue & 0xFF;      // Mask least significant 8 bits and copy them into MotPwr
         RxValue>>=8;                // Shift 8 bits right to select next reading
         BitTest=RxValue & 0x80;
         if (BitTest==0)                 // Test if positive!!!
             SteerEdge=RxValue & 0xFF;   // Mask least significant 8 bits and copy them into SteerEdge
         else {                           // Number is 8-bit negative, translate it to 16 bit negative
             BitTest=RxValue & 0xFF;
             SteerEdge=BitTest | 0xFF00;   // Mask least significant 8 bits and extend sign-bit to 16 bits number for SteerEdge
             }
         RxValue>>=8;
         Brake=RxValue & 0x01;       // Mask least significant 1 bits and copy them into Brake
         RxValue>>=1;
         PRND=RxValue & 0x07;        // Mask least significant 3 bits and copy them into PRND
         }
      }
}

task BT_TxRx()
{
     while (true) {
       // Send monitored values to Car
       SendToRC();

       // Receive status information from Car
       ReceiveFromRC();
       }
}

task ChangeDebug ()                // Wait for NXT Button (to enable/disable debug mode)
                                   // Use the ShowDebugInfo flag elsewhere in your program to dos Debug Things
{
      bool tmp;
      tmp=ButtonCount(BTNCENTER, true);                                         // Read & Clear pressed counter (in NXT)
      while (true)
       {
       Wait(100);
       if (ButtonCount(BTNCENTER, true)>0) {
          PlayTone(1760,50);
          Wait(100);
          ShowDebugInfo=!ShowDebugInfo;
          if (ShowDebugInfo) {PlayTone(1980,50);}
          }
       }
}

task StatusDisplay ()               // Show dashboard controls on NXT Display
{
    #define BlinkCycle 600          // Dutty cylcle for blinking signals on display
    #define BlinkOn    300          // Must be less then total duty cycle time
    
    bool Blink=0;                                                               // Blink=1 = Show blinking symbol now!
    B_Time = CurrentTick()-BlinkCycle-1;                                        // Set Blink Timer for Blink-function minus any value longer then blink duty cycle
    while (true)
       {
       if ((ReFuel) || (GoRight) || (GoLeft) || (Blink))                        // If starting to blink or blinking busy
           {
           if (!Blink) //Blink is dimmed at the moment
               {
               if (B_Time<CurrentTick()-BlinkCycle)
                   {
                   Blink=true;
                   if (ReFuel) B_ReFuel=1;
                   if (GoLeft) B_GoLeft=1;
                   if (GoRight) B_GoRight=1;
                   if ((GoRight) || (GoLeft)) PlayTone(300,5);                  // Sound a Click when turning Left or Right
                   B_Time=CurrentTick();                                        // Start a new BlinkCycle in timer
                   }
               }
           else // Blink is active
               {
               if (B_Time<CurrentTick()-BlinkOn)
                   {
                   Blink=false;
                   B_ReFuel=0;
                   B_GoLeft=0;
                   B_GoRight=0;
                   }
               }
           }
       ShowDashboard();
       Wait(75);
       }
}
task SteerInit()                    // Put frontwheels in forward direction, then stop this sequence
{
    short DeltaEdge;
    short SteerForce;


    // Steer to one side (with limited force) until motor is mechanically blocked
    OnFwd(OUT_A,50);                          // This command clears the TachoCounter by default.
    SteerEdge_Now=MotorTachoCount(OUT_A);     // Measure motor movement
    Wait(20);
    while (SteerEdge_Now<>MotorTachoCount(OUT_A)) {
      SteerEdge_Now=MotorTachoCount(OUT_A);   // Measure motor movement again
      Wait(20);
      }
    Off(OUT_A);                               // This command clears the TachoCounter by default.

    // Steer to other side (with limited force) until motor is mechanically blocked again
    OnRev(OUT_A,50);                          // This command clears the TachoCounter by default.
    SteerEdge_Now=MotorTachoCount(OUT_A);     // Measure motor movement
    Wait(20);
    while (SteerEdge_Now<>MotorTachoCount(OUT_A)) {
      SteerEdge_Now=MotorTachoCount(OUT_A);   // Measure motor movement again
      Wait(20);
      }
    Off(OUT_A);                               // This command clears the TachoCounter by default.
    SteerEdge_Now=-SteerEdge_Now/2;

    // Set Front wheels in forward direction

    RotateMotor(OUT_A, 90, SteerEdge_Now);
    Wait(500);
    // Ready to go!

    //It appeared that the first attempt to read the MotorTachoCount allways fails when
    //this routine is called just after a TachoCount Reset. The Reset appears only effective
    //when the MotorTachoCount function is called for a second time here.
    //Incorrect MotorTachoCount readings appear only after a previous ResetTachoCount.
    //The first reading deliveres the "old" value, before reset.
    ResetTachoCount(OUT_A);
    Wait(40);
    SteerEdge_Now=MotorTachoCount(OUT_A);  // Measure Tacho once (dummy read)

}

task Steering()                     // Control Steering Motor_A
{
    short DeltaEdge;
    short SteerForce;
    
    // Control Steering of Front wheels
    while(true) {
      SteerEdge_Now=SteerEdge*TachoRatio;
      DeltaEdge=abs(MotorTachoCount(OUT_A)- SteerEdge_Now);
      if (DeltaEdge>1) {
         SteerForce=15+(DeltaEdge*2); if (SteerForce>100) SteerForce=100;
         //if (DeltaEdge<100) SteerForce=20; else SteerForce=100;
         if (MotorTachoCount(OUT_A)<SteerEdge_Now) OnFwdEx(OUT_A, SteerForce, RESET_NONE);
         else OnRevEx(OUT_A, SteerForce, RESET_NONE);
         }
      else { OffEx(OUT_A, RESET_NONE); SteerForce=0;}
      }
}

task Operate()                      // Drive the CAR!
{
    #define BeepPause 750 // milliseconds pause between ADC/CPA beeps
    short TmpFuel, TmpMotPwr;
    long  SpeedTime_OLD, SpeedTime;                                             // Previous Rotation Ticks
    short TmpSpeed;
    long  SoundTimer;                                                           // For beeps on ADC and CPA
    long  LightsOffTimer;
    
    // Reset basic parameters
    GoLeft=0;
    GoRight=0;
    ReFuel=false;
    PRND=Park;

    // Start operation loop (Endless)
    while (true)
       {
       // Motor Management
       // MotPwr = Powerlevel 0..100% received from RC
       // Stop the Car when Brake is active OR when the Remote Control is not Connected anymore!
       if ((Brake) || (NoRC)) Off(OUT_B); // Set wheels in Emergency BRAKE
       else
        switch (PRND)
         {
         case Park :
           Off(OUT_B); // Set wheels in BRAKE
           break;
         case Reverse :
           if (MotPwr>0) OnRev(OUT_B, MotPwr); else Coast(OUT_B);
           break;
         case Neutral :
           if (CPA) Off(OUT_B);
           else Coast(OUT_B);
           break;
         case Drive :
           if (MotPwr>0) {
              if (CPA)
                 Off(OUT_B);
              else
                 if (ADC) {
                    if (Speed>30)
                       Off(OUT_B);
                    else {
                       if (MotPwr>35) TmpMotPwr=35;
                       else TmpMotPwr=MotPwr;
                       OnFwd(OUT_B, TmpMotPwr);
                       }
                    }
                 else
                    OnFwd(OUT_B, MotPwr);
              }
           else {
              if (CPA) Off(OUT_B);
              else Coast(OUT_B);
              }
           break;
         default :
           Off(OUT_B); // Set wheels in BRAKE
         }

       // BeepSounds on Ultrasonic system
       if ((ADC || CPA) && (SoundTimer+BeepPause<CurrentTick()) && (PRND!=Reverse)) {
           SoundTimer=CurrentTick();
           PlayTone(1200,50);                                                    // Play ADC/CPA Tone after BeepPause again
           }

       // Monitor Speed
       if (CurrentTick()>SpeedTime_OLD+250) {
          TmpSpeed=abs(SENSOR_1);
          ClearSensor(S1);
          SpeedTime=CurrentTick();
          Speed=(TmpSpeed*SpeedRatio)/(SpeedTime-SpeedTime_OLD);
          SpeedTime_OLD=SpeedTime;
          }
          
       // Monitor Fuel Level (Battery)
       TmpFuel=BatteryLevel()-BattEmpty;
       if (TmpFuel<0) TmpFuel=0;                                                // Disgard negative vaulues
       TmpFuel= TmpFuel / ((BattFull-BattEmpty) / 100);
       if (TmpFuel>100) TmpFuel=100;                                            // Disgard extremely charged batteries
       TmpFuel=(TmpFuel+Fuel+Fuel_OLD)/3;                                       // Take avarage of three measurements
       Fuel_OLD=Fuel;                                                           // because of instable readings
       Fuel=TmpFuel;
       if (Fuel<BattCharge) ReFuel=true; else ReFuel=false;

       // Decide to Activate CPA or ADC
       Distance=SensorUS(IN_2);                                                 // Measure distance in cm with Ultrasonic Sensor
       DebugVal=Distance;
       if (Distance<BreakDist) {                                                // Decide to activate CPA
          CPA=true;                                                                // Enable emergency breaking to avoid collision
          ADC=false;
          }
       else {
          CPA=false;                                                                // Decide to activate ADC, only when CPA is not active
          if (Distance<CoastDist) ADC=true;
          else ADC=false;
          }

       // Activate Richtingaanwijzers when a sharp curve is made (more degrees then "FlashLight"
       if (SteerEdge<-FlashLight) GoRight=true; else GoRight=false;
       if (SteerEdge>FlashLight) GoLeft=true; else GoLeft=false;

       // Activate Head Lights when driving
       if (PRND!=Park) {
         OnFwdEx(OUT_C, 80, 0);
         LightsOffTimer=CurrentTick();
         }
       else
         if (LightsOffTimer+2000<CurrentTick()) Off(OUT_C);                     // Dimm Headligts after xx msec of in-activity

       // End of OPERATE loop
       }
}

task main()                         // Initialize Remote Control.
{
    Precedes(ChangeDebug,Operate,Steering);
    SetSensor(S1, SENSOR_ROTATION);                                             // Port S1 = Touch Sensor for Breaking and Reverse
    SetSensorLowspeed(IN_2);                                                    // Port S2 = Ultrasonic Sensor.

    ClearSensor(S1);                                                            // Reset rotation reading to ZERO

    start SteerInit;                                                            // Reset SteeringMotor.
    start StatusDisplay;                                                        // Start displaying Car Status
    
    // Start Dashboard Test Sequence
    Tmp=0;
    while (Tmp<1) {
        Tmp++;
        PRND=0;
        while (PRND<4) {
           PRND++;
           Wait(200);
           }

        Speed=0;
        while (Speed<101) {
           Speed=Speed+4;
           MotPwr=Speed;
           Fuel=Speed;
           Wait(25);
           }
         PRND=Neutral;
         while (Speed>-1) {
           if (Speed<20) Speed=Speed-1; else Speed=Speed-3;
           MotPwr=Speed;
           Fuel=Speed;
           Wait(50);
           }

        Speed=0;
        ADC=true;
        GoLeft=1;
        GoRight=1;
        ReFuel=1;
        Wait(500);
        CPA=true;
        ADC=false;
        Wait(500);
        CPA=false;
        GoLeft=0;
        GoRight=0;
        ReFuel=0;
       }

    // SetUp Bluetooth Communications (This NXT is the Slave)
    // Wait for connection request from MASTER NXT and Show Status!!!
    // Standard NXT Firmware (v1.04) is sufficient for these BT Calls.
    while (!BluetoothStatus(BT_CONN)==NO_ERR){
       PlayFile ("Attention.rso");
       NoRC=true;
       Wait(2000);
       }
    Wait(1000); // Wait for Steering Init to complete
    
    start BT_TxRx;
    ShowDebugInfo=false;

    // Init procedure has finished, start other tasks on termination of Main task.
}