/**
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);
}