Arduino IDE 2.x interface layout:

┌──────────────────────────────────────────────────────────────────┐
│  [File] [Edit] [Sketch] [Tools] [Help]          Menubar          │
├──────────────────────────────────────────────────────────────────┤
│  [✓ Verify] [→ Upload] [Debug] [Serial Monitor]   Toolbar       │
│                                                                  │
│  Board: Arduino Uno   Port: /dev/ttyACM0          Status bar    │
├──────┬───────────────────────────────────────────────────────────┤
│      │                                                           │
│ File │  void setup() {                    ←── Code editor       │
│  Ex. │    pinMode(13, OUTPUT);                                   │
│  Lib │  }                                                        │
│      │                                                           │
│      │  void loop() {                                            │
│      │    digitalWrite(13, HIGH);                                │
│      │    delay(1000);                                           │
│      │    digitalWrite(13, LOW);                                 │
│      │    delay(1000);                                           │
│      │  }                                                        │
│      │                                                           │
├──────┴───────────────────────────────────────────────────────────┤
│  Output / Error panel                                            │
│  Sketch uses 924 bytes (2%) of program storage space.           │
│  Global variables use 9 bytes (0%) of dynamic memory.           │
└──────────────────────────────────────────────────────────────────┘

void setup() {
  // Runs ONCE when the board is powered on or reset
  // Use for: pin mode configuration, Serial.begin(), library initialization,
  //           moving servos to starting positions, calibration routines
}

void loop() {
  // Runs CONTINUOUSLY after setup() completes
  // Repeats from top to bottom, then immediately restarts from top
  // Use for: reading sensors, making decisions, controlling actuators,
  //           sending data over Serial, updating display
}

Power on / Reset
      ↓
  [C runtime initialization — global variables set to zero/initial values]
      ↓
  [Arduino framework init — timer setup, interrupt configuration, USB serial]
      ↓
  setup()  ← runs once
      ↓
  ┌── loop()  ← runs
  │     ↓
  │   [return from loop()]
  └─── loop()  ← immediately called again
  ↑         (no delay between loop() calls unless you add one)
  └── forever

// GLOBAL variable — declared outside any function
// Lives for entire program duration (allocated in SRAM at startup)
int globalCounter = 0;     // Accessible from all functions
float lastSensorReading;   // Be careful: 6 floats × 4 bytes = 24 bytes of permanent SRAM

void setup() {
  // LOCAL variable — declared inside a function
  // Created when function is entered, destroyed when it exits
  int setupVar = 42;  // Only visible inside setup()
  // setupVar ceases to exist when setup() returns
}

void loop() {
  // Local variable in loop — re-created every loop iteration
  int loopTemp = analogRead(A0);  // Fresh every iteration — previous value lost

  // STATIC local variable — persists between calls but scoped to function
  static int callCount = 0;  // Initialized once, retains value between loop() calls
  callCount++;
}

Compilation pipeline:

1. PREPROCESSING
   Source: your_sketch.ino
   Tool: avr-gcc preprocessor
   Process: - Expands #include directives (inserts header files)
            - Expands #define macros
            - Removes comments
            - Adds Arduino-specific header: #include <Arduino.h>
            - Generates function prototypes automatically (so you can
              define functions below where they're called)
   Output: preprocessed_source.cpp

2. COMPILATION
   Source: preprocessed_source.cpp + all included .cpp files
   Tool: avr-g++ (C++ compiler for AVR architecture)
   Process: - Parses C++ syntax
            - Checks types, function signatures, scope
            - Optimizes code (removes dead code, inlines small functions)
            - Generates AVR assembly instructions
   Output: object files (.o) for each .cpp file

3. LINKING
   Source: all .o files + Arduino core library .o files
   Tool: avr-ld (linker)
   Process: - Resolves cross-file function references
            - Places code sections in correct memory regions
              (Flash for program code, SRAM for variables)
            - Generates the final executable with exact memory layout
   Output: your_sketch.elf (executable with debug symbols)

4. CONVERSION
   Tool: avr-objcopy
   Process: Strips debug symbols, converts to uploadable format
   Output: your_sketch.hex (Intel HEX format — human-readable hex)

5. UPLOAD
   Tool: avrdude (for classic Arduino boards)
   Process: - Resets the board (via DTR pin toggle on USB)
            - Bootloader on board receives hex over Serial/USB
            - Bootloader writes hex to Flash memory
            - Board resets, new program begins running
   Output: "Done uploading." in IDE status bar

// WRONG: optimizer may keep ledState in a register, never re-reading from SRAM
bool ledState = false;
void myISR() { ledState = !ledState; }

// CORRECT: volatile tells compiler "this variable can change outside normal flow"
volatile bool ledState = false;
void myISR() { ledState = !ledState; }

Serial Monitor flow:

Arduino sketch         USB cable         Serial Monitor
     │                                        │
     │  Serial.println("Sensor: 423")  →      │  displays: "Sensor: 423"
     │                                        │
     │         ← "r\n" (you typed 'r')        │  you type 'r', press Send
     │                                        │
     │  if (Serial.available()) {             │
     │    char c = Serial.read();  // = 'r'   │
     │    // handle 'r' command               │
     │  }                                     │

void setup() {
  Serial.begin(9600);   // 9600 baud — visible in Serial Monitor at 9600 baud
  // OR
  Serial.begin(115200); // 115200 baud — faster, better for high-frequency data
  // The two are NOT interchangeable — match them or you'll see garbage characters
}

// Serial.print() — print without newline
Serial.print("Distance: ");    // Text
Serial.print(42);              // Integer
Serial.print(3.14159, 4);      // Float with 4 decimal places: "3.1416"
Serial.print(0b11010101, BIN); // Binary representation: "11010101"
Serial.print(255, HEX);        // Hexadecimal: "FF"

// Serial.println() — print with newline (carriage return + line feed)
Serial.println("Hello!");      // "Hello!\r\n" — starts new line in monitor

// Serial.print() + Serial.println() pattern for labeled values:
Serial.print("Left: ");        // "Left: "
Serial.print(brightLeft);      // "Left: 423"
Serial.print("  Right: ");     // "Left: 423  Right: "
Serial.println(brightRight);   // "Left: 423  Right: 371\r\n" (new line)

// Efficient pattern for multiple variables on one line:
// CSV format — can be pasted into spreadsheet or plotted
Serial.print(millis());  Serial.print(",");
Serial.print(sensorA);  Serial.print(",");
Serial.print(sensorB);  Serial.print(",");
Serial.println(motorSpeed);
// Output: "1523,423,371,180"

// Reading a single character command
void loop() {
  if (Serial.available() > 0) {
    char cmd = Serial.read();   // Read one byte
    Serial.print(F("Received: "));
    Serial.println(cmd);

    switch (cmd) {
      case 'f': driveForward(150);  break;
      case 's': stopMotors();       break;
      case '1': Serial.println(analogRead(A0)); break;
    }
  }
}

// Reading a complete line (newline-terminated string)
void loop() {
  if (Serial.available() > 0) {
    String line = Serial.readStringUntil('\n');
    line.trim();  // Remove trailing whitespace/CR
    Serial.print(F("Command: "));
    Serial.println(line);
    processCommand(line);
  }
}

// Serial Plotter format: comma-separated values, one set per line
// Labels in the format "Label:Value" are supported in IDE 2.x

void loop() {
  int sensorLeft  = analogRead(A0);
  int sensorRight = analogRead(A1);
  float battVolt  = analogRead(A2) * (5.0 / 1023.0) / 0.319;

  // IDE 2.x labeled format:
  Serial.print("Left:");    Serial.print(sensorLeft);
  Serial.print(",Right:");  Serial.print(sensorRight);
  Serial.print(",Battery:"); Serial.println(battVolt * 10);  // Scale for visibility

  delay(50);  // 20 Hz update rate — faster than display refresh is unnecessary
}

A library installed in ~/Arduino/libraries/ServoLibrary/:

ServoLibrary/
├── src/
│   ├── Servo.h      ← Header: declares classes, functions, constants
│   └── Servo.cpp    ← Implementation: defines the actual code
├── examples/
│   ├── Sweep/
│   │   └── Sweep.ino ← Example sketches showing how to use the library
│   └── Knob/
│       └── Knob.ino
├── library.properties  ← Name, version, author, dependencies
└── keywords.txt        ← Words to highlight in the IDE editor

// Include a library by its header file name
#include <Servo.h>      // Servo library (built into Arduino)
#include <Wire.h>       // I2C communication (built into Arduino)
#include <EEPROM.h>     // EEPROM read/write (built into Arduino)
#include <AccelStepper.h>  // Third-party: install via Library Manager first

// After including: all classes and functions from the library are available
Servo myServo;            // Create a Servo object (defined in Servo.h)
myServo.attach(9);        // Use a method from the Servo class
myServo.write(90);

Built-in Arduino libraries:
  Servo          — Hobby servo PWM control
  Wire           — I2C (two-wire interface) communication
  SPI            — SPI (four-wire interface) communication
  EEPROM         — Read/write microcontroller EEPROM
  SD             — SD card file read/write (requires SD module)
  LiquidCrystal  — HD44780-compatible LCD displays
  Ethernet       — Ethernet shield networking
  WiFi           — WiFi shields (classic; use WiFi libraries for ESP32)
  Stepper        — Basic stepper motor control (AccelStepper is better)
  HardwareSerial — Accessed as Serial, Serial1, etc. (Mega boards)

Error message:
  sketch_name:14:3: error: expected ';' before 'digitalWrite'

Translation:
  In your sketch file, at line 14, column 3:
  The compiler expected a semicolon but found 'digitalWrite'.
  Look at line 13 — the previous statement is missing its semicolon.

Example:
  Line 13: digitalWrite(13, HIGH)   ← missing semicolon here
  Line 14: delay(1000);             ← error reported here (compiler lost context)

Error message:
  sketch_name:23:5: error: 'motorSpeed' was not declared in this scope

Translation:
  At line 23, 'motorSpeed' is used but was never declared with a type.
  Either: you misspelled it (check case: 'motorSpeed' ≠ 'MotorSpeed'),
  or you forgot to declare it (add: int motorSpeed = 0; before use),
  or it's declared inside a different function than where it's used.

Error message:
  sketch_name:31:20: error: invalid conversion from 'float' to 'int'

Translation:
  At line 31, you're trying to assign a float to an int variable (or
  pass a float to a function that expects int) without explicit casting.

Fix:
  int pinNumber = 3.5;          // Error: float → int implicit
  int pinNumber = (int)3.5;     // OK: explicit cast → pinNumber = 3
  int pinNumber = round(3.5);   // OK: round → pinNumber = 4

Error message:
  sketch_name:1:10: fatal error: AccelStepper.h: No such file or directory

Translation:
  The included library header doesn't exist on this computer.
  The library is not installed.

Fix: Open Library Manager, search "AccelStepper", install it.

Error message:
  sketch_name:12:5: error: 'driveForward' was not declared in this scope

Translation:
  The function 'driveForward' is used at line 12 but isn't declared yet.
  In standard C++, functions must be declared before use.
  Arduino's preprocessor auto-generates prototypes for .ino files,
  but sometimes fails for complex function signatures.

Fix: Add a function prototype before setup():
  void driveForward(int speed);  // Prototype

Or: Move the function definition above setup() in the file.

Strategy for compiler error lists:
  1. Read the first error only
  2. Note: filename, line number, column, error text
  3. Go to that line in the editor
  4. Fix the issue
  5. Recompile — many subsequent errors will disappear
  6. Repeat with the new first error

Common board packages to install:

Board                     Package name in Board Manager
─────────────────────────────────────────────────────────────────
Arduino Mega, Nano, Uno   Arduino AVR Boards (included by default)
Arduino Due               Arduino SAM Boards
Arduino Zero, MKR series  Arduino SAMD Boards
ESP32 boards              esp32 by Espressif Systems
ESP8266 boards            esp8266 by ESP8266 Community
RP2040 / Raspberry Pi Pico  Raspberry Pi Pico/RP2040 by Earle Philhower
Teensy                    Teensyduino (separate installer at pjrc.com)

Installing a board package:
  Tools → Board → Boards Manager
  Search for the package name above
  Install (may require adding additional URLs in Preferences first)
  
  Example: ESP32 requires adding this URL to Additional Boards Manager URLs:
  https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_index.json

Port naming conventions:

Windows:
  COM1–COM256 (e.g., COM3, COM7)
  Arduino Uno typically appears as COM3–COM10
  Check Device Manager → Ports (COM & LPT) if unsure which port

macOS:
  /dev/cu.usbmodem... (e.g., /dev/cu.usbmodem14201)
  /dev/cu.SLAB_USBtoUART (for CP2102-based boards like some ESP32)
  Multiple ports may appear — choose the one with "usbmodem" or the board name

Linux:
  /dev/ttyACM0 (Arduino Uno, Mega — CDC ACM USB serial)
  /dev/ttyUSB0 (ESP32, NodeMCU, boards with CH340 USB-serial chip)
  If port not visible: add user to 'dialout' group:
    sudo usermod -aG dialout $USER  (then log out and back in)

General rule: unplug the board, check available ports, plug it back in,
  see which new port appears — that's your board's port.

Feature comparison:

Feature                    IDE 1.8.x          IDE 2.x
────────────────────────────────────────────────────────────────────────
Interface framework        Java/Processing    Electron (web-based)
Startup time               ~3–5 sec           ~5–10 sec (heavier)
Autocompletion             None               Yes — function/method suggestions
Error highlighting         After compile      Real-time (underlines errors as you type)
Compilation cache          Limited            Full incremental caching (much faster)
Hardware debugger          No                 Yes (supported boards only)
Serial Monitor style       Separate window    Integrated panel at bottom
Serial Plotter             Separate window    Integrated panel
Board/Library managers     Separate windows   Integrated sidebar panels
Dark mode                  Limited            Full dark mode support
Multiple sketches          Multiple windows   Tabbed interface
Offline library docs       No                 Limited
Resource usage             Low (~200MB RAM)   Higher (~400–600MB RAM)

Recommendation:
  IDE 2.x for all new users — better experience, same hardware compatibility
  IDE 1.8.x if running on a very old computer (< 4GB RAM)

Essential Arduino IDE keyboard shortcuts:

