AI Desktop Companion Robot

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_BME280.h>
#include <Adafruit_PWMServoDriver.h>
#include <math.h>

// All external I2C logic and pullups are 3.3V. Motor V+ is a separate 5V rail.
constexpr int SDA_PIN=21, SCL_PIN=22, BUTTON_PIN=27, OE_PIN=25;
Adafruit_SSD1306 screen(128,64,&Wire,-1);
Adafruit_BME280 bme;
Adafruit_PWMServoDriver driver(0x40,Wire);
bool screenOK=false, sensorOK=false, driverOK=false, armed=false;
bool talking=false, mouthOpen=false, asleep=false, dirty=true;
float tempC=NAN, humidity=NAN;
String mood="happy", caption="Hello, friend!";
int pos[2]={90,90}, target[2]={90,90};
uint32_t lastPing=0, lastMotion=0, lastSensor=0, lastMouth=0;
uint32_t lastBlink=0, blinkEnd=0, lastButtonChange=0, armStart=0;
bool blink=false, buttonRaw=HIGH, buttonStable=HIGH;
char input[192]; size_t used=0; bool overflow=false;

bool present(uint8_t addr) { Wire.beginTransmission(addr); return Wire.endTransmission()==0; }
void stopMotion() { digitalWrite(OE_PIN,HIGH); armed=false; }
void ack(bool ok) { Serial.println(ok ? "{\"event\":\"ack\",\"ok\":true}" : "{\"event\":\"ack\",\"ok\":false}"); }
void drawFace() {
  if (!screenOK || !dirty) return;
  dirty=false; screen.clearDisplay(); screen.setTextColor(SSD1306_WHITE);
  screen.setTextSize(1); screen.setCursor(0,0);
  if (isfinite(tempC)) { screen.print(tempC,1); screen.print("C "); screen.print(humidity,0); screen.print("%"); }
  else screen.print("Sensor unavailable");
  bool shut=blink || asleep;
  int eyeH=shut?2:(mood=="surprise"?20:14);
  screen.fillRoundRect(22,18,24,eyeH,shut?1:5,SSD1306_WHITE);
  screen.fillRoundRect(82,18,24,eyeH,shut?1:5,SSD1306_WHITE);
  if (mood=="curious" || mood=="confused") screen.drawLine(80,13,107,17,SSD1306_WHITE);
  if (mood=="happy") { screen.drawLine(54,40,64,44,SSD1306_WHITE); screen.drawLine(64,44,74,40,SSD1306_WHITE); }
  else screen.drawLine(55,43,73,43,SSD1306_WHITE);
  if (talking && mouthOpen) screen.fillRoundRect(56,38,16,9,3,SSD1306_WHITE);
  if (asleep) { screen.setCursor(112,24); screen.print("z"); }
  screen.setCursor(0,49); screen.setTextWrap(true); screen.print(caption.substring(0,42));
  screen.display();
}
void handleLine(char *line) {
  if (!strcmp(line,"PING")) { lastPing=millis(); return; }
  if (!strcmp(line,"STOP")) { stopMotion(); ack(true); return; }
  if (!strcmp(line,"ARM 1")) {
    if (!driverOK) { ack(false); return; }
    // Center with horns REMOVED before attaching the head. No automatic arming.
    for (int i=0;i<2;i++) { pos[i]=target[i]=90; driver.writeMicroseconds(i,1500); }
    lastPing=armStart=millis(); digitalWrite(OE_PIN,LOW); armed=true; ack(true); return;
  }
  int a,b;
  if (sscanf(line,"MOVE %d %d",&a,&b)==2) {
    if (!armed) { ack(false); return; }
    target[0]=constrain(a,70,110); target[1]=constrain(b,80,100); ack(true); return;
  }
  if (!strncmp(line,"FACE ",5)) {
    String next=line+5;
    if (next!="happy" && next!="curious" && next!="surprise" && next!="confused" && next!="sleep") { ack(false); return; }
    mood=next; asleep=(mood=="sleep"); dirty=true; ack(true); return;
  }
  if (!strncmp(line,"SAY ",4)) { caption=String(line+4).substring(0,42); dirty=true; ack(true); return; }
  if (!strcmp(line,"TALK 1") || !strcmp(line,"TALK 0")) { talking=(line[5]=='1'); mouthOpen=false; dirty=true; ack(true); return; }
  ack(false);
}
void setup() {
  // OE high disables pulses. Physical motor switch must remain OFF at boot.
  digitalWrite(OE_PIN,HIGH); pinMode(OE_PIN,OUTPUT);
  pinMode(BUTTON_PIN,INPUT_PULLUP); Serial.begin(115200);
  Wire.begin(SDA_PIN,SCL_PIN); Wire.setClock(100000); Wire.setTimeOut(30);
  uint8_t addr=present(0x3C)?0x3C:(present(0x3D)?0x3D:0);
  if (addr) screenOK=screen.begin(SSD1306_SWITCHCAPVCC,addr,true,false);
  sensorOK=bme.begin(0x77,&Wire); if (!sensorOK) sensorOK=bme.begin(0x76,&Wire);
  if (present(0x40)) { driverOK=driver.begin(); if(driverOK) { driver.setPWMFreq(50); driver.setOutputMode(true); } }
  stopMotion();
  Serial.printf("{\"event\":\"ready\",\"protocol\":1,\"display\":%s,\"sensor\":%s,\"motion\":%s}\n",screenOK?"true":"false",sensorOK?"true":"false",driverOK?"true":"false");
}
void loop() {
  uint32_t now=millis();
  // Bound work per loop and reject overlong lines rather than parsing fragments.
  for (int n=0;n<128 && Serial.available();n++) {
    char c=Serial.read(); if(c=='\r') continue;
    if(c=='\n') { if(!overflow) { input[used]=0; handleLine(input); } else ack(false); used=0; overflow=false; }
    else if(used<sizeof(input)-1 && !overflow) input[used++]=c; else overflow=true;
  }
  if(armed && ((uint32_t)(now-lastPing)>2000 || (uint32_t)(now-armStart)>15000)) {
    stopMotion(); Serial.println("{\"event\":\"motion_timeout\"}");
  }
  if(armed && (uint32_t)(now-lastMotion)>=40) {
    lastMotion=now;
    for(int i=0;i<2;i++) if(pos[i]!=target[i]) {
      pos[i]+=target[i]>pos[i]?1:-1; // at most 25 degrees per second
      driver.writeMicroseconds(i,1500+(pos[i]-90)*10);
    }
  }
  bool raw=digitalRead(BUTTON_PIN);
  if(raw!=buttonRaw) { buttonRaw=raw; lastButtonChange=now; }
  if((uint32_t)(now-lastButtonChange)>35 && raw!=buttonStable) {
    buttonStable=raw;
    if(raw==LOW) { Serial.println("{\"event\":\"activate\"}"); mood="curious"; asleep=false; caption="Ready for input"; dirty=true; }
  }
  if((uint32_t)(now-lastSensor)>=2000) {
    lastSensor=now; float t=NAN,h=NAN;
    if(sensorOK) { t=bme.readTemperature(); h=bme.readHumidity(); }
    if(isfinite(t) && isfinite(h) && t>=-40 && t<=85 && h>=0 && h<=100) {
      if(!isfinite(tempC) || lroundf(t*10)!=lroundf(tempC*10) || lroundf(h)!=lroundf(humidity)) dirty=true;
      tempC=t; humidity=h;
      Serial.printf("{\"event\":\"environment\",\"c\":%.2f,\"humidity\":%.2f}\n",t,h);
    } else { if(isfinite(tempC)) dirty=true; tempC=humidity=NAN; Serial.println("{\"event\":\"sensor_error\"}"); }
  }
  if(!asleep && (uint32_t)(now-lastBlink)>=4500) { lastBlink=now; blinkEnd=now; blink=true; dirty=true; }
  if(blink && (uint32_t)(now-blinkEnd)>=130) { blink=false; dirty=true; }
  if(talking && (uint32_t)(now-lastMouth)>=180) { lastMouth=now; mouthOpen=!mouthOpen; dirty=true; }
  drawFace(); delay(2);
}