Power Racing Series Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.

<code>

/**
  ESC controller software for PPPRS.  Runs on a teensy 3.1/3.2 at 3.3V and 96MHz

*/

//INCLUDES
//these are standard teensy libraries
#include <ADC.h>
#include <SD.h>
#include <EEPROM.h>
#include <Wire.h>
#include <FreqMeasureMulti.h>
//This is a pre-made library from sparkfun
#include <SparkFunLSM6DS3.h>
//This is a slight modification of Arduino's built-in servo library
#include <PulseServo.h>
//my custom PulseServo library sends out a pulse whenever I write to it, not on a predefined interval like the normal servo library
//it still has a predefined interval, but that is set artificially long so that anytime I write a new value to the servo it immediately sends out a pulse of that length
//removes the variable time delay of not knowing whether you just missed the boat on a servo pulse, I now send them out completely on-demand
//Yay for latency removal!
#define HWSERIAL Serial1

#define DEBUGMODE  false
float debuggedvalue = 0;
#define BLUETOOTH  false

//
//--------------------PIN DEFINITIONS
//
//communications
#define CPIN_SERIALRX     0
#define CPIN_SERIALTX     1
#define CPIN_SPISS        10
#define CPIN_SPIMOSI      11
#define CPIN_SPIMISO      12
#define CPIN_SPISCK       13
#define CPIN_I2CSDA       18
#define CPIN_I2CSCL       19
//digital
#define DPIN_BRAKE        2
#define DPIN_LED          3
#define DPIN_SPEED        5
#define DPIN_ESCOUT       6
#define DPIN_REVERSE      7
#define DPIN_ACC          9
//analog
#define APIN_ISHUNT       A0
#define APIN_IFUSE        A1
#define APIN_PACKV        A2
#define APIN_THROTTLE     A3
#define APIN_STEERING     A6

#define APIN_MOTORTEMP    A7
#define APIN_GEARTEMP     A8
#define APIN_FUSETEMP     A9
#define APIN_COOLANTTEMP  A10
#define APIN_BRAKETEMP    A11


#define APIN_ECUTEMP      38

#define AVAILABLE_1       5
#define AVAILABLE_2       A12 //Available if pin is bridged to digital pin 5
#define AVAILABLE_3       9
#define AVAILABLE_4       A15 //Available if pin is bridged to digital pin 9
#define AVAILABLE_5       A14

//----------------------------LOOP AND SAMPLE TIMING--------------------

#define SERVOMIN              1000              //LOOP AND SAMPLE TIMING  //absolute minimum value writable to the servo
#define SERVOMAX              2000              //LOOP AND SAMPLE TIMING  //absolute maximum value writable to the servo
#define MINPERIOD             2100   //LOOP AND SAMPLE TIMING  //minimum time between sending out pulses.  this enures we do not send out a new pulse before one is finished.
unsigned long prevmicros =    0;                //LOOP AND SAMPLE TIMING  //the last time we SHOULD HAVE completed a loop
unsigned long prevperiod =    0;                //LOOP AND SAMPLE TIMING  //This is the last time we ACTUALLY completed a loop
unsigned long prevmillis =    0;                //millis the last time we completed a loop
unsigned long prevsample =    0;                //the millis duration of the last sampling period.  used to calculate variable I2P


//--------------------------------THROTTLE INPUT AND OUTPUT----------------------------

#define THROTTLEMIN               530                       //THROTTLE INPUT AND OUTPUT  //MAX THROTTLE.  THROTTLE IS REVERSED FOR SAFETY WHEN DISCONNECTED.  Minimum ADC value that will be mapped for throttle purposes
#define THROTTLEMAX               950                      //THROTTLE INPUT AND OUTPUT  //MIN THROTTLE.  THROTTLE IS REVERSED FOR SAFETY WHEN DISCONNECTED.  Maximum ADC value that will be mapped for throttle purposes
unsigned int THROTTLERANGE =     THROTTLEMAX - THROTTLEMIN; //THROTTLE INPUT AND OUTPUT  //the total ADC counts difference in our min and max throttle readings
#define SERVOZERO                 1500                      //THROTTLE INPUT AND OUTPUT  //'Zero' point for servo library.
#define SERVODEAD                 1584                      //THROTTLE INPUT AND OUTPUT  //space between this and the zero band won't be sent because it will cause odd behavior with the ESC
#define LIMPLEVEL                 0.25f                        //THROTTLE INPUT AND OUTPUT  //percentage of full throttle allowed under full limp home mode (low voltage)
#define BRAKEPOWER                SERVOZERO                 //THROTTLE INPUT AND OUTPUT  //amount of negative throttle to apply when we hit the brakes.  right now, it's NOTHING.  Because regen be bad for the fuse!
#define RAMPUPRATE                12                      //THROTTLE INPUT AND OUTPUT  //uS of throttle rampup per 4mS of time.
#define LIMPCUTOFF                200                       //THROTTLE INPUT AND OUTPUT  //if we're below this at idle, we're in maximum limp home mode
#define LIMPHYST                  230                       //THROTTLE INPUT AND OUTPUT  //if we rebound above this, take us out of limp home mode.  anything between these 2 values scales our maximum throttle linearly between normal max and <LIMPLEVEL>
#define DEBOUNCE                  50                        //debounce time for the reverse switch because I'm too lazy to do good wire routing.  also the 'pause' time that ramp up is held at the minimum rate

unsigned long rampinguptime =     0;
unsigned long rampingdowntime =   0;
boolean rampingdown =             false;
boolean rampingup =               false;
boolean needrampup =              false;
boolean reversing =               0;
int traveldirection =             0;
#define GOFORWARD                 1
#define GOREVERSE                 -1
unsigned int limphyst;
unsigned int limpcutoff;
unsigned int limpthrottle;
int throttlepos =               SERVOZERO;    //the throttle we calculate.  Initialize with a throttle output of zero
int refthrottle =               SERVOZERO;
int testramp =                  SERVOZERO;
int lastthrottle =              SERVOZERO;    //the zero point for our servo band

unsigned int rampdownbase =       0; 

boolean limphome =                false;

#define MILLISPERRAMPUP   4
#define RAMPUPTABLESIZE   57   //used for indexing, so the actual number of entries is this plus one
const byte rampuprates_table[] PROGMEM = {1, 1, 1, 1, 1, 2, 2, 2, 3, 3, 4, 4, 5, 6, 7, 8, 9, 10, 11, 13, 15, 17, 19, 21, 23, 25, 27, 29, 31, 33, 36, 39, 42, 45, 48, 51, 54, 58, 62, 66, 70, 75, 80, 85, 90, 95, 101, 107, 113, 120, 127, 134, 142, 150, 159, 168, 178, 188
                                         };

//--------------------------------CURRENT LIMITING----------------

#define TIME1X5                   90000      //CURRENT LIMITING  //time the fuse can tolerate 150% current.  It's a trap, this is less relevant than you think
#define TIME2X                    3000       //CURRENT LIMITING  
#define TIME3X                    300        //CURRENT LIMITING  
#define FULLSCALECURRENT          2860          //CURRENT LIMITING  //current at ADC count 1023, in 100mA increments
#define SHUNTOFFSET               -207
#define FUSEOFFSET                426
#define SHUNTMULTIPLIER           0.1623f
#define FUSEMULTIPLIER            0.1080f
#define SHUNTTHRESHOLD            22            //number of ADC counts to blank once we subtract by our offset.  if we're below this, we call it zero
#define FUSETHRESHOLD             1111          //number of ADC counts to blank once we subtract by our offset.  if we're below this, we call it zero
#define LIPOLIMIT                 295           //CURRENT LIMITING  //WITCHCRAFT! if we boot and our battery voltage is above this limit, assume we're running lipos and we need to reduce our fuse limit to 50A
#define NOVSENSE                  100
#define LIPOFUSE                  500           //CURRENT LIMITING  //this is our base fuse limit on LiPos.
#define LIFEFUSE                  600           //CURRENT LIMITING  //this is our base fuse limit on LiFes
#define REACTIONTIME              25            //the amount of millis we want to look into the future to check for possible overcurrent conditions
#define BUCKET3XSPEED             30            //don't allow access to the bucket 3x limit unless we're going this fast

unsigned int currentbase;                      //CURRENT LIMITING  //Numerical current for nominal limit
long current1x1;                       //CURRENT LIMITING  //is cooling off and we will 'harvest' I2P points back into our buckets to allow us to use higher current modes again.
unsigned int cur1x1display;
#define LIMITING_ON               1             //CURRENT LIMITING  //switch this to 0 in order to turn limiting off

unsigned long current1x5;                       //CURRENT LIMITING  //Numerical current for bucket 1 limit
unsigned int cur1x5display;
long i2p1x5;                           //CURRENT LIMITING  //133000000  //I^2 * t information is accumulated every time we complete a loop (pulse).  each current bucket has its own I2P bucket that can be filled, and this is bucket 1's limit
float i2p1x5divisor;

unsigned long current2x;                        //CURRENT LIMITING  //Numerical current for bucket 2 limit
unsigned int cur2xdisplay;
long i2p2x;                            //CURRENT LIMITING  //18000000   //I^2 * t information is accumulated every time we complete a loop (pulse).  each current bucket has its own I2P bucket that can be filled, and this is bucket 2's limit
float i2p2xdivisor;

unsigned long current3x;                        //CURRENT LIMITING  //Numerical current for bucket 3 limit
unsigned int cur3xdisplay;
long i2p3x;                            //CURRENT LIMITING  //6000000    //I^2 * t information is accumulated every time we complete a loop (pulse).  each current bucket has its own I2P bucket that can be filled, and this is bucket 3's limit
float i2p3xdivisor;

boolean full3x =                  false;        //CURRENT LIMITING  //Is bucket 3 full?
boolean full2x =                  false;        //CURRENT LIMITING  //Is bucket 2 full?
boolean full1x5 =                 false;        //CURRENT LIMITING  //Is bucket 1 full?
boolean empty3x =                 true;         //CURRENT LIMITING  //Is bucket 3 empty?
boolean empty2x =                 true;         //CURRENT LIMITING  //Is bucket 2 empty?
boolean empty1x5 =                true;         //CURRENT LIMITING  //Is bucket 1 empty?

long limit =                      LIPOFUSE;     //CURRENT LIMITING  // our current limit right now
byte limitbucket =                0;            //CURRENT LIMITING  //which bucket are we limiting from?  0 = not limiting.  1, 2, 3 = bucket 1x5, 2x, 3x
byte recoverybucket =             0;            //CURRENT LIMITING  //which bucket are we recovering to?  0 = not recovering.  1, 2, 3 = bucket 1x5, 2x, 3x
unsigned int refcurrent =         0;
long overcurrentratio =           0;
long lastloopcurrent =            0;
//--------------------------------------SPEED AND ODOMETRY------------

