renegadetechworks.com - OpenSource

RenegadeTechWorks 3000gt ECS Model 01/02 Controller Open Source Down Loads

support@renegadetechworks.com (TechWorks Admin) — Fri, 29 Aug 2014 22:28:07 +0000

***************************************************************************************
Description of Software &  electrical design:    
    Mitsubishi 3000gt/Stealth Custom Controller system
    for controlling stock OEM mitsubishi ECS Struts.
    Hardware Platform:   ATAMega 328 MicroController

    Copyright (C) 2014  Marcus Diaz, RenegadeTechWorks, LLC

***************************************************************************************
Licensing:
            Licensed under GNU GPL Version 2

  These software and hardware designs dowloadable below are free software. 
  you can redistribute it and/or modify it under the terms of the 
  GNU General Public License as published by the Free Software Foundation
  under Version 2 of the License

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 
 
  See this link GNU General Public License for more details. or write to the Free
  Software Foundation, Inc.,51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
****************************************************************************************

Do a "right click -> save link as " to download and save the files below.

The system platform is based around an amtel328 core design using 12 channels of the Microcontroller for monitoring and driving the struts using a Texas Inst. 4 channel H-Bridge motor driver . 2 channels for the sport /tour lights, 1 channel for the Selector switch, & 2 channels for controlling the addressable 4 LED array, 1 channel for switch selector mode type. Total channels used = 18.

Renegade TechWorks 3000gt ECS Model 01 Controller Schematic PDF

The software contained in the .INO file below was developed using the Arduino IDE - set for an Uno / Amtel328 compile target. The techworks controller can be programmed by using the current Arduino IDE with the standard Upload command & USB FTDI upload cable. The cable connects to the 6- Pin programming headers on the controller board.

The coding style is flat C-coding. No C++ and tons of comments.

Renegade TechWorks 3000gt ECS M01/M02 Controller Firmware

RenegadeTechWorks 3000gt ECS Model 03 Controller Open Source Down Loads (2)

support@renegadetechworks.com (TechWorks Admin) — Fri, 29 Aug 2014 22:28:07 +0000

***************************************************************************************
Description of Software &  electrical design:    
    Mitsubishi 3000gt/Stealth Custom Controller system
    for controlling stock OEM mitsubishi ECS Struts.
    Hardware Platform:   ATAMega 328 MicroController

    Copyright (C) 2014  Marcus Diaz, RenegadeTechWorks, LLC

***************************************************************************************
Licensing:
            Licensed under GNU GPL Version 2

  These software and hardware designs dowloadable below are free software. 
  you can redistribute it and/or modify it under the terms of the 
  GNU General Public License as published by the Free Software Foundation
  under Version 2 of the License

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 
 
  See this link GNU General Public License for more details. or write to the Free
  Software Foundation, Inc.,51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
****************************************************************************************

Do a "right click -> save link as " to download and save the files below.

The system platform is based around an amtel328 core design using 12 channels of the Microcontroller for monitoring and driving the struts using a Texas Inst. 4 channel H-Bridge motor driver . 2 channels for the sport /tour lights, 1 channel for the Selector switch, & 1 channels for controlling the addressable 4 LED array, 2 channels for I2C controll of the Acceleromter Total channels used = 18.

Renegade TechWorks 3000gt ECS Model 03 Controller Schematic PDF

TechWorks ECS Model 01/02 Electrical Schematics

support@renegadetechworks.com (TechWorks Admin) — Fri, 29 Aug 2014 20:42:34 +0000

TechWorks ECS Model 01/02 Firmware Source Code

support@renegadetechworks.com (TechWorks Admin) — Fri, 22 Aug 2014 19:06:53 +0000

//****************************************************************************
//
//     Description of Software:    
//
//     Mitsubishi 3000gt/Stealth Custom Controller Program
//     for controlling stock OEM mitsubishi ECS Struts.
//     Hardware Platform:   ATAMega 328 MicroController
//
//     Copyright (C) 2014  Marcus Diaz, RenegadeTechWorks, LLC
//
//****************************************************************************
// Licensing:
//              Licensed under GNU GPL Version 2
//
//    This program is free software; you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation under Version 2 of the License
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License along
//    with this program; if not, write to the Free Software Foundation, Inc.,
//    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//****************************************************************************
// VERSION HISTORY
// --------------------------------------------------------------------
// SW Ver  :  v1.0
// HWDesign:  ECM M01 PCB Rev1.1
// Date:      09/15/2012
// Comments:  Production version of code based on branch from original
//            prototype controller 
//            Diag Mode working - this version disables Select SW. 
//            check during Diag Mode Execution.'
//            This overcomes Voltage Drop.
//
//--------------------------------------------------------------------
// SW Ver  :  v1.1
// HWDesign:  ECM M01 PCB Rev1.1
// Date:      10/7/2012
// Comments:  Production version of code based on branch from original
//            prototype controller 
//            -v1.1 uses a Jumper accross Pins 6 <--> Pin3 on the
//             program header to indicate what type of selector switch
//             is being used as follows:
//                Jumper On : USE push Button ECS Switch
//                Jumper Off: Use Rotatry 4 Position Selector Switch
//            -v1.1 Has implemented the new re-written Diagonstic seq.
//                for using both LED's and OEM Tour/Sport Lights
//            -v1.1 also implements Diagnostic mode interrupt code
//            -v1.1 Implements a Dimming control option. To use at 
//                 start up put the system in Diagnostic Mode using
//                 using either the stock ECS switch by pressing and
//                 holding or Rotary switch to position 4. 
//*******************************************************************
// SW Ver  :  v1.1
// HWDesign:  ECM M01 PCB Rev1.1
// Date:      10/12/2012
// Comments:  Base  SVN production version of the code.
//            
//          Production version of code based on branch from original
//            prototype controller 
//            -v1.1 uses a Jumper accross Pins 6 <--> Pin3 on the
//             program header to indicate what type of selector switch
//             is being used as follows:
//                Jumper On : USE push Button ECS Switch
//                Jumper Off: Use Rotatry 4 Position Selector Switch
//            -v1.1 Has implemented the new re-written Diagonstic seq.
//                for using both LED's and OEM Tour/Sport Lights
//            -v1.1 also implements Diagnostic mode interrupt code
//            -v1.1 Implements a Dimming control option. To use at 
//                 start up put the system in Diagnostic Mode using
//                 using either the stock ECS switch by pressing and
//                 holding or Rotary switch to position 4. 
//
//--------------------------------------------------------------------
// SW Ver  :  v1.2
// HWDesign:  ECM M01 PCB Rev1.1
// Date:      10/13/2012
// Comments:  
//            added 2000 msec delay in diagonstic routine during display
//            of sucessfull strut test when both Tour & Sport lights
//            are turned on.            
//            
//--------------------------------------------------------------------
// SW Ver  :  v1.3
// HWDesign:  ECM M01 PCB Rev1.1
// Date:      10/23/2012
// Comments:  
//          - Corrected all code paths to correctly handle UNKNOWN Strut State
//          - Made Setup initialization routine more robust - all struts must aggree
//          - Also during setup Tour/Sport will go into error mode if there
//            was disagreement
//          - added displayStrut as derivative of original readstrut()
//          - doubled MAXTRIES to 10000 - helps with laggy struts 
//          - fixed bug in readSelector() in case of PUSHBUTTON - was 
//            missing " else return (strutState)" at end
//--------------------------------------------------------------------
// SW Ver  :  v1.5
// HWDesign:  ECM M01 PCB Rev1.1
// Date:      10/31/2012
// Comments:  
//          - Baselining this as the final production version for the first production
//            run. 
//          - Other than these comments there are no diffs between this and 
//          - the previous verson
//--------------------------------------------------------------------
// SW Ver  :  v1.6
// HWDesign:  ECM M01 PCB Rev1.1
// Date:      4/13/2014
// Comments:  
//          - Added saftey code to detect intermittent broken signal wire
//            and prevent motor driver burn out
//          - When broken wire detected - Both Tour Sport Lights will alternately flash
//          -added new diagnostic read status mode (only available from pushbutton)
//          -added new startup & error sequencing display routines for Sport/Tour Lights
//          -LEDs now Have Yellow Error Condition = Signal Wire Failure
//          -LEDs Red Error Condition = Motor command sequence failure.
//--------------------------------------------------------------------
// SW Ver  :  v1.7
// HWDesign:  ECM M01 PCB Rev1.1
// Date:      4/13/2014
// Comments:  
//          -Added EProm Read/Write Code during startup to retrieve/store LED 
//           brigtness defaults
//          -Increased MaxTries by 20% from 10000 to 12000
//--------------------------------------------------------------------
// SW Ver  :  v1.7
// HWDesign:  ECM M01 PCB Rev1.1
// Date:      4/17/2014
// Comments:  
//          -Minor display tweek for Diagnostic display of Sport / Tourlights
//           So that they just blink 3 times in groups of 3 regardless of which 
//           strut is being reported for a Motor Command Failure
//--------------------------------------------------------------------
// SW Ver  :  v1.7a
// HWDesign:  ECM M01 PCB Rev1.1
// Date:      4/17/2014
// Comments:  
//          -increased MaxTries to 15000 from 12000
//--------------------------------------------------------------------
// SW Ver  :  v1.8
// HWDesign:  ECM M01 PCB Rev1.1,1.2
// Date:      12/28/2014
// Comments:  
//          -Replaced LED CODE to use FAST SPI LED Library 
//          -Now supports WS2801 & WS2812b & Others
//          -Added code to read Pin Jumper on Pins 2 to 3.
//          -If Pin 2 Jumpered to ground/Pin3 Code will use WS2801 LED 
//           IF Pin 2 not Jumpered to Ground/ Pin 3 Code will use WS2812b LED 
//
//          -In displayStrut - Added setLED(x,D_OFF)to Case Unknown to make
//           timed loop equal for all cases while Maxtries is incrementing
//          --NOTE 1.8a & 1.8 have a timming issue with WS2801 LEDs.
//           all other code fine.
//--------------------------------------------------------------------
// SW Ver  :  v1.8a
// HWDesign:  ECM M01 PCB Rev 1.1,1.2
// Date:      1/1/2015
// Comments:  
//          -removed led.Show() call from setLed()
//          -added led.Show() in all places needed
//          -NOTE 1.8a & 1.8 have a timming issue with WS2801 LEDs.
//           all other code fine.    
//--------------------------------------------------------------------
// SW Ver  :  v1.9
// HWDesign:  ECM M01 PCB Rev1.1,1.2
// Date:      2/18/2015
// Comments:  
//          -removed all delays that were added to try and force sync up
//          -upgraded to fastLED lib 3.0 (fixed sync problem with WS2801's)
//--------------------------------------------------------------------
// SW Ver  :  v1.9a
// HWDesign:  ECM M01 PCB Rev 1.1,1.2
// Date:      2/18/2015
// Comments:  
//          -added 1 millesecond delay to Motor enable drive loop to fix
//           problem with motor turning of too soon on the edge case
//--------------------------------------------------------------------
// SW Ver  :  v1.10
// HWDesign:  ECM M01 PCB Rev 1.1,1.2
// Date:      8/02/2015
// Comments:  
//          -setStrutMode() added 33 msec delay just outside of Motor While loop to
//           force motor to run 33 msec longer.  Forces overrun of switch target
//          -setStrutMode() removed saftey checks for previous strut errors, code 
//           will now retry a strut even if in previous cycle it had any kind of error.
//          -increased MaxTries to 7194(2.0 secs) from 2000
//--------------------------------------------------------------------
// SW Ver  :  v1.11
// HWDesign:  ECM M01 PCB Rev 1.1,1.2
// Date:      8/09/2015
// Comments:  
//          -displayStrut() 
//             -added debounce algorithm to ignore S1 S2 transients
//              less than X microseconds apart.
//             -added timeout code to abort after Y milleseconds of trying
//
//--------------------------------------------------------------------
// SW Ver  :  v1.12
// HWDesign:  ECM M01 PCB Rev 1.1,1.2
// Date:      8/09/2015
// Comments:  
//          -Removed MotorEnable(motorx,LOW) commands all placess except Startup
//          -Set Motor Delay overun to 5msec instead of 33msec
//
//*****************************************************************************

#include 
#include 

#define strutFR 1
#define strutFL 2
#define strutRR 3
#define strutRL 4

#define HARD     1
#define MEDIUM     2
#define SOFT     3
#define DIAG    4
#define READDIAG 5
#define UNKNOWN 7
#define NOCHANGE 10
#define STARTUP  11

#define D_RED       1
#define D_GREEN       2
#define D_BLUE       3
#define D_TURQUOIS 4
#define D_PURPLE   5
#define D_YELLOW   6
#define D_STARTUP  7
#define D_HARD     8
#define D_MEDIUM   9
#define D_SOFT     10
#define D_WHITE    11
#define D_OFF      12 


#define RED100   0xFF0000 // Bright Red
#define GREEN100 0x00FF00 // Bright Green
#define BLUE100  0x0000FF // Bright Blue
#define RED001   0x010000 // Faint red
#define RED050   0x800000 // 1/2 red (0x80 = 128 out of 256)
#define RED025   0x300000 // 25% RED
#define GREEN050 0x008000 // half Green
#define GREEN025 0x003000 // 25% GREEN
#define BLUE050  0x000080 // half Blue
#define BLUE025  0x000030 // 25% BLUE
#define WHITE015 0x151515 // 15% White
#define WHITE010 0x101010 // 10% White
#define WHITE005 0x050505 // 05% White
#define WHITE001 0x020202 // 05% White
#define WHITE100 0xFFFFFF // 100% White

#define LEDOFF           0x000000    
#define RED            0xFF0000 
#define ORANGE         0xFF5500
#define ORANGEYELLOW   0xFFA000
#define YELLOW         0xFFFF00
#define YELLOWGREEN    0xA0FF00
#define GREENYELLOW    0x50FF00
#define GREEN          0x00FF00
#define GREENCYAN      0x00FF4B
#define CYAN           0x00FFFF
#define CYANBLUE       0x00A0FF
#define BLUECYAN       0x005AFF  
#define BLUE           0x0000FF
#define BLUEVIOLET     0x4800FF
#define VIOLETBLUE     0x7500FF
#define MAGENTA        0xFF00FF
#define PINKDEEP       0xFF1493
#define PINKHOT        0xFF69B4
#define PINK           0xF3967A
 
#define VIOLET         0xEE82EE


#define ERROR 1
#define NOERROR 0

#define SIGNAL_WIRE_FAILURE   2
#define MOTOR_COMMAND_FAILURE 1
#define NO_FAILURES 0

#define MAXTRIES 7194    // maxtries = 7194 = max of 2 secs of Motor on


#define MAXLIGHTLOOPS 20

#define ROTARY 1
#define PUSHBUTTON 2
#define SAMEMODE 1
#define NEXTMODE 2

#define LED_LEVEL1 1       // minimum brightness level
#define LED_LEVEL2 2
#define LED_LEVEL3 3
#define LED_LEVEL4 4       // Maximum brightness level

#define EPROM_LEDMODE 1    // address in EPROM for LEDMODE

//
//  Allocate Structure For LED Strip
//
#define NUM_LEDS 4
struct CRGB ledColors[NUM_LEDS];

int ledMode = LED_LEVEL2;

int selectorType = ROTARY;  // selectorType captures what kind of selector switch is hooked up to unit

// Previous State of Selector Switch
int currentSelectorState = STARTUP;

int strutState = HARD;
int strutStateFL;
int strutStateFR;
int strutStateRL;
int strutStateRR;

long ledHardValue ;
long ledMediumValue;
long ledSoftValue;
long ledRedValue;


int LED1 = 0;    // Front Left Strut LED
int LED2 = 1;    // Front Right Strut LED
int LED3 = 2;    // Rear Left Strut LED
int LED4 = 3;    // Rear Right Strut LED 

int errorFL = NO_FAILURES;
int errorFR = NO_FAILURES;
int errorRL = NO_FAILURES;
int errorRR = NO_FAILURES;

int tryCount = 0;

int lightLoopCounter = MAXLIGHTLOOPS;
int lightLoopCounterValue;

///////////////////////////////////////////////////////////////////////////////////////
//
//  Map Board I/O Pins to World
//
// /////////////////////////////////////////////////////////////////////////////////////
// Front Left Strut Pins
int strutFLswitch1 = 3; 
int strutFLswitch2 = 2;
int strutFLmotor   = 10;

// Front Right Strut Pins
int strutFRswitch1 = 5; 
int strutFRswitch2 = 4;
int strutFRmotor   = 11;

// Rear Left Strut Pins
int strutRLswitch1 = 7; 
int strutRLswitch2 = 6;
int strutRLmotor   = 14;

// Rear Right Strut Pins
int strutRRswitch1 = 9; 
int strutRRswitch2 = 8;
int strutRRmotor   = 15;

//Mode Selector Switch Pin
int ModeSelectSwitch = 17;

//Switch Type Selector Option Pin
int switchTypeSelect = 1;

//LED Type Selector Option Pin
int LEDTypeSelect = 0;

//MotorEnable Pin
int MotorEnable = 16;

//
// LED Light Ports 
//
#define DATA_PIN 18    // Data pin that led data will be written out over
#define CLOCK_PIN 19   // Clock pin only needed for SPI based chipsets when not using hardware SP

// Sport Light Port
int SportLight = 12; 

