feat: update float ESP-NOW runtime flow

.github/workflows/ci.yml

@@ -18,7 +18,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        environment: [espA, espB, espA_pool]
+        environment: [espA, espB]
 
     steps:
       - name: Checkout

.github/workflows/release.yml

@@ -15,7 +15,7 @@ jobs:
     strategy:
       fail-fast: true
       matrix:
-        environment: [espA, espB, espA_pool]
+        environment: [espA, espB]
 
     steps:
       - name: Checkout

CONTRIBUTING.md

@@ -57,25 +57,24 @@ Subject in imperative ("add X", not "added X" / "adds X"), <72 chars. Body expla
 ### Pull request expectations
 
 - The branch must be **rebased on `master`** before merging (or at least up-to-date) — the PR UI will warn if not.
-- All [CI checks](.github/workflows/ci.yml) must be green: `build (espA)`, `build (espB)`, `build (espA_pool)`.
+- All [CI checks](.github/workflows/ci.yml) must be green: `build (espA)`, `build (espB)`.
 - At least **one approving review** from another maintainer.
 - Description should state *what* changes and *why*, plus a manual test plan if the change touches motion/PID/comms (CI does not run hardware tests).
 
 ## CI
 
-`.github/workflows/ci.yml` runs on every push to any branch and on every pull request to `master`. It compiles all three PlatformIO environments in parallel:
+`.github/workflows/ci.yml` runs on every push to any branch and on every pull request to `master`. It compiles both PlatformIO environments in parallel:
 
 - `espA` — float controller firmware
 - `espB` — communication bridge firmware
-- `espA_pool` — float controller with shallow-pool profile
 
 Caching of PlatformIO core and build artifacts keeps a typical run under 2 minutes after the first warm-up.
 
 **Hardware tests are not run by CI.** The `test/unit_hw/` and `test/integration/` suites need real ESP32 boards plus motor / TOF / Bar02. Run them locally on a bench setup — see the *Development and Testing* section in [README.md](README.md).
 
 ### Releases
 
