2026年2月12日 星期四

[OTTO GO] 馬達校正以及輪流動一顆

 

範例一、90度

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
#include <Arduino.h>
#include <ESP32Servo.h>

// OTTO GO Servo pins
#define SERVO_LLEG 21   // PWM1
#define SERVO_RLEG 22   // PWM2
#define SERVO_LFOOT 4   // PWM3
#define SERVO_RFOOT 14  // PWM4

Servo sLleg, sRleg, sLfoot, sRfoot;

void setup() {
  Serial.begin(115200);

  // 建議把脈寬範圍設保守一點，避免頂住抖動
  // attach(pin, minUs, maxUs)
  sLleg.attach(SERVO_LLEG,  600, 2400);
  sRleg.attach(SERVO_RLEG,  600, 2400);
  sLfoot.attach(SERVO_LFOOT, 600, 2400);
  sRfoot.attach(SERVO_RFOOT, 600, 2400);

  // 全部轉到 90 度
  sLleg.write(90);
  sRleg.write(90);
  sLfoot.write(90);
  sRfoot.write(90);

  Serial.println("All servos -> 90 deg (calibration position).");
}

void loop() {
  // 校正時不做事
}



範例二、每 2 秒輪流動一顆

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
#include <Arduino.h>
#include <ESP32Servo.h>

#define SERVO_LLEG 21
#define SERVO_RLEG 22
#define SERVO_LFOOT 4
#define SERVO_RFOOT 14

Servo s[4];

void setup() {
  Serial.begin(115200);
  int pins[4] = {SERVO_LLEG, SERVO_RLEG, SERVO_LFOOT, SERVO_RFOOT};

  for (int i=0;i<4;i++){
    s[i].attach(pins[i], 600, 2400);
    s[i].write(90);
  }
  Serial.println("All -> 90. Now testing one by one...");
}

void loop() {
  for (int i=0;i<4;i++){
    // 只動第 i 顆
    s[i].write(80); delay(800);
    s[i].write(90); delay(800);
    s[i].write(100);delay(800);
    s[i].write(90); delay(800);
  }
}


張貼者： 於 沒有留言:
標籤： , ,

[OTTO GO] 超音波控制虎嘴


 Arduino:


  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7789.h>

// ===== TFT pins (OTTO GO) =====
#define TFT_MOSI 19
#define TFT_SCLK 18
#define TFT_CS    5
#define TFT_DC   16
#define TFT_RST  23

// ===== Ultrasonic pins (OTTO GO) =====
#define US_TRIG 27
#define US_ECHO 39   // ESP32 輸入專用腳，適合 ECHO

Adafruit_ST7789 tft = Adafruit_ST7789(TFT_CS, TFT_DC, TFT_RST);

static inline uint16_t RGB565(uint8_t r, uint8_t g, uint8_t b) {
  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
}

// ================== 虎嘴繪圖 ==================
void drawMuzzleBase(int cx, int cy) {
  uint16_t furLight = RGB565(235, 228, 210);
  uint16_t shadow   = RGB565(160, 150, 135);
  uint16_t dotDark  = RGB565(70, 70, 70);

  tft.fillScreen(ST77XX_BLACK);

  // 口鼻白毛區
  tft.fillCircle(cx - 55, cy + 10, 55, furLight);
  tft.fillCircle(cx + 55, cy + 10, 55, furLight);
  tft.fillRoundRect(cx - 90, cy - 25, 180, 110, 28, furLight);

  // 下巴陰影
  tft.fillRoundRect(cx - 85, cy + 60, 170, 22, 12, shadow);
  tft.fillRoundRect(cx - 40, cy + 55, 80, 10, 5, RGB565(120,110,100));

  // 鬍鬚點
  int dotY1 = cy + 6;
  int dotY2 = cy + 26;
  for (int k = 0; k < 3; k++) {
    tft.fillCircle(cx - 55 - k * 12, dotY1 + k * 2, 2, dotDark);
    tft.fillCircle(cx - 55 - k * 12, dotY2 + k * 2, 2, dotDark);
    tft.fillCircle(cx + 55 + k * 12, dotY1 + k * 2, 2, dotDark);
    tft.fillCircle(cx + 55 + k * 12, dotY2 + k * 2, 2, dotDark);
  }
}