Action                     Windows/Linux        macOS
─────────────────────────────────────────────────────────────────
Verify (compile)           Ctrl+R              Cmd+R
Upload                     Ctrl+U              Cmd+U
Open Serial Monitor        Ctrl+Shift+M        Cmd+Shift+M
Auto-format code           Ctrl+T              Cmd+T
Find and Replace           Ctrl+H              Cmd+H
Comment/uncomment line     Ctrl+/              Cmd+/
New sketch                 Ctrl+N              Cmd+N
Open sketch                Ctrl+O              Cmd+O
Save                       Ctrl+S              Cmd+S
Increase font size         Ctrl++              Cmd++
Go to line                 Ctrl+G              Cmd+G

Sketch folder structure with multiple files:

my_robot_sketch/
├── my_robot_sketch.ino   ← Main file: setup(), loop()
├── motor_control.ino     ← Motor functions: driveForward(), turnLeft(), etc.
├── sensor_reading.ino    ← Sensor functions: measureDistance(), readBrightness()
└── pid_control.ino       ← PID controller: computeCorrection(), resetPID()

// PIDController.h — header file: declares the class interface

#ifndef PID_CONTROLLER_H   // Include guard: prevents double inclusion
#define PID_CONTROLLER_H

class PIDController {
public:
  PIDController(float kp, float ki, float kd);
  void reset();
  float compute(float error, float dt);

private:
  float _kp, _ki, _kd;
  float _integral;
  float _lastError;
};

#endif

// PIDController.cpp — source file: implements the class

#include "PIDController.h"

PIDController::PIDController(float kp, float ki, float kd)
  : _kp(kp), _ki(ki), _kd(kd), _integral(0), _lastError(0) {}

void PIDController::reset() {
  _integral = 0;
  _lastError = 0;
}

float PIDController::compute(float error, float dt) {
  _integral += error * dt;
  _integral = constrain(_integral, -500, 500);
  float derivative = (error - _lastError) / dt;
  _lastError = error;
  return _kp * error + _ki * _integral + _kd * derivative;
}

// In your main .ino file:
#include "PIDController.h"

PIDController linePID(0.25, 0.001, 0.8);

void loop() {
  float error = computePosition();
  float correction = linePID.compute(error, dt);
  // apply correction to motors...
}

Key example categories to explore:

01. Basics
  └── Blink        — The simplest possible sketch (LED on/off)
  └── AnalogRead   — Reading a potentiometer (the ADC in practice)
  └── DigitalRead  — Reading a button
  └── Fade         — PWM LED dimming with analogWrite()

02. Digital
  └── Debounce     — Button debouncing with millis() (covered in article 66)
  └── StateChange  — Detecting button press events (not just state)

03. Analog
  └── Smoothing    — Running average for noisy sensor readings

04. Communication
  └── SerialEvent  — Event-driven serial reading (not blocking)
  └── Graph        — Serial Plotter usage

05. Sensors
  └── Various ultrasonic, light, temperature examples

Library examples (appear under Examples → Library Name):
  Servo → Sweep    — Basic servo position control
  Servo → Knob     — Potentiometer-controlled servo (article 78 basis)
  Wire  → master_reader — I2C communication between two Arduinos
  EEPROM → eeprom_read   — Reading/writing EEPROM (article 72 basis)

void loop() {
  float distance = measureDistance();
  Serial.print(F("Distance: ")); Serial.println(distance, 1);

  if (distance < 20.0) {
    Serial.println(F("Would avoid obstacle"));
    // driveBackward(150);  ← commented out during testing
    // delay(500);
  } else {
    Serial.println(F("Would drive forward"));
    // driveForward(150);   ← commented out during testing
  }
  delay(200);
}

// Read three samples and average to reduce HC-SR04 noise
// Each measureDistance() call takes ~30ms (limited by 60ms sensor cycle / 2)
float getSmoothedDistance() {
  long sum = 0;
  for (int i = 0; i < 3; i++) {
    sum += measureDistance();
    delay(30);  // 30ms between reads — prevents echo interference
  }
  return sum / 3.0;
}

my_rover_v1.ino          ← working basic version
my_rover_v2_pid.ino      ← PID control added (may break things)
my_rover_v3_sensors.ino  ← three sensors added

Pen holder cross-section:

           [Pen lift servo arm]
                    │
            ┌───────┴───────┐
            │  Pen holder   │  ← small tube or bracket matching pen diameter
            │               │
            │   [PEN]       │  ← pen can slide up/down ~10mm
            └───────────────┘
                    │
               Paper surface

Pen lifted: servo rotates arm, pushing pen holder upward → pen clears paper
Pen down:   servo rotates back, allowing pen weight to contact paper
            (gravity provides gentle, consistent contact pressure)

#include <Servo.h>

Servo penLiftServo;
const int PEN_LIFT_PIN = 6;
const int PEN_UP   = 60;   // Servo angle when pen is lifted (adjust for your mechanism)
const int PEN_DOWN = 30;   // Servo angle when pen contacts paper

void penUp() {
  penLiftServo.write(PEN_UP);
  delay(150);  // Allow time for servo to fully lift pen before moving arm
}

void penDown() {
  penLiftServo.write(PEN_DOWN);
  delay(150);  // Allow time for pen to settle before drawing
}

Arm plotter coordinate system (top view):

              y
              │
              │
  ────────────┼────────────  x
              │ BASE
              │

A point on the paper at Cartesian (x, y) from the arm base:
  Polar distance: r = sqrt(x² + y²)
  Polar angle:    θ = atan2(y, x)   [in radians, use atan2f() in C]

For the arm to position its end effector at this point:
  Base servo J1: rotate to θ (converted from radians to servo degrees)
  Shoulder + elbow: configure to achieve reach r at the correct height z

Given target point (x, y) on paper:

Step 1: Convert to polar
  r = sqrt(x² + y²)
  theta_base = atan2f(y, x)  → base servo angle

Step 2: 2-link inverse kinematics for planar reach r at height z_draw
  Using the law of cosines:
  
  d = sqrt(r² + z_draw²)   [3D distance from shoulder joint to target]
  
  cos(elbow_angle) = (d² - L1² - L2²) / (2 × L1 × L2)
  elbow_angle = acos(result)    [in radians; convert to degrees for servo]

  alpha = atan2f(z_draw, r)    [angle of line from shoulder to target]
  beta  = acos((d² + L1² - L2²) / (2 × d × L1))
  shoulder_angle = alpha + beta  [or alpha - beta for elbow-down configuration]

Step 3: Convert radians to degrees, apply servo offset corrections
  servo_base     = degrees(theta_base) + BASE_OFFSET
  servo_shoulder = degrees(shoulder_angle) + SHOULDER_OFFSET
  servo_elbow    = degrees(elbow_angle) + ELBOW_OFFSET

#include <math.h>

// Link lengths in cm
const float L1 = 15.0;  // Shoulder to elbow
const float L2 = 12.0;  // Elbow to pen tip

// Drawing surface height below shoulder joint (adjust for your setup)
const float Z_DRAW = -10.0;  // Pen is 10cm below shoulder when drawing

// Servo offsets (calibrate these to your physical arm assembly)
const int BASE_OFFSET     =  90;  // servo.write(90) = arm facing forward
const int SHOULDER_OFFSET =   0;  // adjust until shoulder horizontal = 90°
const int ELBOW_OFFSET    =   0;

struct JointAngles {
  float base;
  float shoulder;
  float elbow;
  bool reachable;
};

JointAngles inverseKinematics(float x_cm, float y_cm) {
  JointAngles result;
  result.reachable = true;

  // Step 1: Base angle from Cartesian
  result.base = atan2f(y_cm, x_cm) * 180.0 / PI + BASE_OFFSET;

  // Step 2: Planar distance from base
  float r = sqrtf(x_cm * x_cm + y_cm * y_cm);
  float d = sqrtf(r * r + Z_DRAW * Z_DRAW);  // 3D reach from shoulder

  // Check reachability
  if (d > L1 + L2 || d < fabsf(L1 - L2)) {
    result.reachable = false;
    return result;
  }

  // Elbow angle (law of cosines)
  float cosElbow = (d * d - L1 * L1 - L2 * L2) / (2.0 * L1 * L2);
  cosElbow = constrain(cosElbow, -1.0, 1.0);  // Clamp for floating-point errors
  float elbowRad = acosf(cosElbow);

  // Shoulder angle
  float alpha = atan2f(-Z_DRAW, r);  // Negative: pen is below shoulder
  float beta  = acosf((d * d + L1 * L1 - L2 * L2) / (2.0 * d * L1));
  float shoulderRad = alpha + beta;

  result.shoulder = shoulderRad * 180.0 / PI + SHOULDER_OFFSET;
  result.elbow    = elbowRad    * 180.0 / PI + ELBOW_OFFSET;

  return result;
}

Cartesian plotter layout:

    ┌──────────────────────────────────────────┐
    │  Y motor → moves pen carriage along Y axis│
    │                                           │
    │  Carriage ──────────────────────── Guide  │
    │      │                                    │
    │      X motor → moves pen along X axis     │
    │      │                                    │
    │   [PEN]                                   │
    │                                           │
    │         DRAWING SURFACE (paper)           │
    └──────────────────────────────────────────┘

X axis: pen moves left/right (one stepper motor + belt or lead screw)
Y axis: carriage moves forward/backward (second stepper motor)
Z axis: pen up/down (servo motor)

This is identical to a desktop 3D printer without the extruder —
the mechanics are the same, only the tool at the end differs.

// Cartesian plotter using AccelStepper library
// Requires: AccelStepper library (install via Arduino IDE Library Manager)

#include <AccelStepper.h>
#include <Servo.h>

// Stepper setup (4-wire stepper with A4988 driver)
// A4988 STEP and DIR pins:
AccelStepper stepperX(AccelStepper::DRIVER, 2, 3);  // STEP=2, DIR=3
AccelStepper stepperY(AccelStepper::DRIVER, 4, 5);  // STEP=4, DIR=5

Servo penServo;
const int PEN_UP   = 70;
const int PEN_DOWN = 40;

// Steps per mm (calibrate for your specific belt/pulley)
// Typical GT2 belt, 20-tooth pulley, A4988 at 1/16 microstepping:
//   Steps per mm = (200 × 16) / (20 × 2) = 80 steps/mm
const float STEPS_PER_MM = 80.0;

void setup() {
  Serial.begin(9600);
  penServo.attach(6);
  penServo.write(PEN_UP);

  // Configure stepper speeds
  stepperX.setMaxSpeed(3000);      // steps/second
  stepperX.setAcceleration(2000);  // steps/second²
  stepperY.setMaxSpeed(3000);
  stepperY.setAcceleration(2000);

  // Home position
  stepperX.setCurrentPosition(0);
  stepperY.setCurrentPosition(0);

  Serial.println(F("Cartesian plotter ready."));
}

void moveTo_mm(float x_mm, float y_mm) {
  long x_steps = (long)(x_mm * STEPS_PER_MM);
  long y_steps = (long)(y_mm * STEPS_PER_MM);

  stepperX.moveTo(x_steps);
  stepperY.moveTo(y_steps);

  // Run both steppers simultaneously until both reach target
  while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) {
    stepperX.run();
    stepperY.run();
  }
}

void penUp()   { penServo.write(PEN_UP);   delay(150); }
void penDown() { penServo.write(PEN_DOWN); delay(150); }

// Draw a line from current position to (x_mm, y_mm) with pen down
void lineTo(float x_mm, float y_mm) {
  moveTo_mm(x_mm, y_mm);
}

// Move without drawing from current position to (x_mm, y_mm)
void moveTo(float x_mm, float y_mm) {
  penUp();
  moveTo_mm(x_mm, y_mm);
  penDown();
}

// All coordinates in mm from home position (0,0)

void drawLine(float x0, float y0, float x1, float y1) {
  moveTo(x0, y0);   // Lift pen, move to start
  lineTo(x1, y1);   // Draw to end
}

void drawRectangle(float x, float y, float w, float h) {
  moveTo(x, y);         // Move to top-left corner
  lineTo(x + w, y);     // Top edge
  lineTo(x + w, y + h); // Right edge
  lineTo(x, y + h);     // Bottom edge
  lineTo(x, y);         // Left edge (close)
  penUp();
}

void drawCircle(float cx, float cy, float r, int segments = 36) {
  // Approximate circle with many short line segments
  float angleStep = 2.0 * PI / segments;

  // Move to start point (pen up)
  moveTo(cx + r, cy);  // Start at rightmost point

  // Draw segments
  for (int i = 1; i <= segments; i++) {
    float angle = i * angleStep;
    float x = cx + r * cosf(angle);
    float y = cy + r * sinf(angle);
    lineTo(x, y);
  }
  penUp();
}

void drawPolygon(float cx, float cy, float r, int sides) {
  float angleStep = 2.0 * PI / sides;
  float startAngle = -PI / 2.0;  // Start at top

  // Move to first vertex
  moveTo(cx + r * cosf(startAngle), cy + r * sinf(startAngle));

  for (int i = 1; i <= sides; i++) {
    float angle = startAngle + i * angleStep;
    lineTo(cx + r * cosf(angle), cy + r * sinf(angle));
  }
  penUp();
}

// Epitrochoid (spirograph-style curve)
// Produces complex looping patterns from two radii and an offset parameter
// Classic spirograph equation: parametric form

void drawEpitrochoid(float cx, float cy,
                     float R, float r, float d,
                     int steps = 360) {
  // Parameters:
  //   R = radius of fixed circle
  //   r = radius of rolling circle
  //   d = distance from center of rolling circle to pen
  // Classic spirograph values to try:
  //   R=70, r=30, d=50 → 3-petal flower
  //   R=70, r=10, d=60 → star with 7 points
  //   R=60, r=25, d=40 → complex looping rose

  float tMax = 2.0 * PI * (r / gcd_approx(R, r));  // Full period
  float tStep = tMax / steps;

  float x0 = cx + (R + r) * cosf(0) - d * cosf(0);
  float y0 = cy + (R + r) * sinf(0) - d * sinf(0);
  moveTo(x0, y0);

  for (int i = 1; i <= steps; i++) {
    float t = i * tStep;
    float x = cx + (R + r) * cosf(t) - d * cosf((R + r) / r * t);
    float y = cy + (R + r) * sinf(t) - d * sinf((R + r) / r * t);
    lineTo(x, y);
  }
  penUp();
}

