Biometric Car Entry - True Keyless Car

/****************************************************************************
  Drive Mode(SPORTS/NORMAL/ECO Selection Switch, Wheel Position Warning, Automatic Brake hold enabling, Speed autolock, 
  Fingerprint Door locking and unlocking implementation module for Toyota C-HR(Model ZYX10R - AHXNBW 4186)

  Written by Rajeev Velikkal
  Details of implementation available on www.rajeev.velikkal.com
  Examples from SparkFun CAN library, please have the library added to your Arduino IDE
  Special thanks to Mr. Michel van der Zwet for his guidance and support on this project
  Distributed as-is; no warranty is given.
  Caution: Playing with CAN BUS Newtork Gateway is not advisable unless you know what you are doing. 
  A wrong message sent to CAN BUS can even pop the AIRBAGS
  Version History:
  Ver16: Added vehicle speed < 1 to the wheel not steady warning condition(line 125)
*************************************************************************/

#include <Canbus.h> //SparkFun Can Library
#include <defaults.h> //SparkFun Can Library
#include <global.h> //SparkFun Can Library
#include <mcp2515.h> //SparkFun Can Library
#include <mcp2515_defs.h> //SparkFun Can Library
#include <avr/wdt.h> //For Watchdog timer setup

#define CAR_ON_INTERRUPT_PIN 2 //This is a hardware interrupt pin and is really fast

#define MODE_SWITCH_PIN_ECO 4 //Please use an external pull up resistor of 4.7K between this pin and GND. For bringing it high connect 5V through the switch
#define MODE_SWITCH_PIN_SPORTS 5  //Please use an external pull up resistor of 4.7K between this pin and GND.  For bringing it high connect 5V through the switch
#define FINGER_PRINT_STATUS_PIN 6 //Please use and external pull up resistor of 4.7k between this pin and GND. For bringing this pin high connect 5V through finger print module relay

#define BRAKE_HOLD_ACTIVATE_PIN 8 // This output PIN is used to trigger brake hold switch. The code will just bring the pin high for just 1 second once the car starts moving. 
// Connect a BC 547 transistor's(NPN) base via a 1k resistor to this pin, the emitter to brake hold swith (pink wire of EPB Module connector) and collector to ground of UNO
#define STEERING_NOT_STEADY_ALERT_PIN 9 // This output pin is connected to a beeper, so that if the wheels are not steady a beep sound will be heard after parked.

uint8_t loopcount = 0;  // The loop counter that loops through the CAN board initialisation if it fails, attempted 5 times only after which no further attempt is done
uint8_t count = 0;  //Counter for sending the drive mode command once the engine started
uint8_t sendcount = 0;  //Counter for sending unlock comment by finger print sensor

volatile uint8_t ECUModeSelected = 2; // 1 = Sport, 2 = Normal, 3 = ECO
volatile uint8_t ECUModePrev = 0; // ECUModeSelected is saved to this when routine ECU_Mode is called, to track mode switch changes. If previous in not equal to current then no changes to mode done by user
volatile uint8_t carOn = 0; // Variable to denote if car is in on postion. 1 is ON and 0 is OFF

uint8_t doorsOpen = 0;  // Whether door was opened. If opened then change flag to 1
uint8_t lockedflag = 0; // Whether autolocking is done. If done then changed to 1
uint8_t lockORunlock = 0; // Variable passed to the Locking and Unlocking routine, 1 is passed for locking and 0 for unlocking
uint8_t EnableDisableWarning = 0; // Variable passed to EnableDisableWarning routine 0 is passed for disabling and 1 for enabling key not found warnings
uint8_t OnceOrTwice = 0; // Variable passed to BlinkInidcators routine to blink indicator lights once or twice for locking blink once, for unlocking blink twice

uint8_t reverseGearEngagedFlag = 0; //Flag to catch any shifting to reverse gear. Wheel not steady alert should be given only if the car was engaged in reverse gear(parking attempt)
uint8_t gear = 0;  //Determines in what gear the car is. This is used mainly for Wheel not steady alert
uint8_t steeringDirection = 0; //Steering turning to left or right. 0 is left and 1 is right
uint16_t steeringAngle = 0; //Calculated angle value of Steering
uint8_t WheelSteadyAllowedAngle = 20; //Maximum allowed Wheel turn Angle for not giving warning beep