// Tour Light Port
int TourLight = 13;



 
///////////////////////////////////////////////////////////////////////////////////////
// setup initializes startup 
///////////////////////////////////////////////////////////////////////////////////////
void setup(){

        int selectorValue;
        int diagLoopNumber;
  

      // initalize MotorEnable Pin for digital output and disable motors
    pinMode(MotorEnable, OUTPUT);
    digitalWrite(MotorEnable,LOW);
 
        // Initialize the Sport & Tour Light OutPut Pins
        pinMode(SportLight, OUTPUT); 
        pinMode(TourLight, OUTPUT);
 
          // intialiaze all the motor out pins
        initializeMotorIOPin(strutFLmotor);
        initializeMotorIOPin(strutFRmotor);
        initializeMotorIOPin(strutRRmotor);
        initializeMotorIOPin(strutRLmotor);
    
        // Initialize all the strut input pins 
     initializeStrutIOPin(strutFRswitch1);
     initializeStrutIOPin(strutFRswitch2);
     initializeStrutIOPin(strutFLswitch1);
     initializeStrutIOPin(strutFLswitch2);
     initializeStrutIOPin(strutRRswitch1);
     initializeStrutIOPin(strutRRswitch2);
     initializeStrutIOPin(strutRLswitch1);
     initializeStrutIOPin(strutRLswitch2);
 


    // analyze   LED  Pin To figure out what type of leds are being used 
        // HIGH = WS2801   LOW = WS2812
        pinMode(LEDTypeSelect, INPUT);
        digitalWrite(LEDTypeSelect,HIGH); 

        // Read the pin to see if it's HIGH or LOW      
        if(digitalRead(LEDTypeSelect) == HIGH){
           FastLED.addLeds(ledColors, NUM_LEDS);
    }else{
            FastLED.addLeds(ledColors, NUM_LEDS);
    }

        // ************************************************************
        //
        // LED Setup & Initialization
        //
        // ************************************************************
        // Uncomment one of the following lines for your leds arrangement.
        // FastLED.addLeds(ledColors, NUM_LEDS);
        // FastLED.addLeds(leds, NUM_LEDS);
        // FastLED.addLeds(leds, NUM_LEDS);
        // FastLED.addLeds(leds, NUM_LEDS);
        // FastLED.addLeds(leds, NUM_LEDS);
        // FastLED.addLeds(leds, NUM_LEDS);
        // FastLED.addLeds(ledColors, NUM_LEDS);
        // FastLED.addLeds(leds, NUM_LEDS);
        // FastLED.addLeds(ledColors, NUM_LEDS);
        // FastLED.addLeds(leds, NUM_LEDS);
        // FastLED.addLeds(leds, NUM_LEDS);
        // FastLED.addLeds(leds, NUM_LEDS);
        // FastLED.addLeds(leds, NUM_LEDS);
       
        // Clear Out the LED arrays
        memset(ledColors, 0,  NUM_LEDS * sizeof(struct CRGB));   
        LEDS.show(); //Push the current color frame to the LEDs


    // analyze Selector TYPE Input Pin To figure out if Rotary of Pushbutton is connected 
        // HIGH = Rotary   LOW = PUSHBUTTON
        pinMode(switchTypeSelect, INPUT);
        digitalWrite(switchTypeSelect,HIGH); 

        // Read the pin to see if it's HIGH or LOW      
        if(digitalRead(switchTypeSelect) == LOW){
             selectorType = PUSHBUTTON;
             pinMode(ModeSelectSwitch, INPUT);
             digitalWrite(ModeSelectSwitch, HIGH);
             lightLoopCounterValue = 900; // the lightloopcounter is timing dependent based on selector type
             diagLoopNumber = 450;
        }else{
             selectorType = ROTARY;
             pinMode(ModeSelectSwitch, INPUT);
             digitalWrite(ModeSelectSwitch, LOW);
             lightLoopCounterValue = 60;  // the lightloopcounter is timing dependent based on selector type
             diagLoopNumber = 40;
        } 
        lightLoopCounter = lightLoopCounterValue;

    //
    // Retrieve LED Brigtness level from EPROM - if not within range set it to 2nd level and write it back        //
        ledMode = eepromReadInt(EPROM_LEDMODE);    
    //
    // Make sure the value is with in range
    //
    if (ledMode != LED_LEVEL1 && ledMode != LED_LEVEL2 && ledMode != LED_LEVEL3 && ledMode != LED_LEVEL4){
        // It's not within range - set to LEVEL two and write it back
        ledMode = LED_LEVEL1;
            eepromWriteInt(EPROM_LEDMODE,ledMode);
    } 


    //
    // Blink the Sport / Tour Lights at startup as test to driver to show they are working.
    //
        blinkBothLights(1,3000,1,800);
    
  
    //
    //SET the LEDs to what the EPROM Value Says
    //
    switch(ledMode){
        case LED_LEVEL1:
                     //SET the LEDs to Minimum Brightness Mode    
                         ledHardValue = rgbEncode(2,0,2);
                         ledMediumValue = rgbEncode(0,2,2);
                         ledSoftValue = rgbEncode(0,0,2);
                         ledRedValue = rgbEncode(2,0,0);

            break;
        case LED_LEVEL2:
                     //SET the LEDs to 2nd to Minimum Brightness Mode    
                         ledHardValue = rgbEncode(10,0,10);
                         ledMediumValue = rgbEncode(0,10,5);
                         ledSoftValue = rgbEncode(0,0,10);
                         ledRedValue = rgbEncode(10,0,0);
            break;
        case LED_LEVEL3:
                     //SET the LEDs to Medium Brightness Mode    
                         ledHardValue = rgbEncode(70,0,70);
                         ledMediumValue = rgbEncode(0,70,35);
                         ledSoftValue = rgbEncode(0,0,70);
                         ledRedValue = rgbEncode(70,0,0);
            break;
        case LED_LEVEL4:
                     //SET the LEDs to Maximum Brightness Mode    
                         ledHardValue = rgbEncode(120,0,120);
                         ledMediumValue = rgbEncode(0,120,60);
                         ledSoftValue = rgbEncode(0,0,150);
                         ledRedValue = rgbEncode(120,0,0);
            break;
    }

        //  
        //See if user wants to set brightness level of LEDs
        //
        // Let them know it's time & wait 3 secs
        setLED(LED1, D_WHITE);
        setLED(LED2, D_WHITE);
        setLED(LED3, D_WHITE);
        setLED(LED4, D_WHITE);
        
        LEDS.show();
    
        delay(2000);



    //read the selector PUSHBUTTON or Rotary and see if they have it in DIAGMODE
        selectorValue = readSelectorSwitch();
        if(selectorValue == READDIAG || selectorValue == DIAG ){

            // ok..they want to adjust - put the leds into their medium value colors so they know
            strutState=DIAG;
            
        //Default the LEDs to MEDIUM Mode    
            ledHardValue = rgbEncode(10,0,10);
            ledMediumValue = rgbEncode(0,10,5);
            ledSoftValue = rgbEncode(0,0,10);
            ledRedValue = rgbEncode(10,0,0);
            strutState = MEDIUM;
  
            setLED(LED1, D_HARD);
            setLED(LED2, D_MEDIUM);
            setLED(LED3, D_SOFT);
            setLED(LED4, D_RED);
            LEDS.show();
                    
            // Loop for a while to let the adjust and then bail
        for(int i = 1; i                   delay(20);
                   switch(readSelectorSwitch()){
                      case HARD:
                     //SET the LEDs to Minimum Brightness Mode    
                     ledMode = LED_LEVEL1;
                         ledHardValue = rgbEncode(2,0,2);
                         ledMediumValue = rgbEncode(0,2,2);
                         ledSoftValue = rgbEncode(0,0,2);
                         ledRedValue = rgbEncode(2,0,0);
  
                         setLED(LED1, D_HARD);
                         setLED(LED2, D_MEDIUM);
                         setLED(LED3, D_SOFT);
                         setLED(LED4, D_RED);
                         LEDS.show();
                      break;

                      case MEDIUM:
                     //SET the LEDs to 2nd to Minimum Brightness Mode    
                     ledMode = LED_LEVEL2;
                         ledHardValue = rgbEncode(10,0,10);
                         ledMediumValue = rgbEncode(0,10,5);
                         ledSoftValue = rgbEncode(0,0,10);
                         ledRedValue = rgbEncode(10,0,0);
  
                         setLED(LED1, D_HARD);
                         setLED(LED2, D_MEDIUM);
                         setLED(LED3, D_SOFT);
                         setLED(LED4, D_RED);
                         LEDS.show();
                      break;

                      case SOFT:
                     //SET the LEDs to Medium Brightness Mode    
                     ledMode = LED_LEVEL3;
                         ledHardValue = rgbEncode(70,0,70);
                         ledMediumValue = rgbEncode(0,70,35);
                         ledSoftValue = rgbEncode(0,0,70);
                         ledRedValue = rgbEncode(70,0,0);
  
                         setLED(LED1, D_HARD);
                         setLED(LED2, D_MEDIUM);
                         setLED(LED3, D_SOFT);
                         setLED(LED4, D_RED);
                         LEDS.show();
                      break;
               
              case DIAG:
                     //SET the LEDs to Maximum Brightness Mode    
                     ledMode = LED_LEVEL4;
                         ledHardValue = rgbEncode(120,0,120);
                         ledMediumValue = rgbEncode(0,120,60);
                         ledSoftValue = rgbEncode(0,0,150);
                         ledRedValue = rgbEncode(120,0,0);
  
                         setLED(LED1, D_HARD);
                         setLED(LED2, D_MEDIUM);
                         setLED(LED3, D_SOFT);
                         setLED(LED4, D_RED);
                         LEDS.show();
              break;
                 }
        }
        //
        // IF the new mode is not whats already in EProm Update EProm
        //
            if(ledMode != eepromReadInt(EPROM_LEDMODE)) eepromWriteInt(EPROM_LEDMODE,ledMode);
        }
        
    // Let them know it's over
        setLED(LED1,D_WHITE);
        setLED(LED2,D_WHITE);
        setLED(LED3,D_WHITE);
        setLED(LED4,D_WHITE);
        LEDS.show();
        delay(2000);
        
        // Set LEDS & Lights to current state of Struts 
        strutStateFL = displayStrut(strutFLswitch1,strutFLswitch2,LED1,ERROR);
        strutStateFR = displayStrut(strutFRswitch1,strutFRswitch2,LED2,ERROR);
        strutStateRL = displayStrut(strutRLswitch1,strutRLswitch2,LED3,ERROR);
        strutStateRR = displayStrut(strutRRswitch1,strutRRswitch2,LED4,ERROR); 
        
        
        // IF the state of all the struts match 
    // set strutState to whatever the Front Left Strut is reading     
    if(strutStateFL == strutStateFR && 
       strutStateFL == strutStateRL &&     
       strutStateFL == strutStateRR &&     
       strutStateFL != UNKNOWN ){
        
          strutState=strutStateFL;
          currentSelectorState = strutStateFL;
          
                  // Set the TourSport Lights to indicate initial setting 
              setTourSportLights();
        }else{
         // not sure what state all struts are in. Arbitarily try and set to HARD
         // Let user try and move to different state
         strutState=HARD;
         currentSelectorState = HARD;
                 setStrutMode( HARD, HARD, strutFLmotor, strutFLswitch1, strutFLswitch2,LED1, &errorFL);
                 setStrutMode( HARD, HARD, strutFRmotor, strutFRswitch1, strutFRswitch2,LED2, &errorFR);
                 setStrutMode( HARD, HARD, strutRLmotor, strutRLswitch1, strutRLswitch2,LED3, &errorRL);
                 setStrutMode( HARD, HARD, strutRRmotor, strutRRswitch1, strutRRswitch2,LED4, &errorRR);        

                 setTourSportLights();
    }
}


