智慧生活科技專業社群: [OTTO Biped] OTTO樹藝森林精靈

Arduino程式：
/*   
*    Otto Starter + Web Bluetooth Controller
*    Add: Ultrasound interactive features (auto-avoid + stream distance)
*/

#include <Otto.h>
#include <EEPROM.h>

#define LEFTLEG 2
#define RIGHTLEG 3
#define LEFTFOOT 4
#define RIGHTFOOT 5

// 你指定的接腳
#define TRIG 9
#define ECHO 10

#define BLE_TX 11
#define BLE_RX 12
#define BUZZER 13

#if defined(ARDUINO_ARCH_ESP32)
  #include "BluetoothSerial.h"
  String device_name = "Forest_Elf_OTTO_20";
  BluetoothSerial bluetooth;
#else
  #include <SoftwareSerial.h>
  String device_name = "Forest_Elf_OTTO_20";
  SoftwareSerial bluetooth(BLE_TX, BLE_RX);
#endif

int move_speed[] = {3000, 2000, 1000, 750, 500, 250};
int n = 2;

int ultrasound_threeshold = 15; // cm
String command = "";

int v;
int i;
int positions[] = {90, 90, 90, 90};
int8_t trims[4] = {0,0,0,0};
unsigned long sync_time = 0;

bool calibration = false;
int isavailable = 1;

Otto Ottobot;

// ===== 新增：超音波互動狀態 =====
bool autoAvoidEnabled = false;     // BLE 指令開關: autoavoid_on / autoavoid_off
bool distanceStreamEnabled = false;// BLE 指令開關: us_stream_on / us_stream_off

unsigned long usMeasureDue = 0;
unsigned long usStreamDue  = 0;

const unsigned long US_MEASURE_INTERVAL_MS = 120; // 自動避障量測頻率
const unsigned long US_STREAM_INTERVAL_MS  = 250; // 距離串流回報頻率

// 防止 pulseIn 卡死：30ms 約可量到 5m (HC-SR04 常見)
long ultrasound_distance_once_cm() {
  long duration;
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  duration = pulseIn(ECHO, HIGH, 30000UL); // timeout 30,000 us
  if (duration == 0) return 999;           // 超時視為很遠
  long distance = duration / 58;           // us -> cm
  return distance;
}

// 簡單去雜訊：看 3 次取中位數
long ultrasound_distance_cm() {
  long a = ultrasound_distance_once_cm();
  long b = ultrasound_distance_once_cm();
  long c = ultrasound_distance_once_cm();

  // median of 3
  if ((a <= b && b <= c) || (c <= b && b <= a)) return b;
  if ((b <= a && a <= c) || (c <= a && a <= b)) return a;
  return c;
}

// ===== 新增：避障互動邏輯 =====
void ultrasoundInteractionTick() {
  unsigned long now = millis();

  // 1) 距離串流回報（不影響自動避障開關）
  if (distanceStreamEnabled && now >= usStreamDue) {
    usStreamDue = now + US_STREAM_INTERVAL_MS;
    long d = ultrasound_distance_cm();
    bluetooth.print("US:");
    bluetooth.println(d);
  }

  // 2) 自動避障
  if (!autoAvoidEnabled) return;
  if (now < usMeasureDue) return;
  usMeasureDue = now + US_MEASURE_INTERVAL_MS;

  long d = ultrasound_distance_cm();

  // 太近：做互動（停止/後退/轉向/表情/聲音）
  if (d <= ultrasound_threeshold) {
    // 先停
    Ottobot.home();
    Ottobot.sing(S_surprise);
    Ottobot.playGesture(OttoConfused);

    // 後退 2 步
    Ottobot.walk(2, move_speed[n], BACKWARD);

    // 左右掃描判斷轉向（用轉頭概念：實際用 turn 旋轉身體）
    // 左量測
    Ottobot.turn(1, 800, LEFT);
    long dl = ultrasound_distance_cm();

    // 右量測（回到原位再往右）
    Ottobot.turn(2, 800, RIGHT);
    long dr = ultrasound_distance_cm();

    // 回正（往左轉回來）
    Ottobot.turn(1, 800, LEFT);

    // 選較遠的方向轉開
    if (dl >= dr) {
      Ottobot.turn(2, move_speed[n], LEFT);
    } else {
      Ottobot.turn(2, move_speed[n], RIGHT);
    }

    // 回報一次距離（可選）
    bluetooth.print("AVOID:");
    bluetooth.println(d);

    // 避完障後不強制前進，交給你原本指令/模式控制
  }
}