uint8_t ModeStateSports = 0; // Initialising button status of Sports mode. If 1 then SPORT MODE to be activated
uint8_t ModeStateEco = 0; // Initialising button status of Eco mode. If 1 then ECO MODE to be activated

uint8_t brakeHoldFlag = 0; //Initialising brakeHoldFlag. Once brake hold button is pressed set to 1 so that this is not done again

uint8_t FingerPrintStatus = 0; // Initialising the status of finger print reader, if success then value set to 1, else 0.
uint8_t FingerPrintPIN = 0; //Status flag to say if fingerprint unlocking to be done. Using this flag since finger print reader makes the pin high for 10 sec
uint8_t DoorFlag = 0; // Initialising the status handler for DoorUnlocking using finger print. Once unlocked the value changed to 1.
uint8_t DoorOpenedStatus = 0; // Initialising the DoorOpenedStatus flag so that warning can be thrown if trying to lock while doors are already open

float VehicleSpeed = 0; //Initialising VehicleSpeed variable

uint8_t PowerState = 0; //Variable to hold the car power status, OFF/ACC/ON
uint8_t PowerOnFlag = 0; //Variable to control the entry to the condition once things are all set after power on

tCAN message;

//***************************************************************************//
//********************************Setup *************************************//
//***************************************************************************//

void setup()
{
  wdt_disable(); // Disable watchdog timer
  delay(1000);
  wdt_enable(WDTO_8S); //Enable watchdog timer

  // initialize the digital pins as an input
  pinMode(MODE_SWITCH_PIN_SPORTS, INPUT);
  pinMode(MODE_SWITCH_PIN_ECO, INPUT);
  pinMode(FINGER_PRINT_STATUS_PIN, INPUT);
  
  // initialize the digital pins as an output
  pinMode(BRAKE_HOLD_ACTIVATE_PIN, OUTPUT);
  pinMode(STEERING_NOT_STEADY_ALERT_PIN, OUTPUT);
  
  // bring pins low
  digitalWrite(STEERING_NOT_STEADY_ALERT_PIN, LOW);
  digitalWrite(BRAKE_HOLD_ACTIVATE_PIN, LOW);
  
  //Serial.begin(9600);
INIT:
  if (Canbus.init(CANSPEED_500) && loopcount < 6) //Initialise MCP2515 CAN controller at the specified speed, attempt 5 times
  {
    //Serial.println("CAN Init ok");
  }
  else
  {
    delay(1000); //Give some time before next attempt
    loopcount ++; //Increment loop count so as to try attempting only 5 times
    goto INIT; // Try initialising again if current attempt failed
  }
}

//***************************************************************************//
//********************************FUNCTIONS**********************************//
//***************************************************************************//

void BrakeHoldActivate()
{
  if ( brakeHoldFlag == 0 )
  {
    digitalWrite(BRAKE_HOLD_ACTIVATE_PIN, HIGH);
    delay(100);
    digitalWrite(BRAKE_HOLD_ACTIVATE_PIN, LOW);
    brakeHoldFlag = 1;
  }
}

//***BEEP IF STEERING IS NOT STEADY**STARTS**//
void wheelNotSteadyAlert()
{
  if (reverseGearEngagedFlag == 1 && steeringAngle > WheelSteadyAllowedAngle && VehicleSpeed < 1) //Checks if a parking attempt was done, engaging reverse gear followed by engaging Park
  {
    digitalWrite(STEERING_NOT_STEADY_ALERT_PIN, HIGH); // Set pin high so that the beep is activated
  }
  else
  {
    digitalWrite(STEERING_NOT_STEADY_ALERT_PIN, LOW); // stop beeping stop beeping if wheel is steady (<15degree)
  }
}
//***BEEP IF STEERING IS NOT STEADY**ENDS**//