/////////////////////////////////////////////////////////////////////////////////////
//
//            main loop  
//
/////////////////////////////////////////////////////////////////////////////////////
void loop()
{
  
   //
   // Make Sure the Strut Motors are OFF
   //
   digitalWrite(strutFRmotor, LOW);
   digitalWrite(strutFLmotor, LOW);
   digitalWrite(strutRRmotor, LOW);
   digitalWrite(strutRLmotor, LOW);

   //  SIGNAL_WIRE_FAILURE   
   //  MOTOR_COMMAND_FAILURE 
   //  NO_FAILURES 
 
   
   delay(20);
   //
   // Read the state of the Switch
   //   
     switch(readSelectorSwitch()){
    case(NOCHANGE):
                // Set LEDS & Lights to current state of Struts 
                if (errorFL == NO_FAILURES && currentSelectorState != displayStrut(strutFLswitch1,strutFLswitch2,LED1,ERROR)) {
            errorFL = SIGNAL_WIRE_FAILURE;
            setLED(LED1,D_YELLOW);
                        LEDS.show();
                        // delayMicroseconds(800); //Wait for 800us to go into reset
        }

                if (errorFR == NO_FAILURES && currentSelectorState != displayStrut(strutFRswitch1,strutFRswitch2,LED2,ERROR)){
            errorFR = SIGNAL_WIRE_FAILURE;
                   setLED(LED2,D_YELLOW);
                        LEDS.show();
                        // delayMicroseconds(800); //Wait for 800us to go into reset
        }

                if (errorRL == NO_FAILURES && currentSelectorState != displayStrut(strutRLswitch1,strutRLswitch2,LED3,ERROR)){
            errorRL = SIGNAL_WIRE_FAILURE;
                   setLED(LED3,D_YELLOW);
                        LEDS.show();
                        // delayMicroseconds(800); //Wait for 800us to go into reset
        }

                if (errorRR == NO_FAILURES && currentSelectorState != displayStrut(strutRRswitch1,strutRRswitch2,LED4,ERROR)){
            errorRR = SIGNAL_WIRE_FAILURE;
            setLED(LED4,D_YELLOW); 
                        LEDS.show();
                        // delayMicroseconds(800); //Wait for 800us to go into reset
        }

        setTourSportLights();
                break;

    case(HARD):

                  // Front Right Strut 
                setStrutMode( HARD, HARD, strutFLmotor, strutFLswitch1, strutFLswitch2,LED1, &errorFL);
                setStrutMode( HARD, HARD, strutFRmotor, strutFRswitch1, strutFRswitch2,LED2, &errorFR);
                setStrutMode( HARD, HARD, strutRLmotor, strutRLswitch1, strutRLswitch2,LED3, &errorRL);
                setStrutMode( HARD, HARD, strutRRmotor, strutRRswitch1, strutRRswitch2,LED4, &errorRR);        

        // Set the TourSport Lights to indicate if setting of all four was sucessfull or Error 
        setTourSportLights();
        break;

    case(MEDIUM):

                setStrutMode( MEDIUM, MEDIUM, strutFLmotor, strutFLswitch1, strutFLswitch2,LED1, &errorFL);
                setStrutMode( MEDIUM, MEDIUM, strutFRmotor, strutFRswitch1, strutFRswitch2,LED2, &errorFR);
                setStrutMode( MEDIUM, MEDIUM, strutRLmotor, strutRLswitch1, strutRLswitch2,LED3, &errorRL);
                setStrutMode( MEDIUM, MEDIUM, strutRRmotor, strutRRswitch1, strutRRswitch2,LED4, &errorRR);        

        // Set the TourSport Lights to indicate if setting of all four was sucessfull or Error 
        setTourSportLights();
        break;

    case(SOFT):
                
                setStrutMode( SOFT, SOFT, strutFLmotor, strutFLswitch1, strutFLswitch2,LED1, &errorFL);
                setStrutMode( SOFT, SOFT, strutFRmotor, strutFRswitch1, strutFRswitch2,LED2, &errorFR);
                setStrutMode( SOFT, SOFT, strutRLmotor, strutRLswitch1, strutRLswitch2,LED3, &errorRL);
                setStrutMode( SOFT, SOFT, strutRRmotor, strutRRswitch1, strutRRswitch2,LED4, &errorRR);                 

        // Set the TourSport Lights to indicate if setting of all four was sucessfull or Error 
        setTourSportLights();
                break;



    case(READDIAG):
                readDiagnostics();
                break;

    case(DIAG):
                runDiagnostic();
                break;

        case(UNKNOWN):
        setTourSportLights();
                break;

       }

}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to Blink one of the lights
///////////////////////////////////////////////////////////////////////////////////////
void blinkLights(int lightaddress, int numberBlinks,int blinkDelay,int numberGroupBlinks,int groupDelay){
    
    setLights(lightaddress,LOW);
    
    for (int groupcount = numberGroupBlinks; groupcount > 0; groupcount--){
        
    for (int count = numberBlinks; count > 0; count--){
       delay(blinkDelay);
       setLights(lightaddress,HIGH);
       delay(blinkDelay);
       setLights(lightaddress,LOW);
        }
    delay(groupDelay);
    }
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to Blink one of the lights
///////////////////////////////////////////////////////////////////////////////////////
void blinkBothLights(int numberBlinks,int blinkDelay,int numberGroupBlinks,int groupDelay){
    
    // Turn off both Lights
    setLights(SportLight,LOW);
    setLights(TourLight,LOW);
    
    for (int groupcount = numberGroupBlinks; groupcount > 0; groupcount--){
        
    for (int count = numberBlinks; count > 0; count--){
       delay(blinkDelay);
           setLights(SportLight,HIGH);
           setLights(TourLight,HIGH);
       
       delay(blinkDelay);
           setLights(SportLight,LOW);
           setLights(TourLight,LOW);
        }
    delay(groupDelay);
    }
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to Set TourSport Lights to CorrectMode
///////////////////////////////////////////////////////////////////////////////////////
void setTourSportLights(){

                lightLoopCounter--;
                if(lightLoopCounter <= 0){
                    lightLoopCounter = lightLoopCounterValue;
                    
  
                  // if there was a problem start flashing shit.. to the do sport/tourlight error thing
            if (errorFL == SIGNAL_WIRE_FAILURE || 
                errorFR == SIGNAL_WIRE_FAILURE ||
                errorRL == SIGNAL_WIRE_FAILURE ||
                errorRR == SIGNAL_WIRE_FAILURE ){
               
                           for(int i=0;i<4;i++){
                         setLights(SportLight,HIGH);
                             setLights(TourLight,LOW);
                 delay(250);
                         setLights(SportLight,LOW);
                             setLights(TourLight,HIGH);
                 delay(250);
                   }
                    }
                          
                    if(errorFL == MOTOR_COMMAND_FAILURE || 
               errorFR == MOTOR_COMMAND_FAILURE ||
               errorRL == MOTOR_COMMAND_FAILURE ||
               errorRR == MOTOR_COMMAND_FAILURE ) {
                            blinkBothLights(1,300,3,1000);
                           // blinkLights(SportLight, 3,800,1,1); 
                            delay(300); 
                    }   

         }

                switch (strutState){
               case(HARD):
                         setLights(SportLight,HIGH);
                             setLights(TourLight,LOW);
                 break;

               case(MEDIUM):
                         setLights(SportLight,HIGH);
                             setLights(TourLight,HIGH);
                 break;

               case(SOFT):
                         setLights(SportLight,LOW);
                             setLights(TourLight,HIGH);
                 break;
                 
               case(UNKNOWN):
                         setLights(SportLight,LOW);
                             setLights(TourLight,LOW);
                 break;
        }
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to intialiaze IOPins for Strut Motors
///////////////////////////////////////////////////////////////////////////////////////
void initializeMotorIOPin(int pin){

    // Set pin used for strut motor as output 
    pinMode(pin, OUTPUT);

        // Make sure Motor is off
    digitalWrite(pin,LOW);
}

///////////////////////////////////////////////////////////////////////////////////////
// Routine to intialiaze IOPins for Strut Switches
///////////////////////////////////////////////////////////////////////////////////////
void initializeStrutIOPin(int pin){

    // Set pin used for strut motor as output 
    pinMode(pin, INPUT);

        // Pull Pin HIGH         
    digitalWrite(pin,HIGH);
}


///////////////////////////////////////////////////////////////////////////////////////
// Routine to set an LED  to a color by Address
///////////////////////////////////////////////////////////////////////////////////////
void setLED(int ledaddress, int color){
    //
        // rgbEncode(int red,int green,int blue)
    // red,green,blue =  0, 255
    //

    switch(color){
        case D_HARD:
            setLedColors(ledaddress,ledHardValue);
                break;    
        case D_MEDIUM:
            setLedColors(ledaddress,ledMediumValue);
                break;    
        case D_SOFT:
            setLedColors(ledaddress,ledSoftValue);
                break;    
        case D_RED:
            setLedColors(ledaddress,ledRedValue);
                break;    
        case D_GREEN:
            setLedColors(ledaddress,rgbEncode(0,10,0));
                break;    
        case D_BLUE:
            setLedColors(ledaddress,rgbEncode(0,0,25));
                break;    
        case D_TURQUOIS:
            setLedColors(ledaddress,rgbEncode(0,40,64));
                break;    
        case D_PURPLE:
            setLedColors(ledaddress,rgbEncode(10,0,10));
                break;    
        case D_YELLOW:
            setLedColors(ledaddress,rgbEncode(10,10,0));
                break;
        case D_STARTUP:
            setLedColors(ledaddress,rgbEncode(2,2,2));
                break;    
        case D_WHITE:
            setLedColors(ledaddress,rgbEncode(15,15,15));
                break;    
        case D_OFF:
            setLedColors(ledaddress,rgbEncode(0,0,0));
                break;    

             
        }
         // LEDS.show(); //PUSH the current color definitions of ALL the LEDs out
         // delayMicroseconds(600); //Wait for 200us to go into reset
     
}


///////////////////////////////////////////////////////////////////////////////////////
// Routine to turn an LED off
//////////////////////////////////////////////////////////////////////////////////////
void offLED(int ledaddress){
        
    setLedColors(ledaddress,LEDOFF);
        LEDS.show(); //PUSH the current color definitions of ALL the LEDs out
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to set either the Tour Or Sport light to an HIGH/ON or LOW/OFF state
///////////////////////////////////////////////////////////////////////////////////////
void setLights(int light, int state){
  digitalWrite(light,state);
        
}

/////////////////////////////////////////////////////////////////////////////////////
// Reference 5v = 1023
//
// Hard     0.0v     / xxx
//    Cutoff  0.415  / 85
// Medium   0.83v    / xxx
//    Cutoff   1.245   / 255
// Soft     1.66v    / xxx
//    Cutoff   2.075   / 424
// Diag     2.5      / xxx
//    Cutoff   2.915   / 596
/////////////////////////////////////////////////////////////////////////////////////
int readSelectorSwitch(){
    int voltage;
    int voltage2;
    float voltdiff;

    // Based on the selector switch type do the appropriate read
    if(selectorType == ROTARY){
        
       voltage = analogRead(ModeSelectSwitch)+1; // read the voltage    
       delay(500);                               // debounce for 500 msec
       voltage2 = analogRead(ModeSelectSwitch)+1;// read the voltage again
       voltdiff = abs(1.0-voltage/voltage2);     // Calculate the percentage difference 

       //
       // as long as voltage difference > 10% keep reading
       //
       while(voltdiff > .1 ){
          voltage = analogRead(ModeSelectSwitch)+1; // read the voltage    
          delay(500);                             // debounce for 500 msec
          voltage2 = analogRead(ModeSelectSwitch)+1;// read the voltage again
          voltdiff = abs(1.0-voltage/voltage2);   // Calculate the percentage difference 
       }

           //
       // based on the converted voltage value and the range return the value
           //
       if(voltage < 85){                          
             // Selector is in HARD mode 
             if (currentSelectorState == HARD) return (NOCHANGE) ;
         else {
               if ( currentSelectorState != STARTUP ) currentSelectorState =HARD;
           strutState = HARD;
               return(HARD);
         }
     
       }else if (voltage >= 85 && voltage < 255){ 
         // Selector is in MEDIUM mode 
             if (currentSelectorState == MEDIUM) return (NOCHANGE) ;
         else {
               if ( currentSelectorState != STARTUP ) currentSelectorState = MEDIUM;
               strutState = MEDIUM;
           return(MEDIUM);
         }

       }else if (voltage >= 255 && voltage < 424){
             // Selector is in SOFT mode 
             if (currentSelectorState == SOFT) return (NOCHANGE) ;
         else {
               if ( currentSelectorState != STARTUP ) currentSelectorState = SOFT;
               strutState = SOFT;
           return(SOFT);
         }

       }else if(voltage >= 424 && voltage <596){
             // Selector is in DIAG mode 
             if (currentSelectorState == DIAG) return (NOCHANGE) ;
         else {
               if ( currentSelectorState != STARTUP) currentSelectorState = DIAG;
               strutState = DIAG;
           return(DIAG);
         }

       }else{
              strutState=UNKNOWN;
          return (UNKNOWN);

       }

    }else if (selectorType == PUSHBUTTON){
            // if they are pressing switch i.e. LOW then decide what's next
            if(digitalRead(ModeSelectSwitch) == LOW){
                  delay(1000);
          //
          // If they are no longer pressing switch then shift the struts to next state
          //
                  if(digitalRead(ModeSelectSwitch) == HIGH){
                        switch(strutState){
                           case HARD:
                             currentSelectorState = MEDIUM;
                             strutState = MEDIUM;
                             return(MEDIUM);
                           break;

                           case MEDIUM:
                             currentSelectorState = SOFT;
                     strutState = SOFT; 
                             return(SOFT);
                           break;

                           case SOFT:
                             currentSelectorState = HARD;
                     strutState = HARD; 
                             return(HARD);
                           break;
               
               case READDIAG:
               case DIAG:
               case UNKNOWN:
                             currentSelectorState = DIAG;
                     strutState = HARD; 
                             return(HARD);
                           break;
                   }
            }else{
             //
             // They are still holding button down wait 3 secs
                     delay(3000);

             //If they are still holding the switch after 3secs - see if they want DIAGNOSTIC or READ DIAG mode
                     if(digitalRead(ModeSelectSwitch) == LOW){ 
                         blinkBothLights(3,150,2,1000); // Blink both Lights to let them know they are at this mode
                         setLED(LED1,D_OFF);
                         setLED(LED2,D_OFF);
                         setLED(LED3,D_OFF);
                         setLED(LED4,D_OFF);
                         LEDS.show();
                 delay (3000);
        
 
                         if(digitalRead(ModeSelectSwitch) == LOW){ 
                              //
                              // Blink both Lights to let them know they are at this mode
                  // They should let go now after seeing blinks if they want to run diag mode
                  // 
                               blinkBothLights(3,150,2,1000); // Blink both Lights to let them know they are at this mode
                          
                   // Let them know it's over
                               setLED(LED1,D_WHITE);
                               setLED(LED2,D_WHITE);
                               setLED(LED3,D_WHITE);
                               setLED(LED4,D_WHITE);
                               LEDS.show();
                       delay (3000);        

                     //If they let go before 3secs - then they want DIAGNOSTIC 
                             if(digitalRead(ModeSelectSwitch) == HIGH){ 
                     // Ok they let go they want to Run Diagnostic Mode
                                strutState = DIAG; 
                                return(strutState);
                 } else return (strutState);
                 
             }else {
                // Ok they let go they want to Read the current Diagnostic Codes
                            strutState = READDIAG;
                            return(strutState);
             }

             } else return (strutState);   //Nope they dont' want anything - just return current State.
               }
           
       }else if (currentSelectorState == DIAG && strutState == HARD){
           // if here then this is 2nd pass thru just after comming out of DIAG or READDIAG MODE
               // Switch was not being pressed and last state was DIAG mode Now RETURN HARD to MAIN loop one more time
               currentSelectorState = HARD;
               return(HARD);
       } else return(NOCHANGE);  // Switch is not being pressed - return NOCHANGE state
    }
}

/////////////////////////////////////////////////////////////////////////////////////
// Reference 5v = 1023
//
// Hard     0.0v     / xxx
//    Cutoff  0.415  / 85
// Medium   0.83v    / xxx
//    Cutoff   1.245   / 255
// Soft     1.66v    / xxx
//    Cutoff   2.075   / 424
// Diag     2.5      / xxx
//    Cutoff   2.915   / 596
/////////////////////////////////////////////////////////////////////////////////////
int readDiagSelectorSwitch(){
    int voltage;
    int voltage2;
    float voltdiff;

    // Based on the selector switch type do the appropriate read
    if(selectorType == ROTARY){
        
       voltage = analogRead(ModeSelectSwitch)+1; // read the voltage    
       delay(500);                               // debounce for 500 msec
       voltage2 = analogRead(ModeSelectSwitch)+1;// read the voltage again
       voltdiff = abs(1.0-voltage/voltage2);     // Calculate the percentage difference 

       //
       // as long as voltage difference > 10% keep reading
       //
       while(voltdiff > .1 ){
          voltage = analogRead(ModeSelectSwitch)+1; // read the voltage    
          delay(500);                             // debounce for 500 msec
          voltage2 = analogRead(ModeSelectSwitch)+1;// read the voltage again
          voltdiff = abs(1.0-voltage/voltage2);   // Calculate the percentage difference 
       }
           currentSelectorState = DIAG;

           //
       // based on the converted voltage value and the range return the value
           //
       if(voltage < 85) return (HARD) ;
       else if (voltage >= 85 && voltage < 255)  return (MEDIUM) ;
       else if (voltage >= 255 && voltage < 424) return (SOFT);
       else if(voltage >= 424 && voltage <596) return (NOCHANGE);
       else return (UNKNOWN);


    }else if (selectorType == PUSHBUTTON){
            // if they are pressing switch i.e. LOW then decide what's next
            if(digitalRead(ModeSelectSwitch) == LOW){
                 blinkBothLights(3,150,2,1000); // Blink both Lights to let them know they are at this mode
         delay (2000);
          //
          // they are pressing switch then shift the struts out of DIAG mode to HARD 
          //
                  currentSelectorState = DIAG;
          strutState = HARD; 
                  return(HARD);
            }else return (NOCHANGE);
         }
}

///////////////////////////////////////////////////////////////////////////////////////
//
//
// Routine to Read actual status of strut , set LED color apprropriately and return value
//
//
///////////////////////////////////////////////////////////////////////////////////////
int readstrut(int strutS1,int strutS2){
  int switch1;
  int switch2;


  switch2 = !digitalRead(strutS1);
  switch1 = !digitalRead(strutS2);
  
  if (switch1 == LOW && switch2 == HIGH){
    // Strut is in HARD mode
    return(HARD);
    
  }else if (switch1 == HIGH &&  switch2 == HIGH){
    // Strut is in MEDIUM mode  
    return(MEDIUM);
    
  }else if (switch1 == HIGH && switch2 == LOW){
    // Strut is in SOFT Mode
    return(SOFT);
  }else {
    return(UNKNOWN);
  }
}

///////////////////////////////////////////////////////////////////////////////////////
//
//
// Routine to Read actual status of strut , set LED color apprropriately and return value
//
//
///////////////////////////////////////////////////////////////////////////////////////
int displayStrut(int strutS1,int strutS2,int strutLED,int displayMode){
  int switch1;
  int switch2;
  
  int switch1b;
  int switch2b;

  int timeout = 0;
  int numberTries = 0;
/*
  unsigned long timestart;
  unsigned long timenow;
  timestart = millis();
  */

  //
  // intial switch reads
  //
  switch2 = !digitalRead(strutS1);
  switch1 = !digitalRead(strutS2);
  delayMicroseconds(50); //Wait for 100us to debounce contacts
  switch2b = !digitalRead(strutS1);
  switch1b = !digitalRead(strutS2);


  //
  // See if the switches are reading the same
  //
  while ((switch2 != switch2b || switch1 != switch1b)&& numberTries < 40){
      switch2 = !digitalRead(strutS1);
      switch1 = !digitalRead(strutS2);
      delayMicroseconds(50); //Wait for 50us to debounce contacts
      switch2b = !digitalRead(strutS1);
      switch1b = !digitalRead(strutS2);
      numberTries++;
      
  }
  
 
  if (switch1 == LOW && switch2 == HIGH && numberTries <40 ){
    // Strut is in HARD mode
    setLED(strutLED, D_HARD);
    LEDS.show();
    return(HARD);
    
  }else if (switch1 == HIGH &&  switch2 == HIGH && numberTries <40 ){
    // If Strut is in MEDIUM mode  
    setLED(strutLED, D_MEDIUM);
    LEDS.show();
    return(MEDIUM);
    
  }else if (switch1 == HIGH && switch2 == LOW && numberTries <40 ){
    // Strut is in SOFT Mode
    setLED(strutLED, D_SOFT);
    LEDS.show();
    return(SOFT);
  }else {
    if(displayMode == ERROR) setLED(strutLED, D_RED);
    else setLED(strutLED, D_OFF);
    LEDS.show();
    return(UNKNOWN);
  }
}
///////////////////////////////////////////////////////////////////////////////////////
//
//
// Routine to Set the desired strut to a particular mode        
//
/////////////////////////////////////////////////////////////////////////////////////
int setStrutMode( int selectorDesiredMode, int strutDesiredMode, int strutMotor, int strutSwitch1, int strutSwitch2,int displayLED, int *strutErrorStatus){
  int tryCount;
  tryCount = 0;

   //  SIGNAL_WIRE_FAILURE   
   //  MOTOR_COMMAND_FAILURE 
   //  NO_FAILURES 

                //
                // Reset the Error Flag since we are now allowing a retry for each strut.
                //
                *strutErrorStatus = NO_FAILURES;

                //
                // Enable MotorChip
                //
                digitalWrite(MotorEnable,HIGH);


                //
        // IF any previous failures - display status & kick out 
        //
                /***** This code disabled as of firmware version 1.10 **************
                if (*strutErrorStatus == MOTOR_COMMAND_FAILURE ){
                        setLED(displayLED,D_RED);
                        LEDS.show();
            return(ERROR);
        }else if (*strutErrorStatus == SIGNAL_WIRE_FAILURE){
                        setLED(displayLED,D_YELLOW);
                        LEDS.show();
            return(ERROR);
        }
                ********************************************************************/

                //
        // ok so no previous problems with this strut - get readay to set to new mode
                // 
        
                
                //
           // Turn on motor as long as strut is not in desired mode but only try the loop for MAXTRIES number of times
                //
                 while( displayStrut(strutSwitch1, strutSwitch2,displayLED,NOERROR) != strutDesiredMode &&
               tryCount < MAXTRIES ){
            
                digitalWrite(strutMotor, HIGH);
                  tryCount++;
            }
        
            // run the motors for 5 msecs more to force strut further into this zone.
            delay(5);
        
        // Turn the Motor Off
            digitalWrite(strutMotor, LOW);
                    
                 // if  MaxTries not exceeded strut is now in desired mode
          if(tryCount < MAXTRIES ){

                     // Strut is now in desired mode
                     *strutErrorStatus = NO_FAILURES;
                     return (NOERROR);

                }else {
            //
            // Maxtries was exceeded - display RED for 2 secs turn off for 1 sec & try again.
            //
                    setLED(displayLED,D_RED);
                    LEDS.show();
                    delay(1000);
                    setLED(displayLED,D_OFF);
                    LEDS.show();

            //
            // Try One More Time
            // 
            tryCount = 0;


                     while( displayStrut(strutSwitch1, strutSwitch2,displayLED,NOERROR) != strutDesiredMode && 
              tryCount < MAXTRIES ) {
                  digitalWrite(strutMotor, HIGH);
                    tryCount++;
                }

                // run the motors for 33msecs more to force strut further into this zone.
                    delay(33);

            // Turn the Motor Off
                digitalWrite(strutMotor, LOW);
    
              if(tryCount < MAXTRIES){
                         // Strut Managed to get into desired mode on 2nd try
                        *strutErrorStatus = NO_FAILURES;
                         return (NOERROR);
              }else{
               //
               // Maxtries was exceeded - Flag this strut as bad.
               //
                        setLED(displayLED,D_RED);
                        LEDS.show();
                        *strutErrorStatus = MOTOR_COMMAND_FAILURE;
                        return (ERROR);
              }
        }
         
}

//*******************************************************************************************
//
// Diagnostic routine
//
//
//*******************************************************************************************
void runDiagnostic(){
  
             while(readDiagSelectorSwitch()== NOCHANGE){ 
                   errorFR=0;
                   errorFL=0;
                   errorRR=0;
                   errorRL=0;
                   
                   // Turn Off All LEDS
                   offLED(LED1);
                   offLED(LED2);
                   offLED(LED3);
                   offLED(LED4);
                   
                   // Turn off Lights
               setLights(SportLight,LOW);
                   setLights(TourLight,LOW);
                   delay(1000);
  
                   // See if abort desired
                   if(readDiagSelectorSwitch()!= NOCHANGE) return; 

                   //////////////////////////////////////////////////////////////////////////////////////
                   //                    TEST FRONT LEFT STRUT
                   ///////////////////////////////////////////////////////////////////////////////////////
           
           
                   //SET LED 1 to White to indicate testing Front Left Strut 
                   setLED(LED1,D_WHITE);
                   LEDS.show();
           
           // Blink Sport Light 1 time as group 4 times To indicate FL strut
                   blinkLights(SportLight, 1,300,4,1000);  

           // See if abort desired
                   if(readDiagSelectorSwitch()!= NOCHANGE) return; 

                   // Do the test for this strut as many times as desired
               for(int i =1; i<= 1; i++){

                      //Attempt to set  Front Left Strut HARD MODE
                      setStrutMode( DIAG, HARD, strutFLmotor, strutFLswitch1, strutFLswitch2,LED1, &errorFL);
                      delay(1000);
                   
              // See if abort desired
              if(readDiagSelectorSwitch()!= NOCHANGE) return; 
                                      

                      //Attempt to set  Front Left Strut Medium MODE
                      setStrutMode( DIAG, MEDIUM, strutFLmotor, strutFLswitch1, strutFLswitch2,LED1, &errorFL);
                      delay(1000);
              
              // See if abort desired
              if(readDiagSelectorSwitch()!= NOCHANGE) return; 

                                      
                      //Attempt to set  Front Left Strut Soft MODE
                      setStrutMode( DIAG, SOFT, strutFLmotor, strutFLswitch1, strutFLswitch2,LED1, &errorFL);
                      delay(1000);

              // See if abort desired
              if(readDiagSelectorSwitch()!= NOCHANGE) return; 
              
           }

           // Display Final test results for this Strut
           if (errorFL == 0){
                       setLED(LED1,D_GREEN);
                       LEDS.show();
               setLights(TourLight,HIGH);
               setLights(SportLight,HIGH);
                       delay(2500);
           }else{
                      blinkBothLights(3,300,3,1000);
                      setLED(LED1,D_RED);
                      LEDS.show();
           }

           // See if abort desired
           if(readDiagSelectorSwitch()!= NOCHANGE) return; 
           
                   delay(2000);

                   
                   //////////////////////////////////////////////////////////////////////////////////////
                   //                    TEST FRONT Right STRUT
                   ///////////////////////////////////////////////////////////////////////////////////////
           // Turn off Lights
               setLights(SportLight,LOW);
                   setLights(TourLight,LOW);
           
                   //SET LED 2 to White to indicate testing Front Right Strut 
                   setLED(LED2,D_WHITE);
                   LEDS.show();
             
           // Blink Sport Light 2 time as group 3 times To indicate FR strut  
                   blinkLights(SportLight, 2,300,4,1000);  

           // See if abort desired
                   if(readDiagSelectorSwitch()!= NOCHANGE) return; 

                   // Do the test for this strut as many times as desired
               for(int i =1; i<= 1; i++){

                      //Attempt to set  Front Right Strut HARD MODE
                      setStrutMode( DIAG, HARD, strutFRmotor, strutFRswitch1, strutFRswitch2,LED2, &errorFR);
                      delay(1000);
                   
              // See if abort desired
              if(readDiagSelectorSwitch()!= NOCHANGE) return; 
              

                      //Attempt to set  Front Right Strut Medium MODE
                      setStrutMode( DIAG, MEDIUM, strutFRmotor, strutFRswitch1, strutFRswitch2,LED2, &errorFR);
                      delay(1000);
              
              // See if abort desired
              if(readDiagSelectorSwitch()!= NOCHANGE) return; 

                                      
                      //Attempt to set  Front Left Strut Soft MODE
                      setStrutMode( DIAG, SOFT, strutFRmotor, strutFRswitch1, strutFRswitch2,LED2, &errorFR);
                      delay(1000);

              // See if abort desired
              if(readDiagSelectorSwitch()!= NOCHANGE) return; 
              
              // Turn off Lights
              setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
           }

           // Display Final test results for this Strut
           if (errorFR == 0){
                       setLED(LED2,D_GREEN);
                       LEDS.show();
               setLights(TourLight,HIGH);
               setLights(SportLight,HIGH);
                       delay(2500);
           }else{
                      blinkBothLights(3,300,3,1000);
                      setLED(LED2,D_RED);
                      LEDS.show();
           }

           // See if abort desired
           if(readDiagSelectorSwitch()!= NOCHANGE) return; 
           
                   delay(2000);
  


                   //////////////////////////////////////////////////////////////////////////////////////
                   //                    TEST REAR LEFT STRUT
                   ///////////////////////////////////////////////////////////////////////////////////////
           // Turn off Lights
               setLights(SportLight,LOW);
                   setLights(TourLight,LOW);
           
                   //SET LED 3 to White to indicate testing Front Left Strut 
                   setLED(LED3,D_WHITE);
                   LEDS.show();
           
           // Blink Sport Light 3 time as group 3 times To indicate RL strut 
                   blinkLights(SportLight, 3,300,4,1000); 

           // See if abort desired
                   if(readDiagSelectorSwitch()!= NOCHANGE) return; 

                   // Do the test for this strut as many times as desired
               for(int i =1; i<= 1; i++){

                      //Attempt to set  Rear Left Strut HARD MODE
                      setStrutMode( DIAG, HARD, strutRLmotor, strutRLswitch1, strutRLswitch2,LED3, &errorRL);
                      delay(1000);
                   
              // See if abort desired
              if(readDiagSelectorSwitch()!= NOCHANGE) return; 
              
                      //Attempt to set  Rear Left Strut Medium MODE
                      setStrutMode( DIAG, MEDIUM, strutRLmotor, strutRLswitch1, strutRLswitch2,LED3, &errorRL);
                      delay(1000);
              
              // See if abort desired
              if(readDiagSelectorSwitch()!= NOCHANGE) return; 

                                      
                      //Attempt to set  Rear Left Strut Soft MODE
                      setStrutMode( DIAG, SOFT, strutRLmotor, strutRLswitch1, strutRLswitch2,LED3, &errorRL);
                      delay(1000);

              // See if abort desired
              if(readDiagSelectorSwitch()!= NOCHANGE) return; 
              

              // Turn off Lights
              setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
           }

           // Display Final test results for this Strut
           if (errorRL == 0){
                       setLED(LED3,D_GREEN);
                       LEDS.show();
               setLights(TourLight,HIGH);
               setLights(SportLight,HIGH);
                       delay(2500);
           }else{
                      blinkBothLights(3,300,3,1000);
                      setLED(LED3,D_RED);
                      LEDS.show();

           }

           // See if abort desired
           if(readDiagSelectorSwitch()!= NOCHANGE) return; 
           
                   delay(2000);
  

                   //////////////////////////////////////////////////////////////////////////////////////
                   //                    TEST REAR RIGHT STRUT
                   ///////////////////////////////////////////////////////////////////////////////////////
           // Turn off Lights
               setLights(SportLight,LOW);
                   setLights(TourLight,LOW);
           
                   //SET LED 1 to White to indicate testing Rear Right Strut 
                   setLED(LED4,D_WHITE);
                   LEDS.show();
            
           // Blink Sport Light 4 time as group 3 times To indicate RR strut 
                   blinkLights(SportLight, 4,300,4,1000);  

           // See if abort desired
                   if(readDiagSelectorSwitch()!= NOCHANGE) return; 

                   // Do the test for this strut as many times as desired
               for(int i =1; i<= 1; i++){

                      //Attempt to set  Rear Right Strut HARD MODE
                      setStrutMode( DIAG, HARD, strutRRmotor, strutRRswitch1, strutRRswitch2,LED4, &errorRR);        
                      delay(1000);
                   
              // See if abort desired
              if(readDiagSelectorSwitch()!= NOCHANGE) return; 
              

                      //Attempt to set  Rear Right Strut Medium MODE
                      setStrutMode( DIAG, MEDIUM, strutRRmotor, strutRRswitch1, strutRRswitch2,LED4, &errorRR);        
                      delay(1000);
              
              // See if abort desired
              if(readDiagSelectorSwitch()!= NOCHANGE) return; 

                                      
                      //Attempt to set  Rear Right Strut Soft MODE
                      setStrutMode( DIAG, SOFT, strutRRmotor, strutRRswitch1, strutRRswitch2,LED4, &errorRR);        
                      delay(1000);

              // See if abort desired
              if(readDiagSelectorSwitch()!= NOCHANGE) return; 
              

              // Turn off Lights
              setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
           }

           // Display Final test results for this Strut
           if (errorRR == 0){
                       setLED(LED4,D_GREEN);
                       LEDS.show();
               setLights(TourLight,HIGH);
               setLights(SportLight,HIGH);
                       delay(2500);
           }else{
                      blinkBothLights(3,300,3,1000);
                      setLED(LED4,D_RED);
                      LEDS.show();
           }

           
           
                   delay(2000);
          }
          return; 

}
//*******************************************************************************************
//
//Read Diagnostic routine
//This routine displays the current error status of all the struts based on current statuses incurred
//this is mainly for Cars that have Sports Tour Lights Only
//
//*******************************************************************************************
void readDiagnostics(){
                   
                   //  Possible Error Codes for errorFL, errorFR, errorRL, errorRR  =
                   //    
                   //         SIGNAL_WIRE_FAILURE   
                   //         MOTOR_COMMAND_FAILURE 
                   //         NO_FAILURES 
                   //

         while( readDiagSelectorSwitch()== NOCHANGE){  


                   // Turn Off All LEDS
                   offLED(LED1);
                   offLED(LED2);
                   offLED(LED3);
                   offLED(LED4);
                   
                   // Turn off Lights
               setLights(SportLight,LOW);
                   setLights(TourLight,LOW);
                   delay(1000);
             
                   // See if abort desired
                   if(readDiagSelectorSwitch()!= NOCHANGE) return; 

  
                   //////////////////////////////////////////////////////////////////////////////////////
                   //                    REPORT FRONT LEFT STRUT
                   ///////////////////////////////////////////////////////////////////////////////////////
           
           
           // Blink Sport Light 1 time as group 4 times To indicate FL strut
                   blinkLights(SportLight, 1,300,4,1000);  

           // See if abort desired
                   if(readDiagSelectorSwitch()!= NOCHANGE) return; 


           // Display Final test results for this Strut
           if (errorFL == NO_FAILURES){
                       setLED(LED1,D_GREEN);
                       LEDS.show();
               setLights(TourLight,HIGH);
               setLights(SportLight,HIGH);
                       delay(2500);
           }else if (errorFL == MOTOR_COMMAND_FAILURE){
                      blinkBothLights(3,300,3,1000);
                      setLED(LED1,D_RED);
                      LEDS.show();

           }else if (errorFL == SIGNAL_WIRE_FAILURE){ 
                  setLED(LED1,D_YELLOW);
                      LEDS.show();
                   for(int i=0;i<8;i++){
                     setLights(SportLight,HIGH);
                         setLights(TourLight,LOW);
             delay(250);
                     setLights(SportLight,LOW);
                         setLights(TourLight,HIGH);
             delay(250);
              }
              // Turn off Lights
                  setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
           }

           // See if abort desired
           if(readDiagSelectorSwitch()!= NOCHANGE) return; 
           
                   delay(2000);

                   
                   //////////////////////////////////////////////////////////////////////////////////////
                   //                    REPORT FRONT Right STRUT
                   ///////////////////////////////////////////////////////////////////////////////////////
           // Turn off Lights
               setLights(SportLight,LOW);
                   setLights(TourLight,LOW);
           
                   //SET LED 2 to White to indicate testing Front Right Strut 
                   setLED(LED2,D_WHITE);
                   LEDS.show();
           
           // Blink Sport Light 2 time as group 3 times To indicate FR strut  
                   blinkLights(SportLight, 2,300,4,1000);  

           // See if abort desired
                   if(readDiagSelectorSwitch()!= NOCHANGE) return; 


           // Display Final test results for this Strut
           if (errorFR == NO_FAILURES){
                       setLED(LED2,D_GREEN);
                       LEDS.show();
               setLights(TourLight,HIGH);
               setLights(SportLight,HIGH);
                       delay(2500);
           }else if (errorFR == MOTOR_COMMAND_FAILURE){
                      blinkBothLights(3,300,3,1000);
                      setLED(LED2,D_RED);
                      LEDS.show();
                      delay(2500);

           }else if (errorFR == SIGNAL_WIRE_FAILURE){ 
                    setLED(LED2,D_YELLOW);
                      LEDS.show();
              for(int i=0;i<8;i++){
                     setLights(SportLight,HIGH);
                         setLights(TourLight,LOW);
             delay(250);
                     setLights(SportLight,LOW);
                         setLights(TourLight,HIGH);
             delay(250);
              }
              // Turn off Lights
                  setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
           }

           // See if abort desired
           if(readDiagSelectorSwitch()!= NOCHANGE) return; 
           
                   delay(2000);
  



                   //////////////////////////////////////////////////////////////////////////////////////
                   //                    REPORT REAR LEFT STRUT
                   ///////////////////////////////////////////////////////////////////////////////////////
           // Turn off Lights
               setLights(SportLight,LOW);
                   setLights(TourLight,LOW);
           
                   //SET LED 3 to White to indicate testing Front Left Strut 
                   setLED(LED3,D_WHITE);
                   LEDS.show();
           
           // Blink Sport Light 3 time as group 3 times To indicate RL strut 
                   blinkLights(SportLight, 3,300,3,1000); 

           // See if abort desired
                   if(readDiagSelectorSwitch()!= NOCHANGE) return; 


           // Display Final test results for this Strut
           if (errorRL == NO_FAILURES){
                       setLED(LED3,D_GREEN);
                       LEDS.show();
               setLights(TourLight,HIGH);
               setLights(SportLight,HIGH);
                       delay(2500);
           }else if (errorRL == MOTOR_COMMAND_FAILURE){
                      blinkBothLights(3,300,3,1000);
                      setLED(LED3,D_RED);
                      LEDS.show();
                      delay(2500);

           }else if (errorRL == SIGNAL_WIRE_FAILURE){
                   setLED(LED3,D_YELLOW); 
                      LEDS.show();
               for(int i=0;i<8;i++){
                     setLights(SportLight,HIGH);
                         setLights(TourLight,LOW);
             delay(250);
                     setLights(SportLight,LOW);
                         setLights(TourLight,HIGH);
             delay(250);
              }
              // Turn off Lights
                  setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
           }



           // See if abort desired
           if(readDiagSelectorSwitch()!= NOCHANGE) return; 
           
                   delay(2000);
  

                   //////////////////////////////////////////////////////////////////////////////////////
                   //                    REPORT REAR RIGHT STRUT
                   ///////////////////////////////////////////////////////////////////////////////////////
           // Turn off Lights
               setLights(SportLight,LOW);
                   setLights(TourLight,LOW);
           
                   //SET LED 1 to White to indicate testing Rear Right Strut 
                   setLED(LED4,D_WHITE);
                   LEDS.show();
           
           // Blink Sport Light 4 time as group 3 times To indicate RR strut 
                   blinkLights(SportLight, 4,300,3,1000);  

           // See if abort desired
                   if(readDiagSelectorSwitch()!= NOCHANGE) return; 

           // Display Final test results for this Strut
           if (errorRR == NO_FAILURES){
                       setLED(LED4,D_GREEN);
                       LEDS.show();
               setLights(TourLight,HIGH);
               setLights(SportLight,HIGH);
                       delay(2500);
           }else if (errorRR == MOTOR_COMMAND_FAILURE){
                      blinkBothLights(3,300,3,1000);
                      setLED(LED4,D_RED);
                      LEDS.show();
                      delay(2500);

           }else if (errorRR == SIGNAL_WIRE_FAILURE){
                   setLED(LED4,D_YELLOW); 
                      LEDS.show();
              for(int i=0;i<8;i++){
                     setLights(SportLight,HIGH);
                         setLights(TourLight,LOW);
             delay(250);
                     setLights(SportLight,LOW);
                         setLights(TourLight,HIGH);
             delay(250);
              }
              // Turn off Lights
                  setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
           }
            
                   delay(2000);
          }
          return;
}

//**************************************************************************
// this routine converts three seperate RGB values in to a correctly encoded RGB integer
//**************************************************************************
long rgbEncode(int red,int green,int blue){
 long rgb;

 //if (red > 255) red=255;
 //if (green > 255) green=255;
 //if (blue > 255) blue=255;

 rgb = red*65536 + green*256 + blue;

 return (rgb);
 
} 



/****************************************************************************************
 *
 *
 * Routine to read int from EProm
 *
 *
 ****************************************************************************************/
int eepromReadInt(int address){
   int value = 0x0000;
   value = value | (EEPROM.read(address) << 8);
   value = value | EEPROM.read(address+1);
   return value;
}

/****************************************************************************************
 *
 *
 * Routine to Write int to EProm
 *
 *
 ****************************************************************************************/
void eepromWriteInt(int address, int value){
   EEPROM.write(address, (value >> 8) & 0xFF );
   EEPROM.write(address+1, value & 0xFF);
}

//****************************************************
//
// Convert Desired HEX color into  rgb values and store in LED array
//
//****************************************************

void setLedColors(int LED,long hexValue) {

  ledColors[LED].r = ((hexValue >> 16) & 0xFF) ;  // Extract the RR byte
  ledColors[LED].g = ((hexValue >> 8) & 0xFF) ;   // Extract the GG byte
  ledColors[LED].b = ((hexValue) & 0xFF) ;        // Extract the BB byte

}

TechWorks ECS Model 03 Firmware Source Code

support@renegadetechworks.com (TechWorks Admin) — Fri, 22 Aug 2014 19:06:53 +0000

//****************************************************************************
//
//     Description of Software:    
//
//     Mitsubishi 3000gt/Stealth Controller Program
//     for controlling stock OEM mitsubishi ECS Struts.
//     Mitsubish 3000gt model years 1991 - 1999.
//     Note:models years 1995-1999 are retrofit of this system+Original Struts.
//     Hardware Platform:   ATAMega 328 MicroController
//
//     Copyright (C) 2010,2011,2012,2013,2014,2015,2016
//     Marcus Diaz, RenegadeTechWorks, LLC
//
//****************************************************************************
// Licensing:
//              Licensed under GNU GPL Version 2
//
//    This program is free software; you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation under Version 2 of the License
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License along
//    with this program; if not, write to the Free Software Foundation, Inc.,
//    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//****************************************************************************
// VERSION HISTORY
//
//--------------------------------------------------------------------
// SW Ver  :  v3.00	
// HWDesign:  ECM M03 PCB Rev 
// Date:      03/08/2016	
// Comments:  
//            This code represents a branch and re-write for the Model 03 of 
//            of theTechWorks ECS controller.
//            The new Gen3/ Model03(M03) Controller uses a modified MO1 design
//            New Features are
//                -- freed  up I/O ports to support I2C buss devices
//                -- includes add on daughther board support for an DLH303 6-axis Accellerometer
//                -- Code has been rewritten in more Ojbect Oriented style.
//                -- Auto Mode added to use accelerometer data to auto set struts
//                -- Manual Mode remains
//               
//
//--------------------------------------------------------------------
// SW Ver  :  v3.01	
// HWDesign:  ECM M03 PCB Rev 
// Date:      07/05/2016	
// Comments:  
//            - Added to startup sequence to retrieve from eprom the 
//              default startup mode
//            - Added Sport/Tour light blinking patterns to startup sequence that
//              shows what default startup  mode is 
//             - Added Set Led Brightness Mode
//            - Added Save current mode as default startup mode
//            - Modes are:
//              1 push - set next stiffnes setting
//              2 pushes - toggle auto/manual modes
//              3 pushes - set led brightness
//              4 pushes - save curreent mode as default startup mode
//              5 pushes - read diagnostics mode
//              6 pushes - run diagnostics mode
//--------------------------------------------------------------------
// SW Ver  :  v3.01	
// HWDesign:  ECM M03 PCB Rev 3.1
// Date:      07/09/2016	
// Comments:  
//            - fixed logic in  sportTourLoopLights routine to correctly display 
//              command mode & signal wire failures
//            - changed manual strut mode to do struts one at a time
//
///--------------------------------------------------------------------
// SW Ver  :  v3.01	
// HWDesign:  ECM M03 PCB Rev 3.1
// Date:      07/16/2016	
// Comments:  
//            - changed manual strut mode back to parrallel mode 
//            - change H/W design to use 1.5 amp 9v VReg for old strut
///--------------------------------------------------------------------
// SW Ver  :  v3.2
// HWDesign:  ECM M03 PCB Rev 3.1
// Date:      07/25/2016	
// Comments:  
//            - added modes to set threshold values
//            - added modes to Calibrate accelerometer
//            - added code to use accelerometer calibration values
//            - added code in setStrutMode to turn motors back on for old struts
///--------------------------------------------------------------------
// SW Ver  :  v3.3  Official Production Release Used in Shipping M03 Controllers
// HWDesign:  ECM M03 PCB Rev 3.1
// Date:      10/17/2016	
// Comments:  
//            - added logging Mode
//            - added firmware version & startup values to print at startup
//            - added seperate maxtries params for Auto vs Manual strut modes
//            - added LED = OFF when strut state cannot be determined during transition
//            - added LED = GREENYELLOW if strut goes into mode but cannot stay there
//            - reworked the logic & assignments in readStrutState() - to match reality
//            - changed sport tour lights for automode to show max target across the four struts
///***********************************************************************************
#include 
#include 
#include 
#include 

//
//********* Firmware Version  ****************
//
#define FIRMWARE_VERSION "Firmware Version M03 v3.3"


//
//********* LSM303DLHC Accelerometer/ Compass I2C Addresses  ****************
//
#define LSM303_ADDRESS_ACC            (0x32 >> 1)         // 0011001x
#define LSM303_ADDRESS_MAG            (0x3C >> 1)         // 0011110x

#define MANUAL         1
#define AUTO 	       2
#define DIAG	       3
#define READDIAG       4
#define SETSTARTUP     5
#define SETLED         6
#define SETTHRESHOLD   7
#define CALIBRATEACCEL 8

#define NOMINAL     0
#define CHANGING    1
#define FAILED 	    2
#define OLDSTRUTS   3
#define PRENOMINAL  4

#define STRUT_FL 0
#define STRUT_FR 1
#define STRUT_RL 2
#define STRUT_RR 3

#define HARD 	 1
#define MEDIUM 	 2
#define SOFT 	 3
#define UNKNOWN  7
#define NOCHANGE 10
#define STARTUP  11

#define D_RED	       1
#define D_GREEN	       2
#define D_BLUE	       3
#define D_TURQUOIS     4
#define D_PURPLE       5
#define D_YELLOW       6
#define D_GREENYELLOW  7
#define D_STARTUP      8
#define D_HARD         9
#define D_MEDIUM       10
#define D_SOFT         11
#define D_WHITE        12
#define D_OFF          13 
#define D_UNKNOWN      14  


#define RED100   0xFF0000 // Bright Red
#define GREEN100 0x00FF00 // Bright Green
#define BLUE100  0x0000FF // Bright Blue
#define RED001   0x010000 // Faint red
#define RED050   0x800000 // 1/2 red (0x80 = 128 out of 256)
#define RED025   0x300000 // 25% RED
#define GREEN050 0x008000 // half Green
#define GREEN025 0x003000 // 25% GREEN
#define BLUE050  0x000080 // half Blue
#define BLUE025  0x000030 // 25% BLUE
#define WHITE015 0x151515 // 15% White
#define WHITE010 0x101010 // 10% White
#define WHITE005 0x050505 // 05% White
#define WHITE001 0x020202 // 05% White
#define WHITE100 0xFFFFFF // 100% White

#define LEDOFF	       0x000000	
#define RED            0xFF0000 
#define ORANGE         0xFF5500
#define ORANGEYELLOW   0xFFA000
#define YELLOW         0xFFFF00
#define YELLOWGREEN    0xA0FF00
#define GREENYELLOW    0x50FF00
#define GREEN          0x00FF00
#define GREENCYAN      0x00FF4B
#define CYAN           0x00FFFF
#define CYANBLUE       0x00A0FF
#define BLUECYAN       0x005AFF  
#define BLUE           0x0000FF
#define BLUEVIOLET     0x4800FF
#define VIOLETBLUE     0x7500FF
#define MAGENTA        0xFF00FF
#define PINKDEEP       0xFF1493
#define PINKHOT        0xFF69B4
#define PINK           0xF3967A
 
#define VIOLET         0xEE82EE


#define ERROR 1
#define NOERROR 0

#define CLEAR 1
#define DONT_CLEAR 0

#define SIGNAL_WIRE_FAILURE   2
#define MOTOR_COMMAND_FAILURE 1
#define NO_FAILURES 0

#define AUTO_MAXTRIES        475     // maxtries = 475 = approximately  2.75 secs of Motor on
                                     // AUTO_MAXTRIES time equivalent should be less than STRUT_HOLD_INTERVAL
#define MANUAL_MAXTRIES      10000   // maxtries = 10000 = approximately  4 secs of Motor on
#define MAXFAILURES   100    // maxiumum number of faliures allowed for a strut during a power on cycle


#define MAXLIGHTLOOPS 20

#define ROTARY 1
#define PUSHBUTTON 2
#define SAMEMODE 1
#define NEXTMODE 2

#define LED_LEVEL1 1       // minimum brightness level
#define LED_LEVEL2 2
#define LED_LEVEL3 3
#define LED_LEVEL4 4       // Maximum brightness level

// ******************************************************************
//
//                   EPROM ADDRESSES
//
// ******************************************************************
#define EPROM_LEDMODE           1   // address in EPROM for default start up LED Brightness Level
#define EPROM_CONTROLLER_MODE   3   // address in EPROM for default start up Controller Mode (Auto or Manual)
#define EPROM_STRUT_MODE        5   // address in EPROM for default start up Strut Stiffness Mode
#define EPROM_MEDIUM_THRESHOLD  7   // address in EPROM for default start up Strut Stiffness Mode
#define EPROM_HARD_THRESHOLD    9   // address in EPROM for default start up Strut Stiffness Mode
#define EPROM_ACC_X_NEG_1G      11  // address in EPROM for Accelerometer Negative X axis 1G Calibration Value 
#define EPROM_ACC_Y_NEG_1G      13  // address in EPROM for Accelerometer Negative Y axis 1G Calibration Value  
#define EPROM_ACC_Z_NEG_1G      15  // address in EPROM for Accelerometer Negative Z axis 1G Calibration Value  
#define EPROM_ACC_X_POS_1G      17  // address in EPROM for Accelerometer Positive X axis 1G Calibration Value  
#define EPROM_ACC_Y_POS_1G      19  // address in EPROM for Accelerometer Positive Y axis 1G Calibration Value  
#define EPROM_ACC_Z_POS_1G      21  // address in EPROM for Accelerometer Positive Z axis 1G Calibration Value  



///////////////////////////////////////////////////////////////////////////////////////
//
//  Map Board I/O Pins to World
//
// /////////////////////////////////////////////////////////////////////////////////////
// Front Left Strut Pins
#define STRUT_FL_SWITCH1  3 
#define STRUT_FL_SWITCH2  2
#define STRUT_FL_MOTOR    10

// Front Right Strut Pins
#define STRUT_FR_SWITCH1  5 
#define STRUT_FR_SWITCH2  4
#define STRUT_FR_MOTOR    11

// Rear Left Strut Pins
#define STRUT_RL_SWITCH1  7 
#define STRUT_RL_SWITCH2  6
#define STRUT_RL_MOTOR    14

// Rear Right Strut Pins
#define STRUT_RR_SWITCH1  9 
#define STRUT_RR_SWITCH2  8
#define STRUT_RR_MOTOR    15

//
// LED Light Ports 
//
#define DATA_PIN 16    // Data pin that led data will be written out over

//
// Switch State 
//
#define NO_PUSH_TO_PROCESS   0
#define NEW_PUSH_OCCURED     1
#define CONTINUE_PROCESSING  2



#define ACC_DATA_POINTS      10   // maximum number of Acceleration DataPoints
#define STRUT_HOLD_INTERVAL  3000 // numbers of milliseconds to hold a strut higher state;

#define X_AXIS 0   // Acceleromter X Axis in Array
#define Y_AXIS 1   // Acceleromter Y Axis in Array 
#define Z_AXIS 2   // Acceleromter Z Axis in Array 

//
// Logging modes
//
#define LOG_TIME              1
#define LOG_STRUTS_NAME       2
#define LOG_ACCELEROMETER     3

//***************************************************************************************
//
//   Create and Intialize (some ) Global Variables
//
//***************************************************************************************

// 
// Map Physical Accelerometer axis & sign to normalized variables
//

int ForwardAxis        = Y_AXIS;      // default to Y axis
int ForwardSign        = 1;      // default to Y+ +Acc = Acceleration

int LeftRightAxis      = Z_AXIS;      // default to Z axis
int LeftRightSign      = 1;      // default to Z+ +Acc = Left Turn

int UpDownAxis         = X_AXIS;      // default to X axis
int UpDownSign         = 1;      // default to X+ +Acc = Up 
int accelDataPoints    = 10;     // default number of acceleration data points to average

float ForwardAccelerationArray[ACC_DATA_POINTS] ;
float LeftRightAccelerationArray[ACC_DATA_POINTS] ;
float UpDownAccelerationArray[ACC_DATA_POINTS] ;

float ForwardAcceleration;
float LeftRightAcceleration;
float UpDownAcceleration;

//
// X Y Z Axis Calibration Variables
//
int XAxisNegative1gCalibration  = 0;
int YAxisNegative1gCalibration  = 0;
int ZAxisNegative1gCalibration  = 0;
int XAxisPositive1gCalibration  = 0;
int YAxisPositive1gCalibration  = 0;
int ZAxisPositive1gCalibration  = 0;

//
// variables to contain integer level value that corresponds to actual decimal value of threshold
// 1 = 0.1g   2= 0.2g    3= 0.3g    4= 0.4g     5 =0.5g    6= 0.6g   7=0.7g
int mediumThresholdLevel; 
int hardThresholdLevel; 
//
// Default Threshold variables for Auto Mode Strut transitions in G's of force
//
float mediumThreshold = 0.2;
float hardThreshold = 0.4;

//Mode Selector Switch Pin
#define SELECTOR A3        // A3=Analog pin A3=physical pin 26 on AT328)  is part of the PinChangeInt.h library defines
int selectorSwitchPin = 17;

//Switch Type Selector Option Pin
int switchTypeSelect = 1;

//LED Type Selector Option Pin
int LEDTypeSelect = 0;

// Sport Light Port
int SportLight = 12; 

// Tour Light Port
int TourLight = 13;

//Print Mode  1=log/print 0=no logging
int logMode = 0;
int logSequence = 0;
 
long ledHardValue ;
long ledMediumValue;
long ledSoftValue;
long ledRedValue;

#define FL_LED  0    // Front Left Strut LED
#define FR_LED  1    // Front Right Strut LED
#define RL_LED  2    // Rear Left Strut LED
#define RR_LED  3    // Rear Right Strut LED 

int lightLoopCounter = MAXLIGHTLOOPS;
int lightLoopCounterValue;

int accelerometerStrutTargets[4];

int previousMode;
//
//  Allocate Structure For LED Strip
//
#define NUM_LEDS 8
struct CRGB ledColors[NUM_LEDS];
long ledCurrentValue[4];


unsigned long lightCycleStartTime =0;
unsigned long signalWireFailureCycleStartTime;
unsigned long commandFailureCycleStartTime;

// **********************************
//  Selector Switch variables & Intterupt function
// **********************************

/*
// Notice that anything that gets modified inside an interrupt, that I wish to access
// outside the interrupt, is marked "volatile". That tells the compiler not to optimize
// them.
*/
volatile uint8_t pushCount=0;
volatile uint8_t pushEvent= NO_PUSH_TO_PROCESS;
volatile unsigned long pushTime=0;

int firstTime=1;

//
// Interupt functgion for Selector Switch
//
void selectorInterruptfunc() {
  pushCount = pushCount +1;
  pushTime=millis();
  pushEvent = NEW_PUSH_OCCURED;
}
  


// **********************************
//  Strut Object Class Definition
// **********************************
class Strut{
	public:
		Strut(){


		}

		~Strut(){}

		//************************************
                int strutPosition() { 
			return vstrutPosition; 
		}
                int setStrutPosition(int position) { 
			vstrutPosition = position; 
		}
		//************************************
                int desiredState() { 
			return vdesiredState; 
		}
                int setDesiredState(int target) { 
		     vdesiredState = target; 
		}	
		//************************************
                unsigned long lastChangeTime() { 
			return vlastChangeTime; 
		}
                void setLastChangeTime() { 
		     vlastChangeTime= millis(); 
		}	
		//************************************
                int actualState(int display) { 
			vstate = readStrutState(vstrutPosition,display);
			return vstate; 
		}
		//***********************************
                int transitionStatus() { 
			return vtransitionStatus; 
		}
                int setTransitionStatus(int status) { 
			vtransitionStatus = status; 
		}
		//************************************
                int switch1() { 
			return vs1Pin; 
		}

                int setSwitch1(int pin) { 
			vs1Pin = pin; 
		}
		//************************************
                int switch2() { 
			return vs2Pin; 
		}
                int setSwitch2(int pin) { 
			vs2Pin = pin; 
		}
		//************************************
                int motorPin() { 
			return vmotorPin; 
		}
                int setMotor(int pin) { 
			vmotorPin = pin; 
		}
                int motorOn() { 
        		digitalWrite(vmotorPin, HIGH);
		}
                int motorOff() { 
        		digitalWrite(vmotorPin, LOW);
		}
		//************************************
                int tryCount() { 
			return vtryCount; 
		}
                void clearTryCount() { 
			vtryCount =0; 
		}
                void incrementTryCount() { 
			vtryCount++; 
		}
		//************************************
                int failures() { 
			return vtotalFailures; 
		}
                void clearFailures() { 
		     vtotalFailures = 0; 
		}
                void incrementFailures() { 
			vtotalFailures++; 
		}

		//************************************
                int errorStatus() { 
			return verrorStatus; 
		}
                void setErrorStatus(int status) { 
			verrorStatus = status;
		}



	private:
		int vstrutPosition;     // Strut Postion:  STRUT_FL _FR _RL _RR                                   
		int vstate;             // Actual State of the Struts:  HARD, MEDIUM, SOFT
		int vdesiredState;     // Desired Target State based on last command loop execution: H, M, S
		
		int vtransitionStatus;  // Indicate if the Strut in transition from last command mode to a new desired target or
		                       // Or is stable and in quiet state
				       // Modes Are:
	                               // NOMINAL  =  Strut Actual Mode matches the DesiredCommand Target. 
				       //             Motors should be off
				       // CHANGING =  Strut In a commanded transitional state changing
				       //              from Medium to Hard for example. Motors Should be On
				       // Neutral  =  Everything has been shut down and Motors are off.  Command Target is not guaranteed
				       //             Strut is whatever state it is in.
				       
		int verrorStatus = NO_FAILURES; // Indicates current Error Status of Strut Possible Error Status modes are
                                              //   NO_FAILURES    Means no failure has previously Been detected Since Last Command given
		                              //   SIGNAL_WIRE_FAILURE     Means a signal wire failure has been previously detected 
                                              //   MOTOR_COMMAND_FAILURE   Means a Motor Command failure has been previously detected   
					      
		int vtryCount = 0;      // Indicates how many times previously a Strut has has passed thru the command loop
		                        // while trying to achive the desired targetState
				       
		int vtotalFailures = 0; // Indicates how many total times previously a Strut has exceeded maxtries/tryCount
		                        // while trying to achive the desired targetState

		int vs1Pin;
		int vs2Pin;
		int vmotorPin;
		unsigned long vlastChangeTime;
};

// *********************************************************
//  Controller Object  Class Definition
// *********************************************************
class Controller{
	public:
		Controller(){


		}

		~Controller(){}

		int readSwitch(){
			return(readControllerSwitch());
		}
		
                int mode() { 
			return vcontrollerMode; 
		}
                int setMode(int mode) { 
			vcontrollerMode = mode; 
		}
                int strutMode() { 
			return vcontrollerStrutMode; 
		}
                int setControllerStrutMode(int mode) { 
			vcontrollerStrutMode = mode; 
		}

                int selectorType() { 
			return(vselectorType);
		}
                int setSelectorType(int type) { 
		     vselectorType = type;
		}

		//
                // Maybe put read switch as function of Controller??
		//
	private:
		int vcontrollerMode;           // Auto, Manual, Diag
		int vcontrollerStrutMode;      // Hard, Med, Soft
                int vselectorType = ROTARY;    // selectorType captures what kind of selector switch is hooked up to unit
};



//
// ** instantiate & Initialize Strut Array Object **
//
Strut myStruts[4];


//
// ** instantiate & Initialize Controller Array Object **
//
Controller myController;


int ledMode = LED_LEVEL2;


// Previous State of Selector Switch
int currentSelectorState = STARTUP;




 
///////////////////////////////////////////////////////////////////////////////////////
// setup initializes startup 
///////////////////////////////////////////////////////////////////////////////////////
void setup(){

        int strutStateFL; 
        int strutStateFR; 
        int strutStateRL; 
        int strutStateRR; 

        int selectorValue;
        int diagLoopNumber;
        int epromValue;

 
        // Initialize the Sport & Tour Light OutPut Pins
        pinMode(SportLight, OUTPUT); 
        pinMode(TourLight, OUTPUT);
 
	//
        // intialize all the motor & Switch state pins
	//
        myStruts[0].setSwitch1(STRUT_FL_SWITCH1);
        myStruts[0].setSwitch2(STRUT_FL_SWITCH2);
        myStruts[0].setMotor(STRUT_FL_MOTOR);
        myStruts[0].setStrutPosition(STRUT_FL);
        myStruts[1].setSwitch1(STRUT_FR_SWITCH1);
        myStruts[1].setSwitch2(STRUT_FR_SWITCH2);
        myStruts[1].setMotor(STRUT_FR_MOTOR);
        myStruts[1].setStrutPosition(STRUT_FR);
        myStruts[2].setSwitch1(STRUT_RL_SWITCH1);
        myStruts[2].setSwitch2(STRUT_RL_SWITCH2);
        myStruts[2].setMotor(STRUT_RL_MOTOR);
        myStruts[2].setStrutPosition(STRUT_RL);
        myStruts[3].setSwitch1(STRUT_RR_SWITCH1);
        myStruts[3].setSwitch2(STRUT_RR_SWITCH2);
        myStruts[3].setMotor(STRUT_RR_MOTOR);
        myStruts[3].setStrutPosition(STRUT_RR);

        for (int i=0; i < 4; i++){
            initializeMotorIOPin(myStruts[i].motorPin());
 	    initializeStrutIOPin(myStruts[i].switch1());
 	    initializeStrutIOPin(myStruts[i].switch2());
	}

	//
	// initialize current led value array
	//
        for (int i=0; i < 4; i++){
            ledCurrentValue[i] = 0x00;
	}
     
        //
        //
        // Initialize Accelerometer 
        //
        Wire.begin();  // Start up I2C, required for LSM303 communication
        initAccelerometer();


        // ************************************************************
        //
        // LED Setup & Initialization
        //
        // ************************************************************
	

        FastLED.addLeds(ledColors, NUM_LEDS);

        // Clear Out the LED arrays
        memset(ledColors, 0,  NUM_LEDS * sizeof(struct CRGB));   
        LEDS.show(); //Push the current color frame to the LEDs

	//
	// initialize current led value array
	//
        for (int i=0; i < 4; i++){
            ledCurrentValue[i] = 0x00;
	}

	// analyze Selector TYPE Input Pin To figure out if Rotary of Pushbutton is connected 
        // HIGH = Rotary   LOW = PUSHBUTTON
        pinMode(switchTypeSelect, INPUT);
        digitalWrite(switchTypeSelect,HIGH); 

        // Read the pin to see if it's HIGH or LOW      
        if(digitalRead(switchTypeSelect) == HIGH){
             myController.setSelectorType(PUSHBUTTON);

	     /*  original non-interrupt switch config
             pinMode(selectorSwitchPin, INPUT);
	     */
	     
             //
             // new code to use interrupt for pushbutton switch
             //
             pinMode(SELECTOR, INPUT_PULLUP);
             attachPinChangeInterrupt(SELECTOR, selectorInterruptfunc , RISING);     // Rising state change will trigger the interrupt.
             // attachPinChangeInterrupt(SELECTOR, selectorInterruptfunc , CHANGE);  // Any state change will trigger the interrupt.
             // attachPinChangeInterrupt(SELECTOR, selectorInterruptfunc , FALLING); // Falling state change will trigger the interrupt.
/*
             digitalWrite(selectorSwitchPin, HIGH);
*/

             lightLoopCounterValue = 900; // the lightloopcounter is timing dependent based on selector type
             diagLoopNumber = 450;
        }else{
             myController.setSelectorType(ROTARY);
             pinMode(selectorSwitchPin, INPUT);
             digitalWrite(selectorSwitchPin, LOW);
             lightLoopCounterValue = 60;  // the lightloopcounter is timing dependent based on selector type
             diagLoopNumber = 40;
        } 
        lightLoopCounter = lightLoopCounterValue;


	//
	// Retrieve LED Brigtness level from EPROM - if not within range set it to 2nd level and write it back
	//
        ledMode = eepromReadInt(EPROM_LEDMODE);	

	//
	// Make sure the value is with in range
	//
	if (ledMode != LED_LEVEL1 && ledMode != LED_LEVEL2 && ledMode != LED_LEVEL3 && ledMode != LED_LEVEL4){
	    // It's not within range - set to LEVEL two and write it back
	    ledMode = LED_LEVEL2;
            eepromWriteInt(EPROM_LEDMODE,ledMode);
	} 

	//
	// Set the LED brightness values based on EProm value
	//
	setLedValues();

	
        // EPROM_MEDIUM_THRESHOLD 
        // EPROM_HARD_THRESHOLD  
	
	//
	// Retrieve medium threshold level from EPROM - if not within range set it to 2nd level and write it back   
	//
        mediumThresholdLevel = eepromReadInt(EPROM_MEDIUM_THRESHOLD );	
	//
	// Make sure the value is with in range
	//
	if (mediumThresholdLevel < 1 ||   mediumThresholdLevel > 7 ){
	    // It's not within range - set to LEVEL two and write it back
	   mediumThresholdLevel = 2 ; 
            eepromWriteInt(EPROM_MEDIUM_THRESHOLD, mediumThresholdLevel );
	} 
	//
	// Set g threshold level based on EEProm
	// 
	mediumThreshold = float( mediumThresholdLevel/10.0)  ; 
 
	//
	// Retrieve hard threshold level from EPROM - if not within range set it to 2nd level and write it back     
	//
        hardThresholdLevel = eepromReadInt(EPROM_HARD_THRESHOLD );	
	//
	// Make sure the value is with in range
	//
	if (hardThresholdLevel < 1 ||   hardThresholdLevel > 9 ){
	    // It's not within range - set to LEVEL two and write it back
	   hardThresholdLevel = 4 ; 
           eepromWriteInt(EPROM_HARD_THRESHOLD, hardThresholdLevel );
	} 
	//
	// Set g threshold level based on EEProm
	// 
	hardThreshold = float( hardThresholdLevel/10.0)  ; 
  

	
	//
	// Retrieve Strut Mode from EPROM 
        epromValue= eepromReadInt(EPROM_STRUT_MODE);

        /*
        Serial.print(F("    Eprom Strut Mode ="));
        Serial.println(epromValue);
        */

	//
	// if not valid eprom value set it to Soft;	
	//
	//
	if (epromValue != HARD && epromValue != MEDIUM && epromValue != SOFT ){
	    // It's not within range - set to Soft as default
	    epromValue = SOFT;
            eepromWriteInt(EPROM_STRUT_MODE,epromValue);
	} 
	myController.setControllerStrutMode(epromValue);

	


       //
       // Retrieve X Axis Neg 1g Galibration Value From Eprom if not with in range set to default value and write back 
       //
       XAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_X_NEG_1G);	
       if (XAxisNegative1gCalibration >32000 || XAxisNegative1gCalibration < 1  ){
           // It's not within range - set to default   and write it back
	   XAxisNegative1gCalibration = 16380;
           eepromWriteInt(EPROM_ACC_X_NEG_1G , XAxisNegative1gCalibration);
       } 

       //
       // Retrieve Y Axis Neg 1g Galibration Value From Eprom if not with in range set to default value and write back 
       //
       YAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_Y_NEG_1G);	
       if (YAxisNegative1gCalibration >32000 || YAxisNegative1gCalibration < 1  ){
           // It's not within range - set to default   and write it back
	   YAxisNegative1gCalibration = 16380;
           eepromWriteInt(EPROM_ACC_Y_NEG_1G , YAxisNegative1gCalibration);
       } 

       //
       // Retrieve Z Axis Neg 1g Galibration Value From Eprom if not with in range set to default value and write back 
       //
       ZAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_Z_NEG_1G);	
       if (ZAxisNegative1gCalibration >32000 || ZAxisNegative1gCalibration < 1  ){
           // It's not within range - set to default   and write it back
	   ZAxisNegative1gCalibration = 16380;
           eepromWriteInt(EPROM_ACC_Z_NEG_1G , ZAxisNegative1gCalibration);
       } 

       //
       // Retrieve X Axis Positive 1g Galibration Value From Eprom if not with in range set to default value and write back 
       //
       XAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_X_POS_1G);	
       if (XAxisPositive1gCalibration > 32000 || XAxisPositive1gCalibration  < 1  ){
           // It's not within range - set to default   and write it back
	   XAxisPositive1gCalibration  = 16380;
           eepromWriteInt(EPROM_ACC_X_POS_1G , XAxisPositive1gCalibration  );
       } 

       //
       // Retrieve Y Axis Positive 1g Galibration Value From Eprom if not with in range set to default value and write back 
       //
       YAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_Y_POS_1G);	
       if (YAxisPositive1gCalibration > 32000 || YAxisPositive1gCalibration  < 1  ){
           // It's not within range - set to default   and write it back
	   YAxisPositive1gCalibration  = 16380;
           eepromWriteInt(EPROM_ACC_Y_POS_1G , YAxisPositive1gCalibration  );
       } 

       //
       // Retrieve Z Axis Positive 1g Galibration Value From Eprom if not with in range set to default value and write back 
       //
       ZAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_Z_POS_1G);	
       if (ZAxisPositive1gCalibration > 32000 || ZAxisPositive1gCalibration  < 1  ){
           // It's not within range - set to default   and write it back
	   ZAxisPositive1gCalibration  = 16380;
           eepromWriteInt(EPROM_ACC_Z_POS_1G , ZAxisPositive1gCalibration  );
       } 



	//
	// Turn on the Sport / Tour Lights at startup for 1 sec as test to driver to show they are working.
	//
        setLights(SportLight,HIGH);
        setLights(TourLight,HIGH);
        delay(1000);	
        // Turn off Lights
        setLights(SportLight,LOW);
        setLights(TourLight,LOW);
        delay(300);	

	
	//
	// Retrieve Controller Mode from EPROM 
	//
        epromValue= eepromReadInt(EPROM_CONTROLLER_MODE);
	/*
        Serial.print(F("Eprom Controller Mode ="));
        Serial.print(epromValue);
	*/
	//
	// if controller mode not Auto or Manual set to manual
	//
	if (epromValue != AUTO && epromValue != MANUAL ){
	    // It's not within range - set to Manual as default
	    epromValue = MANUAL;
            eepromWriteInt(EPROM_CONTROLLER_MODE,epromValue);
	} 
	myController.setMode(epromValue);

	//
	// Based on Controller default startup mode flash lights accordingly
	//
	if(epromValue == AUTO){

             //
             // Blink both lights alternating(3 times) to indicate auto mode
             //
 	     for(int i=0;i<2;i++){
	       setLights(SportLight,HIGH);
               setLED(STRUT_FR,D_WHITE);

               setLights(TourLight,LOW);
               setLED(STRUT_FL,D_OFF);
	       
	       delay(250);
	       setLights(SportLight,LOW);
               setLED(STRUT_FR,D_OFF);

               setLights(TourLight,HIGH);
               setLED(STRUT_FL,D_WHITE);
	       delay(250);
	     }
	}else if (epromValue == MANUAL){
			     //
	     // Blink both lights at together (3 times) to indicate manual mode
             //
             blinkBothLights(3,250,1,0,1);
			     
	     // Turn off Lights
	     setLights(SportLight,LOW);
             setLights(TourLight,LOW);

	}


        lightCycleStartTime =millis();
        pushCount=0;

	//
	// ********* Set Up for Logging & Print some Initial Values
	//
        Serial.begin(115200); 
        while (!Serial) {
            ; // wait for serial port to connect. Needed for Leonardo only
        };
	Serial.println(F("*************************************************************************"));
        Serial.println(F("Renegade Techworks Mitsubishi 3000gt/Stealth ECS Controller"));
        Serial.println(FIRMWARE_VERSION);
        Serial.print(F("Strut G-Force Medium Threshold:  "));
        Serial.println( mediumThreshold);
        Serial.print(F("Strut G-Force Hard Threshold:  "));
        Serial.println( hardThreshold );
	readPrintEpromValues();

}