// Helper: approximate GCD for computing epitrochoid period
float gcd_approx(float a, float b) {
  while (b > 0.001) {
    float temp = b;
    b = fmod(a, b);
    a = temp;
  }
  return a;
}

// Lissajous curve: two sinusoids at different frequencies
void drawLissajous(float cx, float cy, float Ax, float Ay,
                   float fx, float fy, float phase,
                   int steps = 300) {
  // Try: Ax=50, Ay=50, fx=3, fy=2, phase=PI/4 → classic 3:2 Lissajous

  float x0 = cx + Ax * sinf(0);
  float y0 = cy + Ay * sinf(phase);
  moveTo(x0, y0);

  for (int i = 1; i <= steps; i++) {
    float t = (float)i / steps * 2.0 * PI;
    float x = cx + Ax * sinf(fx * t);
    float y = cy + Ay * sinf(fy * t + phase);
    lineTo(x, y);
  }
  penUp();
}

// Polar rose: r = cos(n×θ)
void drawRose(float cx, float cy, float radius, int n, int steps = 360) {
  // n petals if n is odd, 2n petals if n is even
  // Try: n=3 (3-petal rose), n=5 (5-petal), n=4 (8-petal)

  bool firstPoint = true;
  for (int i = 0; i <= steps; i++) {
    float theta = (float)i / steps * 2.0 * PI;
    float r = radius * cosf(n * theta);
    float x = cx + r * cosf(theta);
    float y = cy + r * sinf(theta);
    if (firstPoint) {
      moveTo(x, y);
      firstPoint = false;
    } else {
      lineTo(x, y);
    }
  }
  penUp();
}

void setup() {
  // ... servo/stepper initialization ...
  penUp();
  delay(1000);

  // Center of paper (assuming 150×150mm drawing area)
  float cx = 75, cy = 75;

  // Draw a series of nested polygons from 3 sides to 9 sides
  for (int sides = 3; sides <= 9; sides++) {
    float radius = (sides - 2) * 10.0;  // Increasing radius
    drawPolygon(cx, cy, radius, sides);
    delay(200);
  }

  // Draw a spirograph in the center
  drawEpitrochoid(cx, cy, 40, 15, 30, 180);

  // Draw a circle framing the composition
  drawCircle(cx, cy, 70, 72);  // 72 segments → very smooth

  // Return home
  moveTo(0, 0);

  Serial.println(F("Drawing complete!"));
}

G-code subset for plotters:

G0 X{x} Y{y}        → Rapid move (pen up) to (x, y) in mm
G1 X{x} Y{y} F{f}   → Linear move (pen down) to (x, y) at feed rate f
G28                  → Home all axes (return to origin)
M3 S255              → Pen down (some plotters use spindle commands for pen)
M5                   → Pen up

Example G-code for drawing a 50×50mm square:
  G28
  M5          ; pen up
  G0 X0 Y0    ; move to origin
  M3 S255     ; pen down
  G1 X50 Y0   ; right 50mm
  G1 X50 Y50  ; up 50mm
  G1 X0 Y50   ; left 50mm
  G1 X0 Y0    ; down 50mm
  M5          ; pen up

// Minimal G-code interpreter over Serial
// Upload sketch, connect via Serial Monitor at 115200 baud
// Send G-code commands line by line

float currentX = 0, currentY = 0;
bool penIsDown = false;

void parseGCode(String line) {
  line.trim();
  if (line.startsWith(F(";"))) return;   // Comment
  if (line.length() == 0)    return;   // Empty line

  // Parse G0 / G1 commands
  if (line.startsWith(F("G0")) || line.startsWith(F("G1"))) {
    float x = currentX, y = currentY;

    // Extract X value
    int xIdx = line.indexOf('X');
    if (xIdx >= 0) x = line.substring(xIdx + 1).toFloat();

    // Extract Y value
    int yIdx = line.indexOf('Y');
    if (yIdx >= 0) y = line.substring(yIdx + 1).toFloat();

    if (line.startsWith(F("G0"))) {
      // Rapid move: pen up first
      if (penIsDown) { penUp(); penIsDown = false; }
      moveTo_mm(x, y);
    } else {
      // Cutting move: pen must be down
      if (!penIsDown) { penDown(); penIsDown = true; }
      moveTo_mm(x, y);
    }

    currentX = x;
    currentY = y;
    Serial.println(F("ok"));

  } else if (line.startsWith(F("G28"))) {
    if (penIsDown) { penUp(); penIsDown = false; }
    moveTo_mm(0, 0);
    currentX = 0; currentY = 0;
    Serial.println(F("ok"));

  } else if (line.startsWith(F("M3"))) {
    penDown(); penIsDown = true;
    Serial.println(F("ok"));

  } else if (line.startsWith(F("M5"))) {
    penUp(); penIsDown = false;
    Serial.println(F("ok"));

  } else {
    Serial.print(F("unknown: "));
    Serial.println(line);
  }
}

void loop() {
  if (Serial.available()) {
    String line = Serial.readStringUntil('\n');
    parseGCode(line);
  }
}

// Calibration: command 100mm, measure actual distance
// Actual 100mm, commanded 100mm → steps_per_mm is correct
// Actual 96mm, commanded 100mm → robot is moving too little
//   New STEPS_PER_MM = old × (100 / 96) = old × 1.0417

float calibrateStepsPerMm(float commandedMm, float measuredMm) {
  return STEPS_PER_MM * (commandedMm / measuredMm);
}

// Calibration grid: draw a grid of points, measure their actual positions
void drawCalibrationGrid() {
  for (float x = -10; x <= 10; x += 5) {
    for (float y =  10; y <= 25; y += 5) {
      JointAngles angles = inverseKinematics(x, y);
      if (angles.reachable) {
        baseServo.write(angles.base);
        shoulderServo.write(angles.shoulder);
        elbowServo.write(angles.elbow);
        delay(500);
        penDown();
        delay(200);  // Mark dot
        penUp();
      }
    }
  }
}
// Measure the printed dots, note systematic offset or distortion,
// adjust link length constants and servo offsets accordingly

// Simple nearest-neighbor path optimization
// Input: array of line segments (start, end points)
// Output: reordered segments minimizing total pen-up distance

struct Segment {
  float x0, y0;  // Start point
  float x1, y1;  // End point
};

float distance(float x0, float y0, float x1, float y1) {
  return sqrtf((x1-x0)*(x1-x0) + (y1-y0)*(y1-y0));
}

void optimizePath(Segment segments[], int n) {
  float currentX = 0, currentY = 0;

  for (int i = 0; i < n; i++) {
    // Find nearest undrawn segment start (or end — segments can be reversed)
    int nearest = i;
    float minDist = 1e9;
    bool reversed = false;

    for (int j = i; j < n; j++) {
      float distToStart = distance(currentX, currentY, segments[j].x0, segments[j].y0);
      float distToEnd   = distance(currentX, currentY, segments[j].x1, segments[j].y1);

      if (distToStart < minDist) { minDist = distToStart; nearest = j; reversed = false; }
      if (distToEnd   < minDist) { minDist = distToEnd;   nearest = j; reversed = true;  }
    }

    // Swap nearest segment into position i
    Segment temp = segments[i];
    segments[i] = segments[nearest];
    segments[nearest] = temp;

    // Reverse if approaching from the end
    if (reversed) {
      float tx = segments[i].x0; segments[i].x0 = segments[i].x1; segments[i].x1 = tx;
      float ty = segments[i].y0; segments[i].y0 = segments[i].y1; segments[i].y1 = ty;
    }

    currentX = segments[i].x1;
    currentY = segments[i].y1;
  }
}

Backlash effect on a square corner:

Intended:         Actual (with backlash):
    ┌──            ┌──
    │              │ ← slight overrun before
    └──            └────── reversal takes effect

Measurement: command a 10mm move, reverse 5mm, forward 5mm.
  Net should be 10mm from start.
  If less: backlash measured = 10mm - actual distance.

Compensation:
  When reversing direction: move an extra BACKLASH_COMP steps before
  counting position, to take up the gear play.

// Backlash compensation for Cartesian plotter
const float BACKLASH_X_MM = 0.3;  // Measure for your machine
const float BACKLASH_Y_MM = 0.2;

int lastDirectionX = 1;  // +1 or -1
int lastDirectionY = 1;

void moveTo_mm_compensated(float x_mm, float y_mm) {
  // Determine movement direction
  int dirX = (x_mm > currentX) ? 1 : -1;
  int dirY = (y_mm > currentY) ? 1 : -1;

  // Apply backlash compensation on direction reversal
  if (dirX != lastDirectionX && x_mm != currentX) {
    long comp = (long)(BACKLASH_X_MM * STEPS_PER_MM * dirX);
    stepperX.move(comp);  // Take up backlash
    while (stepperX.distanceToGo() != 0) stepperX.run();
  }
  if (dirY != lastDirectionY && y_mm != currentY) {
    long comp = (long)(BACKLASH_Y_MM * STEPS_PER_MM * dirY);
    stepperY.move(comp);
    while (stepperY.distanceToGo() != 0) stepperY.run();
  }

  lastDirectionX = dirX;
  lastDirectionY = dirY;

  // Now move to target
  moveTo_mm(x_mm, y_mm);
}

Diagnosing step loss:
1. Draw a vertical line from y=0 to y=100mm
2. Return home (G28 / moveTo(0,0))
3. Draw a second vertical line next to the first
4. The two lines should be perfectly parallel
5. If the second line is offset: steps were lost during the first pass

Fix:
  Reduce maximum speed (setMaxSpeed())
  Reduce acceleration (setAcceleration())
  Increase motor current (A4988 vref trimmer — carefully, too high causes overheating)
  Add cooling to motor driver if drawing many complex pieces back-to-back

// Logical coordinate offset: add to all coordinates before sending to machine
const float ORIGIN_X = 20.0;  // mm offset from home
const float ORIGIN_Y = 20.0;

void drawAt(float logical_x, float logical_y) {
  moveTo_mm(ORIGIN_X + logical_x, ORIGIN_Y + logical_y);
}

void lineAt(float logical_x, float logical_y) {
  lineTo_mm(ORIGIN_X + logical_x, ORIGIN_Y + logical_y);
}

// Now all drawing code uses logical 0,0 as the bottom-left of the drawing area,
// and the machine adds the offset to keep it in the accurate zone.

DOF and reachable workspace:

1-DOF arm (single joint):
  ──[BASE]──[LINK]──[END]
             ↻ one joint
  Traces an arc in a single plane.
  Use: simple pendulum mechanisms, single-axis scanners.

2-DOF arm (two joints):
  ──[BASE]──[LINK1]──[LINK2]──[END]
              ↻ J1      ↻ J2
  Reaches any point within a 2D disk (the combined reach of both links).
  Use: SCARA robots (horizontal plane work), simple drawing arms.

3-DOF arm (three joints):
  ──[BASE]──[LINK1]──[LINK2]──[LINK3]──[END]
              ↻ J1      ↻ J2     ↻ J3
  With a vertical base rotation: reaches points in a 3D hemisphere.
  Use: most hobby arms, simple pick-and-place, desktop manipulators.

6-DOF arm (six joints):
  The minimum for full 3D position AND orientation control.
  Can place end effector at any point with any orientation in workspace.
  Use: industrial welding/assembly robots, surgical robots, drone arms.

More DOF = more flexibility, more complexity, more control challenges.
A 7-DOF arm is "redundant" (like a human arm) — can reach same
point in infinite configurations, enabling obstacle avoidance in joint space.

Singularity example (3-DOF planar arm):

At full extension (all links in a straight line):
  ──[BASE]──[LINK1]──[LINK2]──[LINK3]──[END]

The end effector is at maximum reach. Any small motion
of the endpoint would require extremely fast joint motion
or is geometrically impossible with these joint angles.
Moving the endpoint sideways (perpendicular to the arm) requires
the innermost joint to move while the outer joints compensate —
at full extension, the Jacobian matrix (relating joint velocities
to endpoint velocity) becomes singular (non-invertible).

Practical result: don't design arm tasks that bring the arm
near full extension, and avoid the "dead straight" configuration.
Build with 20% workspace margin: if the arm can reach 30cm,
only use the 24cm radius working zone.

Revolute joint:

   LINK_IN ──────[JOINT]
                    ↓ axis of rotation (into page)
                 [JOINT]──────LINK_OUT

The output link rotates relative to the input link.
The axis of rotation doesn't move — only the angle changes.
Servo horn, gear, or direct shaft drive implements this.

Example arm configurations:
  RRR: Three revolute joints → standard 3-DOF hobby arm
  RRP: Two revolute + one prismatic → SCARA robot variant
  RRRRRR: Six revolute joints → standard 6-DOF industrial arm

Servo motor PWM control protocol:

Signal: 50Hz PWM (20ms period)
  Pulse width 1000µs (1ms) → servo moves to 0°
  Pulse width 1500µs (1.5ms) → servo moves to 90° (center)
  Pulse width 2000µs (2ms) → servo moves to 180°

Intermediate pulse widths → intermediate angles (linear interpolation)

Arduino: servo.write(angle) handles this automatically
  servo.write(0)   → 1000µs pulse → 0°
  servo.write(90)  → 1500µs pulse → 90°
  servo.write(180) → 2000µs pulse → 180°

Servo size comparison:

Micro servo (SG90, ~3.5g):
  Torque: 1.8 kg·cm (at 4.8V) ≈ 0.18 N·m
  Speed: 0.1 sec/60° at 4.8V
  Current: 100–200mA stall
  Best for: end effector (gripper), wrist joint (light payload)
  Cost: ~$1–3

Standard servo (MG996R, ~55g):
  Torque: 9.4 kg·cm (at 4.8V) ≈ 0.92 N·m
  Speed: 0.19 sec/60° at 4.8V
  Current: 500–900mA stall
  Best for: shoulder and elbow joints (medium payload)
  Cost: ~$5–12

High-torque servo (DS3225, ~60g):
  Torque: 25 kg·cm (at 6V) ≈ 2.45 N·m
  Speed: 0.15 sec/60° at 6V
  Current: up to 2A stall
  Best for: base rotation with heavy arm, heavy-payload shoulder joints
  Cost: ~$15–25

Torque calculation for a 3-DOF arm:

Arm parameters:
  Link 1 (shoulder to elbow): 15cm long, 80g weight, acts at 7.5cm from shoulder
  Link 2 (elbow to wrist):   12cm long, 50g weight, acts at 6cm from elbow
  Link 3 (wrist to tip):      8cm long, 30g weight, acts at 4cm from wrist
  Payload at tip:             50g