//***DOOR LOCK UNLOCK******//
void doorLockUnlock(uint8_t lockORunlock)
{
    tCAN message_lock;
    message_lock.id = 0x750;
    message_lock.header.rtr = 0;
    message_lock.header.length = 8;
    message_lock.data[0] = 0x40;
    message_lock.data[1] = 0x05;
    message_lock.data[2] = 0x30;
    message_lock.data[3] = 0x11;
    message_lock.data[4] = 0x00; //0x06 Changed to handle alarm due to internal unlocking when locked from outside 
    if (lockORunlock == 1 ) //LOCK DOORS {0x40,0x05,0x30,0x11,0x00,0x80,0x00,0x00} normal locking 
    {
    message_lock.data[5] = 0x80;
    }
    else if (lockORunlock == 0 ) //UNLOCK DOORS  {0x40,0x05,0x30,0x11,0x00,0x40,0x00,0x00} normal unlocking 
    {
    message_lock.data[5] = 0x40;
    }
    message_lock.data[6] = 0x00;
    message_lock.data[7] = 0x00;

    mcp2515_bit_modify(CANCTRL, (1 << REQOP2) | (1 << REQOP1) | (1 << REQOP0), 0);
    mcp2515_send_message(&message_lock); //SEND MESSAGE
}
//***DOOR LOCK UNLOCK***ENDS***//

//***BLINK INDICATORS***STARTS***//
void BlinkIndicators(uint8_t OnceOrTwice)
{
    tCAN message_blink;
    message_blink.id = 0x623;
    message_blink.header.rtr = 0;
    message_blink.header.length = 8;
    message_blink.data[0] = 0x19;
    message_blink.data[1] = 0x80;
    message_blink.data[2] = 0x80;
    message_blink.data[3] = 0x00;
    message_blink.data[4] = 0x00; 
    message_blink.data[5] = 0x00; 
    message_blink.data[6] = 0x00;
    if (OnceOrTwice == 1 ) //Blink Once for DOORs locked acknowledgement     0x623; //623#19 80 80 00 00 00 00 20
    {
    message_blink.data[7] = 0x20;
    }
    else if ( OnceOrTwice == 2 ) //Blink Twice for DOORs unlocked acknowledgement 0x623; //623#19 80 80 00 00 00 00 40
    {
    message_blink.data[7] = 0x40;
    }

    mcp2515_bit_modify(CANCTRL, (1 << REQOP2) | (1 << REQOP1) | (1 << REQOP0), 0);
    mcp2515_send_message(&message_blink); //SEND ATLEAST TWICE MESSAGE
  } 
//***BLINK INDICATORS***ENDS***//

//****CAR TURNED ON INTERRUPT ***STARTS**//
void CAR_ON_ISR()
{
  carOn = digitalRead(CAR_ON_INTERRUPT_PIN); //Hardware PIN interrupt to see if the CAR key is in ACC/Ignition ON position
  if (carOn == HIGH)
  {
    ECU_Mode(ECUModeSelected); //CALL DRIVE MODE SWITCHING ROUTINE. This is to set drive mode after car restarts.
  }
}
//****CAR TURNED ON INTERRUPT ***ENDS**//

//*****DRIVE MODE SELECTION****STARTS**//

void ECU_Mode(uint8_t mode)
{
    //***PREPARE MESSAGE***//
    tCAN message_mode;
    message_mode.id = 0x6D1; 
    message_mode.header.rtr = 0;
    message_mode.header.length = 8; 
    message_mode.data[0] = 0xD6;
    message_mode.data[1] = 0x80;
    message_mode.data[2] = 0x00;
    message_mode.data[3] = 0x00;
    if ( ECUModeSelected == 1 ) // = SPORT        {0xD6,0x80,0x00,0x00,0x40,0x00,0x00,0x00} SPORT MODE
    {
    message_mode.data[4] = 0x40;
    }
    else if ( ECUModeSelected == 3 ) // = ECO     {0xD6,0x80,0x00,0x00,0x08,0x00,0x00,0x00} ECO MODE
    {
    message_mode.data[4] = 0x08;
    }
    else if ( ECUModeSelected == 2 ) // = NORMAL  {0xD6,0x80,0x00,0x00,0x04,0x00,0x00,0x00}; NORMAL MODE
    {
    message_mode.data[4] = 0x04;
    }
    message_mode.data[5] = 0x00;
    message_mode.data[6] = 0x00;
    message_mode.data[7] = 0x00;

    mcp2515_bit_modify(CANCTRL, (1 << REQOP2) | (1 << REQOP1) | (1 << REQOP0), 0);
    mcp2515_send_message(&message_mode); //SEND MESSAGE
  }
//*****DRIVE MODE SELECTION****ENDS**//

