Ultrasonic radar on an Arduino Uno

// Ultrasonic radar: an HC-SR04 on an SG90 servo sweeps from side to side and
// draws what it finds on a 128x64 OLED, like a radar screen.
//
//   servo     signal -> pin 3, VCC -> 5 V, GND -> GND
//   HC-SR04   TRIG -> pin 11, ECHO -> pin 10, VCC -> 5 V, GND -> GND
//   OLED      SDA -> A4, SCL -> A5, VCC -> 5 V, GND -> GND (address 0x3C)
//   piezo     pin 2 and GND: a short ping for anything closer than 20 cm
//
// The sensor sits on the servo horn. Every 3 degrees, from 15 to 165 and back,
// the servo turns, waits for the horn to get there, and the sensor measures.
// The sweep line on the screen points where the sensor points, and a dot is
// drawn at the distance it measured. A dot stays until the sweep comes back
// past it and finds nothing there.
//
// The Serial Monitor gets one line per step, "angle,distance." in centimeters,
// the format the well-known Processing radar sketch reads.

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

const uint8_t SERVO_PIN = 3;
const uint8_t TRIG = 11;
const uint8_t ECHO = 10;
const uint8_t PIEZO = 2;

const int MIN_ANGLE = 15;
const int MAX_ANGLE = 165;
const int STEP = 3;
const int STEPS = (MAX_ANGLE - MIN_ANGLE) / STEP + 1;  // 51
const int RANGE_CM = 50;  // the edge of the screen
const int CLOSE_CM = 20;  // closer than this pings

// The radar's center is the middle of the bottom edge; 60 pixels is 50 cm.
const int CX = 64;
const int CY = 63;
const int RADIUS = 60;

Adafruit_SSD1306 display(128, 64, &Wire, -1);
Servo turret;

// The last distance seen at each step, in cm; 0 is nothing in range.
uint8_t seen[STEPS];
int angle = MIN_ANGLE;
int direction = STEP;

long measureCm() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  // 30 ms is about 5 m and back: anything longer is nothing at all.
  unsigned long us = pulseIn(ECHO, HIGH, 30000UL);
  if (us == 0) return 0;
  // Sound covers 0.0343 cm a microsecond, there and back: 58 us a centimeter.
  return us / 58;
}

// Where a point `cm` away at `deg` lands on the screen. 0 degrees is the
// right-hand side, 90 straight up, 180 the left.
void polar(int deg, int cm, int &x, int &y) {
  float r = cm * (float)RADIUS / RANGE_CM;
  float a = deg * 0.0174533;
  x = CX + (int)(r * cos(a));
  y = CY - (int)(r * sin(a));
}

// The end of the sweep line on the screen last time, so it can be rubbed out.
int lineX = -1;
int lineY = 0;

void ring() {
  display.drawCircle(CX, CY, RADIUS / 2, SSD1306_WHITE);
  display.drawCircle(CX, CY, RADIUS, SSD1306_WHITE);
  display.drawFastHLine(0, CY, 128, SSD1306_WHITE);
}

// Change only what moved. The driver sends just the parts of the screen that
// changed, so rubbing out the old line and drawing the new one is far less
// to send than clearing the screen and drawing it all again.
void drawScreen(int i, uint8_t was, long cm) {
  int x, y;
  if (lineX >= 0) display.drawLine(CX, CY, lineX, lineY, SSD1306_BLACK);
  if (was != 0) {
    polar(MIN_ANGLE + i * STEP, was, x, y);
    display.fillCircle(x, y, 2, SSD1306_BLACK);
  }
  // the reading, top left, where the rings never reach
  display.fillRect(0, 0, 26, 17, SSD1306_BLACK);
  display.setCursor(0, 0);
  display.print(angle);
  display.print((char)247);  // the degree sign in the built-in font
  display.setCursor(0, 9);
  if (cm == 0 || cm > RANGE_CM) {
    display.print("none");
  } else {
    display.print(cm);
    display.print("cm");
  }
  // put back whatever the rubbing out took off the rings and the other echoes
  ring();
  for (int k = 0; k < STEPS; k++) {
    if (seen[k] == 0) continue;
    polar(MIN_ANGLE + k * STEP, seen[k], x, y);
    display.fillCircle(x, y, 2, SSD1306_WHITE);
  }
  polar(angle, RANGE_CM, lineX, lineY);
  display.drawLine(CX, CY, lineX, lineY, SSD1306_WHITE);
  display.display();
}

void setup() {
  Serial.begin(115200);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(PIEZO, OUTPUT);
  turret.attach(SERVO_PIN);
  turret.write(angle);
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("No OLED at 0x3C: check SDA and SCL");
    while (true) {
    }
  }
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  ring();
  display.display();
  delay(300);
  Serial.println("Radar sweeping");
}

void loop() {
  turret.write(angle);
  delay(20);  // an SG90 turns 3 degrees in about 5 ms; 20 leaves it settled
  long cm = measureCm();
  int i = (angle - MIN_ANGLE) / STEP;
  uint8_t was = seen[i];
  seen[i] = (cm > 0 && cm <= RANGE_CM) ? cm : 0;
  if (cm > 0 && cm < CLOSE_CM) tone(PIEZO, 1500, 25);
  drawScreen(i, was, cm);

  Serial.print(angle);
  Serial.print(',');
  Serial.print(cm);
  Serial.println('.');

  angle += direction;
  if (angle >= MAX_ANGLE || angle <= MIN_ANGLE) direction = -direction;
}