void setup() {
  Serial.begin(9600);
  Ottobot.init(LEFTLEG, RIGHTLEG, LEFTFOOT, RIGHTFOOT, true, BUZZER);

  pinMode(TRIG, OUTPUT); 
  pinMode(ECHO, INPUT);

#if defined(ARDUINO_ARCH_ESP32)
  Serial.print("ESP32");
  bluetooth.begin(device_name);
#else
  Serial.print("No ESP32");
  Serial.print(device_name);
  bluetooth.begin(9600);
  bluetooth.print("AT+NAME" + device_name + "\r\n");
#endif

  Ottobot.home();
  v = 0;
}

void loop() {
  checkBluetooth();

  // 允許從序列埠輸入簡單指令測試
  if (Serial.available() > 0) {
    command = Serial.readStringUntil('\n');
    command.trim();
    Serial.print("Received: ");
    Serial.println(command);
  }

  // ===== 新增：超音波互動 tick（非阻塞）=====
  ultrasoundInteractionTick();

  // 既有命令處理
  if (command == "forward") {
    Forward();
  }
  else if (command == "backward") {
    Backward();
  }
  else if (command == "right") {
    Right();
  }
  else if (command == "left") {
    Left();
  }
  else if (command == "avoidance") {
    Avoidance();
  }
  else if (command == "force") {
    UseForce();
  }
}

void checkBluetooth() {
  char charBuffer[30]; // 放大一點避免新指令溢出
  
  if (bluetooth.available() > 0) {
    isavailable = 1;

    int numberOfBytesReceived = bluetooth.readBytesUntil('\n', charBuffer, 29);
    if (numberOfBytesReceived <= 0) return;

    charBuffer[numberOfBytesReceived] = '\0';
    Serial.print("Received by BLE: ");
    Serial.println(charBuffer);

    // 最後一個字元當作速度 index
    char last = charBuffer[numberOfBytesReceived - 1];
    if (last >= '0' && last <= '9') {
      n = last - '0';
      if (n < 0) n = 0;
      if (n > 5) n = 5;
    }

    // ===== 行走指令 =====
    if (strstr(charBuffer, "forward") == &charBuffer[0]) {
      command = "forward";
    }   
    else if (strstr(charBuffer, "backward") == &charBuffer[0]) {
      command = "backward";
    }
    else if (strstr(charBuffer, "right") == &charBuffer[0]) {
      command = "right";
    }
    else if (strstr(charBuffer, "left") == &charBuffer[0]) {
      command = "left";
    }
    else if (strstr(charBuffer, "stop") == &charBuffer[0]) {
      command = "stop";
      Stop();
    }

    // ===== 超音波：單次回報距離 =====
    else if (strstr(charBuffer, "ultrasound") == &charBuffer[0]) {
      Stop();
      long d = ultrasound_distance_cm();
      bluetooth.print("US:");
      bluetooth.println(d);
    }

    // ===== 新增：自動避障開關 =====
    else if (strstr(charBuffer, "autoavoid_on") == &charBuffer[0]) {
      autoAvoidEnabled = true;
      command = "";
      bluetooth.println("AUTOAVOID:ON");
    }
    else if (strstr(charBuffer, "autoavoid_off") == &charBuffer[0]) {
      autoAvoidEnabled = false;
      command = "";
      bluetooth.println("AUTOAVOID:OFF");
    }

    // ===== 新增：距離串流回報開關 =====
    else if (strstr(charBuffer, "us_stream_on") == &charBuffer[0]) {
      distanceStreamEnabled = true;
      usStreamDue = millis(); // 立即開始
      command = "";
      bluetooth.println("US_STREAM:ON");
    }
    else if (strstr(charBuffer, "us_stream_off") == &charBuffer[0]) {
      distanceStreamEnabled = false;
      command = "";
      bluetooth.println("US_STREAM:OFF");
    }

    // ===== 既有：模式 =====
    else if (strstr(charBuffer, "avoidance") == &charBuffer[0]) {
      command = "avoidance";
    }
    else if (strstr(charBuffer, "force") == &charBuffer[0]) {
      command = "force";
    }

    // ===== 表情 / 手勢指令區 =====
    else if (strstr(charBuffer, "happy") == &charBuffer[0]) {
      command = "";
      Ottobot.playGesture(OttoHappy);
    }
    else if (strstr(charBuffer, "superhappy") == &charBuffer[0]) {
      command = "";
      Ottobot.playGesture(OttoSuperHappy);
    }
    else if (strstr(charBuffer, "sad") == &charBuffer[0]) {
      command = "";
      Ottobot.playGesture(OttoSad);
    }
    else if (strstr(charBuffer, "sleeping") == &charBuffer[0]) {
      command = "";
      Ottobot.playGesture(OttoSleeping);
    }
    else if (strstr(charBuffer, "confused") == &charBuffer[0]) {
      command = "";
      Ottobot.playGesture(OttoConfused);
    }
    else if (strstr(charBuffer, "fretful") == &charBuffer[0]) {
      command = "";
      Ottobot.playGesture(OttoFretful);
    }
    else if (strstr(charBuffer, "love") == &charBuffer[0]) {
      command = "";
      Ottobot.playGesture(OttoLove);
    }
    else if (strstr(charBuffer, "angry") == &charBuffer[0]) {
      command = "";
      Ottobot.playGesture(OttoAngry);
    }
    else if (strstr(charBuffer, "magic") == &charBuffer[0]) {
      command = "";
      Ottobot.playGesture(OttoMagic);
    }
    else if (strstr(charBuffer, "wave") == &charBuffer[0]) {
      command = "";
      Ottobot.playGesture(OttoWave);
    }
    else if (strstr(charBuffer, "victory") == &charBuffer[0]) {
      command = "";
      Ottobot.playGesture(OttoVictory);
    }
    else if (strstr(charBuffer, "fail") == &charBuffer[0]) {
      command = "";
      Ottobot.playGesture(OttoFail);
    }
    else if (strstr(charBuffer, "fart") == &charBuffer[0]) {
      command = "";
      Ottobot.playGesture(OttoFart);
    }

    // 校正與測試
    else if (strstr(charBuffer, "C") == &charBuffer[0]) {
      if (calibration == false) {
        Ottobot._moveServos(10, positions);
        calibration = true;
        delay(50);
      } 
      command = "calibration";
      Calibration(String(charBuffer));
    }
    else if (strstr(charBuffer, "walk_test") == &charBuffer[0]) {
      command = "";
      Ottobot.walk(3, 1000, FORWARD);
    }
    else if (strstr(charBuffer, "save_calibration") == &charBuffer[0]) {
      command = "";
      readChar('s');
    }
  }
  else {
    if (isavailable == 1) {
      Serial.println("No Bluetooth is available.");
      isavailable = 0;
    }
  }
}

