testMission_Arduino_Indoor.ino
#include <Stepper.h>
#include <Servo.h>
#include "HX711.h"
// 指令：
// PING // 回 PONG（測試用）
// L:<vL> R:<vR> // 車輛馬達控制 PWM（-255..255）
// STOP // 車輛停止
// LED <R|G|Y> // LED亮指定色（會關掉其他色），保持亮直到 LED_STOP
// LED_STOP // LED全關
// STRETCH // 步進馬達伸5圈→收5圈（測試用）
// STRETCH <grams> // 步進馬達伸5圈→等重量達目標→立刻收回3圈→ 回 Orin STRETCH_OK / STRETCH_ERR
// UP [deg] // 伺服相對「正轉」deg（預設 15°）；回 UP_OK <now_deg>
// DOWN [deg] // 伺服相對「反轉」deg（預設 15°）；回 DOWN_OK <now_deg>
// ====== 腳位、參數設定 ======
// 左馬達
const int L_EN_FWD = 2;
const int L_EN_REV = 4;
const int L_PWM_FWD = 3;
const int L_PWM_REV = 6;
// 右馬達
const int R_EN_FWD = 7;
const int R_EN_REV = 8;
const int R_PWM_FWD = 5;
const int R_PWM_REV = 9;
// LED
const int LED_R = A5;
const int LED_G = 12;
const int LED_Y = 13;
// 步進馬達 28BYJ48 + ULN2003
const int STEPS_PER_REV = 2048;
const int STP_IN1 = A0; // IN1
const int STP_IN2 = A1; // IN2
const int STP_IN3 = A2; // IN3
const int STP_IN4 = A3; // IN4
Stepper stepperMotor(STEPS_PER_REV, STP_IN1, STP_IN3, STP_IN2, STP_IN4);
// 重量感測器 HX711
const int HX_DOUT = A4; // DOUT
const int HX_SCK = 11; // SCK
HX711 scale;
float SCALE_CAL = 599.0f; // 校正係數（校正後更新
const float WEIGHT_TOL_G = 3.0f; // 允許誤差 ±g
const int RPM_EXTEND = 20; // 伸出速度（RPM）
const int RPM_RETRACT = 20; // 收回速度（RPM）
const unsigned long MAX_WAIT_MS = 20000; // 等待收料的最長時間（安全逾時）
// 伺服馬達 SG90
const int SERVO_PIN = 10; // Servo 用 Timer1；attach 期間會關掉 D9/D10 PWM
const int SERVO_INIT_DEG = 0; // 程式啟動預設角度（內部狀態）
const int SERVO_SETTLE_MS = 15; // 每 1° 的等待（平滑用）
const int SERVO_DEFAULT_DELTA = 15; // UP/DOWN 未給角度時的預設位移
const int SERVO_US_MIN = 500;
const int SERVO_US_MAX = 2400;
const int SERVO_MIN_DEG = 0;
const int SERVO_MAX_DEG = 180;
int servoAngle = SERVO_INIT_DEG; // 目前記錄的伺服角度（0..180）
Servo feederServo;
// =====================
String buf;
// ------- 小工具 -------
int clampInt(int v, int lo, int hi) { if (v < lo) return lo; if (v > hi) return hi; return v; }
// ------- 馬達控制 -------
void setMotorPWM(int enFwd, int enRev, int pwmFwd, int pwmRev, int speed) {
int pwm = abs(speed);
if (pwm > 255) pwm = 255;
if (speed > 0) {
// 前進：FWD PWM 有值、REV PWM=0
digitalWrite(enFwd, HIGH);
digitalWrite(enRev, HIGH);
analogWrite(pwmFwd, pwm);
analogWrite(pwmRev, 0);
} else if (speed < 0) {
// 後退：REV PWM 有值、FWD PWM=0
digitalWrite(enFwd, HIGH);
digitalWrite(enRev, HIGH);
analogWrite(pwmFwd, 0);
analogWrite(pwmRev, pwm);
} else {
// 停止：PWM 皆 0
digitalWrite(enFwd, HIGH);
digitalWrite(enRev, HIGH);
analogWrite(pwmFwd, 0);
analogWrite(pwmRev, 0);
}
}
void stopMotors() {
setMotorPWM(L_EN_FWD, L_EN_REV, L_PWM_FWD, L_PWM_REV, 0);
setMotorPWM(R_EN_FWD, R_EN_REV, R_PWM_FWD, R_PWM_REV, 0);
}
// ------- LED 控制 -------
void ledsAllOff() {
digitalWrite(LED_R, LOW);
digitalWrite(LED_G, LOW);
digitalWrite(LED_Y, LOW);
}
void setLEDColor(char c) {
ledsAllOff();
c = toupper(c);
if (c == 'R') digitalWrite(LED_R, HIGH);
else if (c == 'G') digitalWrite(LED_G, HIGH);
else if (c == 'Y') digitalWrite(LED_Y, HIGH);
}
// ------- 解析 L:<vL> R:<vR> -------
bool handleLRCommand(const String& s) {
int lpos = s.indexOf("L:");
int rpos = s.indexOf("R:");
if (lpos < 0 || rpos < 0) return false;
if (rpos < lpos) {
int lpos2 = rpos;
int rpos2 = lpos;
lpos = lpos2;
rpos = rpos2;
}
// 抓數值
int vL = s.substring(lpos + 2, rpos).toInt();
int vR = s.substring(rpos + 2).toInt();
setMotorPWM(L_EN_FWD, L_EN_REV, L_PWM_FWD, L_PWM_REV, vL);
setMotorPWM(R_EN_FWD, R_EN_REV, R_PWM_FWD, R_PWM_REV, vR);
return true;
}
// ------- 平均取重（已 set_scale/tare） -------
float readWeightAvg(uint8_t n = 6) {
if (!scale.is_ready()) return NAN;
float sum = 0; int cnt = 0;
for (uint8_t i = 0; i < n; i++) {
float g = scale.get_units(1);
if (!isnan(g)) { sum += g; cnt++; }
delay(5);
}
return (cnt > 0) ? (sum / cnt) : NAN;
}
// ------- STRETCH <g> -------
bool stretch_to_weight(float target_g) {
if (target_g <= 0) { Serial.println("STRETCH_ERR BAD_ARG"); return false; }
if (!scale.is_ready()) { Serial.println("STRETCH_ERR SCALE_NOT_READY"); return false; }
// 歸零
scale.tare(20);
delay(80);
// 伸 5 圈
stepperMotor.setSpeed(RPM_EXTEND);
stepperMotor.step(-5 * STEPS_PER_REV); // - 往外伸；若方向反了就把符號換成 +
// 盯重量直到達標（目標 - 容差），一到就結束等待
bool reached = false;
unsigned long t0 = millis();
while (millis() - t0 < MAX_WAIT_MS) {
float w = readWeightAvg(4);
if (!isnan(w) && w >= (target_g - WEIGHT_TOL_G)) {
reached = true;
break; // 達標就立刻收回
}
delay(20);
}
// 收回 3 圈
stepperMotor.setSpeed(RPM_RETRACT);
stepperMotor.step(+3 * STEPS_PER_REV);
// 回報結果
if (reached) {
Serial.println("STRETCH_OK");
return true;
} else {
Serial.println("STRETCH_ERR TIMEOUT");
return false;
}
}
// ------- 伺服馬達相對移動（UP/DOWN 共用） -------
void servoMoveRelative(int deltaDeg, bool reportUp) {
// 安全：停車時才轉伺服
stopMotors();
// 目標角度（夾在 0..180）
long target = (long)servoAngle + (long)deltaDeg;
target = clampInt((int)target, SERVO_MIN_DEG, SERVO_MAX_DEG);
if ((int)target == servoAngle) {
Serial.print(reportUp ? "UP_OK " : "DOWN_OK ");
Serial.println(servoAngle);
return;
}
int step = (target >= servoAngle) ? 1 : -1;
// 擴脈寬範圍以利到達接近 180°
feederServo.attach(SERVO_PIN, SERVO_US_MIN, SERVO_US_MAX);
feederServo.write(servoAngle);
delay(SERVO_SETTLE_MS);
for (int a = servoAngle + step; (step > 0) ? (a <= target) : (a >= target); a += step) {
feederServo.write(a);
delay(SERVO_SETTLE_MS);
}
servoAngle = (int)target;
feederServo.detach(); // 釋放 Timer1
Serial.print(reportUp ? "UP_OK " : "DOWN_OK ");
Serial.println(servoAngle);
}
// ------- 指令處理 -------
void handleCommand(String s) {
s.trim();
if (!s.length()) return;
// L:<vL> R:<vR>
if (handleLRCommand(s)) return;
String t = s;
t.toUpperCase();
if (t == "PING") {
Serial.println("PONG");
return;
}
if (t == "STOP") {
stopMotors();
Serial.println("STOP OK");
return;
}
if (t == "LED_STOP") {
ledsAllOff();
Serial.println("LED_STOP OK");
return;
}
// LED <R|G|Y>
if (t.startsWith("LED ")) {
int sp1 = s.indexOf(' ');
if (sp1 > 0 && s.length() >= sp1 + 2) {
char c = s.substring(sp1 + 1, sp1 + 2)[0];
setLEDColor(c); // 亮該色，保持亮直到 LED_STOP
Serial.println("LED OK");
}
return;
}
// STRETCH（伸縮機構動作）
if (t.startsWith("STRETCH")) {
int sp1 = s.indexOf(' ');
if (sp1 > 0) {
// STRETCH <grams>
float grams = s.substring(sp1 + 1).toFloat();
stretch_to_weight(grams); // 完成印 STRETCH_OK；逾時印 STRETCH_ERR TIMEOUT
} else {
// STRETCH（測試）
stepperMotor.setSpeed(20);
stepperMotor.step(+5 * STEPS_PER_REV); //往外伸
stepperMotor.step(-5 * STEPS_PER_REV); //往內縮
Serial.println("STRETCH TEST OK");
}
return;
}
// SCALE?（重量感測器測試）
if (t == "SCALE?") {
if (!scale.is_ready()) {
Serial.println("WEIGHT NAN");
return;
}
float w = scale.get_units(8); // 取 8 次平均
Serial.print("WEIGHT ");
Serial.println(w, 1);
return;
}
// UP [deg]：相對「正轉」 → 送正角度
if (t == "UP" || t.startsWith("UP ")) {
int delta = SERVO_DEFAULT_DELTA;
int sp1 = s.indexOf(' ');
if (sp1 > 0) {
String num = s.substring(sp1 + 1); num.trim();
if (num.length() > 0) delta = num.toInt();
}
delta = clampInt(delta, 0, 180);
servoMoveRelative(+delta, true);
return;
}
// DOWN [deg]：相對「反轉」 → 送負角度
if (t == "DOWN" || t.startsWith("DOWN ")) {
int delta = SERVO_DEFAULT_DELTA;
int sp1 = s.indexOf(' ');
if (sp1 > 0) {
String num = s.substring(sp1 + 1); num.trim();
if (num.length() > 0) delta = num.toInt();
}
delta = clampInt(delta, 0, 180);
servoMoveRelative(-delta, false);
return;
}
// 其它代補指令
}
void setup() {
// 馬達腳位
pinMode(L_EN_FWD, OUTPUT);
pinMode(L_EN_REV, OUTPUT);
pinMode(L_PWM_FWD, OUTPUT);
pinMode(L_PWM_REV, OUTPUT);
pinMode(R_EN_FWD, OUTPUT);
pinMode(R_EN_REV, OUTPUT);
pinMode(R_PWM_FWD, OUTPUT);
pinMode(R_PWM_REV, OUTPUT);
// LED 腳位
pinMode(LED_R, OUTPUT);
pinMode(LED_G, OUTPUT);
pinMode(LED_Y, OUTPUT);
ledsAllOff();
// 步進馬達初始化
stepperMotor.setSpeed(20); // RPM
// 重量感測器
scale.begin(HX_DOUT, HX_SCK);
scale.set_scale(SCALE_CAL); // 校正因子
scale.tare(20);
// 伺服馬達（不 attach，等用到再 attach）
servoAngle = SERVO_INIT_DEG; // 起始 0°
Serial.begin(9600);
stopMotors(); // 安全起步
}
void loop() {
while (Serial.available()) {
char ch = (char)Serial.read();
if (ch == '\\n' || ch == '\\r') {
if (buf.length() > 0) {
handleCommand(buf);
buf = "";
}
} else {
buf += ch;
if (buf.length() > 120) buf = ""; // 防呆
}
}
}
testMission_indoor.py
"""
全部移動都「寫死分段」(多段 PWM×秒數)；
只有：
1) 第一關停車用側鏡 YOLO 決定目標動物（第一個穩定）→ 亮 LED、記下重量
2) 中間到飼料機：到點直接 STRETCH <重量>，等 STRETCH_OK。
3) 最後一關：分段推進「邊跑邊看」目標動物 → 看到就 STOP → FEED
"""
import cv2
import time
import sys
import serial
from collections import deque
from ultralytics import YOLO
# ========= 可調參數 =========
MODEL_PATH = "best.pt"
SERIAL_PORT = "/dev/ttyACM0"
BAUDRATE = 9600
# 只開側視相機（偵測用）
CAM_SIDE_INDEX = 4
CAM_W, CAM_H = 640, 480
# --- 行走分段（(秒, 左PWM, 右PWM)） ---
# 起點 → 動物區：跑完後停下來辨識
PRE_TO_ANIMAL_SEGMENTS = [
(12, 113, 50), # 12秒 113 50
]
# 動物區 → 飼料機：跑完後停下來，直接 STRETCH <重量>
ANIMAL_TO_FEEDER_SEGMENTS = [
(18.5, 115, 50), # 18.5秒 115 50
]
# 飼料機 → 投餵區（最後一關）：邊跑邊看指定動物，看到就停車 FEED
FEEDER_TO_FEEDZONE_SEGMENTS = [
(3.0, 120, 120),
(2.0, 100, 100),
]
# 出場（可自行配置）
EXIT_SEGMENTS = [
# (1.0, -120, -120),
# (2.0, 140, 140),
]
# 類別/LED/重量
YOLO_CLASS_ROLE = {"chicken":"CHICKEN", "cows":"COW", "pig":"PIG", "machine":"FEEDER"}
ALIASES = {"cow":"cows", "feeder":"machine"}
ANIMAL_ROLES = ("CHICKEN", "PIG", "COW")
ROLE_TO_LED = {"CHICKEN":"G", "PIG":"Y", "COW":"R"}
ROLE_TO_WEIGHT = {"CHICKEN":70, "PIG":155, "COW":205}
# 偵測穩定條件
DET_CONF_TH = 0.50
DET_STABLE_FRAMES = 5 # 第一關定案幀數
MAX_DET_WAIT_SEC = 5.0
# 最後一關「邊跑邊看」觸發條件
FINAL_SEE_CONF_TH = 0.50
FINAL_SEE_STABLE_FR = 5 # 連續幀數達標就停
FINAL_SEE_MIN_WIDTH = 0 # >0 才檢查 bbox 寬度門檻（像素）
# ===========================
# ===== 工具 =====
def clamp(v, lo, hi): return max(lo, min(hi, v))
def normalize_classname(name: str):
k = name.strip().lower()
if k in ALIASES: k = ALIASES[k]
return k
def bbox_center_w(b):
x1,y1,x2,y2 = map(int, b)
cx = (x1+x2)//2
w = (x2-x1)
return cx, w
# ===== 串列 =====
class SerialClient:
def __init__(self, port, baud):
self.ser = serial.Serial(port, baud, timeout=0.05)
time.sleep(2.0) # 等 Arduino reset
def send(self, line):
self.ser.write((line.strip()+"\\n").encode("utf-8"))
def read_line(self):
try:
s = self.ser.readline().decode("utf-8", errors="ignore").strip()
return s if s else None
except:
return None
def expect(self, prefix, timeout=20.0):
t0 = time.time()
while time.time()-t0 < timeout:
s = self.read_line()
if s and s.startswith(prefix):
return s
return None
def close(self):
try: self.ser.close()
except: pass
def drive(sc, vL, vR): sc.send(f"L:{int(clamp(vL,-255,255))} R:{int(clamp(vR,-255,255))}")
def stop(sc): sc.send("STOP")
def led(sc, color): sc.send(f"LED {color}")
def stretch(sc, grams): sc.send(f"STRETCH {int(grams)}")
def feed_servo(sc): sc.send("FEED")
# ===== 分段執行 =====
def run_segments(sc, segments, tag="segment"):
for i, (sec, vL, vR) in enumerate(segments, 1):
print(f"[{tag}] {i}/{len(segments)}: {sec:.2f}s L={vL} R={vR}")
drive(sc, vL, vR)
time.sleep(max(0.0, sec))
stop(sc); time.sleep(0.2)
def run_segments_until_seen(sc, segments, tag, model, cam, target_role):
"""
分段推進，但每 30ms 偵測一次 target_role。
連續 FINAL_SEE_STABLE_FR 幀達標（conf 與寬度門檻）就 STOP 並回 True。
跑完仍未觸發則回 False。
"""
stable = deque(maxlen=FINAL_SEE_STABLE_FR)
for i, (sec, vL, vR) in enumerate(segments, 1):
print(f"[{tag}] {i}/{len(segments)}: {sec:.2f}s L={vL} R={vR} (watch {target_role})")
t0 = time.time()
drive(sc, vL, vR)
while time.time()-t0 < sec:
ok, frm = cam.read()
if not ok:
time.sleep(0.01); continue
r = model.predict(source=frm, verbose=False)[0]
seen = False
for b, c, conf in zip(r.boxes.xyxy.cpu().numpy(),
r.boxes.cls.cpu().numpy(),
r.boxes.conf.cpu().numpy()):
name = normalize_classname(r.names[int(c)])
if name not in YOLO_CLASS_ROLE:
continue
role = YOLO_CLASS_ROLE[name]
if role != target_role:
continue
if conf < FINAL_SEE_CONF_TH:
continue
_, bw = bbox_center_w(b)
if FINAL_SEE_MIN_WIDTH > 0 and bw < FINAL_SEE_MIN_WIDTH:
continue
seen = True
break
stable.append(1 if seen else 0)
if len(stable)==FINAL_SEE_STABLE_FR and sum(stable)==FINAL_SEE_STABLE_FR:
print(f"[{tag}] 偵測到 {target_role}（穩定 {FINAL_SEE_STABLE_FR} 幀）→ 停車")
stop(sc); time.sleep(0.2)
return True
time.sleep(0.03)
stop(sc); time.sleep(0.2)
return False
# ===== YOLO 輔助 =====
def yolo_detect(model, frame_bgr):
out = []
r = model.predict(source=frame_bgr, verbose=False)[0]
for b, c, conf in zip(r.boxes.xyxy.cpu().numpy(),
r.boxes.cls.cpu().numpy(),
r.boxes.conf.cpu().numpy()):
name = normalize_classname(r.names[int(c)])
if name not in YOLO_CLASS_ROLE:
continue
if conf < DET_CONF_TH:
continue
out.append((YOLO_CLASS_ROLE[name], float(conf), b))
return out
def decide_first_animal(model, cam_side):
print("[INFO] 停車偵測第一關動物 ...")
stable = deque(maxlen=DET_STABLE_FRAMES)
t0 = time.time()
while True:
ok, frm = cam_side.read()
if not ok:
if time.time()-t0 > MAX_DET_WAIT_SEC:
print("[WARN] 影像讀取超時"); return None
time.sleep(0.02); continue
dets = yolo_detect(model, frm)
animals = [d for d in dets if d[0] in ANIMAL_ROLES]
if animals:
animals.sort(key=lambda x:x[1], reverse=True)
stable.append(animals[0][0])
print(f"[DBG] animal={animals[0][0]} conf={animals[0][1]:.2f} ({len(stable)}/{DET_STABLE_FRAMES})")
if len(stable)==DET_STABLE_FRAMES and len(set(stable))==1:
return stable[-1]
if time.time()-t0 > MAX_DET_WAIT_SEC:
return None
# ===== 主流程 =====
def main():
# 側視相機
cam_side = cv2.VideoCapture(CAM_SIDE_INDEX)
cam_side.set(cv2.CAP_PROP_FRAME_WIDTH, CAM_W)
cam_side.set(cv2.CAP_PROP_FRAME_HEIGHT, CAM_H)
if not cam_side.isOpened():
print("[ERR] 開不了側視相機"); sys.exit(2)
# YOLO
model = YOLO(MODEL_PATH)
# 串列
sc = SerialClient(SERIAL_PORT, BAUDRATE)
# 1) 起點 → 動物區
run_segments(sc, PRE_TO_ANIMAL_SEGMENTS, tag="to_animal")
# 2) 第一關：定案目標動物 → 亮燈 + 設定重量
target_role = decide_first_animal(model, cam_side) or "CHICKEN"
target_grams = ROLE_TO_WEIGHT[target_role]
led(sc, ROLE_TO_LED[target_role])
print(f"[INFO] 目標={target_role} LED={ROLE_TO_LED[target_role]} 重量={target_grams}g")
# 3) 動物區 → 飼料機
run_segments(sc, ANIMAL_TO_FEEDER_SEGMENTS, tag="to_feeder")
# 4) 到飼料機後：直接 STRETCH <重量>
print(f"[STEP] STRETCH {target_grams}")
stretch(sc, target_grams)
resp = sc.expect("STRETCH_", timeout=30.0)
print(f"[INFO] Arduino 回覆：{resp or '（逾時）'}")
# 5) 飼料機 → 投餵區：分段推進「邊跑邊看」指定動物 → 看到就停 & FEED
seen = run_segments_until_seen(
sc, FEEDER_TO_FEEDZONE_SEGMENTS, tag="to_feed_zone_watch",
model=model, cam=cam_side, target_role=target_role
)
if seen:
print("[STEP] FEED")
feed_servo(sc)
_ = sc.expect("FEED_OK", timeout=5.0)
else:
print("[WARN] 跑完仍未看到目標；不出飼料（可調 FINAL_SEE_* 或段落）")
# 6) 出場
if EXIT_SEGMENTS:
run_segments(sc, EXIT_SEGMENTS, tag="exit")
# 清理
sc.close()
cam_side.release()
cv2.destroyAllWindows()
print("[DONE] 全流程完成。")
if __name__ == "__main__":
try:
main()
except KeyboardInterrupt:
print("\\n[EXIT] 手動中止")