App-Controlled Animated Head

#include <Arduino.h>
#include <Arduino_GFX_Library.h>
#include "driver/i2s.h"
#include <math.h>

#define TFT_SCK 7
#define TFT_MOSI 9
#define TFT_CS 44
#define TFT_DC 1
#define TFT_RST 2
#define PAN_SERVO_PIN 3
#define TILT_SERVO_PIN 4
#define I2S_BCLK 5
#define I2S_LRC 6
#define I2S_DIN 43
#define SERIAL_BAUD 115200
#define AUDIO_SAMPLE_RATE 16000
#define SERVO_FREQUENCY 50
#define SERVO_RESOLUTION 16
#define SERVO_MIN_US 500
#define SERVO_MAX_US 2500

Arduino_DataBus *displayBus = new Arduino_ESP32SPI(TFT_DC, TFT_CS, TFT_SCK, TFT_MOSI, GFX_NOT_DEFINED);
Arduino_GFX *gfx = new Arduino_ST7789(displayBus, TFT_RST, 0, true);

int panAngle = 90;
int tiltAngle = 90;
String faceName = "NEUTRAL";
String commandLine;

uint32_t servoDutyFromAngle(int angle) {
  angle = constrain(angle, 20, 160);
  const uint32_t pulseUs = map(angle, 0, 180, SERVO_MIN_US, SERVO_MAX_US);
  return (pulseUs * 65535UL) / 20000UL;
}

void writeServo(int pin, int angle) {
  ledcWrite(pin, servoDutyFromAngle(angle));
}

void setPan(int angle) {
  panAngle = constrain(angle, 20, 160);
  writeServo(PAN_SERVO_PIN, panAngle);
}

void setTilt(int angle) {
  tiltAngle = constrain(angle, 20, 160);
  writeServo(TILT_SERVO_PIN, tiltAngle);
}

void drawEye(int x, int y, bool happy, bool sad) {
  const uint16_t eyeColor = 0xFFFF;
  if (happy) {
    gfx->fillCircle(x, y + 12, 34, eyeColor);
    gfx->fillRect(x - 40, y - 30, 80, 36, 0x0000);
  } else if (sad) {
    gfx->fillCircle(x, y - 12, 34, eyeColor);
    gfx->fillRect(x - 40, y - 6, 80, 36, 0x0000);
  } else {
    gfx->fillRoundRect(x - 30, y - 44, 60, 88, 28, eyeColor);
  }
}

void drawFace() {
  const bool happy = faceName == "HAPPY";
  const bool sad = faceName == "SAD";
  gfx->fillScreen(0x0000);
  drawEye(92, 165, happy, sad);
  drawEye(228, 165, happy, sad);
  if (happy) {
    gfx->drawArc(160, 285, 76, 52, 20, 160, 0xFFE0);
    gfx->drawArc(160, 286, 75, 51, 20, 160, 0xFFE0);
  } else if (sad) {
    gfx->drawArc(160, 350, 76, 52, 200, 340, 0xFFE0);
    gfx->drawArc(160, 349, 75, 51, 200, 340, 0xFFE0);
  } else {
    gfx->fillRoundRect(108, 300, 104, 16, 8, 0xFFE0);
  }
}

void initialiseAudio() {
  i2s_config_t config = {};
  config.mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX);
  config.sample_rate = AUDIO_SAMPLE_RATE;
  config.bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT;
  config.channel_format = I2S_CHANNEL_FMT_ONLY_LEFT;
  config.communication_format = I2S_COMM_FORMAT_STAND_I2S;
  config.dma_buf_count = 8;
  config.dma_buf_len = 64;
  config.tx_desc_auto_clear = true;
  i2s_pin_config_t pins = {};
  pins.bck_io_num = I2S_BCLK;
  pins.ws_io_num = I2S_LRC;
  pins.data_out_num = I2S_DIN;
  pins.data_in_num = I2S_PIN_NO_CHANGE;
  i2s_driver_install(I2S_NUM_0, &config, 0, nullptr);
  i2s_set_pin(I2S_NUM_0, &pins);
  i2s_zero_dma_buffer(I2S_NUM_0);
}

void playBeep(uint16_t frequency = 660, uint16_t durationMs = 140) {
  const int samples = AUDIO_SAMPLE_RATE * durationMs / 1000;
  int16_t buffer[128];
  int made = 0;
  while (made < samples) {
    const int count = min(128, samples - made);
    for (int i = 0; i < count; ++i) {
      const float phase = 2.0f * PI * frequency * (made + i) / AUDIO_SAMPLE_RATE;
      buffer[i] = (int16_t)(sinf(phase) * 10000.0f);
    }
    size_t written = 0;
    i2s_write(I2S_NUM_0, buffer, count * sizeof(int16_t), &written, portMAX_DELAY);
    made += count;
  }
}

void sendState() {
  Serial.printf("STATE PAN=%d TILT=%d FACE=%s\n", panAngle, tiltAngle, faceName.c_str());
}

void handleCommand(String line) {
  line.trim();
  line.toUpperCase();
  if (line.startsWith("PAN=")) {
    setPan(line.substring(4).toInt());
    Serial.printf("OK PAN=%d\n", panAngle);
  } else if (line.startsWith("TILT=")) {
    setTilt(line.substring(5).toInt());
    Serial.printf("OK TILT=%d\n", tiltAngle);
  } else if (line.startsWith("FACE=")) {
    const String requestedFace = line.substring(5);
    if (requestedFace == "NEUTRAL" || requestedFace == "HAPPY" || requestedFace == "SAD") {
      if (requestedFace != faceName) {
        faceName = requestedFace;
        drawFace();
      }
      Serial.printf("OK FACE=%s\n", faceName.c_str());
    } else {
      Serial.println("ERR FACE must be NEUTRAL, HAPPY, or SAD");
    }
  } else if (line == "SOUND=BEEP") {
    playBeep();
    Serial.println("OK SOUND=BEEP");
  } else if (line == "STATE?") {
    sendState();
  } else if (line.length() > 0) {
    Serial.println("ERR Use PAN=20..160, TILT=20..160, FACE=NEUTRAL|HAPPY|SAD, SOUND=BEEP, or STATE?");
  }
}

void setup() {
  Serial.begin(SERIAL_BAUD);
  delay(300);
  ledcAttach(PAN_SERVO_PIN, SERVO_FREQUENCY, SERVO_RESOLUTION);
  ledcAttach(TILT_SERVO_PIN, SERVO_FREQUENCY, SERVO_RESOLUTION);
  setPan(panAngle);
  setTilt(tiltAngle);
  gfx->begin();
  drawFace();
  initialiseAudio();
  Serial.println("HEAD READY");
  sendState();
}

void loop() {
  while (Serial.available()) {
    const char incoming = (char)Serial.read();
    if (incoming == '\n' || incoming == '\r') {
      if (commandLine.length() > 0) {
        handleCommand(commandLine);
        commandLine = "";
      }
    } else if (commandLine.length() < 80) {
      commandLine += incoming;
    }
  }
  delay(1);
}