Project/andrei.rusanescu

website/versioned_docs/version-acs_cc/project/2026/andrei.rusanescu/index.md

@@ -14,15 +14,17 @@ Remote-controlled cargo robot with Bluetooth telemetry
 
 CargoBot is a remote-controlled four-wheeled robot that carries cargo and navigates surfaces with imperfections. It is controlled from a laptop keyboard via Bluetooth and uses an STM32 Nucleo-U545RE-Q microcontroller programmed in Rust with Embassy-rs. It sends real-time telemetry data to a PC dashboard via Bluetooth, while simultaneously displaying information on an onboard OLED.
 
-The central idea of the project is measuring and visualizing the impact of cargo load on motor performance: when the robot carries something heavy, the PID controller automatically increases the PWM duty cycle to maintain a constant speed. This compensation is observable live on the robot's OLED, on the PC dashboard, and through 3 LEDs (green/yellow/red) that visually indicate the effort level.
+The central idea of the project is measuring and visualizing the impact of cargo load on motor performance: when the robot carries something heavy, the PID controller automatically increases the PWM duty cycle to maintain a constant speed. This compensation is observable live on the robot's OLED, on the PC dashboard, and through 3 LEDs (green/blue/red) that visually indicate the effort level.
 
 ## Motivation
 
-I am particularly interested in cars and networking. This project combines multiple peripherals studied in the lab (PWM, GPIO, I2C, UART, Bluetooth) into a functional system. It is a challenge that clearly demonstrates technical effort - the difference in motor effort with and without cargo, and how to compensate for the load.
+I am particularly interested in cars and networking. This project combines multiple peripherals studied in the lab (PWM, GPIO, I2C, UART (Bluetooth)) into a functional system. It is a challenge that clearly demonstrates technical effort - the difference in motor effort with and without cargo, and how to compensate for the load. It really excites me because I get to apply what I've learnt in electronics, microprocessor programming in Rust and really build a hardware product.
 
 ## Architecture
 
-![Architecture](schema_bloc.svg)
+![Hardware diagram](schema_bloc.svg)
+
+Both the hardware and software diagrams are drawn in `draw.io`.
 
 The system is organized around five main subsystems:
 
@@ -38,42 +40,41 @@ The L298N dual H-bridge receives PWM signals from the STM32 and drives 4 DC moto
 - 2x IR LM393 encoders: RPM feedback for PID
 
 **4. Communication Subsystem**
-HC-06 Bluetooth module connected to STM32 via UART. Bidirectional: laptop sends movement commands (forward/backward/left/right + speed), robot sends back JSON telemetry at 5Hz.
+HC-06 Bluetooth module connected to STM32 via UART. Bidirectional: laptop sends raw keyboard characters (w, a, s, d, x, p), and the robot streams back high-efficiency text-based CSV telemetry (T,Load,Comp,Ax,Ay,Gz,Slope) at 10Hz to reduce microcontroller overhead.
 
 **5. Display & Indicators Subsystem**
-- OLED SSD1306 128x64 (I2C, address 0x3C, shared bus with IMU): displays RPM, tilt angle, obstacle distance, state, load level
-- 3x LEDs (green/yellow/red) on GPIO: visual indicator of motor effort based on PWM duty cycle
-- Passive buzzer on PWM: horn and audio feedback
+- OLED SSD1306 128x64 (I2C, address 0x3C, shared bus with IMU): displays active metrics like distance, encoder ticks, load %, slope, and current power mode.
+- 3x LEDs (green/blue/red) on GPIO: visual indicator of motor effort based on PWM duty cycle
+- PC Dashboard: Tkinter window with embedded real-time Matplotlib animation canvas displaying streaming physics graphs.
 
 **Embassy-rs Async Tasks:**
 
 | Task | Frequency | Responsibility |
 |------|-----------|----------------|
-| sensor_task | 20 Hz | Read IMU (I2C) + ultrasonic sensors |
-| motor_task | 50 Hz | Apply PWM to L298N, read encoders |
-| navigation_task | 10 Hz | State machine: FORWARD / COMPENSATE / AVOID / STOP |
-| telemetry_task | 5 Hz | Serialize JSON and send over UART to HC-06 |
-| display_task | 4 Hz | Update OLED |
+| encoder_left_task / right | Interrupt-driven | Counts wheel encoder slot edges using GPIO EXTI triggers. |
+| distance_task | 16 Hz | Triggers and measures echo responses from front/rear HC-SR04 sensors. |
+| imu_task | 50 Hz | Reads raw Accel/Gyro data from MPU-6500 over I2C and stores scaled values. |
+| pid_task | 5 Hz | Performs Low-Pass filtering, fast auto-calibration, and runs the PI speed compensation loop. |
+| telemetry_task | 10 Hz | Formats data into a CSV string and transmits it wirelessly over UART. |
+| display_task | 2 Hz | Refreshes the on-board OLED graphics. |
+| bluetooth_task | Async Rx | Listens for incoming control characters from the PC. |
 
