Mars rover on an Arduino Uno

// Mars rover: drive to waypoints, avoid hazards, take samples, report home.
//
//   pins 10..5   L298N: ENA, IN1, IN2, IN3, IN4, ENB (the ENA and ENB
//                jumpers off, so 10 and 5 set the speed with PWM)
//   pins 3, 2    wheel encoders, left and right: 20 slots, 40 edges a turn
//   A0           mast servo, on a 5 V supply of its own: 90 looks ahead,
//                150 left, 30 right
//   A1           soil moisture probe
//   A2           light sensor (the LDR module's AO: brighter reads lower)
//   A3           START button
//   A4, A5       the I2C bus: the VL53L0X hazard sensor on the mast at 0x29,
//                and the SSD1306 OLED mission log at 0x3C
//
// # Knowing where you are
//
// A rover on Mars has no GPS. It counts wheel turns and works out where that
// must have put it, which is called dead reckoning (or odometry). Each slot
// edge is a 65 mm wheel turning a fortieth of a turn: 5.1 mm of ground. The
// difference between the two wheels, over the 130 mm between them, is how far
// the rover has turned. Add it all up and you have x, y and a heading.
//
// # The mission
//
// Three waypoints, in millimeters from where the rover starts (x ahead, y to
// its left), then home. At each one it stops, reads the soil and the light,
// and logs the sample. Between them it turns to face the next point and
// drives, steering to hold the bearing.
//
// # Hazards
//
// If the laser rangefinder sees something closer than 25 cm, the rover stops, backs off,
// looks left and right with the mast, turns away from the closer side and
// drives clear. Dead reckoning means it still knows where the waypoint is, so
// it simply turns back toward it. In Mokxi the VL53L0X reads its own slider
// wherever the mast points, so drag the slider to put a rock in the way.
//
// Press START to begin.

#include <Wire.h>
#include <Servo.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <VL53L0X.h>

const int ENA = 10, IN1 = 9, IN2 = 8, IN3 = 7, IN4 = 6, ENB = 5;
const int ENC_L = 3, ENC_R = 2;
const int PIN_MAST = A0;
const int PIN_SOIL = A1;
const int PIN_LIGHT = A2;
const int PIN_START = A3;

const float MM_PER_EDGE = 3.14159 * 65.0 / 40.0;
const float TRACK_MM = 130.0;
const int HAZARD_MM = 250;
const int DRIVE_PWM = 170;
const int TURN_PWM = 150;

struct Waypoint {
  int x, y;
};
const Waypoint ROUTE[] = {{700, 0}, {700, 500}, {150, 500}, {0, 0}};
const int STOPS = 4;          // the last one is home, where nothing is sampled

Adafruit_SSD1306 oled(128, 64, &Wire, -1);
Servo mast;
VL53L0X ranger;

volatile unsigned int edgesL = 0, edgesR = 0;
int dirL = 0, dirR = 0;
float x = 0, y = 0, heading = 0;

enum Mode { WAITING, TURNING, DRIVING, AVOIDING, SAMPLING, DONE };
Mode mode = WAITING;
int target = 0;
int samples = 0;
int hazards = 0;
unsigned long modeAt = 0;

char lines[4][22];
bool dirty = true;     // the log has news the screen has not shown

void countLeft() { edgesL++; }
void countRight() { edgesR++; }

// --- the mission log, on the OLED and the serial monitor -----------------

void logLine(const char *text) {
  Serial.println(text);
  for (int i = 0; i < 3; i++) memcpy(lines[i], lines[i + 1], sizeof(lines[0]));
  strncpy(lines[3], text, sizeof(lines[0]) - 1);
  lines[3][sizeof(lines[0]) - 1] = 0;
  dirty = true;
}

void drawScreen() {
  oled.clearDisplay();
  oled.setTextColor(SSD1306_WHITE);
  oled.setCursor(0, 0);
  oled.print("ROVER WP ");
  oled.print(target < STOPS - 1 ? target + 1 : STOPS - 1);
  oled.print("/");
  oled.print(STOPS - 1);
  oled.print(" S");
  oled.print(samples);
  oled.setCursor(0, 10);
  oled.print("x");
  oled.print((int)x);
  oled.print(" y");
  oled.print((int)y);
  oled.print(" h");
  oled.print((int)(heading * 57.3));
  oled.drawFastHLine(0, 20, 128, SSD1306_WHITE);
  for (int i = 0; i < 4; i++) {
    oled.setCursor(0, 24 + i * 10);
    oled.print(lines[i]);
  }
  oled.display();
}

// --- motors and odometry -----------------------------------------------------

void drive(int left, int right) {
  dirL = left > 0 ? 1 : left < 0 ? -1 : 0;
  dirR = right > 0 ? 1 : right < 0 ? -1 : 0;
  digitalWrite(IN1, left > 0);
  digitalWrite(IN2, left < 0);
  digitalWrite(IN3, right > 0);
  digitalWrite(IN4, right < 0);
  analogWrite(ENA, abs(left));
  analogWrite(ENB, abs(right));
}

void stopMotors() { drive(0, 0); }

// Dead reckoning: fold the edges counted since last time into x, y, heading.
void updatePose() {
  noInterrupts();
  unsigned int l = edgesL, r = edgesR;
  edgesL = 0;
  edgesR = 0;
  interrupts();
  float dl = l * MM_PER_EDGE * dirL;
  float dr = r * MM_PER_EDGE * dirR;
  heading += (dr - dl) / TRACK_MM;
  float d = (dl + dr) / 2;
  x += d * cos(heading);
  y += d * sin(heading);
}

