User Arduino code

From Torque Wiki
⧼monobook-jumptonavigation⧽⧼monobook-jumptosearch⧽

Below is a user contribution to the arduino code - from this forum thread: https://torque-bhp.com/forums/?wpforumaction=viewtopic&t=7737.0

Paste the code into the Arduino IDE and then upload to your arduino when you've finished building!


#include <avr/pgmspace.h>
#include <SoftwareSerial.h>
#include <String.h>

// ******INPUTS******
      int MAPsensor = A0;                      //inputpin for the MAP sensor
      int TPSsensor = A1;                      //input pin for the TPS sensor
      int AFRsensor = A2;                      //input pin for the AFR sensor
      int RPMsensor = 21;                      //input pin for the RPM sensor. Pin 21 is tied to interrupt#2
      int Temps = 0;                           //input pin for the temperature sensor(s)
      
// ******OUTPUTS******
      int Injector = 2;                        //injector connected to digital pin 2

// ******VARIABLE DECLARATION******
      volatile bool Running = 0;               //is the motor running or not
      volatile int FireInjector = 0;           //set when the injector should be fired
      volatile float RPMcount;                 //number of triggers on the RPM sensor

      volatile float RPMValue = 2453;          //calculated RPM from interrupt #2. Declared volatile so it is loaded from RAM
      float OldRPMValue = 0;                   //keep track of what the previous rpm value was, for time rollover purposes
      float MAPsensorValue = 0;                //value coming from the MAP sensor
      float TPSsensorValue = 0;                //value coming from the TPS sensor
      float AFRsensorValue = 0;                //value coming from the AFR sensor
      float RPMpercent = 0;                    //percent of 8000rpm
      float MAPsensorpercent = 0;              //percent output from the MAP sensor
      float TPSsensorpercent = 0;              //percent output from the TPS sensor
      float AFRsensorpercent = 0;              //percent output from the AFR sensor
      float MaxPulsewidth = 0;                 //maximum anount of time for injector to be ON
      float InjectorONtime = 0;                //amount of time the injectors are commanded ON (msec)
      float Toothcount = 1;                    //number of triggers per one flywheel revolution
      float InjectorOndelay = 1.5;             //how long it takes the injector to open(msec)
      float InjectorOffdelay = 0.5;            //how long it takes the injector to close(msec)
      float CrankingPulsewidth =60.0;          //how long to fire the injector during starting(msec)
      float AFR = 0;                           //actual AFR reading from AFR display   
      float CubicInches = 30;
      float SCFM = 0;                          //Standard Cubic Feet Per Minute Air Flow
      float AFRRAW = 0;
      float TimePerCycle = (60/RPMValue)*2;
      float VolumetricEfficiency = 0.5;        //Volumetric Eficiency of the motor
      float RequiredCFMAir = 0;                //Requird CFM of Air = SCFM * VolumetricEfficiency
      float LbsAirMin = 0;                     //Pounds of Air Flow per Minute
      float LbsFuelMin = 0;                    //Pounds of Fuel Required per Minute
      float LbsAirCycle = 0;                   //Pounds of Air Flow per cycle
      float LbsFuelCycle = 0;                  //Pounds of Fuel Required per cycle
      float InjectorFlowRate = 51;             //Pounds per Hours rating of the injector
      int AFRdiff = 0;                         //voltage difference from target
      int MAPLookup = 0;                       //which array element to use for MAP range
      int TPSLookup = 0;                       //which array element to use for TPS range
      int dTPSLookup = 0;
      int RPMLookup = 0;                       //which array element to use for RPM range
      int IATLookup = 0;                       //which array element to use for IAT range
      int x = 0;
      float TPSMapvalue = 0;                   //the value returned from the TPS fuel map
      float MAPMapvalue = 0;                   //the value returned from the MAP fuel map
      float AFRtarget = 14;                    //the value returned from the TPS fuel map
      float LastTPS = 0;
      float dTPSpercent = 0;
      float dTPS = 0;
      bool InjectionComplete = 1;
      unsigned long timeold;                   //the last time RPM was calculated
      const String ATE = "ATE";                //Echo off/on
      const String ATI = "ATI";                //Version id
      const String ATZ = "ATZ";                //Reset
      const String ATS = "ATS";                //Set protocol X
      const String ATH = "ATH";                //Headers off / on
      const String ATL = "ATL";                //Linefeeds off/on
      const String ATM = "ATM";                //Memory off/on
      const String GETDEFINITIONS = "GETDEFINITIONS"; //Get sensor definitions
      const String GETCONFIGURATION = "GETCONFIGURATION"; //Get config of app (hide car sensors, devices sensors, etc)
      const String GETSENSORS = "G";           //Get sensor values, one shot.
      const String SETSENSOR = "S";            //Set a sensor value
      const String PROMPT = ">";
      const String CANBUS = "6";               //canbus 500k 11 bit protocol id for elm.
      const String ATDPN = "ATDPN";
      const String ATDESC = "AT@1";
      const String ATAT = "ATAT";
      const String LF = "\n";
      const String VERSION = "Torque Protocol Interface v0.0.1"; //Don't change this - it's used by Torque so it knows what interface it is connected to
      const String VERSION_DESC = "Torque For Android Protocol Interface";
      const String OK = "OK";
      const String ANALOG = "a";
      const String DIGITAL = "d";
      const String IS_INPUT = "i";
      const String IS_OUTPUT = "o";
      String fromTorque = "";
      int test=75;