/////////////////////////////////////////////////////////////////////////////////////
//
//            main loop  
//
/////////////////////////////////////////////////////////////////////////////////////

void loop()
{
	
   //
   // Check for updates to the state of the Controller Mode 
   //   
   //
   myController.readSwitch();


   if(logMode) doLogMode(LOG_TIME,0,0);

   switch(myController.mode()){

	case(MANUAL):
            if(logMode) doLogMode(LOG_ACCELEROMETER,0,0);
	    setManualStrutTargets();	
            setStruts();       
            // setManualStruts();        
	    break;

	case(AUTO):
            readAccelerometerValues();    
            setAccelerometerTargets();
	    setAutoStrutTargets();
            setStruts();       
	    break;

	case(SETLED):
            setLed();
	    break;

	case(SETSTARTUP):
            setStartup(); 
	    break;
	    
	case(DIAG):
            runDiagnostics();
	    break;

	case(READDIAG):
            readDiagnostics();
	    break;
	    
   	case(SETTHRESHOLD):
            setAutoThreshold();
	    break;

   	case(CALIBRATEACCEL):
            calibrateAccelerometer();
	    break;

   }

   //
   // display current state of Sport-Tour lights
   // 
   setLoopTourSportLights();
}

//*************************************************************************************
//
//
//    setAutoThreshold()
//
//
//************************************************************************************
void setAutoThreshold(){
                             
        // Turn off Lights while setting LEDs
        setLights(SportLight,LOW);
        setLights(TourLight,LOW);

	int autoThresholdSet = 0;

        mediumThresholdLevel = 2;
	hardThresholdLevel = 4;
	
        /*
	 * Stay in this loop until button is pressed multple times to indicate save
	 */	
	while(1){ 
		
		//read the selector PUSHBUTTON or Rotary and see if they have it in DIAGMODE
                switch(readPushButtonSwitch()){

	            case NOCHANGE:
	            break;

                    case 1:
		          //
		          // Ok they want to change to next Threshold Values 
		          //

		          if(autoThresholdSet == 5) autoThresholdSet =1;
		          else autoThresholdSet++;

		          //
		          // Blink both lights at together  x times to indicate which threshold set is selected
		          //
                          blinkBothLights(autoThresholdSet,500,1,0,1);
                          switch(autoThresholdSet){
				  case 1:
					  mediumThresholdLevel = 1;
					  hardThresholdLevel = 3;
					  break;

				  case 2:
					  mediumThresholdLevel = 2;
					  hardThresholdLevel = 4;
					  break;
					  
				  case 3:
					  mediumThresholdLevel = 3;
					  hardThresholdLevel = 5;
					  break;

				  case 4:
					  mediumThresholdLevel = 4;
					  hardThresholdLevel = 6;
					  break;
					  
				  case 5:
					  mediumThresholdLevel = 5;
					  hardThresholdLevel = 7;
					  break;
					  
			  }

                          break;

                    case 2:
                    case 3:
                    case 4:
		          //
		          // Ok they want to save the auto Threshhold values and exit
		          //
                          eepromWriteInt(EPROM_MEDIUM_THRESHOLD,mediumThresholdLevel);
                          eepromWriteInt(EPROM_HARD_THRESHOLD,hardThresholdLevel);

	                  mediumThreshold = float( mediumThresholdLevel/10.0)  ; 
	                  hardThreshold = float( hardThresholdLevel/10.0)  ; 
		  
		          //
		          // Blink both lights at together (7 times) to indicate saving Threshold mode
		          //
                          blinkBothLights(7,500,1,0,1);
                          delay(500);
	                  //
	                  // Restore the previous state of the controller
	                  //
		          myController.setMode(previousMode);
		          return;

                          break;
		}
	}
}

