L1N16R-V3

L1N16R - Advanced Maze-Solving Line Follower Robot (Teensy 4.1)

Short: Competition-ready firmware for a high-speed maze-solving line follower using Teensy 4.1, 16 IR sensors (QTR array), N20 encoders, TB6612FNG motor drivers, optional ADRC+PID control, speed profiling, enhanced flood-fill maze solving, OLED display and SD logging.

---

Contents

1. Overview
2. Key features
3. Hardware
4. Wiring / Pinout
5. Firmware architecture
6. Build & Upload
7. Calibration & Tuning
8. Performance analysis & optimization tips
9. Troubleshooting
10. File layout
11. License

---

1. Overview

This repository contains the Teensy 4.1 firmware (L1N16R_V3.ino) for an advanced line follower robot aimed at competitive maze-solving. The code combines an adaptive PID controller with an Active Disturbance Rejection Controller (ADRC) mixer, dynamic speed profiling, an enhanced flood-fill algorithm for mapping and path planning, and buffered SD-card logging.

The firmware is written for performance and flexibility — it supports both PID-only mode and a hybrid ADRC+PID mode, buffered telemetry, encoder interrupts, and advanced sensor calibration routines.

2. Key features

• Teensy 4.1 targeted (high-performance Cortex-M7)
• 16 IR QTR sensor array & calibration (readLineBlack)
• Adaptive PID with anti-windup + ADRC mixing
• Dynamic speed profiling and motion profiles for straight/turns
• Flood-Fill based maze mapping with enhanced update rules
• Encoder support (N20) via interrupts
• OLED status display (SSD1306)
• SD card buffered logging & large-buffer flush
• Non-blocking main loop (no delays in RUNNING state)
• Configurable compile-time constants and runtime debug levels

3. Hardware

Core components (recommended):

• Teensy 4.1
• QTR-16RC / QTR sensor array (16 sensors)
• 2 × N20 motors with incremental encoders
• TB6612FNG motor driver (or equivalent H-bridge)
• SSD1306 128x64 OLED (I2C)
• SD card breakout (SPI)
• Battery/power system sized for desired runtime

Optional:

• External IMU (if you want heading feedback)
• Higher quality encoders (for more reliable odometry)

4. Wiring / Pinout

The firmware uses a custom PCB pin mapping near the top of L1N16R_V3.ino. Example pins (adjust to your build):

• Motor PWM: MOTOR_LEFT_PWM = D5, MOTOR_RIGHT_PWM = D6
• Motor DIR: MOTOR_LEFT_DIR1 = D7, MOTOR_LEFT_DIR2 = D8, MOTOR_RIGHT_DIR1 = D9, MOTOR_RIGHT_DIR2 = D10
• Encoders: ENC_LEFT_A = D20, ENC_LEFT_B = D21, ENC_RIGHT_A = D22, ENC_RIGHT_B = D23
• QTR sensors: wired to analog pins (see code to map qtr array)
• OLED: I2C (SDA, SCL)
• SD CS: SD_CS_PIN (modify if using different CS pin)
• Buttons/LEDs: BUTTON_PIN, LED_PIN, END_ZONE_LED_PIN

Important: Verify pin numbers in the code match your board/PCB before powering anything.

5. Firmware architecture

The code is organized into high-level functional blocks:

• setup() — initialization: sensors, encoders (attachInterrupt), display, SD, calibration
• loop() — state machine (READY, RUNNING, PAUSED, STOPPED). In RUNNING the loop calls:
  • updateEncoderData() — read / compute odometry (encoders updated via ISRs)
  • updateSpeedProfile() — compute targetSpeed from motion planner
  • executeMazeNavigation() — FSM handling FOLLOWING_LINE, INTERSECTION, TURNING etc.
  • monitorSystemHealth() — battery/thermals/timeout checks
  • Periodic display & log flush
• Control core: enhancedFollowLine() does sensor read (qtr.readLineBlack), computes error, applies enhanced PID + optional ADRC, produces left/right PWM via setMotorSpeeds()
• Maze solver: enhancedFloodFillUpdate() and handleIntersection() implement mapping and decision logic
• Logging: buffered logger with periodic flushLargeLogBuffer() to SD

6. Build & Upload