-**Navigation State Machine:**
+**Navigation & PI Compensation Logic:**
+Instead of basic tilt-triggering, CargoBot uses an intelligent sensor-fusion approach:
+1. **Auto-Calibration**: On first throttle, it calibrates the ideal steady-state wheel RPM.
+2. **Load Tracking**: A Low-Pass filter smooths out encoder noise. If the filtered RPM drops below the reference value due to cargo weight or friction, the robot calculates the exact `Load %`.
+3. **PI Regulation**: A Proporțional-Integral loop dynamically scales up the PWM duty cycle to maintain constant cruise speed, bypassing inertia during start-up via a dedicated blind window.
 
-| State | Entry Condition | Action |
-|-------|----------------|--------|
-| FORWARD | No obstacle | Move at target speed, PID active |
-| COMPENSATE | Pitch > 5 deg (ramp) | Increase PWM proportional to tilt angle |
-| AVOID | Obstacle < 30cm | Stop, rotate, resume forward |
-| STOP | Manual command / error | Motors off, telemetry continues |
 
 **Peripheral Usage:**
 
 | Peripheral | Component | Usage |
 |-----------|-----------|-------|
 | PWM | STM32 -> L298N | Motor speed control (0–100% duty cycle) |
-| PWM | STM32 -> Buzzer | Horn |
-| GPIO Output | STM32 -> HC-SR04 trigger | 10 micro-s pulse to trigger ultrasonic |
+| GPIO Output | STM32 -> HC-SR04 trigger | pulse to trigger ultrasonic |
 | GPIO Input Interrupt | HC-SR04 echo -> STM32 | Measure echo duration -> distance |
 | GPIO Input Interrupt | LM393 encoders -> STM32 | Count pulses -> compute RPM |
-| GPIO Output | STM32 -> LEDs R/Y/G | Load indicator: green (&lt;35%), yellow (35–65%), red (>65%) |
+| GPIO Output | STM32 -> LEDs R/G/B | Load indicator: green (ECO mode), blue (DRIVE mode), red (SPEED mode) |
 | GPIO Output | STM32 -> L298N IN1-IN4 | Motor direction control |
 | I2C (shared bus) | STM32 -> MPU-6500 (0x68) | Accelerometer + gyroscope for tilt angle |
 | I2C (shared bus) | STM32 -> SSD1306 (0x3C) | OLED telemetry display |
@@ -101,15 +102,59 @@ more male-female and female-female jumpers and a smaller breadboard (400 points)
 Soldered IMU and OLED display in the lab.
 Assembled final product.
 Started to write the software for the cargobot and tested it carrying another car.
-The HC-SR04 sensors are not so precise as the car sometimes crashes into walls if driven into.
+The car automatically stops when it detects an object at less than 15cm distance.
 
 ### Week 11 - 17 May