//*************************************************************************************
//
//
//    setLed()
//
//
//************************************************************************************
void setLed(){
                             
        // Turn off Lights while setting LEDs
        setLights(SportLight,LOW);
        setLights(TourLight,LOW);

	/*
	 *  Display LedValues
	 */
        setLedValues();

	
        /*
	 * Stay in this loop until button is pressed multple times to indicate save
	 */	
	while(1){ 
		
		//read the selector PUSHBUTTON or Rotary and see if they have it in DIAGMODE
                switch(readPushButtonSwitch()){

	            case NOCHANGE:
	            break;

                    case 1:
		          //
		          // Ok they want to change to next LED Value 
		          //

		          if(ledMode == 4) ledMode =1;
		          else ledMode++;
		 
		          setLedValues();
                          break;

                    case 2:
                    case 3:
                    case 4:
		          //
		          // Ok they want to save the current LED Brightness value and exit
		          //
                          eepromWriteInt(EPROM_LEDMODE,ledMode);
		  
		          //
		          // Blink both lights at together (3 times) to indicate LED mode
		          //
                          blinkBothLights(3,500,1,0,1);

	                  //
	                  // Restore the previous state of the controller
	                  //
		          myController.setMode(previousMode);
		          return;

                          break;
		}
	}
}


//*************************************************************************************
//
//
//   setLedValues
//
//
//************************************************************************************
void setLedValues(){
	
           switch(ledMode){
                      case LED_LEVEL1:
	                       //SET the LEDs to Minimum Brightness Mode	
                         ledHardValue = rgbEncode(2,0,2);
                         ledMediumValue = rgbEncode(0,2,2);
                         ledSoftValue = rgbEncode(0,0,2);
                         ledRedValue = rgbEncode(2,0,0);
  
                         setLED(FL_LED, D_HARD);
                         setLED(FR_LED, D_MEDIUM);
                         setLED(RL_LED, D_SOFT);
                         setLED(RR_LED, D_RED);
                      break;

                      case LED_LEVEL2:
	                       //SET the LEDs to 2nd to Minimum Brightness Mode	
                         ledHardValue = rgbEncode(10,0,10);
                         ledMediumValue = rgbEncode(0,10,5);
                         ledSoftValue = rgbEncode(0,0,10);
                         ledRedValue = rgbEncode(10,0,0);
  
                         setLED(FL_LED, D_HARD);
                         setLED(FR_LED, D_MEDIUM);
                         setLED(RL_LED, D_SOFT);
                         setLED(RR_LED, D_RED);
                      break;

                      case LED_LEVEL3:
	                       //SET the LEDs to Medium Brightness Mode	
                         ledHardValue = rgbEncode(70,0,70);
                         ledMediumValue = rgbEncode(0,70,35);
                         ledSoftValue = rgbEncode(0,0,70);
                         ledRedValue = rgbEncode(70,0,0);
  
                         setLED(FL_LED, D_HARD);
                         setLED(FR_LED, D_MEDIUM);
                         setLED(RL_LED, D_SOFT);
                         setLED(RR_LED, D_RED);
                      break;
			   
		      case LED_LEVEL4:
	                       //SET the LEDs to Maximum Brightness Mode	
                         ledHardValue = rgbEncode(120,0,120);
                         ledMediumValue = rgbEncode(0,120,60);
                         ledSoftValue = rgbEncode(0,0,150);
                         ledRedValue = rgbEncode(120,0,0);
  
                         setLED(FL_LED, D_HARD);
                         setLED(FR_LED, D_MEDIUM);
                         setLED(RL_LED, D_SOFT);
                         setLED(RR_LED, D_RED);
		                     break;
       }

}

//*************************************************************************************
//
//
//    setSTARTUP()
//
//
//************************************************************************************
void setStartup(){
        int smode;
	//
	// Save current controller mode 
	//
        eepromWriteInt(EPROM_CONTROLLER_MODE,previousMode);
        
	/*
        Serial.print(F("    Saving Controller Mode ="));
        Serial.print(previousMode);
        */                    

	//
	// Save current current controller strut mode
	//
	smode = myController.strutMode();
        eepromWriteInt(EPROM_STRUT_MODE,smode);

	/*
        Serial.print(F("    Saving Strut Mode ="));
        Serial.println(smode);
        */                    

	//
	// Restore the previous state of the controller
	//
	myController.setMode(previousMode);    

	
}
//*************************************************************************************
//
//
//    setStruts()
//
//
//************************************************************************************ 
void setStruts(){

     for(int strut = 0; strut < 4; strut++){
	     if(logMode) doLogMode(LOG_STRUTS_NAME , strut,0);
     	     setStrutMode(strut);
     }
	
}

//*************************************************************************************
//
//
//    setManualStruts()
//
//
//************************************************************************************ 
void setManualStruts(){

     for(int strut = 0; strut < 4; strut++){
             setStrutDirectMode( strut, myStruts[strut].desiredState(), DONT_CLEAR);
     }
	
}

