232 lines
6.6 KiB
C++
232 lines
6.6 KiB
C++
/*
|
||
0004_DenshaBekutoru – State Machine implemented
|
||
Target dev board (later): Arduino Pro Mini 5V / 16 MHz (ATmega328P)
|
||
|
||
----------------------------------------------------------------------------
|
||
State machine (direction memory)
|
||
----------------------------------------------------------------------------
|
||
Inputs derived from v1, v2, VdiffThreshold:
|
||
|
||
N (Neutral): |v1 - v2| <= VdiffThreshold
|
||
D1 (v1>>v2): (v1 - v2) > VdiffThreshold
|
||
D2 (v2>>v1): (v2 - v1) > VdiffThreshold
|
||
|
||
State encoding (kept transfer-friendly, and compatible with earlier pin-mapping):
|
||
0 = NONE
|
||
2 = DIR1
|
||
3 = DIR2
|
||
|
||
Transition table:
|
||
|
||
Current \ Input | N (neutral) | D1 (v1>>v2) | D2 (v2>>v1)
|
||
-----------------------------------------------------------------
|
||
0 (NONE) | 0 | 2 | 3
|
||
2 (DIR1) | 2 | 2 | 3
|
||
3 (DIR2) | 3 | 2 | 3
|
||
|
||
Mermaid (documentation)
|
||
-----------------------
|
||
stateDiagram-v2
|
||
[*] --> NONE
|
||
state "0 : NONE" as NONE
|
||
state "2 : DIR1 (v1>>v2)" as DIR1
|
||
state "3 : DIR2 (v2>>v1)" as DIR2
|
||
|
||
NONE --> NONE : |v1 - v2| <= T
|
||
NONE --> DIR1 : v1 - v2 > T
|
||
NONE --> DIR2 : v2 - v1 > T
|
||
|
||
DIR1 --> DIR1 : |v1 - v2| <= T
|
||
DIR1 --> DIR1 : v1 - v2 > T
|
||
DIR1 --> DIR2 : v2 - v1 > T
|
||
|
||
DIR2 --> DIR2 : |v1 - v2| <= T
|
||
DIR2 --> DIR2 : v2 - v1 > T
|
||
DIR2 --> DIR1 : v1 - v2 > T
|
||
*/
|
||
|
||
const unsigned long SAMPLE_WINDOW_MS = 100; // averaging window per measurement
|
||
const unsigned long SAMPLE_DELAY_US = 500; // delay between ADC samples (~2 kHz)
|
||
|
||
const uint8_t ADC_PIN_1 = A2; // Pro Mini A2 -> later ATtiny85 PB3 (XTAL1, Pin 2)
|
||
const uint8_t ADC_PIN_2 = A3; // Pro Mini A3 -> later ATtiny85 PB4 (XTAL2, Pin 3)
|
||
|
||
// Serial output control (0 = no output, 1 = output)
|
||
bool SerialOutputAllow = true;
|
||
|
||
// ---- Calibration parameters ----
|
||
const uint16_t AverageRepeat = 100; // number of init measurements
|
||
const float VdiffThresholdMargin = 1.50f; // multiplier (e.g., 1.5 = +50% margin)
|
||
const float VdiffThresholdMinVolts = 0.03f; // floor in volts (optional but practical)
|
||
|
||
// Globals: latest measured averaged voltages
|
||
float v1 = 0.0f;
|
||
float v2 = 0.0f;
|
||
|
||
// Calibration globals
|
||
float VdiffBaseline = 0.0f; // median(|V1-V2|) measured at boot (no movement)
|
||
float VdiffThreshold = 0.0f; // final threshold used for direction decision
|
||
|
||
// Direction encoding:
|
||
// 0 = NONE, 2 = DIR1, 3 = DIR2
|
||
byte direction = 0;
|
||
|
||
static inline float absf(float x) { return (x >= 0.0f) ? x : -x; }
|
||
static inline float maxf(float a, float b) { return (a > b) ? a : b; }
|
||
|
||
// Measure averaged voltages (updates globals v1/v2)
|
||
static void measureAveragedVoltages()
|
||
{
|
||
unsigned long startTime = millis();
|
||
|
||
unsigned long sum1 = 0;
|
||
unsigned long sum2 = 0;
|
||
unsigned long samples = 0;
|
||
|
||
while (millis() - startTime < SAMPLE_WINDOW_MS) {
|
||
sum1 += analogRead(ADC_PIN_1);
|
||
sum2 += analogRead(ADC_PIN_2);
|
||
samples++;
|
||
delayMicroseconds(SAMPLE_DELAY_US);
|
||
}
|
||
|
||
float avg1 = (float)sum1 / samples;
|
||
float avg2 = (float)sum2 / samples;
|
||
|
||
// Convert ADC value to voltage (default analog reference = Vcc ~ 5V)
|
||
v1 = avg1 * (5.0f / 1023.0f);
|
||
v2 = avg2 * (5.0f / 1023.0f);
|
||
}
|
||
|
||
static void sortFloatArray(float *a, uint16_t n)
|
||
{
|
||
// Insertion sort (n=100 is small, OK)
|
||
for (uint16_t i = 1; i < n; i++) {
|
||
float key = a[i];
|
||
int16_t j = (int16_t)i - 1;
|
||
while (j >= 0 && a[j] > key) {
|
||
a[j + 1] = a[j];
|
||
j--;
|
||
}
|
||
a[j + 1] = key;
|
||
}
|
||
}
|
||
|
||
static float medianOfSortedFloatArray(const float *a, uint16_t n)
|
||
{
|
||
if (n == 0) return 0.0f;
|
||
if (n & 1) {
|
||
return a[n / 2];
|
||
} else {
|
||
return (a[(n / 2) - 1] + a[n / 2]) * 0.5f;
|
||
}
|
||
}
|
||
|
||
// Calibrate baseline + threshold once at boot/reset
|
||
// Parameter must be only AverageRepeat (per your requirement).
|
||
static void calibrateVdiffThreshold(uint16_t averageRepeat)
|
||
{
|
||
if (averageRepeat < 1) averageRepeat = 1;
|
||
if (averageRepeat > 200) averageRepeat = 200; // RAM safety cap
|
||
|
||
float diffs[200];
|
||
|
||
for (uint16_t i = 0; i < averageRepeat; i++) {
|
||
measureAveragedVoltages();
|
||
diffs[i] = absf(v1 - v2); // baseline mismatch sample
|
||
}
|
||
|
||
sortFloatArray(diffs, averageRepeat);
|
||
VdiffBaseline = medianOfSortedFloatArray(diffs, averageRepeat);
|
||
|
||
// Final threshold with margin + floor
|
||
VdiffThreshold = maxf(VdiffBaseline * VdiffThresholdMargin, VdiffThresholdMinVolts);
|
||
}
|
||
|
||
// Implements the state machine table above
|
||
static void updateDirectionFromVoltages()
|
||
{
|
||
const float diff = v1 - v2;
|
||
const float absDiff = absf(diff);
|
||
|
||
// Neutral region: hold direction (only NONE stays NONE)
|
||
if (absDiff <= VdiffThreshold) {
|
||
// direction remains unchanged (0 stays 0; 2 stays 2; 3 stays 3)
|
||
return;
|
||
}
|
||
|
||
// Clear dominance: set direction deterministically
|
||
// diff > 0 => v1 >> v2 => DIR1 (2)
|
||
// diff < 0 => v2 >> v1 => DIR2 (3)
|
||
direction = (diff > 0.0f) ? (byte)2 : (byte)3;
|
||
}
|
||
|
||
static void serialPrintAll()
|
||
{
|
||
if (!SerialOutputAllow) return;
|
||
|
||
Serial.print("A2 for PB3 XTAL1, Pin2: ");
|
||
Serial.print(v1, 3);
|
||
Serial.print(" V | ");
|
||
|
||
Serial.print("A3 for PB4 XTAL2, Pin3: ");
|
||
Serial.print(v2, 3);
|
||
Serial.print(" V | ");
|
||
|
||
Serial.print("|V1-V2|: ");
|
||
Serial.print(absf(v1 - v2), 3);
|
||
Serial.print(" V | ");
|
||
|
||
Serial.print("VdiffBaseline: ");
|
||
Serial.print(VdiffBaseline, 3);
|
||
Serial.print(" V | ");
|
||
|
||
Serial.print("VdiffThresholdMargin: ");
|
||
Serial.print(VdiffThresholdMargin, 2);
|
||
Serial.print(" x | ");
|
||
|
||
Serial.print("VdiffThresholdMinVolts: ");
|
||
Serial.print(VdiffThresholdMinVolts, 3);
|
||
Serial.print(" V | ");
|
||
|
||
Serial.print("VdiffThreshold: ");
|
||
Serial.print(VdiffThreshold, 3);
|
||
Serial.print(" V | ");
|
||
|
||
Serial.print("direction: ");
|
||
Serial.println(direction);
|
||
}
|
||
|
||
void setup() {
|
||
Serial.begin(115200);
|
||
|
||
pinMode(ADC_PIN_1, INPUT);
|
||
pinMode(ADC_PIN_2, INPUT);
|
||
|
||
// Initial calibration: compute VdiffThreshold once after boot/reset
|
||
calibrateVdiffThreshold(AverageRepeat);
|
||
|
||
// One measurement for initial display
|
||
measureAveragedVoltages();
|
||
updateDirectionFromVoltages();
|
||
|
||
if (SerialOutputAllow) {
|
||
Serial.println("=== DenshaBekutoru State Machine and Variable Output (Arduino Pro Mini 5V/16MHz) ===");
|
||
Serial.println("~~~ Init ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
|
||
Serial.print("AverageRepeat: ");
|
||
Serial.println(AverageRepeat);
|
||
Serial.println("~~~ Init ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
|
||
}
|
||
|
||
serialPrintAll();
|
||
}
|
||
|
||
void loop() {
|
||
measureAveragedVoltages();
|
||
updateDirectionFromVoltages();
|
||
|
||
serialPrintAll();
|
||
|
||
delay(300);
|
||
}
|