I am running a control program for an ebike on an Arduino MEGA2560 which uses this MCU: http://www.atmel.com/devices/atmega2560.aspx. Currently, the program loops every 100ms which gives me a sample rate of 10Hz for measurements and control signal. I would like at least 10 times that so 100Hz. How can I work out which specs I need based on the arduino MEGA2560 and the perf it's giving me? Thanks!
Here is my code, it basically calculates battery capacity, range as well as controls and smooths the throttle inputs:
#include <EEPROM.h>
#include <memorysaver.h>
//#include <TFT_HX8357.h> // Hardware-specific library
#include <URTouch.h>
#include <URTouchCD.h>
#include <memorysaver.h>
#include <UTFT.h>
//TFT_HX8357 myGLCD = TFT_HX8357(); // Invoke custom library#include <TFT_HX8357.h> // Hardware-specific library
//
//TFT_HX8357 myGLCD = TFT_HX8357(); // Invoke custom library
UTFT myGLCD(ILI9481,38,39,40,41);
URTouch myTouch(6, 5, 4, 3, 2);
int buttonst, horn=5, throttle=8, powermode=1, alarmsystem=1, xtouch, ytouch, k1=0, k2=0,modechange,ncycles, p=9, divider=12, addresstd=0;
float pushed, pushedt, timesp, timespm1, powtimesm, factor, powtimesp, start, elapsed, powtime, powresttime, shuntvoltage, batvoltage, internresistance=0.00112, current, power, consumah, capacstore, consumwh, consumbatper, consumbatrange, distance, distanceint, throttlein, throttleout, distancedisp, Totaldist, velocity, Totaldistcheck;
float throttlesmooth, capaccheck, capacactual,consumahstore, kpow[3]={0,0,0}, kcur[3]={0,0,0}, ksp[3]={0,0,0}, errorpower[2], errorpoweri, errorpowerli, errorcuri, errorcurrent[2], errorpowerl[2], diff, errorspeed[2], errorisp, outspeed, outcurrentctl, outspeedctl, elapsedtot, errorsmooth[2];
float batteryspec[6][56]={{4.2,4,3.94,3.90,3.876,3.86,3.852,3.844,3.826,3.809,3.795,3.783,3.765,3.744,3.728, 3.710,3.692,3.672,3.650,3.631,3.613,3.591,3.573,3.556,3.532,3.508,3.488,3.463,3.439,3.417,3.399,3.380,3.366,3.352,3.332,3.312,3.291,3.273,3.253,3.237,3.217,3.196,3.176,3.150,3.123,3.087,3.051,2.994,2.929,2.868,2.796,2.727,2.650,5.261,2.502},//5A discharge 18650GA SPEC
{0.001,0.002,0.015,0.047,0.085,0.133,0.186,0.244,0.309,0.383,0.447,0.506,0.554,0.612,0.681,0.735,0.793,0.852,0.921,1.007,1.081,1.150,1.236,1.300,1.358,1.438,1.518,1.587,1.667,1.747,1.822,1.888,1.955,2.014,2.072,2.152,2.232,2.317,2.389,2.466,2.524,2.594,2.658,2.722,2.786,2.847,2.925,2.983,3.063,3.133,3.186,3.245,3.293,3.330,3.357,3.368},
{4.20,4.02,3.980,3.945,3.927,3.914,3.904,3.890,3.874,3.860,3.843,3.821,3.797,3.776,3.756,3.734,3.713,3.695,3.675,3.652,3.634,3.614,3.593,3.569,3.543,3.522,3.5,3.480,3.459,3.441,3.419,3.398,3.378,3.360,3.339,3.321,3.301,3.278,3.254,3.230,3.199,3.164,3.128,3.085,3.037,2.990,2.937,2.878,2.837,2.795,2.746,2.693,2.648,2.598,2.549,2.496},
{0,0.007,0.033,0.084,0.151,0.223,0.294,0.377,0.457,0.521,0.574,0.646,0.718,0.785,0.851,0.931,1.011,1.091,1.171,1.251,1.323,1.397,1.469,1.547,1.632,1.701,1.778,1.853,1.928,1.994,2.088,2.175,2.258,2.333,2.413,2.487,2.562,2.634,2.706,2.775,2.842,2.908,2.697,3.023,3.074,3.116,3.159,3.205,3.234,3.261,3.287,3.314,3.333,3.352,3.365,3.379},//3A discharge 18650GA SPEC
{4.2,4.131,4.094,4.073,4.054,4.038,4.028,4.015,4.003,3.985,3.965,3.945,3.921,3.894,3.870,3.850,3.830,3.811,3.795,3.775,3.759,3.739,3.716,3.692,3.674,3.648,3.623,3.601,3.578,3.561,3.542,3.522,3.502,3.486,3.467,3.445,3.423,3.401,3.374,3.350,3.324,3.291,3.253,3.210,3.168,3.123,3.075,3.026,2.964,2.897,2.838,2.781,2.715,2.652,2.585,2.502},
{0,0,0.036,0.084,0.154,0.223,0.289,0.356,0.420,0.489,0.553,0.620,0.686,0.761,0.838,0.908,0.985,1.054,1.134,1.214,1.283,1.358,1.438,1.523,1.584,1.667,1.755,1.835,1.920,2.002,2.085,2.170,2.255,2.327,2.402,2.482,2.567,2.644,2.724,2.791,2.850,2.908,2.964,3.015,3.058,3.095,3.132,3.167,3.207,3.245,3.271,3.295,3.320,3.336,3.354,3.365}}; //1A discharge 18650GA SPEC
float cyclicspec[2][44]={{0,0.894,7.151,16.09,26.4,37.5,50.95,61.68,74.19,85.36,96.54,107.26,118.88,130.95,142.12,154.64,168.04,182.35,194.86,208.27,219.89,234.2,246.7,262.8,276.2,291.4,303.9,317.3,327.15,341.5,354,364,374.5,383.46,395.08,405.8,418.3,431.7,444,455.9,468.8,481.9,491.6,499.7},
{3441.3,3435.7,3342.9,3292.9,3235.7,3178.6,3121.4,3078.6,3014.28,2957.1,2921.4,2892.9,2842.9,2792.9,2750,2721.4,2678.6,2642.9,2607.1,2564.3,2542.9,2507.1,2478.6,2435.7,2407.1,2378.6,2357.1,2335.7,2335.7,2335.7,2335.7,2335.7,2335.7,2321.4,2300,2278.6,2271.4,2242.9,2228.6,2214.3,2192.9,2178.6,2164.3,2157.1}};
extern uint8_t BigFont[];
extern uint8_t SevenSegNumFont[];
void EEPROMWritelong(int address, long value)
{
//Decomposition from a long to 4 bytes by using bitshift.
//One = Most significant -> Four = Least significant byte
byte four = (value & 0xFF);
byte three = ((value >> 8) & 0xFF);
byte two = ((value >> 16) & 0xFF);
byte one = ((value >> 24) & 0xFF);
//Write the 4 bytes into the eeprom memory.
EEPROM.write(address, four);
EEPROM.write(address + 1, three);
EEPROM.write(address + 2, two);
EEPROM.write(address + 3, one);
}
float EEPROMReadlong(int address)
{
//Read the 4 bytes from the eeprom memory.
long four = EEPROM.read(address);
long three = EEPROM.read(address + 1);
long two = EEPROM.read(address + 2);
long one = EEPROM.read(address + 3);
//Return the recomposed long by using bitshift.
return ((four << 0) & 0xFF) + ((three << 8) & 0xFFFF) + ((two << 16) & 0xFFFFFF) + ((one << 24) & 0xFFFFFFFF);
}
float pidcurrent_control() {
float dt, outcurrent=0, outpower=0;
int i=0, descurrent, despower;
i++;
dt=millis()-powtime;
powtime = millis();
if (powermode==1) { //Furo default mode
if (current>40) {
descurrent=40;
errorcurrent[1]=descurrent-current;
if (i>=2) {
//Integrate the error
errorcuri+=errorcurrent[1]*dt;
outcurrent=(kcur[0]*errorcurrent[1]+kcur[1]*errorcuri+kcur[2]*((errorcurrent[1]-errorcurrent[0])/dt));
}
errorcurrent[0]=errorcurrent[1];
return outcurrent;
}
if ((batvoltage*current)>2000) {
despower=2000;
errorpower[1]=despower-batvoltage*current;
if (i>=2) {
//Integrate the error
errorpoweri+=errorpower[1]*dt;
outpower=kpow[0]*errorpower[1]+kpow[1]*errorpoweri+kpow[2]*((errorpower[1]-errorpower[0])/dt);
}
errorpower[0]=errorpower[1];
return outpower;
}
}
else { //Eco mode
despower=550;
errorpowerl[1]=despower-batvoltage*current;
if (i>=2) {
//Integrate the error
errorpowerli+=errorpowerl[1]*dt;
outpower=kpow[0]*errorpowerl[1]+kpow[1]*errorpowerli+kpow[2]*((errorpowerl[1]-errorpowerl[0])/dt);
}
errorpowerl[0]=errorpowerl[1];
return outpower;
}
}
void cyclescaling() {
int j=1;
if (ncycles<=499) {
while ((ncycles<cyclicspec[0][j-1])||(ncycles>cyclicspec[0][j])){
j++;
}
factor=(cyclicspec[1][j-1]+(ncycles-cyclicspec[0][j-1])*(cyclicspec[1][j]-cyclicspec[1][j-1])/(cyclicspec[0][j]-cyclicspec[0][j-1]))/cyclicspec[1][0];
}
else {
factor=(cyclicspec[1][41]+(ncycles-cyclicspec[0][41])*(cyclicspec[1][42]-cyclicspec[1][41])/(cyclicspec[0][42]-cyclicspec[0][41]))/cyclicspec[1][0];
}
}
float pidspeed_control() {
int isp, dtsp;
isp++;
dtsp=millis()-powtimesp;
powtimesp = millis();
errorspeed[1]=46-velocity;
if (isp>=2) {
errorisp+=errorspeed[1]*dtsp;
outspeed=(ksp[0]*errorspeed[1]+ksp[1]*errorisp+ksp[2]*((errorspeed[1]-errorspeed[0])/dtsp));
}
errorspeed[0]=errorspeed[1];
return outspeed;
}
void throttlesmoothing() {
int i=0, dtsm;
i++;
float currentmap=map(throttlein,0.8,3.6,0,40);
dtsm=millis()-powtimesm;
powtimesm = millis();
errorsmooth[1]=currentmap-current;
if (i>=2) {
//Integrate the error
throttlesmooth=kcur[0]*errorsmooth[1]+kcur[2]*((errorsmooth[1]-errorsmooth[0])/dtsm);
}
errorsmooth[0]=errorsmooth[1];
}
void SOC() {
float currentref=current/p;
int j=1,l=1;
float stateofc1=0, stateofc2=0;
if (currentref<=1){
while (((batvoltage/14)>batteryspec[0][j-1])||((batvoltage/14)<batteryspec[0][j])){
j++;
}
capacactual=(batteryspec[1][55]-(batteryspec[1][j-1]+(batvoltage/14-batteryspec[0][j-1])*(batteryspec[1][j]-batteryspec[1][j-1])/(batteryspec[0][j]-batteryspec[0][j-1])))*p;
}
if ((currentref>1)&&(currentref<=3)){
while (((batvoltage/14)>batteryspec[0][j-1])||((batvoltage/14)<batteryspec[0][j])){
j++;
}
stateofc1=(batteryspec[1][55]-(batteryspec[1][j-1]+(batvoltage/14-batteryspec[0][j-1])*(batteryspec[1][j]-batteryspec[1][j-1])/(batteryspec[0][j]-batteryspec[0][j-1])))*p;
while (((batvoltage/14)>batteryspec[2][l-1])||((batvoltage/14)<batteryspec[2][l])){
l++;
}
stateofc2=(batteryspec[3][55]-(batteryspec[3][l-1]+(batvoltage/14-batteryspec[2][l-1])*(batteryspec[3][l]-batteryspec[3][l-1])/(batteryspec[2][l]-batteryspec[2][l-1])))*p;
capacactual=stateofc1+(currentref-1)*(stateofc2-stateofc1)/(3-1);
}
if ((currentref>3)&&(currentref<=5)){
while (((batvoltage/14)>batteryspec[2][j-1])||((batvoltage/14)<batteryspec[2][j])){
j++;
}
// stateofc1=(batteryspec[3][55]-(batteryspec[3][j-1]+(batvoltage/14-batteryspec[2][j-1])*(batteryspec[3][j]-batteryspec[3][j-1])/(batteryspec[2][j]-batteryspec[2][j-1])))*p;
while (((batvoltage/14)>batteryspec[2][l-1])||((batvoltage/14)<batteryspec[2][l])){
l++;
}
// stateofc2=(batteryspec[3][55]-(batteryspec[3][l-1]+(batvoltage/14-batteryspec[2][l-1])*(batteryspec[3][l]-batteryspec[3][l-1])/(batteryspec[2][l]-batteryspec[2][l-1])))*p;
capacactual=(batteryspec[3][55]-(batteryspec[3][j-1]+(batvoltage/14-batteryspec[2][j-1])*(batteryspec[3][j]-batteryspec[3][j-1])/(batteryspec[2][j]-batteryspec[2][j-1])))*p+(currentref-1)*((batteryspec[5][55]-(batteryspec[5][l-1]+(batvoltage/14-batteryspec[4][l-1])*(batteryspec[5][l]-batteryspec[5][l-1])/(batteryspec[4][l]-batteryspec[4][l-1])))*p-(batteryspec[3][55]-(batteryspec[3][j-1]+(batvoltage/14-batteryspec[2][j-1])*(batteryspec[3][j]-batteryspec[3][j-1])/(batteryspec[2][j]-batteryspec[2][j-1])))*p)/(5-3);
}
if (currentref>5) {
while (((batvoltage/14)>batteryspec[4][j-1])||((batvoltage/14)<batteryspec[4][j])){
j++;
}
capacactual=(batteryspec[5][55]-(batteryspec[5][j-1]+(batvoltage/14-batteryspec[4][j-1])*(batteryspec[5][j]-batteryspec[5][j-1])/(batteryspec[4][j]-batteryspec[4][j-1])))*p;
}
}
void autonomy() {
if (modechange==1) {
consumah=0;
}
power=batvoltage*current;
SOC();
if (capacactual!=capaccheck) {
capaccheck=(capacactual+capaccheck)/2;
}
if (abs(capacactual-capacstore)>=0.1){
EEPROMWritelong(8,roundf(capacactual*1000));
capacstore=capacactual;
}
consumah += current*elapsed/1000/3600;
capaccheck -= current*elapsed/1000/3600;
consumwh += current*elapsed/1000/3600*batvoltage;
elapsedtot += elapsed;
consumbatper = (capaccheck)/(batteryspec[1][55]*p)*100;
if (distance==0) {
switch (powermode) {
case 1: consumbatrange=45*batvoltage*capaccheck/980;
break;
case 2: consumbatrange=35*batvoltage*capaccheck/550;
break;
}
} else {
consumbatrange = distance/consumah*(capaccheck);
}
myGLCD.printNumF(consumbatrange, 2, 350,275);
myGLCD.printNumF(power, 2, 350,205);
myGLCD.printNumF(current, 1, 30,50);
myGLCD.printNumF(ncycles, 1, 30,50+16*2);
myGLCD.printNumF(consumbatper,1,350,50);
myGLCD.printNumF(batvoltage,1,350,50+16*2);
return 0;
}
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
start=millis();
pinMode(43, OUTPUT);//Speedo power
pinMode(46, INPUT);//Speedometer
pinMode(A2, INPUT);//Throttle in
pinMode(A7, INPUT);//Battery voltage
pinMode(A4, INPUT);//Shunt
pinMode(throttle, OUTPUT);
pinMode(11, OUTPUT);
pinMode(10, OUTPUT);
pinMode(47, INPUT);
analogWrite(10,255);
analogWrite(11,255);
myGLCD.InitLCD(LANDSCAPE);
myTouch.InitTouch(LANDSCAPE);
myGLCD.fillScr(0,0,0);
myGLCD.setColor(VGA_WHITE);
myGLCD.setBackColor(0,0,0);
myGLCD.setFont(BigFont);
myGLCD.print("FURO SYSTEMS",(480-16*12)/2,16);
myGLCD.print("WELCOME",(480-16*7)/2,(320-16)/2);
myGLCD.setDisplayPage(0);
delay(2000);
myGLCD.clrScr();
myGLCD.print("FURO SYSTEMS",(480-16*12)/2,16);
myGLCD.fillRoundRect((480-3*16-8)/2,320-16*2-4,(480-3*16-8)/2+3*16+6,228);
myGLCD.print("ECO", (480-3*16)/2, 320-16*5);
// EEPROMWritelong(4,1);
ncycles=EEPROMReadlong(4);
cyclescaling();
for (int m=1; m<=5;m+=2) {
for (int n=0; n<=55;n++) {
batteryspec[m][n]*=factor;
}
}
current=analogRead(A4);
current=current/internresistance*5/1023/75;;
batvoltage=analogRead(A7);
batvoltage=batvoltage*5*divider/1023;
SOC();
capaccheck=capacactual;
capacstore=EEPROMReadlong(8)/1000;
if (capacactual>capacstore) {
ncycles++;
EEPROMWritelong(4,ncycles);
EEPROMWritelong(8,roundf(capacactual*1000));
}
if (capacactual<0.5) {
powermode=2;
myGLCD.fillRoundRect((480-4*16-8)/2,320-16*2-4,(480-4*16-8)/2+4*16+6,228);
myGLCD.print("FURO", (480-4*16)/2, 320-16*5);
}//3.21V per cell
EEPROMReadlong(addresstd);
Totaldist=EEPROMReadlong(addresstd);
Totaldistcheck=Totaldist;
digitalWrite(43,HIGH);
}
void loop() {
throttlein=analogRead(A2);
throttlein=throttlein*5/1023;
throttlesmoothing();
throttleout=round((0.4+throttlein)*255/5);
analogWrite(8,throttleout);
modechange=0;
if (digitalRead(47)==LOW)
{
if (buttonst==1) {
pushed = millis();
}
buttonst=0;
if ((millis()-pushed)>=2000)
{
distancedisp=0;
}
}
if (digitalRead(47)==HIGH) {
if (buttonst==0) {
modechange=1;
pushedt=millis()-pushed;
if (pushedt<1900){
if (powermode==1)
{ powermode=2;
myGLCD.clrScr();
myGLCD.print("FURO SYSTEMS",(480-16*12)/2,16);
myGLCD.fillRoundRect((480-4*16-8)/2,320-16*2-4,(480-4*16-8)/2+4*16+6,228);
myGLCD.print("FURO", (480-4*16)/2, 320-16*5);
}
else
{
powermode=1;
myGLCD.clrScr();
myGLCD.print("FURO SYSTEMS",(480-16*12)/2,16);
myGLCD.fillRoundRect((480-3*16-8)/2,320-16*2-4,(480-3*16-8)/2+3*16+6,228);
myGLCD.print("ECO", (480-3*16)/2, 320-16*5);
}
}
}
buttonst=1;
}
batvoltage=analogRead(A7);
batvoltage=batvoltage*5*divider/1023;
current=analogRead(A4);
current=current/internresistance*5/1023/75;
elapsed = millis() - start;
start = millis();
if (modechange==1) {
distance=0;
}
distanceint = (velocity/3.6*elapsed*0.001)/1000;
distance += distanceint;
distancedisp += distanceint;
Totaldist += distanceint;
if (abs(Totaldist-Totaldistcheck)>=0.1) { //later do it upon switch off
EEPROMWritelong(addresstd,round(Totaldist));
Totaldistcheck=Totaldist;
}
autonomy();
if (velocity>45) {
outspeedctl=pidspeed_control();
outcurrentctl=pidcurrent_control();
float throttleout1=min(outspeedctl,outcurrentctl);
throttleout=min(throttleout1,throttlesmooth);
analogWrite(throttle, map(throttleout, 0,5,0,255));
} else{
outcurrentctl=5;
outcurrentctl=pidcurrent_control();
throttleout=min(throttlesmooth,outcurrentctl);
analogWrite(throttle, map(throttleout, 0,5,0,255));
}
myGLCD.setFont(SevenSegNumFont);
myGLCD.printNumI(velocity, (480-32*3)/2, (320-50)/2);
myGLCD.setFont(BigFont);
myGLCD.printNumF(distancedisp, 1, 30,175);
myGLCD.printNumI(Totaldist, 30,175+2*16);
}