Wheel-encoder square driver on an Arduino Uno

// Wheel-encoder square driver: closed-loop speed and distance by counting.
//
//   Slot sensors on the chassis: left ENC on pin 3, right ENC on pin 2
//   (VCC to 5V, GND to ground), 20 pulses for every turn of a wheel
//   L9110 driver: IA pin 5, IB pin 6 (motor A, left)   B-IA pin 9, B-IB pin 10 (right motor)
//
// # Counting
//
// Each slot that passes the sensor is one rising edge, and an interrupt adds
// one to a counter. With 65 mm wheels one turn is 204 mm, so one pulse is a bit
// more than 10 mm of floor. That is the whole of odometry: distance is pulses
// times 10.2 mm, and the two wheels' counts tell the robot how far it has gone
// and, from the difference, how far it has turned.
//
// # Feedback
//
// With no feedback, "PWM 150" gives a different speed on a fresh battery than
// on a flat one. This robot asks the wheel instead. Every 250 ms it counts the
// pulses each wheel made and compares them with the target. If a wheel is too
// slow its PWM goes up by a little; too fast, down. That is an integral
// controller, and each wheel has its own, so two motors that are not quite the
// same still run at the same speed.
//
// # The square
//
// Four sides of about 580 mm, each a count of pulses, and four corners. A
// corner is a spin on the spot: the left wheel back and the right wheel
// forward, until each has counted the pulses that a quarter turn of a 130 mm
// track takes, 10 of travel in all.
// The wheels coast a little after the PWM stops, so the sketch brakes by
// holding both inputs low, which this driver treats as a brake.

const int ENC_LEFT = 3;
const int ENC_RIGHT = 2;
const int A_IA = 5;
const int A_IB = 6;
const int B_IA = 9;
const int B_IB = 10;

const unsigned long WINDOW_MS = 250;
// Pulses per window the speed loop aims for: 32 a second, a bit over 300 mm/s.
const int TARGET = 8;
const int GAIN = 6;

// The robot is still rolling when the brake goes on, about 5 pulses' worth at
// this speed, so 52 counted is about 57 on the floor: 580 mm a side. A corner
// is 10 pulses of wheel travel, less the 2 or so it spins on after the brake.
const int SIDE_PULSES = 52;
const int CORNER_PULSES = 8;
const int SPIN_PWM = 150;
const int FIRST_GUESS = 130;

volatile unsigned int leftCount = 0;
volatile unsigned int rightCount = 0;

void onLeft() { leftCount++; }
void onRight() { rightCount++; }

int leftPwm = FIRST_GUESS;
int rightPwm = FIRST_GUESS;

unsigned int sideStartLeft = 0;
unsigned int sideStartRight = 0;
unsigned int lastLeft = 0;
unsigned int lastRight = 0;
unsigned long lastWindow = 0;
int sides = 0;
bool done = false;

void motor(int ia, int ib, int speed) {
  if (speed >= 0) {
    analogWrite(ib, 0);
    analogWrite(ia, speed);
  } else {
    analogWrite(ia, 0);
    analogWrite(ib, -speed);
  }
}

void drive(int left, int right) {
  motor(A_IA, A_IB, left);
  motor(B_IA, B_IB, right);
}

int clampPwm(int v) {
  if (v < 40) return 40;
  if (v > 255) return 255;
  return v;
}

void readCounts(unsigned int &l, unsigned int &r) {
  noInterrupts();
  l = leftCount;
  r = rightCount;
  interrupts();
}

// Spin on the spot until each wheel has counted `pulses`, then brake.
void corner(int pulses) {
  unsigned int l0, r0, l, r;
  readCounts(l0, r0);
  drive(0, 0);
  delay(150);
  drive(-SPIN_PWM, SPIN_PWM);
  unsigned long started = millis();
  do {
    readCounts(l, r);
  } while (((int)(l - l0) < pulses || (int)(r - r0) < pulses) && millis() - started < 3000);
  drive(0, 0);
  delay(300);
}

void setup() {
  pinMode(A_IA, OUTPUT);
  pinMode(A_IB, OUTPUT);
  pinMode(B_IA, OUTPUT);
  pinMode(B_IB, OUTPUT);
  pinMode(ENC_LEFT, INPUT);
  pinMode(ENC_RIGHT, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENC_LEFT), onLeft, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC_RIGHT), onRight, RISING);
  Serial.begin(115200);
  Serial.println("Mokxi Arduino Uno: wheel-encoder square driver");
  Serial.println("ready: driving a square, 4 sides of about 580 mm");
  readCounts(sideStartLeft, sideStartRight);
  lastLeft = sideStartLeft;
  lastRight = sideStartRight;
  lastWindow = millis();
  drive(leftPwm, rightPwm);
}

void loop() {
  if (done) return;

  if (millis() - lastWindow >= WINDOW_MS) {
    lastWindow += WINDOW_MS;
    unsigned int l, r;
    readCounts(l, r);
    int leftSpeed = (int)(l - lastLeft);
    int rightSpeed = (int)(r - lastRight);
    lastLeft = l;
    lastRight = r;
    leftPwm = clampPwm(leftPwm + GAIN * (TARGET - leftSpeed));
    rightPwm = clampPwm(rightPwm + GAIN * (TARGET - rightSpeed));
    drive(leftPwm, rightPwm);
    Serial.print("pulses ");
    Serial.print(leftSpeed);
    Serial.print(" ");
    Serial.print(rightSpeed);
    Serial.print("  pwm ");
    Serial.print(leftPwm);
    Serial.print(" ");
    Serial.println(rightPwm);
  }

  unsigned int l, r;
  readCounts(l, r);
  if (((int)(l - sideStartLeft) + (int)(r - sideStartRight)) / 2 >= SIDE_PULSES) {
    drive(0, 0);
    sides++;
    Serial.print("side ");
    Serial.print(sides);
    Serial.println(" done");
    corner(CORNER_PULSES);
    if (sides >= 4) {
      Serial.println("square complete");
      done = true;
      return;
    }
    readCounts(sideStartLeft, sideStartRight);
    lastLeft = sideStartLeft;
    lastRight = sideStartRight;
    lastWindow = millis();
    leftPwm = FIRST_GUESS;
    rightPwm = FIRST_GUESS;
    drive(leftPwm, rightPwm);
  }
}