Wrist joint torque (J3) — supports only Link 3 + payload:
  T_wrist = (30g × 4cm) + (50g × 8cm)
           = 120 + 400 = 520 g·cm ≈ 5.2 kg·cm at worst case (horizontal)
  → SG90 (1.8 kg·cm) is insufficient. Use MG996R (9.4 kg·cm) with margin.

Elbow joint torque (J2) — supports Link 2 + Link 3 + payload:
  T_elbow = (50g × 6cm) + (30g × 12+4cm) + (50g × 12+8cm)
           = 300 + 480 + 1000 = 1780 g·cm ≈ 17.8 kg·cm
  → MG996R (9.4 kg·cm) is insufficient at full extension!
  Use DS3225 (25 kg·cm) for elbow, or redesign for shorter links.

Shoulder joint torque (J1) — supports entire arm + payload:
  T_shoulder = all link weights × their distances from shoulder
             = even larger than elbow
  → Definitely requires high-torque servo or counterweighting.

Rule of thumb: use servos rated at 2-3× the calculated requirement.
  Calculations assume worst-case (horizontal arm, full extension).
  Add safety factor for acceleration, dynamic loads, and motor aging.

3-DOF arm configuration (side view):

                         J3 (elbow)
                         ↓
              J2 ────────────────── End effector
            (shoulder)  LINK2
              │
            LINK1
              │
            [BASE] ── J1 (base rotation, looking from above)

Material comparison for arm links:

Foam board (5mm craft foam + paper):
  Weight: ~5g per 10cm link
  Stiffness: low — deflects under load
  Cost: < $1
  Best for: ultra-light test arms, prototyping

3D-printed PLA:
  Weight: ~20–40g per 10cm link depending on infill
  Stiffness: moderate-high
  Cost: ~$0.50–2 in filament
  Best for: custom-shaped links, hub/joint integration
  Notes: design for the servo horn bolt pattern

Laser-cut acrylic (3mm):
  Weight: ~30–50g per 10cm link
  Stiffness: high; brittle under impact
  Cost: ~$1–3 per link depending on size
  Best for: flat planar arms, precise dimensions needed

Aluminum extrusion (15×15 or 20×20):
  Weight: significant per length
  Stiffness: excellent
  Cost: ~$3–8 per link
  Best for: heavy-payload arms, professional builds

Pre-made MeArm-style kits:
  Acrylic sheet laser cut kit, includes all hardware
  Cost: $15–30 complete kit
  Best for: beginners wanting a reliable starting point

Base servo mounting:

  ┌──────────────────────┐
  │    Base plate        │
  │  ┌────────────────┐  │
  │  │  J1 Servo      │  │
  │  │  (horizontal)  │  │
  │  └────────────────┘  │
  │      ↑ servo horn    │
  │      connects to     │
  │      vertical upright │
  └──────────────────────┘

The servo is fixed to the base. The arm rotates above it.
Mount with M3 screws through servo flange holes.

Wiring for 3-servo arm:

Arduino Pin 9  ──── Servo J1 signal (orange/yellow wire)
Arduino Pin 10 ──── Servo J2 signal
Arduino Pin 11 ──── Servo J3 signal

5V power rail ──┬── Servo J1 VCC (red wire)
                ├── Servo J2 VCC
                └── Servo J3 VCC

GND rail ───────┬── Servo J1 GND (black/brown wire)
                ├── Servo J2 GND
                └── Servo J3 GND

CRITICAL: Do NOT power servos from the Arduino's 5V pin.
At stall, each servo draws 500–900mA. Three servos stalling simultaneously
can draw 1.5–2.7A — far exceeding the Arduino's 500mA USB limit.

Use a separate 5V supply for servos:
  Option 1: 4× AA batteries (6V) → direct to servo power
  Option 2: Buck converter (12V → 5V/3A) from main robot battery
  Option 3: USB power bank with sufficient output current (≥ 2A)

Connect servo GND to Arduino GND (common ground — essential for signal reference).
Do NOT connect servo VCC to Arduino 5V.

#include <Servo.h>

Servo base;     // J1 — base rotation
Servo shoulder; // J2 — shoulder pitch
Servo elbow;    // J3 — elbow pitch

void setup() {
  base.attach(9);       // Attach servo to pin 9
  shoulder.attach(10);
  elbow.attach(11);

  // Move to home position (all servos at 90° — arm pointing straight up)
  moveToHome();
  delay(1000);  // Wait for arm to reach home before any other motion
}

void moveToHome() {
  base.write(90);      // Center: arm faces forward
  shoulder.write(90);  // Level: arm horizontal (adjust based on your arm geometry)
  elbow.write(90);     // Level forearm
}

2-DOF planar arm geometry:

     J2 ───────────── end effector
    /  \
L2 /    angle θ2
  /
J1 ─────── (fixed base, angle θ1 from horizontal)
 \
  L1

End effector position (x, y) from joint 1:
  x = L1 × cos(θ1) + L2 × cos(θ1 + θ2)
  y = L1 × sin(θ1) + L2 × sin(θ1 + θ2)

Where:
  L1, L2 = link lengths (in consistent units, e.g., cm)
  θ1 = angle of link 1 from horizontal (shoulder angle)
  θ2 = angle of link 2 relative to link 1 (elbow angle)

// Forward kinematics for 2-DOF planar arm (ignoring base rotation)
const float L1 = 15.0;  // cm, link 1 length (shoulder to elbow)
const float L2 = 12.0;  // cm, link 2 length (elbow to end effector)

void computePosition(float theta1_deg, float theta2_deg,
                     float &x_out, float &y_out) {
  float theta1 = theta1_deg * PI / 180.0;  // Convert degrees to radians
  float theta2 = theta2_deg * PI / 180.0;

  // Forward kinematics equations for planar 2-DOF arm
  x_out = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
  y_out = L1 * sin(theta1) + L2 * sin(theta1 + theta2);
}

// Usage: print end effector position for current servo angles
void reportPosition() {
  float x, y;
  computePosition(shoulder.read(), elbow.read(), x, y);
  Serial.print(F("End effector: x="));
  Serial.print(x, 1);
  Serial.print(F("cm, y="));
  Serial.print(y, 1);
  Serial.println(F("cm"));
}

3-DOF arm position computation:

Step 1: compute radial reach (r) and height (z) from the planar arm:
  r = L1 × cos(θ_shoulder) + L2 × cos(θ_shoulder + θ_elbow)
  z = L1 × sin(θ_shoulder) + L2 × sin(θ_shoulder + θ_elbow)

Step 2: base rotation (θ_base) sweeps r in the horizontal plane:
  x = r × cos(θ_base)
  y = r × sin(θ_base)
  z = z (height unchanged by base rotation)

So the full 3D end effector position is:
  x = (L1×cos(θ_s) + L2×cos(θ_s+θ_e)) × cos(θ_b)
  y = (L1×cos(θ_s) + L2×cos(θ_s+θ_e)) × sin(θ_b)
  z =  L1×sin(θ_s) + L2×sin(θ_s+θ_e)

Where θ_b=base, θ_s=shoulder, θ_e=elbow angle (all in radians)

/*
 * 3-DOF Robot Arm Control
 * Joystick control via two analog joysticks (3 axes):
 *   Joystick 1 X → base rotation (J1)
 *   Joystick 1 Y → shoulder pitch (J2)
 *   Joystick 2 Y → elbow pitch (J3)
 *
 * Joystick wiring: center-tap potentiometers, 5V/GND/output
 *   JS1_X: A0, JS1_Y: A1, JS2_Y: A2
 */

#include <Servo.h>

// Servo objects
Servo baseServo;
Servo shoulderServo;
Servo elbowServo;

// Servo pin assignments
const int BASE_PIN     = 9;
const int SHOULDER_PIN = 10;
const int ELBOW_PIN    = 11;

// Joystick pins
const int JS1_X = A0;  // Base rotation
const int JS1_Y = A1;  // Shoulder
const int JS2_Y = A2;  // Elbow

// Servo angle state (current positions)
float baseAngle     = 90.0;
float shoulderAngle = 90.0;
float elbowAngle    = 90.0;

// Servo limits (adjust for your physical arm to prevent crashes)
const float BASE_MIN = 10,   BASE_MAX = 170;
const float SHLD_MIN = 20,   SHLD_MAX = 160;
const float ELBW_MIN = 10,   ELBW_MAX = 170;

// Control speed (degrees per loop iteration)
const float SPEED = 0.8;

// Joystick dead zone (joystick center noise)
const int DEAD_ZONE = 30;

// Link lengths for forward kinematics
const float L1 = 15.0;  // cm
const float L2 = 12.0;  // cm

int readJoystick(int pin) {
  // Returns -100 to +100, with dead zone
  int raw = analogRead(pin);
  int centered = raw - 512;  // Center around 0 (512 = mid ADC value)
  if (abs(centered) < DEAD_ZONE) return 0;
  return map(centered, -512, 512, -100, 100);
}

void setup() {
  baseServo.attach(BASE_PIN);
  shoulderServo.attach(SHOULDER_PIN);
  elbowServo.attach(ELBOW_PIN);

  Serial.begin(9600);
  Serial.println(F("3-DOF Arm Controller — Ready"));

  // Move to home position
  baseServo.write(90);
  shoulderServo.write(90);
  elbowServo.write(90);
  delay(1500);

  Serial.println(F("Home position reached. Use joysticks to move."));
}

void loop() {
  // Read joystick axes
  int js1x = readJoystick(JS1_X);  // Base
  int js1y = readJoystick(JS1_Y);  // Shoulder
  int js2y = readJoystick(JS2_Y);  // Elbow

  // Update angles based on joystick input
  baseAngle     += js1x * SPEED * 0.01;  // Scale to degrees/iteration
  shoulderAngle += js1y * SPEED * 0.01;
  elbowAngle    += js2y * SPEED * 0.01;

  // Clamp to limits
  baseAngle     = constrain(baseAngle,     BASE_MIN, BASE_MAX);
  shoulderAngle = constrain(shoulderAngle, SHLD_MIN, SHLD_MAX);
  elbowAngle    = constrain(elbowAngle,    ELBW_MIN, ELBW_MAX);

  // Write to servos
  baseServo.write((int)baseAngle);
  shoulderServo.write((int)shoulderAngle);
  elbowServo.write((int)elbowAngle);

  // Compute and display end effector position (forward kinematics)
  static unsigned long lastPrint = 0;
  if (millis() - lastPrint > 200) {  // Print every 200ms
    float theta_b = baseAngle     * PI / 180.0;
    float theta_s = shoulderAngle * PI / 180.0;
    float theta_e = elbowAngle    * PI / 180.0;

    float r = L1 * cos(theta_s) + L2 * cos(theta_s + theta_e);
    float z = L1 * sin(theta_s) + L2 * sin(theta_s + theta_e);
    float x = r * cos(theta_b);
    float y = r * sin(theta_b);

    Serial.print(F("B:"));  Serial.print(baseAngle,    1);
    Serial.print(F(" S:")); Serial.print(shoulderAngle, 1);
    Serial.print(F(" E:")); Serial.print(elbowAngle,   1);
    Serial.print(F(" → x:")); Serial.print(x, 1);
    Serial.print(F(" y:")); Serial.print(y, 1);
    Serial.print(F(" z:")); Serial.println(z, 1);
    lastPrint = millis();
  }
}

// Smooth servo motion: interpolate from current to target over duration_ms

void smoothMove(Servo &servo, float &currentAngle, float targetAngle,
                int duration_ms, int steps = 50) {
  float startAngle = currentAngle;
  float stepSize   = (targetAngle - startAngle) / steps;
  int   stepDelay  = duration_ms / steps;

  for (int i = 0; i <= steps; i++) {
    currentAngle = startAngle + stepSize * i;
    servo.write((int)currentAngle);
    delay(stepDelay);
  }
  currentAngle = targetAngle;  // Ensure exact final position
}

// Move all three joints simultaneously to a target configuration
void smoothMoveAll(float targetBase, float targetShoulder, float targetElbow,
                   int duration_ms, int steps = 50) {
  float deltaBase     = (targetBase     - baseAngle)     / steps;
  float deltaShoulder = (targetShoulder - shoulderAngle) / steps;
  float deltaElbow    = (targetElbow    - elbowAngle)    / steps;
  int   stepDelay     = duration_ms / steps;

  for (int i = 0; i <= steps; i++) {
    baseServo.write((int)(baseAngle     + deltaBase     * i));
    shoulderServo.write((int)(shoulderAngle + deltaShoulder * i));
    elbowServo.write((int)(elbowAngle   + deltaElbow    * i));
    delay(stepDelay);
  }

  // Update current angles
  baseAngle     = targetBase;
  shoulderAngle = targetShoulder;
  elbowAngle    = targetElbow;
}

// Example: pick-and-place sequence
void examplePickAndPlace() {
  // Move to pick position (above object)
  smoothMoveAll(90, 120, 45, 1000);  // 1 second to reach position
  delay(300);

  // Lower to object
  smoothMoveAll(90, 100, 60, 800);
  delay(300);

  // Close gripper (if J4 is gripper servo)
  // gripperServo.write(30);  // Closed position
  delay(500);

  // Lift up
  smoothMoveAll(90, 130, 30, 800);
  delay(300);

  // Move to place position
  smoothMoveAll(45, 120, 45, 1200);
  delay(300);

  // Lower to place
  smoothMoveAll(45, 105, 60, 800);
  delay(300);

  // Open gripper
  // gripperServo.write(80);  // Open position
  delay(500);

  // Return to home
  smoothMoveAll(90, 90, 90, 1500);
}

Workspace boundaries for L1=15cm, L2=12cm:

Maximum reach (full extension): L1 + L2 = 15 + 12 = 27cm from base
Minimum reach (full fold-back): |L1 - L2| = |15 - 12| = 3cm from base

The arm can reach any point in the annular region from 3cm to 27cm,
subject to joint angle limits.

If shoulder joint is limited to 30°–150° (avoiding floor collision):
  Only the upper half of the annulus is accessible.

Practical working region: 5cm–22cm (avoiding singularities near extremes)

// Print reachable workspace points for 2D visualization
// Copy Serial output to Excel / Python / Desmos to plot