void Forward() {  Ottobot.walk(1, move_speed[n], FORWARD); }
void Backward(){  Ottobot.walk(1, move_speed[n], BACKWARD); }
void Right()   {  Ottobot.walk(1, move_speed[n], RIGHT); }
void Left()    {  Ottobot.walk(1, move_speed[n], LEFT); }
void Stop()    {  Ottobot.home(); }

void Avoidance() {
  if (ultrasound_distance_cm() <= ultrasound_threeshold) {
    Ottobot.playGesture(OttoConfused);
    for (int count=0 ; count<2 ; count++) {
      Ottobot.walk(1, move_speed[n], BACKWARD);
    }
    for (int count=0 ; count<4 ; count++) {
      Ottobot.turn(1, move_speed[n], LEFT);
    }
  }
  Ottobot.walk(1, move_speed[n], FORWARD);
}

void UseForce() {
  long d = ultrasound_distance_cm();
  if (d <= ultrasound_threeshold) {
    Ottobot.walk(1, move_speed[n], BACKWARD);
  }
  if ((d > 10) && (d < 15)) {
    Ottobot.home();
  }
  if ((d > 15) && (d < 30)) {
    Ottobot.walk(1, move_speed[n], FORWARD);
  }
  if (d > 30) {
    Ottobot.home();
  }  
}

void Settings(String ts_ultrasound) { ultrasound_threeshold = ts_ultrasound.toInt(); }

void Calibration(String c) {
  if (sync_time < millis()) {
    sync_time = millis() + 50;
    for (int k = 1; k < c.length(); k++) {
      readChar((c[k]));
    }
  } 
}

void readChar(char ch) {
  switch (ch) {
  case '0'...'9':
    v = (v * 10 + ch) - 48;
    break;
  case 'a':
    trims[0] = v-90; setTrims(); v = 0; break;
  case 'b':
    trims[1] = v-90; setTrims(); v = 0; break;
  case 'c':
    trims[2] = v-90; setTrims(); v = 0; break;
  case 'd':
    trims[3] = v-90; setTrims(); v = 0; break;
  case 's':
    for (i=0 ; i<=3 ; i=i+1) EEPROM.write(i,trims[i]);
    delay(500);
    Ottobot.sing(S_superHappy);
    Ottobot.crusaito(1, 1000, 25, -1);
    Ottobot.crusaito(1, 1000, 25, 1);
    Ottobot.sing(S_happy_short);
    break;
  }
}

void setTrims() {
  Ottobot.setTrims(trims[0],trims[1],trims[2],trims[3]);
  Ottobot._moveServos(10, positions); 
}
智慧生活科技專業社群

2026年2月6日星期五

[OTTO Biped] OTTO樹藝森林精靈

沒有留言:

張貼留言

2026年2月6日 星期五

[OTTO Biped] OTTO樹藝森林精靈

沒有留言:

張貼留言

2026年2月6日星期五