#define DISTANCEDIVISOR                 17     //SPEED AND ODOMETRY  //with our 54:10 output gearing, this is how many 1/10 miles we go per revolution of the intermediate shaft
#define DISTANCEMULTIPLIER              53
#define FEETDIVISOR                 5
#define FEETMULTIPLIER              7
#define SPEEDMULTIPLIER                 1491477
#define SPEEDTIMEOUT                    400000   //SPEED AND ODOMETRY  //number of uS to wait for a signal from the motor before declaring us stopped (0.4mph)
#define ODOMETERCHECKADDRESS            0x00
#define ODOMETERADDRESS                 0x02
#define SYSTEMWEIGHT                    120         //weight of the kart + driver, in Kg.
#define METRICSPEEDMULTIPLIER           51
#define METRICSPEEDDIVISOR              8
#define BLOCKINGSWITCHOVER              25          //max speed above which we use our current speed to select our reverse crossover
//^this speed helps us assume we have a working tachometer, so reverse works like it used to if our tach is broken.
#define REVERSECROSSOVER                6           //speed below which we can transfer to reverse from forward (or vice versa)
#define SPEEDPCT1    0.20f        //20%
#define SPEEDLVL1       6        //0.6mph
const unsigned int odometercheck =      0x0A5F;     //0000101001011111
//const unsigned int odometercheck =      0xF5A0;   //1111010110100000
volatile boolean timedout =             false;    //SPEED AND ODOMETRY
volatile boolean triggered =            false;    //SPEED AND ODOMETRY
volatile int numtriggers =     0;        //SPEED AND ODOMETRY
unsigned long motorlife =      0;        //SPEED AND ODOMETRY
unsigned long trignewtime =    0;        //SPEED AND ODOMETRY

boolean speedsensing =                  true;
unsigned int maxthrottle =              SERVOMAX;
boolean atmaxthrottle =                 false;
long maxthrottlestart =                 0;
#define NOSPEEDTIMEOUT                  3000      //number of mS to wait at our 'max' throttle without seeing any speed before we assume the speed sensor's broken and turn it off
unsigned int maxspeed =                 0;
unsigned long RPM =                     0;
unsigned long tripref =                 0;
unsigned long motorspeed =              0;        //SPEED AND ODOMETRY
unsigned long timeouttime =             0;        //SPEED AND ODOMETRY

const int speedpower_table [] PROGMEM = {0, 50, 100, 150, 200, 250, 300, 350, 400, 450, 500, 550, 600, 650, 700, 750, 800, 850, 900, 950, 1000, 1050, 1100, 1150, 1200};

//----------------------------DATALOGGING AND DISPLAY------------
struct Datastruct {
  String dataname;
  String dataunits;
  boolean slowlogging;
  boolean fastlogging;
  boolean averaging;
  byte decimals;
  unsigned int diffthreshold;
  long val;
  long lastval;
  long avgval;
  unsigned int averagecycles;
};

#define LOGGEDVARIABLES        48

Datastruct mydata[LOGGEDVARIABLES + 1];
#define   LOG_TIME             0  //
#define   LOG_THROTIN          1  //
#define   LOG_THROTOUT         2  //
#define   LOG_RAMPRATE         3  //
#define   LOG_CURRENT          4  //
#define   LOG_FILTEREDCURRENT  5  //
#define   LOG_PREDCURRENT      6  //
#define   LOG_CURR2            7  //
#define   LOG_FUSERATIO        8  //
#define   LOG_LIMIT            9  //
#define   LOG_OVERCURRENT     10  //
#define   LOG_CAPACITY        11  //
#define   LOG_VOLTAGE         12  //
#define   LOG_OCV             13  //
#define   LOG_POWER           14  //
#define   LOG_ENERGY          15  //
#define   LOG_POUT            16  //
#define   LOG_EFFICIENCY      17  //
#define   LOG_SPEED           18  //
#define   LOG_AX              19  //
#define   LOG_AY              20  //
#define   LOG_AZ              21  //
#define   LOG_RX              22  //
#define   LOG_RY              23  //
#define   LOG_RZ              24  //
#define   LOG_KE              25  //
#define   LOG_TRIP            26  //
#define   LOG_ODO             27  //
#define   LOG_REVERSE         28  //
#define   LOG_BRAKE           29  //
#define   LOG_STEER           30  //
#define   LOG_FUSETEMP        31  //
#define   LOG_MOTORTEMP       32  //
#define   LOG_COOLANTTEMP     33  //
#define   LOG_GEARTEMP        34  //
#define   LOG_BRAKETEMP       35  //
#define   LOG_ECUTEMP         36  //
#define   LOG_FUSEIMP         37  //
#define   LOG_LIMBIN          38  //
#define   LOG_RECBIN          39  //
#define   LOG_BIN1            40  //
#define   LOG_BIN2            41  //
#define   LOG_BIN3            42  //
#define   LOG_I2P1            43  //
#define   LOG_I2P2            44  //
#define   LOG_I2P3            45  //
#define   LOG_LOOPS           46  //
#define   LOG_CPU             47  //

void initializedata()
{
  //THESE DO NOT NEED TO BE IN ANY SPECIFIC ORDER SINCE THEY GET THEIR INDICES FROM THE ABOVE TABLE
  //Storage Bucket Name         |Column Label        |Units      |Slow |Fast  |Avg  |Dec |Diff |Val|Lastval|Avgval|Avgcycles //
  mydata[LOG_TIME] =            {"TIME",             "s",        true , true , false, 3, 999,  0, 0, 0, 0};
  mydata[LOG_THROTIN] =         {"THROTTLE IN",      "%",        true , false, false, 1, 9,    0, 0, 0, 0};
  mydata[LOG_THROTOUT] =        {"THROTTLE OUT",     "%",        true , true , false, 1, 9,    0, 0, 0, 0};
  mydata[LOG_RAMPRATE] =        {"RAMP RATE",        "us/4ms",   false, false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_CURRENT] =         {"CURRENT",          "A",        false, true , true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_FILTEREDCURRENT] = {"FILTERED CURRENT", "A",        true , false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_PREDCURRENT] =     {"20ms PREDICTION",  "A",        false, false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_CURR2] =           {"FUSE HEATING",     "(A^2)*ms", false, false, true,  0, 9999, 0, 0, 0, 0};
  mydata[LOG_FUSERATIO] =       {"FUSE RATIO",       "%",        false, false, true,  1, 99,   0, 0, 0, 0};
  mydata[LOG_LIMIT] =           {"LIMIT",            "A",        true , false, false, 0, 0,    0, 0, 0, 0};
  mydata[LOG_OVERCURRENT] =     {"OVERCURRENT",      "A",        false, false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_CAPACITY] =        {"CAPACITY",         "Ahr",      true , false, false, 2, 9,    0, 0, 0, 0};
  mydata[LOG_VOLTAGE] =         {"BATTERY",          "V",        true , false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_OCV] =             {"OCV",              "V",        false, false, true,  1, 0,    0, 0, 0, 0};
  mydata[LOG_POWER] =           {"BATTERY POWER",    "W",        true , false, true,  0, 99,   0, 0, 0, 0};
  mydata[LOG_ENERGY] =          {"ENERGY",           "Whr",      true , false, false, 0, 0,    0, 0, 0, 0};
  mydata[LOG_POUT] =            {"POWER OUTPUT",     "W",        true , false, true,  0, 99,   0, 0, 0, 0};
  mydata[LOG_EFFICIENCY] =      {"EFFICIENCY",       "%",        false, false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_SPEED] =           {"SPEED",            "mph",      true , false, true,  1, 0,    0, 0, 0, 0};
  mydata[LOG_AX] =              {"ACCEL-X",          "g",        true , false, true,  2, 9,    0, 0, 0, 0};
  mydata[LOG_AY] =              {"ACCEL-Y",          "g",        true , false, true,  2, 9,    0, 0, 0, 0};
  mydata[LOG_AZ] =              {"ACCEL-Z",          "g",        false, false, true,  2, 9,    0, 0, 0, 0};
  mydata[LOG_RX] =              {"ROTATION-X",       "*/s",      false, false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_RY] =              {"ROTATION-Y",       "*/s",      false, false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_RZ] =              {"ROTATION-Z",       "*/s",      true , false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_KE] =              {"KINETIC ENERGY",   "J",        false, false, true,  1, 99,   0, 0, 0, 0};
  mydata[LOG_TRIP] =            {"TRIP",             "ft",       true , false, false, 0, 9,    0, 0, 0, 0};
  mydata[LOG_ODO] =             {"ODOMETER",         "mi",       true , false, false, 1, 0,    0, 0, 0, 0};
  mydata[LOG_REVERSE] =         {"GEAR",             " ",        false, false, false, 0, 0,    0, 0, 0, 0};
  mydata[LOG_BRAKE] =           {"BRAKE",            " ",        true , false, false, 0, 0,    0, 0, 0, 0};
  mydata[LOG_STEER] =           {"STEERING ANGLE",   "*",        true , false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_FUSETEMP] =        {"FUSE TEMP",        "C",        true , false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_MOTORTEMP] =       {"MOTOR TEMP",       "C",        true , false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_COOLANTTEMP] =     {"COOLANT TEMP",     "C",        true , false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_GEARTEMP] =        {"GEARBOX TEMP",     "C",        true , false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_BRAKETEMP] =       {"BRAKE TEMP",       "C",        true , false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_ECUTEMP] =         {"ECU TEMP",         "C",        true , false, true,  1, 9,    0, 0, 0, 0};
  mydata[LOG_FUSEIMP] =         {"FUSE IMPEDANCE",   "mOhm",     true , true , true,  3, 99,   0, 0, 0, 0};
  mydata[LOG_LIMBIN] =          {"LIMITING BIN",     "A",        false, false, false, 0, 0,    0, 0, 0, 0};
  mydata[LOG_RECBIN] =          {"RECOVERY BIN",     "A",        true , false, false, 0, 0,    0, 0, 0, 0};
  mydata[LOG_BIN1] =            {"BIN 1",            "%",        true , false, false, 0, 0,    0, 0, 0, 0};
  mydata[LOG_BIN2] =            {"BIN 2",            "%",        true , false, false, 0, 0,    0, 0, 0, 0};
  mydata[LOG_BIN3] =            {"BIN 3",            "%",        true , false, false, 0, 0,    0, 0, 0, 0};
  mydata[LOG_I2P1] =            {"I2P 1",            "(A^2)*ms", false, false, false, 0, 9999, 0, 0, 0, 0};
  mydata[LOG_I2P2] =            {"I2P 2",            "(A^2)*ms", false, false, false, 0, 9999, 0, 0, 0, 0};
  mydata[LOG_I2P3] =            {"I2P 3",            "(A^2)*ms", false, false, false, 0, 9999, 0, 0, 0, 0};
  mydata[LOG_LOOPS] =           {"LOOPS",            " ",        false, false, false, 0, 9999, 0, 0, 0, 0};
  mydata[LOG_CPU] =             {"CPU USAGE",        "%",        false, false, true,  0, 4,    0, 0, 0, 0};
}