-Push a tag like `v11.3.0` and the [release workflow](.github/workflows/release.yml) builds all three environments and publishes a GitHub Release with the `firmware.bin` and `firmware.elf` for each one attached.
+Push a tag like `v11.3.0` and the [release workflow](.github/workflows/release.yml) builds both environments and publishes a GitHub Release with the `firmware.bin` and `firmware.elf` for each one attached.
 
 ```bash
 # After the change is merged to master:
@@ -99,7 +98,7 @@ Enable:
   - Dismiss stale pull request approvals when new commits are pushed
 - Require status checks to pass before merging
   - Require branches to be up to date before merging
-  - Status checks: `Build espA`, `Build espB`, `Build espA_pool`
+  - Status checks: `Build espA`, `Build espB`
 - Do not allow bypassing the above settings (recommended)
 - Restrict who can push to matching branches (admin only — for emergency hotfixes)
 
@@ -110,7 +109,6 @@ gh api -X PUT repos/:owner/:repo/branches/master/protection \
   -F required_status_checks.strict=true \
   -F 'required_status_checks.contexts[]=Build espA' \
   -F 'required_status_checks.contexts[]=Build espB' \
-  -F 'required_status_checks.contexts[]=Build espA_pool' \
   -F enforce_admins=false \
   -F required_pull_request_reviews.required_approving_review_count=1 \
   -F required_pull_request_reviews.dismiss_stale_reviews=true \

include/config.h

@@ -50,16 +50,23 @@ constexpr uint32_t MOTOR_MAX_ACCELERATION = 1800; // Normal acceleration/deceler
 constexpr uint32_t MOTOR_HOMING_SPEED    = 1800;   // Homing speed (steps/s)
 constexpr uint16_t MOTOR_ENDSTOP_MARGIN  = 10;    // Safety margin from endstops (steps)
 
-// Geometria reale: home (motor_pos=0) = siringa retratta, vuota → float galleggia.
-// MOTOR_MAX_STEPS = siringa estesa, piena d'acqua → float affonda.
-// Convenzione "logica" del PID/profile: u=0 → galleggia (siringa vuota),
-//                                       u=1 → affonda (siringa piena).
-// La geometria coincide con la convenzione logica: nessuna inversione necessaria.
-constexpr bool MOTOR_INVERT_LOGICAL = false;
+// Geometria reale (verificata col balance, coerente con l'homing):
+//   home (motor_pos=0)      = piastra lontana dal TOF, acqua spinta fuori → galleggia.
+//   motor_pos negativo      = piastra verso il TOF, prende acqua → affonda.
+// La direzione "verso il TOF / prende acqua" è NEGATIVA (vedi homeWithTof fase 1).
+// Convenzione "logica" del PID/profile: u=0 → galleggia, u=1 → affonda.
+// Quindi u cresce muovendosi in direzione negativa:
+//   uToMotorPos(0.0f) = 0 (home), uToMotorPos(1.0f) = -(MAX - 2*margin).
 inline long uToMotorPos(float u) {
-    const long usable = (long)MOTOR_MAX_STEPS - 2L * (long)MOTOR_ENDSTOP_MARGIN;
-    const float uPhys = MOTOR_INVERT_LOGICAL ? (1.0f - u) : u;
-    return (long)MOTOR_ENDSTOP_MARGIN + (long)(uPhys * (float)usable);
+    const long travel = (long)MOTOR_MAX_STEPS - 2L * (long)MOTOR_ENDSTOP_MARGIN;
+    return -(long)(u * (float)travel);
+}
+inline float motorPosToU(long position) {
+    const long travel = (long)MOTOR_MAX_STEPS - 2L * (long)MOTOR_ENDSTOP_MARGIN;
+    if (travel <= 0) return 0.0f;
+
+    const float u = -(float)position / (float)travel;
+    return constrain(u, 0.0f, 1.0f);
 }
 constexpr uint32_t MOTOR_HOMING_TIMEOUT  = 30000;  // Homing timeout (ms)
 constexpr uint16_t MOTOR_HOMING_TOF_PERIOD_MS = 50; // TOF polling period during homing (ms)
@@ -72,10 +79,42 @@ constexpr uint8_t  TOF_GPIO1_PIN         = 15;    // Optional INT pin, unused in
 constexpr uint8_t  TOF_MATRIX_ZONE_COUNT = 16;
 constexpr uint16_t TOF_ZONE_ENABLE_MASK  = 0x0660; // Central zones: 5, 6, 9, 10
 constexpr float    TOF_DISTANCE_RAW_OFFSET_MM = 6.0f; // Raw distance is this much higher than real distance
-constexpr float    TOF_HOMING_THRESHOLD     = 75.0f; // Homing stop distance: stop when TOF reads ABOVE this (siringa retratta = lontana dal TOF) (mm)
+constexpr float    TOF_HOMING_THRESHOLD     = 70.0f; // Homing stop distance: stop when TOF reads ABOVE this (siringa retratta = lontana dal TOF) (mm). Lo stop reale cade qualche mm sopra (polling 50ms + conferma + risoluzione TOF grezza): a 70 lo stop effettivo ~73-76 mm, con margine sotto TOF_SAFE_RANGE_MAX_MM=82.
 constexpr float    TOF_HOMING_APPROACH_MM   = 50.0f; // Approach phase: move toward TOF until reading BELOW this, then start homing (mm)
-constexpr float    TOF_SAFE_RANGE_MIN_MM    = 40.0f; // Safety range lower bound: siringa estesa, troppo vicina al TOF (mm)
-constexpr float    TOF_SAFE_RANGE_MAX_MM    = 85.0f; // Safety range upper bound: siringa retratta, troppo lontana dal TOF (mm). 10 mm sopra TOF_HOMING_THRESHOLD per coprire il rumore TOF post-homing senza spingere il pistone a sbattere meccanicamente.
+// Letture TOF consecutive oltre soglia richieste prima di accettare il trigger
+// di homing in ciascuna fase. Un singolo campione (frame recuperato, zona valida
+// ma rumorosa, riflesso) non deve fermare la fase: serve conferma. Stesso pattern
+// di TOF_SAFETY_STOP_SAMPLES.
+constexpr uint8_t  TOF_HOMING_CONFIRM_SAMPLES = 2;
+// Soglie tarate sulla finestra TOF reale misurata in piscina (distanza CORRETTA,
+// cioè raw - TOF_DISTANCE_RAW_OFFSET_MM): pistone esteso ≈ 29 mm, retratto ≈ 79 mm
+// (raw 85). Pendenza ≈ 1.1 mm TOF per mm motore.
+// MIN: sotto il fondo corsa esteso si APRE IL TAPPO ed entra acqua. 32 mm lascia
+// ~3 mm di margine sopra il 29 fisico: al raggiungimento si fa uno STOP PULITO
+// (clamp, niente emergency) per fermare il pistone PRIMA del tappo senza abortire
+// la missione (vedi MotionController::tofGuard / TofGuard::ExtendLimit).
+constexpr float    TOF_SAFE_RANGE_MIN_MM    = 32.0f; // Safety range lower bound: siringa estesa, vicina al TOF — oltre = tappo aperto (mm)
+// MAX: massimo gestibile raw 85 → 79 mm corretta (offset 6 mm). 82 mm = ~3 mm
+// sopra il 79 fisico, copre il rumore post-homing. Verso il retratto il motore
+// può sforare ancora parecchio: oltre 82 è un'ANOMALIA (passi persi, verso
+// sbagliato) → emergency stop (TofGuard::Emergency).
+constexpr float    TOF_SAFE_RANGE_MAX_MM    = 82.0f; // Safety range upper bound: siringa retratta, lontana dal TOF — oltre = anomalia (mm)
+// Soglia (numero di letture TOF consecutive fuori range) prima di scatenare un
+// emergency stop durante un movimento. Un singolo campione fuori soglia in
+// acqua (bolle, riflessi, torbidità) non deve fermare la missione: serve una
+// conferma. Stesso pattern del pressure-stop del balance.
+constexpr uint8_t  TOF_SAFETY_STOP_SAMPLES  = 3;
+// Range PID utile: limitiamo l'output u del PID a [MIN, MAX] (anziché [0,1])
+// così la siringa non raggiunge mai gli estremi meccanici che coincidono con
+// le soglie TOF di sicurezza (32/82 mm), lasciando margine contro passi persi
+// e rumore. Con lo zero a TOF≈75 mm e pendenza ~1.1, a u=0.92 il TOF ≈ 40 mm,
+// sopra MIN=32 con margine; a u=1.0 ≈ 34 mm (ecco perché MAX resta < 1.0).
+// PID_U_MIN=0 così il PID può svuotare completamente la siringa per risalire
+// (un MIN>0 lasciava il float troppo galleggiante e nascondeva la dinamica
+// reale agli u bassi). Il MAX resta sotto 1.0 per margine verso la soglia TOF
+// in piena estensione.
+constexpr float    PID_U_MIN                = 0.0f;
+constexpr float    PID_U_MAX                = 0.92f;
 
 // ---------------------------------------------------------------------------
 // BALANCE / PURGE CONTROL
@@ -90,30 +129,37 @@ constexpr uint16_t BALANCE_PRESSURE_SAMPLE_PERIOD_MS = 50; // Bar02 polling peri
 constexpr uint16_t PERIOD_MEASUREMENT   = 100;   // Between depth readings
 constexpr uint16_t PERIOD_CONN_CHECK    = 500;   // Between idle acknowledgements
 
-#ifdef POOL_TEST_PROFILE
-constexpr uint16_t PERIOD_EEPROM_WRITE   = 2000; // Faster hold checks for shallow pool tests
-constexpr uint16_t PROFILE_LOG_PERIOD_MS = 500;  // Denser flash log for short pool runs
-constexpr uint16_t DATA_PACKET_PERIOD_MS = 2000; // Denser replay packets for short pool runs
-#else
 constexpr uint16_t PERIOD_EEPROM_WRITE   = 5000; // Between EEPROM writes / hold checks
 constexpr uint16_t PROFILE_LOG_PERIOD_MS = 1000; // Between flash profile writes
 constexpr uint16_t DATA_PACKET_PERIOD_MS = 5000; // Packet cadence shown to judges
-#endif
 
 // ---------------------------------------------------------------------------
 // PID TUNING (output normalizzato in [0,1] = frazione di corsa siringa)
 // ---------------------------------------------------------------------------
-// Kp/Ki/Kd mutabili a runtime via CMD_UPDATE_PID (8); periodMs e alphaD via
-// CMD_UPDATE_PID_EXT (14). Espressi in "frazione di corsa per metro di errore",
-// portabili tra siringhe — se cambia MOTOR_MAX_STEPS, i guadagni restano validi.
+// Defaults runtime per PID_CONFIG_SET/GET. Espressi in "frazione di corsa per
+// metro di errore", portabili tra siringhe — se cambia MOTOR_MAX_STEPS, i
+// guadagni restano validi.
 constexpr uint16_t PID_PERIOD_DEFAULT_MS  = 50;    // Default tick PID (ms)
-constexpr float    PID_KP_DEFAULT         = 0.17f; // frazione_corsa / m
-constexpr float    PID_KI_DEFAULT         = 0.0f;  // frazione_corsa / (m·s)
-constexpr float    PID_KD_DEFAULT         = 0.13f; // frazione_corsa / (m/s)
+// Tuning iterato sui test in piscina:
+//  - Kp=0.17 (originale): troppo debole, il float non si muoveva (u≈0.03).
+//  - Kp=2.0 Kd=0.13: il float si muoveva ma OSCILLAVA (±15cm, pompaggio) —
+//    Kp troppo alto e Kd insufficiente per un sistema lento come il float.
+//  - Kp=1.0 Kd=0.5: smorzato ma si "sedeva" in superficie (ripresa debole).
+//  - Kp=1.7 Ki=0.1 Kd=0.3 (attuale): converge sul target con oscillazione
+//    finale ±1cm. Il Ki vince l'offset di galleggiamento (ripresa), Kd smorza.
+//    Affinare ancora a runtime con PID_CONFIG_SET se serve.
+constexpr float    PID_KP_DEFAULT         = 1.7f;  // frazione_corsa / m
+constexpr float    PID_KI_DEFAULT         = 0.1f;  // frazione_corsa / (m·s)
+constexpr float    PID_KD_DEFAULT         = 0.3f;  // frazione_corsa / (m/s)
 constexpr float    PID_INTEGRAL_LIMIT     = 5.0f;  // m·s (bound conservativo)
 constexpr float    PID_ALPHA_D_DEFAULT    = 0.25f; // LPF IIR coeff per derivata
 constexpr float    PID_U_NEUTRAL          = 0.011f;// kick-start offset (~500/47100)
 constexpr float    PID_MIN_RETARGET_FRAC  = 0.001f;// dead-band ri-comando (frazione corsa)
+// Pre-posizionamento siringa all'inizio della discesa PID (kick-start): u alto
+// per avviare l'affondamento. Era 0.979 (siringa quasi piena) ma faceva tirare
+// il float dritto fino al fondo prima che il PID frenasse (overshoot ~26cm in
+// vasca). Ridotto per avviare la discesa senza superare il target.
+constexpr float    PID_DESCENT_KICK_U     = 0.30f;
 
 // ---------------------------------------------------------------------------
 // FLOAT PHYSICAL / MISSION CONSTANTS
@@ -130,24 +176,13 @@ constexpr float    SENSOR_TO_TOP_M       = FLOAT_TOP_TO_SENSOR_M;
 constexpr float    SURFACE_TARGET_OFFSET_M = 0.10f;
 constexpr float    DEPTH_EPSILON       = 0.01f;  // "Stationary" tolerance (m)
 
-#ifdef POOL_TEST_PROFILE
-constexpr float    POOL_TEST_WATER_DEPTH = 0.70f; // Reference only: assumed test pool depth (m)
-constexpr uint8_t  PROFILE_MAX_COUNT     = 1;     // One cycle keeps shallow-pool tests shorter and safer
-constexpr float    DEPTH_MAX_ERROR       = 0.025f; // Narrow tolerance because pool targets are close together
-constexpr float    TARGET_DEPTH          = 0.63f; // Deep hold: bottom reference (m), ~7 cm above a 70 cm floor
-constexpr float    TARGET_SHALLOW_TOP_DEPTH = 0.06f; // Shallow hold: top reference (m)
-constexpr float    STAT_TIME             = 8.0f;  // Short pool hold; actual check cadence is PERIOD_EEPROM_WRITE
-constexpr float    TIMEOUT_PID_TIME      = 45.0f; // Max PID phase time (s)
-constexpr float    TIMEOUT_ASCENT        = 45.0f; // Max ascent + shallow hold time (s)
-#else
 constexpr uint8_t  PROFILE_MAX_COUNT   = 2;      // Profiles before auto-stop
 constexpr float    DEPTH_MAX_ERROR     = 0.33f;  // MATE depth tolerance (m)
 constexpr float    TARGET_DEPTH        = 2.50f;  // Deep hold: bottom reference (m)
 constexpr float    TARGET_SHALLOW_TOP_DEPTH = 0.40f; // Shallow hold: top reference (m)
 constexpr float    STAT_TIME           = 30.0f;  // MATE hold time at target (s)
 constexpr float    TIMEOUT_PID_TIME    = 180.0f; // Max PID phase time (s)
 constexpr float    TIMEOUT_ASCENT      = 120.0f; // Max ascent + shallow hold time (s)
-#endif
 
 constexpr float    TARGET_SHALLOW_BOTTOM_DEPTH =
     TARGET_SHALLOW_TOP_DEPTH + SENSOR_TO_BOTTOM_M + SENSOR_TO_TOP_M;