/*

S_CARmod11   = 1.1 version - Auto Steering Initialisation added. (Front Wheels automatically point forward on power-up)
S_CARmod10   = 1.0 version - 4WD version Adapted for 2nd Car, high speed version.
                             Steering Ratio adapted to other mechanism in Car (TachoRatio Changed from 5 into 1)
                             Motor B now is a NXT Motor not a XL-Motor
                             Motor B polarity is reversed (Fwd <> Rev) because of different gearing
                             Speed measurement through Tachometer of Motor B (No RCX Rotation Sensor needed)
                             Speed Measurement placed as First Routine in operational Motor Management Loop
                             SpeedRatio changed from 1923 into 125
                             ADC Emergency Braking Distance "CoastDist" changed from 30 into 40cm because of higher speeds
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 connectd 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     40           // 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     1           // One degree SteerEdge = xx Ticks in TachoCount of NXT Motor
#define SteerTick      1           // One Sensor Tick = xx Degrees Steering
#define SpeedRatio     125         // 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,30);                          // This command clears the TachoCounter by default.
    SteerEdge_Now=MotorTachoCount(OUT_A);     // Measure motor movement
    Wait(50);
    while (SteerEdge_Now<>MotorTachoCount(OUT_A)) {
      SteerEdge_Now=MotorTachoCount(OUT_A);   // Measure motor movement again
      Wait(50);
      }
    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,30);                          // This command clears the TachoCounter by default.
    SteerEdge_Now=MotorTachoCount(OUT_A);     // Measure motor movement
    Wait(50);
    while (SteerEdge_Now<>MotorTachoCount(OUT_A)) {
      SteerEdge_Now=MotorTachoCount(OUT_A);   // Measure motor movement again
      Wait(50);
      }
    Off(OUT_A);                               // This command clears the TachoCounter by default.
    SteerEdge_Now=-SteerEdge_Now/2;

    // Set Front wheels in forward direction

    RotateMotor(OUT_A, 50, SteerEdge_Now);
    Wait(100);
    // 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)
       {
       // Monitor Speed MUST BE PLACED BEFORE MOTOR MANAGEMENT BECAUSE OF TACHO RESETS
       TmpSpeed=TmpSpeed+abs(MotorTachoCount(OUT_B));  //Increase TmpSpeed because Tacho is reset every Motor Management Loop
       if (CurrentTick()>SpeedTime_OLD+250) {
          SpeedTime=CurrentTick();
          Speed=(TmpSpeed*SpeedRatio)/(SpeedTime-SpeedTime_OLD);
          DebugVal=TmpSpeed;
          SpeedTime_OLD=SpeedTime;
          TmpSpeed=0;  // Reset TachoSum.
          }

       // 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) OnFwd(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;
                       OnRev(OUT_B, TmpMotPwr);
                       }
                    }
                 else
                    OnRev(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 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
       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);
       }

    start BT_TxRx;
    ShowDebugInfo=false;

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