1. Install Teensyduino (matching your Arduino IDE version) and Teensy loader for your OS.
2. Open L1N16R_V3.ino in Arduino IDE.
3. Select Teensy 4.1 as the board. Enable maximum optimization (-O3) if available.
4. Configure any compile-time constants (pins, USE_ADRC, MAX_SPEED, etc.) at the top of the file.
5. Compile and upload using the Teensy Loader.

Tip: disable verbose Serial logs for fastest loop performance (or gate logging by a LOG_LEVEL macro).

7. Calibration & Tuning

Sensor calibration

• Run the calibrateSensors() routine while slowly moving the robot over black/white transitions to record correct min/max for each QTR sensor.
• Verify sensorCal.range and sensorCal.scaleFactor look reasonable in serial output.

PID / ADRC tuning

Default control constants are near the top of the file. Suggested workflow:

1. Start in PID only mode (USE_ADRC = false).
2. Reduce maxSpeed to a safe low value so mistakes don't damage hardware.
3. Tune Kp until you get acceptable tracking with moderate overshoot.
4. Add Ki for steady-state error. Use small values to avoid windup.
5. Add Kd to damp oscillation. The code already computes derivative using deltaTime.
6. Once stable, enable USE_ADRC and tune ADRC internal params carefully — ADRC helps reject disturbances at speed but can interact with PID.

Anti-windup: the code includes an integral limit computation. If you still see windup, reduce Ki or reduce integral limit.

Speed profiling

• updateSpeedProfile() controls acceleration and cornering speeds. Tune maxSpeed, accel, and turn target speeds to match your robot's traction.

8. Performance analysis & optimization tips (for maximum speed)

This project already implements many competition-oriented practices. Below are additional suggestions to eke out the last bits of performance.

A. Reduce floating-point math

• Teensy 4.1 has an FPU, but heavy floating math in sensor/ctl loops still costs cycles. Consider converting core PID calculations to fixed-point (Q15/Q16) if profiling shows CPU bound.

B. Minimize blocking I/O & Serial

• Avoid Serial.print inside the running control loop. Use a log buffer (already present) and flush infrequently.
• Gate debug prints behind #if LOG_LEVEL > 0 macros.

C. Encoder ISRs

• Current ISRs call digitalRead() which is safe but slower. For maximum performance, read the port register directly (e.g., GPIOx_PDIR or digitalReadFast() / direct port access) and keep ISRs as short as possible.
• Ensure shared variables updated in ISRs (encoderLeftCount) are declared volatile and access them briefly inside critical sections with interrupts disabled when reading multi-byte values.

D. PWM & Motor updates

• Use fast PWM (avoid slow software PWM). Teensy 4.1 hardware PWM or analogWrite() configured for high frequency is preferred.
• Offload low-level motor PWMs to dedicated timers if jitter is observed.

E. Avoid delay() during RUNNING

• The state machine avoids delays in RUNNING. Ensure any future code additions also avoid blocking delays.

F. Sensor read frequency

• QTR readLineBlack is fairly efficient, but you can optimize by adjusting sample count and emitter on/off timing. Reducing the number of samples (if safe) increases loop frequency.

G. Compile optimizations

• Ensure build uses optimization level -O3 and link-time optimization where possible.
• Strip debug symbols for race runs.

H. Use DMA / hardware acceleration

• If using more advanced motor drivers or ADCs, use DMA to collect sensor samples without CPU overhead.

9. Troubleshooting

• Robot oscillates violently: reduce Kp, increase Kd, reduce maxSpeed, and check wheel slip.
• Encoders behave inconsistently: debounce encoder signals in hardware or switch to differential receivers; inspect ISR frequency.
• Lost line at high speed: increase QTR sample rate, retune normalization, adjust speed profile for turns.
• SD card write failures: ensure SD.begin(SD_CS_PIN) uses the correct CS pin and the card is formatted FAT32.

10. File layout

• L1N16R_V3.ino — main firmware file (contains setup, loop, controllers, maze solver)
• README.md — this file

11. License

This project is provided under the MIT License. See LICENSE file.

---

Appendix: Quick optimization checklist

1. Disable or gate Serial logging during runs.
2. Use hardware PWM; increase frequency if necessary.
3. Optimize encoder ISRs (use direct port reads / digitalReadFast).
4. Consider fixed-point arithmetic for PID and ADRC inner loops.
5. Tune motion profiler: reduce corner speeds, maximize straight-line speed.
6. Ensure calibrateSensors() run before timed runs.

---