const long digitsize_table[] = {0, 1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000};

const int screwtemp_table[] = { -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999,
                                -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -250, -249, -246, -243, -240, -237, -235, -232, -229, -226, -224, -221, -219, -216, -214, -211, -209, -206, -204, -201, -199, -197, -194, -192, -190, -188, -185, -183, -181, -179, -177, -175, -172, -170, -168, -166, -164, -162,
                                -160, -158, -156, -154, -152, -150, -148, -146, -144, -143, -141, -139, -137, -135, -133, -131, -130, -128, -126, -124, -123, -121, -119, -117, -116, -114, -112, -110, -109, -107, -105, -104, -102, -101, -99, -97, -96, -94, -93, -91, -89, -88, -86, -85, -83, -82, -80, -79, -77, -76, -74, -73, -71, -70, -68,
                                -67, -65, -64, -62, -61, -60, -58, -57, -55, -54, -53, -51, -50, -48, -47, -46, -44, -43, -42, -40, -39, -38, -36, -35, -34, -32, -31, -30, -28, -27, -26, -24, -23, -22, -21, -19, -18, -17, -16, -14, -13, -12, -11, -9, -8, -7, -6, -4, -3, -2, -1, 1, 2, 3, 4, 5, 7, 8, 9, 10, 11, 13, 14, 15, 16, 17, 18, 20, 21, 22, 23, 24, 25, 26,
                                28, 29, 30, 31, 32, 33, 34, 35, 36, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111,
                                112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172,
                                173, 174, 175, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 226, 227, 228, 229, 230,
                                231, 232, 233, 234, 235, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 254, 255, 256, 257, 258, 259, 260, 261, 262, 263, 263, 264, 265, 266, 267, 268, 269, 270, 271, 272, 273, 273, 274, 275, 276, 277, 278, 279, 280, 281, 282, 283, 283, 284, 285, 286, 287, 288,
                                289, 290, 291, 292, 293, 294, 294, 295, 296, 297, 298, 299, 300, 301, 302, 303, 304, 305, 305, 306, 307, 308, 309, 310, 311, 312, 313, 314, 315, 316, 317, 317, 318, 319, 320, 321, 322, 323, 324, 325, 326, 327, 328, 329, 330, 331, 332, 332, 333, 334, 335, 336, 337, 338, 339, 340, 341, 342, 343, 344, 345, 346, 347, 348,
                                349, 350, 350, 351, 352, 353, 354, 355, 356, 357, 358, 359, 360, 361, 362, 363, 364, 365, 366, 367, 368, 369, 370, 371, 372, 373, 374, 375, 376, 377, 378, 379, 380, 381, 382, 383, 384, 385, 386, 387, 388, 389, 390, 391, 392, 393, 394, 395, 396, 397, 398, 399, 400, 401, 402, 403, 404, 405, 406, 407, 408, 409, 410, 411,
                                412, 413, 414, 415, 416, 417, 418, 419, 420, 421, 422, 423, 425, 426, 427, 428, 429, 430, 431, 432, 433, 434, 435, 436, 437, 438, 439, 441, 442, 443, 444, 445, 446, 447, 448, 449, 450, 451, 453, 454, 455, 456, 457, 458, 459, 460, 461, 463, 464, 465, 466, 467, 468, 469, 470, 472, 473, 474, 475, 476, 477, 478, 480, 481,
                                482, 483, 484, 485, 487, 488, 489, 490, 491, 493, 494, 495, 496, 497, 498, 500, 501, 502, 503, 505, 506, 507, 508, 509, 511, 512, 513, 514, 516, 517, 518, 519, 521, 522, 523, 524, 526, 527, 528, 529, 531, 532, 533, 535, 536, 537, 539, 540, 541, 542, 544, 545, 546, 548, 549, 550, 552, 553, 555, 556, 557, 559, 560, 561,
                                563, 564, 565, 567, 568, 570, 571, 572, 574, 575, 577, 578, 580, 581, 582, 584, 585, 587, 588, 590, 591, 593, 594, 596, 597, 599, 600, 602, 603, 605, 606, 608, 609, 611, 612, 614, 616, 617, 619, 620, 622, 623, 625, 627, 628, 630, 631, 633, 635, 636, 638, 640, 641, 643, 645, 646, 648, 650, 652, 653, 655, 657, 658, 660,
                                662, 664, 666, 667, 669, 671, 673, 675, 676, 678, 680, 682, 684, 686, 688, 689, 691, 693, 695, 697, 699, 701, 703, 705, 707, 709, 711, 713, 715, 717, 719, 721, 723, 725, 727, 729, 731, 734, 736, 738, 740, 742, 744, 747, 749, 751, 753, 756, 758, 760, 762, 765, 767, 769, 772, 774, 777, 779, 781, 784, 786, 789, 791, 794,
                                796, 799, 801, 804, 807, 809, 812, 815, 817, 820, 823, 825, 828, 831, 834, 837, 840, 842, 845, 848, 851, 854, 857, 860, 864, 867, 870, 873, 876, 879, 883, 886, 889, 893, 896, 900, 903, 907, 910, 914, 917, 921, 925, 929, 932, 936, 940, 944, 948, 952, 956, 960, 964, 968, 973, 977, 981, 986, 990, 995, 999, 1004, 1009,
                                1013, 1018, 1023, 1028, 1033, 1038, 1044, 1049, 1055, 1060, 1066, 1072, 1077, 1083, 1089, 1095, 1102, 1108, 1114, 1121, 1127, 1134, 1141, 1148, 1156, 1163, 1171, 1179, 1187, 1195, 1203, 1211, 1220, 1229, 1238, 1248, 1258, 1268, 1278, 1289, 1299, 1311, 1323, 1335, 1347, 1360, 1374, 1388, 1402, 1417, 1433,
                                1449, 1467, 1485, 1497, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999
                              };




const int fusetemp_table[] = { -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999,
                               -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -300, -299, -296, -293, -290, -287, -284, -281, -278, -276, -273, -270, -267, -265, -262, -260, -257, -254, -252, -249, -247, -244, -242, -240, -237, -235, -232, -230, -228,
                               -225, -223, -221, -218, -216, -214, -212, -210, -207, -205, -203, -201, -199, -197, -195, -193, -190, -188, -186, -184, -182, -180, -178, -176, -174, -172, -170, -168, -166, -165, -163, -161, -159, -157, -155, -153, -151, -150, -148, -146, -144, -142, -140, -139, -137, -135, -133, -132, -130, -128, -126,
                               -125, -123, -121, -120, -118, -116, -115, -113, -111, -110, -108, -106, -105, -103, -101, -100, -98, -97, -95, -93, -92, -90, -89, -87, -86, -84, -83, -81, -80, -78, -76, -75, -73, -72, -70, -69, -67, -66, -64, -63, -62, -60, -59, -57, -56, -54, -53, -51, -50, -49, -47, -46, -44, -43, -42, -40, -39, -37, -36, -35,
                               -33, -32, -30, -29, -28, -26, -25, -24, -22, -21, -20, -18, -17, -16, -14, -13, -12, -10, -9, -8, -6, -5, -4, -2, -1, 0, 2, 3, 4, 6, 7, 8, 9, 11, 12, 13, 14, 16, 17, 18, 19, 21, 22, 23, 24, 26, 27, 28, 29, 31, 32, 33, 34, 35, 37, 38, 39, 40, 42, 43, 44, 45, 46, 48, 49, 50, 51, 52, 54, 55, 56, 57, 58, 59, 61, 62, 63, 64, 65, 66, 68, 69,
                               70, 71, 72, 73, 75, 76, 77, 78, 79, 80, 81, 83, 84, 85, 86, 87, 88, 89, 91, 92, 93, 94, 95, 96, 97, 99, 100, 101, 102, 103, 104, 105, 106, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 142, 143, 144, 145, 146, 147,
                               148, 149, 150, 151, 152, 153, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215,
                               216, 217, 218, 219, 220, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 256, 257, 258, 259, 261, 262, 263, 264, 265, 266, 267, 268, 269, 270, 271, 272, 273, 274, 275, 276, 278, 279, 280, 281, 282, 283,
                               284, 285, 286, 287, 288, 289, 290, 291, 292, 294, 295, 296, 297, 298, 299, 300, 301, 302, 303, 304, 305, 306, 307, 309, 310, 311, 312, 313, 314, 315, 316, 317, 318, 319, 320, 322, 323, 324, 325, 326, 327, 328, 329, 330, 331, 332, 334, 335, 336, 337, 338, 339, 340, 341, 342, 343, 345, 346, 347, 348, 349, 350, 351, 352,
                               353, 355, 356, 357, 358, 359, 360, 361, 362, 363, 365, 366, 367, 368, 369, 370, 371, 372, 374, 375, 376, 377, 378, 379, 380, 381, 383, 384, 385, 386, 387, 388, 389, 391, 392, 393, 394, 395, 396, 398, 399, 400, 401, 402, 403, 405, 406, 407, 408, 409, 410, 412, 413, 414, 415, 416, 417, 419, 420, 421, 422, 423, 425, 426,
                               427, 428, 429, 431, 432, 433, 434, 435, 437, 438, 439, 440, 441, 443, 444, 445, 446, 448, 449, 450, 451, 453, 454, 455, 456, 458, 459, 460, 461, 463, 464, 465, 466, 468, 469, 470, 471, 473, 474, 475, 476, 478, 479, 480, 482, 483, 484, 485, 487, 488, 489, 491, 492, 493, 495, 496, 497, 499, 500, 501, 503, 504, 505, 507,
                               508, 509, 511, 512, 513, 515, 516, 517, 519, 520, 521, 523, 524, 526, 527, 528, 530, 531, 533, 534, 535, 537, 538, 540, 541, 542, 544, 545, 547, 548, 550, 551, 552, 554, 555, 557, 558, 560, 561, 563, 564, 566, 567, 569, 570, 572, 573, 575, 576, 578, 579, 581, 582, 584, 585, 587, 588, 590, 592, 593, 595, 596, 598, 599,
                               601, 603, 604, 606, 607, 609, 611, 612, 614, 615, 617, 619, 620, 622, 624, 625, 627, 629, 630, 632, 634, 635, 637, 639, 641, 642, 644, 646, 647, 649, 651, 653, 654, 656, 658, 660, 662, 663, 665, 667, 669, 671, 672, 674, 676, 678, 680, 682, 684, 685, 687, 689, 691, 693, 695, 697, 699, 701, 703, 705, 707, 709, 711, 713,
                               715, 717, 719, 721, 723, 725, 727, 729, 731, 733, 735, 737, 739, 741, 743, 746, 748, 750, 752, 754, 756, 759, 761, 763, 765, 767, 770, 772, 774, 777, 779, 781, 784, 786, 788, 791, 793, 795, 798, 800, 803, 805, 808, 810, 812, 815, 818, 820, 823, 825, 828, 830, 833, 836, 838, 841, 844, 846, 849, 852, 854, 857, 860, 863,
                               866, 869, 871, 874, 877, 880, 883, 886, 889, 892, 895, 898, 900, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999,
                               1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999,
                               1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999, 1999
                             };