-
-
-## Hardware
-
-The robot is built on a 4WD chassis with 4 DC motors (3–6V) driven through an L298N dual H-bridge using skid steering (left/right side in parallel). Speed and direction are controlled via PWM from the STM32. Two IR optical sensors read encoder discs on the motors to compute RPM. An MPU-6500 IMU over I2C measures tilt angle using a complementary filter. Two HC-SR04 ultrasonic sensors handle obstacle detection front and rear. An SSD1306 OLED displays live telemetry. An HC-06 Bluetooth module provides bidirectional communication with the laptop.
-
+Wrote even more software. Noticed that with adding more components, the responsiveness of the
+bluetooth commands degraded, so I had to debug a lot with the frequencies, which led me to
+activate the 16MHz crystal of the board to raise the frequency on i2c (for the display).
+
+I had problems with the HC-SR04 distance sensors as well. They are placed back-to-back,
+which initially caused significant acoustic interference (cross-talk) because they were
+attempting to fire and update simultaneously at the same frequency. This ultrasonic overlap
+caused one of the sensors to constantly freeze or report false timeouts, as it would accidentally
+catch the stray echo generated by the opposite sensor. 
+
+To solve this limitation, I refactored the software into a single, synchronized Embassy async
+task (`distance_task`). Instead of running concurrently, the sensors are now triggered sequentially:
+the system measures the front distance, waits for a deliberate 30ms gap to let any residual acoustic
+noise dissipate, and only then triggers the rear sensor. This simple interleaving completely eliminated
+the sensor locking issue.
+
+### Week 18 - 24 May
+Finalized the closed-loop motor control by deploying a tuned PI velocity regulator ($K_P=35, K_I=15$) and locking the noisy derivative term ($K_D$) to zero.
+Implemented a $7/8$ exponential moving average Low-Pass filter to smooth out encoder slot jitter, a rapid auto-calibration routine that samples the target cruise RPM during the first seconds of movement and a 1.6-second startup blind window to stop the controller from confusing physical takeoff inertia with cargo load.
+
+Added another 2 compensation channels:
+1. Yaw-rate correction (lateral drift): The gyroscope $G_z$ axis is sampled
+at every PI loop tick (5 Hz). When the robot is commanded straight
+(`w` / `s`) and a non-zero yaw rate is detected, caused by an
+asymmetrically placed load or a difference in motor friction, the
+controller applies an asymmetric PWM correction. The yaw mixer is disabled
+during deliberate turns (`a` / `d`) via the `IS_TURNING` flag so that it
+does not fight the intentional rotation.
+It takes a few tries to get the right values.
+
+2. Slope detection and pre-emptive compensation: The accelerometer $A_y$
+axis (aligned with the driving direction after the IMU mounting orientation
+was taken into account) is read each PI tick. The slope compensation is
+clamped to +/- 30 % PWM. The result is published to `SLOPE_COMP` and added 
+**symmetrically** to both wheels in the motor loop, acting as a 
+feed-forward term that pre-emptively increases the
+duty cycle before the PI loop has had time to react to the RPM drop.
+
+Built the custom companion PC interface using Python Tkinter and Matplotlib
+to handle low-latency key debouncing and animate the 10Hz text-based CSV
+telemetry streams (`T,Load,Comp,Ax,Ay,Gz,Slope`) in real time.
+
+
+## Hardware design
+
+The robot is built on a 4WD chassis powered by four DC motors (3–6V) wired in parallel per side (skid steering) and driven by an L298N dual H-bridge. Speed is dynamically regulated via 1kHz PWM signals from the STM32, while directional control is managed through discrete GPIO pins. Two LM393 IR optical sensors read wheel encoder discs to provide real-time RPM feedback, which serves as the primary metric for the PI load-compensation algorithm.
+
+For environmental and physics telemetry, an MPU-6500 IMU communicates over a shared I2C bus to track linear accelerations ($A_x$, $A_y$, $A_z$) and yaw rate ($G_z$). Spatial awareness is handled by two HC-SR04 ultrasonic sensors placed at the front and rear, utilizing timed GPIO echo interrupts for obstacle detection. Visual feedback is split between an onboard SSD1306 OLED display for standalone diagnostics and an HC-06 Bluetooth module that streams raw, high-frequency CSV telemetry packets to a custom Python dashboard on a PC.
+
+![Car at night](cargobot_night.webp)
 ![Car from above](IMG_3541.webp)
 ![Car from a side](IMG_3536.webp)
 
@@ -120,6 +165,68 @@ KiCad Schematic:
 ![KiCad Schematic](PM_PROJECT.svg)
 
 