//*************************************************************************************
//
//
//    setManualStrutsTargets()
//
//
//************************************************************************************
void setManualStrutTargets(){

     for(int strut = 0; strut < 4; strut++){

        
	 //
	 // See if we are changing states
	 //
	 if (myStruts[strut].desiredState() != myController.strutMode()){

	     //
	     // Yes the human wants a new state - put it in motion
	     //
             myStruts[strut].setDesiredState(myController.strutMode());
	     myStruts[strut].setTransitionStatus(CHANGING);
             myStruts[strut].setLastChangeTime();
             myStruts[strut].clearTryCount();
             myStruts[strut].setErrorStatus(NO_FAILURES);
	 }else {
             //
	     // No Change - do nothing
             //
	 }
     }
	
}
//*************************************************************************************
//
//
//    setAutoStrutsTargets()
//
//
//************************************************************************************
void setAutoStrutTargets(){


     for(int strut = 0; strut < 4; strut++){
	 //
	 // See if we are changing states
	 //
	 if (myStruts[strut].desiredState() != accelerometerStrutTargets[strut]){

	     //
	     // Yes the accelerometer readings dictate a new state - put it in motion
	     //
             myStruts[strut].setDesiredState(accelerometerStrutTargets[strut]);
	     myStruts[strut].setTransitionStatus(CHANGING);
             myStruts[strut].clearTryCount();
             myStruts[strut].setErrorStatus(NO_FAILURES);
	 }else {
             //
	     // No Change - do nothing
             //
	 }
     }
	
}



//*******************************************************************************************
//
//
// Rountine to only allow a strut downward change to Medium or Soft
// until after the hold interval has passed 
//
//
//*******************************************************************************************
void setAutoStrutState(int strut, int mode){
	
unsigned long currentTime = millis();

	if ( mode == SOFT && myController.strutMode() == MEDIUM) mode = MEDIUM;

	if (mode == HARD){
                	       
		accelerometerStrutTargets[strut] = mode;
                myStruts[strut].setLastChangeTime();
	
	} else if (mode == MEDIUM){
		if( accelerometerStrutTargets[strut] == HARD &&  currentTime < (myStruts[strut].lastChangeTime() + STRUT_HOLD_INTERVAL)) return;

		accelerometerStrutTargets[strut] = MEDIUM;
                myStruts[strut].setLastChangeTime();

		
	} else if (mode == SOFT){

		if( (accelerometerStrutTargets[strut] == HARD ||accelerometerStrutTargets[strut] == MEDIUM) 
		     &&  currentTime < (myStruts[strut].lastChangeTime() + STRUT_HOLD_INTERVAL)) return;

		accelerometerStrutTargets[strut] = SOFT;
                myStruts[strut].setLastChangeTime();
        }
}


//*******************************************************************************************
//
//
// Rountine to read current Accelerometer values and set Strut Targets 
//
//
//*******************************************************************************************
void setAccelerometerTargets(){

     
     /*
      ForwardAcceleration  = 0.0;
      LeftRightAcceleration = 0.0;
      UpDownAcceleration = 0.0;
      STRUT_FL 
      STRUT_FR 
      STRUT_RL 
      STRUT_RR
     */

      /*
      Serial.print(F("Forward Accleration ="));
      Serial.print(ForwardAcceleration );
      Serial.print(F("    Left Right Accleration ="));
      Serial.println(LeftRightAcceleration );
      */

     //
     // Check to see if we doing NO Acceleration or Braking ?
     //
     if (ForwardAcceleration > (-1)*mediumThreshold  && ForwardAcceleration < mediumThreshold ){ 
            //
            // No Foward or Braking Acceleration 
            // 

	   /*
           Serial.println(F("No Forward or Braking Acceleration"));
           */                    

	    if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration                 //
                // No lateral Acceleration - set all struts to soft
                // 
	        setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, SOFT);

            }else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration                 //
                //  Doing a MEDIUM  Left Turn at same time 
		//  Set right side struts to medium & 
                // 
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, MEDIUM);

            }else if (LeftRightAcceleration >= hardThreshold ) {
                //
                //  Doing a HARD Left Turn at same time- 
		//  set rear struts to SOFT & right side struts to Hard 
                // 
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, HARD);
	     
            }else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
                //
                //  Doing a MEDIUM RIGHT Turn at same time- 
		//  set rear to SOFT & left side struts to medium & 
                // 
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, SOFT);

            }else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
                //
                //  Doing a HARD RIGHT Turn at same time- 
		//  set rear struts to SOFT & left side struts to Hard 
                // 
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, SOFT);
	    }
 	

     //
     //Check to see if we are doing MEDIUM range  Forward Acceleration ?
     // 
     } else if(ForwardAcceleration >= mediumThreshold  && ForwardAcceleration < hardThreshold ){
            //
            // Doing MEDIUM Forward Acceleration between mediumThresholdg's and hardThreshold 
	    // In this range the Rear Struts will be set to Medium Mode
	    // And any Lateral accelerations due to turns will set the LEFT or RIGHT Struts
            // 

	    /*
            Serial.println(F("MEDIUM Forward Acceleration"));
            */

	    if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration                 //
                // No lateral Acceleration - set just rear struts to medium
                // 
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, MEDIUM);

            }else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration                 //
                //  Doing a MEDIUM  Left Turn at same time 
		//  Set RIGHT side struts to medium & 
                // 
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, MEDIUM);

            }else if (LeftRightAcceleration >= hardThreshold ) {
                //
                //  Doing a HARD Left Turn at same time- 
		//  set RIGHT side struts to Hard 
                // 
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, HARD);
	     
            }else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
                //
                //  Doing a MEDIUM RIGHT Turn at same time- 
		//  set LEFT side struts to medium  
                // 
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, MEDIUM);

            }else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
                //
                //  Doing a HARD RIGHT Turn at same time- 
		//  set LEFT side struts to Hard 
                // 
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, MEDIUM);
	    }
 
     //
     //Check to see if we are doing Hard range  Forward Acceleration ?
     // 
     } else if(ForwardAcceleration >= hardThreshold ){ 
	    //
            // Doing HARD Forward Acceleration greater than hardThresholdgs 
	    // In this range the Rear Struts will be set to HARD Mode
	    // And any Lateral accelerations due to turns will set the LEFT or RIGHT Struts
            // 

            /*
	     Serial.println(F("HARD Forward Acceleration"));
	    */

	    if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration                 //
                // No lateral Acceleration - set just rear struts to HARD
                // 
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, HARD);

            }else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration                 //
                //  Doing a MEDIUM  Left Turn at same time 
		//  Set right side struts to medium  
                // 
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, HARD);

            }else if (LeftRightAcceleration >= hardThreshold ) {
                //
                //  Doing a HARD Left Turn at same time- 
		//  set rear struts to medium & right side struts to Hard 
                // 
                setAutoStrutState(STRUT_FL, SOFT);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, HARD);
	     
            }else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
                //
                //  Doing a MEDIUM RIGHT Turn at same time- 
		//  set rear & left side struts to medium & 
                // 
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, HARD);

            }else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
                //
                //  Doing a HARD RIGHT Turn at same time- 
		//  set rear struts to medium & left side struts to Hard 
                // 
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, SOFT);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, HARD);
	    }
 
     //
     //Check to see if we are doing MEDIUM range Braking
     // 
     } else if(ForwardAcceleration > (-1)*hardThreshold  && ForwardAcceleration < (-1)*mediumThreshold){ 
	    //
            // Doing MEDIUM range Braking between 2.0 & 4.0 g's 
	    // In this range the Front Struts will be set to MEDIUM Mode
	    // And any Lateral accelerations due to turns will set the LEFT or RIGHT Struts
            // 

            /*
	     * Serial.println(F("MEDIUM Braking"));
             */
	    if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration                 //
                // No lateral Acceleration - set just Front struts to HARD
                // 
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, SOFT);

            }else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration                 //
                //  Doing a MEDIUM  Left Turn at same time 
		//  Set right side struts to medium  
                // 
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, MEDIUM);

            }else if (LeftRightAcceleration >= hardThreshold ) {
                //
                //  Doing a HARD Left Turn at same time- 
		//  Set right side struts to Hard 
                // 
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, HARD);
	     
            }else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
                //
                //  Doing a MEDIUM RIGHT Turn at same time- 
		//  Set left side struts to medium & 
                // 
                setAutoStrutState(STRUT_FL, MEDIUM);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, SOFT);

            }else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
                //
                //  Doing a HARD RIGHT Turn at same time- 
		//  left side struts to Hard 
                // 
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, MEDIUM);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, SOFT);
	    }
 
     //
     //Check to see if we are doing HARD range Braking
     // 
     } else if(ForwardAcceleration <= (-1)*hardThreshold ){ 
	    //
            // Doing HARD range Braking greater than  4.0 g's 
	    // In this range the Front Struts will be set to Hard Mode
	    // And any Lateral accelerations due to turns will set the LEFT or RIGHT Struts
            // 
	    
            /*
	     Serial.println(F("Hard Braking"));
             */

	    if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration                 //
                // No lateral Acceleration - set just Front struts to HARD
                // 
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, SOFT);

            }else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration                 //
                //  Doing a MEDIUM  Left Turn at same time 
		//  Set right side struts to medium  
                // 
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, MEDIUM);

            }else if (LeftRightAcceleration >= hardThreshold ) {
                //
                //  Doing a HARD Left Turn at same time- 
		//  Set right side struts to Hard 
                // 
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, SOFT);
                setAutoStrutState(STRUT_RR, HARD);
	     
            }else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
                //
                //  Doing a MEDIUM RIGHT Turn at same time- 
		//  Set left side struts to medium & 
                // 
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, MEDIUM);
                setAutoStrutState(STRUT_RR, SOFT);

            }else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
                //
                //  Doing a HARD RIGHT Turn at same time- 
		//  left side struts to Hard 
                // 
                setAutoStrutState(STRUT_FL, HARD);
                setAutoStrutState(STRUT_FR, HARD);
                setAutoStrutState(STRUT_RL, HARD);
                setAutoStrutState(STRUT_RR, SOFT);
	    }
 
     } 
}


///////////////////////////////////////////////////////////////////////////////////////
//
//
// Routine to make one pass thru a Strut to Set the desired strut to a particular mode        
// this routine must be called in a loop
//
/////////////////////////////////////////////////////////////////////////////////////
void setStrutMode( int strut ){

  int maxTries;	
  int currentState;
  unsigned long currentTime;

                currentTime = millis();

               if (myController.mode() == MANUAL) maxTries = MANUAL_MAXTRIES;
	       else if (myController.mode() == AUTO) maxTries = AUTO_MAXTRIES; 

                //
		// If this strut has exceeded maximum allowed failures bail                   
                // 
		if (myStruts[strut].failures() > MAXFAILURES){
		       // Make sure Motor is Off
        	       myStruts[strut].motorOff();
	               if(logMode){
		                Serial.print("MAXFAILS,"); 
			        Serial.print("FAILED,"); 
			        Serial.println("FAILED"); 
		      }
                      return;
	        }

                //
		// Jump to strut state
                // 
		switch (myStruts[strut].transitionStatus()){
		

			case OLDSTRUTS:
                                //
				// This case deals with old struts where the motors & mechansim
				// can cause the strut to have problems getting into mode
				// This state will purposely run this strut motor 
				// for an additional 20msec to try an force it into mode 
				// turn off the motors and then see if the strut is in mode
				// and then set state accordingly
			

                                
                                if( currentTime < myStruts[strut].lastChangeTime() + 20) break;
                                
				//
				// OK the motors for this strut have been forced on for 20 msecs
		                // Turn them off or make sure their still off.  
        	                myStruts[strut].motorOff();
				
				//
				// Wait for an additional 10 msecs( 20+10) for strut & switches to stablize
				//
                                if( currentTime < myStruts[strut].lastChangeTime() + 30) break;
				
                                currentState =  myStruts[strut].actualState(ERROR);           
				
			    	if( currentState == myStruts[strut].desiredState()  ){

     	                              // Ok Struts have held desired mode for 5 msecs
				      // Switch to Nominal
				     
                                      myStruts[strut].setErrorStatus(NO_FAILURES);
                                      myStruts[strut].setTransitionStatus(NOMINAL);
                                      myStruts[strut].setLastChangeTime();

				}else{
				       //
				       // Ok we were not able to get the struts into the correct 
				       // mode using old struts mode
				       // flag them as bad
				       //
        	                       myStruts[strut].motorOff();
                                       myStruts[strut].setLastChangeTime();
                                       myStruts[strut].setTransitionStatus(FAILED);
	             		       myStruts[strut].setErrorStatus(MOTOR_COMMAND_FAILURE); 
                                       myStruts[strut].incrementFailures();
                                       setLED(strut,D_GREENYELLOW);

				}
                                break;
				
			case PRENOMINAL:

				 //
				 // This state is used just after a strut has indicated it
				 // has achieved the desired mode
				 // it will double check the state of after 5 msec
				 // and declare it Nominal if it stays in the desired mode
				 //

				 //
				 // wait 10 msec before checking status
				 //
				 if(currentTime < myStruts[strut].lastChangeTime() + 10) break;

                                 currentState =  myStruts[strut].actualState(ERROR);           

			    	 if( currentState == myStruts[strut].desiredState()  ){

     	                              // Ok Struts have held desired mode
				      // Switch to Nominal
				     
                                      myStruts[strut].setErrorStatus(NO_FAILURES);
                                      myStruts[strut].setTransitionStatus(NOMINAL);
                                      myStruts[strut].setLastChangeTime();

				 }else{
				       //
				       // Ok it looks like we might be dealing with old struts
				       // turn the motors back on and set to Old Struts mode
				       //
        	                       myStruts[strut].motorOn();
                                       myStruts[strut].incrementTryCount();
                                       myStruts[strut].setLastChangeTime();
                                       myStruts[strut].setTransitionStatus(OLDSTRUTS);

				 }
			     break;

			case FAILED:
			     if (myStruts[strut].errorStatus() == SIGNAL_WIRE_FAILURE){
		             	setLED(strut,D_YELLOW);
			     }else if (myStruts[strut].errorStatus() == MOTOR_COMMAND_FAILURE){
                                setLED(strut,D_RED);
			     }
			     break;

			case NOMINAL:
                             currentState =  myStruts[strut].actualState(ERROR);           
			     
                             if (currentState != myStruts[strut].desiredState()) {
		             	setLED(strut,D_YELLOW);
	             		myStruts[strut].setErrorStatus(SIGNAL_WIRE_FAILURE); 
                                myStruts[strut].setTransitionStatus(FAILED);
                                myStruts[strut].incrementFailures();
		             }

			     break;

			case CHANGING:
			     
                             currentState =  myStruts[strut].actualState(ERROR);           
			     
     	                     if(  myStruts[strut].tryCount() < maxTries   ){


     	                          if( currentState == myStruts[strut].desiredState()  ){
				       //
				       // Ok.. we just got the strut into the desired state
				       //
				       //
				       
		                       // make sure the motors are off
        	                       myStruts[strut].motorOff();
			 
     	                               // Strut looks like it is now in desired mode
				       // change state to PRENOMINAL - that state will figure out if strut 
				       // really stayed in desired state
				       //
                                       myStruts[strut].setErrorStatus(NO_FAILURES);
                                       myStruts[strut].setTransitionStatus(PRENOMINAL);
                                       myStruts[strut].setLastChangeTime();

    		                  }else {
				       //
				       // Ok still not in correct state keep motors on
        	                       myStruts[strut].motorOn();
                                       myStruts[strut].incrementTryCount();
		                  } 

		             }else {
		                 //
		                 // Maxtries was exceeded - display RED 
		                 //

		                 // Turn the Motor Off
        	                 myStruts[strut].motorOff();

                                 setLED(strut,D_RED);
                                 myStruts[strut].setErrorStatus(MOTOR_COMMAND_FAILURE);
                                 myStruts[strut].setTransitionStatus(FAILED);
                                 myStruts[strut].incrementFailures();

		             }
			     break;
		}

	        if(logMode){
		      if (myStruts[strut].errorStatus() == SIGNAL_WIRE_FAILURE){
		           Serial.print("WIRE_FAIL,"); 
		      }else if (myStruts[strut].errorStatus() == MOTOR_COMMAND_FAILURE){
			   Serial.print("MOTOR_FAIL,"); 
		      }else if (myStruts[strut].errorStatus() == NO_FAILURES){
			        Serial.print("STRUT_OK,"); 
		      }

		      if (myStruts[strut].desiredState() == HARD){
		           Serial.print("T=HARD,"); 
		      }else if (myStruts[strut].desiredState() == MEDIUM){
			   Serial.print("T=MEDIUM,"); 
		      }else if (myStruts[strut].desiredState() == SOFT){
			   Serial.print("T=SOFT,"); 
		      }
                      if(strut == 3){
		         if (currentState == HARD){
		           Serial.println("HARD"); 
		         }else if (currentState == MEDIUM){
			   Serial.println("MEDIUM"); 
		         }else if (currentState == SOFT){
			   Serial.println("SOFT"); 
		         }else Serial.println("Unknown");  
		      }else{
		         if (currentState == HARD){
		           Serial.print("HARD, "); 
		         }else if (currentState == MEDIUM){
			   Serial.print("MEDIUM, "); 
		         }else if (currentState == SOFT){
			   Serial.print("SOFT, "); 
		         }else Serial.print("Unknown, ");  
		      }
	        }


}
///////////////////////////////////////////////////////////////////////////////////////
//
//
// Routine to Read actual status of strut , set LED color apprropriately and return value
//
//
///////////////////////////////////////////////////////////////////////////////////////
int readStrutState( int strut, int displayMode){

  int strutS1;
  int strutS2;
  
  int switch1;
  int switch2;
  int switch1b;
  int switch2b;

  int timeout = 0;
  int numberTries = 0;
/*
  unsigned long timestart;
  unsigned long timenow;
  timestart = millis();
  */

  strutS1 = myStruts[strut].switch1();
  strutS2 = myStruts[strut].switch2();

  //
  // intial switch reads
  //
  switch1 = digitalRead(strutS1);
  switch2 = digitalRead(strutS2);
  delayMicroseconds(50); //Wait for 100us to debounce contacts
  switch1b = digitalRead(strutS1);
  switch2b = digitalRead(strutS2);


  //
  // See if the switches are reading the same
  //
  while ((switch2 != switch2b || switch1 != switch1b)&& numberTries < 40){
      switch1 = digitalRead(strutS1);
      switch2 = digitalRead(strutS2);
      delayMicroseconds(50); //Wait for 50us to debounce contacts
      switch1b = digitalRead(strutS1);
      switch2b = digitalRead(strutS2);
      numberTries++;
      
  }
  
 
  if (switch2 == HIGH && switch1 == LOW && numberTries <40 ){
    // Strut is in HARD mode
    setLED(strut, D_HARD);
    return(HARD);
    
  }else if (switch2 == LOW &&  switch1 == LOW && numberTries <40 ){
    // If Strut is in MEDIUM mode  
    setLED(strut, D_MEDIUM);
    return(MEDIUM);
    
  }else if (switch2 == LOW && switch1 == HIGH && numberTries <40 ){
    // Strut is in SOFT Mode
    setLED(strut, D_SOFT);
    return(SOFT);
  }else {
    // setLED(strut, D_UNKNOWN);
    setLED(strut, D_OFF);
    return(UNKNOWN);
  }
}


///////////////////////////////////////////////////////////////////////////////////////
//
//
// Routine to set a Strut to a desired mode        
// this routine has it's own internal loop
//
/////////////////////////////////////////////////////////////////////////////////////
void setStrutDirectMode( int strut, int Mode,int Clear ){
	
  int currentState;
  int maxTries = 20000;


             /*
                SIGNAL_WIRE_FAILURE   
                MOTOR_COMMAND_FAILURE 
                NO_FAILURES 
	       if (myStruts[strut].desiredState() != myController.strutMode()){*
               myStruts[strut].setErrorStatus(NO_FAILURES);
               myStruts[strut].setDesiredState(myController.strutMode());
	       myStruts[strut].setTransitionStatus(CHANGING);
               STRUT_FL 0
               STRUT_FR 1
               STRUT_RL 2
               STRUT_RR 3
	     */

          if(Clear == CLEAR){
             myStruts[strut].clearFailures();
             myStruts[strut].clearTryCount();
             myStruts[strut].setErrorStatus(NO_FAILURES);
	    
          }  

          myStruts[strut].setDesiredState(Mode);
	  myStruts[strut].setTransitionStatus(CHANGING);
		
          while (myStruts[strut].transitionStatus() == CHANGING ){
		  
		if (myStruts[strut].failures() > MAXFAILURES){
		       // Make sure Motor is Off
        	       myStruts[strut].motorOff();
                       return;
		}

		switch (myStruts[strut].transitionStatus()){

			case FAILED:
                             setLED(strut,D_RED);
			     return;
			     break;

			case NOMINAL:
                             currentState =  myStruts[strut].actualState(ERROR);           
			     
                             if (currentState != myStruts[strut].desiredState()) {
		             	setLED(strut,D_YELLOW);
	             		myStruts[strut].setErrorStatus(SIGNAL_WIRE_FAILURE); 
                                myStruts[strut].setTransitionStatus(FAILED);
                                myStruts[strut].incrementFailures();
		             }
			     return;

			case CHANGING:
			     
                             currentState =  myStruts[strut].actualState(ERROR);           
			     
     	                     if(  myStruts[strut].tryCount() < maxTries   ){


     	                          if( currentState == myStruts[strut].desiredState()  ){
				       //
				       // Ok.. we just got the strut into the desired state

				       // delay for X milliseconds to make sure into mode.
	                               // delay(30); 
			       	       
		                       // Turn the Motor Off
        	                       myStruts[strut].motorOff();
			  
     	                               // Strut is now in desired mode
                                        myStruts[strut].setErrorStatus(NO_FAILURES);
                                        myStruts[strut].setTransitionStatus(NOMINAL);

					//
					// Pause for 1 second and do a final check to see if strut stayed in the desired mode
					//
					delay(1000);

                                        currentState =  myStruts[strut].actualState(ERROR);           
			     
                                        if (currentState != myStruts[strut].desiredState()) {
		             	           setLED(strut,D_YELLOW);
	             		           myStruts[strut].setErrorStatus(SIGNAL_WIRE_FAILURE); 
                                           myStruts[strut].setTransitionStatus(FAILED);
                                           myStruts[strut].incrementFailures();
		                        }

    		                  }else {
				       //
				       // Ok still not in correct state keep motors on
        		               myStruts[strut].motorOn();
                                       myStruts[strut].incrementTryCount();
		                  } 

		             }else {
		                 //
		                 // Maxtries was exceeded - display RED 
		                 //

		                 // Turn the Motor Off
        	                 myStruts[strut].motorOff();

                                 setLED(strut,D_RED);
                                 myStruts[strut].setErrorStatus(MOTOR_COMMAND_FAILURE);
                                 myStruts[strut].setTransitionStatus(FAILED);
                                 myStruts[strut].incrementFailures();

		             }
			     break;
		}
         }
}