const float L1 = 15.0;
const float L2 = 12.0;
const float SHLD_MIN_DEG = 30.0;
const float SHLD_MAX_DEG = 150.0;
const float ELBW_MIN_DEG = 10.0;
const float ELBW_MAX_DEG = 170.0;

void printWorkspace() {
  Serial.println(F("x_cm,y_cm"));  // CSV header
  for (float s = SHLD_MIN_DEG; s <= SHLD_MAX_DEG; s += 5.0) {
    for (float e = ELBW_MIN_DEG; e <= ELBW_MAX_DEG; e += 5.0) {
      float sr = s * PI / 180.0;
      float er = e * PI / 180.0;
      float x = L1 * cos(sr) + L2 * cos(sr + er);
      float y = L1 * sin(sr) + L2 * sin(sr + er);
      Serial.print(x, 2);
      Serial.print(F(","));
      Serial.println(y, 2);
    }
  }
  Serial.println(F("Done."));
}

// Call from setup() once, copy output to spreadsheet, plot as scatter plot

Placement strategy:

If picking objects from a tray:
  Place arm base so tray is between 40–70% of maximum reach
  (not too close where arm is over-folded, not too far where arm is near singularity)

If writing or drawing on a surface:
  Surface should be in the arm's "sweet spot" — directly in front of base,
  at a distance where shoulder ≈ 45°–90° and elbow ≈ 60°–120°

If mounting on a mobile robot:
  Mount the arm elevated, facing forward, with working area below and ahead
  Arm base height above ground = L1 (so folded arm clears chassis)

// Position recording and playback system
// Record up to MAX_POSES arm configurations, then play them back

const int MAX_POSES = 20;
int recordedBase[MAX_POSES];
int recordedShoulder[MAX_POSES];
int recordedElbow[MAX_POSES];
int poseCount = 0;

void recordCurrentPosition() {
  if (poseCount >= MAX_POSES) {
    Serial.println(F("Memory full! Maximum poses recorded."));
    return;
  }
  recordedBase[poseCount]     = (int)baseAngle;
  recordedShoulder[poseCount] = (int)shoulderAngle;
  recordedElbow[poseCount]    = (int)elbowAngle;
  poseCount++;

  Serial.print(F("Pose "));
  Serial.print(poseCount);
  Serial.print(F(" recorded: B="));
  Serial.print(baseAngle, 0);
  Serial.print(F(" S="));
  Serial.print(shoulderAngle, 0);
  Serial.print(F(" E="));
  Serial.println(elbowAngle, 0);
}

void playbackSequence(int repeatCount = 1) {
  Serial.print(F("Playing back "));
  Serial.print(poseCount);
  Serial.print(F(" poses, "));
  Serial.print(repeatCount);
  Serial.println(F(" times"));

  for (int rep = 0; rep < repeatCount; rep++) {
    for (int i = 0; i < poseCount; i++) {
      smoothMoveAll(recordedBase[i], recordedShoulder[i], recordedElbow[i], 800);
      delay(200);
    }
  }
  Serial.println(F("Playback complete."));
}

// Serial command interface: 'r' = record, 'p' = play, 'c' = clear
void handleSerialCommands() {
  if (Serial.available()) {
    char cmd = Serial.read();
    switch (cmd) {
      case 'r': recordCurrentPosition(); break;
      case 'p': playbackSequence(3);     break;  // Play 3 times
      case 'c':
        poseCount = 0;
        Serial.println(F("Sequence cleared."));
        break;
    }
  }
}

// Gripper control — add to existing arm code

Servo gripperServo;
const int GRIPPER_PIN  = 6;
const int GRIP_OPEN    = 80;  // Angle for open (adjust for your gripper geometry)
const int GRIP_CLOSED  = 20;  // Angle for closed (gripping object)
const int GRIP_NEUTRAL = 50;  // Partial open (carrying without firm grip)

void setupGripper() {
  gripperServo.attach(GRIPPER_PIN);
  gripperServo.write(GRIP_OPEN);  // Start open
}

void openGripper()   { gripperServo.write(GRIP_OPEN);   delay(300); }
void closeGripper()  { gripperServo.write(GRIP_CLOSED); delay(300); }

// Grip force estimation from servo current (if using current sensing):
// Normal grip: servo holds position, current near zero
// Object too large: servo stalls, current spikes → detect and stop
// This prevents crushing delicate objects by monitoring servo load

// Simple version without current sensing: use intermediate angle for delicate objects
void gentleGrip() {
  // Move gripper slowly to closed, stopping early
  for (int angle = GRIP_OPEN; angle >= GRIP_CLOSED + 10; angle -= 2) {
    gripperServo.write(angle);
    delay(20);
  }
}

Left sensor ──────────────────────── Left motor
Right sensor ─────────────────────── Right motor

Light intensity directly drives motor speed.
More light everywhere = faster everywhere.
Both sensors equal = drives straight toward or away from light source.
Sensor asymmetry (source off-center) = slight steering, both motors active.

Behavior: Drives straight toward the source, accelerating as it approaches.
          Appears to "charge" at the light.
          If source is very bright and close: runs into it at high speed.

Left sensor ─────────────────────── Right motor  (CROSSED)
Right sensor ────────────────────── Left motor   (CROSSED)

More light on LEFT → RIGHT motor speeds up → robot turns LEFT (toward light)
More light on RIGHT → LEFT motor speeds up → robot turns RIGHT (toward light)

Behavior: Steers toward light source regardless of direction.
          As it approaches and centers on source, sensors equalize, drives straight.
          Slows as it arrives directly beneath the light (source overhead = equal sensors).
          Appears to "seek" and "approach" — photophilic behavior.

Left sensor ─────────────────────── Left motor   (DIRECT)
Right sensor ────────────────────── Right motor  (DIRECT)

More light on LEFT → LEFT motor speeds up → robot turns RIGHT (away from light)
More light on RIGHT → RIGHT motor speeds up → robot turns LEFT (away from light)

Behavior: Steers away from light source.
          Accelerates when exposed to bright light (faster escape).
          Appears to "fear" the light — photophobic behavior.

GL5528 photoresistor specifications:

Resistance in bright light (10 lux): ~8–20kΩ (depending on exact unit)
Resistance in dim light (1 lux):     ~70–100kΩ
Resistance in darkness:               1MΩ+
Response time:                        ~20ms (rise), ~30ms (fall) — not for fast flashing
Spectral peak:                        ~540nm (green-yellow light, close to human eye peak)
Operating voltage:                    Any low DC voltage; typical 5V in divider circuits
Size:                                 5mm or 10mm diameter disc

Resistance-to-light relationship:
  R_LDR ∝ 1 / lux^0.7 (approximately — response is non-linear, logarithmic)
  This means:
  - 10× brighter → resistance decreases to ~20% of previous value
  - Not a precision instrument: ±20% variation unit-to-unit
  - Temperature-dependent: resistance increases in cold
  For a light-seeking robot: precision doesn't matter — only the difference
  between left and right readings matters, not absolute values.

VCC (5V) ──[R_fixed: 10kΩ]──┬──── Analog input (e.g. A0)
                             │
                         [LDR (R_variable)]
                             │
                            GND

V_out = 5V × R_LDR / (R_fixed + R_LDR)

In bright light:   R_LDR ≈ 10kΩ  → V_out = 5 × 10/(10+10) = 2.5V → ADC ≈ 511
In dim light:      R_LDR ≈ 80kΩ  → V_out = 5 × 80/(10+80) = 4.4V → ADC ≈ 904
In darkness:       R_LDR ≈ 1MΩ   → V_out = 5 × 1000/(10+1000) ≈ 4.95V → ADC ≈ 1012

Important: in this divider, V_out is HIGH in darkness and LOW in bright light.
To get a value that increases with brightness: brightness = 1023 - ADC_reading
Or: swap the positions of R_fixed and R_LDR (LDR on top → V_out high in bright light)

Left LDR circuit:
  5V ──[10kΩ]──┬── Arduino A0 (left light sensor)
               │
          [LDR_left]
               │
              GND

Right LDR circuit:
  5V ──[10kΩ]──┬── Arduino A1 (right light sensor)
               │
          [LDR_right]
               │
              GND

Top view of LDR placement:

           ↗ LDR_left  (faces 45° forward-left)
          ──────────────────
          │      ROBOT     │
          ──────────────────
           ↘ LDR_right (faces 45° forward-right)

With this placement:
- A light source directly ahead → both sensors read equally → drives straight
- A light source to the left → LDR_left reads brighter → steers left (Type 2a)
- A light source to the right → LDR_right reads brighter → steers right (Type 2a)
- Light from behind → both sensors read dim → robot slows or stops

// LDR sensor test sketch

const int LDR_LEFT  = A0;
const int LDR_RIGHT = A1;

void setup() {
  Serial.begin(9600);
  Serial.println(F("LDR Test — cover/uncover each sensor"));
}

void loop() {
  int rawLeft  = analogRead(LDR_LEFT);
  int rawRight = analogRead(LDR_RIGHT);

  // Convert to brightness (higher = brighter)
  int brightLeft  = 1023 - rawLeft;
  int brightRight = 1023 - rawRight;

  Serial.print(F("Left: "));
  Serial.print(brightLeft);
  Serial.print(F("  Right: "));
  Serial.print(brightRight);
  Serial.print(F("  Difference: "));
  Serial.println(brightLeft - brightRight);

  delay(200);
}

// Calibration offset to balance sensors
// Set this to (left_in_identical_light - right_in_identical_light) / 2
const int BALANCE_OFFSET = 30;  // Example: left reads 30 higher than right

int calibratedLeft  = brightLeft - BALANCE_OFFSET;
int calibratedRight = brightRight + BALANCE_OFFSET;

void calibrateSensors(int &leftOffset, int &rightOffset) {
  // Average 50 readings in current light conditions
  long sumLeft = 0, sumRight = 0;
  for (int i = 0; i < 50; i++) {
    sumLeft  += 1023 - analogRead(LDR_LEFT);
    sumRight += 1023 - analogRead(LDR_RIGHT);
    delay(20);
  }
  int avgLeft  = sumLeft  / 50;
  int avgRight = sumRight / 50;

  // Compute offsets to equalize sensors at current ambient light
  int avgBoth = (avgLeft + avgRight) / 2;
  leftOffset  = avgBoth - avgLeft;   // Add to left reading
  rightOffset = avgBoth - avgRight;  // Add to right reading

  Serial.print(F("Calibration: left offset="));
  Serial.print(leftOffset);
  Serial.print(F(" right offset="));
  Serial.println(rightOffset);
}

/*
 * Light-Seeking Robot — Braitenberg Vehicle Type 2a
 *
 * Crossed connections: left sensor → right motor, right sensor → left motor
 * Result: robot steers toward the brightest light source
 *
 * Hardware:
 *   LDR_left:  A0, voltage divider with 10kΩ to 5V
 *   LDR_right: A1, voltage divider with 10kΩ to 5V
 *   Motors: same as collision-avoiding rover (pins 5–10)
 */

// Motor pins (same as Articles 75–76)
const int IN1 = 5, IN2 = 6, ENA = 9;
const int IN3 = 7, IN4 = 8, ENB = 10;

// Sensor pins
const int LDR_LEFT  = A0;
const int LDR_RIGHT = A1;

// Behavior parameters
const int MIN_SPEED    = 60;   // Minimum motor speed (prevent stall, keep moving)
const int MAX_SPEED    = 220;  // Maximum motor speed
const int DARK_THRESH  = 50;   // Below this brightness: "dark" — stop or wander
const float GAIN       = 0.8;  // How strongly light difference drives steering
const int SMOOTH_N     = 5;    // Readings to average per sensor per loop

// Calibration offsets (set from calibration run or leave at 0)
int leftOffset  = 0;
int rightOffset = 0;

// Motor control
void setMotors(int leftPWM, int rightPWM) {
  leftPWM  = constrain(leftPWM,  0, MAX_SPEED);
  rightPWM = constrain(rightPWM, 0, MAX_SPEED);

  analogWrite(ENA, leftPWM);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  analogWrite(ENB, rightPWM);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void stopMotors() {
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

// Averaged sensor read
int readBrightness(int pin, int n = SMOOTH_N) {
  long sum = 0;
  for (int i = 0; i < n; i++) {
    sum += 1023 - analogRead(pin);  // Invert: high = bright
    delay(5);
  }
  return sum / n;
}

void setup() {
  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT); pinMode(ENA, OUTPUT);
  pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT); pinMode(ENB, OUTPUT);
  stopMotors();

  Serial.begin(9600);
  Serial.println(F("Light-Seeking Robot — initializing..."));

  // Auto-calibrate in ambient light (robot should be in typical environment, no directed light)
  calibrateSensors(leftOffset, rightOffset);

  delay(1000);
  Serial.println(F("Ready. Point a flashlight to steer!"));
}

void loop() {
  // Read and calibrate sensor brightnesses
  int brightLeft  = readBrightness(LDR_LEFT)  + leftOffset;
  int brightRight = readBrightness(LDR_RIGHT) + rightOffset;

  brightLeft  = constrain(brightLeft,  0, 1023);
  brightRight = constrain(brightRight, 0, 1023);

  int totalLight = brightLeft + brightRight;

  // Debug output
  Serial.print(F("L:"));
  Serial.print(brightLeft);
  Serial.print(F(" R:"));
  Serial.print(brightRight);
  Serial.print(F(" Diff:"));
  Serial.println(brightLeft - brightRight);

  // If overall scene is very dark: wander (slow random-ish movement)
  if (totalLight < DARK_THRESH * 2) {
    // Wander: slow forward with gentle oscillation
    setMotors(MIN_SPEED + 20, MIN_SPEED);
    return;
  }

  // ── Braitenberg Type 2a: CROSSED connections ────────────────────
  // Left sensor  → RIGHT motor  (more left light = faster right = turns left)
  // Right sensor → LEFT motor   (more right light = faster left = turns right)

  // Map brightness (0–1023) to motor speed (MIN_SPEED–MAX_SPEED)
  int rightMotorSpeed = map(brightLeft,  0, 1023, MIN_SPEED, MAX_SPEED);
  int leftMotorSpeed  = map(brightRight, 0, 1023, MIN_SPEED, MAX_SPEED);

  setMotors(leftMotorSpeed, rightMotorSpeed);
}

// ── Braitenberg Type 2b: DIRECT connections ────────────────────
// Left sensor  → LEFT motor   (more left light = faster left = turns right = AWAY from left)
// Right sensor → RIGHT motor  (more right light = faster right = turns left = AWAY from right)

