Line-following cart on an Arduino Uno

// Line-following cart: a five-channel line tracker across the front, two
// gearbox motors on a TB6612FNG, and two 18650 cells for the motors.
//
//   A4..A0  the tracker's S1..S5, left to right (high over the black line):
//           the board sits turned around under the Uno, so S1 lands on A4
//   pin 5   PWMA, the left motor's speed      pin 6  PWMB, the right motor's
//   pin 2   AIN1    pin 4  AIN2               pin 7  BIN1    pin 8  BIN2
//   pin 9   STBY
//   VM      the 2S pack's +, 8.4 V full; VCC the Uno's 5 V; all grounds joined
//
// # Where the line is
//
// The sensors are 15 mm apart. Each one over the tape counts its place, -2 at
// S1 to +2 at S5, and the line's position is the average of the ones that see
// it, in millimeters: 0 under S3, +7 between S3 and S4, +30 under S5.
//
// # Steering
//
// The error is that position. The cart slows the wheel on the side the line
// is on and speeds up the other, by KP x error plus KD x how fast the error
// is changing (the damping that stops it weaving). With the line to the right
// the left wheel runs faster, and the cart turns right, back over it.
//
// Lose the line and it keeps turning the way it last saw it. All five black
// at once is the finish bar across the track: it stops.
//
// # Speed and the battery
//
// Two full 18650s are 8.4 V and these are 6 V gearbox motors, so the sketch
// never asks for more than 180 of 255: about 5.9 V on average.

#include <SparkFun_TB6612.h>

// S1 (left) to S5 (right).
const int SENSORS[5] = {A4, A3, A2, A1, A0};

const int AIN1 = 2;
const int AIN2 = 4;
const int PWMA = 5;
const int BIN1 = 7;
const int BIN2 = 8;
const int PWMB = 6;
const int STBY = 9;

const int BASE = 120;
const int MAX_SPEED = 180;
const int SEARCH = 110;
// Steering gains: counts of PWM per mm of error, and per mm of change.
const int KP = 3;
const int KD = 6;

Motor leftMotor = Motor(AIN1, AIN2, PWMA, 1, STBY);
Motor rightMotor = Motor(BIN1, BIN2, PWMB, 1, STBY);

int lastError = 0;
bool finished = false;
unsigned long lastReport = 0;

int clampSpeed(int v) {
  if (v > MAX_SPEED) return MAX_SPEED;
  if (v < -MAX_SPEED) return -MAX_SPEED;
  return v;
}

void setup() {
  Serial.begin(115200);
  Serial.println("Mokxi Arduino Uno: line-following cart");
  for (int i = 0; i < 5; i++) pinMode(SENSORS[i], INPUT);
}

void report(const char *what, int error, int left, int right) {
  if (millis() - lastReport < 200) return;
  lastReport = millis();
  Serial.print(what);
  Serial.print(" ");
  Serial.print(error);
  Serial.print(" mm   left ");
  Serial.print(left);
  Serial.print("  right ");
  Serial.println(right);
}

void loop() {
  if (finished) return;

  int seen = 0;
  int sum = 0;
  for (int i = 0; i < 5; i++) {
    if (digitalRead(SENSORS[i]) == HIGH) {
      seen++;
      sum += (i - 2) * 15;
    }
  }

  if (seen == 5) {
    brake(leftMotor, rightMotor);
    finished = true;
    Serial.println("finish bar: stopped");
    return;
  }

  int left;
  int right;
  if (seen == 0) {
    // Lost it: turn toward where it was last.
    left = lastError >= 0 ? SEARCH : -SEARCH;
    right = -left;
    leftMotor.drive(left);
    rightMotor.drive(right);
    report("searching, last", lastError, left, right);
    delay(10);
    return;
  }

  int error = sum / seen;
  int turn = KP * error + KD * (error - lastError);
  lastError = error;
  left = clampSpeed(BASE + turn);
  right = clampSpeed(BASE - turn);
  leftMotor.drive(left);
  rightMotor.drive(right);
  report("line", error, left, right);
  delay(10);
}