/**
 * Array of sensors we will advertise to Torque so it can automatically import them. Using strings
 * Stucture is:
 *  
 *  Arduino Pin, Arduino pin type, Input/Ouput, Default value(if output), ShortName, Long name, units, minimum value, maximum value
 *  
 *  Caveats:  Don't use a '>' in any of the names, 
 *            Update 'sensorsSize' with the number of elements.
 *            Analog outputs are PWM on digital pins.
 *  
 */
const int SENSORSSIZE = 9 * 8; // each line is 9 attributes, and we have 8 lines.
const String sensors[SENSORSSIZE] = {
                    "1",  ANALOG, IS_INPUT,  "0",    "Test1",  "Arduino1",  "Pot1",  "0",  "100",
                    "2",  ANALOG, IS_INPUT,  "512",  "MAP",    "Arduino2",  "MAP",   "0",  "1023",
                    "3",  ANALOG, IS_INPUT,  "512",  "TPS",    "Arduino3",  "TPS",   "0",  "1023",
                    "4",  ANALOG, IS_INPUT,  "1500", "RPM",    "Arduino4",  "RPM",   "0",  "8000",
                    "5",  ANALOG, IS_INPUT,  "14.7", "AFR",    "Arduino5",  "AFR",   "8",  "22",
                    "6",  ANALOG, IS_INPUT,  "50",   "MAP%",   "Arduino6",  "MAP%",  "0",  "100",
                    "7",  ANALOG, IS_INPUT,  "50",   "TPS%",   "Arduino7",  "TPS%",  "0",  "100",
                    "8",  ANALOG, IS_INPUT,  "50",   "AFR%",   "Arduino8",  "AFR%",  "0",  "100"
                   }; 
/**
 * Configuration directives for the app to hide various things. Comma separated. Remove to enable visibility in Torque
 *  - handy if your project isn't car related or you want to make sensor selections relatively easy.
 *  
 *  Supported types:
 *    NO_CAR_SENSORS  - hide any car related sensors
 *    NO_DEVICE_SENSORS - hide any device (phone) sensors
 *    
 */
const String CONFIGURATION = "NO_CAR_SENSORS,NO_DEVICE_SENSORS"; 

// Setup bluetooth module on pins 10 and 11 on Arduino Mega 2560(you can't use these digial pins in the sensor list or it'll break comms)
SoftwareSerial mySerial(10,11); // Most other boards can use pins 2 and 3