// (Replace the motor assignment lines in loop() with these:)
int leftMotorSpeed  = map(brightLeft,  0, 1023, MIN_SPEED, MAX_SPEED);
int rightMotorSpeed = map(brightRight, 0, 1023, MIN_SPEED, MAX_SPEED);

setMotors(leftMotorSpeed, rightMotorSpeed);

// Light-comfort robot: seeks dim light, avoids bright light

const int TOO_DARK   = 100;   // Below this: seek more light (type 2a behavior)
const int TOO_BRIGHT = 700;   // Above this: avoid light (type 2b behavior)
// Between 100 and 700: comfortable — drive forward slowly

void loop() {
  int brightLeft  = readBrightness(LDR_LEFT)  + leftOffset;
  int brightRight = readBrightness(LDR_RIGHT) + rightOffset;
  int avgBright   = (brightLeft + brightRight) / 2;

  if (avgBright < TOO_DARK) {
    // Too dark — seek light (crossed connections: type 2a)
    int rightMotorSpeed = map(brightLeft,  0, 1023, MIN_SPEED, MAX_SPEED);
    int leftMotorSpeed  = map(brightRight, 0, 1023, MIN_SPEED, MAX_SPEED);
    setMotors(leftMotorSpeed, rightMotorSpeed);
    Serial.println(F("SEEKING"));

  } else if (avgBright > TOO_BRIGHT) {
    // Too bright — avoid light (direct connections: type 2b)
    int leftMotorSpeed  = map(brightLeft,  0, 1023, MIN_SPEED, MAX_SPEED);
    int rightMotorSpeed = map(brightRight, 0, 1023, MIN_SPEED, MAX_SPEED);
    setMotors(leftMotorSpeed, rightMotorSpeed);
    Serial.println(F("AVOIDING"));

  } else {
    // Comfortable light level — cruise slowly forward
    setMotors(MIN_SPEED + 30, MIN_SPEED + 30);
    Serial.println(F("COMFORTABLE"));
  }
}

// Combined: light-seeking + collision avoidance

const int OBSTACLE_THRESH = 20.0;  // cm

void loop() {
  float distance = getSmoothedDistance();  // HC-SR04 from Article 75 code

  if (distance < OBSTACLE_THRESH) {
    // Obstacle priority: avoid first
    stopMotors();
    delay(200);
    driveBackward(150);
    delay(500);
    turnRight(150);
    delay(600);
    stopMotors();
    return;  // Skip light-seeking this iteration
  }

  // No obstacle: seek light normally
  int brightLeft  = readBrightness(LDR_LEFT)  + leftOffset;
  int brightRight = readBrightness(LDR_RIGHT) + rightOffset;

  int rightMotorSpeed = map(brightLeft,  0, 1023, MIN_SPEED, MAX_SPEED);
  int leftMotorSpeed  = map(brightRight, 0, 1023, MIN_SPEED, MAX_SPEED);
  setMotors(leftMotorSpeed, rightMotorSpeed);
}

Sensor          Measurement      Possible behavior
──────────────────────────────────────────────────────────────────
LDR             Light intensity   Seek or avoid bright regions
Thermistor      Temperature       Seek warm areas (thermotaxis)
Microphone      Sound volume      Orient toward sound sources (phonotaxis)
IR distance     Obstacle range    Gradient descent away from walls
Soil moisture   Humidity          Seek damp areas (hygrotaxis)
Chemical sensor Gas concentration  Seek or avoid chemical gradients (chemotaxis)
Compass (HMC)   Magnetic heading  Maintain constant heading (magnetotaxis)

// Sound-seeking robot (Braitenberg Type 2a with microphone inputs)
// Uses two electret microphone modules with analog output

const int MIC_LEFT  = A0;
const int MIC_RIGHT = A1;
const int SILENCE   = 512;    // ADC value at silence (mid-rail for AC-coupled mic)

int readSoundLevel(int pin) {
  // Sound level = deviation from silence (RMS approximation: peak detection)
  int peak = 0;
  for (int i = 0; i < 100; i++) {  // Sample for ~10ms
    int sample = abs(analogRead(pin) - SILENCE);
    if (sample > peak) peak = sample;
  }
  return peak;  // 0–511, higher = louder
}

void loop() {
  int soundLeft  = readSoundLevel(MIC_LEFT);
  int soundRight = readSoundLevel(MIC_RIGHT);

  // Crossed connections: louder on left → faster right → turns left (toward sound)
  int rightMotorSpeed = map(soundLeft,  0, 511, MIN_SPEED, MAX_SPEED);
  int leftMotorSpeed  = map(soundRight, 0, 511, MIN_SPEED, MAX_SPEED);
  setMotors(leftMotorSpeed, rightMotorSpeed);
}

/*
 * Light-Seeking Robot with State Machine
 * States: WANDERING, SEEKING, RESTING
 *
 * WANDERING: ambient light low, no clear direction → wander slowly
 * SEEKING:   light detected with clear direction → pursue actively
 * RESTING:   very bright, centered under light source → stop and "bask"
 */

enum RobotState {
  STATE_WANDERING,
  STATE_SEEKING,
  STATE_RESTING
};

RobotState currentState = STATE_WANDERING;
unsigned long stateStartTime = 0;

const int WANDER_THRESH   = 150;   // Total brightness below which: wander
const int SEEK_THRESH     = 150;   // Above this and difference > MIN_DIFF: seek
const int REST_THRESH     = 800;   // Total brightness above this AND small difference: rest
const int MIN_DIFF        = 80;    // Minimum brightness difference to have clear direction
const int REST_MAX_DIFF   = 60;    // If centered (diff < this) AND very bright: rest

void transitionTo(RobotState newState) {
  if (newState != currentState) {
    currentState = newState;
    stateStartTime = millis();
    Serial.print(F("→ State: "));
    Serial.println(newState == STATE_WANDERING ? F("WANDERING") :
                   newState == STATE_SEEKING   ? F("SEEKING")   : F("RESTING"));
  }
}

void loop() {
  // Read sensors
  int brightLeft  = readBrightness(LDR_LEFT)  + leftOffset;
  int brightRight = readBrightness(LDR_RIGHT) + rightOffset;
  brightLeft  = constrain(brightLeft,  0, 1023);
  brightRight = constrain(brightRight, 0, 1023);

  int totalBright = brightLeft + brightRight;
  int diff        = abs(brightLeft - brightRight);

  // ── State transitions ──────────────────────────────────────────
  if (totalBright < WANDER_THRESH) {
    transitionTo(STATE_WANDERING);

  } else if (totalBright > REST_THRESH && diff < REST_MAX_DIFF) {
    transitionTo(STATE_RESTING);

  } else if (totalBright >= SEEK_THRESH && diff >= MIN_DIFF) {
    transitionTo(STATE_SEEKING);

  } else if (totalBright >= SEEK_THRESH && diff < MIN_DIFF) {
    // Bright but centered — keep seeking (heading toward source)
    transitionTo(STATE_SEEKING);
  }

  // ── State behaviors ────────────────────────────────────────────
  switch (currentState) {

    case STATE_WANDERING: {
      // Meander: alternate gentle curves every 1.5 seconds
      unsigned long elapsed = millis() - stateStartTime;
      if ((elapsed / 1500) % 2 == 0) {
        setMotors(MIN_SPEED + 30, MIN_SPEED);  // Gentle right curve
      } else {
        setMotors(MIN_SPEED, MIN_SPEED + 30);  // Gentle left curve
      }
      break;
    }

    case STATE_SEEKING: {
      // Braitenberg Type 2a: crossed connections
      int rightMotorSpeed = map(brightLeft,  0, 1023, MIN_SPEED, MAX_SPEED);
      int leftMotorSpeed  = map(brightRight, 0, 1023, MIN_SPEED, MAX_SPEED);
      setMotors(leftMotorSpeed, rightMotorSpeed);
      break;
    }

    case STATE_RESTING: {
      // Stop and wait — "basking" in the light
      stopMotors();
      // If resting for more than 3 seconds and still bright: stay resting
      // (transitions handled in the transition logic above)
      break;
    }
  }

  // Debug output
  Serial.print(F("L:"));  Serial.print(brightLeft);
  Serial.print(F(" R:")); Serial.print(brightRight);
  Serial.print(F(" T:")); Serial.print(totalBright);
  Serial.print(F(" D:")); Serial.println(diff);
}

// Dynamic range calibration — run in setup() after robot is placed in environment

int minLeft = 1023, maxLeft = 0;
int minRight = 1023, maxRight = 0;

void autoRangeCalibrate(int durationMs = 3000) {
  Serial.println(F("Calibrating... wave robot around for 3 seconds"));
  unsigned long start = millis();
  while (millis() - start < durationMs) {
    int l = 1023 - analogRead(LDR_LEFT);
    int r = 1023 - analogRead(LDR_RIGHT);
    minLeft  = min(minLeft,  l);
    maxLeft  = max(maxLeft,  l);
    minRight = min(minRight, r);
    maxRight = max(maxRight, r);
    delay(20);
  }
  Serial.print(F("Left range: "));  Serial.print(minLeft);  Serial.print(F("–")); Serial.println(maxLeft);
  Serial.print(F("Right range: ")); Serial.print(minRight); Serial.print(F("–")); Serial.println(maxRight);
}

// During normal operation: scale to 0–1023 using measured range
int scaledLeft()  { return map(1023 - analogRead(LDR_LEFT),  minLeft,  maxLeft,  0, 1023); }
int scaledRight() { return map(1023 - analogRead(LDR_RIGHT), minRight, maxRight, 0, 1023); }

// Directional sensitivity test
// Rotate robot slowly in place, record both sensor readings
// Plot the result to see the angular sensitivity pattern

void measureDirectionality() {
  Serial.println(F("angle,left,right,diff"));
  for (int angle = 0; angle < 360; angle += 10) {
    // Rotate 10° steps manually, pressing a button or waiting for input
    Serial.print(angle);
    Serial.print(F(","));
    int l = readBrightness(LDR_LEFT);
    int r = readBrightness(LDR_RIGHT);
    Serial.print(l);
    Serial.print(F(","));
    Serial.print(r);
    Serial.print(F(","));
    Serial.println(l - r);
    delay(2000);  // 2 seconds to rotate to next position
  }
}

IR reflective sensor operation:

          ┌───────────────────┐
          │  IR LED  │ Photo  │  (sensor bottom view)
          │  (emits) │ (rcv)  │
          └────┬─────┴────┬───┘
               │ IR       │ Reflected
               ↓ light    │ light
          ══════════════════════  Surface

Dark surface (black line):
  IR light absorbed → little reflection → phototransistor receives little light
  → high resistance → output voltage HIGH (with pull-up) or LOW depending on circuit

Light surface (white paper):
  IR light reflected → phototransistor receives lots of light
  → low resistance → output voltage LOW (with pull-up) or HIGH depending on circuit

Typical sensor modules (TCRT5000-based):
  Over white: analog output ~0.5V, digital output LOW
  Over black: analog output ~3.5V, digital output HIGH
  Effective sensing range: 2–15mm from surface
  Optimum height: 5–8mm from line surface

2-sensor array (simplest):
  [L]   [R]        L=left sensor, R=right sensor

  States:
  L=white, R=white: on line (line between sensors), drive straight
  L=black, R=white: drifted right (left sensor on line), steer left
  L=white, R=black: drifted left (right sensor on line), steer right
  L=black, R=black: completely off line (both sensors on line?), or on junction

  Limitation: no proportional information — can only tell which side, not how far

3-sensor array (good for beginners):
  [L]  [C]  [R]    C=center sensor

  States allow: centered (C=black), slightly off (C+L or C+R), moderately off (L or R only)
  Better than 2-sensor; still limited positional resolution

5-sensor array (recommended):
  [LL] [L] [C] [R] [RR]   LL=far left, RR=far right

  16 possible states (each sensor on/off)
  Most common line configurations well-distinguished
  Good balance of resolution vs. cost and wiring

8-sensor array (advanced):
  Many commercial line follower modules use 8 sensors
  256 possible states; computed position 0–7000 for proportional control
  Excellent for fast, smooth PID following at high speed

Chassis side view showing sensor placement:

─────────────────────────────
[Motor]   [Arduino/L298N]   [Motor]
  Wheel                      Wheel
         ↑ chassis bottom
   ┌─────────────────────┐  ← sensor array (front edge of chassis)
   │[L][ML][C][MR][R]   │     5–8mm above floor
   └─────────────────────┘
              ↓
         ════════════════  Floor / line surface

5-sensor digital array wiring:

Sensor VCC  ──────── Arduino 5V
Sensor GND  ──────── Arduino GND
Sensor LL (far left) ────── Arduino A0 (or D3)
Sensor L  (left)     ────── Arduino A1 (or D4)
Sensor C  (center)   ────── Arduino A2 (or D5)
Sensor R  (right)    ────── Arduino A3 (or D6)
Sensor RR (far right) ───── Arduino A4 (or D7)

Note: Digital outputs can connect to any digital pin.
      Analog outputs require A0–A5 for analogRead().

// Sensor test sketch — confirm correct readings before control code

const int SENSOR_PINS[] = {A0, A1, A2, A3, A4};  // LL, L, C, R, RR
const int NUM_SENSORS = 5;

void setup() {
  Serial.begin(9600);
  for (int i = 0; i < NUM_SENSORS; i++) {
    pinMode(SENSOR_PINS[i], INPUT);
  }
}

void loop() {
  // Read and print all sensors
  for (int i = 0; i < NUM_SENSORS; i++) {
    int val = digitalRead(SENSOR_PINS[i]);
    Serial.print(val);
    Serial.print(" ");
  }
  Serial.println();
  delay(100);
}

/*
 * Line Follower — Bang-Bang Control
 * Sensor array: LL=A0, L=A1, C=A2, R=A3, RR=A4
 * Motors: same as Article 75 rover
 */

// Motor pins (same as collision-avoiding rover)
const int IN1 = 5, IN2 = 6, ENA = 9;
const int IN3 = 7, IN4 = 8, ENB = 10;

// Sensor pins
const int S_LL = A0, S_L = A1, S_C = A2, S_R = A3, S_RR = A4;

// Speed constants
const int BASE_SPEED  = 150;
const int TURN_SPEED  = 150;