#define ECUTEMPOFFSET         250
#define ECUADCOFFSET          2463
#define ECUTEMPBIT            -1.7f
#define LOGBUFFERSIZE         2047      //DATALOGGING AND DISPLAY 
#define LOGINTERVAL           100        //DATALOGGING AND DISPLAY  //number of main loops we run through before submitting a datapoint for our log
#define LOGTIMEOUT            1000                                //maximum amount of time between logs, when logs are omitted for space saving.
#define PGOODADJUSTEDLIMIT    160       //DATALOGGING AND DISPLAY  //below this adjusted stack voltage, we should close the file and turn off logging because we've probably been turned off.
#define PGOODLIMIT            90       //DATALOGGING AND DISPLAY  //below this unadjusted voltage, we should close the file and turn off logging because we're probably going to brownout
#define MAXDIGITS             10
#define STEERCENTER           2052
#define STEERRATE             1
#define IMPEDANCECURRENT      250

boolean logging =             true;     //DATALOGGING AND DISPLAY
boolean fastlog =         false;    //DATALOGGING AND DISPLAY
char filebuffer[LOGBUFFERSIZE];         //DATALOGGING AND DISPLAY
char filename[24];                      //DATALOGGING AND DISPLAY
unsigned int bufflength =     0;        //DATALOGGING AND DISPLAY
unsigned long wattpulses =    0;        //DATALOGGING AND DISPLAY
boolean skiplog =             false;    //DATALOGGING AND DISPLAY
unsigned long amppulses =     0;        //DATALOGGING AND DISPLAY  //running counter we use for amphours
unsigned int paranumber =     1;        //DATALOGGING AND DISPLAY  //current parameter we're writing out on the serial port.  right now we have 11, so they each update at ~44Hz
unsigned int loopssincelog =  0;        //number of times we've looped without sending a datalog
#define IMUVARIABLES 7
byte IMUcounter = 0;

//------------------------GENERIC VARIABLES--------------------

#define ADCBITS           10
#define FULLSCALEADC      1023          //CURRENT LIMITING  //why yes, that is a 10-bit ADC in my pocket
#define AVERAGEBITS       3
#define ADCAVERAGING      8
#define CONVERSIONSPEED   ADC_HIGH_SPEED
#define SAMPLINGSPEED     ADC_HIGH_SPEED
#define PERCENT           100               //GENERIC VARIABLES  
#define FULLSCALEVOLTAGE  735               //maximum 100mV increments measurable by our divider.
float pctdivisor =        02.14f;           //GENERIC VARIABLES  //maps our input uS throttle output to a percentage
float thrdivisor =        0.598f;           //GENERIC VARIABLES  //maps our ADC-read throttle by this to get a uS throttle to output
float curdivisor =        1.80f;            //GENERIC VARIABLES  //maps our ADC-read current to 100-mA resolution
float voltdivisor =       0.839f;           //GENERIC VARIABLES  //maps our ADC-read voltage to get 100-mV resolution
float ahrdivisor =        0.000002778f;       //(10/3.600.000)
float whrdivisor =        0.000002778f;      // (10/3.600.000)

#define lifeimpedance     0.034f           //GENERIC VARIABLES  //impedance of the battery and wiring, used for calculating unloaded battery voltage  34mOhm
#define lipoimpedance     0.012f           //GENERIC VARIABLES  //impedance of the battery and wiring, used for calculating unloaded battery voltage  12mOhm.  LiPos are insane!
float battimpedance;
#define lipodivisor       0.83333333f      //GENERIC VARIABLES  //amount to multiply or dvide variables by to switch from LiFE toLiPo
#define safetyfactor      0.40f             //GENERIC VARIABLES  //ratio of theoretical I2t vs used I2t.  because if you fly too close to the sun, you get a blown fuse.
#define lipomultiplier    1.15625f         //GENERIC VARIABLES  //undervoltage ratio of lipos to lifes.  Slightly(slightly) different from the reciprocal of lipodivisor
float limpscale =         0.25f;

//
//INSTANTIATIONS
PulseServo myservo;          //the output is basically just a servo output, specially-controlled
LSM6DS3 myIMU ( I2C_MODE, 0x6B ); //Default constructor is I2C, addr 0x6B
FreqMeasureMulti myspeed;
File f;
ADC *adc = new ADC(); // adc object;


//------------------------ANALOG SAMPLING-------------------------------------
//
//  takes 8 samples of all our analog inputs,
//  and constrains them to our relevant data ranges
//
//------------------------ANALOG SAMPLING-------------------------------------
void sample()
{
  //make sure everything is zeroed out first
  mydata[LOG_CURRENT].val = 0;
  mydata[LOG_FUSERATIO].val = 0;
  for (int i = 0; i < 16; i++)
  {
    mydata[LOG_CURRENT].val += adc->analogRead(APIN_ISHUNT, ADC_0);
    mydata[LOG_FUSERATIO].val += adc->analogRead(APIN_IFUSE, ADC_0);
  }
  mydata[LOG_CURRENT].val += SHUNTOFFSET;
  mydata[LOG_FUSERATIO].val += FUSEOFFSET;

  if (mydata[LOG_CURRENT].val < SHUNTTHRESHOLD)
  {
    mydata[LOG_CURRENT].val = 0;
  }
  if (mydata[LOG_FUSERATIO].val < FUSETHRESHOLD)
  {
    mydata[LOG_FUSERATIO].val = 0;
  }






  mydata[LOG_VOLTAGE].val = adc->analogRead(APIN_PACKV, ADC_0);

  mydata[LOG_THROTIN].val = adc->analogRead (APIN_THROTTLE, ADC_0);
  mydata[LOG_STEER].val = adc->analogRead(APIN_STEERING, ADC_0);

  mydata[LOG_FUSETEMP].val = fusetemp_table[adc->analogRead(APIN_FUSETEMP, ADC_0)];
  mydata[LOG_MOTORTEMP].val = screwtemp_table[adc->analogRead(APIN_MOTORTEMP, ADC_0)];
  mydata[LOG_GEARTEMP].val = screwtemp_table[adc->analogRead(APIN_GEARTEMP, ADC_0)];
  mydata[LOG_BRAKETEMP].val = screwtemp_table[adc->analogRead(APIN_BRAKETEMP, ADC_0)];
  mydata[LOG_COOLANTTEMP].val = screwtemp_table[adc->analogRead(APIN_COOLANTTEMP, ADC_0)];


  mydata[LOG_BRAKE].val = 100 * !digitalRead(DPIN_BRAKE);
  mydata[LOG_REVERSE].val = !digitalRead(DPIN_REVERSE);


  //convert the current ADC readings to a numerical current
  if (((mydata[LOG_CURRENT].val * SHUNTMULTIPLIER) > 10) && ((mydata[LOG_FUSERATIO].val * FUSEMULTIPLIER) > 10))
  {
    mydata[LOG_FUSERATIO].val = (1000 * mydata[LOG_FUSERATIO].val) / mydata[LOG_CURRENT].val;

  }
  else
  {
    mydata[LOG_FUSERATIO].val = 1333;
  }
  mydata[LOG_FUSEIMP].val = 0.750f *  float(mydata[LOG_FUSERATIO].val);

  mydata[LOG_CURRENT].val *= SHUNTMULTIPLIER;                        //get our current flow to Amps range from ADC range

  mydata[LOG_FILTEREDCURRENT].val = ((3 *  mydata[LOG_FILTEREDCURRENT].val + lastloopcurrent) >> 3) +  ( mydata[LOG_CURRENT].val >> 1);
  amppulses += ( mydata[LOG_CURRENT].val * prevsample);                             //increment our capacity counter with our current reading
  mydata[LOG_VOLTAGE].val *= voltdivisor;            //first we need to multiply by our divisor
  mydata[LOG_POWER].val = ( mydata[LOG_CURRENT].val *  mydata[LOG_VOLTAGE].val) / 100;
  wattpulses += ( mydata[LOG_POWER].val * prevsample);                                  //watt-hours calculation thrown in here for shits and giggles

  if (false)//mydata[LOG_VOLTAGE].val < PGOODLIMIT)               //if we're logging and we detect a precipitous drop in voltage, close the file before data corruption occurs and stalls our throttle response for a few seconds.
  {
    mydata[LOG_THROTIN].val = 0;
    EEPROM.put(ODOMETERADDRESS, motorlife);

    if (logging)
    {
      logging = false;
      f.close();
    }
  }

  //this is where the magic happens
  //to get our I2P value, square the difference from our reading with the 1x1 limit
  mydata[LOG_CURR2].val =  mydata[LOG_CURRENT].val - long(current1x1);
  mydata[LOG_CURR2].val *=  mydata[LOG_CURR2].val;
  mydata[LOG_CURR2].val *= prevsample;
  mydata[LOG_CURR2].val /=  100;



  mydata[LOG_OCV].val =  mydata[LOG_VOLTAGE].val + float(battimpedance *  mydata[LOG_CURRENT].val);           //take a 'fake OCV' measurement using the impedance, current, and voltage
  if (false)//mydata[LOG_OCV].val  < limphyst)                                                    //if we're below our limp home threshold at all, do the following
  {
    if ( mydata[LOG_OCV].val < PGOODADJUSTEDLIMIT)
    {
      mydata[LOG_THROTIN].val = 0;
      EEPROM.put(ODOMETERADDRESS, motorlife);

      if (logging)
      {
        logging = false;
        f.close();
      }
    }
    limpthrottle = (constrain( mydata[LOG_OCV].val, limpcutoff, limphyst) - limpcutoff) * limpscale;               //bracket our voltage reading
    limpthrottle += LIMPLEVEL; //then map it to a 100%-<LIMPHYST>% reading based on how far we are to our absolute low cutoff
    limphome = true;                                                                  //make sure evryone knows we're in limp home mode
    //digitalWrite(DPIN_LED, HIGH);                                                       //and turn on the indicator LED

  }
  else
  {
    limphome = false;                                                                 //otherwise, take us out of limp mode
    //digitalWrite(DPIN_LED, LOW);                                                        //and turn off the indicator LED
  }

  //convert the throttle ADC readings to a servo duration
  mydata[LOG_THROTIN].val = mydata[LOG_THROTIN].val - THROTTLEMIN;                //invert the range (so that a disconnected pot = zero throttle)
  mydata[LOG_THROTIN].val = constrain( mydata[LOG_THROTIN].val, 0, THROTTLERANGE); //make sure we stay in bounds
  mydata[LOG_THROTIN].val *= mydata[LOG_THROTIN].val;
  mydata[LOG_THROTIN].val /= THROTTLERANGE;
  if (limphome)
  {
    mydata[LOG_THROTIN].val = ( mydata[LOG_THROTIN].val * limpthrottle); //make sure we stay in bounds
  }

  throttlepos = float( mydata[LOG_THROTIN].val) * thrdivisor;                              //run our divider to get us in the uS range rather than ADC range
  mydata[LOG_THROTIN].val = throttlepos * pctdivisor;                 //get our percent value here in the middle
  throttlepos += SERVODEAD;                               //lastly, add the offset of the zero position
  if (throttlepos == SERVODEAD)
  {
    throttlepos = SERVOZERO;
  }

}

