Turn LED ON using accelerometer

Hi Everyone,
I am working on a project where i have a 3 Axis accelerometer fitted on a RC car. This function is when the car impacts an objects, LED is activated. I am using an Arduino Uno and MMA8451 accelerometer for this project.
I am looking to adapt my code to set g-force limits (example 0.4g) to temporarily activate a orange LED’s on the car for 5 seconds then go off. If this Orange LED is activated three times a second (Red) LED is activated and stays ON until the program or button is reset.

The current code I am using operates differently

1st impact - Orange LED ON for 5 seconds then go’s OFF.
2nd impact - Orange LED ON for 10 seconds then go’s OFF, After 5 seconds of this the Red LED comes ON and stays ON.
3rd impact - Orange LED ON for 5 seconds then go’s OFF.
4th impact - Orange LED ON for 5 seconds then go’s OFF

Can anybody help with adapting the below code so the LED will come ON when g-force limit is reached ?

Thanking you in advance of your help.

#include <Wire.h>
#include <Adafruit_MMA8451.h>
#include <Adafruit_Sensor.h>

//#define TEST_RESULTANT      1               //comment out to exclude resultant vector from g check
#define MAX_G               3.0             //g limit over which an "event" is logged (org LED etc) (individual axes)
#define MAX_G_RESULTANT     3.0             //g limit over which an "event" is logged (org LED etc) (resultant vector only)
#define ORG_LED_ONTIME      5000ul          //mS orange LED flash time when G limit exceeded
#define EVENTS_LIMIT        3.0             //# of times G limit can be exceeded before RED LED comes on
#define ERROR_BLINK         300ul           //mS blink time for error condition
//
#define SAMPLE_RATE         5000            //uS (microseconds) per sample
#define LOGGING_RATE        100             //mS between logging file writes 

const int pinORANGE =       13;             //pin driving orange LED. Logic assumes HIGH turns LED on
const int pinRED =          12;             //pin driving red LED. Logic assumes HIGH turns LED on

Adafruit_MMA8451 mma = Adafruit_MMA8451();

char
   szStr[50];

//accumulators for x, y and z axes averaging
//samples are taken once per millisecond
//average is written to SD card at 100mS intervals 
float
   xg_f,
   yg_f,
   zg_f,    
   xg_sum,
   yg_sum,
   zg_sum;
unsigned long
   sample_count;
bool
   bGLimitExceeded;
   
void setup() 
{
   // Open serial communications and wait for port to open:
   Serial.begin(9600);
   pinMode( pinORANGE, OUTPUT );
   digitalWrite( pinORANGE, LOW );         //LED off.
   pinMode( pinRED, OUTPUT );
   digitalWrite( pinRED, LOW );            //LED off.
       
   Serial.println("Adafruit MMA8451 OK");
   if (!mma.begin()) 
   {
       Serial.println("###Failed to start MMA8451.");
       ErrorLED();
       
   }//if
   else
       Serial.println("MMA8451 found!");
 
   mma.setRange(MMA8451_RANGE_4_G);        //need +/-4g for checking >3g...
   Serial.print(" Range = "); Serial.print(2 << mma.getRange());  
   Serial.println("G");
   
   bGLimitExceeded = false;
   xg_sum = 0.0;
   yg_sum = 0.0;
   zg_sum = 0.0;
   sample_count = 0;
   
}//setup

//if an error occurs during init, LEDs alternate blinking rapidly
void ErrorLED( void )
{
   bool
       bLEDState = false;
   unsigned long
       timeBlink = 0;
   unsigned long
       timeNow;

   while( true )
   {
       timeNow = millis();
       //time for a blink?
       if( timeNow - timeBlink >= ERROR_BLINK )
       {
           //yes, save time
           timeBlink = timeNow;
           //toggle flag indicating LED state
           bLEDState ^= true;
           //update orange and red LEDs
           digitalWrite( pinORANGE, (bLEDState)?HIGH:LOW );
           digitalWrite( pinRED, (bLEDState)?LOW:HIGH );
           
       }//if
   
   }//while
   
}//ErrorLED

void TakeSamples()
{
   float
       intermediate,
       resultant;
   static unsigned long
       timeSample = 0;
   unsigned long
       timeNow;

   timeNow = micros();
   if( (timeNow - timeSample ) < SAMPLE_RATE )
       return;
   timeSample = timeNow;

   mma.read();
   xg_f = (float)mma.x/2048.0;
   yg_f = (float)mma.y/2048.0;
   zg_f = (float)mma.z/2048.0;
   
   xg_sum = xg_sum + xg_f;
   yg_sum = yg_sum + yg_f;
   zg_sum = zg_sum + zg_f;
   sample_count = sample_count + 1;

   //look for excursions over pre-set limits for LED flag
   //include check of resultant vector; can comment this out
   //if not desired
#ifndef TEST_RESULTANT
   resultant = 0.0; 
#else
   resultant = sqrt( pow(xg_f,2) + pow(yg_f,2) + pow(zg_f,2) );
#endif

   if( (abs(xg_f) > MAX_G ) ||
           (abs(yg_f) > MAX_G ) ||
               (abs(zg_f) > MAX_G ) ||
                   resultant > MAX_G_RESULTANT )
   {
       bGLimitExceeded = true;
          
   }//if
   
}//TakeSamples

#define LED_IDLE        0
#define LED_TIME        1
void LED_StateMachine( void )
{
   static int
       nEvents = 0;
   static byte
       stateLED = LED_IDLE;
   static unsigned long
       timeLED;
   unsigned long
       timeNow;

   switch( stateLED )
   {
       case    LED_IDLE:
           if( bGLimitExceeded )
           {
               bGLimitExceeded = false;
               nEvents++;
               if( nEvents >= EVENTS_LIMIT )
                   digitalWrite( pinRED, HIGH );
                   
               digitalWrite( pinORANGE, HIGH );
               timeLED = millis();
               stateLED = LED_TIME;
               
           }//if
       break;

       case    LED_TIME:
           if( (millis() - timeLED) >= ORG_LED_ONTIME )
           {
               digitalWrite( pinORANGE, LOW );
               stateLED = LED_IDLE;
               
           }//if
       break;
   
   }//switch
   
}//LED_StateMachine

void Logging( void )
{
   float
       x, y, z;
       
   static unsigned long
       timeLogging = 0;
   unsigned long
       timeNow;

   timeNow = millis();
   if( (timeNow - timeLogging) < LOGGING_RATE )
       return;
   timeLogging = timeNow;

   x = xg_sum / (float)sample_count;
   y = yg_sum / (float)sample_count;
   z = zg_sum / (float)sample_count;
   xg_sum = 0.0;
   yg_sum = 0.0;
   zg_sum = 0.0;
   sample_count = 0;
 
    
   Serial.print("\tX:\t"); Serial.print(x); 
   Serial.print("\tY:\t"); Serial.print(y); 
   Serial.print("\tZ:\t"); Serial.println(z); 

}//Logging

void loop() 
{
   //samping, logging and LED control done in their own functions
   TakeSamples();
   Logging();
   LED_StateMachine();
     
}//loop

Arduino Schematic - 3.pdf (134 KB)