//******CAN ACTIVATION*****STARTS****//
void activateCAN()
{
 //***PREPARE MESSAGE***//
    tCAN message_act;
    message_act.id = 0x750;
    message_act.header.rtr = 0;
    message_act.header.length = 8;
    message_act.data[0] = 0x00;
    message_act.data[1] = 0x00;
    message_act.data[2] = 0x00;
    message_act.data[3] = 0x00;
    message_act.data[4] = 0x00; 
    message_act.data[5] = 0x00;  
    message_act.data[6] = 0x00;
    message_act.data[7] = 0x00;
    
    mcp2515_bit_modify(CANCTRL, (1 << REQOP2) | (1 << REQOP1) | (1 << REQOP0), 0);
    mcp2515_send_message(&message_act); //SEND MESSAGE
}
//******CAN ACTIVATION*****ENDS****//

//******FINGER PRINT DOOR POSITION WARNINGs*****STARTS****//
void Warnings(uint8_t warnings)
{
   if ( warnings == 1 ) // Warn that door is not closed
    {
      //***PREPARE MESSAGE***//
      tCAN message_warD;
      message_warD.id = 0x620; //620#10 00 00 00 00 03 00 00
      message_warD.header.rtr = 0;
      message_warD.header.length = 8;
      message_warD.data[0] = 0x10;
      message_warD.data[1] = 0x00;
      message_warD.data[2] = 0x00;
      message_warD.data[3] = 0x00;
      message_warD.data[4] = 0x00; 
      message_warD.data[5] = 0x03;
      message_warD.data[6] = 0x00;
      message_warD.data[7] = 0x00;
    
      mcp2515_bit_modify(CANCTRL, (1 << REQOP2) | (1 << REQOP1) | (1 << REQOP0), 0);
      mcp2515_send_message(&message_warD); //SEND MESSAGE
    }
    else if (warnings == 3) //CAR STILL ON WARNING
    {
      //***PREPARE MESSAGE***//
      tCAN message_warP;
      message_warP.id = 0x633; //633#81 00 00 00 00 00 01 00 Turn off ignition
      message_warP.header.rtr = 0;
      message_warP.header.length = 8;
      message_warP.data[0] = 0x81;
      message_warP.data[1] = 0x00;
      message_warP.data[2] = 0x00;
      message_warP.data[3] = 0x00;
      message_warP.data[4] = 0x00; 
      message_warP.data[5] = 0x00;
      message_warP.data[6] = 0x01;
      message_warP.data[7] = 0x00;
    
      mcp2515_bit_modify(CANCTRL, (1 << REQOP2) | (1 << REQOP1) | (1 << REQOP0), 0);
      mcp2515_send_message(&message_warP); //SEND MESSAGE
    }
}
//******FINGER PRINT DOOR POSITION WARNINGs*****ENDS****// 

//**************************************************************************//
//********************************Main Loop*********************************//
//**************************************************************************//