//------------------------CURRENT AND THROTTLE LIMTING-------------------------------------
//
//  uses our current values and the contents of the I2P
//  buckets to determine what our limit should be.  Then
//  we alter the input throttle to hit our current limit.
//
//------------------------CURRENT AND THROTTLE LIMTING-------------------------------------
void control()
{
  //right away, we check whether we're over our 1x1 'forever' limit.  if we're at or below it, we do no limiting
  if ( mydata[LOG_CURRENT].val > long(current1x1))
  {
    //let the serial port know we're not recovering any current
    recoverybucket = 0;
    mydata[LOG_RECBIN].val = 0;
    limiting();
  }

  //if we're below the 1x1 limit, no limiting needs to happen
  else
  {
    analogWrite(DPIN_ACC, 85);
    //let the serial port know we're not limiting
    limitbucket = 0;
    mydata[LOG_LIMBIN].val = 0;
    //if our current is below our forever limit, we can recover I2P points
    if ( mydata[LOG_CURRENT].val < current1x1)
    {
      recover();
    }
    //if we're RIGHT ON our forever limit, no recovery and no limting takes place
    else
    {
      //let the serial port know we're not recovering
      recoverybucket = 0;
      mydata[LOG_RECBIN].val = 0;
    }
  }

  if (full3x || (speedsensing && ( mydata[LOG_SPEED].val < BUCKET3XSPEED)))
  {
    if (full2x)
    {
      if (full1x5)
      {
        limit = current1x1;            //our current limit is the middle
        mydata[LOG_LIMIT].val = cur1x1display;
      }
      else
      {
        limit = current1x5;
        mydata[LOG_LIMIT].val = cur1x5display;
      }
    }
    else
    {
      limit = current2x;
      mydata[LOG_LIMIT].val = cur2xdisplay;
    }
  }
  else
  {
    limit = current3x;
    mydata[LOG_LIMIT].val = cur3xdisplay;
  }

  mydata[LOG_RAMPRATE].val = 0;
  //if we've been bad and our current is too high, limit us
  mydata[LOG_PREDCURRENT].val =  mydata[LOG_CURRENT].val + ((( mydata[LOG_CURRENT].val - lastloopcurrent) * ((REACTIONTIME * 1) >> 0)) / long(prevsample));
  lastloopcurrent =  mydata[LOG_CURRENT].val;
  int looplimit = (limit * 9) >> 3;
  mydata[LOG_PREDCURRENT].val = constrain( mydata[LOG_PREDCURRENT].val, 0, (current3x * 5) >> 2);
  mydata[LOG_OVERCURRENT].val =  mydata[LOG_FILTEREDCURRENT].val - limit;

  if ( mydata[LOG_OVERCURRENT].val > (limit >> 4))
  {
    rampingup = false;
    unsigned long testthrottle = lastthrottle;
    long tempratio;
    if (!rampingdown)
    {
      rampingdown = true;
      rampingdowntime = millis();
      refthrottle = lastthrottle;
      overcurrentratio = (( 5 * ((limit * 17) >> 4) + 11 *  mydata[LOG_FILTEREDCURRENT].val) << 4 ) /  mydata[LOG_FILTEREDCURRENT].val;
      testthrottle = (refthrottle - SERVODEAD) * overcurrentratio;
      testthrottle = (testthrottle >> 8) + SERVODEAD;
    }
    else
    {
      tempratio = (( 5 * ((limit * 17) >> 4) + 11 *  mydata[LOG_FILTEREDCURRENT].val) << 4 ) /  mydata[LOG_FILTEREDCURRENT].val;
      if (millis() - rampingdowntime > REACTIONTIME)
      {
        refthrottle = lastthrottle;
        overcurrentratio = tempratio;
        rampingdowntime = millis();
      }

      if ( tempratio <= overcurrentratio)
      {
        testthrottle = (refthrottle - SERVODEAD) * tempratio;
        testthrottle = (testthrottle >> 8) + SERVODEAD;
      }

    }
    throttlepos = min(throttlepos, testthrottle);
  }
  else
  {
    if ((throttlepos < SERVODEAD) && (!speedsensing || (maxspeed < BLOCKINGSWITCHOVER))) //if our throttle reading is just at the bottom end of the range, set the output signal to zero and remove our forward and reverse blocking.
    {
      traveldirection = 0;
    }

    if (( mydata[LOG_PREDCURRENT].val >= looplimit) || rampingdown)
    {
      rampingdown = false;
      rampinguptime = millis();
      refthrottle = lastthrottle;
      rampingup = true;
    }
    testramp = lastthrottle + ((prevsample * 3 * (rampuprates_table[RAMPUPTABLESIZE] - rampuprates_table[RAMPUPTABLESIZE - 1])) / (2 * MILLISPERRAMPUP));
    if (rampingup)
    {
      unsigned long overlimit = (millis() - rampinguptime) / MILLISPERRAMPUP;
      if (overlimit > RAMPUPTABLESIZE)
      {
        rampingup = false;
      }
      else
      {
        testramp = refthrottle + pgm_read_byte(&(rampuprates_table[overlimit]));
      }
    }
    //if our last throttle reading wasn't zero, let us ramp up a bit
    if (throttlepos > testramp)
    {
      if (lastthrottle < SERVODEAD)
      {
        throttlepos = SERVODEAD;
      }
      else
      {
        throttlepos = testramp;
      }
    }
  }

  //this is set to detect a brake switch closing and choose how to apply our throttle

  if (mydata[LOG_BRAKE].val)
  {
    throttlepos = BRAKEPOWER;    //apply braking power to the RC line
    mydata[LOG_RAMPRATE].val = throttlepos - lastthrottle;
    lastthrottle = SERVOZERO;       //set all our ramp functions back to zero
  }
  else
  {
    //make sure we're only dealing with values within the usable throttle range
    if ((throttlepos > maxthrottle) && !((traveldirection == GOREVERSE) || ( mydata[LOG_REVERSE].val && !traveldirection)))
    {
      if (!atmaxthrottle)
      {
        atmaxthrottle = true;
        maxthrottlestart = millis();
      }
      else
      {
        if (speedsensing && (millis() - maxthrottlestart < NOSPEEDTIMEOUT))
        {
          throttlepos = maxthrottle;
        }
        else
        {
          speedsensing = false;
        }
      }

    }
    else
    {
      atmaxthrottle = false;
      if (throttlepos < SERVODEAD)
      {
        throttlepos = SERVOZERO;
      }
    }
    mydata[LOG_RAMPRATE].val = throttlepos - lastthrottle;

    lastthrottle = throttlepos;       //store this for our next time through the loop to reference
    mydata[LOG_BRAKE].val = 0;                    //otherwise, let the serial port know we're not braking
  }
  //this catches a throttle of technically less than zero (outside SERVODEAD-SERVOMAX range) and tells it to be zero for our display
  if (throttlepos < SERVODEAD)
  {
    mydata[LOG_THROTOUT].val = 0;
    throttlepos = SERVOZERO;
  }
  else
  {
    mydata[LOG_THROTOUT].val = (throttlepos - SERVODEAD) * pctdivisor;  //map our current throttle reading into percent.  takes less time than the map function (mapping takes for-fricken-ever).
  }


  if (mydata[LOG_REVERSE].val)                            //if the driver is holding the reverse button
  {

    if (traveldirection == GOFORWARD)                   //but if we're actually moving forward
    {
      //don't do anything differently.  the reverse button is held but we're moving forward, so our throttle should help us go forward
    }
    else                                              //if we're stopped (ready to reverse) or moving backwards already
    {
      throttlepos = (SERVOZERO << 1) - throttlepos; //we invert our throttle direction
      traveldirection = GOREVERSE;                    //and let our travel direction know we're reversing
    }             //this may get reset to 0 during our calculatevalues phase but eventually we'll latch into reverse
  }
  else                                                //if the reverse button is released
  {
    if (traveldirection == GOREVERSE)                   //if we're actually moving backward
    {
      throttlepos = (SERVOZERO << 1) - throttlepos;   //don't do anything differently.  the reverse button is released but we're moving backward, so our throttle should help us go backward
    }
    else                                              //otherwise, if we're moving forward or stopped,
    {
      traveldirection = GOFORWARD;                      //apply no throttle transform, and let our log know we're moving forward
    }
  }

  myservo.writeMicroseconds(throttlepos);  //finally, write the throttle output

}


//------------------------I2P LIMITING-------------------------------------
//
//  Determines which I2P bucket to limit
//  from (highest bucket first), then manages
//  what their quantity is and whether they
//  report as empty
//
//------------------------I2P LIMITING-------------------------------------
void limiting()
{
  //we can steal from bucket 3 ONLY IF bucket 3 isn't full.
  //additionally, both bucket 1 and 2 must be full for us to use this if our current isn't in bucket 3's range.
  //this allows us to put current of all levels in bucket 3 if need be, since it is the first bucket to recover.
  if ((!full3x) && (( mydata[LOG_CURRENT].val > current2x) || (full1x5 && full2x)))
  {
    limitbucket = 3;              //make sure the serial port know which bucket we're limiting from
    mydata[LOG_LIMBIN].val = cur3xdisplay;
    mydata[LOG_I2P3].val +=  mydata[LOG_CURR2].val;   //add our I2P to bucket 3
    empty3x = false;              //and clarify that it isn't empty
    if ( mydata[LOG_I2P3].val > i2p3x)         //if we've filled the bucket
    {
      full3x = true;              //tell everyone it's full
    }

  }
  else
  {
    //we can steal from bucket 2 ONLY IF bucket 2 isn't full.
    //additionally, bucket 1 must be full for us to use this if our current isn't in bucket 2's range.
    //this allows us to put current of bucket 1's and 2's level here if need be, since bucket 2 recovers capacity before bucket 1.
    if ((!full2x) && (( mydata[LOG_CURRENT].val > current1x5) || full1x5))
    {
      limitbucket = 2;              //make sure the serial port know which bucket we're limiting from
      mydata[LOG_LIMBIN].val = cur2xdisplay;
      mydata[LOG_I2P2].val +=  mydata[LOG_CURR2].val;   //add our I2P to bucket 3
      empty2x = false;              //and clarify that it isn't empty
      if ( mydata[LOG_I2P2].val > i2p2x)         //if we've filled the bucket
      {
        full2x = true;              //tell everyone it's full
      }

    }
    else
    {
      if (!full1x5)
      {
        //our current limit is the low end
        limitbucket = 1;              //make sure the serial port know which bucket we're limiting from
        mydata[LOG_LIMBIN].val = cur1x5display;
        mydata[LOG_I2P1].val +=  mydata[LOG_CURR2].val;  //add our I2P to bucket 3
        empty1x5 = false;             //and clarify that it isn't empty
        if ( mydata[LOG_I2P1].val > i2p1x5)       //if we've filled the bucket
        {
          full1x5 = true;             //tell everyone it's full
        }

      }
      else
      {
        limitbucket = 4;              //make sure the serial port know which bucket we're limiting from
        mydata[LOG_LIMBIN].val = cur1x1display;
      }
    }
  }

}