void drawMouthClosed(int cx, int cy) {
  uint16_t lineDark = RGB565(40, 40, 40);

  // 人中線
  tft.drawLine(cx,     cy - 5, cx,     cy + 20, lineDark);
  tft.drawLine(cx - 1, cy - 5, cx - 1, cy + 20, RGB565(80,80,80));

  // 上唇弧線
  int mouthY = cy + 18;
  for (int i = 0; i < 34; i++) {
    int y = mouthY + (i * i) / 95;
    tft.drawPixel(cx - i, y, lineDark);
    tft.drawPixel(cx - i, y + 1, lineDark);
    tft.drawPixel(cx + i, y, lineDark);
    tft.drawPixel(cx + i, y + 1, lineDark);
  }

  // 嘴角
  tft.fillCircle(cx - 36, mouthY + 12, 3, lineDark);
  tft.fillCircle(cx + 36, mouthY + 12, 3, lineDark);

  // 嘴中間小暗線
  tft.fillRoundRect(cx - 10, mouthY + 20, 20, 4, 2, RGB565(90,80,70));
}

void drawMouthOpen(int cx, int cy) {
  uint16_t lineDark = RGB565(40, 40, 40);
  uint16_t mouthIn  = RGB565(15, 15, 15);
  uint16_t tongue   = RGB565(210, 120, 120);

  // 人中線（到上唇）
  tft.drawLine(cx,     cy - 5, cx,     cy + 10, lineDark);
  tft.drawLine(cx - 1, cy - 5, cx - 1, cy + 10, RGB565(80,80,80));

  // 上唇弧線（更彎）
  int mouthY = cy + 16;
  for (int i = 0; i < 36; i++) {
    int y = mouthY + (i * i) / 85;
    tft.drawPixel(cx - i, y, lineDark);
    tft.drawPixel(cx - i, y + 1, lineDark);
    tft.drawPixel(cx + i, y, lineDark);
    tft.drawPixel(cx + i, y + 1, lineDark);
  }

  // 嘴角
  tft.fillCircle(cx - 38, mouthY + 14, 3, lineDark);
  tft.fillCircle(cx + 38, mouthY + 14, 3, lineDark);

  // 口腔
  int w = 90, h = 45;
  int x = cx - w / 2;
  int y0 = cy + 28;
  tft.fillRoundRect(x, y0, w, h, 18, mouthIn);
  tft.fillCircle(cx, y0 + h, 22, mouthIn);

  // 舌頭
  tft.fillRoundRect(cx - 22, y0 + 18, 44, 20, 10, tongue);
  tft.fillCircle(cx, y0 + 36, 14, tongue);
}

bool mouthOpen = false;

void drawTigerMouth(bool open) {
  int cx = tft.width() / 2;
  int cy = tft.height() / 2;

  drawMuzzleBase(cx, cy);
  if (open) drawMouthOpen(cx, cy);
  else      drawMouthClosed(cx, cy);

  // 顯示距離用的小區塊（可刪）
  tft.setTextSize(2);
  tft.setTextColor(ST77XX_YELLOW, ST77XX_BLACK);
  tft.setCursor(10, 10);
  tft.print(open ? "MOUTH: OPEN " : "MOUTH: CLOSE");
}