+## Software design
+The entire program is `#![no_std]` and `#![no_main]`;
+there is no heap, no operating system, and no RTOS scheduler, thus
+all concurrency is cooperative and driven by `async`/`await`.
+
+### Concurrency Model
+
+Embassy runs a single-threaded cooperative executor. Every task is a Rust
+`async fn` marked with `#[embassy_executor::task]`. Because only one task
+runs at a time and tasks yield at every `.await` point, shared state can be
+safely exchanged through **lock-free atomics** (`AtomicU32`, `AtomicI32`,
+`AtomicBool`) for single-value reads/writes, and through an
+`embassy_sync::channel::Channel` for the command queue. The I2C bus, which
+is shared between the IMU and the OLED, is protected by an
+`embassy_sync::mutex::Mutex<ThreadModeRawMutex, I2c>` stored in a
+`StaticCell` so that its `'static` lifetime can be passed to multiple tasks.
+
+I chose lock-free atomics because they are fast they can be used with ease
+in more then 2 functions (if I used a channel instead of atomics it would have gotten
+very complicated very fast). Atomics ar fast and simple.
+
+
+### Global Shared State - Atomic Variables
+
+All inter-task communication goes through a flat set of `static` atomics.
+The table below maps each atomic to the task that **writes** it and the tasks that **reads** it.
+
+| Atomic | Type | Writer | Readers | Meaning |
+|--------|------|--------|---------|---------|
+| `DIST_FRONT_CM` | `AtomicU32` | `distance_task` | `display_task`, `main` (motor) | Front HC-SR04 distance (cm) |
+| `DIST_REAR_CM` | `AtomicU32` | `distance_task` | `display_task`, `main` (motor) | Rear HC-SR04 distance (cm) |
+| `EMERGENCY_STOP_FRONT` | `AtomicBool` | `distance_task` | `main` (motor) | True when front obstacle < 15 cm |
+| `EMERGENCY_STOP_REAR` | `AtomicBool` | `distance_task` | `main` (motor) | True when rear obstacle < 15 cm |
+| `ENC_L_TICKS` | `AtomicU32` | `encoder_left_task` | `pid_task`, `display_task` | Cumulative left encoder ticks (20 per slot) |
+| `ENC_R_TICKS` | `AtomicU32` | `encoder_right_task` | `pid_task`, `display_task` | Cumulative right encoder ticks |
+| `IMU_AX` | `AtomicI32` | `imu_task` | `pid_task`, `telemetry_task` | Accel X x100 (% of 1 g) |
+| `IMU_AY` | `AtomicI32` | `imu_task` | `pid_task`, `telemetry_task` | Accel Y x100, LPF-filtered |
+| `IMU_AZ` | `AtomicI32` | `imu_task` | `telemetry_task` | Accel Z x100 |
+| `IMU_GZ` | `AtomicI32` | `imu_task` | `pid_task`, `telemetry_task` | Yaw rate x100 (deg/s), LPF-filtered |
+| `LOAD_PERCENT` | `AtomicU32` | `pid_task` | `display_task`, `telemetry_task` | Load % (0 = no load, 100 = motor blocked) |
+| `PID_COMPENSATION` | `AtomicI32` | `pid_task` | `display_task`, `telemetry_task` | Symmetric PI output (% PWM delta) |
+| `PID_COMP_LEFT` | `AtomicI32` | `pid_task` | `main` (motor) | Left wheel comp after yaw mixer |
+| `PID_COMP_RIGHT` | `AtomicI32` | `pid_task` | `main` (motor) | Right wheel comp after yaw mixer |
+| `SLOPE_COMP` | `AtomicI32` | `pid_task` | `main` (motor), `display_task`, `telemetry_task` | Feed-forward slope compensation (% PWM) |
+| `PWM_LEFT_ACTUAL` | `AtomicU8` | `main` (motor) | `pid_task` | Actual left PWM duty sent to L298N |
+| `PWM_RIGHT_ACTUAL` | `AtomicU8` | `main` (motor) | `pid_task` | Actual right PWM duty sent to L298N |
+| `POWER_MODE` | `AtomicU8` | `main` (motor) | `display_task`, `telemetry_task` | 0=ECO, 1=DRIVE, 2=SPORT |
+| `PID_CALIBRATED` | `AtomicBool` | `pid_task` | `display_task`, `telemetry_task`, `pid_task` | True once reference RPM is locked |
+| `REFERENCE_RPM_X100` | `AtomicU32` | `pid_task` | `pid_task` | Calibrated cruise RPM x100 |
+| `IS_TURNING` | `AtomicBool` | `main` (motor) | `pid_task` | True during `a`/`d` commands |
+
+The single channel:
+| Channel | Type | Sender | Receiver | Meaning |
+|---------|------|--------|----------|---------|
+| `CMD_CHANNEL` | `Channel<_, u8, 4>` | `bluetooth_task` | `main` (motor) | Raw command bytes: `w s a d x p` |
+
+
+### Software diagram
+
+![software_diagram](software_diagram_colored.svg)
+
+
 
 ### Bill of Materials
 
@@ -155,7 +262,7 @@ KiCad Schematic:
 | [defmt](https://github.com/knurling-rs/defmt) + defmt-rtt | Logging framework | Debug output via probe |
 | [libm](https://github.com/rust-lang/libm) | Math functions (no_std) | atan2, sqrt for complementary filter |
 | Python pyserial | Serial communication | PC-side script to receive telemetry and send commands over Bluetooth |
-| Python matplotlib / Flask | Data visualization | Live dashboard with RPM, PWM, tilt, distance graphs |
+| Python matplotlib + tkinter | Desktop GUI & Plotting | Live dashboard with telemetry and PID |
 
 ## Links