// Motor control (reused from rover)
void setMotors(int leftPWM, bool leftFwd, int rightPWM, bool rightFwd) {
  analogWrite(ENA, leftPWM);
  digitalWrite(IN1, leftFwd ? HIGH : LOW);
  digitalWrite(IN2, leftFwd ? LOW  : HIGH);
  analogWrite(ENB, rightPWM);
  digitalWrite(IN3, rightFwd ? HIGH : LOW);
  digitalWrite(IN4, rightFwd ? LOW  : HIGH);
}

void setup() {
  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT); pinMode(ENA, OUTPUT);
  pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT); pinMode(ENB, OUTPUT);
  Serial.begin(9600);
}

void loop() {
  // Read sensors (1 = on line/black, 0 = off line/white — adjust if inverted)
  bool ll = digitalRead(S_LL);
  bool l  = digitalRead(S_L);
  bool c  = digitalRead(S_C);
  bool r  = digitalRead(S_R);
  bool rr = digitalRead(S_RR);

  // Bang-bang control logic
  if (c && !l && !r) {
    // Center sensor on line, edges off → drive straight
    setMotors(BASE_SPEED, true, BASE_SPEED, true);

  } else if (l || ll) {
    // Line is to the LEFT → turn LEFT (left motor slow/reverse, right fast)
    setMotors(0, true, TURN_SPEED, true);  // Gentle: stop left, full right

  } else if (r || rr) {
    // Line is to the RIGHT → turn RIGHT (right motor slow, left fast)
    setMotors(TURN_SPEED, true, 0, true);  // Gentle: full left, stop right

  } else {
    // No sensors on line — lost track
    // Option 1: stop and wait
    setMotors(0, true, 0, true);
    // Option 2: continue last known direction (set before this block)
  }
}

// Compute weighted position: returns value representing line position
// Returns: negative = line left of center, 0 = centered, positive = line right

int computePosition() {
  // Sensor weights: position (distance from center in arbitrary units)
  // LL=-4, L=-2, C=0, R=2, RR=4
  const int weights[] = {-4, -2, 0, 2, 4};
  const int sensorPins[] = {S_LL, S_L, S_C, S_R, S_RR};

  int weightedSum = 0;
  int activeSensors = 0;

  for (int i = 0; i < 5; i++) {
    if (digitalRead(sensorPins[i]) == HIGH) {  // Adjust HIGH/LOW for your sensor polarity
      weightedSum += weights[i];
      activeSensors++;
    }
  }

  if (activeSensors == 0) return 999;  // Special value: line lost
  return weightedSum / activeSensors;  // Average position of active sensors
}

// Analog position computation — requires analogRead() on sensor pins
// Returns position from -1000 (far left) to +1000 (far right)

int computeAnalogPosition() {
  const int sensorPins[] = {S_LL, S_L, S_C, S_R, S_RR};
  const int weights[]    = {-1000, -500, 0, 500, 1000};

  long weightedSum = 0;
  long totalSum = 0;

  for (int i = 0; i < 5; i++) {
    int val = analogRead(sensorPins[i]);
    // Invert if needed: 1023 = on line, 0 = off line
    // (adjust subtraction based on your sensor: over line = high or low?)
    weightedSum += (long)val * weights[i];
    totalSum += val;
  }

  if (totalSum == 0) return 0;
  return weightedSum / totalSum;
}

/*
 * Line Follower — Proportional Control (P-only)
 */

// Proportional gain — tune this for your robot
// Too low: sluggish, can't handle curves
// Too high: oscillates like bang-bang
float Kp = 0.4;

void loop() {
  int position = computePosition();  // -4 to +4; 0 = centered

  if (position == 999) {
    // Line lost — stop or implement recovery behavior
    setMotors(0, true, 0, true);
    return;
  }

  // Proportional correction: error × gain = steering adjustment
  int correction = (int)(position * Kp * 50);  // Scale to useful PWM range

  int leftSpeed  = BASE_SPEED - correction;  // Reduce left when line is left
  int rightSpeed = BASE_SPEED + correction;  // Increase right when line is left

  // Clamp speeds to valid range (0–255)
  leftSpeed  = constrain(leftSpeed,  0, 255);
  rightSpeed = constrain(rightSpeed, 0, 255);

  setMotors(leftSpeed, true, rightSpeed, true);
}

/*
 * Line Follower — Full PID Control
 * Uses analog sensor readings for better resolution
 */

// PID gains — tune these systematically (see tuning guide below)
float Kp = 0.25;   // Proportional gain
float Ki = 0.001;  // Integral gain (small — prevents integral windup)
float Kd = 0.8;    // Derivative gain

// PID state variables
float integral    = 0.0;
float lastError   = 0.0;
unsigned long lastTime = 0;

// Speed constants
const int BASE_SPEED     = 180;  // Base forward speed
const int MAX_SPEED      = 255;  // Maximum motor speed
const int MIN_SPEED      = 0;    // Minimum motor speed
const float MAX_INTEGRAL = 500;  // Anti-windup clamp

void setup() {
  // ... (pin setup as before) ...
  lastTime = millis();
  Serial.begin(9600);
}

void loop() {
  unsigned long now = millis();
  float dt = (now - lastTime) / 1000.0;  // Time since last iteration (seconds)
  lastTime = now;

  // Read position error (-1000 = far left, 0 = centered, +1000 = far right)
  int error = computeAnalogPosition();

  // Check for lost line
  static bool prevLost = false;
  bool lost = (abs(error) > 900 && /* all sensors off */ true);
  // Simplified: you'd check if all sensors read near-zero (no line detected)

  // Integral term with anti-windup
  integral += error * dt;
  integral = constrain(integral, -MAX_INTEGRAL, MAX_INTEGRAL);

  // Derivative term
  float derivative = (error - lastError) / dt;
  lastError = error;

  // PID output
  float correction = Kp * error + Ki * integral + Kd * derivative;

  // Apply correction to base speed
  int leftSpeed  = (int)(BASE_SPEED - correction);
  int rightSpeed = (int)(BASE_SPEED + correction);

  leftSpeed  = constrain(leftSpeed,  MIN_SPEED, MAX_SPEED);
  rightSpeed = constrain(rightSpeed, MIN_SPEED, MAX_SPEED);

  setMotors(leftSpeed, true, rightSpeed, true);

  // Debug output (comment out after tuning — Serial.print() adds delay)
  Serial.print(error);
  Serial.print(",");
  Serial.print(correction, 1);
  Serial.print(",");
  Serial.print(leftSpeed);
  Serial.print(",");
  Serial.println(rightSpeed);
}

PID tuning quick reference:

Symptom                              Adjustment
─────────────────────────────────────────────────────────────────
Barely steers, slides off curves     Increase Kp
Oscillates, weaves on straight       Decrease Kp, or increase Kd
Overshoots curves, wiggles           Increase Kd
Jerky response to small errors       Decrease Kd (too sensitive to noise)
Persistent one-sided bias            Increase Ki
Slowly growing oscillations          Decrease Ki, or lower MAX_INTEGRAL
Works well on gentle curves only     Re-tune for higher base speed
Works at low speed, fails at high    Kd may need to increase for faster dynamics

Track progression:

Level 1: Straight line
  ────────────────────────────────────────
  Just drives forward. Tests sensor polarity and basic motor response.

Level 2: Gentle oval
  ╭──────────────────────────────────────╮
  │                                      │
  ╰──────────────────────────────────────╯
  Introduces continuous curves. Tests proportional response.

Level 3: Oval with one sharp corner
  ╭────────────────────────────────────╮
  │                            ╔═══════╝
  │                            ║
  └────────────────────────────╝
  Sharp corners test derivative damping and corner speed management.

Level 4: Figure-eight
  ╭─────╮     ╭─────╮
  │     ╰─────╯     │
  │     ╭─────╮     │
  ╰─────╯     ╰─────╯
  Includes line crossings (intersections) — sensors read all-black briefly.
  Requires handling the "lost" state gracefully.

Level 5: Competition course
  Multiple sharp turns, narrow straightaways, chicanes
  Tests speed management and PID robustness

// Line loss recovery with last-direction memory

int  lastKnownPosition = 0;  // Remember which side line was last seen on
bool lineWasLost = false;

void loop() {
  int position = computeAnalogPosition();

  // Detect line loss: all sensors reading below threshold
  long totalSensorValue = 0;
  const int sensorPins[] = {S_LL, S_L, S_C, S_R, S_RR};
  for (int i = 0; i < 5; i++) totalSensorValue += analogRead(sensorPins[i]);
  bool lineLost = (totalSensorValue < 200);  // Threshold: adjust for your sensors

  if (!lineLost) {
    lastKnownPosition = position;
    lineWasLost = false;

    // Normal PID control
    // ... (PID code as above) ...

  } else {
    // Line lost — execute recovery
    lineWasLost = true;

    if (lastKnownPosition < 0) {
      // Line was last to the left — spin left to find it
      setMotors(0, true, TURN_SPEED, true);
    } else {
      // Line was last to the right — spin right to find it
      setMotors(TURN_SPEED, true, 0, true);
    }
  }
}

Control method comparison on a standard oval track:

Metric              Bang-Bang    P-only    PID
─────────────────────────────────────────────────────────────────────
Straight tracking   Oscillates   Smooth    Very smooth
Gentle curve        Handles      Good      Excellent
Sharp curve speed   Slow (stalls) Moderate  Fast with tuning
Steady-state drift  Ignores      Ignores   Corrects (Ki term)
Noise sensitivity   Low          Moderate  Higher (Kd amplifies noise)
Tuning required     None         1 gain    3 gains
Code complexity     Very simple  Simple    Moderate
Suitable for        Learning     Most uses  Fast, precise tracking
Maximum speed       ~100 PWM     ~150 PWM  ~200+ PWM (tuned)

// High-frequency control loop without Serial.print()
void loop() {
  // Remove all delay() calls
  // Remove all Serial.print() calls during normal operation
  // The control loop then runs as fast as the sensors can be read

  int error = computeAnalogPosition();  // ~5 × 104µs ADC reads = ~520µs
  // PID computation: ~30µs
  // setMotors: ~10µs
  // Total loop time without delays: ~560µs → control at ~1,785 Hz

  // This gives 56× more corrections per meter than a 100ms loop
}

// Speed scaling based on position error
// Large error = in a curve = slow down
// Small error = straight = speed up

const int MIN_BASE = 100;  // Minimum speed on sharpest curves
const int MAX_BASE = 220;  // Maximum speed on straights

void loop() {
  int error = computeAnalogPosition();

  // Scale base speed inversely with error magnitude
  int absError = abs(error);
  int dynamicBase = map(absError, 0, 1000, MAX_BASE, MIN_BASE);
  dynamicBase = constrain(dynamicBase, MIN_BASE, MAX_BASE);

  // Apply PID correction to this dynamic base
  float correction = Kp * error + Ki * integral + Kd * derivative;

  int leftSpeed  = constrain((int)(dynamicBase - correction), 0, MAX_SPEED);
  int rightSpeed = constrain((int)(dynamicBase + correction), 0, MAX_SPEED);

  setMotors(leftSpeed, true, rightSpeed, true);
}

// Dead band: suppress small corrections when nearly centered
const int DEAD_BAND = 50;  // In position units (0–1000 scale)

void loop() {
  int error = computeAnalogPosition();

  // If error within dead band, treat as zero error
  if (abs(error) < DEAD_BAND) {
    error = 0;
    integral = 0;  // Also reset integral to prevent it winding up in center
  }

  // Rest of PID as normal...
}

if (lineLost) {
  integral = 0;  // Clear integral on line loss
  // Recovery behavior...
}

// Filtered error for derivative — prevents noise amplification
float filteredError = 0.8 * filteredError + 0.2 * error;  // EMA, alpha=0.2
float derivative = (filteredError - lastFilteredError) / dt;
lastFilteredError = filteredError;

// Sensor fusion: blend IR position error with gyro heading error
// Prevents wheel slip from allowing the robot to drift off course

float irError   = computeAnalogPosition() / 1000.0;  // Normalize to -1 to +1
float gyroRate  = readGyroZ();                        // Degrees per second

// Expected heading rate for this steering command
float expectedRate = correction * HEADING_SCALE;      // Calibrated experimentally

// Gyro error: robot not rotating as commanded
float gyroError = expectedRate - gyroRate;

// Fused correction: IR position error + gyro heading error
float fusedCorrection = Kp * irError + Kg * gyroError;

Top view of completed rover:

          ┌──────────────────────┐
          │    [HC-SR04 sensor]  │  ← faces forward
          │     (  )    (  )    │     eyes of the robot
          │                     │
          │   [Arduino Uno]     │
          │                     │
          │   [L298N driver]    │
          │                     │
  [Motor]─┤                     ├─[Motor]
  [Wheel] │   [Battery pack]   │ [Wheel]
          └──────────────────────┘
                  (caster)
                    ↓
              front of travel

L298N module terminal layout:

┌─────────────────────────────────────┐
│   12V  GND  5V                      │  ← Power input / 5V output terminals
│                                     │
│   OUT1 OUT2     OUT3 OUT4           │  ← Motor output terminals
│                                     │
│   IN1  IN2  EN_A    IN3  IN4  EN_B  │  ← Arduino control inputs
└─────────────────────────────────────┘

Terminal functions:
  12V:  Motor supply voltage (6–12V from battery; label says 12V but accepts lower)
  GND:  Common ground (connect to battery − AND Arduino GND)
  5V:   Output: 5V regulated output when motor supply ≥ 7V
        (Can power Arduino; short-circuits jumper when motor supply < 7V)

  OUT1/OUT2: Motor A output terminals (connect to left motor)
  OUT3/OUT4: Motor B output terminals (connect to right motor)

  IN1, IN2: Direction control for Motor A
    IN1=HIGH, IN2=LOW:  Motor A forward
    IN1=LOW, IN2=HIGH:  Motor A reverse
    IN1=HIGH, IN2=HIGH: Motor A brake (stop)
    IN1=LOW, IN2=LOW:   Motor A coast (stop, no braking)

  IN3, IN4: Direction control for Motor B (same logic as IN1/IN2)

  EN_A: Enable/speed for Motor A
    Jumper installed: Motor A always at full speed
    Jumper removed, PWM signal connected: speed control via PWM
  EN_B: Enable/speed for Motor B (same as EN_A)