// ================== 超音波測距 ==================
long readDistanceCM() {
  // 觸發 10us 脈衝
  digitalWrite(US_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(US_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(US_TRIG, LOW);

  // 讀回波時間（避免卡住，加 timeout）
  // 30ms 約等於 5m（足夠）
  unsigned long duration = pulseIn(US_ECHO, HIGH, 30000UL);
  if (duration == 0) return -1; // 代表沒量到

  // 距離(cm) = 時間(us) / 58
  return (long)(duration / 58UL);
}

// ================== 主程式：距離控制開闔 ==================
// 遲滯門檻：靠近到 OPEN_CM 才張開；遠離到 CLOSE_CM 才閉合
const int OPEN_CM  = 15;
const int CLOSE_CM = 20;

// 更新間隔
const unsigned long UPDATE_MS = 80;
unsigned long lastUpdateMs = 0;

// 讓距離比較穩：簡單做 3 次取中位數
long median3(long a, long b, long c) {
  if (a > b) { long t=a; a=b; b=t; }
  if (b > c) { long t=b; b=c; c=t; }
  if (a > b) { long t=a; a=b; b=t; }
  return b;
}

void setup() {
  // TFT
  SPI.begin(TFT_SCLK, -1, TFT_MOSI, TFT_CS);
  tft.init(240, 320);      // 若你確定是 170x320 才改
  tft.setRotation(1);

  // Ultrasonic
  pinMode(US_TRIG, OUTPUT);
  pinMode(US_ECHO, INPUT);

  // 初始狀態
  mouthOpen = false;
  drawTigerMouth(mouthOpen);
}

void loop() {
  unsigned long now = millis();
  if (now - lastUpdateMs < UPDATE_MS) return;
  lastUpdateMs = now;

  long d1 = readDistanceCM();
  long d2 = readDistanceCM();
  long d3 = readDistanceCM();
  long d  = median3(d1, d2, d3);

  // 沒量到就不改狀態
  if (d < 0) return;

  // 在畫面顯示距離（可刪）
  tft.fillRect(10, 30, 140, 24, ST77XX_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ST77XX_CYAN, ST77XX_BLACK);
  tft.setCursor(10, 30);
  tft.print("D=");
  tft.print(d);
  tft.print("cm");

  bool newState = mouthOpen;
  if (!mouthOpen && d <= OPEN_CM)  newState = true;   // 靠近 -> 張開
  if ( mouthOpen && d >= CLOSE_CM) newState = false;  // 遠離 -> 閉合

  if (newState != mouthOpen) {
    mouthOpen = newState;
    drawTigerMouth(mouthOpen);
  }
}


張貼者： 於 沒有留言:
標籤： , , , ,

[OTTO GO] 拍手控制虎嘴

Arduino:

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7789.h>
#include "driver/i2s.h"

// ===== TFT pins (OTTO GO) =====
#define TFT_MOSI 19
#define TFT_SCLK 18
#define TFT_CS    5
#define TFT_DC   16
#define TFT_RST  23

// ===== I2S MEMS Mic pins (OTTO GO) =====
#define MIC_I2S_SCK 13   // I2S BCLK/SCK
#define MIC_I2S_WS  15   // I2S WS/LRCK
#define MIC_I2S_SD  12   // I2S SD (DATA IN)

Adafruit_ST7789 tft = Adafruit_ST7789(TFT_CS, TFT_DC, TFT_RST);

static inline uint16_t RGB565(uint8_t r, uint8_t g, uint8_t b) {
  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
}

// ================== 虎嘴繪圖 ==================
void drawMuzzleBase(int cx, int cy) {
  uint16_t furLight = RGB565(235, 228, 210);
  uint16_t shadow   = RGB565(160, 150, 135);
  uint16_t dotDark  = RGB565(70, 70, 70);

  tft.fillScreen(ST77XX_BLACK);

  tft.fillCircle(cx - 55, cy + 10, 55, furLight);
  tft.fillCircle(cx + 55, cy + 10, 55, furLight);
  tft.fillRoundRect(cx - 90, cy - 25, 180, 110, 28, furLight);

  tft.fillRoundRect(cx - 85, cy + 60, 170, 22, 12, shadow);
  tft.fillRoundRect(cx - 40, cy + 55, 80, 10, 5, RGB565(120,110,100));

  int dotY1 = cy + 6;
  int dotY2 = cy + 26;
  for (int k = 0; k < 3; k++) {
    tft.fillCircle(cx - 55 - k * 12, dotY1 + k * 2, 2, dotDark);
    tft.fillCircle(cx - 55 - k * 12, dotY2 + k * 2, 2, dotDark);
    tft.fillCircle(cx + 55 + k * 12, dotY1 + k * 2, 2, dotDark);
    tft.fillCircle(cx + 55 + k * 12, dotY2 + k * 2, 2, dotDark);
  }
}

void drawMouthClosed(int cx, int cy) {
  uint16_t lineDark = RGB565(40, 40, 40);

  tft.drawLine(cx,     cy - 5, cx,     cy + 20, lineDark);
  tft.drawLine(cx - 1, cy - 5, cx - 1, cy + 20, RGB565(80,80,80));

  int mouthY = cy + 18;
  for (int i = 0; i < 34; i++) {
    int y = mouthY + (i * i) / 95;
    tft.drawPixel(cx - i, y, lineDark);
    tft.drawPixel(cx - i, y + 1, lineDark);
    tft.drawPixel(cx + i, y, lineDark);
    tft.drawPixel(cx + i, y + 1, lineDark);
  }
  tft.fillCircle(cx - 36, mouthY + 12, 3, lineDark);
  tft.fillCircle(cx + 36, mouthY + 12, 3, lineDark);
  tft.fillRoundRect(cx - 10, mouthY + 20, 20, 4, 2, RGB565(90,80,70));
}

void drawMouthOpen(int cx, int cy) {
  uint16_t lineDark = RGB565(40, 40, 40);
  uint16_t mouthIn  = RGB565(15, 15, 15);
  uint16_t tongue   = RGB565(210, 120, 120);

  tft.drawLine(cx,     cy - 5, cx,     cy + 10, lineDark);
  tft.drawLine(cx - 1, cy - 5, cx - 1, cy + 10, RGB565(80,80,80));

  int mouthY = cy + 16;
  for (int i = 0; i < 36; i++) {
    int y = mouthY + (i * i) / 85;
    tft.drawPixel(cx - i, y, lineDark);
    tft.drawPixel(cx - i, y + 1, lineDark);
    tft.drawPixel(cx + i, y, lineDark);
    tft.drawPixel(cx + i, y + 1, lineDark);
  }
  tft.fillCircle(cx - 38, mouthY + 14, 3, lineDark);
  tft.fillCircle(cx + 38, mouthY + 14, 3, lineDark);

  int w = 90, h = 45;
  int x = cx - w / 2;
  int y0 = cy + 28;
  tft.fillRoundRect(x, y0, w, h, 18, mouthIn);
  tft.fillCircle(cx, y0 + h, 22, mouthIn);

  tft.fillRoundRect(cx - 22, y0 + 18, 44, 20, 10, tongue);
  tft.fillCircle(cx, y0 + 36, 14, tongue);
}

bool mouthOpen = false;

void drawTigerMouth(bool open) {
  int cx = tft.width() / 2;
  int cy = tft.height() / 2;
  drawMuzzleBase(cx, cy);
  if (open) drawMouthOpen(cx, cy);
  hookupLabel(open);
}

void hookupLabel(bool open){
  tft.setTextSize(2);
  tft.setTextColor(ST77XX_YELLOW, ST77XX_BLACK);
  tft.setCursor(10, 10);
  tft.print(open ? "MOUTH: OPEN " : "MOUTH: CLOSE");
}

// ================== 麥克風 I2S 讀取 + 拍手偵測 ==================
// 使用 I2S_NUM_0 做 RX
const i2s_port_t MIC_PORT = I2S_NUM_0;

// 取樣參數
static const int SAMPLE_RATE = 16000;
static const int FRAME_SAMPLES = 256;  // 約 16ms
static int32_t micBuf[FRAME_SAMPLES];

// 拍手偵測狀態
float noiseFloor = 2000.0f;        // 自動學習的環境噪音能量
unsigned long lastClapMs = 0;
const unsigned long CLAP_COOLDOWN_MS = 350; // 拍手冷卻，避免連發

void micInit() {
  i2s_config_t cfg = {
    .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX),
    .sample_rate = SAMPLE_RATE,
    .bits_per_sample = I2S_BITS_PER_SAMPLE_32BIT, // 很多 MEMS I2S 是 24-bit/32-bit 輸出
    .channel_format = I2S_CHANNEL_FMT_ONLY_LEFT,
    .communication_format = I2S_COMM_FORMAT_I2S_MSB,
    .intr_alloc_flags = ESP_INTR_FLAG_LEVEL1,
    .dma_buf_count = 6,
    .dma_buf_len = 256,
    .use_apll = false,
    .tx_desc_auto_clear = false,
    .fixed_mclk = 0
  };

  i2s_pin_config_t pin_cfg = {
    .bck_io_num = MIC_I2S_SCK,
    .ws_io_num = MIC_I2S_WS,
    .data_out_num = I2S_PIN_NO_CHANGE,
    .data_in_num = MIC_I2S_SD
  };

  i2s_driver_install(MIC_PORT, &cfg, 0, NULL);
  i2s_set_pin(MIC_PORT, &pin_cfg);
  i2s_zero_dma_buffer(MIC_PORT);
}

// 讀一個 frame，回傳「能量」(平均絕對值)
float readMicFrameEnergy() {
  size_t bytesRead = 0;
  i2s_read(MIC_PORT, (void*)micBuf, sizeof(micBuf), &bytesRead, portMAX_DELAY);

  int n = bytesRead / 4; // int32
  if (n <= 0) return 0;

  // 轉成 16-bit 有效值（很多 I2S MEMS 會左對齊）
  // 用 abs 平均值做能量指標
  uint64_t sumAbs = 0;
  for (int i = 0; i < n; i++) {
    int32_t s32 = micBuf[i];
    int16_t s16 = (int16_t)(s32 >> 16); // 取高 16 位
    sumAbs += (uint16_t)abs(s16);
  }
  return (float)sumAbs / (float)n;
}

// 簡單拍手偵測：
// 1) 能量 > 動態門檻（noiseFloor 倍數 + 固定值）
// 2) 且是突發（相對前一幀上升很快）
// 3) 冷卻時間避免連續觸發
bool detectClap() {
  static float prevE = 0;

  float e = readMicFrameEnergy();

  // 更新環境噪音（只在相對安靜時慢慢跟隨）
  // 避免拍手時把噪音底拉太高
  if (e < noiseFloor * 1.5f) {
    noiseFloor = noiseFloor * 0.98f + e * 0.02f;
  }

  // 動態門檻
  float thr = noiseFloor * 4.0f + 800.0f; // 你覺得太敏感/不敏感就改這兩個係數

  // 突發條件
  bool spike = (e > thr) && (e > prevE * 1.8f);

  prevE = e;

  unsigned long now = millis();
  if (spike && (now - lastClapMs > CLAP_COOLDOWN_MS)) {
    lastClapMs = now;
    return true;
  }
  return false;
}

// ================== 主程式 ==================
void setup() {
  // TFT
  SPI.begin(TFT_SCLK, -1, TFT_MOSI, TFT_CS);
  tft.init(240, 320);     // 若你確定是 170x320 再改
  tft.setRotation(1);

  // 麥克風 I2S
  micInit();

  // 初始畫面：嘴巴閉合
  mouthOpen = false;
  drawTigerMouth(mouthOpen);
}

void loop() {
  if (detectClap()) {
    mouthOpen = !mouthOpen;
    drawTigerMouth(mouthOpen);
  }
}


張貼者： 於 沒有留言:
標籤： , , , ,
訂閱： 意見 (Atom)