//*********************************************************************************************
//
//
//  Logging Routine
//
//
//*********************************************************************************************

void doLogMode(int mode,int val1,int val2){
unsigned long currentTime = millis();
         /*
          *         Time, Controller Mode, Controller Target , Acc FWD Avg, Acc LeftRight Avg , Acc UP Avg,
          *         Strut Name, FL Strut STatus, FL Target, FL Strut Actual 
          *         Strut Name, FR Strut STatus, FR Target, FR Strut Actual 
          *         Strut Name, RL Strut STatus, RL Target, RL Strut Actual 
          *         Strut Name, RR Strut STatus, RR Target, RR Strut Actual 
          *
          */

	switch (mode){

		case LOG_TIME:
                        Serial.print(currentTime);
                        Serial.print(F(","));
			
                        if(myController.mode() == MANUAL){
                            Serial.print(F("MANUAL,"));

			}else if(myController.mode() == AUTO){
                            Serial.print(F("AUTO,"));
			}else{
                            Serial.print(F("MODE?,"));
			}
	                if (myController.strutMode() == HARD){
                            Serial.print(F("HARD,"));
			}else if (myController.strutMode() == MEDIUM){
                            Serial.print(F("MEDIUM,"));
			}else if (myController.strutMode() == SOFT){
                            Serial.print(F("SOFT,"));
			}

			break;
			
		case LOG_STRUTS_NAME:
		        if (val1 == 0) Serial.print("FL,"); 
		        else if (val1 == 1) Serial.print("FR,"); 
		        else if (val1 == 2) Serial.print("RL,"); 
		        else if (val1 == 3) Serial.print("RR,"); 
			break;

		case LOG_ACCELEROMETER:
                         readAccelerometerValues();    


			break;
	}
}



//**************************************************************************
// this routine converts three seperate RGB values in to a correctly encoded RGB integer
//**************************************************************************
long rgbEncode(int red,int green,int blue){
 long rgb;

 //if (red > 255) red=255;
 //if (green > 255) green=255;
 //if (blue > 255) blue=255;

 rgb = red*65536 + green*256 + blue;

 return (rgb);
 
} 