//*************Correction Tables********************

const float VETable [20][20] PROGMEM ={ 
/*                   5% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  10% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  15% */  {0.43,  0.43,  0.43,  0.43,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  20% */  {0.43,  0.43,  0.43,  0.40,  0.40,  0.40,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  25% */  {0.45,  0.45,  0.40,  0.40,  0.40,  0.45,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  30% */  {0.50,  0.50,  0.40,  0.40,  0.45,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  35% */  {0.50,  0.50,  0.40,  0.45,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  40% */  {0.50,  0.50,  0.45,  0.45,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  45% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*       TPS        50% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*    Throttle      55% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*    Position      60% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  65% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  70% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  75% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  80% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  85% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  90% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                  95% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  
/*                 100% */  {0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50,  0.50},  };
//                          ===========================================================================================================================================
//                             4      8      1      1      2      2      2      3      3      4      4      4      5      5      6      6      6      7      7      8
//                             0      0      2      6      0      4      8      2      6      0      4      8      2      6      0      4      8      2      6      0
//                             0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0
//                                           0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0
                                                                                                    /* RPM */


const float dTPSTable [20][20] PROGMEM ={ 
/*                   5% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  10% */  {1.0,  1.0,  20.0,  20.0,  20.0,  20.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  15% */  {1.0,  1.0,  20.0,  20.0,  20.0,  20.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  20% */  {1.0,  1.0,  20.0,  20.0,  20.0,  20.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  25% */  {1.0,  1.0,  20.0,  20.0,  20.0,  20.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  30% */  {1.0,  1.0,  20.0,  20.0,  20.0,  20.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  35% */  {1.0,  1.0,  20.0,  20.0,  20.0,  20.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  40% */  {1.0,  1.0,  20.0,  20.0,  20.0,  20.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*      dTPS        45% */  {1.0,  1.0,  20.0,  20.0,  20.0,  20.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*      Delta       50% */  {1.0,  1.0,  20.0,  20.0,  20.0,  20.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*     Throttle     55% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*     Position     60% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  65% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  70% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  75% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  80% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  85% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  90% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                  95% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},
/*                 100% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,},  };
//                          ========================================================================================================================
//                             4     8     1     1     2     2     2     3     3     4     4     4     5     5     6     6     6     7     7     8
//                             0     0     2     6     0     4     8     2     6     0     4     8     2     6     0     4     8     2     6     0
//                             0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0
//                                         0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0
                                                                                          /* RPM */


const float MAPTable [20][20] PROGMEM ={ 
/*                   5% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  10% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  15% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  20% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  25% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  30% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  35% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  40% */  {1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  45% */  {1.0,  1.0,  8.0,  7.0,  6.0,  5.0,  4.0,  3.0,  2.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*       MAP        50% */  {1.0,  1.0,  8.0,  7.0,  6.0,  5.0,  4.0,  3.0,  2.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*     Percent      55% */  {1.0,  1.0,  8.0,  7.0,  6.0,  5.0,  4.0,  3.0,  2.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*     Feedback     60% */  {1.0,  1.0,  8.0,  7.0,  6.0,  5.0,  4.0,  3.0,  2.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  65% */  {1.0,  1.0,  8.0,  7.0,  6.0,  5.0,  4.0,  3.0,  2.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  70% */  {1.0,  1.0,  8.0,  7.0,  6.0,  5.0,  4.0,  3.0,  2.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  75% */  {1.0,  1.0,  8.0,  7.0,  6.0,  5.0,  4.0,  3.0,  2.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  80% */  {1.0,  1.0,  8.0,  7.0,  6.0,  5.0,  4.0,  3.0,  2.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  85% */  {1.0,  1.0,  8.0,  7.0,  6.0,  5.0,  4.0,  3.0,  2.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  90% */  {1.0,  1.0,  8.0,  7.0,  6.0,  5.0,  4.0,  3.0,  2.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                  95% */  {1.0,  1.0,  8.0,  7.0,  6.0,  5.0,  4.0,  3.0,  2.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},
/*                 100% */  {1.0,  1.0,  8.0,  7.0,  6.0,  5.0,  4.0,  3.0,  2.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0},  };
//                          ========================================================================================================================
//                             4     8     1     1     2     2     2     3     3     4     4     4     5     5     6     6     6     7     7     8
//                             0     0     2     6     0     4     8     2     6     0     4     8     2     6     0     4     8     2     6     0
//                             0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0
//                                         0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0
                                                                                          /* RPM */



const float AFRTable [20][20] PROGMEM ={ 
/*                   5% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  10% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  15% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  20% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  25% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  30% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  35% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  40% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  45% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*       MAP        50% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*     Percent      55% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*     Feedback     60% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  65% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  70% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  75% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  80% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  85% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  90% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                  95% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},
/*                 100% */  {14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,  14.0,},  };
//                          =============================================================================================================================================
//                              4      8      1      1      2      2      2      3      3      4      4      4      5      5      6      6      6      7      7      8
//                              0      0      2      6      0      4      8      2      6      0      4      8      2      6      0      4      8      2      6      0
//                              0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0
//                                            0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0      0
                                                                                                     /* RPM */


const float IATTable [20] PROGMEM ={0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2,  0.2};
//                                 ========================================================================================================================
//                                   50°   60°   70°   80°   90°  100°  110°  120°  130°  140°  150°  160°  170°  180°  190°  200°  210°  220°  230°  240°
                                                                                          /* IAT */


// ******SETUP - RUNS ONE TIME ON POWERUP******
void setup() {
      Serial.begin(115200);
      pinMode(RPMsensor, INPUT);
      pinMode(Temps, INPUT);
      pinMode(Injector, OUTPUT);
      attachInterrupt(2, RPMTRIGGER, RISING);             //call interrupt #2 when the tach input goes high

    // Init the pins 
    initSensors();
    Serial.begin(9600);    // the GPRS baud rate 
    delay(600);
    mySerial.begin(9600); 
             }

// ******MAIN PROGRAM LOOP - RUNS CONTINUOUSLY******
void loop() {

//This is used for testing the Torque App  
  if (test > 100){
    test = 0;
    delay(300);}
   
  delay(25); 
    if (test <= 100){
  test=test+1;}

//Grab data from the bluetooth module, parse it.
  if (mySerial.available()) {
     char c = mySerial.read(); 
     if ((c == '\n' || c == '\r') && fromTorque.length() > 0) {
        fromTorque.toUpperCase();
        processCommand(fromTorque);
        fromTorque = "";
     } else if (c != ' ' && c != '\n' && c !='\r') {
         //Ignore spaces.
         fromTorque += c; 
     }
  }
            
//read the value from the sensors:
      MAPsensorValue = analogRead(MAPsensor);
      TPSsensorValue = analogRead(TPSsensor);
      AFRsensorValue = analogRead(AFRsensor);
      
      MAPsensorpercent = (MAPsensorValue/1023)*100;       // percent output from the MAP sensor
      TPSsensorpercent = ((TPSsensorValue/1023)*100);     // percent output from the TPS sensor
      AFRsensorpercent = (AFRsensorValue/1023)*100;       // percent output from the AFR sensor

      AFRRAW = map(AFRsensorValue, 0, 1023, 735, 2239);   // actual AFR reading from AFR display
      AFR = AFRRAW/100;
      RPMpercent = (RPMValue/8000)*100;                   // percent of 8000rpm. A number that can't be reached.
      MaxPulsewidth = ((((60/RPMValue)*2)*1000)*0.85);    // maximum allowable pulse width(msec)

      SCFM = (CubicInches * RPMValue) / 3456;             //Standard Calculation for Engine CFM
      RequiredCFMAir = SCFM * VolumetricEfficiency;       //Corrected Calculation for Engine CFM x VE
      LbsAirMin = RequiredCFMAir * 0.0765;                //Pounds of Air Required per Minute
      LbsFuelMin = LbsAirMin / AFRtarget ;                //Pounds of Fuel Required per Minute
      LbsFuelCycle = (LbsFuelMin * (60 / RPMValue) * 2);  //Pounds of Fuel Required per cycle
 
//Acceleration Enrichment
   if (TPSsensorpercent > LastTPS)
     dTPSpercent = (TPSsensorpercent - LastTPS);
      if (InjectionComplete == 1){
        x = x + 1;
        if (x >= 1000){
          x = 0;
        LastTPS = TPSsensorpercent;
        InjectionComplete = 0;}}

//This determines which TABLE value to use for dTPS        
     dTPSLookup = (int)(dTPSpercent / 5);
      if (dTPSLookup > 19)
        dTPSLookup = 19;
        
//This determines which TABLE value to use for RPM
      RPMLookup = (int)(RPMpercent / 5);
      if (RPMLookup > 19)
        RPMLookup = 19;
        
//This determines which TABLE value to use for TPS
      TPSLookup = (int)(TPSsensorpercent / 5);
      if (TPSLookup > 19)
        TPSLookup = 19;
        
//This determines which TABLE value to use for MAP
      MAPLookup = (int)(MAPsensorpercent / 5);
      if (MAPLookup > 19)
        MAPLookup = 19;
        
//calculate the injector ON time
//sense the % throttle position because it is what the drivers wants to see happen.
//calculate % injector time based on that
//lookup in VEtable for correction for current VE range at current RPM
//lookup in MAPtable for correction for current MAP range at current RPM
//then adjust for AFRsensorValue - TBD
//multiply by MaxPulsewidth to get a percentage of MaxPulsewidth
//limit InjectorONtime to MaxPulsewidth
//factor in the InjectorOndelay and InjectorOffdelay

//the arrays are stored in FLASH memory
      VolumetricEfficiency = pgm_read_float(&VETable[TPSLookup][RPMLookup]);
      TPSMapvalue = pgm_read_float(&VETable[TPSLookup][RPMLookup]);
      MAPMapvalue = pgm_read_float(&MAPTable[MAPLookup][RPMLookup]);
      AFRtarget = pgm_read_float(&AFRTable[MAPLookup][RPMLookup]);
      dTPS = pgm_read_float(&dTPSTable[dTPSLookup][RPMLookup]);

      InjectorONtime = (LbsFuelMin / (InjectorFlowRate / 60)) * (((60/RPMValue)*2)*1000);     //Injector ON Time
        InjectorONtime = InjectorONtime * MAPMapvalue;                                        //calculate correction for MAP vs RPM
          InjectorONtime = InjectorONtime * dTPS;                                             //calculate correction for dTPS vs RPM
            if (InjectorONtime > MaxPulsewidth)
              InjectorONtime = MaxPulsewidth;                                                 //limit the InjectorONtime to MaxPulsewidth

//is the engine running or being started?
     if (RPMValue > 50)
       (Running = 1); 
     else 
       (Running = 0); 
         
//this is for when the engine is cranking during start         
      if (RPMValue < 550)
        (InjectorONtime = CrankingPulsewidth);
        if (RPMValue < 50) 
          (InjectorONtime = 0);

      if (Running == 1) {
        if (FireInjector >= 1){
          if (InjectorONtime > 0) {
            digitalWrite(Injector, HIGH);
              if (InjectorONtime < 15) {  
               delayMicroseconds(InjectorONtime * 1000); }
              else {
                delay(InjectorONtime); 
                                   }
        FireInjector = 0;
        InjectionComplete = 1;  }
                        }
      }
// wait to finish injection period   
      digitalWrite(Injector, LOW);
     }

//Parse the commands sent from Torque
void processCommand(String command) {
 
   // Debug - see what torque is sending on your serial monitor
   Serial.println(command);
 
   // Simple command processing from the app to the arduino..
   if (command.equals(ATZ)) {
       initSensors(); // reset the pins
       mySerial.print(VERSION);
       mySerial.print(LF); 
       mySerial.print(OK);
   } else if (command.startsWith(ATE)) {
       mySerial.print(OK); 
   } else if(command.startsWith(ATI)) {
       mySerial.print(VERSION);
       mySerial.print(LF);
       mySerial.print(OK);
   } else if (command.startsWith(ATDESC)) {
       mySerial.print(VERSION_DESC); 
       mySerial.print(LF);
       mySerial.print(OK);
   } else if (command.startsWith(ATL)) {
       mySerial.print(OK);
   } else if (command.startsWith(ATAT)) {
       mySerial.print(OK);
   } else if (command.startsWith(ATH)) {
       mySerial.print(OK);
   } else if (command.startsWith(ATM)) {
       mySerial.print(OK);
   } else if (command.startsWith(ATS)) {
       // Set protocol
       mySerial.print(OK);
   } else if (command.startsWith(ATDPN)) {
       mySerial.print(CANBUS);
   } else if (command.startsWith(GETDEFINITIONS)) {
       showSensorDefinitions();
   }  else if (command.startsWith(SETSENSOR)) {
       setSensorValue(command);
   } else if (command.startsWith(GETSENSORS)) {
       getSensorValues();
   } else if (command.startsWith(GETCONFIGURATION)) {
       getConfiguration();
   }
 
   mySerial.print(LF); 
   mySerial.print(PROMPT);
}

//List all the sensors to the app
void showSensorDefinitions() {
   int id = 0;
   for (int i = 0; i < SENSORSSIZE/9; i++) {
      for (int j = 0; j < 9; j++) {
         id = (i*9)+j;
         mySerial.print(sensors[id]);
 
         if (id+1 < SENSORSSIZE) {
            mySerial.print(',');
         }
      }
      mySerial.print(LF);
   }
}
 
//Dump sensor information for input sensors.
//Format to Torque is id:type:value
void getSensorValues() {
   for (int i = 0; i < SENSORSSIZE/9; i++) {
      int group = i * 9;
      int id = sensors[group].toInt();
      String type = sensors[group+1];
      boolean isOutput = sensors[group+2].equals(IS_OUTPUT);
      if (!isOutput) {
        if (id == 1){
         mySerial.print(id);
         mySerial.print(":");
         mySerial.print(type);
         mySerial.print(":");
         if (type.equals(ANALOG)) {
           mySerial.print(test);}
         else if (type.equals(DIGITAL)) {
           mySerial.print(digitalRead(id));}
      }
   }

      if (!isOutput) {
        if (id == 2){
         mySerial.print(id);
         mySerial.print(":");
         mySerial.print(type);
         mySerial.print(":");
         if (type.equals(ANALOG)) { 
           mySerial.print((MAPsensorValue));}
         else if (type.equals(DIGITAL)) {
           mySerial.print(digitalRead(id));}
      }
   }

      if (!isOutput) {
        if (id == 3){ 
         mySerial.print(id);
         mySerial.print(":");
         mySerial.print(type);
         mySerial.print(":");
         if (type.equals(ANALOG)){
           mySerial.print(TPSsensorValue);}
         else if (type.equals(DIGITAL)) {
           mySerial.print(digitalRead(id));}
      }
   }
   
      if (!isOutput) {
        if (id == 4){ 
         mySerial.print(id);
         mySerial.print(":");
         mySerial.print(type);
         mySerial.print(":");
         if (type.equals(ANALOG)){
           mySerial.print(RPMValue);}
         else if (type.equals(DIGITAL)) {
           mySerial.print(digitalRead(id));}
      }
   }

      if (!isOutput) {
        if (id == 5){ 
         mySerial.print(id);
         mySerial.print(":");
         mySerial.print(type);
         mySerial.print(":");
         if (type.equals(ANALOG)){
           mySerial.print(AFR);}
         else if (type.equals(DIGITAL)) {
           mySerial.print(digitalRead(id));}
      }
  }

      if (!isOutput) {
        if (id == 6){ 
         mySerial.print(id);
         mySerial.print(":");
         mySerial.print(type);
         mySerial.print(":");
         if (type.equals(ANALOG)){
           mySerial.print(MAPsensorpercent);}
         else if (type.equals(DIGITAL)) {
           mySerial.print(digitalRead(id));}
      }
  }
  
      if (!isOutput) {
        if (id == 7){ 
         mySerial.print(id);
         mySerial.print(":");
         mySerial.print(type);
         mySerial.print(":");
         if (type.equals(ANALOG)){
           mySerial.print(TPSsensorpercent);}
         else if (type.equals(DIGITAL)) {
           mySerial.print(digitalRead(id));}
      }
  }
  
      if (!isOutput) {
        if (id == 8){ 
         mySerial.print(id);
         mySerial.print(":");
         mySerial.print(type);
         mySerial.print(":");
         if (type.equals(ANALOG)){
           mySerial.print(AFRsensorpercent);}
         else if (type.equals(DIGITAL)) {
           mySerial.print(digitalRead(id));}
      }
  }
  
            mySerial.print('\n');
   }
}

//Sets a sensors value
void setSensorValue(String command) {
  int index = command.indexOf(":");
  int id = command.substring(1,index).toInt();
  int value = command.substring(index+1, command.length()).toInt();
  for (int i = 0; i < SENSORSSIZE/9; i++) {
     int group = i * 9;
     int sid = sensors[group].toInt();
     boolean isOutput = sensors[group+2].equals(IS_OUTPUT);
     if (!isOutput) {
       if (sid == id) {
          String type = sensors[group+1];
          if (type.equals(ANALOG)) {
            analogWrite(sid, constrain(value,0,255));
          } else if (type.equals(DIGITAL)) {
            digitalWrite(sid, value > 0 ? HIGH: LOW);
          }
          break;
       }
    }
  }
}
 
//Init the sensor definitions (input/output, default output states, etc)
void initSensors() {
   for (int i = 0; i < SENSORSSIZE/9; i++) {
      int group = i * 9;
      int id = sensors[group].toInt();
      String type = sensors[group+1];
      boolean isOutput = sensors[group+2].equals(IS_OUTPUT);
      int defaultValue = sensors[group+3].toInt();
 
      if (isOutput) {
         if (type.equals(ANALOG)) {
             pinMode(id, OUTPUT);
             analogWrite(id, constrain(defaultValue, 0, 255));

         } else if (type.equals(DIGITAL)) {
             pinMode(id, OUTPUT);
             digitalWrite(id, defaultValue > 0 ? HIGH : LOW);
         }
      }
   }
}
 
void getConfiguration() {
  mySerial.print(CONFIGURATION);
}
     
// ******INTERRUPT #2 - GETS CALLED WHEN RPM SENSOR IS TRIGGERED******
void RPMTRIGGER() {
      RPMcount = RPMcount + 1;              //Increment RPMcount every time the sensor triggers. Return the new value
      FireInjector = FireInjector + 1;      //set when the injector should be fired(every other cycle)
      if (RPMcount >= Toothcount) {
        if ((micros() - timeold) < 0) {
          RPMValue = OldRPMValue; }         //this handles the rollover of the microsecond clock
        else {
          RPMValue = (60000000/(micros() - timeold)*RPMcount)/Toothcount; }
          
      timeold = micros();                   //store the time that the RPM sensor was triggered
      RPMcount = 0;                         //reset the RPMcount to zero
      OldRPMValue = RPMValue;
                                  }
                  }