diff --git a/firmware/ArduinoTest/Def_InOut_PrepLogic/Def_InOut_PrepLogic.ino b/firmware/ArduinoTest/Def_InOut_PrepLogic/Def_InOut_PrepLogic.ino new file mode 100644 index 0000000..d16db3b --- /dev/null +++ b/firmware/ArduinoTest/Def_InOut_PrepLogic/Def_InOut_PrepLogic.ino @@ -0,0 +1,217 @@ +/* + 0004_DenshaBekutoru – Optocoupler Averaging Test + Target dev board (later): Arduino Pro Mini 5V / 16 MHz (ATmega328P) + + Measures average voltage on: + - A2 -> later ATtiny85 PB3 (XTAL1, Pin 2) + - A3 -> later ATtiny85 PB4 (XTAL2, Pin 3) + + Boot calibration: + - assumes "no movement" at startup + - measures V1/V2 AverageRepeat times + - computes di = |V1 - V2| each time + - VdiffBaseline = median(di) + - VdiffThreshold = max(VdiffBaseline * VdiffThresholdMargin, VdiffThresholdMinVolts) +*/ + +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) + +// LEDs for direction indication (avoid D0/D1) +const uint8_t LED_DIR1_PIN = 2; // D2 +const uint8_t LED_DIR2_PIN = 3; // D3 + +// 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 requirement: +// 0 = no direction +// 2 = direction 1 (maps to LED pin D2) +// 3 = direction 2 (maps to LED pin D3) +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; } + +static void applyDirectionOutputs() +{ + digitalWrite(LED_DIR1_PIN, LOW); + digitalWrite(LED_DIR2_PIN, LOW); + + if (direction == LED_DIR1_PIN) { + digitalWrite(LED_DIR1_PIN, HIGH); + } else if (direction == LED_DIR2_PIN) { + digitalWrite(LED_DIR2_PIN, HIGH); + } +} + +// 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); +} + +static void updateDirectionFromVoltages() +{ + float diff = v1 - v2; + float absDiff = absf(diff); + + if (absDiff <= VdiffThreshold) { + direction = 0; + } else { + // Mapping choice: + // v1 lower than v2 -> direction 1 (D2) + // v2 lower than v1 -> direction 2 (D3) + direction = (diff < 0.0f) ? LED_DIR1_PIN : LED_DIR2_PIN; + } +} + +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); + + pinMode(LED_DIR1_PIN, OUTPUT); + pinMode(LED_DIR2_PIN, OUTPUT); + + calibrateVdiffThreshold(AverageRepeat); + + // One measurement for initial output + outputs + measureAveragedVoltages(); + updateDirectionFromVoltages(); + applyDirectionOutputs(); + + if (SerialOutputAllow) { + Serial.println("=== DenshaBekutoru Optocoupler Average Test (Arduino Pro Mini 5V/16MHz) ==="); + Serial.print("AverageRepeat: "); + Serial.println(AverageRepeat); + } + + serialPrintAll(); +} + +void loop() { + measureAveragedVoltages(); + updateDirectionFromVoltages(); + applyDirectionOutputs(); + + serialPrintAll(); + + delay(300); +} diff --git a/firmware/ArduinoTest/enshaBekutoru_0004___Version_0.1_UnoTest/enshaBekutoru_0004___Version_0.1_UnoTest.ino b/firmware/ArduinoTest/enshaBekutoru_0004___Version_0.1_UnoTest/enshaBekutoru_0004___Version_0.1_UnoTest.ino new file mode 100644 index 0000000..99054a7 --- /dev/null +++ b/firmware/ArduinoTest/enshaBekutoru_0004___Version_0.1_UnoTest/enshaBekutoru_0004___Version_0.1_UnoTest.ino @@ -0,0 +1,292 @@ +/* + 0004_DenshaBekutoru – Version 0.1 Test + 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: + 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) + +// Direction LEDs (avoid D0/D1 because you may want RX/TX later) +const uint8_t LED_DIR1_PIN = 2; // D2 +const uint8_t LED_DIR2_PIN = 3; // D3 + +const unsigned int LED_BLINK_ON_MS = 150; +const unsigned int LED_BLINK_OFF_MS = 150; + +// 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 + +// 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; } + +// ----------------------------------------------------------------------------- +// LED helpers +// ----------------------------------------------------------------------------- +static void blinkLED(uint8_t pin, uint8_t times) +{ + for (uint8_t i = 0; i < times; i++) { + digitalWrite(pin, HIGH); + delay(LED_BLINK_ON_MS); + digitalWrite(pin, LOW); + delay(LED_BLINK_OFF_MS); + } +} + +static void initBlinkSequence() +{ + // LED at Output 2 blinks 2 times + blinkLED(LED_DIR1_PIN, 2); + + // LED at Output 3 blinks 3 times + blinkLED(LED_DIR2_PIN, 3); +} + +static void applyDirectionOutputs() +{ + digitalWrite(LED_DIR1_PIN, LOW); + digitalWrite(LED_DIR2_PIN, LOW); + + if (direction == 2) { + digitalWrite(LED_DIR1_PIN, HIGH); + } else if (direction == 3) { + digitalWrite(LED_DIR2_PIN, HIGH); + } +} + +// ----------------------------------------------------------------------------- +// Measurement +// ----------------------------------------------------------------------------- +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); +} + +// ----------------------------------------------------------------------------- +// Median helpers (for calibration) +// ----------------------------------------------------------------------------- +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. +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); +} + +// ----------------------------------------------------------------------------- +// State machine +// ----------------------------------------------------------------------------- +static void updateDirectionFromVoltages() +{ + const float diff = v1 - v2; + const float absDiff = absf(diff); + + // Neutral region: hold direction + if (absDiff <= VdiffThreshold) { + 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; +} + +// ----------------------------------------------------------------------------- +// Serial output +// ----------------------------------------------------------------------------- +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); +} + +// ----------------------------------------------------------------------------- +// Arduino entry points +// ----------------------------------------------------------------------------- +void setup() { + Serial.begin(115200); + + pinMode(ADC_PIN_1, INPUT); + pinMode(ADC_PIN_2, INPUT); + + pinMode(LED_DIR1_PIN, OUTPUT); + pinMode(LED_DIR2_PIN, OUTPUT); + + // Initial calibration (assumes no movement) + calibrateVdiffThreshold(AverageRepeat); + + // Init blink sequence: 2x on D2, 3x on D3 + initBlinkSequence(); + + // One measurement for initial display + initial direction + measureAveragedVoltages(); + updateDirectionFromVoltages(); + applyDirectionOutputs(); + + if (SerialOutputAllow) { + Serial.println("=== DenshaBekutoru 0004 – Version 0.1 Test (Arduino Uno Test) ==="); + Serial.println(" ~~~ InitStart ~~~"); + Serial.print(" AverageRepeat: "); + Serial.println(AverageRepeat); + Serial.println(" ~~~ InitEnd ~~~"); + } + + serialPrintAll(); +} + +void loop() { + measureAveragedVoltages(); + updateDirectionFromVoltages(); + applyDirectionOutputs(); + + serialPrintAll(); + + delay(300); +} diff --git a/firmware/ArduinoTest/enshaBekutoru_0004___Version_0.2_UnoTest/enshaBekutoru_0004___Version_0.2_UnoTest.ino b/firmware/ArduinoTest/enshaBekutoru_0004___Version_0.2_UnoTest/enshaBekutoru_0004___Version_0.2_UnoTest.ino new file mode 100644 index 0000000..c89378e --- /dev/null +++ b/firmware/ArduinoTest/enshaBekutoru_0004___Version_0.2_UnoTest/enshaBekutoru_0004___Version_0.2_UnoTest.ino @@ -0,0 +1,286 @@ +/* + 0004_DenshaBekutoru – Optocoupler Averaging Test + Target dev board (later): Arduino Pro Mini 5V / 16 MHz (ATmega328P) + + ---------------------------------------------------------------------------- + State machine (direction memory) + Hysteresis (Version A) + ---------------------------------------------------------------------------- + We use two thresholds: + T_hold (smaller): below -> treat as neutral and HOLD state + T_enter (larger): above -> allow direction change (set by sign) + + Behavior: + If |diff| <= T_hold : hold direction + If |diff| >= T_enter: set direction by sign + Else (between) : hold direction + + Presets (implemented as multipliers of the calibrated VdiffThreshold): + T_hold = 1.0 * VdiffThreshold + T_enter = 2.0 * VdiffThreshold + + Inputs derived from v1, v2: + diff = v1 - v2 +*/ + +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) + +// Direction LEDs (avoid D0/D1 because you may want RX/TX later) +const uint8_t LED_DIR1_PIN = 2; // D2 +const uint8_t LED_DIR2_PIN = 3; // D3 + +const unsigned int LED_BLINK_ON_MS = 150; +const unsigned int LED_BLINK_OFF_MS = 150; + +// 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 + +// ---- Hysteresis presets (multipliers of VdiffThreshold) ---- +const float THOLD_MULT = 1.0f; // T_hold = THOLD_MULT * VdiffThreshold +const float TENTER_MULT = 2.0f; // T_enter = TENTER_MULT * VdiffThreshold + +// 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; // calibrated base threshold (used to derive T_hold/T_enter) + +// Hysteresis thresholds (derived once after calibration) +float T_hold = 0.0f; +float T_enter = 0.0f; + +// 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; } + +// ----------------------------------------------------------------------------- +// LED helpers +// ----------------------------------------------------------------------------- +static void blinkLED(uint8_t pin, uint8_t times) +{ + for (uint8_t i = 0; i < times; i++) { + digitalWrite(pin, HIGH); + delay(LED_BLINK_ON_MS); + digitalWrite(pin, LOW); + delay(LED_BLINK_OFF_MS); + } +} + +static void initBlinkSequence() +{ + // LED at Output 2 blinks 2 times + blinkLED(LED_DIR1_PIN, 2); + + // LED at Output 3 blinks 3 times + blinkLED(LED_DIR2_PIN, 3); +} + +static void applyDirectionOutputs() +{ + digitalWrite(LED_DIR1_PIN, LOW); + digitalWrite(LED_DIR2_PIN, LOW); + + if (direction == 2) { + digitalWrite(LED_DIR1_PIN, HIGH); + } else if (direction == 3) { + digitalWrite(LED_DIR2_PIN, HIGH); + } +} + +// ----------------------------------------------------------------------------- +// Measurement +// ----------------------------------------------------------------------------- +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); +} + +// ----------------------------------------------------------------------------- +// Median helpers (for calibration) +// ----------------------------------------------------------------------------- +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 + VdiffThreshold once at boot/reset +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); + + // Calibrated base threshold with margin + floor + VdiffThreshold = maxf(VdiffBaseline * VdiffThresholdMargin, VdiffThresholdMinVolts); + + // Derive hysteresis thresholds (Version A) + T_hold = THOLD_MULT * VdiffThreshold; + T_enter = TENTER_MULT * VdiffThreshold; +} + +// ----------------------------------------------------------------------------- +// State machine with hysteresis (Version A) +// ----------------------------------------------------------------------------- +static void updateDirectionFromVoltages() +{ + const float diff = v1 - v2; + const float absDiff = absf(diff); + + // 1) Neutral region: HOLD + if (absDiff <= T_hold) { + return; + } + + // 2) Strong region: allow direction change + if (absDiff >= T_enter) { + direction = (diff > 0.0f) ? (byte)2 : (byte)3; + return; + } + + // 3) Deadband between T_hold and T_enter: HOLD + return; +} + +// ----------------------------------------------------------------------------- +// Serial output +// ----------------------------------------------------------------------------- +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("VdiffThreshold: "); + Serial.print(VdiffThreshold, 3); + Serial.print(" V | "); + + Serial.print("T_hold: "); + Serial.print(T_hold, 3); + Serial.print(" V | "); + + Serial.print("T_enter: "); + Serial.print(T_enter, 3); + Serial.print(" V | "); + + Serial.print("direction: "); + Serial.println(direction); +} + +// ----------------------------------------------------------------------------- +// Arduino entry points +// ----------------------------------------------------------------------------- +void setup() { + Serial.begin(115200); + + pinMode(ADC_PIN_1, INPUT); + pinMode(ADC_PIN_2, INPUT); + + pinMode(LED_DIR1_PIN, OUTPUT); + pinMode(LED_DIR2_PIN, OUTPUT); + + // Initial calibration (assumes no movement) + calibrateVdiffThreshold(AverageRepeat); + + // Init blink sequence: 2x on D2, 3x on D3 + initBlinkSequence(); + + // One measurement for initial display + initial direction + measureAveragedVoltages(); + updateDirectionFromVoltages(); + applyDirectionOutputs(); + + if (SerialOutputAllow) { + Serial.println("=== DenshaBekutoru Optocoupler Average Test (Arduino Pro Mini 5V/16MHz) ==="); + Serial.print("AverageRepeat: "); + Serial.println(AverageRepeat); + Serial.print("THOLD_MULT: "); + Serial.print(THOLD_MULT, 2); + Serial.print(" TENTER_MULT: "); + Serial.println(TENTER_MULT, 2); + } + + serialPrintAll(); +} + +void loop() { + measureAveragedVoltages(); + updateDirectionFromVoltages(); + applyDirectionOutputs(); + + serialPrintAll(); + + delay(300); +} diff --git a/firmware/ArduinoTest/implementstatemachine/implementstatemachine.ino b/firmware/ArduinoTest/implementstatemachine/implementstatemachine.ino new file mode 100644 index 0000000..9d9b116 --- /dev/null +++ b/firmware/ArduinoTest/implementstatemachine/implementstatemachine.ino @@ -0,0 +1,231 @@ +/* + 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); +}