//------------------------I2P RECOVERY-------------------------------------
//
//  Determines which I2P bucket to recover
//  from (highest bucket first), then manages
//  what their quantity is and whether they
//  report as empty
//
//------------------------I2P RECOVERY-------------------------------------
void recover()                                //here we 'recover' I2P points back into our buckets based on the I2P difference
{
  if (!empty3x)                               //start by trying to empty bucket 3X
  {
    recoverybucket = 3;
    mydata[LOG_RECBIN].val = cur3xdisplay;
    if ( mydata[LOG_I2P3].val >  mydata[LOG_CURR2].val)            //if there's more in the bucket than we have to take out
    {
      mydata[LOG_I2P3].val -=  mydata[LOG_CURR2].val;             //subtract what we have to take out

    }
    else
    {
      mydata[LOG_I2P3].val = 0;                           //if there's more to remove than the bucket has,
      empty3x = true;                         //empty the bucket
    }
    if ( mydata[LOG_I2P3].val < ((i2p3x * 7) >> 3))
    {
      full3x = false;                         //and let the bucket know it's not full any more (but only do this at 25% availability)
    }
  }
  else
  {
    if (!empty2x)                             //next, try to empty bucket 2x
    {
      recoverybucket = 2;
      mydata[LOG_RECBIN].val = cur2xdisplay;
      if ( mydata[LOG_I2P2].val >  mydata[LOG_CURR2].val)          //if there's more in the bucket than we have to take out
      {
        mydata[LOG_I2P2].val -=  mydata[LOG_CURR2].val;           //subtract what we have to take out

      }
      else
      {
        mydata[LOG_I2P2].val = 0;                         //if there's more to remove than the bucket has,
        empty2x = true;                       //empty the bucket
      }
      if ( mydata[LOG_I2P2].val < ((i2p2x * 7) >> 3))
      {
        full2x = false;                       //and let the bucket know it's not full any more
      }
    }
    else
    {
      if (!empty1x5)                          //last, try to empty bucket 1x5
      {
        recoverybucket = 1;
        mydata[LOG_RECBIN].val = cur1x5display;
        if ( mydata[LOG_I2P1].val >  mydata[LOG_CURR2].val)       //if there's more in the bucket than we have to take out
        {
          mydata[LOG_I2P1].val -=  mydata[LOG_CURR2].val;        //subtract what we have to take out

        }
        else
        {
          mydata[LOG_I2P1].val = 0;                      //if there's more to remove than the bucket has,
          empty1x5 = true;                    //empty the bucket
        }
        if ( mydata[LOG_I2P1].val < ((i2p1x5 * 7) >> 3))
        {
          full1x5 = false;                    //and let the bucket know it's not full any more
        }
      }
      else
      {

        recoverybucket = 4;                   //if all buckets are empty, do nothing
        mydata[LOG_RECBIN].val = 0;
      }
    }
  }
}



void calculatevalues()
{

  mydata[LOG_BIN3].val =  mydata[LOG_I2P3].val * i2p3xdivisor;
  mydata[LOG_BIN3].val = constrain ( mydata[LOG_BIN3].val, 0, PERCENT);
  mydata[LOG_BIN2].val =  mydata[LOG_I2P2].val * i2p2xdivisor;
  mydata[LOG_BIN2].val = constrain ( mydata[LOG_BIN2].val, 0, PERCENT);
  mydata[LOG_BIN1].val =  mydata[LOG_I2P1].val * i2p1x5divisor;
  mydata[LOG_BIN1].val = constrain ( mydata[LOG_BIN1].val, 0, PERCENT);
  mydata[LOG_CAPACITY].val = amppulses * ahrdivisor;
  mydata[LOG_ENERGY].val = wattpulses * whrdivisor;
  mydata[LOG_TIME].val = millis();

  switch (IMUcounter) {
    case 0:
      mydata[LOG_ECUTEMP].val = 10.0 * myIMU.readTempC();
      mydata[LOG_ECUTEMP].val += 10.0 * myIMU.readTempC(); //ADDED FOR TIMING
      mydata[LOG_ECUTEMP].val /= 2;
      break;
    case 1:
      mydata[LOG_AX].val = 100.0 * myIMU.readFloatAccelY(); //X and Y axes are switched
      mydata[LOG_RX].val = 10.0 * myIMU.readFloatGyroY();   //X and Y axes are switched
      break;
    case 2:
      mydata[LOG_AY].val = 100.0 * myIMU.readFloatAccelX(); //X and Y axes are switched
      mydata[LOG_RY].val = 10.0 * myIMU.readFloatGyroX();   //X and Y axes are switched
      break;
    case 3:
      mydata[LOG_AZ].val = 100.0 * myIMU.readFloatAccelZ();
      mydata[LOG_RZ].val = 10.0 * myIMU.readFloatGyroZ();
      break;
    default:
      break;
  }
  IMUcounter++;
  if (IMUcounter >= 4)
  {
    IMUcounter = 0;
  }

  if (speedsensing)
  {

    if (myspeed.available())
    {
      if ((micros() - trignewtime) > (2 * SPEEDTIMEOUT))
      {
        mydata[LOG_SPEED].val = 0;
      }
      else
      {
        mydata[LOG_SPEED].val = 3.28f * myspeed.countToFrequency(myspeed.read());

        if ( mydata[LOG_SPEED].val > maxspeed)
        {
          maxspeed =  mydata[LOG_SPEED].val;
        }
      }
      trignewtime = micros();
      motorlife += myspeed.readCounts();
    }
    else
    {
      if ((micros() - trignewtime) > SPEEDTIMEOUT)
      {
        mydata[LOG_SPEED].val = 0;
      }
    }
    if (( mydata[LOG_SPEED].val > SPEEDLVL1) || !speedsensing)
    {
      maxthrottle = SERVOMAX;
    }
    else
    {
      maxthrottle = SERVODEAD + (SPEEDPCT1 * float(SERVOMAX - SERVODEAD));
    }

    if (traveldirection != 0)
    {
      if (maxspeed >= BLOCKINGSWITCHOVER)
      {
        if (abs( mydata[LOG_SPEED].val) <= REVERSECROSSOVER)
        {
          traveldirection = 0;
        }
      }
    }
    if (traveldirection == GOREVERSE && ( mydata[LOG_SPEED].val > 0))
    {
      mydata[LOG_SPEED].val = -mydata[LOG_SPEED].val;
    }
    mydata[LOG_TRIP].val = ((motorlife - tripref)  * FEETMULTIPLIER) >> FEETDIVISOR;

    if (((motorlife * DISTANCEMULTIPLIER) >> DISTANCEDIVISOR) > mydata[LOG_ODO].val)
    {
      EEPROM.put(ODOMETERADDRESS, motorlife);
      if (DEBUGMODE)
      {
        Serial.println("Saving Odometer");
      }
    }
    mydata[LOG_ODO].val = (motorlife * DISTANCEMULTIPLIER) >> DISTANCEDIVISOR;
    /**
      mydata[LOG_KE].val = abs(mydata[LOG_SPEED].val);
      mydata[LOG_KE].val *= mydata[LOG_KE].val;
      mydata[LOG_KE].val *= SYSTEMWEIGHT;
      mydata[LOG_KE].val *= METRICSPEEDMULTIPLIER;
      int numbitshifts = 0;
      if (mydata[LOG_KE].val > 33552896)
      {
      mydata[LOG_KE].val = mydata[LOG_KE].val >> 6;
      numbitshifts += 6;
      }
      mydata[LOG_KE].val *= 51;
      if (mydata[LOG_KE].val > 33552896)
      {
      mydata[LOG_KE].val = mydata[LOG_KE].val >> 6;
      numbitshifts += 6;
      }
      mydata[LOG_KE].val *= 51;
      mydata[LOG_KE].val = mydata[LOG_KE].val >> (19 + METRICSPEEDDIVISOR - numbitshifts);

      mydata[LOG_POUT].val = 1000 * (mydata[LOG_KE].val -  mydata[LOG_KE].lastval);
      mydata[LOG_POUT].val /= (mydata[LOG_TIME].val -  mydata[LOG_TIME].lastval);
    */
  }

}

//Unlike the bluetooth link, we want all our logged data for each data point to represent exactly the same point in time.
//so for logs, we normally skip this and only log if we've gone through <LOGINTERVAL> loops since our last log
//  most of the code is me dynamically removing leading zeros, because it's worth it to waste a couple micros removing data that will save me tens of micros blocking my important loop from running
//if I were really smart, I'd implement this as a generic function where I can feed it the data, the minimum number of digits, the maximum, and how many places it needs after the decimal.
//hey look at that, apparently I'm getting smarter!
//this is now a single function that iterates through all our variables, checks to see if they should be logged, removes leading zeroes, and adds a decimal point where necessary (specified in log tables)

