智慧生活科技專業社群: [OTTO Ninja]OTTO忍者機器人加入自動避障和音效

Arduino程式：
#include <ESP8266WiFi.h>
#include <ESP8266WebServer.h>
#include <WebSocketsServer.h>
#include <Servo.h>

/* ========== WiFi AP ========== */
const char* ssid = "OTTO NINJA";
const char* password = "12345678";

/* ========== Web ========== */
ESP8266WebServer server(80);
WebSocketsServer webSocket(81);

/* ========== Servo pulse range（SG90 建議較保守，避免頂住抖動/重啟） ========== */
const int SERVO_MIN_US = 580;
const int SERVO_MAX_US = 2350;

/* ========== Calibration SETTINGS（沿用你原本） ========== */
// Feet forward/back speed（保留：ROLL 模式仍使用）
int LFFWRS= 20;
int RFFWRS= 20;
int LFBWRS= 20;
int RFBWRS= 20;

// Walk stand (legs)
int LA0= 60;
int RA0= 120;

// Walk tilt positions
int LATL= 100;
int RATL= 175;
int LATR= 5;
int RATR= 80;

// Roll (legs)
int LA1= 168;
int RA1= 8;

// Mode: 0=WALK, 1=ROLL
int ModeCounter = 0;

/* ========== Pinout ========== */
const uint8_t ServoLeftFootPin   = 13; // D7
const uint8_t ServoLeftLegPin    = 15; // D8
const uint8_t ServoRightFootPin  = 0;  // D3
const uint8_t ServoRightLegPin   = 2;  // D4

// 手/頭腳位保留宣告，但程式不再 attach 它們（避免 RX/TX 干擾）
const uint8_t ServoLeftArmPin    = 16; // D0
const uint8_t ServoRightArmPin   = 3;  // RX (GPIO3)
const uint8_t ServoHeadPin       = 1;  // TX (GPIO1)

/* === 互動腳位 === */
const uint8_t BUZZER_PIN  = 14; // D5 (GPIO14) 無源蜂鳴器（上電安靜 -> 可 tone）
const uint8_t HEAD_SW_PIN = 12; // D6 (GPIO12) 頭部開關（建議接 GND，使用 INPUT_PULLUP）
const uint8_t US_TRIG_PIN = 5;  // D1 (GPIO5)
const uint8_t US_ECHO_PIN = 4;  // D2 (GPIO4)

Servo myservoLeftFoot;
Servo myservoLeftLeg;
Servo myservoRightFoot;
Servo myservoRightLeg;

Servo myservoLeftArm;
Servo myservoRightArm;
Servo myservoHead;

/* ========== Web 控制輸入 ========== */
int CTRL_JX = 0;   // -100..100
int CTRL_JY = 0;   // -100..100

/* ========== 超音波/互動狀態 ========== */
volatile bool autoAvoid = false;      // 自動避障（先預設關）
int usDistanceCm = 999;              // 最新距離
const int AVOID_CM = 8;              // 避障門檻（可自行調整）
const unsigned long US_PERIOD_MS = 120; // 量測週期
unsigned long lastUsMs = 0;

/* ========== 頭部開關去彈跳 ========== */
bool headSwStable = true;            // INPUT_PULLUP: true=未按, false=按下
bool headSwLastRead = true;
unsigned long headSwLastChangeMs = 0;
const unsigned long DEBOUNCE_MS = 30;
bool headSwPressedEdge = false;      // 本輪偵測到「按下邊緣」