// The smallest signed angle from a to b, in radians.
float angleTo(float a, float b) {
  float d = b - a;
  while (d > 3.14159) d -= 6.28318;
  while (d < -3.14159) d += 6.28318;
  return d;
}

float bearingTo(int i) { return atan2(ROUTE[i].y - y, ROUTE[i].x - x); }
float distanceTo(int i) {
  float dx = ROUTE[i].x - x, dy = ROUTE[i].y - y;
  return sqrt(dx * dx + dy * dy);
}

// --- the mast --------------------------------------------------------------------

int rangeMm() {
  uint16_t mm = ranger.readRangeSingleMillimeters();
  if (ranger.timeoutOccurred() || mm >= 8190) return 8000;   // nothing in range
  return mm;
}

int lookAt(int degrees) {
  mast.write(degrees);
  mast.hold(350);
  return rangeMm();
}

// --- the mission -------------------------------------------------------------------

void enter(Mode next) {
  mode = next;
  modeAt = millis();
}

void takeSample() {
  int soilRaw = analogRead(PIN_SOIL);
  int lightRaw = analogRead(PIN_LIGHT);
  // The probe reads about 520 in air and 260 in water.
  int soil = constrain(map(soilRaw, 520, 260, 0, 100), 0, 100);
  int light = 100 - (int)((long)lightRaw * 100 / 1023);
  samples++;
  char text[22];
  snprintf(text, sizeof(text), "S%d soil%d%% lt%d%%", samples, soil, light);
  logLine(text);
}

void startMission() {
  logLine("MISSION START");
  target = 0;
  enter(TURNING);
}

void setup() {
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(ENC_L, INPUT);
  pinMode(ENC_R, INPUT);
  pinMode(PIN_START, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENC_L), countLeft, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENC_R), countRight, CHANGE);
  Serial.begin(115200);
  mast.attach(PIN_MAST);
  mast.write(90);
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) Serial.println("no OLED at 0x3C");
  ranger.setTimeout(100);
  if (!ranger.init()) Serial.println("no VL53L0X at 0x29");
  logLine("ROVER ONLINE");
  logLine("press START");
  drawScreen();
}

void loop() {
  mast.refresh();
  updatePose();

  switch (mode) {
    case WAITING:
      if (digitalRead(PIN_START) == LOW) startMission();
      break;

    case TURNING: {
      float error = angleTo(heading, bearingTo(target));
      // Slow down as the heading comes round, so it does not overshoot.
      int pwm = constrain(90 + (int)(fabs(error) * 120), 90, TURN_PWM);
      if (fabs(error) < 0.06) {
        stopMotors();
        enter(DRIVING);
      } else if (error > 0) {
        drive(-pwm, pwm);              // spin left on the spot
      } else {
        drive(pwm, -pwm);
      }
      break;
    }

    case DRIVING: {
      if (distanceTo(target) < 40) {
        stopMotors();
        if (target == STOPS - 1) {
          char text[22];
          snprintf(text, sizeof(text), "HOME S%d H%d", samples, hazards);
          logLine(text);
          logLine("MISSION COMPLETE");
          enter(DONE);
        } else {
          char text[22];
          snprintf(text, sizeof(text), "AT WAYPOINT %d", target + 1);
          logLine(text);
          enter(SAMPLING);
        }
        break;
      }
      // The rangefinder takes a while to measure, so look ten times a second.
      static unsigned long lookedAt = 0;
      int range = 8000;
      if (millis() - lookedAt >= 100) {
        lookedAt = millis();
        range = rangeMm();
      }
      if (range < HAZARD_MM) {
        stopMotors();
        hazards++;
        char text[22];
        snprintf(text, sizeof(text), "HAZARD %d cm", range / 10);
        logLine(text);
        enter(AVOIDING);
        break;
      }
      // Steer to hold the bearing: slow the wheel on the side to turn toward.
      float error = angleTo(heading, bearingTo(target));
      if (fabs(error) > 0.5) {
        enter(TURNING);
        break;
      }
      int speed = distanceTo(target) < 150 ? 110 : DRIVE_PWM;   // ease in
      int trim = (int)(error * 150);
      drive(speed - trim, speed + trim);
      break;
    }

    case AVOIDING: {
      // Back off, look both ways, and turn away from the closer side.
      drive(-DRIVE_PWM, -DRIVE_PWM);
      delay(400);
      stopMotors();
      updatePose();
      int left = lookAt(150);
      int right = lookAt(30);
      mast.write(90);
      bool goLeft = left >= right;
      logLine(goLeft ? "clear to the left" : "clear to the right");
      float aim = heading + (goLeft ? 1.0 : -1.0);
      while (fabs(angleTo(heading, aim)) > 0.08) {
        if (goLeft) drive(-TURN_PWM, TURN_PWM);
        else drive(TURN_PWM, -TURN_PWM);
        delay(10);
        updatePose();
      }
      drive(DRIVE_PWM, DRIVE_PWM);
      delay(700);
      stopMotors();
      updatePose();
      enter(TURNING);
      break;
    }

    case SAMPLING:
      // Lower the mast toward the ground for the reading, then stand it up.
      if (millis() - modeAt < 50) mast.write(150);
      if (millis() - modeAt > 1500) {
        takeSample();
        mast.write(90);
        target++;
        enter(TURNING);
      }
      break;

    case DONE:
      break;
  }

  // The screen takes a good part of a tenth of a second to send, which is too
  // long to look away while the wheels turn, so it is redrawn at the stops.
  if (dirty && (mode == SAMPLING || mode == DONE || mode == WAITING)) {
    dirty = false;
    drawScreen();
  }
  delay(5);
}