//ARRRGH HERE BE UNCOMMENTED CODE
void bufferData()
{
  if (false) {
    Serial.print(millis(), DEC);
    Serial.print(" : ");
    Serial.println(debuggedvalue);
  }
  if ((loopssincelog >= LOGINTERVAL) || fastlog)
  {

    loopssincelog = 0;

    unsigned int digits = 0;
    long tempdiff = 0;
    long tempvall = 0;
    long comparator = 0;
    unsigned int decimals = 0;
    boolean firstvalue = true;
    //digitalWrite(DPIN_LED, HIGH);
    int buffbuff = bufflength;
    for (int i = 0; i < LOGGEDVARIABLES; i++)
    {

      if ((!fastlog && mydata[i].slowlogging) || (fastlog && mydata[i].fastlogging))
      {
        if (firstvalue)
        {
          firstvalue = false;
        }
        else
        {
          filebuffer[bufflength] = ',';
          bufflength++;
        }

        if (mydata[i].val < 0)
        {
          filebuffer[bufflength] = '-';
          mydata[i].val = -mydata[i].val;
          bufflength++;
        }
        tempvall = mydata[i].val;

        if (tempvall !=  mydata[i].lastval)
        {
          int tempint = mydata[i].diffthreshold;
          if (!tempint)
          {
            skiplog = false;
          }
          else
          {
            tempdiff = tempvall -  mydata[i].lastval;
            tempdiff  = abs(tempdiff);
            if (tempdiff > tempint)
            {
              skiplog = false;
            }
          }

        }
        decimals = mydata[i].decimals;
        digits = MAXDIGITS;
        for (int n = digits; n > ((mydata[i].decimals) + 1); n--)
        {
          if (tempvall < digitsize_table[n])
          {
            digits--;
          }
          else
          {
            n = 1; //force us to exit this loop
          }
        }
        for (int n = 1; n <= digits; n++)
        {

          long templong = tempvall;
          for (int p = 10; p > 1; p--)
          {
            if (templong >= digitsize_table[p])
            {
              templong -= digitsize_table[p];
              p++;
            }
          }
          filebuffer[bufflength + digits - n] = '0' + templong;
          tempvall /= 10;
        }

        if (decimals > 0)
        {
          for (int n = decimals; n > 0; n--)
          {
            filebuffer[bufflength + digits + n - decimals] = filebuffer[bufflength + digits + n - (decimals + 1)];
          }
          filebuffer[bufflength + digits - (decimals)] = '.';
          bufflength++;

        }
        bufflength += digits;
      }

    }
    //skiplog = false;
    if (!skiplog)
    {
      for (int i = 0; i < LOGGEDVARIABLES; i++)
      {
        mydata[i].lastval = mydata[i].val;
      }
      filebuffer[bufflength] = '\r';
      bufflength++;
      filebuffer[bufflength] = '\n';
      bufflength++;
    }
    else
    {
      bufflength = buffbuff;
    }
  }
}



void logData()
{
  if (bufflength > 0)                           //if there's a card in and we actually have stuff to log
  {
    if (DEBUGMODE) {
      Serial.write((uint8_t *)filebuffer, bufflength);
    }
    //digitalWrite(DPIN_LED, HIGH);
    f.write((uint8_t *)filebuffer, bufflength);      //write the buffer
    if (mydata[LOG_THROTIN].val)
    {
      f.flush();
    }
    bufflength = 0;                                          //and set its length back to 0
    mydata[LOG_LOOPS].val = 0;
    //digitalWrite(DPIN_LED, HIGH);
  }
}