///////////////////////////////////////////////////////////////////////////////////////
// Routine to Blink one of the lights
///////////////////////////////////////////////////////////////////////////////////////
void blinkLights(int lightaddress, int numberBlinks,int blinkDelay,int numberGroupBlinks,int groupDelay){
    
    setLights(lightaddress,LOW);
    
    for (int groupcount = numberGroupBlinks; groupcount > 0; groupcount--){
        
	for (int count = numberBlinks; count > 0; count--){
	   delay(blinkDelay);
	   setLights(lightaddress,HIGH);
	   delay(blinkDelay);
	   setLights(lightaddress,LOW);
        }
	delay(groupDelay);
    }
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to Blink Both of the lights at the same time
///////////////////////////////////////////////////////////////////////////////////////
void blinkBothLights(int numberBlinks,int blinkDelay,int numberGroupBlinks,int groupDelay,int doLed){
    
    // Turn off both Lights
    setLights(SportLight,LOW);
    setLights(TourLight,LOW);
    
    for (int groupcount = numberGroupBlinks; groupcount > 0; groupcount--){
        
	for (int count = numberBlinks; count > 0; count--){
	   delay(blinkDelay);
           setLights(SportLight,HIGH);
           setLights(TourLight,HIGH);
	   if(doLed){
              setLED(STRUT_FL,D_WHITE);
              setLED(STRUT_FR,D_WHITE);
	   }
	   
	   delay(blinkDelay);
           setLights(SportLight,LOW);
           setLights(TourLight,LOW);
	   if(doLed){
              setLED(STRUT_FL,D_OFF);
              setLED(STRUT_FR,D_OFF);
	   }
        }
	delay(groupDelay);
    }
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to Set TourSport Lights during command loop
///////////////////////////////////////////////////////////////////////////////////////

void setLoopTourSportLights(){

     int strutMode;
     unsigned long currentTime = millis();
  
     if(currentTime <= lightCycleStartTime + 30000){
          //
          // time to do Standard Lights Status
          //

	  if(myController.mode() == MANUAL) strutMode = myController.strutMode();
	  else if (myController.mode() == AUTO) {
              strutMode = SOFT;
              for(int strut = 0; strut < 4; strut++){
		      if(myStruts[strut].desiredState() == MEDIUM && strutMode == SOFT ) strutMode = MEDIUM;
		      if(myStruts[strut].desiredState() == HARD && ( strutMode == MEDIUM || strutMode == SOFT)) strutMode = HARD;
	      }
	  }
          switch (strutMode){
			   case(HARD):
	                     setLights(SportLight,HIGH);
                             setLights(TourLight,LOW);
			     break;

			   case(MEDIUM):
	                     setLights(SportLight,HIGH);
                             setLights(TourLight,HIGH);
			     break;

			   case(SOFT):
	                     setLights(SportLight,LOW);
                             setLights(TourLight,HIGH);
			     break;
			     
			   case(UNKNOWN):
	                     setLights(SportLight,LOW);
                             setLights(TourLight,LOW);
			     break;
	  } 

          signalWireFailureCycleStartTime = millis();
	  
     }else if(lightCycleStartTime+30000 < currentTime && currentTime <= lightCycleStartTime + 31500){
          //
          // time to do Signal Wire Status
          //

 
  	  // if there was a problem start flashing shit.. to the do sport/tourlight error thing
	  if (myStruts[STRUT_FL].errorStatus() == SIGNAL_WIRE_FAILURE || 
	      myStruts[STRUT_FR].errorStatus() == SIGNAL_WIRE_FAILURE ||
	      myStruts[STRUT_RL].errorStatus() == SIGNAL_WIRE_FAILURE ||
	      myStruts[STRUT_RR].errorStatus() == SIGNAL_WIRE_FAILURE ) {
		  

	       if(currentTime < signalWireFailureCycleStartTime + 250 ){
	              setLights(SportLight,LOW);
                      setLights(TourLight,HIGH);
	       }else if(currentTime < signalWireFailureCycleStartTime + 500 ){
	              setLights(SportLight,HIGH);
                      setLights(TourLight,LOW);
	       }else if(currentTime < signalWireFailureCycleStartTime + 750 ){
	              setLights(SportLight,LOW);
                      setLights(TourLight,HIGH);
	       }else if(currentTime < signalWireFailureCycleStartTime + 1000 ){
	              setLights(SportLight,HIGH);
                      setLights(TourLight,LOW);
	       }else if(currentTime < signalWireFailureCycleStartTime + 1250 ){
	              setLights(SportLight,LOW);
                      setLights(TourLight,HIGH);
	       }else if(currentTime < signalWireFailureCycleStartTime + 1500 ){
	              setLights(SportLight,HIGH);
                      setLights(TourLight,LOW);
               }
           }
          commandFailureCycleStartTime = millis();

     }else if(lightCycleStartTime+31500 < currentTime && currentTime <= lightCycleStartTime + 33000){
          //
          // time to do Motor Command Status
          //

  	  // if there was a problem start flashing shit.. to the do sport/tourlight error thing
	  if (myStruts[STRUT_FL].errorStatus() == MOTOR_COMMAND_FAILURE || 
	      myStruts[STRUT_FR].errorStatus() == MOTOR_COMMAND_FAILURE ||
	      myStruts[STRUT_RL].errorStatus() == MOTOR_COMMAND_FAILURE ||
	      myStruts[STRUT_RR].errorStatus() == MOTOR_COMMAND_FAILURE ) {
		  

	       if(currentTime < commandFailureCycleStartTime + 250 ){
	              setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
	       }else if(currentTime < commandFailureCycleStartTime + 500 ){
	              setLights(SportLight,HIGH);
                      setLights(TourLight,HIGH);
	       }else if(currentTime < commandFailureCycleStartTime + 750 ){
	              setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
	       }else if(currentTime < commandFailureCycleStartTime + 1000 ){
	              setLights(SportLight,HIGH);
                      setLights(TourLight,HIGH);
	       }else if(currentTime < commandFailureCycleStartTime + 1250 ){
	              setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
	       }else if(currentTime < commandFailureCycleStartTime + 1500 ){
	              setLights(SportLight,HIGH);
                      setLights(TourLight,HIGH);
               }
           }
     } else if (currentTime > lightCycleStartTime + 33000) lightCycleStartTime = millis();
     
}

///////////////////////////////////////////////////////////////////////////////////////
// Routine to intialiaze IOPins for Strut Motors
///////////////////////////////////////////////////////////////////////////////////////
void initializeMotorIOPin(int pin){

	// Set pin used for strut motor as output 
	pinMode(pin, OUTPUT);

        // Make sure Motor is off
	digitalWrite(pin,LOW);
}

///////////////////////////////////////////////////////////////////////////////////////
// Routine to intialiaze IOPins for Strut Switches
///////////////////////////////////////////////////////////////////////////////////////
void initializeStrutIOPin(int pin){

	// Set pin used for strut motor as output 
	pinMode(pin, INPUT);

        // Pull Pin HIGH         
	digitalWrite(pin,HIGH);
}


///////////////////////////////////////////////////////////////////////////////////////
// Routine to set an LED  to a color by Address
///////////////////////////////////////////////////////////////////////////////////////
void setLED(int ledaddress, int color){
	//
        // rgbEncode(int red,int green,int blue)
	// red,green,blue =  0, 255
	//

	switch(color){
		case D_HARD:
			setLedColors(ledaddress,ledHardValue);
		        break;	
		case D_MEDIUM:
			setLedColors(ledaddress,ledMediumValue);
		        break;	
		case D_SOFT:
			setLedColors(ledaddress,ledSoftValue);
		        break;	
		case D_RED:
			setLedColors(ledaddress,ledRedValue);
		        break;	
		case D_GREEN:
			setLedColors(ledaddress,rgbEncode(0,10,0));
		        break;	
		case D_BLUE:
			setLedColors(ledaddress,rgbEncode(0,0,25));
		        break;	
		case D_TURQUOIS:
			setLedColors(ledaddress,rgbEncode(0,40,64));
		        break;	
		case D_PURPLE:
			setLedColors(ledaddress,rgbEncode(10,0,10));
		        break;	
		case D_YELLOW:
			setLedColors(ledaddress,rgbEncode(15,15,0));
		        break;
		case D_GREENYELLOW:
			setLedColors(ledaddress,rgbEncode(55,128,0));
		        break;
		case D_STARTUP:
			setLedColors(ledaddress,rgbEncode(2,2,2));
		        break;	
		case D_WHITE:
			setLedColors(ledaddress,rgbEncode(15,15,15));
		        break;	
		case D_OFF:
			setLedColors(ledaddress,rgbEncode(0,0,0));
		        break;	
		case D_UNKNOWN:
			setLedColors(ledaddress,rgbEncode(53,18,0));
		        break;	

	         
		}
         // delayMicroseconds(600); //Wait for 200us to go into reset
	 
}


///////////////////////////////////////////////////////////////////////////////////////
// Routine to turn an LED off
//////////////////////////////////////////////////////////////////////////////////////
void offLED(int ledaddress){
        
	setLedColors(ledaddress,LEDOFF);
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to set either the Tour Or Sport light to an HIGH/ON or LOW/OFF state
///////////////////////////////////////////////////////////////////////////////////////
void setLights(int light, int state){
  digitalWrite(light,state);
}


///////////////////////////////////////////////////////////////////////////////////////
// Routine to turn off all strut motors
///////////////////////////////////////////////////////////////////////////////////////
void motorsOff(){
  myStruts[0].motorOff();
  myStruts[1].motorOff();
  myStruts[2].motorOff();
  myStruts[3].motorOff();
}

//****************************************************
//
// Convert Desired HEX color into  rgb values and store in LED array
//
//****************************************************

void setLedColors(int LED,long hexValue) {
 
    if( ledCurrentValue[LED] == hexValue ) return;
    ledCurrentValue[LED] = hexValue ;
    ledColors[LED].r = ((hexValue >> 16) & 0xFF) ;  // Extract the RR byte
    ledColors[LED].g = ((hexValue >> 8) & 0xFF) ;   // Extract the GG byte
    ledColors[LED].b = ((hexValue) & 0xFF) ;        // Extract the BB byte

    ledColors[LED+4].r = ledColors[LED].r ;
    ledColors[LED+4].g = ledColors[LED].g ;
    ledColors[LED+4].b = ledColors[LED].b ;


    LEDS.show(); //PUSH the current color definitions of ALL the LEDs out
}


/////////////////////////////////////////////////////////////////////////////////////
//
//
// routine to read the switch pushes while setting LED brightness 
//
//
//
/////////////////////////////////////////////////////////////////////////////////////
int readPushButtonSwitch(){
            int numberPushes =0;
            unsigned long currentTime;
            currentTime	= millis();

            //
	    // If no presses have occurred or < 0.5 secs since push then return
	    //
            if(pushCount == 0) return(NOCHANGE); 
            if(currentTime < (pushTime + 1500)) {
	          /*
                  Serial.print(F("PushCount < 2000ms ="));
                  Serial.println(pushCount);
	          */
		  return(NOCHANGE);
	    }

	    /*
            Serial.print(F("PushCount >2000 ="));
            Serial.println(pushCount);
	    */


            //
	    // Make sure pushCount is in valid range - if out of range set back to zero
	    //
            if(pushCount > 4){
		   pushCount = 0;
	           return (NOCHANGE); 
	    }
            numberPushes = pushCount;
	    pushCount = 0;
	    return(numberPushes);

           /* 
            *  Alternate code for more complex return
            //
	    //  Ok button has been pressed one or more times and 0.5 seconds has passed since last push 
	    //  now decide what new state we are in
	    //
            switch (pushCount){
		    case 1:
		        pushCount = 0;
                        return(1);
		        break;

		     case 2: 
		        pushCount = 0;
                        return(2);
		        break;
			
		     case 3: 
		        pushCount = 0;
                        return(3);
		        break;
			
		     case 4: 
		        pushCount = 0;
                        return(4);
		        break;
		     break;


	    }
           */

}



/////////////////////////////////////////////////////////////////////////////////////
// Reference 5v = 1023
//
// Hard     0.0v     / xxx
//    Cutoff  0.415  / 85
// Medium   0.83v    / xxx
//    Cutoff   1.245   / 255
// Soft     1.66v    / xxx
//    Cutoff   2.075   / 424
// Diag     2.5      / xxx
//    Cutoff   2.915   / 596
/////////////////////////////////////////////////////////////////////////////////////
int readControllerSwitch(){
	int voltage;
	int voltage2;
	float voltdiff;

        unsigned long currentTime ;

	// Based on the selector switch type do the appropriate read
	if( myController.selectorType() == ROTARY){
		
	   voltage = analogRead(selectorSwitchPin)+1; // read the voltage	
	   delay(50);                               // debounce for 50 msec
	   voltage2 = analogRead(selectorSwitchPin)+1;// read the voltage again
	   voltdiff = abs(1.0-voltage/voltage2);     // Calculate the percentage difference 

	   //
	   // as long as voltage difference > 10% keep reading
	   //
	   while(voltdiff > .1 ){
	      voltage = analogRead(selectorSwitchPin)+1; // read the voltage	
	      delay(50);                               // debounce for 50 msec
	      voltage2 = analogRead(selectorSwitchPin)+1;// read the voltage again
	      voltdiff = abs(1.0-voltage/voltage2);   // Calculate the percentage difference 
	   }

           //
	   // based on the converted voltage value and the range return the value
           //
	   if(voltage < 85){                          
             // Selector is in HARD mode 
             if (myController.strutMode() == HARD) return (NOCHANGE) ;
	     else {
	       myController.setMode(MANUAL);
	       myController.setControllerStrutMode(HARD);
               return(HARD);
	     }
	 
	   }else if (voltage >= 85 && voltage < 255){ 
	     // Selector is in MEDIUM mode 
             if (myController.strutMode() == MEDIUM) return (NOCHANGE) ;
	     else {
	       myController.setMode(MANUAL);
	       myController.setControllerStrutMode(MEDIUM);
	       return(MEDIUM);
	     }

	   }else if (voltage >= 255 && voltage < 424){
             // Selector is in SOFT mode 
             if (myController.strutMode() == SOFT) return (NOCHANGE) ;
	     else {
	       myController.setMode(MANUAL);
	       myController.setControllerStrutMode(SOFT);
	       return(SOFT);
	     }

	   }else if(voltage >= 424 && voltage <596){
             // Selector is in DIAG mode 
             if (myController.mode() == DIAG) return (NOCHANGE) ;
	     else {
               myController.setMode(DIAG);
	       return(DIAG);
	     }

	   }else{
              myController.setMode(UNKNOWN);
	      return (UNKNOWN);

	   }
	}else if( myController.selectorType() == PUSHBUTTON){
            currentTime = millis();
            //
	    // If no presses have occurred or < 1.5 secs since last push then return
	    //
            if(pushCount ==0) return(NOCHANGE); 
            if(currentTime < (pushTime + 1500)) {
	          /*
                  Serial.print(F("PushCount < 2000ms ="));
                  Serial.println(pushCount);
	          */
		  return(NOCHANGE);

	    }
	    /*
            Serial.print(F("PushCount >2000 ="));
            Serial.println(pushCount);
	    */
            //
	    // Make sure pushCount is in valid range - if out of range set back to zero
	    //
            if(pushCount > 9){
		   pushCount = 0;
	           return (NOCHANGE); 
	    }

            //
	    //  Ok button has been pressed one or more times and 1.5 seconds has passed since last push 
	    //  now decide what new state we are in
	    //
            switch (pushCount){
		    //
		    //  Changing to next strut mode using sequence  H -> S -> M -> H -> S -> M ->....
		    case 1:

		        pushCount = 0;

			//
			// If in Diag or readDiag mode and single push occurs treat this as an abort to go back to manual mode
			//
                       if(myController.mode() == DIAG || myController.mode() == READDIAG){
                             myController.setMode(MANUAL);
                             return(myController.strutMode());
		       }
                        
			
			switch(myController.strutMode()){
                           case HARD:
                             myController.setControllerStrutMode(SOFT);
                             return(SOFT);
                           break;

                           case MEDIUM:
			     if(myController.mode() == AUTO) myController.setControllerStrutMode(SOFT);
			     else myController.setControllerStrutMode(HARD);
                             return(HARD);
                           break;

                           case SOFT:
                             myController.setControllerStrutMode(MEDIUM);
                             return(MEDIUM);
                           break;
			}
		        break;
		     //
		     //  Changing Controller mode from Auto to Manual or vice versa
		     case 2: 
                       motorsOff();

		       pushCount = 0;
                       switch(myController.mode()){
                           case MANUAL:

                             myController.setMode(AUTO);     // Change controller to Auto MODE
			     myController.setControllerStrutMode(SOFT);

			     //
			     // Blink both lights alternating(3 times) to indicate auto mode
			     //
 	                     for(int i=0;i<2;i++){
	                        setLights(SportLight,HIGH);
                                setLED(STRUT_FR,D_WHITE);
				
                                setLights(TourLight,LOW);
                                setLED(STRUT_FL,D_OFF);
				
			        delay(250);
	                        setLights(SportLight,LOW);
                                setLED(STRUT_FR,D_OFF);
				
                                setLights(TourLight,HIGH);
                                setLED(STRUT_FL,D_WHITE);
			        delay(250);
		             }

		             // Turn off Lights
	                     setLights(SportLight,LOW);
                             setLights(TourLight,LOW);
                             setLED(STRUT_FL,D_OFF);
                             setLED(STRUT_FR,D_OFF);
			     
                             return(myController.strutMode());
                           break;

                           case AUTO:
			   case DIAG:
			   case READDIAG:
			   
                             myController.setMode(MANUAL);    // Change Controller to Manual MODE

			     //
			     // Blink both lights at together (3 times) to indicate manual mode
			     //
                             blinkBothLights(3,250,1,0,1);
			     
		             // Turn off Lights
	                     setLights(SportLight,LOW);
                             setLights(TourLight,LOW);
                             return(myController.strutMode());

                           break;

		        }
		        break;

		     //
		     //  Changing Controller LED Settings
		     //
		     case 3: 
                        motorsOff();
		        pushCount = 0;
                        previousMode = myController.mode();
			myController.setMode(SETLED);    // Change Controller to change LED MODE

			//
			// Blink both lights at together (3 times) to indicate LED mode
			//
                        blinkBothLights(3,500,1,0,1);
			
		        // Turn off Lights
	                setLights(SportLight,LOW);
                        setLights(TourLight,LOW);

		        break;	

		     //
		     //  Set default startup Mode
		     //
		     case 4: 
                        motorsOff();
		        pushCount = 0;
                        previousMode = myController.mode();

		        // Turn off Lights
	                setLights(SportLight,LOW);
                        setLights(TourLight,LOW);

			//
			// Blink both lights at together (4 times) to indicate saving startup mode
			//
                        blinkBothLights(4,500,1,0,1);

			myController.setMode(SETSTARTUP);    // Change Controller to change LED MODE
		        break;	

		     //
		     //  Changing Controller to Read Diagnostics Mode
		     //
		     case 5: 
                        motorsOff();
                        myController.setMode(READDIAG);
		        pushCount = 0;
			
		        // Turn off Lights
	                setLights(SportLight,LOW);
                        setLights(TourLight,LOW);

			//
			// Blink both lights at together (5 times) to indicate read diag mode
			//
                        blinkBothLights(5,500,1,0,1);
			delay(500);

                        return(READDIAG);

			break;
		
		     //
		     //  Changing Controller to RUN Diagnostics Mode
		     //
		     case 6: 
                        motorsOff();
                        myController.setMode(DIAG);
		        pushCount = 0;
			
		        // Turn off Lights
	                setLights(SportLight,LOW);
                        setLights(TourLight,LOW);

			//
			// Blink both lights at together (6 times) to indicate saving startup mode
			//
                        blinkBothLights(6,500,1,0,1);
			delay(500);

                        return(DIAG);
			break;

		     //
		     //  Changing Controller to Set Thresholds  Mode
		     //
		     case 7: 

                        motorsOff();
		        pushCount = 0;
                        previousMode = myController.mode();
			myController.setMode(SETTHRESHOLD);    // Change Controller to change LED MODE

			//
			// Blink both lights at together (7 times) to indicate Set Threshold mode
			//
                        blinkBothLights(7,500,1,0,1);
			
		        // Turn off Lights
	                setLights(SportLight,LOW);
                        setLights(TourLight,LOW);

                        return(SETTHRESHOLD);
			break;


		     //
		     //  Toggle Controller in out of logging mode
		     //
		     case 8: 
			//
			// Blink both lights at together (9 times) to indicate logging mode on or Off
			//
		        pushCount = 0;
                        blinkBothLights(8,500,1,0,1);
                        if(logMode == 0) {
				logMode =1;
				logSequence++;
		                Serial.println(F("**********************************************************************"));
				Serial.print(F("LogSequence #:"));
				Serial.println(logSequence);
		                Serial.println(F("******** Strut GForce Transition Thresholds ********"));
                                Serial.print(F("Medium Threshold:  "));
                                Serial.println( mediumThreshold);
                                Serial.print(F("Hard Threshold:  "));
                                Serial.println( hardThreshold );
			        readPrintEpromValues();
                                Serial.print(F("1:Time, 2:Controller Mode, 3:Controller Target , 4:AccFWD, 5:AccLeftRight,6:AccUP,"));
                                Serial.print(F("7:Strut Name, 8:FL Strut STatus, 9:FL Target, 10:FL Strut Actual,")); 
                                Serial.print(F("11:Strut Name, 12:FR Strut STatus, 13:FR Target, 14:FR Strut Actual,")); 
                                Serial.print(F("15:Strut Name, 16:RL Strut STatus, 17:RL Target, 18:RL Strut Actual")); 
                                Serial.println(F("19:Strut Name, 20:RR Strut STatus, 21:RR Target, 22:RR Strut Actual "));
 

			}
			else if (logMode ==1) logMode =0;
			break;
			

		     //
		     //  Changing Controller to Calibrate Accelerometer Mode
		     //
		     case 9: 

                        motorsOff();
		        pushCount = 0;
                        previousMode = myController.mode();
			myController.setMode(CALIBRATEACCEL);    // Change Controller to 

			//
			// Blink both lights at together (8 times) to indicate Calibration mode
			//
                        blinkBothLights(9,500,1,0,1);
			
		        // Turn off Lights
	                setLights(SportLight,LOW);
                        setLights(TourLight,LOW);

                        return(CALIBRATEACCEL);
			break;
              }

          }
}

//*******************************************************************************************
//
// Read Diagnostic routine
//
//
//*******************************************************************************************
void readDiagnostics(){

     while(readControllerSwitch()== NOCHANGE){ 

         // Turn Off All LEDS
         offLED(FL_LED);
         offLED(FR_LED);
         offLED(RL_LED);
         offLED(RR_LED);


         for(int strut = 0; strut < 4; strut++){

             // Turn off Lights
	     setLights(SportLight,LOW);
             setLights(TourLight,LOW);
	    
             //SET strut LED 1->4 to White to indicate testing Front Left Strut 
             setLED(strut,D_WHITE);
             delay(1000);
		   
	     // Blink Sport Light number of times based on strut nuber as group 4 times To indicate strut
             blinkLights(SportLight, (strut+1),300,4,1000);  

	     // See if abort desired
             if(readControllerSwitch()!= NOCHANGE) return; 

                   
	     // See if abort desired
	     if(readControllerSwitch()!= NOCHANGE) return; 

	     // Display Final test results for this Strut
	     if (myStruts[strut].errorStatus() == NO_FAILURES){
                       setLED(strut,D_GREEN);
		       setLights(TourLight,HIGH);
		       setLights(SportLight,HIGH);
                       delay(2500);

	      }else if (myStruts[strut].errorStatus() == MOTOR_COMMAND_FAILURE){
                      blinkBothLights(3,300,3,1000,0);
                      setLED(strut,D_RED);

	      }else if (myStruts[strut].errorStatus() == SIGNAL_WIRE_FAILURE){ 
	              setLED(strut,D_YELLOW);

 	              for(int i=0;i<8;i++){
	                 setLights(SportLight,HIGH);
                         setLights(TourLight,LOW);
			 delay(250);
	                 setLights(SportLight,LOW);
                         setLights(TourLight,HIGH);
			 delay(250);
		      }
		      // Turn off Lights
	              setLights(SportLight,LOW);
                      setLights(TourLight,LOW);
	       }

              // See if abort desired
              if(readControllerSwitch()!= NOCHANGE) return; 
		   
              delay(2000);
         }
     }
}

//*******************************************************************************************
//
// Diagnostic routine
//
//
//*******************************************************************************************
void runDiagnostics(){
	
     while(readControllerSwitch()== NOCHANGE){ 

         // Turn Off All LEDS
         offLED(FL_LED);
         offLED(FR_LED);
         offLED(RL_LED);
         offLED(RR_LED);



         for(int strut = 0; strut < 4; strut++){
             // Turn off Lights
	     setLights(SportLight,LOW);
             setLights(TourLight,LOW);

             //SET strut LED 1->4 to White to indicate testing Front Left Strut 
             setLED(strut,D_WHITE);
		   
             delay(1000);
	    
	     // Blink Sport Light number of times based on strut nuber as group 4 times To indicate strut
             blinkLights(SportLight, (strut+1),300,4,1000);  

	     // See if abort desired
             if(readControllerSwitch()!= NOCHANGE) return; 

             //Attempt to set   Strut HARD MODE
             setStrutDirectMode( strut, HARD, CLEAR);
             delay(1000);
                   
	     // See if abort desired
	     if(readControllerSwitch()!= NOCHANGE) return; 

             //Attempt to set  Front Left Strut Medium MODE
             setStrutDirectMode( strut, MEDIUM, DONT_CLEAR);
             delay(1000);
		      
	     // See if abort desired
	     if(readControllerSwitch()!= NOCHANGE) return; 
                                      
             //Attempt to set  Front Left Strut Soft MODE
             setStrutDirectMode( strut, SOFT, DONT_CLEAR);
             delay(1000);

	     // See if abort desired
	     if(readControllerSwitch()!= NOCHANGE) return; 

	     // Display Final test results for this Strut
	     if (myStruts[strut].errorStatus() == NO_FAILURES){
                       setLED(strut,D_GREEN);
		       setLights(TourLight,HIGH);
		       setLights(SportLight,HIGH);
                       delay(2500);
              }else{
                      blinkBothLights(3,300,3,1000,0);
                      setLED(FL_LED,D_RED);
	      }

              // See if abort desired
              if(readControllerSwitch()!= NOCHANGE) return; 
		   
              delay(2000);
        }
   }
}

/*************************************************************
 *
 *
 * Routine to read int from EProm
 *
 *
 *************************************************************/
int eepromReadInt(int address){
   int value = 0x0000;
   value = value | (EEPROM.read(address) << 8);
   value = value | EEPROM.read(address+1);
   return value;
}

/*************************************************************
 *
 *
 * Routine to Write int to EProm
 *
 *
 *************************************************************/
void eepromWriteInt(int address, int value){
   EEPROM.write(address, (value >> 8) & 0xFF );
   EEPROM.write(address+1, value & 0xFF);
}


//******************************************************************************************
//
//LSM303DLHC I2C Device Routines
//
//******************************************************************************************
  
// Send register address and the byte value you want to write the accelerometer and 
// loads the destination register with the value you send

void WriteDevRegister(int devAddress, byte data, int regaddress) {
    Wire.beginTransmission(devAddress);   // device Address
    Wire.write(regaddress);
    Wire.write(data);  
    Wire.endTransmission();     
}

//
//Send register address to this function and it returns byte value
//for the accelerometer register's contents 
byte ReadDevRegister(int devAddress, int regaddress) {

    byte data;
    Wire.beginTransmission(devAddress);   // device Address                              
    Wire.write(regaddress);
    Wire.endTransmission();
  
    delayMicroseconds(100);

    Wire.requestFrom(devAddress,1);   // device Address                              
    data = Wire.read();
    Wire.endTransmission();   

    delayMicroseconds(100);

    return data;  
}  

//**************************************************************************
//
// Accelerometer Initialization Routine
//
//**************************************************************************

void initAccelerometer(void) {

    WriteDevRegister(LSM303_ADDRESS_ACC,0x67,0x20);  // Enable accelerometer, 200Hz data output

    WriteDevRegister(LSM303_ADDRESS_MAG,0x9c,0x00);  // Enable temperature sensor, 220Hz data output
    WriteDevRegister(LSM303_ADDRESS_MAG,0x20,0x01);  // set gain to +/-1.3Gauss
    WriteDevRegister(LSM303_ADDRESS_MAG,0x00,0x02);  // Enable magnetometer constant conversions
}

//**************************************************************************
//
//    Read  the X,Y,Z axis values from the accelerometer 
//
//**************************************************************************
void readAccelerometerValues() {

    float Acc[3];
    //
    //  ForwardAxis    axis
    //  ForwardSign    +Acc = Acceleration

    //  LeftRightAxis  axis
    //  LeftRightSign      +Acc = Left Turn

    //  UpDownAxis     axis
    //  UpDownSign     +Acc = Up 

    //
    //  reinitialize globals to prepare for new averages
    //
    ForwardAcceleration  = 0.0;
    LeftRightAcceleration = 0.0;
    UpDownAcceleration = 0.0;

    //
    //  Shift all the previous readings down in the array & Start summing the existing valid entries for the average calculation
    //
    for ( int i = accelDataPoints -1;  i > 0 ; i--){
        ForwardAccelerationArray[i] =    ForwardAccelerationArray[i-1];
        ForwardAcceleration = ForwardAcceleration + ForwardAccelerationArray[i]; 

        LeftRightAccelerationArray[i] =  LeftRightAccelerationArray[i-1]; 
        LeftRightAcceleration = LeftRightAcceleration + LeftRightAccelerationArray[i]; 

        UpDownAccelerationArray[i] =     UpDownAccelerationArray[i-1];
        UpDownAcceleration = UpDownAcceleration + UpDownAccelerationArray[i]; 

    }

    //
    // get the raw Accelerometer current values off the I2C device
    // 
    getAccelerometer(Acc);

    //
    //  Alias and Assign the current reading to the correct mappings in the normilized array [0]
    //
    ForwardAccelerationArray[0]   =    float (ForwardSign * Acc[ForwardAxis]);
    LeftRightAccelerationArray[0] =    float (LeftRightSign * Acc[LeftRightAxis]); 
    UpDownAccelerationArray[0]    =    float (UpDownSign * Acc[UpDownAxis]); 


    //
    // add the current acceleration reading to the global value and finish average calculation
    //
    ForwardAcceleration   = (ForwardAcceleration + ForwardAccelerationArray[0]) / accelDataPoints;
    LeftRightAcceleration = (LeftRightAcceleration + LeftRightAccelerationArray[0]) / accelDataPoints;
    UpDownAcceleration    = (UpDownAcceleration + UpDownAccelerationArray[0]) / accelDataPoints;
    
    if(logMode) {
                        Serial.print(ForwardAcceleration);
                        Serial.print(F(","));
                        Serial.print(LeftRightAcceleration);
                        Serial.print(F(","));
                        Serial.print(UpDownAcceleration);
                        Serial.print(F(","));
    }

}  

// ******************************************************************************k
//Readsthe X,Y,Z axis values from the accelerometer and sends the values to the 
// ******************************************************************************k
void getAccelerometer(float *Acc) {

  // accelerometer values
  byte xh = ReadDevRegister(LSM303_ADDRESS_ACC,0x29);
  byte xl = ReadDevRegister(LSM303_ADDRESS_ACC,0x28);
  byte yh = ReadDevRegister(LSM303_ADDRESS_ACC,0x2B);
  byte yl = ReadDevRegister(LSM303_ADDRESS_ACC,0x2A);
  byte zh = ReadDevRegister(LSM303_ADDRESS_ACC,0x2D);
  byte zl = ReadDevRegister(LSM303_ADDRESS_ACC,0x2C);
  
  // need to convert the register contents into a righ-justified 16 bit value
  Acc[0] = (xh<<8|xl); 
  Acc[1] = (yh<<8|yl); 
  Acc[2] = (zh<<8|zl); 

  //
  // Convert raw Accelerometer readings to g's using calibration values 
  //
  if( Acc[0] >= 0) Acc[0] = float( Acc[0] / XAxisPositive1gCalibration) ;
  else Acc[0] = float(Acc[0] / XAxisNegative1gCalibration ); 

  if( Acc[1] >= 0) Acc[1] = float( Acc[1] / YAxisPositive1gCalibration) ;
  else Acc[1] = float(Acc[1] / YAxisNegative1gCalibration ); 

  if( Acc[2] >= 0) Acc[2] = float( Acc[2] / ZAxisPositive1gCalibration) ;
  else Acc[2] = float(Acc[2] / ZAxisNegative1gCalibration ); 

}  
// ******************************************************************************k
//Readsthe X,Y,Z axis values from the accelerometer and sends the values to the 
// ******************************************************************************k
void getRawAccelerometer(float *Acc) {

  // accelerometer values
  byte xh = ReadDevRegister(LSM303_ADDRESS_ACC,0x29);
  byte xl = ReadDevRegister(LSM303_ADDRESS_ACC,0x28);
  byte yh = ReadDevRegister(LSM303_ADDRESS_ACC,0x2B);
  byte yl = ReadDevRegister(LSM303_ADDRESS_ACC,0x2A);
  byte zh = ReadDevRegister(LSM303_ADDRESS_ACC,0x2D);
  byte zl = ReadDevRegister(LSM303_ADDRESS_ACC,0x2C);
  
  // need to convert the register contents into a righ-justified 16 bit value
  Acc[0] = (xh<<8|xl); 
  Acc[1] = (yh<<8|yl); 
  Acc[2] = (zh<<8|zl); 

}  

//******************************************************************
//Read & Print Eprom Calibration Values
//******************************************************************
void readPrintEpromValues(){
           //
           // Now Read Back all the Values
           //
           XAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_X_NEG_1G);	
           YAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_Y_NEG_1G);	
           ZAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_Z_NEG_1G);	
           XAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_X_POS_1G);	
           YAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_Y_POS_1G);	
           ZAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_Z_POS_1G);	

  
          Serial.println("****************   Accelerometer Calibration Values *********************"); 
          Serial.print(F("EProm X Neg Axis 1g Calibration: "));
          Serial.println(XAxisNegative1gCalibration);
          Serial.print(F("EProm Y Neg Axis 1g Calibration: "));
          Serial.println(YAxisNegative1gCalibration);
          Serial.print(F("EProm Z Neg Axis 1g Calibration: "));
          Serial.println(ZAxisNegative1gCalibration);
          Serial.print(F("EProm X Pos Axis 1g Calibration: "));
          Serial.println(XAxisPositive1gCalibration);
          Serial.print(F("EProm Y Pos Axis 1g Calibration: "));
          Serial.println(YAxisPositive1gCalibration);
          Serial.print(F("EProm Z Pos Axis 1g Calibration: "));
          Serial.println(ZAxisPositive1gCalibration);
          Serial.println("*************************************************************************"); 
}
//*************************************************************************************
//
//
//    calibrateAccelerometer()
//
//
//************************************************************************************
void calibrateAccelerometer(){

    float Acc[3];
    float prevAcc[3];
    
     
    int XAxis;
    int YAxis;
    int ZAxis;

    int button;

    //
    // Min Max accumulation variables
    //
    int XAxis_min = 0;
    int XAxis_max = 0;
    int YAxis_min = 0;
    int YAxis_max = 0;
    int ZAxis_min = 0;
    int ZAxis_max = 0; 
    //
    // Initialize Min Max accumulation variables
    //
    XAxis_min = 0;
    XAxis_max = 0;
    YAxis_min = 0;
    YAxis_max = 0;
    ZAxis_min = 0;
    ZAxis_max = 0;

    /*
     * Intialize to Zero
     */ 
    for(int i=0; i<3 ; i++){
        Acc[i] = 0;
    }
                              
        // Turn off Lights while setting LEDs
        setLights(SportLight,LOW);
        setLights(TourLight,LOW);

        Serial.println(F("********************* Calibration Routine ****************************"));
        Serial.println(" o  Press Once to see EEprom Calibration & Threshold values "); 
        Serial.println(" o  Press Twice To Exit Calibration Mode "); 
        Serial.println(" o  Press Three Times To Begin Calibration Mode "); 
        Serial.println("    During Calibration - Press Once to Save Calibration Values & Exit Calibration Routine"); 
        Serial.println(F("**********************************************************************"));
		          	
        /*
	 * Stay in this loop until button is pressed multple times to indicate save
	 */	
	while(1){ 
		
		//read the selector PUSHBUTTON or Rotary and see if they have it in DIAGMODE
	        button = readPushButtonSwitch();
                switch(button){

	            case NOCHANGE:
	            break;

                    case 1:
		          //
		          // Ok they want to print current calibration & threshold values
		          //
			  //
		          Serial.println(F("************************ Thresholds **********************************"));
                          Serial.print(F("Medium Threshold:  "));
                          Serial.println( mediumThreshold);
                          Serial.print(F("Hard Threshold:  "));
                          Serial.println( hardThreshold );
		          Serial.println(F("**********************************************************************"));
			  readPrintEpromValues();
                          Serial.println(" o  Press Once to see EEprom Calibration values "); 
                          Serial.println(" o  Press Twice To Exit Calibration Mode "); 
                          Serial.println(" o  Press Three Times To Begin Calibration Mode "); 
                          Serial.println("    During Calibration - Press Once to Save Calibration Values & Exit Calibration Routine"); 

                          break;



                    case 2:
		          //
		          // Ok they want to Exit 
		          //
			  
                          Serial.println(F("********************* Exiting Calibration Routine ****************************"));
                          Serial.println(F("*********************     NO Save Performed       ****************************"));
		          //
		          // Blink both lights at together (3 times) to indicate exiting calibration
		          //
                          blinkBothLights(3,500,1,0,1);
	                 
	                  // Restore the previous state of the controller
	                  //
		          myController.setMode(previousMode);
			  return;
			  break;

                    case 3:
		          //
		          // Ok they want to Calibrate Accelerometer 
		          //
                          
			  while(1){
				 
			        button = readPushButtonSwitch();
			        if (button != NOCHANGE) break;
				
                                for(int i=0; i<3 ; i++){
		                        prevAcc[i] = Acc[i];
	                        }
                                delay(100);
                                getRawAccelerometer(Acc);

	                        XAxis =Acc[0];
	                        YAxis =Acc[1];
	                        ZAxis =Acc[2];
	                         //
	                         // Check to make sure previous value = current value
	                         // to weed out noise from acc.
	                         //
	                         if(prevAcc[0] == Acc[0]){    
	                           if(Acc[0] < XAxis_min) XAxis_min=XAxis;
	                           if(Acc[0] > XAxis_max) XAxis_max=XAxis;
	                         }

	                         if(prevAcc[1] == Acc[1]){    
	                           if(Acc[1] < YAxis_min) YAxis_min=YAxis;
	                           if(Acc[1] > YAxis_max) YAxis_max=YAxis;
	                         }

	                         if(prevAcc[2] == Acc[2]){    
	                           if(Acc[2] < ZAxis_min) ZAxis_min=ZAxis;
	                           if(Acc[2] > ZAxis_max) ZAxis_max=ZAxis;
	                         }

/*
                                 Serial.print(" X ");
                                 Serial.print(XAxis );  

                                 Serial.print(" Y ");
                                 Serial.print(YAxis );  

                                 Serial.print(" Z ");
                                 Serial.println(ZAxis );  
*/
                                 Serial.print("MIN X=");
                                 Serial.print(XAxis_min);  
                                 Serial.print(" ");
                                 Serial.print("Y=");
                                 Serial.print(YAxis_min);  
                                 Serial.print(" ");
                                 Serial.print("Z=");
                                 Serial.print(ZAxis_min);  

                                 Serial.print("  MAX X=");
                                 Serial.print(XAxis_max);  
                                 Serial.print(" ");
                                 Serial.print("Y=");
                                 Serial.print(YAxis_max);  
                                 Serial.print(" ");
                                 Serial.print("Z=");
                                 Serial.println(ZAxis_max);  

			  }

                          // 
			  // ok their done calibrtaing save values
			  //
                          XAxis_min = abs(XAxis_min) ;
                          eepromWriteInt(EPROM_ACC_X_NEG_1G , XAxis_min);

                          YAxis_min = abs(YAxis_min) ;
                          eepromWriteInt(EPROM_ACC_Y_NEG_1G , YAxis_min);

                          ZAxis_min = abs(ZAxis_min) ;
                          eepromWriteInt(EPROM_ACC_Z_NEG_1G , ZAxis_min);

                          eepromWriteInt(EPROM_ACC_X_POS_1G , XAxis_max);
                          eepromWriteInt(EPROM_ACC_Y_POS_1G , YAxis_max);
                          eepromWriteInt(EPROM_ACC_Z_POS_1G , ZAxis_max);



                          //
                          // Now Read Back & Print all the Values
                          //
                          Serial.println(F("************************** Thresholds *****************************"));
                          Serial.print(F("Medium Threshold:  "));
                          Serial.println( mediumThreshold);
                          Serial.print(F("Hard Threshold:  "));
                          Serial.println( hardThreshold );
                          Serial.println(F("**********************************************************************"));

                          readPrintEpromValues(); 

                          Serial.println(F("New Threshold Values have been saved - exiting calibration routine"));

		          //
		          // Blink both lights at together (8 times) to indicate saving calibrations & exiting.
		          //
                          blinkBothLights(8,500,1,0,1);

	                  // Restore the previous state of the controller
	                  //
		          myController.setMode(previousMode);
			  return;
			  break;
		  

		}
	}
}