Battery pack (+) ──────────────────────────── L298N terminal "12V"
Battery pack (+) ──── [Power switch] ──────── (route through switch first)
Battery pack (−) ──────────────────────────── L298N terminal "GND"
L298N terminal "GND" ──────────────────────── Arduino GND pin
L298N terminal "5V" ───────────────────────── Arduino 5V pin (powers Arduino)

Note: L298N 5V output is only present when motor supply ≥ 7V.
  6× AA (9V nominal): 5V output available → can power Arduino this way
  4× AA (6V nominal): 5V output NOT available → power Arduino via USB or 9V snap

Alternative Arduino power: 9V battery with snap connector to Arduino Vin/GND
  (Arduino's onboard regulator handles 9V → 5V)

Left motor (+) wire  ──────── L298N OUT1
Left motor (−) wire  ──────── L298N OUT2

Right motor (+) wire ──────── L298N OUT3
Right motor (−) wire ──────── L298N OUT4

Note: "+" and "−" are provisional — actual direction determined in Step 4.
      If motor spins backward when expected to go forward, swap OUT1↔OUT2
      (or OUT3↔OUT4 for right motor).

Arduino Pin 5  ──────────────── L298N IN1   (Left motor direction 1)
Arduino Pin 6  ──────────────── L298N IN2   (Left motor direction 2)
Arduino Pin 7  ──────────────── L298N IN3   (Right motor direction 1)
Arduino Pin 8  ──────────────── L298N IN4   (Right motor direction 2)
Arduino Pin 9  ──────────────── L298N EN_A  (Left motor speed, PWM)
Arduino Pin 10 ──────────────── L298N EN_B  (Right motor speed, PWM)

HC-SR04 VCC  ──────────────── Arduino 5V  (or L298N 5V output)
HC-SR04 GND  ──────────────── Arduino GND
HC-SR04 TRIG ──────────────── Arduino Pin 11
HC-SR04 ECHO ──────────────── Arduino Pin 12

Verification checklist:

□ Continuity between L298N GND and Arduino GND → should beep
□ Continuity between L298N 12V and battery (+) → should beep
□ Continuity between L298N GND and battery (−) → should beep
□ NO continuity between L298N 12V and GND → open (not shorted)
□ Continuity from HC-SR04 VCC to Arduino 5V → should beep
□ Continuity from HC-SR04 GND to Arduino GND → should beep

Check for motor shorts (with battery disconnected):
□ Continuity from OUT1 to OUT2 → should NOT beep (not shorted)
□ Continuity from OUT3 to OUT4 → should NOT beep (not shorted)

/*
 * Collision-Avoiding Rover — Complete Sketch
 * Hardware:
 *   - L298N motor driver
 *   - Left motor: IN1=5, IN2=6, EN_A=9
 *   - Right motor: IN3=7, IN4=8, EN_B=10
 *   - HC-SR04: TRIG=11, ECHO=12
 *
 * Behavior:
 *   Drive forward → obstacle detected within STOP_DISTANCE → stop →
 *   back up briefly → turn → resume forward
 */

// ── Pin definitions ──────────────────────────────────────────────
const int IN1  = 5;   // Left motor direction 1
const int IN2  = 6;   // Left motor direction 2
const int ENA  = 9;   // Left motor speed (PWM)
const int IN3  = 7;   // Right motor direction 1
const int IN4  = 8;   // Right motor direction 2
const int ENB  = 10;  // Right motor speed (PWM)
const int TRIG = 11;  // Ultrasonic trigger
const int ECHO = 12;  // Ultrasonic echo

// ── Behavior parameters ──────────────────────────────────────────
const int  DRIVE_SPEED    = 180;   // Forward speed (0–255); start conservative
const int  TURN_SPEED     = 160;   // Speed during turns
const int  BACKUP_SPEED   = 150;   // Speed during backup
const float STOP_DISTANCE  = 20.0; // Stop if obstacle within this many cm
const int  BACKUP_TIME    = 600;   // ms to reverse before turning
const int  TURN_TIME      = 700;   // ms to turn (adjust for 90° turn on your robot)

// ── Motor control functions ──────────────────────────────────────

void setMotors(int leftSpeed, bool leftForward,
               int rightSpeed, bool rightForward) {
  // Left motor
  analogWrite(ENA, abs(leftSpeed));
  digitalWrite(IN1, leftForward ? HIGH : LOW);
  digitalWrite(IN2, leftForward ? LOW  : HIGH);

  // Right motor
  analogWrite(ENB, abs(rightSpeed));
  digitalWrite(IN3, rightForward ? HIGH : LOW);
  digitalWrite(IN4, rightForward ? LOW  : HIGH);
}

void driveForward(int speed) {
  setMotors(speed, true, speed, true);
}

void driveBackward(int speed) {
  setMotors(speed, false, speed, false);
}

void turnRight(int speed) {
  // Left motor forward, right motor backward = pivot right
  setMotors(speed, true, speed, false);
}

void turnLeft(int speed) {
  // Left motor backward, right motor forward = pivot left
  setMotors(speed, false, speed, true);
}

void stopMotors() {
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
  // Direction pins don't matter at speed 0, but clean state is good practice
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

// ── Distance measurement ─────────────────────────────────────────

float measureDistance() {
  // Send 10µs trigger pulse
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  // Measure echo pulse duration (timeout after 25ms = ~4.3m max range)
  long duration = pulseIn(ECHO, HIGH, 25000);

  if (duration == 0) return 999.0;  // Timeout → no obstacle detected (report far)

  // Sound travels at ~343 m/s = 0.0343 cm/µs
  // Distance = (duration / 2) × speed_of_sound (divided by 2: out + back)
  return (duration / 2.0) * 0.0343;
}

float getSmoothedDistance() {
  // Average 3 readings with brief pauses between them
  // HC-SR04 needs at least 60ms between measurements to prevent echo interference
  float sum = 0;
  for (int i = 0; i < 3; i++) {
    sum += measureDistance();
    delay(30);
  }
  return sum / 3.0;
}

// ── Setup ────────────────────────────────────────────────────────

void setup() {
  // Configure motor control pins as outputs
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENB, OUTPUT);

  // Configure sensor pins
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  // Start with motors stopped
  stopMotors();

  // Serial for debugging (open Serial Monitor at 9600 baud to see distances)
  Serial.begin(9600);
  Serial.println(F("Collision-Avoiding Rover — Ready"));

  // Brief startup pause before beginning autonomous operation
  delay(2000);
}

// ── Main behavior loop ───────────────────────────────────────────

void loop() {
  float distance = getSmoothedDistance();

  Serial.print(F("Distance: "));
  Serial.print(distance, 1);
  Serial.println(F(" cm"));

  if (distance > STOP_DISTANCE) {
    // Path is clear — drive forward
    driveForward(DRIVE_SPEED);

  } else {
    // Obstacle detected — execute avoidance maneuver
    Serial.println(F("Obstacle! Avoiding..."));

    // 1. Stop
    stopMotors();
    delay(200);

    // 2. Back up
    driveBackward(BACKUP_SPEED);
    delay(BACKUP_TIME);

    // 3. Stop briefly
    stopMotors();
    delay(100);

    // 4. Turn (alternate left/right using a static variable for variety)
    static bool turnDirection = true;  // true = right, false = left
    if (turnDirection) {
      turnRight(TURN_SPEED);
    } else {
      turnLeft(TURN_SPEED);
    }
    turnDirection = !turnDirection;  // Alternate next time
    delay(TURN_TIME);

    // 5. Stop briefly before resuming
    stopMotors();
    delay(100);
  }
}

// Example: left motor slightly faster than right
const int LEFT_SPEED  = 160;  // Reduced to compensate
const int RIGHT_SPEED = 180;  // Full speed

// In driveForward():
setMotors(LEFT_SPEED, true, RIGHT_SPEED, true);

Tuning process:
1. Mark the robot's starting orientation with tape on the floor
2. Trigger an avoidance maneuver manually (block the sensor)
3. Observe how far the robot turns
4. Adjust TURN_TIME:
   - Robot turns less than 90°: increase TURN_TIME (try 900)
   - Robot turns more than 90°: decrease TURN_TIME (try 500)
5. Repeat until turn is approximately 90°

// Three-sensor version
const int TRIG_LEFT = 11, ECHO_LEFT = 12;   // 45° left
const int TRIG_FWD  = A0, ECHO_FWD  = A1;   // Straight ahead
const int TRIG_RIGHT = A2, ECHO_RIGHT = A3; // 45° right

// Choose turn direction based on which side has more space
float distLeft  = measureDistanceOnPins(TRIG_LEFT, ECHO_LEFT);
float distRight = measureDistanceOnPins(TRIG_RIGHT, ECHO_RIGHT);
if (distRight > distLeft) {
  turnRight(TURN_SPEED);
} else {
  turnLeft(TURN_SPEED);
}

#include <Servo.h>
Servo scanServo;

int scanForBestDirection() {
  scanServo.write(180);  // Look left
  delay(300);
  float leftDist = measureDistance();

  scanServo.write(0);    // Look right
  delay(300);
  float rightDist = measureDistance();

  scanServo.write(90);   // Center
  delay(200);

  return (rightDist > leftDist) ? 1 : -1;  // 1=turn right, -1=turn left
}

const int LED_FWD   = 13;  // Green: driving forward
const int LED_AVOID = A4;  // Red: avoidance maneuver active

// In driveForward():
digitalWrite(LED_FWD, HIGH);
digitalWrite(LED_AVOID, LOW);

// At start of avoidance:
digitalWrite(LED_FWD, LOW);
digitalWrite(LED_AVOID, HIGH);

// In loop(), before the distance check:
if (Serial.available()) {
  char cmd = Serial.read();
  switch (cmd) {
    case 'f': driveForward(DRIVE_SPEED);  delay(500); break;
    case 'b': driveBackward(BACKUP_SPEED); delay(500); break;
    case 'l': turnLeft(TURN_SPEED);       delay(300); break;
    case 'r': turnRight(TURN_SPEED);      delay(300); break;
    case 's': stopMotors();               break;
  }
}

Differential drive motion modes:

Both wheels forward, equal speed → drive straight forward
Both wheels backward, equal speed → drive straight backward
Left wheel forward, right wheel stopped → gentle right curve
Left wheel forward, right wheel backward → pivot turn right in place
Left wheel stopped, right wheel forward → gentle left curve
Left wheel backward, right wheel forward → pivot turn left in place
Both wheels different forward speeds → gradual curve toward slower wheel

Turn angle ≈ TURN_SPEED × TURN_TIME × wheel_speed_per_PWM_unit / wheel_base_distance

For a typical small chassis (wheel base ~13cm), TT motors:
At TURN_SPEED = 160:  wheel surface speed ≈ 18 cm/s
Pivot turn speed ≈ 2 × 18 / 13 ≈ 2.77 rad/s ≈ 159°/s

TURN_TIME = 700ms → 159°/s × 0.7s ≈ 111° (close to 90°, varies with surface)
TURN_TIME = 560ms → 159°/s × 0.56s ≈ 89° (near-perfect 90° turn)

Surface matters: carpet increases friction → slower actual wheel speed → larger TURN_TIME needed

Speed of sound vs. temperature:
  v = 331.3 + 0.606 × T_celsius  (m/s)

At 20°C: v = 331.3 + 12.1 = 343.4 m/s (0.03434 cm/µs)
At 35°C: v = 331.3 + 21.2 = 352.5 m/s (0.03525 cm/µs)
Difference: 2.6% faster at 35°C than 20°C

For a reading at 20cm from a wall:
  At 20°C: actual distance = 20cm, measured correctly
  At 35°C: code uses 0.0343, actual speed is 0.03525
           Measured = duration × 0.03434 / 2 = duration × 0.01717
           Actual = duration × 0.03525 / 2 = duration × 0.01763
           Error: (0.01717 - 0.01763) / 0.01763 = -2.6% (reads shorter than actual)
           At 20cm actual: measured ≈ 19.5cm → within 0.5cm → acceptable for rover

Temperature correction for precision applications:
  float speedOfSound = 0.0001 * (331.3 + 0.606 * temperature_C);  // cm/µs
  float distanceCm = (duration / 2.0) * speedOfSound;

Sensor field of view (top view):

                    [HC-SR04]
                       │
               ← 15° ─┼─ 15° →
              /         │         \
             /          │          \
            /           │           \
          detected zone             undetected zone

Objects more than 15° to either side of center won't reflect ultrasound
back to the sensor — robot appears to see "nothing" even if object is close.

Component current draw at operating conditions:

Arduino Uno (running sketch):              ~80 mA
L298N motor driver (quiescent, motors off): ~40 mA
HC-SR04 (active sensing):                  ~15 mA
Two TT motors (driving forward, moderate load): ~200–400 mA total
Two TT motors (stalled):                   ~800–1200 mA total
L298N internal dissipation (at 6V, 400mA): ~200 mA equivalent loss
                                            (voltage drop across L298N at 400mA)

Typical average current during operation:
  Forward driving: 80 + 40 + 15 + 300 = ~435 mA
  During avoidance: ~200 mA (briefer, at lower speeds)
  Combined average: ~380 mA

Battery runtime estimates:
  4× AA Alkaline (2500mAh, 6V nominal, ~80% efficiency in discharge):
    Runtime ≈ 2500 × 0.80 / 380 ≈ 5.3 hours theoretical
    Practical runtime: ~2–3 hours (voltage sag reduces efficiency)

  6× AA Alkaline (2500mAh, 9V nominal):
    Runtime ≈ 2500 × 0.80 / 380 ≈ 5.3 hours (same mAh, more voltage means L298N waste)
    Practical runtime: ~2–3 hours

  2S LiPo 1000mAh (7.4V nominal):
    Runtime ≈ 1000 × 0.85 / 380 ≈ 2.2 hours at C-rate consideration
    Practical runtime: ~1–1.5 hours (1000mAh is modest for this application)

  2S LiPo 2200mAh (7.4V nominal):
    Runtime ≈ 2200 × 0.85 / 380 ≈ 4.9 hours
    Practical runtime: ~3–4 hours — best option for extended operation

Note: The L298N has ~2V voltage drop across its output stage.
At 6V input, motors receive ~4V (reduced torque vs. rated 6V).
At 9V input, motors receive ~7V (slightly over-voltage but acceptable briefly).
At 7.4V LiPo input, motors receive ~5.4V (good operating point).

static bool turnDirection = true;