diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..89c138a --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,64 @@ +name: CI + +on: + push: + branches: ['**'] + pull_request: + branches: [master] + workflow_dispatch: + +concurrency: + group: ci-${{ github.ref }} + cancel-in-progress: true + +jobs: + build: + name: Build ${{ matrix.environment }} + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + environment: [espA, espB, espA_pool] + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: '3.11' + + - name: Cache PlatformIO core + uses: actions/cache@v4 + with: + path: | + ~/.platformio/.cache + ~/.platformio/packages + ~/.platformio/platforms + key: pio-core-${{ runner.os }}-${{ hashFiles('platformio.ini') }} + restore-keys: | + pio-core-${{ runner.os }}- + + - name: Cache PlatformIO build + uses: actions/cache@v4 + with: + path: .pio + key: pio-build-${{ runner.os }}-${{ matrix.environment }}-${{ hashFiles('platformio.ini', 'include/**', 'lib/**', 'src/**') }} + restore-keys: | + pio-build-${{ runner.os }}-${{ matrix.environment }}- + + - name: Install PlatformIO + run: | + python -m pip install --upgrade pip + pip install platformio + + - name: Build ${{ matrix.environment }} + run: pio run -e ${{ matrix.environment }} + + - name: Report firmware size + run: | + echo "### Firmware size (${{ matrix.environment }})" >> "$GITHUB_STEP_SUMMARY" + echo '```' >> "$GITHUB_STEP_SUMMARY" + pio run -e ${{ matrix.environment }} --target size 2>&1 | tail -20 >> "$GITHUB_STEP_SUMMARY" || true + echo '```' >> "$GITHUB_STEP_SUMMARY" diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 0000000..05992dd --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,91 @@ +name: Release + +on: + push: + tags: + - 'v*' + +permissions: + contents: write + +jobs: + build-firmware: + name: Build firmware for release + runs-on: ubuntu-latest + strategy: + fail-fast: true + matrix: + environment: [espA, espB, espA_pool] + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: '3.11' + + - name: Cache PlatformIO core + uses: actions/cache@v4 + with: + path: | + ~/.platformio/.cache + ~/.platformio/packages + ~/.platformio/platforms + key: pio-core-${{ runner.os }}-${{ hashFiles('platformio.ini') }} + restore-keys: | + pio-core-${{ runner.os }}- + + - name: Install PlatformIO + run: | + python -m pip install --upgrade pip + pip install platformio + + - name: Build ${{ matrix.environment }} + run: pio run -e ${{ matrix.environment }} + + - name: Stage firmware artifact + run: | + mkdir -p artifacts + cp .pio/build/${{ matrix.environment }}/firmware.bin \ + "artifacts/${{ matrix.environment }}-${{ github.ref_name }}.bin" + cp .pio/build/${{ matrix.environment }}/firmware.elf \ + "artifacts/${{ matrix.environment }}-${{ github.ref_name }}.elf" + + - name: Upload build artifact + uses: actions/upload-artifact@v4 + with: + name: firmware-${{ matrix.environment }} + path: artifacts/* + retention-days: 7 + + publish-release: + name: Publish GitHub Release + needs: build-firmware + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Download all firmware artifacts + uses: actions/download-artifact@v4 + with: + path: artifacts + pattern: firmware-* + merge-multiple: true + + - name: List artifacts + run: ls -la artifacts/ + + - name: Create / update GitHub Release + uses: softprops/action-gh-release@v2 + with: + tag_name: ${{ github.ref_name }} + name: ${{ github.ref_name }} + generate_release_notes: true + files: | + artifacts/*.bin + artifacts/*.elf + fail_on_unmatched_files: true diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..90516f2 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,134 @@ +# Contributing — Float 2025 + +Team PoliTOcean @ Politecnico di Torino +Maintainers: Colabella Davide, Benevenga Filippo + +## Git workflow + +The project follows a **trunk-based workflow**: `master` is the single source of truth and must always build. Everything else lives on short-lived feature branches. + +### Branch model + +- **`master`** — always green. Protected. Direct pushes are forbidden; changes land via pull request. +- **`feature/`** — short-lived branches off `master` for a specific change (one feature, one fix, one refactor). Aim to merge within a few days. +- **`fix/`** — same as feature but for bug fixes (purely a label convention, behaviour is identical). +- **`hotfix/`** — emergency fix on top of the latest release tag; same PR flow. + +Tags `v*` (e.g. `v11.2.0`) mark released firmware versions and trigger the [release workflow](.github/workflows/release.yml). + +### Standard cycle + +```bash +# 1. Start from an up-to-date master +git checkout master +git pull --ff-only + +# 2. Create a feature branch +git checkout -b feature/surface-offset-runtime + +# 3. Make commits — small, focused, with descriptive messages +git add include/config.h lib/sensors/... +git commit -m "feat: runtime-tunable surface target offset" + +# 4. Push and open a PR +git push -u origin feature/surface-offset-runtime +# then open PR via GitHub UI or: gh pr create --fill + +# 5. Wait for CI to pass and for at least one reviewer to approve +# 6. Merge via the GitHub UI (squash recommended) +# 7. Delete the branch (GitHub does this automatically if configured) +``` + +### Commit message style + +Optional but recommended: prefix the subject with a short type tag for fast scanning. + +| Prefix | When | +|--------|------| +| `feat:` | a new user-visible feature or command | +| `fix:` | a bug fix | +| `refactor:` | internal change with no behavioural difference | +| `docs:` | README, comments, this file | +| `test:` | adding or fixing tests | +| `chore:` | dependencies, CI, build config | + +Subject in imperative ("add X", not "added X" / "adds X"), <72 chars. Body explains *why* if not obvious from the diff. + +### 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)`. +- 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: + +- `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. + +```bash +# After the change is merged to master: +git checkout master +git pull --ff-only +git tag -a v11.3.0 -m "Release v11.3.0: " +git push origin v11.3.0 +``` + +GitHub auto-generates release notes from the merged PRs since the previous tag; edit them on GitHub after the workflow finishes if you want a curated changelog. + +## Branch protection (one-time setup, by repo admin) + +Branch protection rules cannot be applied from CI — a repo admin enables them once on GitHub: + +**Settings → Branches → Add rule → Branch name pattern: `master`** + +Enable: +- Require a pull request before merging + - Require approvals: **1** + - 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` +- Do not allow bypassing the above settings (recommended) +- Restrict who can push to matching branches (admin only — for emergency hotfixes) + +Or via `gh` CLI: + +```bash +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 \ + -F restrictions= +``` + +(Run from a clone with `gh` authenticated; replace `:owner/:repo` if `gh` doesn't auto-detect.) + +## Code style + +The firmware is C++17 over Arduino/ESP-IDF. There is no clang-format config yet — match the surrounding style: + +- 4-space indent, no tabs. +- Braces on the same line for `if`/`for`/function definitions. +- Comments explain *why*, not *what*; the code says what. +- New constants belong in [`include/config.h`](include/config.h) (firmware tuning) or [`include/float_common.h`](include/float_common.h) (shared protocol). Don't sprinkle magic numbers. +- New commands: append to the end of the `FloatCommand` enum (never insert in the middle — breaks compatibility with older ESPB/GUI) and add a matching `CMDxx_ACK` define, an entry in `PROTOCOL_COMMANDS` in [`lib/espb_bridge_core/src/espb_bridge_core.cpp`](lib/espb_bridge_core/src/espb_bridge_core.cpp), and a case in the ESPA main switch. + +## Reporting issues + +Open a GitHub issue with: what you expected, what happened, the steps to reproduce, the relevant ESP-NOW log snippet from `DebugSerial`, and the ESPA/ESPB firmware version (commit hash or tag). diff --git a/README.md b/README.md index 061e463..edbcd5e 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,10 @@ # PoliTOcean Float 2025 - Technical Documentation -**Version:** 11.1.0 +[![CI](https://github.com/PoliTOcean/Float_2025/actions/workflows/ci.yml/badge.svg)](https://github.com/PoliTOcean/Float_2025/actions/workflows/ci.yml) + +**Version:** 11.2.0 **Team:** PoliTOcean @ Politecnico di Torino +**Maintainers:** Colabella Davide, Benevenga Filippo **Competition:** MATE ROV 2025/26 -------------------------------------------------------------------------- @@ -95,6 +98,34 @@ The general idea is that the FLOAT provides some micro-services that the CS can The FLOAT has two main logical states: the command execution one, and the idle one in which it waits for the new command. In idle state, the FLOAT can have buffered flash data from the last completed profile that can be sent to the CS. +#### Syringe / Motor Convention + +The FLOAT changes its buoyancy by pulling and pushing water through a pair of syringes driven by a stepper motor through a lead screw. The mechanical convention is: + +- **Home (`motor_pos = 0`)**: piston fully inserted, **syringes empty of water** → the FLOAT floats. At this position the TOF reads ≈ `TOF_HOMING_THRESHOLD` (75 mm) because the piston is far from the sensor. +- **Full extension (`motor_pos = uToMotorPos(1.0f)`)**: piston extracted, **syringes full of water** → the FLOAT sinks. TOF reads ≈ `TOF_SAFE_RANGE_MIN_MM` (40 mm). +- **PID logical convention**: `u ∈ [0, 1]` with `u = 0` → float (empty) and `u = 1` → sink (full). The helper `uToMotorPos(u)` in `include/config.h` maps `u` to the actual motor target while respecting `MOTOR_INVERT_LOGICAL`, so callers never hard-code signs. + +TOF safety limits used during motion: + +| Constant | Default | Meaning | +|---|---|---| +| `TOF_HOMING_THRESHOLD` | 75 mm | Phase 2 of homing stops when the TOF reads above this | +| `TOF_HOMING_APPROACH_MM` | 50 mm | Phase 1 of homing stops when the TOF reads below this | +| `TOF_SAFE_RANGE_MIN_MM` | 40 mm | Lower bound: syringe fully extended (mechanical limit) | +| `TOF_SAFE_RANGE_MAX_MM` | 85 mm | Upper bound: 10 mm above the homing threshold; any higher and the piston would risk hitting the back mechanical stop | + +#### Surface Target Offset + +When the FLOAT is "floating", we usually want its top a few centimetres below the water surface — not exactly at the waterline — so that the float remains visible without being completely above water. This is controlled by `SURFACE_TARGET_OFFSET_M` (default `0.10` m: top of the float 10 cm below the surface). + +Two ways to change it: + +- **At compile time**: edit `SURFACE_TARGET_OFFSET_M` in `include/config.h`. +- **At runtime**: send command `SURFACE_OFFSET ` via the CS, or `SURFACE_OFFSET ` over USB serial on ESPA. The change persists until the next reboot. + +The offset is geometry-agnostic: `FLOAT_TOP_TO_SENSOR_M` (geometric distance between the top of the float and the barometer) and `SURFACE_TARGET_OFFSET_M` (operational target) are kept as separate constants in `include/config.h`. + -------------------------------------------------------------------------- ## HARDWARE CONFIGURATION @@ -106,8 +137,8 @@ The FLOAT has two main logical states: the command execution one, and the idle o | ESP32 Dev Module x2 | ESPA float controller and ESPB communication bridge | [Espressif ESP32](https://documentation.espressif.com/esp32-wroom-32e_esp32-wroom-32ue_datasheet_en.pdf) | Arduino framework, ESP-NOW link, USB serial bridge on ESPB | | DRV8825 stepper driver | Stepper motor driver for syringe motion | [Pololu DRV8825 carrier](https://www.pololu.com/product/2133) | STEP/DIR control, active-low enable, SLEEP and RESET held HIGH during operation | | Stepper motor with planetary gearbox | Syringe actuator motor | [StepperOnline 17HS15-1684S-PG27](https://www.omc-stepperonline.com/it/nema-17-motore-passo-passo-bipolare-l-38mm-w-rapporto-di-riduzione-27-1-riduttore-epicicloidale-17hs15-1684s-pg27) | NEMA 17, 200 steps/rev, 1.8 deg/step, configured gear ratio 26.85124:1, microstep setting 1 | -| Lead screw / threaded rod | Converts motor rotation to linear travel | [SIENOC 500 mm trapezoidal lead screw](https://www.amazon.it/SIENOC-500mm-Stampante-Trapezoidale-Piombo/dp/B078K7KN8W/) | Pitch 2.0 mm, 4 starts, lead 8.0 mm/rev, configured travel 45 mm | -| VL53L4CD Time-of-Flight sensor | Non-contact homing distance sensor | [ST VL53L4CD](https://www.st.com/en/imaging-and-photonics-solutions/vl53l4cd.html) | I2C 0x29, XSHUT GPIO16, GPIO1 GPIO15, 24 mm offset, 40 mm homing threshold | +| Lead screw / threaded rod | Converts motor rotation to linear travel | [SIENOC 500 mm trapezoidal lead screw](https://www.amazon.it/SIENOC-500mm-Stampante-Trapezoidale-Piombo/dp/B078K7KN8W/) | Pitch 2.0 mm, 4 starts, lead 8.0 mm/rev, configured travel 35 mm | +| VL53L7CX Time-of-Flight sensor | Non-contact homing distance sensor (multi-zone) | [ST VL53L7CX](https://www.st.com/en/imaging-and-photonics-solutions/vl53l7cx.html) | I2C 0x29, LPn (XSHUT) GPIO16, GPIO1 GPIO15, 4×4 zone mode with central-zone mask `0x0660`, 6 mm raw offset, 75 mm homing threshold | | Bar02 pressure sensor | Pressure/depth measurement | [Blue Robotics Bar02](https://bluerobotics.com/store/sensors-cameras/sensors/bar02-sensor-r1/) | MS5837_02BA model, I2C 0x76, used for depth and pressure | | INA219 battery monitor | Battery bus-voltage monitor | [Adafruit INA219 breakout](https://www.adafruit.com/product/904) | I2C 0x40, initialized at 100 kHz, configured with 5 A max and 0.1 ohm shunt | @@ -124,10 +155,10 @@ The FLOAT has two main logical states: the command execution one, and the idle o | SLEEP | GPIO25 | DRV8825 Sleep | Active-LOW, must be HIGH for operation | | RST | GPIO26 | DRV8825 Reset | Active-LOW, must be HIGH for operation | | **TOF Sensor** |||| -| SDA | GPIO21 | VL53L4CD I2C Data | I2C bus (shared with sensors) | -| SCL | GPIO22 | VL53L4CD I2C Clock | I2C bus @ 1MHz | -| XSHUT | GPIO16 | VL53L4CD Shutdown | Sensor shutdown control | -| GPIO1 | GPIO15 | VL53L4CD Interrupt | Optional interrupt pin, unused in polling mode | +| SDA | GPIO21 | VL53L7CX I2C Data | I2C bus (shared with sensors) | +| SCL | GPIO22 | VL53L7CX I2C Clock | I2C bus @ 1MHz | +| XSHUT | GPIO16 | VL53L7CX LPn (shutdown) | Sensor enable / shutdown control | +| GPIO1 | GPIO15 | VL53L7CX Interrupt | Optional interrupt pin, unused in polling mode | | **Sensors** |||| | SDA | GPIO21 | Bar02, INA219 | I2C bus (shared) | | SCL | GPIO22 | Bar02, INA219 | I2C bus (shared) | @@ -150,11 +181,11 @@ The FLOAT has two main logical states: the command execution one, and the idle o | Device | Address | Bus Speed | |:-------|:-------:|:---------| -| VL53L4CD TOF | 0x29 | 1 MHz | +| VL53L7CX TOF | 0x29 | 1 MHz | | Bar02 Pressure | 0x76 | Shared I2C bus | | INA219 Battery | 0x40 | Initialized at 100 kHz | -> The firmware initializes the INA219 at 100 kHz, then the VL53L4CD driver raises the shared `Wire` clock to 1 MHz for TOF ranging. +> The firmware initializes the INA219 at 100 kHz, then the VL53L7CX driver raises the shared `Wire` clock to 1 MHz for TOF ranging. -------------------------------------------------------------------------- @@ -186,7 +217,7 @@ graph TB subgraph "Motor System" DRV8825[DRV8825 Driver] STEPPER[Stepper Motor] - TOF[VL53L4CD TOF Sensor] + TOF[VL53L7CX TOF Sensor] end subgraph "Sensors" @@ -227,7 +258,7 @@ The project follows a modular architecture with separate compilation units: - **Central Config** (`include/config.h`) - pin mapping, motor constants, PID defaults, mission timing, network parameters - **Shared Protocol** (`include/float_common.h`) - ESP-NOW packet structs, ACK strings, EEPROM size, shared LED enum - **Motor Control** (`lib/motor`) - DRV8825/FastAccelStepper setup, position tracking, bounded movement primitives -- **TOF Sensor** (`lib/tof`) - VL53L4CD initialization, corrected distance readings, and raw measurement metadata +- **TOF Sensor** (`lib/tof`) - VL53L7CX initialization (4×4 multi-zone), aggregated minimum-distance reading with raw offset compensation - **Motion Control** (`lib/motion_control`) - TOF homing, safe max-extension move, balance routine, emergency stop handling - **Communication** (`lib/comms`) - ESP-NOW wireless protocol and ElegantOTA session management - **Sensors** (`lib/sensors`) - Bar02 pressure/depth and INA219 battery monitoring @@ -258,11 +289,16 @@ stateDiagram-v2 EXECUTING --> SEND_DATA: LISTENING Command EXECUTING --> CLEAR_DATA: CLEAR_SD Command EXECUTING --> UPDATE_PID: PARAMS Command + EXECUTING --> UPDATE_PID_EXT: PARAMS_EXT Command EXECUTING --> TEST_SPEED: TEST_FREQ Command EXECUTING --> TEST_STEPS: TEST_STEPS Command EXECUTING --> DEBUG_MODE: DEBUG Command EXECUTING --> HOMING: HOME_MOTOR Command EXECUTING --> OTA: TRY_UPLOAD Command + EXECUTING --> SYRINGE_SET: SYRINGE_SET Command + EXECUTING --> PID_HOLD: PID_HOLD Command + EXECUTING --> PID_STEP: PID_STEP Command + EXECUTING --> SET_SURFACE_OFFSET: SURFACE_OFFSET Command PROFILE --> PID_CONTROL: Descending PID_CONTROL --> PID_CONTROL: Depth Control Active @@ -273,10 +309,15 @@ stateDiagram-v2 SEND_DATA --> IDLE: Data Sent CLEAR_DATA --> IDLE: Flash Log Cleared UPDATE_PID --> IDLE: Gains Updated + UPDATE_PID_EXT --> IDLE: Period/alpha Updated TEST_SPEED --> IDLE: Speed Stored TEST_STEPS --> IDLE: Test Move Complete DEBUG_MODE --> IDLE: Debug Toggle Complete OTA --> IDLE: Upload Complete + SYRINGE_SET --> IDLE: Bench Test Complete + PID_HOLD --> IDLE: Hold Complete / Timeout + PID_STEP --> IDLE: Step Complete / Timeout + SET_SURFACE_OFFSET --> IDLE: Offset Stored IDLE_W_DATA --> SENDING: LISTENING Command SENDING --> IDLE: Data Transmitted @@ -404,7 +445,7 @@ Table of FLOAT commands with relative effects and acknowledgements: | :--------------: | :-------------: | --------------------------------------------------------------------------------------------------------------------------------------------------------------- | :-------------------------: | :---------------------------------------: | | GO | 1 | Performs the two MATE vertical profiles, sends the pre-descent data packet before the first descent, and logs pressure/depth records to flash CSV | GO_RECVD | status to 2 (command execution)
| | LISTENING | 2 | Streams flash CSV records as JSON data packets at 5-second cadence, followed by `STOP_DATA` | Ack is data itself | status to 2 after first package arrival | -| BALANCE | 3 | Cycles safe max extension and TOF homing with 5 s holds until Bar02 pressure rises above its startup balance baseline by `BALANCE_STOP_PRESSURE_DELTA_KPA` | CMD3_RECVD | status to 2 | +| BALANCE | 3 | Cycles full extension and retraction with `holdMs` holds until Bar02 pressure rises above the startup baseline by `BALANCE_STOP_PRESSURE_DELTA_KPA`. Requires the motor to be homed first — otherwise the command fails with `Balance: homing required` | CMD3_RECVD | status to 2 | | CLEAR_SD | 4 | Clears and recreates the flash CSV log, and clears the legacy EEPROM buffer. The command string is kept as `CLEAR_SD` for compatibility | CMD4_RECVD | status to 2 | | SWITCH_AUTO_MODE | 5 | Toggles FLOAT Auto Mode | SWITCH_AM_RECVD | status to 2, AM activation state toggled | | SEND_PACKAGE | 6 | Sends a single live JSON snapshot containing company number, time, pressure, judge/reference depth, phase, and raw sensor depth | Ack is the package itself | status to 2 | @@ -415,6 +456,11 @@ Table of FLOAT commands with relative effects and acknowledgements: | DEBUG | 11 | Toggles remote debug forwarding through `DebugSerial` | DEBUG_MODE_RECVD | status to 2 | | HOME_MOTOR | 12 | Runs TOF-based homing remotely | HOME_RECVD | status to 2 | | STOP | 13 | Triggers a remote emergency stop, stops the motor, disables outputs, and returns to idle | STOP_RECVD | status to 2 | +| `PARAMS_EXT period alpha` | 14 | Updates PID tick period (ms) and derivative LPF coefficient `alphaD` at runtime | CHNG_PID_EXT_RECVD | status to 2 | +| `SYRINGE_SET u dur_s` | 15 | Bench test: drives the syringe to normalized position `u ∈ [0,1]` for `dur_s` seconds, logging depth — bypasses the PID (DC gain / time-constant characterization) | SYRINGE_SET_RECVD | status to 2 | +| `PID_HOLD depth dur_s` | 16 | Bench test: holds depth at `depth_m` for `dur_s` seconds with the PID active, logging at 5 Hz | PID_HOLD_RECVD | status to 2 | +| `PID_STEP depth` | 17 | Bench test: step response — drives the PID to `depth_m` for up to 60 s, logging at 10 Hz | PID_STEP_RECVD | status to 2 | +| `SURFACE_OFFSET m` | 18 | Sets the surface target offset (`SURFACE_TARGET_OFFSET_M`) at runtime: the FLOAT will hold its top `m` metres below the waterline when "floating" (default `0.10`) | SURFACE_OFF_RECVD | status to 2 | | STATUS | - | Requests stale ESPB status plus AM state, WiFi connection state, battery millivolts, and last RSSI | - | - | Once a command is completed, ESPA acknowledgement can be: @@ -517,6 +563,11 @@ The GUI sends command strings to ESPB over USB serial. ESPB parses the string, s | `DEBUG` | 11 | `DEBUG_MODE_RECVD` | | `HOME_MOTOR` | 12 | `HOME_RECVD` | | `STOP` | 13 | `STOP_RECVD` | +| `PARAMS_EXT period_ms alpha_d` | 14 | `CHNG_PID_EXT_RECVD` | +| `SYRINGE_SET u dur_s` | 15 | `SYRINGE_SET_RECVD` | +| `PID_HOLD depth_m dur_s` | 16 | `PID_HOLD_RECVD` | +| `PID_STEP depth_m` | 17 | `PID_STEP_RECVD` | +| `SURFACE_OFFSET m` | 18 | `SURFACE_OFF_RECVD` | | `STATUS` | - | ESPB local status line with five ` | `-separated fields | The peer MAC addresses are configured centrally in `include/config.h`: `MAC_ESPA` is used by ESPB, and `MAC_ESPB` is used by ESPA. @@ -529,21 +580,25 @@ The FLOAT is equipped with RGB LEDs on both ESP32 boards that provide visual fee ### ESPA (Float Board) LED States: +Driven by `LEDState` (scoped enum in [`lib/led/include/led.h`](lib/led/include/led.h)): + | LED Color/Pattern | State | Description | |:----------------:|:-----:|:------------| -| **Green Solid / Boot Blinks** | `LED_INIT` | System initializing | -| **Green Solid** | `LED_IDLE` | Ready and idle, waiting for commands | -| **Green Blink** | `LED_IDLE_WITH_DATA` | Idle with data ready to send | -| **Red Solid** | `LED_LOW_BATTERY` | Battery voltage below threshold (12.0V) | -| **Red Blink** | `LED_ERROR` | Error state or motor emergency stop | -| **Blue Solid** | `LED_PROFILE` | Running non-PID profile phase | -| **Yellow Blink** | `LED_AUTO_MODE` | Auto mode active | -| **Purple Blink** | `LED_HOMING` | Motor homing in progress | -| **Purple Solid** | `LED_MOTOR_MOVING` | Motor moving | -| **Cyan Blink** | `LED_PID_CONTROL` | PID depth control active | -| **White Solid** | `LED_COMMUNICATION` | Command received / communicating with ESPB | -| **Orange Blink** | `LED_OTA_MODE` | OTA update mode active | -| **Off** | `LED_OFF` | System off or disabled | +| **Green Solid / Boot Blinks** | `LEDState::INIT` | System initializing | +| **Green Solid** | `LEDState::IDLE` | Ready and idle, waiting for commands | +| **Green Blink** | `LEDState::IDLE_WITH_DATA` | Idle with data ready to send | +| **Red Solid** | `LEDState::LOW_BATTERY` | Battery voltage below `BATT_THRESH` (12.0 V) | +| **Red Blink** | `LEDState::ERROR` | Error state or motor emergency stop | +| **Blue Solid** | `LEDState::PROFILE` | Running non-PID profile phase | +| **Yellow Blink** | `LEDState::AUTO_MODE` | Auto mode active | +| **Purple Blink** | `LEDState::HOMING` | Motor homing in progress | +| **Purple Solid** | `LEDState::MOTOR_MOVING` | Motor moving | +| **Cyan Blink** | `LEDState::PID_CONTROL` | PID depth control active | +| **White Solid** | `LEDState::COMMUNICATION` | Command received / communicating with ESPB | +| **Orange Blink** | `LEDState::OTA_MODE` | OTA update mode active | +| **Off** | `LEDState::OFF` | System off or disabled | + +> ESPB uses a separate `FloatLEDState` enum (`LED_*` prefix) defined in [`include/float_common.h`](include/float_common.h); the two enums are deliberately independent because the two boards have different LED states to signal. ### ESPB (Communication Bridge) LED States: @@ -606,6 +661,19 @@ pio device monitor -e espA pio device monitor -e espB ``` +### Direct USB Tuning Commands (ESPA) + +All commands in the FLOAT Commands table can be sent over the ESPB USB serial bridge using the same string syntax. The commands below — useful for bench tuning — can also be sent **directly** over ESPA's USB serial port (e.g. when ESPA is wired to a laptop for tuning runs), bypassing ESPB and ESP-NOW entirely. + +| Command | Effect | +|:--------|:-------| +| `PARAMS ` | Update PID gains at runtime (same effect as command 8) | +| `PARAMS_EXT ` | Update PID tick period and derivative LPF coefficient (command 14) | +| `SYRINGE_SET ` | Drive the syringe to position `u ∈ [0,1]` for `dur_s` seconds and log depth — bypasses the PID, useful for DC-gain and time-constant estimation (command 15) | +| `PID_HOLD ` | Hold PID at `depth_m` for `dur_s` seconds, log at 5 Hz (command 16) | +| `PID_STEP ` | Step response: PID at `depth_m` for up to 60 s, log at 10 Hz (command 17) | +| `SURFACE_OFFSET ` | Set the surface target offset (`SURFACE_TARGET_OFFSET_M`) at runtime (command 18) | + ### CLI Tests To run all available tests for the `espA` environment: @@ -696,6 +764,16 @@ Hardware-oriented tests are stored under `test/`: - `test/integration/test_motor_direction` checks logical/physical motion direction, optionally using TOF - `test/integration/test_tof_motor_accuracy` checks TOF and motor movement consistency +### Continuous Integration + +GitHub Actions builds all three PlatformIO environments (`espA`, `espB`, `espA_pool`) on every push to any branch and on every pull request to `master`. Workflow file: [.github/workflows/ci.yml](.github/workflows/ci.yml). + +CI does **not** run the `unit_hw/` or `integration/` PlatformIO tests because they need a real ESP32 with the float wired up. Run those locally on the bench. + +Pushing a `v*` tag triggers [.github/workflows/release.yml](.github/workflows/release.yml), which builds all three environments and attaches the resulting `firmware.bin` / `firmware.elf` to a GitHub Release auto-named after the tag. + +See [CONTRIBUTING.md](CONTRIBUTING.md) for the full git workflow (trunk-based with PR review on `master`), commit conventions, and one-time branch protection setup. + -------------------------------------------------------------------------- ## UTILITIES AND RESOURCES @@ -704,7 +782,7 @@ Hardware-oriented tests are stored under `test/`: - BlueRobotics MS5837: https://github.com/bluerobotics/BlueRobotics_MS5837_Library - FastAccelStepper: https://github.com/gin66/FastAccelStepper - INA: https://github.com/Zanduino/INA -- VL53L4CD: https://github.com/stm32duino/VL53L4CD +- VL53L7CX: https://github.com/stm32duino/VL53L7CX - ESPAsyncWebServer: https://github.com/dvarrel/ESPAsyncWebSrv - ElegantOTA: https://github.com/ayushsharma82/ElegantOTA @@ -712,7 +790,7 @@ Hardware-oriented tests are stored under `test/`: - PlatformIO IDE: Modern embedded development platform - ESP32 Arduino Core: Framework for ESP32 development - FastAccelStepper Library: Timer/task-driven stepper motor control -- VL53L4CD Library: Time-of-Flight sensor driver +- VL53L7CX Library: Multi-zone Time-of-Flight sensor driver -------------------------------------------------------------------------- @@ -735,6 +813,14 @@ Hardware-oriented tests are stored under `test/`: -------------------------------------------------------------------------- -**Documentation Version:** 11.1.0 +**Documentation Version:** 11.2.0 **Last Updated:** May 2026 + +Recent changes: +- PID output normalized to `u ∈ [0, 1]` (fraction of syringe travel). Default gains `Kp = 0.17`, `Kd = 0.13`, expressed per metre of depth error so they stay valid if `MOTOR_MAX_STEPS` changes. +- Motor geometry: home = piston fully inserted (empty syringes, floats); full extension = piston extracted (full syringes, sinks). The mapping `uToMotorPos()` in `include/config.h` encapsulates `MOTOR_INVERT_LOGICAL` so motion code never hard-codes signs. +- TOF safety range widened to `[40, 85] mm` to give 10 mm of margin above the homing threshold without risking the mechanical end stop. +- `balance` now refuses to start without a prior homing (was forcing `pos = 0` as a fallback, mechanically risky). +- New `SURFACE_TARGET_OFFSET_M` constant and `SURFACE_OFFSET ` command (number 18) for tuning the surface idle position at runtime. **Team Contact:** PoliTOcean @ Politecnico di Torino +**Maintainers:** Colabella Davide, Benevenga Filippo diff --git a/include/config.h b/include/config.h index 74db6b6..150e27f 100644 --- a/include/config.h +++ b/include/config.h @@ -6,6 +6,8 @@ ******************************************************************************* * config.h * Centralized configuration: pin definitions, tuning constants, network params. + * + * Maintainers: Colabella Davide, Benevenga Filippo * Team PoliTOcean @ Politecnico di Torino ******************************************************************************* */ @@ -47,6 +49,18 @@ constexpr uint32_t MOTOR_MAX_SPEED = 1800; // Normal operating speed (ste constexpr uint32_t MOTOR_MAX_ACCELERATION = 1800; // Normal acceleration/deceleration (steps/s^2) 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; +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); +} 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) @@ -58,11 +72,10 @@ 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 = 40.0f; // Distance threshold for homing (mm) -constexpr float TOF_MAX_STOP_TRAVEL_MM = 40.0f; // Physical safety travel checked by TOF (mm) -constexpr float TOF_MAX_STOP_MARGIN_MM = 2.0f; // Extra margin beyond homing distance + syringe travel -constexpr float TOF_MAX_STOP_DISTANCE_MM = - TOF_HOMING_THRESHOLD + TOF_MAX_STOP_TRAVEL_MM + TOF_MAX_STOP_MARGIN_MM; +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_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. // --------------------------------------------------------------------------- // BALANCE / PURGE CONTROL @@ -88,23 +101,33 @@ constexpr uint16_t DATA_PACKET_PERIOD_MS = 5000; // Packet cadence shown to judg #endif // --------------------------------------------------------------------------- -// PID TUNING +// PID TUNING (output normalizzato in [0,1] = frazione di corsa siringa) // --------------------------------------------------------------------------- -// These are mutable at runtime via command 8 (UPDATE_PID), so they live in -// pid.cpp as extern variables — only defaults are declared here. -constexpr float PID_KP_DEFAULT = 2500.0f; -constexpr float PID_KI_DEFAULT = 0.0f; -constexpr float PID_KD_DEFAULT = 350.0f; -constexpr float PID_OUTPUT_LIMIT = 12000.0f; // Max output magnitude (steps) -constexpr uint16_t PID_MIN_MOVE_STEPS = 2000; // Minimum useful PID correction (steps) -constexpr float PID_INTEGRAL_LIMIT = 5.0f; // Anti-windup clamp +// 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. +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) +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) // --------------------------------------------------------------------------- // FLOAT PHYSICAL / MISSION CONSTANTS // --------------------------------------------------------------------------- -constexpr float FLOAT_LENGTH = 0.51f; // Bottom-to-sensor height (m) -constexpr float SENSOR_TO_BOTTOM_M = FLOAT_LENGTH; // Pressure sensor to bottom reference -constexpr float SENSOR_TO_TOP_M = 0.0f; // Pressure sensor to top reference; calibrate on hardware +constexpr float FLOAT_LENGTH = 0.51f; // Bottom-to-sensor height (m) +constexpr float SENSOR_TO_BOTTOM_M = FLOAT_LENGTH; // Pressure sensor to bottom reference +// Geometric offset between physical top of the float and the barometer. +// The Bar02 sits at the top, so this is ~0 m; calibrate on hardware if needed. +constexpr float FLOAT_TOP_TO_SENSOR_M = 0.0f; +constexpr float SENSOR_TO_TOP_M = FLOAT_TOP_TO_SENSOR_M; +// Operational target: how deep the *top* of the float should sit below the +// water surface when the float is "floating". Runtime-tunable via +// CMD_SET_SURFACE_OFFSET / USB SURFACE_OFFSET command — this is the default. +constexpr float SURFACE_TARGET_OFFSET_M = 0.10f; constexpr float DEPTH_EPSILON = 0.01f; // "Stationary" tolerance (m) #ifdef POOL_TEST_PROFILE diff --git a/include/float_common.h b/include/float_common.h index 319ece2..c929087 100644 --- a/include/float_common.h +++ b/include/float_common.h @@ -3,6 +3,15 @@ #include +/* + ******************************************************************************* + * float_common.h + * Shared command codes, ACK strings, and ESP-NOW message structs used by both + * ESPA and ESPB. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean + ******************************************************************************* + */ + // Communication protocol defines #define OUTPUT_LEN 250-sizeof(uint16_t) // Length of the output on the MAC layer #define EEPROM_SIZE 512 // EEPROM allocation size in bytes @@ -25,6 +34,11 @@ enum FloatCommand : uint8_t { CMD_DEBUG_MODE = 11, CMD_HOME = 12, CMD_STOP = 13, + CMD_UPDATE_PID_EXT = 14, + CMD_SYRINGE_SET = 15, + CMD_PID_HOLD = 16, + CMD_PID_STEP = 17, + CMD_SET_SURFACE_OFFSET = 18, }; // List of messages for the ESPA acknowledgements: CS has to be aware of these @@ -41,6 +55,11 @@ enum FloatCommand : uint8_t { #define CMD11_ACK "DEBUG_MODE_RECVD" #define CMD12_ACK "HOME_RECVD" #define CMD13_ACK "STOP_RECVD" +#define CMD14_ACK "CHNG_PID_EXT_RECVD" +#define CMD15_ACK "SYRINGE_SET_RECVD" +#define CMD16_ACK "PID_HOLD_RECVD" +#define CMD17_ACK "PID_STEP_RECVD" +#define CMD18_ACK "SURFACE_OFF_RECVD" // Sensor data structure typedef struct sensor_data { diff --git a/lib/comms/include/comms.h b/lib/comms/include/comms.h index 2e11b60..cdde787 100644 --- a/lib/comms/include/comms.h +++ b/lib/comms/include/comms.h @@ -11,6 +11,7 @@ ******************************************************************************* * comms.h * ESP-NOW communication and OTA firmware update management. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ diff --git a/lib/comms/src/comms.cpp b/lib/comms/src/comms.cpp index 4cfb8de..fe9980f 100644 --- a/lib/comms/src/comms.cpp +++ b/lib/comms/src/comms.cpp @@ -7,6 +7,9 @@ /* ******************************************************************************* * comms.cpp + * ESP-NOW peer-to-peer messaging between ESPA and ESPB, plus the OTA update + * window (ElegantOTA over a temporary WiFi access point). + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ diff --git a/lib/espb_bridge_core/include/espb_bridge_core.h b/lib/espb_bridge_core/include/espb_bridge_core.h index 6105a97..6b4216a 100644 --- a/lib/espb_bridge_core/include/espb_bridge_core.h +++ b/lib/espb_bridge_core/include/espb_bridge_core.h @@ -3,6 +3,15 @@ #include #include +/* + ******************************************************************************* + * espb_bridge_core.h + * Pure-logic core of the ESPB bridge: command parsing, status formatting, and + * the GUI/ESPA protocol contract. Kept hardware-agnostic to allow unit testing. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean + ******************************************************************************* + */ + constexpr int8_t ESPB_STATUS_UNKNOWN = -1; constexpr int8_t ESPB_STATUS_CONNECTED = 0; constexpr int8_t ESPB_STATUS_CONNECTED_W_DATA = 1; diff --git a/lib/espb_bridge_core/src/espb_bridge_core.cpp b/lib/espb_bridge_core/src/espb_bridge_core.cpp index 10ac4d6..1f41a94 100644 --- a/lib/espb_bridge_core/src/espb_bridge_core.cpp +++ b/lib/espb_bridge_core/src/espb_bridge_core.cpp @@ -5,6 +5,16 @@ #include #include +/* + ******************************************************************************* + * espb_bridge_core.cpp + * Pure-logic implementation of the ESPB bridge: GUI command string parsing + * (table-driven), parameter extraction, and STATUS line formatting. No + * hardware or framework dependencies, to remain unit-testable. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean + ******************************************************************************* + */ + namespace { constexpr size_t COMMAND_BUFFER_SIZE = 96; @@ -22,6 +32,11 @@ constexpr EspbProtocolCommand PROTOCOL_COMMANDS[] = { {"DEBUG", CMD_DEBUG_MODE, CMD11_ACK}, {"HOME_MOTOR", CMD_HOME, CMD12_ACK}, {"STOP", CMD_STOP, CMD13_ACK}, + {"PARAMS_EXT", CMD_UPDATE_PID_EXT, CMD14_ACK}, + {"SYRINGE_SET", CMD_SYRINGE_SET, CMD15_ACK}, + {"PID_HOLD", CMD_PID_HOLD, CMD16_ACK}, + {"PID_STEP", CMD_PID_STEP, CMD17_ACK}, + {"SURFACE_OFFSET", CMD_SET_SURFACE_OFFSET, CMD18_ACK}, }; void zeroMessage(output_message& message) { @@ -131,6 +146,83 @@ EspbParsedCommand espbParseSerialCommand(const char* line) { return parsed; } + if (strcmp(token, "PARAMS_EXT") == 0) { + // PARAMS_EXT period_ms alpha_d (third param reserved, always 0) + float periodMs = 0.0f; + float alphaD = 0.0f; + if (!parseFloatToken(strtok(nullptr, " "), periodMs) || + !parseFloatToken(strtok(nullptr, " "), alphaD) || + !hasNoExtraToken()) { + return parsed; + } + + parsed = makeForwardCommand(CMD_UPDATE_PID_EXT); + parsed.message.params[0] = periodMs; + parsed.message.params[1] = alphaD; + parsed.message.params[2] = 0.0f; + return parsed; + } + + if (strcmp(token, "SYRINGE_SET") == 0) { + // SYRINGE_SET + float u = 0.0f; + float dur = 0.0f; + if (!parseFloatToken(strtok(nullptr, " "), u) || + !parseFloatToken(strtok(nullptr, " "), dur) || + !hasNoExtraToken()) { + return parsed; + } + parsed = makeForwardCommand(CMD_SYRINGE_SET); + parsed.message.params[0] = u; + parsed.message.params[1] = dur; + parsed.message.params[2] = 0.0f; + return parsed; + } + + if (strcmp(token, "PID_HOLD") == 0) { + // PID_HOLD + float depth = 0.0f; + float dur = 0.0f; + if (!parseFloatToken(strtok(nullptr, " "), depth) || + !parseFloatToken(strtok(nullptr, " "), dur) || + !hasNoExtraToken()) { + return parsed; + } + parsed = makeForwardCommand(CMD_PID_HOLD); + parsed.message.params[0] = depth; + parsed.message.params[1] = dur; + parsed.message.params[2] = 0.0f; + return parsed; + } + + if (strcmp(token, "PID_STEP") == 0) { + // PID_STEP + float depth = 0.0f; + if (!parseFloatToken(strtok(nullptr, " "), depth) || + !hasNoExtraToken()) { + return parsed; + } + parsed = makeForwardCommand(CMD_PID_STEP); + parsed.message.params[0] = depth; + parsed.message.params[1] = 0.0f; + parsed.message.params[2] = 0.0f; + return parsed; + } + + if (strcmp(token, "SURFACE_OFFSET") == 0) { + // SURFACE_OFFSET + float offset = 0.0f; + if (!parseFloatToken(strtok(nullptr, " "), offset) || + !hasNoExtraToken()) { + return parsed; + } + parsed = makeForwardCommand(CMD_SET_SURFACE_OFFSET); + parsed.message.params[0] = offset; + parsed.message.params[1] = 0.0f; + parsed.message.params[2] = 0.0f; + return parsed; + } + if (strcmp(token, "TEST_FREQ") == 0) { long freq = 0; if (!parseLongToken(strtok(nullptr, " "), freq) || @@ -160,8 +252,13 @@ EspbParsedCommand espbParseSerialCommand(const char* line) { for (const EspbProtocolCommand& command : PROTOCOL_COMMANDS) { if (strcmp(token, command.commandText) == 0) { if (command.commandCode == CMD_UPDATE_PID || + command.commandCode == CMD_UPDATE_PID_EXT || command.commandCode == CMD_SET_SPEED || command.commandCode == CMD_TEST_STEPS || + command.commandCode == CMD_SYRINGE_SET || + command.commandCode == CMD_PID_HOLD || + command.commandCode == CMD_PID_STEP || + command.commandCode == CMD_SET_SURFACE_OFFSET || !hasNoExtraToken()) { return parsed; } diff --git a/lib/flash_storage/include/flash_storage.h b/lib/flash_storage/include/flash_storage.h index 4fb9a29..ac59cda 100644 --- a/lib/flash_storage/include/flash_storage.h +++ b/lib/flash_storage/include/flash_storage.h @@ -7,6 +7,7 @@ ******************************************************************************* * flash_storage.h * ESP32 internal flash CSV logging helper backed by LittleFS. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ diff --git a/lib/flash_storage/src/flash_storage.cpp b/lib/flash_storage/src/flash_storage.cpp index 16fa0c3..fb6ac0d 100644 --- a/lib/flash_storage/src/flash_storage.cpp +++ b/lib/flash_storage/src/flash_storage.cpp @@ -12,6 +12,10 @@ /* ******************************************************************************* * flash_storage.cpp + * Mission CSV logging on the ESP32 internal flash via LittleFS. Append-only + * writes during a profile and streamed replay via a caller-supplied packet + * sender during the LISTENING phase. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ diff --git a/lib/led/include/led.h b/lib/led/include/led.h index 687109d..7e3d555 100644 --- a/lib/led/include/led.h +++ b/lib/led/include/led.h @@ -8,6 +8,7 @@ * led.h * RGB LED state machine. * Each state maps to a colour and optional blink pattern. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ diff --git a/lib/led/src/led.cpp b/lib/led/src/led.cpp index 8217240..b079a07 100644 --- a/lib/led/src/led.cpp +++ b/lib/led/src/led.cpp @@ -4,6 +4,9 @@ /* ******************************************************************************* * led.cpp + * RGB LED state machine: each state maps to a colour and optional blink + * pattern, driven by a non-blocking update() called from the main loop. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ diff --git a/lib/motion_control/include/motion_control.h b/lib/motion_control/include/motion_control.h index 153ef5b..47df707 100644 --- a/lib/motion_control/include/motion_control.h +++ b/lib/motion_control/include/motion_control.h @@ -8,6 +8,7 @@ ******************************************************************************* * motion_control.h * Firmware-level motion control that coordinates motor, TOF, LED and debug. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ @@ -40,6 +41,18 @@ class MotionController { const char* context); bool pressureStopReached(float stopPressureKpa, uint8_t* pressureStopSamples = nullptr); bool waitWithPressureStop(uint32_t waitMs, float stopPressureKpa, uint8_t* pressureStopSamples = nullptr); + + // Esegue un singolo stroke del balance (extend o retract) come move assoluto + // verso targetPos. Ritorna true se va a buon fine, false in caso di + // remoteStop/pressureStop/timeout (l'esito esatto è loggato e propagato + // tramite pressureStopHit). label è usata solo per i log. + bool _balanceStrokeTo(long targetPos, + const char* label, + float stopPressureKpa, + uint8_t* pressureStopSamples, + uint32_t timeoutMs, + bool& pressureStopHit, + bool& remoteStopHit); }; // Singleton defined by the main firmware. diff --git a/lib/motion_control/src/motion_control.cpp b/lib/motion_control/src/motion_control.cpp index 6dc4632..ce2ae77 100644 --- a/lib/motion_control/src/motion_control.cpp +++ b/lib/motion_control/src/motion_control.cpp @@ -5,6 +5,16 @@ #include "comms.h" #include "DebugSerial.h" +/* + ******************************************************************************* + * motion_control.cpp + * High-level motion routines coordinating motor, TOF, LED and emergency stop: + * two-phase TOF homing, safe full extension, balance/purge cycles with + * pressure-based stop, and remote stop / safety-range supervision. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean + ******************************************************************************* + */ + MotionController::MotionController(MotorController& motor, TofSensor& tof) : _motor(motor), _tof(tof) {} @@ -87,11 +97,7 @@ float MotionController::readPressureKpa() { bool MotionController::tofMaxExtensionStopReached(unsigned long nowMs, unsigned long& lastTofSampleMs, const char* context) { - if (!_tof.isInitialized() || TOF_MAX_STOP_DISTANCE_MM <= 0.0f) { - return false; - } - - if (_motor.distanceToGo() <= 0) { + if (!_tof.isInitialized()) { return false; } @@ -101,16 +107,25 @@ bool MotionController::tofMaxExtensionStopReached(unsigned long nowMs, lastTofSampleMs = nowMs; float distanceMm = 0.0f; - if (!_tof.readDistanceMm(distanceMm) || distanceMm < TOF_MAX_STOP_DISTANCE_MM) { + if (!_tof.readDistanceMm(distanceMm)) { return false; } - Debug.printf("%s: TOF max extension stop reached (%.1f >= %.1f mm)\n", - context, - distanceMm, - TOF_MAX_STOP_DISTANCE_MM); - emergencyStop("TOF max extension limit"); - return true; + if (distanceMm < TOF_SAFE_RANGE_MIN_MM) { + Debug.printf("%s: TOF safety stop, too close (%.1f < %.1f mm)\n", + context, distanceMm, TOF_SAFE_RANGE_MIN_MM); + emergencyStop("TOF below safe range"); + return true; + } + + if (distanceMm > TOF_SAFE_RANGE_MAX_MM) { + Debug.printf("%s: TOF safety stop, too far (%.1f > %.1f mm)\n", + context, distanceMm, TOF_SAFE_RANGE_MAX_MM); + emergencyStop("TOF above safe range"); + return true; + } + + return false; } bool MotionController::pressureStopReached(float stopPressureKpa, uint8_t* pressureStopSamples) { @@ -174,11 +189,77 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui _motor.enableOutputs(); _motor.setMaxSpeed(MOTOR_HOMING_SPEED); _motor.setAcceleration(MOTOR_HOMING_SPEED); - _motor.startMoveSteps(-static_cast(MOTOR_MAX_STEPS) * 2); const unsigned long startMs = millis(); unsigned long lastTofSampleMs = 0; unsigned long lastPressureSampleMs = 0; + + // Phase 1: approach — move TOWARD the TOF (negative direction, siringa che si estende) + // finché il TOF legge sotto TOF_HOMING_APPROACH_MM. Garantisce un punto di partenza + // riproducibile indipendentemente dalla posizione iniziale. + Debug.println("Motor homing: phase 1 (approach toward TOF)"); + _motor.startMoveSteps(-static_cast(MOTOR_MAX_STEPS) * 2); + + bool approachDone = false; + while (_motor.distanceToGo() != 0 && !approachDone) { + if (remoteStopRequested()) { + return false; + } + + const unsigned long nowMs = millis(); + if (millis() - startMs > MOTOR_HOMING_TIMEOUT) { + Debug.println("Motor homing: timed out during approach"); + emergencyStop("homing timeout"); + return false; + } + + _motor.run(); + + if (nowMs - lastTofSampleMs >= MOTOR_HOMING_TOF_PERIOD_MS) { + lastTofSampleMs = nowMs; + + float distanceMm = 0.0f; + if (_tof.readDistanceMm(distanceMm)) { + if (distanceMm < TOF_HOMING_APPROACH_MM) { + Debug.printf("Motor homing: approach reached (%.1f < %.1f mm)\n", + distanceMm, TOF_HOMING_APPROACH_MM); + approachDone = true; + _motor.stop(); + } + } + } + + ledController.update(); + yield(); + } + + if (!approachDone) { + Debug.println("Motor homing: approach phase failed"); + emergencyStop("homing approach failed"); + return false; + } + + { + const unsigned long settleStart = millis(); + while (_motor.distanceToGo() != 0) { + if (remoteStopRequested()) { + return false; + } + if (millis() - settleStart > 2000) { + Debug.println("Motor homing: timeout settling approach"); + emergencyStop("homing approach settle timeout"); + return false; + } + _motor.run(); + ledController.update(); + yield(); + } + } + + // Phase 2: homing — invert direction (positive, siringa che si retrae) finché TOF legge + // sopra TOF_HOMING_THRESHOLD. + Debug.println("Motor homing: phase 2 (retract away from TOF)"); + _motor.startMoveSteps(static_cast(MOTOR_MAX_STEPS) * 2); bool homeDetected = false; while (_motor.distanceToGo() != 0 && !homeDetected) { @@ -210,8 +291,8 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui float distanceMm = 0.0f; if (_tof.readDistanceMm(distanceMm)) { - if (distanceMm < TOF_HOMING_THRESHOLD) { - Debug.printf("Motor homing: threshold reached (%.1f < %.1f mm)\n", + if (distanceMm > TOF_HOMING_THRESHOLD) { + Debug.printf("Motor homing: threshold reached (%.1f > %.1f mm)\n", distanceMm, TOF_HOMING_THRESHOLD); homeDetected = true; _motor.stop(); @@ -231,7 +312,7 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui delay(100); - _motor.startMoveSteps(MOTOR_ENDSTOP_MARGIN); + _motor.startMoveSteps(-static_cast(MOTOR_ENDSTOP_MARGIN)); if (!waitForMotor(2000)) { Debug.println("Motor homing: timeout during backoff"); return false; @@ -287,7 +368,9 @@ bool MotionController::moveToMax(uint32_t timeoutMs, return false; } - const long targetPosition = static_cast(MOTOR_MAX_STEPS - MOTOR_ENDSTOP_MARGIN); + // Home (pos=0) = pistone tutto inserito, siringa vuota → galleggia. + // u=1 → siringa piena → affonda. uToMotorPos() rispetta MOTOR_INVERT_LOGICAL. + const long targetPosition = uToMotorPos(1.0f); _motor.enableOutputs(); _motor.startMoveTo(targetPosition); @@ -318,8 +401,29 @@ bool MotionController::moveToMax(uint32_t timeoutMs, _motor.run(); - if (tofMaxExtensionStopReached(nowMs, lastTofSampleMs, "moveToMax")) { - return false; + // Limite TOF inferiore = siringa completamente estesa: stop pulito, + // non emergency stop. Permette al chiamante (es. balance) di fare + // l'hold a fine corsa invece di considerarlo un errore. + if (_tof.isInitialized() && nowMs - lastTofSampleMs >= MOTOR_HOMING_TOF_PERIOD_MS) { + lastTofSampleMs = nowMs; + float distanceMm = 0.0f; + if (_tof.readDistanceMm(distanceMm) && distanceMm <= TOF_SAFE_RANGE_MIN_MM) { + Debug.printf("moveToMax: TOF reached extension limit (%.1f <= %.1f mm)\n", + distanceMm, TOF_SAFE_RANGE_MIN_MM); + _motor.stop(); + while (_motor.distanceToGo() != 0) { + _motor.run(); + yield(); + } + _motor.disableOutputs(); + return true; + } + if (distanceMm > TOF_SAFE_RANGE_MAX_MM) { + Debug.printf("moveToMax: TOF safety stop, too far (%.1f > %.1f mm)\n", + distanceMm, TOF_SAFE_RANGE_MAX_MM); + emergencyStop("TOF above safe range"); + return false; + } } ledController.update(); @@ -354,8 +458,55 @@ bool MotionController::manualStepTest(long steps, uint32_t speed) { return success; } +bool MotionController::_balanceStrokeTo(long targetPos, + const char* label, + float stopPressureKpa, + uint8_t* pressureStopSamples, + uint32_t timeoutMs, + bool& pressureStopHit, + bool& remoteStopHit) { + pressureStopHit = false; + remoteStopHit = false; + + Debug.printf("Balance: %s to pos=%ld\n", label, targetPos); + _motor.enableOutputs(); + _motor.startMoveTo(targetPos); + + const unsigned long moveStart = millis(); + while (_motor.distanceToGo() != 0) { + if (remoteStopRequested()) { + remoteStopHit = true; + return false; + } + if (pressureStopReached(stopPressureKpa, pressureStopSamples)) { + pressureStopHit = true; + return false; + } + if (timeoutMs > 0 && millis() - moveStart > timeoutMs) { + char reason[48]; + snprintf(reason, sizeof(reason), "balance %s timeout", label); + emergencyStop(reason); + return false; + } + _motor.run(); + ledController.update(); + yield(); + } + + _motor.disableOutputs(); + return true; +} + bool MotionController::balance(uint32_t holdMs) { - if (!motionAllowed()) { + // Reset di sicurezza: la balance è una routine di spurgo manuale, parte + // sempre pulita anche se un emergency stop precedente non è stato cancellato. + clearEmergencyStop(); + Debug.println("Balance: starting"); + + if (!_motor.isPositionKnown()) { + // Senza homing non conosciamo l'orientamento della corsa: estendere + // alla cieca rischia di sbattere meccanicamente. Si richiede CMD_HOME. + Debug.println("Balance: homing required (motor position unknown)"); return false; } @@ -368,37 +519,34 @@ bool MotionController::balance(uint32_t holdMs) { stopPressureKpa, BALANCE_STOP_PRESSURE_DELTA_KPA); - while (motionAllowed()) { - if (remoteStopRequested()) { - return false; - } - - bool pressureStop = false; - - if (pressureStopReached(stopPressureKpa, &pressureStopSamples)) { - return true; - } + // u=1 → siringa piena (extend), u=0 → siringa vuota (retract a home). + // uToMotorPos() rispetta MOTOR_INVERT_LOGICAL: nessuna ipotesi sul segno qui. + const long extendedPos = uToMotorPos(1.0f); + const long retractedPos = uToMotorPos(0.0f); - Debug.println("Balance: extending"); - if (!moveToMax(MOTOR_HOMING_TIMEOUT, stopPressureKpa, &pressureStop, &pressureStopSamples)) { - return false; - } - if (pressureStop) { - return true; + while (motionAllowed()) { + if (remoteStopRequested()) return false; + if (pressureStopReached(stopPressureKpa, &pressureStopSamples)) return true; + + bool pressureHit = false, remoteHit = false; + if (!_balanceStrokeTo(extendedPos, "extend", stopPressureKpa, + &pressureStopSamples, MOTOR_HOMING_TIMEOUT, + pressureHit, remoteHit)) { + return pressureHit; // true se fermato da pressione, false altrimenti } + Debug.printf("Balance: hold extended (%lu ms)\n", (unsigned long)holdMs); if (waitWithPressureStop(holdMs, stopPressureKpa, &pressureStopSamples)) { return true; } - Debug.println("Balance: homing"); - if (!homeWithTof(stopPressureKpa, &pressureStop, &pressureStopSamples)) { - return false; - } - if (pressureStop) { - return true; + if (!_balanceStrokeTo(retractedPos, "retract", stopPressureKpa, + &pressureStopSamples, MOTOR_HOMING_TIMEOUT, + pressureHit, remoteHit)) { + return pressureHit; } + Debug.printf("Balance: hold retracted (%lu ms)\n", (unsigned long)holdMs); if (waitWithPressureStop(holdMs, stopPressureKpa, &pressureStopSamples)) { return true; } diff --git a/lib/motor/include/motor.h b/lib/motor/include/motor.h index 5516d31..e04c8ed 100644 --- a/lib/motor/include/motor.h +++ b/lib/motor/include/motor.h @@ -12,9 +12,12 @@ * belong to the firmware layer that coordinates multiple controllers. * * Coordinate system, once the firmware has established a reference: - * 0 = reference/home position - * MOTOR_MAX_STEPS = fully extended - * Safe range = [MOTOR_ENDSTOP_MARGIN, MOTOR_MAX_STEPS - MOTOR_ENDSTOP_MARGIN] + * 0 = reference/home position (pistone inserito, vuota) + * uToMotorPos(1.0f) = fully extended (siringa piena d'acqua) + * Safe range (symmetric) = [-(MAX-margin), +(MAX-margin)] + * See include/config.h: uToMotorPos() respects MOTOR_INVERT_LOGICAL. + * + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ diff --git a/lib/motor/src/motor.cpp b/lib/motor/src/motor.cpp index 6985812..a68248c 100644 --- a/lib/motor/src/motor.cpp +++ b/lib/motor/src/motor.cpp @@ -4,6 +4,10 @@ /* ******************************************************************************* * motor.cpp + * Stepper motor controller backed by FastAccelStepper: driver init, blocking + * and non-blocking movement primitives, position tracking, and symmetric + * target clamping consistent with uToMotorPos() in config.h. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ @@ -159,9 +163,12 @@ long MotorController::position() { // --------------------------------------------------------------------------- long MotorController::_clampTarget(long targetPosition) const { - return constrain(targetPosition, - static_cast(MOTOR_ENDSTOP_MARGIN), - static_cast(MOTOR_MAX_STEPS - MOTOR_ENDSTOP_MARGIN)); + // Range simmetrico per supportare la convenzione geometrica corrente + // (u=1 → estensione, mappata in direzione negativa con MOTOR_INVERT_LOGICAL=false). + // Vedi uToMotorPos() in config.h. + const long lo = -static_cast(MOTOR_MAX_STEPS - MOTOR_ENDSTOP_MARGIN); + const long hi = static_cast(MOTOR_MAX_STEPS - MOTOR_ENDSTOP_MARGIN); + return constrain(targetPosition, lo, hi); } // --------------------------------------------------------------------------- diff --git a/lib/pid/include/pid.h b/lib/pid/include/pid.h index f657930..260add3 100644 --- a/lib/pid/include/pid.h +++ b/lib/pid/include/pid.h @@ -5,35 +5,45 @@ /* ******************************************************************************* * pid.h - * Discrete PID controller for depth regulation. - * Uses depth-rate derivative (dDepth/dt) instead of d(error)/dt to avoid - * derivative kick on setpoint changes. + * PID di profondità con output normalizzato in [0, 1] (frazione di corsa + * siringa). Derivata su misura filtrata con LPF IIR, anti-windup conditional. + * Guadagni espressi in "frazione di corsa per metro di errore" — portabili tra + * geometrie diverse della siringa/motore. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ class PIDController { public: - // Gains are mutable at runtime (command 8 / UPDATE_PID) + // Mutabili a runtime via CMD_UPDATE_PID (8) float Kp; float Ki; float Kd; - explicit PIDController(float kp, float ki, float kd); + // Mutabili a runtime via CMD_UPDATE_PID_EXT (14) + float alphaD; // LPF coefficient sulla derivata, in (0, 1] + uint16_t periodMs; // Periodo del tick PID nel loop (ms) - // Reset integrator and history — call before each new PID session + // Offset costante di kick-start; somma direttamente all'output normalizzato + float uNeutral; + + PIDController(float kp, float ki, float kd); + + // Da chiamare prima di ogni nuova sessione PID void reset(); - // Compute one PID step. - // Returns a step count (positive = sink deeper, negative = rise). - float compute(float targetDepth, float currentDepth); + // Calcola il prossimo target normalizzato in [0, 1] + // targetDepth, currentDepth: in metri + // Ritorna: posizione siringa desiderata come frazione di corsa + float computeNormalized(float targetDepth, float currentDepth); private: - float _integral = 0.0f; - float _lastDepth = 0.0f; - float _lastError = 0.0f; - unsigned long _lastTimeMs = 0; + float _integral = 0.0f; + float _dFilt = 0.0f; + float _lastDepth = 0.0f; + unsigned long _lastTimeMs = 0; bool _hasLastDepth = false; }; -// Singleton — defined in pid.cpp +// Singleton — definito in pid.cpp extern PIDController pidController; diff --git a/lib/pid/src/pid.cpp b/lib/pid/src/pid.cpp index a9681e5..74fdf08 100644 --- a/lib/pid/src/pid.cpp +++ b/lib/pid/src/pid.cpp @@ -5,52 +5,68 @@ /* ******************************************************************************* * pid.cpp + * Depth PID implementation with output normalized to [0, 1] (syringe travel + * fraction). Filtered derivative on measurement (IIR LPF) and conditional + * anti-windup. Gains are expressed per metre of error, portable across + * syringe geometries. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ PIDController::PIDController(float kp, float ki, float kd) - : Kp(kp), Ki(ki), Kd(kd) {} + : Kp(kp), Ki(ki), Kd(kd), + alphaD(PID_ALPHA_D_DEFAULT), + periodMs(PID_PERIOD_DEFAULT_MS), + uNeutral(PID_U_NEUTRAL) {} void PIDController::reset() { - _integral = 0.0f; - _lastDepth = 0.0f; - _lastError = 0.0f; - _lastTimeMs = millis(); + _integral = 0.0f; + _dFilt = 0.0f; + _lastDepth = 0.0f; + _lastTimeMs = millis(); _hasLastDepth = false; } -float PIDController::compute(float targetDepth, float currentDepth) { - unsigned long now = millis(); +float PIDController::computeNormalized(float targetDepth, float currentDepth) { + const unsigned long now = millis(); float dt = (now - _lastTimeMs) / 1000.0f; - if (dt <= 0.0f) dt = PERIOD_MEASUREMENT / 1000.0f; + // Clamp robusto a primo tick / glitch (millis() rollover o reset) + if (dt <= 0.0f || dt > 1.0f) dt = periodMs / 1000.0f; const float error = targetDepth - currentDepth; // --- Proportional --- - const float proportional = Kp * error; + const float P = Kp * error; - // --- Integral with anti-windup --- - _integral += error * dt; - _integral = constrain(_integral, -PID_INTEGRAL_LIMIT, PID_INTEGRAL_LIMIT); - const float integral = Ki * _integral; + // --- Derivative on measurement, LPF IIR --- + const float dRaw = _hasLastDepth ? (currentDepth - _lastDepth) / dt : 0.0f; + _dFilt = alphaD * dRaw + (1.0f - alphaD) * _dFilt; + // d(error)/dt = -dDepth/dt (target costante a regime) + const float D = -Kd * _dFilt; - // --- Derivative on measurement (avoids setpoint-change kick) --- - const float depthRate = _hasLastDepth ? (_lastDepth - currentDepth) / dt : 0.0f; - const float derivative = Kd * depthRate; + // --- Integral (preview con vecchio accumulo, decisione di aggiornamento sotto) --- + const float I = Ki * _integral; - float output = proportional + integral + derivative; - output = constrain(output, -PID_OUTPUT_LIMIT, PID_OUTPUT_LIMIT); + const float uRaw = uNeutral + P + I + D; + const float uSat = constrain(uRaw, 0.0f, 1.0f); - // Update state for next iteration - _lastError = error; - _lastDepth = currentDepth; - _lastTimeMs = now; + // Conditional integration: aggiorna solo se non saturati nel verso dell'errore + const bool satHigh = (uRaw > 1.0f); + const bool satLow = (uRaw < 0.0f); + if (!((satHigh && error > 0.0f) || (satLow && error < 0.0f))) { + _integral += error * dt; + _integral = constrain(_integral, -PID_INTEGRAL_LIMIT, PID_INTEGRAL_LIMIT); + } + + _lastDepth = currentDepth; + _lastTimeMs = now; _hasLastDepth = true; - Debug.printf("PID: target=%.2f cur=%.2f err=%.2f P=%.2f I=%.2f D=%.2f out=%.2f\n", + Debug.printf("PID: tgt=%.2f cur=%.2f e=%.3f P=%.3f I=%.3f D=%.3f u=%.3f%s\n", targetDepth, currentDepth, error, - proportional, integral, derivative, output); + P, I, D, uSat, + (satHigh || satLow) ? " SAT" : ""); - return output; + return uSat; } diff --git a/lib/profile/include/profile.h b/lib/profile/include/profile.h index 8586a34..7269cb4 100644 --- a/lib/profile/include/profile.h +++ b/lib/profile/include/profile.h @@ -7,6 +7,7 @@ * profile.h * Depth profile execution: PID-controlled descent, bottom hold, ascent. * Also owns mission logging and stored data replay. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ diff --git a/lib/profile/src/profile.cpp b/lib/profile/src/profile.cpp index 9280fed..4e96517 100644 --- a/lib/profile/src/profile.cpp +++ b/lib/profile/src/profile.cpp @@ -14,6 +14,10 @@ /* ******************************************************************************* * profile.cpp + * Depth profile state machine: simple descent/ascent endpoints and PID-driven + * hold phases. Owns the flash CSV mission log and replay of stored packets to + * the control station. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ @@ -114,7 +118,10 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout pidController.reset(); if (isDeepTarget) { // Pre-position syringe to kick-start the deep descent only. - motionController.moveToWithTimeout(500, 0); + // u=0.979 → siringa quasi piena → spinta iniziale per "affondare". + // MOTOR_INVERT_LOGICAL=false: uToMotorPos mappa direttamente la + // convenzione logica sulla geometria nativa. + motionController.moveToWithTimeout(uToMotorPos(0.979f), 0); } } else { ledController.setState(LEDState::PROFILE); @@ -127,6 +134,10 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout bool motorCommanded = false; // For simple (non-PID) phases float lastDepth = 0.0f; int stableCount = 0; + // Per la fase PID: ultimo target assoluto comandato al motore (in step). + // Inizializzato al pre-position (u=0.979) per isDeepTarget, altrimenti alla + // posizione corrente — letta dopo il primo sensors.read() qui sotto. + long lastCommandedTarget = isDeepTarget ? uToMotorPos(0.979f) : motor.position(); // ----------------------------------------------------------------------- while (true) { @@ -145,7 +156,11 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout } // --- Measurement tick --- - if (millis() - lastMeasMs < PERIOD_MEASUREMENT) continue; + // Fase PID gira al ritmo configurabile pidController.periodMs (default 50 ms, + // modificabile via CMD_UPDATE_PID_EXT). Fasi simple restano a PERIOD_MEASUREMENT. + const uint16_t measPeriodMs = + isPIDPhase ? pidController.periodMs : PERIOD_MEASUREMENT; + if (millis() - lastMeasMs < measPeriodMs) continue; lastMeasMs = millis(); sensors.read(); @@ -163,7 +178,10 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout : "descending"; } } else if (isSurfaceTarget) { - phase = (fabsf(currentDepth - TARGET_SURFACE) < DEPTH_EPSILON) + // Riferimento: top del float a `surfaceTargetOffset` sotto il pelo. + // bottomDepth atteso = FLOAT_LENGTH + offset. + const float surfaceRefDepth = FLOAT_LENGTH + sensors.surfaceTargetOffset(); + phase = (fabsf(currentDepth - surfaceRefDepth) < DEPTH_EPSILON) ? "hold_40cm" : "ascending"; } else if (isBottomTarget) { @@ -179,9 +197,11 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout if (!isPIDPhase) { if (!motorCommanded) { if (isBottomTarget) { - motionController.moveToMax(); + // Bottom: u=1 logico → siringa piena → affonda + motionController.moveToWithTimeout(uToMotorPos(1.0f), 0); } else { - motionController.moveToWithTimeout(MOTOR_ENDSTOP_MARGIN, 0); // Surface + // Surface: u=0 logico → siringa vuota → galleggia + motionController.moveToWithTimeout(uToMotorPos(0.0f), 0); } motorCommanded = true; } @@ -200,9 +220,10 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout } } - // Surface hold: wait until depth ≈ FLOAT_LENGTH + // Surface hold: wait until depth ≈ FLOAT_LENGTH + surfaceTargetOffset if (isSurfaceTarget) { - if (fabsf(currentDepth - TARGET_SURFACE) < DEPTH_EPSILON) { + const float surfaceRefDepth = FLOAT_LENGTH + sensors.surfaceTargetOffset(); + if (fabsf(currentDepth - surfaceRefDepth) < DEPTH_EPSILON) { stableCount++; } else { stableCount = 0; @@ -218,13 +239,19 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout } // ---- PID phase ---- - const float pidOutput = pidController.compute(targetDepth, currentDepth); - long pidSteps = static_cast(pidOutput); - if (pidSteps != 0 && fabsf(targetDepth - currentDepth) > DEPTH_EPSILON) { - if (labs(pidSteps) < PID_MIN_MOVE_STEPS) { - pidSteps = (pidSteps > 0) ? PID_MIN_MOVE_STEPS : -PID_MIN_MOVE_STEPS; - } - motionController.moveToWithTimeout(motor.position() + pidSteps, 0, true); + // Output PID = posizione assoluta della siringa, frazione di corsa in [0, 1]. + // Comando motore NON bloccante: startMoveTo aggiorna il target di FastAccelStepper + // al volo, anche se il motore sta ancora viaggiando dal tick precedente. + const float u = pidController.computeNormalized(targetDepth, currentDepth); + const long usableSteps = + (long)MOTOR_MAX_STEPS - 2L * (long)MOTOR_ENDSTOP_MARGIN; + const long posTarget = uToMotorPos(u); + const long deadbandSteps = + (long)(PID_MIN_RETARGET_FRAC * (float)usableSteps); + if (labs(posTarget - lastCommandedTarget) >= deadbandSteps) { + motor.enableOutputs(); + motor.startMoveTo(posTarget); + lastCommandedTarget = posTarget; } // --- EEPROM write tick (also checks hold condition for PID phase) --- diff --git a/lib/sensors/include/sensors.h b/lib/sensors/include/sensors.h index de1dbf0..edd34b2 100644 --- a/lib/sensors/include/sensors.h +++ b/lib/sensors/include/sensors.h @@ -3,12 +3,14 @@ #include #include #include +#include "config.h" /* ******************************************************************************* * sensors.h * Wraps the Bar02 pressure sensor and INA219 power monitor. * Exposes depth calculation and battery voltage reading. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ @@ -36,6 +38,14 @@ class SensorManager { float topDepth(); float referenceDepthForPhase(const char* phase); + // Surface target: the top of the float should sit this many metres below + // the water surface when "floating". Runtime-tunable. + float surfaceTargetOffset() const { return _surfaceTargetOffsetM; } + void setSurfaceTargetOffset(float meters); + // Depth target for the float to reach the desired surface offset, expressed + // in the same reference as topDepth(). Equivalent to surfaceTargetOffset(). + float surfaceTargetDepth() const { return _surfaceTargetOffsetM; } + // Last raw pressure reading (Pa) float pressure(); @@ -51,6 +61,7 @@ class SensorManager { int8_t _inaDeviceIndex = -1; // -1 = not found yet float _atmPressurePa = 0.0f; // Reference pressure set at startup + float _surfaceTargetOffsetM = SURFACE_TARGET_OFFSET_M; void _initPressureSensor(); void _initPowerMonitor(); diff --git a/lib/sensors/src/sensors.cpp b/lib/sensors/src/sensors.cpp index 92199d9..16bab94 100644 --- a/lib/sensors/src/sensors.cpp +++ b/lib/sensors/src/sensors.cpp @@ -8,6 +8,10 @@ /* ******************************************************************************* * sensors.cpp + * Bar02 pressure sensor + INA219 power monitor wrappers. Computes depth from + * raw pressure using Stevino's principle and exposes top/bottom-of-float + * references plus the runtime-tunable surface target offset. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ @@ -93,6 +97,12 @@ float SensorManager::topDepth() { return sensorDepth() - SENSOR_TO_TOP_M; } +void SensorManager::setSurfaceTargetOffset(float meters) { + if (meters < 0.0f) meters = 0.0f; + _surfaceTargetOffsetM = meters; + Debug.printf("Surface target offset set to %.3f m\n", _surfaceTargetOffsetM); +} + float SensorManager::referenceDepthForPhase(const char* phase) { if (phase != nullptr && strcmp(phase, "hold_40cm") == 0) { return topDepth(); diff --git a/lib/tof/include/tof.h b/lib/tof/include/tof.h index dcb5c2c..d947ced 100644 --- a/lib/tof/include/tof.h +++ b/lib/tof/include/tof.h @@ -15,6 +15,7 @@ * zones into a single distance by taking the minimum valid range. This favours * detecting the nearest obstacle, which is the correct behaviour for the * syringe carriage approach. The enabled zone mask is configured in config.h. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean ******************************************************************************* */ diff --git a/lib/tof/src/tof.cpp b/lib/tof/src/tof.cpp index 0607273..e68c656 100644 --- a/lib/tof/src/tof.cpp +++ b/lib/tof/src/tof.cpp @@ -2,6 +2,16 @@ #include "config.h" +/* + ******************************************************************************* + * tof.cpp + * VL53L7CX multi-zone TOF driver wrapper. Initializes the sensor in 4x4 mode, + * aggregates the configured zone mask into a single distance by taking the + * minimum valid range, and applies the configured raw offset. + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean + ******************************************************************************* + */ + TofSensor::TofSensor(TwoWire& wire, uint8_t lpnPin, uint8_t gpio1Pin) : _wire(wire), _lpnPin(lpnPin), diff --git a/src/espA/main.cpp b/src/espA/main.cpp index 1d2a271..2ddcc4c 100644 --- a/src/espA/main.cpp +++ b/src/espA/main.cpp @@ -10,14 +10,14 @@ * config.h — pin definitions, tuning constants * led/led.h — RGB LED state machine * motor/motor.h — stepper motor controller - * tof/tof.h - VL53L4CD Time-of-Flight sensor controller + * tof/tof.h - VL53L7CX Time-of-Flight sensor controller * motion_control.h — homing, safe movement, and emergency stop * pid/pid.h — depth PID controller * sensors/sensors.h — Bar02 pressure sensor + INA219 power monitor * comms/comms.h — ESP-NOW messaging + OTA * profile/profile.h — depth profile execution + flash CSV logging * - * Maintainers: Colabella Davide + * Maintainers: Colabella Davide, Benevenga Filippo * Past contributors: Fachechi Gino Marco, Gullotta Salvatore * Company : Team PoliTOcean @ Politecnico di Torino * Board pkg : esp32 by Espressif Systems v2.0.17 @@ -63,20 +63,26 @@ SensorManager sensors; PIDController pidController(PID_KP_DEFAULT, PID_KI_DEFAULT, PID_KD_DEFAULT); ProfileManager profileManager; // CommsManager comms; // Uncomment if you need this instance here + +// Forward declarations — PID tuning helpers (test via seriale USB diretta) +static void servicePidTuningSerial(); +static void runSyringeSet(float uNorm, float durationS); +static void runPidHold(float depthTarget, float durationS); +static void runPidStep(float depthTarget); // --------------------------------------------------------------------------- // SETUP // --------------------------------------------------------------------------- void setup() { delay(100); Serial.begin(115200); - Serial.println("=== Float ESPA v10.0 — starting ==="); + Debug.println("=== Float ESPA v10.0 — starting ==="); // --- LED (first, so we can signal errors immediately) --- ledController.setState(LEDState::INIT); // --- EEPROM --- if (!EEPROM.begin(EEPROM_SIZE)) { - Serial.println("CRITICAL: EEPROM init failed"); + Debug.println("CRITICAL: EEPROM init failed"); ledController.setState(LEDState::ERROR); while (true) { ledController.update(); yield(); } } @@ -151,6 +157,7 @@ void setup() { void loop() { ledController.update(); motionController.serviceEmergencyStop(); + servicePidTuningSerial(); // ----------------------------------------------------------------------- switch (g_status) { @@ -186,6 +193,7 @@ void loop() { while (millis() - t0 < PERIOD_CONN_CHECK && comms.lastCommand().command == CMD_IDLE) { ledController.update(); + servicePidTuningSerial(); delay(10); } @@ -196,7 +204,7 @@ void loop() { } } else if (g_profileCount < PROFILE_MAX_COUNT && g_autoModeActive) { // No comms — activate autonomous mode - Serial.println("No comms — entering auto mode"); + Debug.println("No comms — entering auto mode"); ledController.setState(LEDState::AUTO_MODE); g_status = CMD_GO; g_autoCommitted = true; @@ -211,6 +219,9 @@ void loop() { if (ack && motionController.motionAllowed()) { Debug.println("MATE mission: starting vertical profiles"); + if (!g_autoCommitted) { + g_profileCount = 0; + } profileManager.resetEEPROM(); if (g_profileCount == 0) { profileManager.logDeploymentPacket(); @@ -330,13 +341,28 @@ void loop() { pidController.Kp = comms.lastCommand().params[0]; pidController.Ki = comms.lastCommand().params[1]; pidController.Kd = comms.lastCommand().params[2]; - Debug.printf("PID updated: Kp=%.2f Ki=%.2f Kd=%.2f\n", + Debug.printf("PID updated: Kp=%.3f Ki=%.3f Kd=%.3f\n", pidController.Kp, pidController.Ki, pidController.Kd); } g_status = CMD_IDLE; break; } + // ----------------------------------------------------------------------- + case CMD_UPDATE_PID_EXT: // Update PID period and derivative LPF coefficient + { + if (comms.sendMessage(CMD14_ACK, 1000)) { + const float periodMs = comms.lastCommand().params[0]; + const float alphaD = comms.lastCommand().params[1]; + pidController.periodMs = (uint16_t)constrain(periodMs, 20.0f, 500.0f); + pidController.alphaD = constrain(alphaD, 0.05f, 1.0f); + Debug.printf("PID ext updated: periodMs=%u alphaD=%.3f\n", + pidController.periodMs, pidController.alphaD); + } + g_status = CMD_IDLE; + break; + } + // ----------------------------------------------------------------------- case CMD_SET_SPEED: // Set test movement speed { @@ -398,6 +424,51 @@ void loop() { break; } + // ----------------------------------------------------------------------- + case CMD_SYRINGE_SET: // Test: posiziona siringa a u in [0,1] per N secondi + { + if (comms.sendMessage(CMD15_ACK, 1000)) { + const float u = comms.lastCommand().params[0]; + const float dur = comms.lastCommand().params[1]; + runSyringeSet(u, dur); + } + g_status = CMD_IDLE; + break; + } + + // ----------------------------------------------------------------------- + case CMD_PID_HOLD: // Test: PID a quota fissa per N secondi + { + if (comms.sendMessage(CMD16_ACK, 1000)) { + const float depth = comms.lastCommand().params[0]; + const float dur = comms.lastCommand().params[1]; + runPidHold(depth, dur); + } + g_status = CMD_IDLE; + break; + } + + // ----------------------------------------------------------------------- + case CMD_PID_STEP: // Test: step response PID a quota X per 60 s + { + if (comms.sendMessage(CMD17_ACK, 1000)) { + const float depth = comms.lastCommand().params[0]; + runPidStep(depth); + } + g_status = CMD_IDLE; + break; + } + + // ----------------------------------------------------------------------- + case CMD_SET_SURFACE_OFFSET: // Imposta target di galleggiamento (m sotto pelo) + { + if (comms.sendMessage(CMD18_ACK, 1000)) { + sensors.setSurfaceTargetOffset(comms.lastCommand().params[0]); + } + g_status = CMD_IDLE; + break; + } + // ----------------------------------------------------------------------- default: Debug.printf("Unknown command: %d\n", g_status); @@ -407,3 +478,231 @@ void loop() { delay(10); } + +// --------------------------------------------------------------------------- +// PID TUNING — comandi via seriale USB diretta +// +// Comandi accettati (uno per riga, terminato da \n): +// PARAMS — aggiorna guadagni PID +// PARAMS_EXT — aggiorna periodo tick e LPF coeff +// SYRINGE_SET — siringa a posizione normalizzata [0,1] +// per N secondi, log depth ogni 100 ms +// PID_HOLD — PID a quota X per N secondi, log a 5 Hz +// PID_STEP — step response: PID a quota X per +// max 60 s (esci a regime), log a 10 Hz +// SURFACE_OFFSET — target di galleggiamento: il top del +// float sta a sotto il pelo (default 0.10) +// +// Tutto il logging finisce su Serial (USB), formato CSV per facile import. +// --------------------------------------------------------------------------- +static String g_serialLineBuf; + +static void servicePidTuningSerial() { + while (Serial.available()) { + char c = Serial.read(); + if (c == '\r') continue; + if (c == '\n') { + String line = g_serialLineBuf; + g_serialLineBuf = ""; + line.trim(); + if (line.length() == 0) return; + + // Tokenize semplice (solo separatore spazio) + const char* cstr = line.c_str(); + char buf[96]; + strncpy(buf, cstr, sizeof(buf) - 1); + buf[sizeof(buf) - 1] = '\0'; + char* tok = strtok(buf, " "); + if (!tok) return; + + if (strcmp(tok, "PARAMS") == 0) { + char* a = strtok(nullptr, " "); + char* b = strtok(nullptr, " "); + char* d = strtok(nullptr, " "); + if (!a || !b || !d) { Debug.println("ERR: PARAMS "); return; } + pidController.Kp = atof(a); + pidController.Ki = atof(b); + pidController.Kd = atof(d); + Debug.printf("OK PARAMS Kp=%.4f Ki=%.4f Kd=%.4f\n", + pidController.Kp, pidController.Ki, pidController.Kd); + } else if (strcmp(tok, "PARAMS_EXT") == 0) { + char* a = strtok(nullptr, " "); + char* b = strtok(nullptr, " "); + if (!a || !b) { Debug.println("ERR: PARAMS_EXT "); return; } + pidController.periodMs = (uint16_t)constrain(atof(a), 20.0f, 500.0f); + pidController.alphaD = constrain((float)atof(b), 0.05f, 1.0f); + Debug.printf("OK PARAMS_EXT period=%u alpha=%.3f\n", + pidController.periodMs, pidController.alphaD); + } else if (strcmp(tok, "SYRINGE_SET") == 0) { + char* a = strtok(nullptr, " "); + char* b = strtok(nullptr, " "); + if (!a || !b) { Debug.println("ERR: SYRINGE_SET "); return; } + runSyringeSet(atof(a), atof(b)); + } else if (strcmp(tok, "PID_HOLD") == 0) { + char* a = strtok(nullptr, " "); + char* b = strtok(nullptr, " "); + if (!a || !b) { Debug.println("ERR: PID_HOLD "); return; } + runPidHold(atof(a), atof(b)); + } else if (strcmp(tok, "PID_STEP") == 0) { + char* a = strtok(nullptr, " "); + if (!a) { Debug.println("ERR: PID_STEP "); return; } + runPidStep(atof(a)); + } else if (strcmp(tok, "SURFACE_OFFSET") == 0) { + char* a = strtok(nullptr, " "); + if (!a) { Debug.println("ERR: SURFACE_OFFSET "); return; } + sensors.setSurfaceTargetOffset(atof(a)); + Debug.printf("OK SURFACE_OFFSET %.3f m\n", sensors.surfaceTargetOffset()); + } else { + Debug.printf("ERR: unknown cmd '%s'\n", tok); + } + return; + } + if (g_serialLineBuf.length() < 95) g_serialLineBuf += c; + } +} + +// Comanda direttamente la siringa a u in [0,1] e logga la profondità per +// caratterizzare la dinamica del float (costanti di tempo, guadagno DC). +// Bypassa il PID: utile per stimare guadagni iniziali. +static void runSyringeSet(float uNorm, float durationS) { + uNorm = constrain(uNorm, 0.0f, 1.0f); + if (durationS < 0.5f || durationS > 300.0f) { + Debug.println("ERR: durationS in [0.5, 300]"); + return; + } + const long posTarget = uToMotorPos(uNorm); + + Debug.printf("# SYRINGE_SET u=%.3f target_steps=%ld dur=%.1fs\n", + uNorm, posTarget, durationS); + Debug.println("# t_ms,depth_m,motor_pos"); + motor.enableOutputs(); + motor.startMoveTo(posTarget); + + const unsigned long t0 = millis(); + unsigned long lastLog = 0; + while (millis() - t0 < (unsigned long)(durationS * 1000.0f)) { + if (Serial.available()) { Debug.println("# aborted"); break; } + if (millis() - lastLog >= 100) { + lastLog = millis(); + sensors.read(); + Debug.printf("%lu,%.3f,%ld\n", + millis() - t0, sensors.depth(), motor.position()); + } + ledController.update(); + yield(); + } + motor.stop(); + motor.disableOutputs(); + Debug.println("# done"); +} + +// Hold PID a quota fissa per N secondi, log a 5 Hz con CSV completo. +// Versione "tarable" di un profile PID phase, senza pre-position né hold check. +static void runPidHold(float depthTarget, float durationS) { + if (depthTarget < 0.1f || depthTarget > 5.0f) { + Debug.println("ERR: depthTarget in [0.1, 5.0] m"); + return; + } + if (durationS < 1.0f || durationS > 600.0f) { + Debug.println("ERR: durationS in [1, 600]"); + return; + } + + Debug.printf("# PID_HOLD target=%.3fm dur=%.1fs Kp=%.4f Ki=%.4f Kd=%.4f " + "period=%u alpha=%.3f\n", + depthTarget, durationS, + pidController.Kp, pidController.Ki, pidController.Kd, + pidController.periodMs, pidController.alphaD); + Debug.println("# t_ms,depth_m,target_m,error_m,u_norm,motor_pos"); + + pidController.reset(); + motor.enableOutputs(); + + const long usable = (long)MOTOR_MAX_STEPS - 2L * (long)MOTOR_ENDSTOP_MARGIN; + const long deadbandSteps = (long)(PID_MIN_RETARGET_FRAC * (float)usable); + long lastCommandedTarget = motor.position(); + + const unsigned long t0 = millis(); + unsigned long lastTick = 0; + unsigned long lastLog = 0; + while (millis() - t0 < (unsigned long)(durationS * 1000.0f)) { + if (Serial.available()) { Debug.println("# aborted"); break; } + if (motionController.remoteStopRequested()) { Debug.println("# remote stop"); break; } + + if (millis() - lastTick >= pidController.periodMs) { + lastTick = millis(); + sensors.read(); + const float depth = sensors.depth(); + const float u = pidController.computeNormalized(depthTarget, depth); + const long posTarget = uToMotorPos(u); + if (labs(posTarget - lastCommandedTarget) >= deadbandSteps) { + motor.startMoveTo(posTarget); + lastCommandedTarget = posTarget; + } + if (millis() - lastLog >= 200) { + lastLog = millis(); + Debug.printf("%lu,%.3f,%.3f,%.3f,%.3f,%ld\n", + millis() - t0, depth, depthTarget, + depthTarget - depth, u, motor.position()); + } + } + ledController.update(); + yield(); + } + motor.stop(); + motor.disableOutputs(); + Debug.println("# done"); +} + +// Step response: cambio istantaneo del setpoint, esci a 60 s. +// Identico a PID_HOLD ma con durata fissa e log a 10 Hz per catturare la rampa. +static void runPidStep(float depthTarget) { + if (depthTarget < 0.1f || depthTarget > 5.0f) { + Debug.println("ERR: depthTarget in [0.1, 5.0] m"); + return; + } + Debug.printf("# PID_STEP target=%.3fm Kp=%.4f Ki=%.4f Kd=%.4f " + "period=%u alpha=%.3f\n", + depthTarget, + pidController.Kp, pidController.Ki, pidController.Kd, + pidController.periodMs, pidController.alphaD); + Debug.println("# t_ms,depth_m,target_m,error_m,u_norm,motor_pos"); + + pidController.reset(); + motor.enableOutputs(); + + const long usable = (long)MOTOR_MAX_STEPS - 2L * (long)MOTOR_ENDSTOP_MARGIN; + const long deadbandSteps = (long)(PID_MIN_RETARGET_FRAC * (float)usable); + long lastCommandedTarget = motor.position(); + + const unsigned long t0 = millis(); + unsigned long lastTick = 0; + unsigned long lastLog = 0; + while (millis() - t0 < 60000UL) { + if (Serial.available()) { Debug.println("# aborted"); break; } + if (motionController.remoteStopRequested()) { Debug.println("# remote stop"); break; } + + if (millis() - lastTick >= pidController.periodMs) { + lastTick = millis(); + sensors.read(); + const float depth = sensors.depth(); + const float u = pidController.computeNormalized(depthTarget, depth); + const long posTarget = uToMotorPos(u); + if (labs(posTarget - lastCommandedTarget) >= deadbandSteps) { + motor.startMoveTo(posTarget); + lastCommandedTarget = posTarget; + } + if (millis() - lastLog >= 100) { + lastLog = millis(); + Debug.printf("%lu,%.3f,%.3f,%.3f,%.3f,%ld\n", + millis() - t0, depth, depthTarget, + depthTarget - depth, u, motor.position()); + } + } + ledController.update(); + yield(); + } + motor.stop(); + motor.disableOutputs(); + Debug.println("# done"); +} diff --git a/src/espB/main.cpp b/src/espB/main.cpp index d14a55f..c5a5ea6 100644 --- a/src/espB/main.cpp +++ b/src/espB/main.cpp @@ -4,7 +4,8 @@ * * Filename: main.cpp (formerly espB.ino) * Version: 10.0.0 -* Developers: Fachechi Gino Marco, Gullotta Salvatore +* Maintainers: Colabella Davide, Benevenga Filippo +* Past contributors: Fachechi Gino Marco, Gullotta Salvatore * Company: Team PoliTOcean @ Politecnico di Torino * Arduino board package: esp32 by Espressif Systems, v2.0.17 * diff --git a/test/unit_hw/espb_bridge/test_protocol_contract/test_protocol_contract.cpp b/test/unit_hw/espb_bridge/test_protocol_contract/test_protocol_contract.cpp index 3592242..4f13737 100644 --- a/test/unit_hw/espb_bridge/test_protocol_contract/test_protocol_contract.cpp +++ b/test/unit_hw/espb_bridge/test_protocol_contract/test_protocol_contract.cpp @@ -22,18 +22,38 @@ void tearDown() {} void test_protocol_contract_table_matches_parser() { size_t count = 0; const EspbProtocolCommand* commands = espbProtocolCommands(count); - TEST_ASSERT_EQUAL_UINT8(13, count); + TEST_ASSERT_EQUAL_UINT8(18, count); for (size_t i = 0; i < count; ++i) { char commandLine[64]; - if (commands[i].commandCode == CMD_UPDATE_PID) { - snprintf(commandLine, sizeof(commandLine), "%s 1 2 3", commands[i].commandText); - } else if (commands[i].commandCode == CMD_SET_SPEED) { - snprintf(commandLine, sizeof(commandLine), "%s 300", commands[i].commandText); - } else if (commands[i].commandCode == CMD_TEST_STEPS) { - snprintf(commandLine, sizeof(commandLine), "%s -100", commands[i].commandText); - } else { - snprintf(commandLine, sizeof(commandLine), "%s", commands[i].commandText); + switch (commands[i].commandCode) { + case CMD_UPDATE_PID: + snprintf(commandLine, sizeof(commandLine), "%s 1 2 3", commands[i].commandText); + break; + case CMD_UPDATE_PID_EXT: + snprintf(commandLine, sizeof(commandLine), "%s 50 0.25", commands[i].commandText); + break; + case CMD_SET_SPEED: + snprintf(commandLine, sizeof(commandLine), "%s 300", commands[i].commandText); + break; + case CMD_TEST_STEPS: + snprintf(commandLine, sizeof(commandLine), "%s -100", commands[i].commandText); + break; + case CMD_SYRINGE_SET: + snprintf(commandLine, sizeof(commandLine), "%s 0.5 5", commands[i].commandText); + break; + case CMD_PID_HOLD: + snprintf(commandLine, sizeof(commandLine), "%s 1.5 30", commands[i].commandText); + break; + case CMD_PID_STEP: + snprintf(commandLine, sizeof(commandLine), "%s 1.5", commands[i].commandText); + break; + case CMD_SET_SURFACE_OFFSET: + snprintf(commandLine, sizeof(commandLine), "%s 0.10", commands[i].commandText); + break; + default: + snprintf(commandLine, sizeof(commandLine), "%s", commands[i].commandText); + break; } EspbParsedCommand parsed = espbParseSerialCommand(commandLine); @@ -62,6 +82,11 @@ void test_ack_constants_match_gui_contract() { TEST_ASSERT_EQUAL_STRING("DEBUG_MODE_RECVD", CMD11_ACK); TEST_ASSERT_EQUAL_STRING("HOME_RECVD", CMD12_ACK); TEST_ASSERT_EQUAL_STRING("STOP_RECVD", CMD13_ACK); + TEST_ASSERT_EQUAL_STRING("CHNG_PID_EXT_RECVD", CMD14_ACK); + TEST_ASSERT_EQUAL_STRING("SYRINGE_SET_RECVD", CMD15_ACK); + TEST_ASSERT_EQUAL_STRING("PID_HOLD_RECVD", CMD16_ACK); + TEST_ASSERT_EQUAL_STRING("PID_STEP_RECVD", CMD17_ACK); + TEST_ASSERT_EQUAL_STRING("SURFACE_OFF_RECVD", CMD18_ACK); } void test_status_tokens_are_gui_parseable() {