void loop()
{
  ModeStateSports = digitalRead(MODE_SWITCH_PIN_SPORTS); //Read Sport Mode Pin
  ModeStateEco = digitalRead(MODE_SWITCH_PIN_ECO);  //Read ECO Mode Pin
  FingerPrintPIN = digitalRead(FINGER_PRINT_STATUS_PIN); //Read to see if this finger print sensor pin is high, which means unlock door


  //0x200 //0x2** 226: Brake amount / position, 266: Steeringwheel angle
  //0x300 //0x3** 396: Park assist, 399: Cruise Control, 3b7: Brake Holding, 3bc: Gear lever PRNDB, Drive Mode, 3cb: Speed (speedmodeplus)
  //0x600 //0x6** 610: Speed (seems missing sometimes), 620: Doors + power state, 621: Door lock status 0 = Unlocked, 1 = Inside locked, 2 = Outside locked, 633: No key detection / turn off warning, 635: Approach / leave
  //0x700 //0x7** 758: Request responses

  
  //******READ MESSAGES*********STARTS***//
  if (mcp2515_check_message()) //See if there is any message in the buffer
  {
    if (mcp2515_get_message(&message)) //Read the message
    {
      if (message.id == 0x620) //Message related to door and Power 
       { 
        if ((message.data[5] & 0x3c) > 0) //See if this message is related to door opened
          {
            lockedflag = 0; // If door opened reset lockedflag, so that locking can be done again after car reaches 10kmph speed 
            DoorOpenedStatus = 1; //Set to 1 so that fingerprint locking will create warning that the doors are open when trying to lock                
          }
         else 
          {
            DoorOpenedStatus = 0; //Set to 0 so that fingerprint locking can be done                
          }
        PowerState = ((message.data[4] & 0x30)>>4); // Get Power status, 0 = Power OFF, 1 = ACC, 2 = IGN?, 3 = Power ON
        count ++;        
        if (count == 50) //Catering for the delay, drive mode cannot be activated immediately after starting car
            {
              ECU_Mode(ECUModeSelected); //CALL DRIVE MODE SWITCHING ROUTINE
            }
        if (PowerState == 3 && PowerOnFlag == 0 )
          {
            lockedflag = 0; // Reset flag to 0 so that door locking routine can run after a power on
            brakeHoldFlag = 0; // Reset the flag so that brakehold activation can be done after a power on            
            PowerOnFlag = 1; // Set Flag so that when power is on ECU_Mode Selecting routine is not called again 
          }           
         else if (PowerState == 0)
         {
          PowerOnFlag = 0; // Reset Flag so that when power is off all initializations are done again
          count = 0;
         }
      }
     else if (message.id == 0x621) //Message related to door locking
     
       { 
        if (message.data[2] == 18) //See if unlocking done  621# 11 80 F8 00 00 00 00 00 unlocked can bus
          {
            DoorFlag = 0; // If door locked or unlocked flag,  locked = 1, and unlocked = 0 
            FingerPrintStatus = 0; //Set status to 0 so that the locking/unlocking can be done next time
          }
         else if (message.data[2] == 78) //See if locking done  621# 11 80 78 00 00 00 00 00 locked can bus
          {
            DoorFlag = 1; // If door locked or unlocked flag,  locked = 1, and unlocked = 0
            FingerPrintStatus = 0; 
          }
       }
     else if ( message.id == 0x635 ) //Approach / leave / interior lights?
        { 
            if ( (message.data[1] == 0x80) ) //0x80 means it is an action, otherwise we ignore it.
              { 
                if ( (message.data[6] & 0x12) > 0 ) // Approaching (in range, outside)
                  { 
                   doorLockUnlock(0);
                  }
              }
        }          
      else if (message.id == 0x610 || message.id == 0x3CB) //See if the message is related to speed
        {
          VehicleSpeed = (uint8_t)message.data[2]; //Get speed value
        }
     
      else if (message.id == 0x3bc) //START GEAR PRNDB MESSAGE
        {
          if ( (message.data[1] & 0x20) > 0 ) // P = 0
            {
              gear = 0; //Park Gear
              wheelNotSteadyAlert(); //CALL WHEEL NOT STEADY ROUTINE
            }
          else if ((message.data[1] & 0x10) > 0) // R = 1
            {
              gear = 1; //Reverse Gear
              reverseGearEngagedFlag = 1; //Flag to capture if Reverse Gear is engaged
            }
          else if ( (message.data[1] & 0x08) > 0 ) // N = 2
            {
              gear = 2; //Neutral Gear
              reverseGearEngagedFlag = 0; //Flag to capture if Reverse Gear is engaged
            }
          else if ( (message.data[5] & 0x80) > 0 ) // D = 3
            {
              gear = 3; //D Drive Gear
              reverseGearEngagedFlag = 0; //Flag to capture if Reverse Gear is engaged
            }
          else if ( (message.data[5] & 0x02) > 0 ) // B = 4
            {
              gear = 4; //B Drive Gear
              reverseGearEngagedFlag = 0; //Flag to capture if Reverse Gear is engaged
            }
        } //END Gear PRNDB MESSAGE

      else if ( message.id == 0x025 ) //Steering Angle Message. The calculation is:- Left Angle = value / 180 = degrees, Right Angle = (0x0FFFFF - value) / 180
        {
          uint32_t SteeringPOS = ((uint32_t)(message.data[0] & 0x0f) << 16) + ((uint32_t)message.data[1] << 8) + (uint32_t)message.data[7];
          if ( SteeringPOS < 0x07ffff ) //Left
          {
            //steeringAngle = (uint16_t)(SteeringPOS/18); //factor 10 higher for precision
            steeringAngle = (uint16_t)(SteeringPOS / 180);
            steeringDirection = 0; //For Left
          }
          else //Right
          {
            //steeringAngle = (uint16_t)((0xfffff-SteeringPOS)/18); //factor 10 higher for precision
            steeringAngle = (uint16_t)((0xfffff - SteeringPOS) / 180);
            steeringDirection = 1; // For Right
          }
        }
      }
  }
  //****READ MESSAGES************ENDS****//
  
  //******* CHECK VEHICLE SPEED AND ACTIVATE STUFFS***STARTS**//

  if (VehicleSpeed > 1) //If speed is greater than 1 KMPH activate brakehold- STARTS
  {
    if (brakeHoldFlag == 0) //if Brake Hold activation not done before then activate
    {
      BrakeHoldActivate(); //Activate brakehold. Once activated, brakeHoldFlag will be set to 1 by the routine, so that this is not done again
    }
  } //If speed is greater than 1 KMPH activate brakehold************ENDS
  
  if (VehicleSpeed > 10) //If speed is greater than 10 KMPH - STARTS
  {
    if (lockedflag == 0) // The doors have not been autolocked before then lock doors
    {
      doorLockUnlock(1); //CALL DOOR LOCKING/ULOCKING ROUTINE for LOCKING
      lockedflag = 1; // Set the door locked flag to 1 so that until a door open is detected the door locking is not done again
    }    
  } //If speed is greater than 10 KMPH************ENDS
  //******* CHECK VEHICLE SPEED AND ACTIVATE STUFFS*****ENDS**//

  //*****READ FINGER PRINT READER STATUS, IF HIGH (SUCCESS) UNLOCK DOORS******STARTS***//
  if (FingerPrintPIN == HIGH && FingerPrintStatus == 0)
  {
    FingerPrintStatus = 1; // Set the flag to 1 so that even though the pin is high for 10 sec we need it only to do once.
  }
  else 
  {
     FingerPrintStatus = 0;
  }
  if (FingerPrintStatus == 1 && DoorFlag == 0)
  {
    if (DoorOpenedStatus == 0 && PowerState == 0)
      {
        activateCAN();
        delay(100);
        doorLockUnlock(1); //CALL DOOR LOCKING/UNLOCKING ROUTINE for LOCKING by FINGERPRINT SENSOR
        delay(200);
        BlinkIndicators(1);
        FingerPrintStatus = 0;  
        delay(3000);
        DoorFlag = 1;
      }
     else if (PowerState == 3) //IF CAR IS STILL ON, THROW A WARNING AND DON'T LOCK DOORS
      {
        Warnings(3); //CAR power on Warning
      }
     else
     {
        Warnings(1); //DOORS not Closed Warning
     }
  }
  else if (FingerPrintStatus == 1 && DoorFlag == 1)
  {
      BlinkIndicators(2);
      delay(10);
      BlinkIndicators(2);      
      delay(100);
      doorLockUnlock(0); //CALL DOOR LOCKING/UNLOCKING ROUTINE for UNLOCKING by FINGERPRINT SENSOR 
      delay(200);
      BlinkIndicators(2);
      FingerPrintStatus = 0; 
      delay(3000);
      DoorFlag = 0;      
  }
  
  //*****READ FINGER PRINT READER STATUS, IF HIGH (SUCCESS) UNLOCK DOORS*********ENDS***//
  
  //****CHECK DRIVE MODE SWITCH POSITION AND ACTIVATE RESPECTIVE MODE********STARTS****//
  // A SPDT ON-OFF-ON switch is used, with its 3 position, Up for Sport, Down for Eco and Middle(no connections) for Normal drive mode user selection
  if (ModeStateSports == HIGH && ModeStateEco == LOW)  // Sports Mode
  {
    ECUModeSelected = 1 ;
  }
  else if (ModeStateSports == LOW && ModeStateEco == HIGH) // Eco Mode
  {
    ECUModeSelected = 3 ;
  }
  else
  {
    ECUModeSelected = 2 ; // Normal Mode
  }
  if ( ECUModePrev != ECUModeSelected ) // If change of mode detected
  {
    ECUModePrev = ECUModeSelected;
    ECU_Mode(ECUModeSelected); //CALL DRIVE MODE SWITCHING ROUTINE
  }
  // ****CHECK DRIVE MODE SWITCH POSITION AND ACTIVATE RESPECTIVE MODE********ENDS****//

  wdt_reset(); //RESET WATCHDOG

} // END OF MAIN LOOP