/* ========== HTML Page（加入：距離/開關/蜂鳴/避障開關） ========== */
const char MAIN_page[] PROGMEM = R"=====(<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8">
<meta name="viewport" content="width=device-width,initial-scale=1">
<title>OTTO NINJA Web</title>
<style>
  body{font-family:Arial;text-align:center;background:#f4f6f8;margin:0;padding:16px;}
  .card{max-width:560px;margin:0 auto;background:#fff;border-radius:14px;padding:16px;box-shadow:0 8px 24px rgba(0,0,0,.08);}
  button{width:150px;height:54px;font-size:16px;margin:6px;border:none;border-radius:12px;background:#2d7ef7;color:#fff}
  button.gray{background:#6c757d}
  button.green{background:#00a86b}
  button.orange{background:#ff8c00}
  button.red{background:#d9534f}
  .row{display:flex;justify-content:center;flex-wrap:wrap}
  .box{margin:10px;padding:8px;border-radius:8px;font-weight:bold;background:#eee;}
  .small{font-size:12px;color:#666;line-height:1.6}

  .joystick-wrap{
    margin:20px auto;
    width:220px;
    height:220px;
    position:relative;
    user-select:none;
    -webkit-user-select:none;
  }
  .joystick-base{
    width:220px;
    height:220px;
    border-radius:50%;
    background:#ddd;
    position:absolute;
    left:0;
    top:0;
  }
  .joystick-stick{
    width:86px;
    height:86px;
    border-radius:50%;
    background:#2d7ef7;
    position:absolute;
    left:67px;
    top:67px;
    touch-action:none;
  }
</style>
</head>
<body>
<div class="card">
  <h2>OTTO NINJA（Web 控制）</h2>

  <div id="statusBox" class="box">狀態：未連線</div>
  <div id="modeBox" class="box">模式：未知</div>
  <div id="actBox"  class="box">動作：未知</div>

  <div id="distBox" class="box">距離：-- cm</div>
  <div id="headBox" class="box">頭部開關：--</div>
  <div id="autoBox" class="box">自動避障：--</div>

  <div class="row">
    <button class="gray" onclick="connectWS()">連線</button>
  </div>

  <div class="row">
    <button class="green" onclick="sendCmd('mode_walk')">Walk（人形）</button>
    <button class="green" onclick="sendCmd('mode_roll')">Roll（車形）</button>
  </div>

  <div class="row">
    <button class="orange" onclick="sendCmd('beep 1')">可愛音效 1</button>
    <button class="orange" onclick="sendCmd('beep 2')">可愛音效 2</button>
  </div>

  <div class="row">
    <button class="red" onclick="sendCmd('auto_toggle')">切換自動避障</button>
  </div>

  <div class="joystick-wrap">
    <div class="joystick-base"></div>
    <div id="joy" class="joystick-stick"></div>
  </div>

  <p class="small">
    連 Wi-Fi：OTTO NINJA / 12345678<br>
    瀏覽器：192.168.4.1
  </p>
</div>

<script>
let ws=null;
let currentMode="WALK";

const statusBox=document.getElementById("statusBox");
const modeBox=document.getElementById("modeBox");
const actBox=document.getElementById("actBox");
const distBox=document.getElementById("distBox");
const headBox=document.getElementById("headBox");
const autoBox=document.getElementById("autoBox");

function setStatus(t,c){statusBox.innerText="狀態："+t; statusBox.style.background=c;}
function connectWS(){
  ws=new WebSocket("ws://"+location.hostname+":81/");
  setStatus("連線中…","#ffeaa7");
  ws.onopen=()=>setStatus("已連線","#55efc4");
  ws.onclose=()=>setStatus("中斷","#fab1a0");
  ws.onerror=()=>setStatus("錯誤","#ff7675");
  ws.onmessage=(e)=>handleMsg(e.data);
}
function sendCmd(cmd){
  if(ws && ws.readyState===1) ws.send(cmd);
  else alert("請先按「連線」");
}
function move(x,y){
  sendCmd("move "+x+" "+y);
}

function handleMsg(m){
  if(m.startsWith("MODE:")){
    currentMode = m.substring(5).trim();
    modeBox.innerText="模式："+currentMode;
  }
  if(m.startsWith("ACT:"))  actBox.innerText ="動作："+m.substring(4);

  if(m.startsWith("DIST:")){
    distBox.innerText = "距離：" + m.substring(5).trim() + " cm";
  }
  if(m.startsWith("HEAD:")){
    headBox.innerText = "頭部開關：" + m.substring(5).trim();
  }
  if(m.startsWith("AUTO:")){
    autoBox.innerText = "自動避障：" + m.substring(5).trim();
  }
}

/* ===== 虛擬搖桿（含節流 25Hz） ===== */
const joy = document.getElementById("joy");
const joyWrap = joy.parentElement;

let dragging = false;
let center = { x: 110, y: 110 };
let maxDist = 80;

let lastSend = 0;
function sendJoy(x, y){
  const now = Date.now();
  if(now - lastSend < 40) return;
  lastSend = now;

  x = Math.round(x);
  y = Math.round(y);
  move(x, y);
}
function resetJoy(){
  joy.style.left = "67px";
  joy.style.top  = "67px";
  move(0,0);
}

joy.addEventListener("pointerdown", e=>{
  dragging = true;
  joy.setPointerCapture(e.pointerId);
});

window.addEventListener("pointermove", e=>{
  if(!dragging) return;

  const rect = joyWrap.getBoundingClientRect();
  let x = e.clientX - rect.left - center.x;
  let y = e.clientY - rect.top  - center.y;

  const dist = Math.sqrt(x*x + y*y);
  if(dist > maxDist){
    x = x / dist * maxDist;
    y = y / dist * maxDist;
  }

  joy.style.left = (center.x + x - 43) + "px";
  joy.style.top  = (center.y + y - 43) + "px";

  let jx = (x / maxDist) * 100;
  let jy = (-y / maxDist) * 100;

  sendJoy(jx, jy);
});

window.addEventListener("pointerup", e=>{
  if(!dragging) return;
  dragging = false;
  resetJoy();
});
</script>
</body>
</html>)=====";

/* ========== 工具：WebSocket 廣播 ========== */
void wsBroadcast(String s){
  webSocket.broadcastTXT(s);
}

/* ========== 小工具：統一 attach/detach（方便校正） ========== */
inline void attachLF(){ myservoLeftFoot.attach(ServoLeftFootPin, SERVO_MIN_US, SERVO_MAX_US); }
inline void attachLL(){ myservoLeftLeg.attach(ServoLeftLegPin, SERVO_MIN_US, SERVO_MAX_US); }
inline void attachRF(){ myservoRightFoot.attach(ServoRightFootPin, SERVO_MIN_US, SERVO_MAX_US); }
inline void attachRL(){ myservoRightLeg.attach(ServoRightLegPin, SERVO_MIN_US, SERVO_MAX_US); }

inline void detachLF(){ myservoLeftFoot.detach(); }
inline void detachLL(){ myservoLeftLeg.detach(); }
inline void detachRF(){ myservoRightFoot.detach(); }
inline void detachRL(){ myservoRightLeg.detach(); }

inline void detachAllLegs(){
  detachLF(); detachLL(); detachRF(); detachRL();
}

/* ========== 無源蜂鳴器：不阻塞可愛旋律播放器（tone + millis） ========== */
struct MelodyPlayer {
  const uint16_t* notes = nullptr;  // Hz，0=休止
  const uint16_t* dur   = nullptr;  // 每音長度 ms
  uint8_t len = 0;
  uint8_t idx = 0;
  bool playing = false;
  unsigned long nextMs = 0;
} mp;

inline void buzzerStop(){
  noTone(BUZZER_PIN);
  digitalWrite(BUZZER_PIN, LOW);
  mp.playing = false;
}

// 可愛 1：跳跳糖（短促上行＋小回彈）
const uint16_t SONG1_NOTES[] = { 784, 988, 1175, 988, 1047, 0, 988, 1319 };
const uint16_t SONG1_DUR[]   = {  90,  90,  120,  90,  160, 70, 110,  220 };

// 可愛 2：開寶箱叮咚（雙叮＋長尾音）
const uint16_t SONG2_NOTES[] = { 1047, 1319, 1568, 0, 1319, 1568, 2093 };
const uint16_t SONG2_DUR[]   = {   90,   90,  180, 60,   90,  120,  320 };

void playSong(uint8_t id){
  if(id == 1){
    mp.notes = SONG1_NOTES;
    mp.dur   = SONG1_DUR;
    mp.len   = sizeof(SONG1_NOTES)/sizeof(SONG1_NOTES[0]);
  }else{
    mp.notes = SONG2_NOTES;
    mp.dur   = SONG2_DUR;
    mp.len   = sizeof(SONG2_NOTES)/sizeof(SONG2_NOTES[0]);
  }
  mp.idx = 0;
  mp.playing = true;
  mp.nextMs = 0; // 立刻開始
}

void melodyTick(){
  if(!mp.playing) return;
  unsigned long now = millis();
  if(now < mp.nextMs) return;

  if(mp.idx >= mp.len){
    buzzerStop();
    return;
  }

  uint16_t f = mp.notes[mp.idx];
  uint16_t d = mp.dur[mp.idx];

  if(f == 0){
    noTone(BUZZER_PIN);
  }else{
    tone(BUZZER_PIN, f, d);
  }

  // 留一點空隙（更可愛、更跳）
  mp.nextMs = now + (unsigned long)(d * 1.25f);
  mp.idx++;
}

/* ========== 超音波量測（pulseIn 有 timeout） ========== */
int readUltrasonicCm(){
  digitalWrite(US_TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(US_TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(US_TRIG_PIN, LOW);

  unsigned long dur = pulseIn(US_ECHO_PIN, HIGH, 25000UL);
  if(dur == 0) return 999;
  int cm = (int)(dur / 58UL);
  cm = constrain(cm, 1, 500);
  return cm;
}

/* ========== 動作函式（穩定優化：不再動 RX/TX 的伺服） ========== */
void NinjaStop(){
  detachAllLegs();
}

void NinjaSetWalk(){
  attachLL();
  attachRL();
  myservoLeftLeg.write(LA0);
  myservoRightLeg.write(RA0);
  delay(300);
  detachLL();
  detachRL();
}

void NinjaSetRoll(){
  attachLL();
  attachRL();
  myservoLeftLeg.write(LA1);
  myservoRightLeg.write(RA1);
  delay(300);
  detachLL();
  detachRL();
}

void NinjaWalkStop(){
  attachLF(); attachRF(); attachLL(); attachRL();
  myservoLeftFoot.write(90);
  myservoRightFoot.write(90);
  myservoLeftLeg.write(LA0);
  myservoRightLeg.write(RA0);
}

void NinjaRollStop(){
  attachLF(); attachRF();
  myservoLeftFoot.write(90);
  myservoRightFoot.write(90);
  detachLF(); detachRF();
}

/* ========== 模式切換（供頭部開關/網頁共用） ========== */
void setModeWalk(){
  NinjaSetWalk();
  ModeCounter = 0;
  wsBroadcast("MODE:WALK");
}
void setModeRoll(){
  NinjaSetRoll();
  ModeCounter = 1;
  wsBroadcast("MODE:ROLL");
}
void toggleMode(){
  if(ModeCounter == 0) setModeRoll();
  else setModeWalk();
  playSong(1); // 切模式給可愛提示音
}

/* ========== WebSocket event：接收控制命令 ========== */
void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length){
  if(type == WStype_CONNECTED){
    wsBroadcast("MODE:" + String(ModeCounter==0 ? "WALK" : "ROLL"));
    wsBroadcast("ACT:STOP");
    wsBroadcast("AUTO:" + String(autoAvoid ? "ON" : "OFF"));
    wsBroadcast("DIST:" + String(usDistanceCm));
    wsBroadcast("HEAD:" + String(headSwStable ? "RELEASE" : "PRESS"));
    return;
  }
  if(type != WStype_TEXT) return;

  String cmd;
  cmd.reserve(length);
  for(size_t i=0;i<length;i++) cmd += (char)payload[i];
  cmd.trim();

  if(cmd == "mode_walk"){
    setModeWalk();
    return;
  }

  if(cmd == "mode_roll"){
    setModeRoll();
    return;
  }

  if(cmd == "auto_toggle"){
    autoAvoid = !autoAvoid;
    wsBroadcast("AUTO:" + String(autoAvoid ? "ON" : "OFF"));
    // 開/關不同提示音
    playSong(autoAvoid ? 2 : 1);
    return;
  }

  if(cmd.startsWith("beep")){
    int sp = cmd.indexOf(' ');
    int n = 1;
    if(sp > 0) n = cmd.substring(sp+1).toInt();

    if(n == 1) playSong(1);
    else if(n == 2) playSong(2);
    else playSong(1);

    return;
  }

  if(cmd.startsWith("move")){
    int sp1 = cmd.indexOf(' ');
    int sp2 = cmd.lastIndexOf(' ');
    if(sp1 > 0 && sp2 > sp1){
      CTRL_JX = cmd.substring(sp1+1, sp2).toInt();
      CTRL_JY = cmd.substring(sp2+1).toInt();
      CTRL_JX = constrain(CTRL_JX, -100, 100);
      CTRL_JY = constrain(CTRL_JY, -100, 100);
    }
    return;
  }
}

/* ========== Setup ========== */
void setup(){
  Serial.begin(115200);

  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite(BUZZER_PIN, LOW);

  pinMode(HEAD_SW_PIN, INPUT_PULLUP);
  pinMode(US_TRIG_PIN, OUTPUT);
  pinMode(US_ECHO_PIN, INPUT);
  digitalWrite(US_TRIG_PIN, LOW);

  WiFi.mode(WIFI_AP);
  WiFi.softAP(ssid, password);

  server.on("/", [](){ server.send_P(200, "text/html", MAIN_page); });
  server.begin();

  webSocket.begin();
  webSocket.onEvent(webSocketEvent);

  NinjaWalkStop();
  delay(300);
  NinjaStop();

  // 開機可愛音
  playSong(2);

  Serial.println("OTTO NINJA Web Ready");
  Serial.println(WiFi.softAPIP());
}

/* ========== Loop ========== */
void loop(){
  server.handleClient();
  webSocket.loop();

  // 旋律播放（不阻塞）
  melodyTick();

  /* ===== 頭部開關：去彈跳 + 短按切模式 ===== */
  bool readNow = digitalRead(HEAD_SW_PIN); // true=release, false=press
  if(readNow != headSwLastRead){
    headSwLastRead = readNow;
    headSwLastChangeMs = millis();
  }
  headSwPressedEdge = false;
  if(millis() - headSwLastChangeMs >= DEBOUNCE_MS){
    if(readNow != headSwStable){
      headSwStable = readNow;
      wsBroadcast("HEAD:" + String(headSwStable ? "RELEASE" : "PRESS"));
      if(headSwStable == false){
        headSwPressedEdge = true;
      }
    }
  }
  if(headSwPressedEdge){
    toggleMode();
  }

  /* ===== 超音波：定期量測 + 回傳 ===== */
  if(millis() - lastUsMs >= US_PERIOD_MS){
    lastUsMs = millis();
    int cm = readUltrasonicCm();
    if(cm == 999){
      usDistanceCm = 999;
    }else{
      usDistanceCm = (usDistanceCm == 999) ? cm : (int)(0.7f*usDistanceCm + 0.3f*cm);
    }
    wsBroadcast("DIST:" + String(usDistanceCm));
  }

  /* ===== 狀態顯示（供網頁） ===== */
  static String lastAct = "";
  String act;

  const int DZ = 15;
  bool idle = (abs(CTRL_JX) <= DZ && abs(CTRL_JY) <= DZ);

  if(idle) act="STOP";
  else if(abs(CTRL_JY) >= abs(CTRL_JX)) act = (CTRL_JY > 0) ? "FORWARD" : "BACK";
  else act = (CTRL_JX > 0) ? "RIGHT" : "LEFT";

  /* ===== 自動避障：只在「前進」時介入 ===== */
  bool avoiding = false;
  if(autoAvoid && act == "FORWARD" && usDistanceCm <= AVOID_CM){
    avoiding = true;
    CTRL_JX = 0;
    CTRL_JY = 0;
    act = "AVOID_STOP";
    NinjaWalkStop();
    NinjaRollStop();
    playSong(1);
  }

  if(act != lastAct){
    lastAct = act;
    wsBroadcast("ACT:" + act);
  }

  if(avoiding){
    return;
  }

  /* ===== WALK：可前後走 + 可原地轉向 + 轉向更明顯（加腿部 bias） ===== */
  if(ModeCounter == 0){

    int x = CTRL_JX;
    int y = CTRL_JY;

    bool wIdle = (abs(x) <= DZ && abs(y) <= DZ);
    bool inPlaceTurn = (abs(y) <= DZ && abs(x) > DZ);

    if(wIdle){
      NinjaWalkStop();
      return;
    }

    // 步幅
    int step = map(constrain(abs(y), 0, 100), 0, 100, 18, 58);

    float steer = (float)constrain(x, -100, 100) / 100.0f;

    const float STEER_GAIN = 0.75f;
    int stepL = (int)round(step * (1.0f + STEER_GAIN * steer));
    int stepR = (int)round(step * (1.0f - STEER_GAIN * steer));

    stepL = constrain(stepL, 12, 65);
    stepR = constrain(stepR, 12, 65);

    if(inPlaceTurn){
      stepL = stepR = 28;
    }

    int bias = map(abs(x), 0, 100, 0, 14);
    if(x < 0) bias = -bias;

    const unsigned long PHASE_MS = 300;
    static unsigned long t0 = 0;
    static int phase = 0;

    if(millis() - t0 >= PHASE_MS){
      t0 = millis();
      phase = (phase + 1) % 4;
    }

    attachLL(); attachRL(); attachLF(); attachRF();

    int dir = (y >= 0) ? +1 : -1;

    if(phase == 0){
      myservoLeftLeg.write(LATR + bias);
      myservoRightLeg.write(RATR + bias);
    }
    else if(phase == 1){
      myservoRightFoot.write(90);

      if(inPlaceTurn){
        if(x < 0){
          myservoLeftFoot.write(90 - stepL);
        }else{
          myservoLeftFoot.write(90 + stepL);
        }
      }else{
        myservoLeftFoot.write(90 + dir * stepL);
      }
    }
    else if(phase == 2){
      myservoLeftLeg.write(LATL + bias);
      myservoRightLeg.write(RATL + bias);
    }
    else if(phase == 3){
      myservoLeftFoot.write(90);

      if(inPlaceTurn){
        if(x < 0){
          myservoRightFoot.write(90 + stepR);
        }else{
          myservoRightFoot.write(90 - stepR);
        }
      }else{
        myservoRightFoot.write(90 - dir * stepR);
      }
    }

    return;
  }

  /* ===== ROLL：差速左右轉 ===== */
  if(ModeCounter == 1){
    if(idle){
      NinjaRollStop();
      return;
    }

    attachLF();
    attachRF();

    int LWS= map(CTRL_JY, 100, -100, 135, 45);
    int RWS= map(CTRL_JY, 100, -100, 45, 135);
    int LWD= map(CTRL_JX, 100, -100, 45, 0);
    int RWD= map(CTRL_JX, 100, -100, 0, -45);

    int outL = constrain(LWS + LWD, 0, 180);
    int outR = constrain(RWS + RWD, 0, 180);

    myservoLeftFoot.write(outL);
    myservoRightFoot.write(outR);
  }
}
智慧生活科技專業社群

2026年1月17日星期六

[OTTO Ninja]OTTO忍者機器人加入自動避障和音效

沒有留言:

張貼留言

2026年1月17日 星期六

[OTTO Ninja]OTTO忍者機器人加入自動避障和音效

沒有留言:

張貼留言

2026年1月17日星期六