//writes the first lines of information so we can read our data back in Datplot
void writeHeaders()
{
  bufflength = 0;

  boolean firstvalue = true;

  for (int i = 0; i < LOGGEDVARIABLES; i++)
  {
    if ((!fastlog && mydata[i].slowlogging) || (fastlog && mydata[i].fastlogging))
    {
      if (firstvalue)
      {
        firstvalue = false;
      }
      else
      {
        filebuffer[bufflength] = ',';
        bufflength++;
      }
      for (int n = 0; n < (sizeof(mydata[i].dataname) - 1); n++)
      {
        if ((mydata[i].dataname).charAt(n))
        {
          filebuffer[bufflength] = (mydata[i].dataname).charAt(n);
          bufflength++;
        }
        else
        {
          n = sizeof(mydata[i].dataname);
        }
      }
    }
  }
  filebuffer[bufflength] = '\r';
  bufflength++;
  filebuffer[bufflength] = '\n';
  bufflength++;
  mydata[LOG_THROTIN].val = 0;

  logData();
  firstvalue = true;

  for (int i = 0; i < LOGGEDVARIABLES; i++)
  {
    if ((!fastlog && mydata[i].slowlogging) || (fastlog && mydata[i].fastlogging))
    {
      if (firstvalue)
      {
        firstvalue = false;
      }
      else
      {
        filebuffer[bufflength] = ',';
        bufflength++;
      }
      for (int n = 0; n < (sizeof(mydata[i].dataunits) - 1); n++)
      {
        if ((mydata[i].dataunits).charAt(n))
        {
          filebuffer[bufflength] = (mydata[i].dataunits).charAt(n);
          bufflength++;
        }
        else
        {
          n = sizeof(mydata[i].dataunits);
        }

      }
    }
  }
  filebuffer[bufflength] = '\r';
  bufflength++;
  filebuffer[bufflength] = '\n';
  bufflength++;

  logData();
  f.flush();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////MAIN FUNCTIONS
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////MAIN FUNCTIONS
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////MAIN FUNCTIONS
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////MAIN FUNCTIONS
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////MAIN FUNCTIONS

void setup()                                             // run once, when the sketch starts
{

  initializedata();

  adc->setAveraging(4); // set number of averages
  adc->setResolution(10); // set bits of resolution
  adc->setConversionSpeed(ADC_HIGH_SPEED); // change the conversion speed
  adc->setSamplingSpeed(ADC_HIGH_SPEED); // change the sampling speed

  adc->setAveraging(4, ADC_1); // set number of averages
  adc->setResolution(12, ADC_1); // set bits of resolution
  adc->setReference(ADC_REF_1V2, ADC_1);
  adc->setConversionSpeed(ADC_MED_SPEED, ADC_1); // change the conversion speed
  adc->setSamplingSpeed(ADC_MED_SPEED, ADC_1); // change the sampling speed


  byte outpins[] = {CPIN_SERIALTX, CPIN_SPISS, CPIN_SPIMOSI, CPIN_SPISCK, CPIN_I2CSDA, CPIN_I2CSCL, DPIN_LED, DPIN_ESCOUT, DPIN_ACC};      //all output pins
  for ( int i = 0; i < sizeof(outpins); i++) {
    pinMode(outpins[i], OUTPUT);                             //turn to output
    digitalWrite(outpins[i], LOW);
  }
  digitalWrite(CPIN_SPISS, HIGH);

  //set our inputs
  byte inpins[] = {CPIN_SERIALRX, CPIN_SPIMISO};         //all input pins
  for ( int i = 0; i < sizeof(inpins); i++) {
    pinMode(inpins[i], INPUT);                            //set tham all to input
  }
  byte pulluppins[] = {DPIN_BRAKE, DPIN_SPEED, DPIN_REVERSE};        //all input pins with pullups
  for ( int i = 0; i < sizeof(pulluppins); i++) {
    pinMode(pulluppins[i], INPUT_PULLUP);                            //set tham all to input with pullup
  }

  /**byte apins[12] = {APIN_FUSETEMP, APIN_GEARTEMP, APIN_MOTORTEMP, APIN_COOLANTTEMP, APIN_ISHUNT, APIN_IFUSE, APIN_THROTTLE, APIN_PACKV, APIN_BRAKETEMP, APIN_STEERING, APIN_ECUTEMP}; //all analog input pins
    for ( int i = 0; i < 11; i++) {
    pinMode(apins[i], INPUT);                            //set tham all to input
    }*/


  //Most important thing is making sure the controller is set to zero throttle when it boots.  seriously, there are protections in the ESC but redundancy is always good for safety
  myservo.attach(DPIN_ESCOUT, SERVOMIN, SERVOMAX);
  myservo.writeMicroseconds(SERVOZERO);
  if (DEBUGMODE) {
    Serial.begin(921600);
    while (digitalRead(DPIN_REVERSE))
    {
      Serial.print(millis(), DEC);
      Serial.print(" : ");
      Serial.println("Waiting for Reverse Press");
      delay(1000);
    }
    while (!digitalRead(DPIN_REVERSE))
    {
      Serial.print(millis(), DEC);
      Serial.print(" : ");
      Serial.println("Waiting for Reverse Release");
      delay(1000);
    }
    Serial.println("Begin");
  }
  if (BLUETOOTH)
  {
    HWSERIAL.begin(115200);
    HWSERIAL.print('<');
  }

  if (DEBUGMODE) {
    Serial.print(millis(), DEC);
    Serial.print(" : ");
    Serial.println("Setting up IMU");
  }
  myIMU.settings.gyroEnabled = 1;  //Can be 0 or 1
  myIMU.settings.gyroRange = 2000;   //Max deg/s.  Can be: 125, 245, 500, 1000, 2000
  myIMU.settings.gyroSampleRate = 833;   //Hz.  Can be: 13, 26, 52, 104, 208, 416, 833, 1666
  myIMU.settings.gyroBandWidth = 200;  //Hz.  Can be: 50, 100, 200, 400;
  myIMU.settings.gyroFifoEnabled = 0;  //Set to include gyro in FIFO
  myIMU.settings.gyroFifoDecimation = 1;  //set 1 for on /1

  myIMU.settings.accelEnabled = 1;
  myIMU.settings.accelRange = 16;      //Max G force readable.  Can be: 2, 4, 8, 16
  myIMU.settings.accelSampleRate = 833;  //Hz.  Can be: 13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 13330
  myIMU.settings.accelBandWidth = 200;  //Hz.  Can be: 50, 100, 200, 400;
  myIMU.settings.accelFifoEnabled = 0;  //Set to include accelerometer in the FIFO
  myIMU.settings.accelFifoDecimation = 1;  //set 1 for on /1
  myIMU.settings.tempEnabled = 1;

  //Non-basic mode settings
  myIMU.settings.commMode = 1;

  //FIFO control settings
  myIMU.settings.fifoThreshold = 100;  //Can be 0 to 4096 (16 bit bytes)
  myIMU.settings.fifoSampleRate = 50;  //Hz.  Can be: 10, 25, 50, 100, 200, 400, 800, 1600, 3300, 6600
  myIMU.settings.fifoModeWord = 0;  //FIFO mode.
  //FIFO mode.  Can be:
  //  0 (Bypass mode, FIFO off)
  //  1 (Stop when full)
  //  3 (Continuous during trigger)
  //  4 (Bypass until trigger)
  //  6 (Continous mode)
  if (DEBUGMODE) {
    Serial.print(millis(), DEC);
    Serial.print(" : ");
    Serial.println("IMU Settings Complete");
  }
  if (DEBUGMODE) {
    Serial.print(millis(), DEC);
    Serial.print(" : ");
    Serial.println("IMU Booting");
  }
  myIMU.begin();
  if (DEBUGMODE) {
    Serial.print(millis(), DEC);
    Serial.print(" : ");
    Serial.println("IMU Boot Complete");
  }
  //update our divisors

  pctdivisor = 1000.0f / (SERVOMAX - SERVODEAD);
  thrdivisor = float(SERVOMAX - SERVODEAD) / float(THROTTLERANGE);
  curdivisor = float(FULLSCALECURRENT) / FULLSCALEADC;
  voltdivisor = float(FULLSCALEVOLTAGE) / FULLSCALEADC;





  //take a battery reading and determine whether we should be obeying 50A fuse limits or 60A limits
  //we need some delay time here since detection is . . . iffy
  delay(500);
  mydata[LOG_VOLTAGE].val = 0;

  if (DEBUGMODE) {
    Serial.print(millis(), DEC);
    Serial.print(" : ");
    Serial.println("Determining Fuse Limit");
  }
  for (int i = 0; i < 8; i++)
  {
    mydata[LOG_VOLTAGE].val += analogRead(APIN_PACKV);
  }
  mydata[LOG_VOLTAGE].val = mydata[LOG_VOLTAGE].val >> 3;
  mydata[LOG_VOLTAGE].val *= voltdivisor;
  if (DEBUGMODE) {
    Serial.print(millis(), DEC);
    Serial.print(" : ");
    Serial.print("Battery Voltage: ");
    Serial.println(mydata[LOG_VOLTAGE].val, DEC);
  }

  if (mydata[LOG_VOLTAGE].val < LIPOLIMIT)
  {
    if (mydata[LOG_VOLTAGE].val < NOVSENSE)
    {
      if (DEBUGMODE) {
        Serial.print(millis(), DEC);
        Serial.print(" : ");
        Serial.println("Voltage Sense Failure, 50A");
      }
      currentbase = LIPOFUSE;
      battimpedance = 0;
      limphyst = 0;
      limpcutoff = 0;
    }
    else
    {
      if (DEBUGMODE) {
        Serial.print(millis(), DEC);
        Serial.print(" : ");
        Serial.println("LiFe Fuse, 60A");
      }
      currentbase = LIFEFUSE;
      battimpedance = lifeimpedance;
      limphyst = LIMPHYST;
      limpcutoff = LIMPCUTOFF;
    }
  }
  else
  {
    if (DEBUGMODE) {
      Serial.print(millis(), DEC);
      Serial.print(" : ");
      Serial.println("LiPo Fuse, 50A");
    }
    currentbase = LIPOFUSE;
    battimpedance = lipoimpedance;
    limphyst = LIMPHYST * lipomultiplier;
    limpcutoff = LIMPCUTOFF * lipomultiplier;
  }

  //This is the really important part, since I tell you how I'm modeling the fuse
  //now the fun begins.  we have base values for our limiting info in our variable declarations at the top, but these DO need to change based on which battery pack I run so I need to calculate them all here anyway
  if (DEBUGMODE) {
    Serial.print(millis(), DEC);
    Serial.print(" : ");
    Serial.println("Setting Fuse Modeling Parameters");
  }
  //our 110% current is exactly 110% of our base current.  duh.  For reasons currently unknown, I have decided to calculate this my multiplying by 11 then dividing by 10.  same for the others.  since we're just booting up and our ESC takes longer to boot up, we don't care too much about time
  current1x1 = currentbase * 11;
  current1x1 = current1x1 / 10;
  cur1x1display = current1x1 / 10;


  //now I'm only going to explain this for the 150% bucket because the others are calculated the same
  //our 150% current is . . . 150% of our base current.  shocker.
  current1x5 = currentbase * 15;
  current1x5 = current1x5 / 10;
  cur1x5display = current1x5 / 10;
  //the amount of I2P we can put into a bucket is the fuse blow time at that limit (divided by our period time) times the difference between the current AT that level and the current at the forever level, squared.
  i2p1x5 = float(safetyfactor * float(TIME1X5)) * (sq(current1x5 - current1x1) / 100);

  //look up I2t heating of resistors.  also the fuse datasheet is helpful here, and littelfuse has a separate app note on fuse heating that may cause me to throw this entire code out. and go with something more math-based.

  current2x = currentbase * 2;
  cur2xdisplay = current2x / 10;
  i2p2x = float(safetyfactor * float(TIME2X)) * (sq(current2x - current1x1) / 100);


  current3x = currentbase * 3;
  cur3xdisplay = current3x / 10;
  i2p3x = float(safetyfactor * float(TIME3X)) * (sq(current3x - current1x1) / 100);


  //and of course, since the fuse heating of one bucket contributes also to the fuse heating of another bucket, we need to resize our buckets based on how much the fuse could have heated up before you got into your current bucket
  //this all works out in the end since a lower current can use a higher bucket if it's the only thing left.
  i2p1x5 -= i2p2x;
  i2p2x -= i2p3x;

  i2p1x5divisor = float(PERCENT) / float(i2p1x5);
  i2p2xdivisor = float(PERCENT) / float(i2p2x);
  i2p3xdivisor = float(PERCENT) / float(i2p3x);

  limpscale = (float(1) - LIMPLEVEL) / float(limphyst - limpcutoff);

  if (DEBUGMODE) {
    Serial.print(millis(), DEC);
    Serial.print(" : ");
    Serial.println("Fuse parameters set");
  }

  if (!digitalRead(DPIN_REVERSE))
  {
    //if we boot up with reverse held down, we log fewer variables, but more often
    fastlog = true;
    if (DEBUGMODE) {
      Serial.print(millis(), DEC);
      Serial.print(" : ");
      Serial.println("Fast Logging Mode");
    }
  }
  else
  {
    fastlog = false;
    if (DEBUGMODE) {
      Serial.print(millis(), DEC);
      Serial.print(" : ");
      Serial.println("Slow Logging Mode");
    }
  }


  //assume that we shouldnt be logging, unless we can open a file
  logging = false;
  if (SD.begin(CPIN_SPISS))
  {
    strcpy(filename, "PRSLOG00.csv");                      //basic file name template to look for
    for (filename[6] = '0'; filename[6] <= '9'; filename[6]++)
    { //change the tens and ones to look for a new file name
      for (filename[7] = '0'; filename[7] <= '9'; filename[7]++)
      { //hopefully we can find one where a file doesn;t already exist
        if (!SD.exists(filename))                                            //if we do . . .
        {
          f = SD.open(filename, FILE_WRITE);                                  //make it our new file
          if (f)
          {
            logging = true;
            if (DEBUGMODE) {
              Serial.print(millis(), DEC);
              Serial.print(" : ");
              Serial.print("Filename: ");
              Serial.write(filename, 12);
              Serial.println("");
            }

            //save these bytes because we're going to force them to '9' to kick us out of this loop
            filename[6] = '9';                                           //we found a name for our new file
            filename[7] = '9';
          }
          else
          {
            if (DEBUGMODE) {
              Serial.print(millis(), DEC);
              Serial.print(" : ");
              Serial.println("File Error");
            }
          }
        }
      }
    }
  }
  else
  {
    if (DEBUGMODE) {
      Serial.print(millis(), DEC);
      Serial.print(" : ");
      Serial.println("Logging Not Started");
    }
  }
  digitalWrite(DPIN_LED, HIGH);
  delay(50);
  if (logging)
  {
    if (DEBUGMODE) {
      Serial.print(millis(), DEC);
      Serial.print(" : ");
      Serial.println("Writing Headers");
    }
    writeHeaders();
  }




  //initialize our running amp-pulses tally
  amppulses = 0;
  wattpulses = 0;
  //if the controller boots up and the throttle is held down, wait here until it returns to zero
  if (analogRead(APIN_THROTTLE) > THROTTLEMIN)
  {
    if (DEBUGMODE) {
      Serial.print(millis(), DEC);
      Serial.print(" : ");
      Serial.println("Waiting for safe throttle");
    }
    digitalWrite(DPIN_LED, HIGH);
    while (analogRead(APIN_THROTTLE) > THROTTLEMIN)
    {

    }
    digitalWrite(DPIN_LED, LOW);
  }


  numtriggers = 0;
  limit = current3x;
  unsigned int tempvali = 0;
  if (logging)
  {
    loopssincelog = LOGINTERVAL;
    skiplog = false;//lets us know that we need to make a log right away
  }
  if (DEBUGMODE) {
    Serial.print(millis(), DEC);
    Serial.print(" : ");
    Serial.println("Retrieving lifetime odometer");
  }
  EEPROM.get(ODOMETERCHECKADDRESS, tempvali);
  if (true)
  {
    unsigned long tempodo = 0;
    EEPROM.get(ODOMETERADDRESS, tempodo);
    tripref = tempodo;
    motorlife = tempodo;
  }
  else
  {
    if (DEBUGMODE) {
      Serial.print(millis(), DEC);
      Serial.print(" : ");
      Serial.println("Invalid Odometer Reading");
    }
    if (DEBUGMODE) {
      Serial.print(millis(), DEC);
      Serial.print(" : ");
      Serial.println("Setting Valid Odometer");
    }
    motorlife = 1150000;  //~46 miles?
    tripref = motorlife;
    unsigned long tempodo = motorlife;
    EEPROM.put(ODOMETERCHECKADDRESS, odometercheck);
    EEPROM.put(ODOMETERADDRESS, tempodo);
  }

  myspeed.begin(5, FREQMEASUREMULTI_INTERLEAVE);
  digitalWrite(DPIN_LED, LOW);
  delay(100);
  if (DEBUGMODE) {
    Serial.print(millis(), DEC);
    Serial.print(" : ");
    Serial.println("GO!");
  }
  if (BLUETOOTH)
  {
    Serial.print('<');
  }
  //digitalWrite(DPIN_LED, LOW);
  //Our loop timing reference starts now.  GO!
  prevmicros = micros();
  prevmillis = millis();
}

void loop() {                                            //main loop
  //take a sample
  sample(); //80uS
  calculatevalues(); //510uS

  control();    //20uS //act on the sample

  if (!skiplog)
  {
    logData();        //20uS
    skiplog = true;
  }
  mydata[LOG_LOOPS].val = prevsample;
  bufferData(); //add to the existing log//120uS


  //pause here to enforce our loop timing
  unsigned long startwait = micros();
  //digitalWrite(DPIN_LED, HIGH);
  //these look like they do conflicting things, but the different ways they're incremented actually let us have a maximum and a minimum loop time
  //if we waste a lot of time somewhere (I'm looking at you, datalog), this lets us 'catch up' on our timing


  while ((micros() - prevperiod) < MINPERIOD) {}
  //digitalWrite(DPIN_LED, LOW);

  prevsample = millis() - prevmillis;
  prevmillis += prevsample;
  loopssincelog += prevsample;

  prevmicros = prevperiod;
  prevperiod = micros();
  mydata[LOG_CPU].val = (PERCENT * (startwait - prevmicros)) / (prevperiod - prevmicros);
  digitalWrite(DPIN_LED, !logging);
}

</code>
Power Racing Series Wiki

User Tools

Site Tools

Page Tools
