273 lines
6.5 KiB
C++
273 lines
6.5 KiB
C++
/*
|
|
* Copyright (c) 2019, Ploopy Corporation, a Canadian federal corporation.
|
|
*
|
|
* This is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public Licensefalse
|
|
* along with this program; if not, write to the Free Software
|
|
* Foundation, Inc.
|
|
* 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
|
*/
|
|
|
|
#if defined(ARDUINO) && ARDUINO >= 100
|
|
#include "Arduino.h"
|
|
#endif
|
|
#include "Scroller.h"
|
|
|
|
Scroller::Scroller(void) {
|
|
state = HIHI;
|
|
lohif = false;
|
|
hilof = false;
|
|
lowA = 1023;
|
|
highA = 0;
|
|
cLowA = false;
|
|
cHighA = false;
|
|
lowIndexA = 0;
|
|
highIndexA = 0;
|
|
lowOverflowA = false;
|
|
highOverflowA = false;
|
|
lowB = 1023;
|
|
highB = 0;
|
|
cLowB = false;
|
|
cHighB = false;
|
|
lowIndexB = 0;
|
|
highIndexB = 0;
|
|
lowOverflowB = false;
|
|
highOverflowB = false;
|
|
scrollThresholdA = 0;
|
|
scrollThresholdB = 0;
|
|
}
|
|
|
|
int Scroller::scroll(int curA, int curB) {
|
|
if (lowOverflowA == false || highOverflowA == false)
|
|
calculateThresholdA(curA);
|
|
if (lowOverflowB == false || highOverflowB == false)
|
|
calculateThresholdB(curB);
|
|
|
|
bool LO = false;
|
|
bool HI = true;
|
|
bool sA, sB;
|
|
int ret = 0;
|
|
|
|
if (curA < scrollThresholdA)
|
|
sA = LO;
|
|
else
|
|
sA = HI;
|
|
|
|
if (curB < scrollThresholdB)
|
|
sB = LO;
|
|
else
|
|
sB = HI;
|
|
|
|
if (state == HIHI) {
|
|
if (sA == LO && sB == HI) {
|
|
state = LOHI;
|
|
if (hilof) {
|
|
ret = 1;
|
|
hilof = false;
|
|
}
|
|
} else if (sA == HI && sB == LO) {
|
|
state = HILO;
|
|
if (lohif) {
|
|
ret = -1;
|
|
lohif = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
else if (state == HILO) {
|
|
if (sA == HI && sB == HI) {
|
|
state = HIHI;
|
|
hilof = true;
|
|
lohif = false;
|
|
} else if (sA == LO && sB == LO) {
|
|
state = LOLO;
|
|
hilof = true;
|
|
lohif = false;
|
|
}
|
|
}
|
|
|
|
else if (state == LOLO) {
|
|
if (sA == HI && sB == LO) {
|
|
state = HILO;
|
|
if (lohif) {
|
|
ret = 1;
|
|
lohif = false;
|
|
}
|
|
} else if (sA == LO && sB == HI) {
|
|
state = LOHI;
|
|
if (hilof) {
|
|
ret = -1;
|
|
hilof = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
else { // state must be LOHI
|
|
if (sA == HI && sB == HI) {
|
|
state = HIHI;
|
|
lohif = true;
|
|
hilof = false;
|
|
} else if (sA == LO && sB == LO) {
|
|
state = LOLO;
|
|
lohif = true;
|
|
hilof = false;
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
int Scroller::calculateThresholdA(int curA) {
|
|
scrollThresholdA = calculateThreshold(curA, lowA, highA,
|
|
cLowA, cHighA, arLowA, arHighA, lowIndexA, highIndexA,
|
|
lowOverflowA, highOverflowA);
|
|
}
|
|
|
|
int Scroller::calculateThresholdB(int curB) {
|
|
scrollThresholdB = calculateThreshold(curB, lowB, highB,
|
|
cLowB, cHighB, arLowB, arHighB, lowIndexB, highIndexB,
|
|
lowOverflowB, highOverflowB);
|
|
}
|
|
|
|
int Scroller::calculateThreshold(int cur, int &low, int &high,
|
|
bool &cLow, bool &cHigh, int arLow[], int arHigh[],
|
|
int &lowIndex, int &highIndex, bool &lowOverflow,
|
|
bool &highOverflow) {
|
|
|
|
if (cur < low)
|
|
low = cur;
|
|
if (cur > high)
|
|
high = cur;
|
|
|
|
int curThresh = thresholdEquation(low, high);
|
|
int range = high - low;
|
|
|
|
// The range is enforced to be over a certain limit because noise
|
|
// can cause erroneous readings, making these calculations unstable.
|
|
if (range >= THRESH_RANGE_LIM) {
|
|
if (cur < curThresh) {
|
|
if (cHigh == true) {
|
|
// We were just high, and now we crossed to low.
|
|
// high reflects a sample of a high reading.
|
|
arHigh[highIndex] = high;
|
|
incrementIndex(highIndex, highOverflow);
|
|
int midpoint = ((high - low) / 2) + low;
|
|
low = midpoint;
|
|
high = midpoint;
|
|
cLow = false;
|
|
cHigh = false;
|
|
} else {
|
|
cLow = true;
|
|
}
|
|
} if (cur > curThresh) {
|
|
if (cLow == true) {
|
|
// We were just low, and now we crossed to high.
|
|
// low reflects a sample of a low reading.
|
|
arLow[lowIndex] = low;
|
|
incrementIndex(lowIndex, lowOverflow);
|
|
int midpoint = ((high - low) / 2) + low;
|
|
low = midpoint;
|
|
high = midpoint;
|
|
cLow = false;
|
|
cHigh = false;
|
|
} else {
|
|
cHigh = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
int calcHigh = 0;
|
|
if (highOverflow == true) {
|
|
for (int i = 0; i < SCROLLER_AR_SIZE; i++) {
|
|
calcHigh += arHigh[i];
|
|
}
|
|
calcHigh = calcHigh / SCROLLER_AR_SIZE;
|
|
} else if (highIndex != 0) {
|
|
for (int i = 0; i < highIndex; i++) {
|
|
calcHigh += arHigh[i];
|
|
}
|
|
calcHigh = calcHigh / highIndex;
|
|
} else {
|
|
calcHigh = high;
|
|
}
|
|
|
|
int calcLow = 0;
|
|
if (lowOverflow == true) {
|
|
for (int i = 0; i < SCROLLER_AR_SIZE; i++) {
|
|
calcLow += arLow[i];
|
|
}
|
|
calcLow = calcLow / SCROLLER_AR_SIZE;
|
|
} else if (lowIndex != 0) {
|
|
for (int i = 0; i < lowIndex; i++) {
|
|
calcLow += arLow[i];
|
|
}
|
|
calcLow = calcLow / lowIndex;
|
|
} else {
|
|
calcLow = low;
|
|
}
|
|
|
|
return thresholdEquation(calcLow, calcHigh);
|
|
}
|
|
|
|
int Scroller::thresholdEquation(int lo, int hi) {
|
|
return ((hi - lo) / 3) + lo;
|
|
}
|
|
|
|
void Scroller::incrementIndex(int &index, bool &ovflw) {
|
|
if (index < SCROLLER_AR_SIZE - 1)
|
|
index++;
|
|
else {
|
|
index = 0;
|
|
ovflw = true;
|
|
}
|
|
}
|
|
|
|
void Scroller::debug(void) {
|
|
/*
|
|
Serial.print(F("lowA: "));
|
|
Serial.print(lowA);
|
|
Serial.print(F(", highA: "));
|
|
Serial.print(highA);
|
|
Serial.print(F(", cLowA: "));
|
|
Serial.print(cLowA ? "true" : "false");
|
|
Serial.print(F(", cHighA: "));
|
|
Serial.print(cHighA ? "true" : "false");
|
|
Serial.print(F(", lowIndexA: "));
|
|
Serial.print(lowIndexA);
|
|
Serial.print(F(", highIndexA: "));
|
|
Serial.print(highIndexA);
|
|
Serial.print(F(", lowOverflowA: "));
|
|
Serial.print(lowOverflowA ? "true" : "false");
|
|
Serial.print(F(", highOverflowA: "));
|
|
Serial.println(highOverflowA ? "true" : "false");
|
|
|
|
Serial.print(F("arLowA: "));
|
|
for (int i = 0; i < SCROLLER_AR_SIZE; i++) {
|
|
Serial.print(arLowA[i]);
|
|
Serial.print(F(","));
|
|
}
|
|
Serial.println(".");
|
|
|
|
Serial.print(F("arHighA: "));
|
|
for (int i = 0; i < SCROLLER_AR_SIZE; i++) {
|
|
Serial.print(arHighA[i]);
|
|
Serial.print(F(","));
|
|
}
|
|
Serial.println(".");
|
|
*/
|
|
|
|
Serial.print(F(", thB: "));
|
|
Serial.print(scrollThresholdB);
|
|
Serial.print(F(", thA: "));
|
|
Serial.print(scrollThresholdA);
|
|
}
|