เล่นกล้อง AI reCamera + gimbal: cross-compile RISC-V + CAN + Node-RED

พอดีได้กล่องชุด reCamera Gimbal มาเล่น เป็นกล้อง AI ตัวเล็กที่วางอยู่บนแกน brushless สองแกน (yaw กับ pitch) ตัวประมวลผลเป็นชิป RISC-V รันโมเดลตรวจจับวัตถุอยู่ในตัว แล้วสั่งมอเตอร์ให้หันตามวัตถุที่เจอ ของเล่นแนวนี้ปกติจะปิดเฟิร์มแวร์หมด แต่ตัวนี้เปิดให้เขียนโปรแกรมคุมมอเตอร์เองผ่าน CAN bus ได้ ผมเลยรื้อดูตั้งแต่การ cross-compile ไปจนถึงการสั่งมอเตอร์ทีละองศา เลยเอามาเรียงเป็นขั้นตอนให้อ่านกันครับ

ชื่อ user, path บางส่วน และค่า PID ในบทความเป็นตัวอย่างที่ปรับให้อ่านง่าย ส่วน IP 192.168.42.1 เป็นค่า default ของ device จริง คำสั่งและโครงสร้าง CAN frame ทั้งหมดคือของจริงจากชุดพัฒนา

ของจริงในกล่อง: RISC-V + กล้อง AI + มอเตอร์ brushless

ตัวประมวลผลเป็น SoC ตระกูล SG2002 ข้างในมี core RISC-V C906 สองตัว (ตัวใหญ่ 1GHz อีกตัว 700MHz) มี NPU แรงราว 1 Tops ที่ Int8 พอวิ่งโมเดลตรวจจับวัตถุบนตัวกล้องได้เลยโดยไม่ต้องพึ่งเครื่องนอก RAM 256MB รันลินุกซ์ (reCamera OS) กล้องเป็น OV5647 5MP ป้อนไฟ 12V เข้าทาง XT30

จุดสำคัญที่ทำให้สั่งมอเตอร์ได้คือต้องเปลี่ยน base board เป็นรุ่น B4_CAN เพื่อเปิดช่อง CAN ออกมา มอเตอร์เป็น brushless สองรุ่น MS3008 กับ MS3506 ต่อกันบน CAN bus เดียว ตัวละ ID ตั้งไว้ 01 กับ 02 แต่ละตัวมี encoder แม่เหล็ก 15-bit อยู่ในตัว คุยกันที่ CAN 1Mbps โหมดที่ผมใช้ตลอดคือ position loop 2KHz

ภาพรวมของทั้งระบบเป็นแบบนี้ครับ เขียนโค้ดบนเครื่อง PC แล้วยิงคำสั่งข้ามไปให้ RISC-V บนกล้องเป็นคนสั่งมอเตอร์จริง

cross-compile ภาษา C ให้รันบน RISC-V musl

กล้องเป็น RISC-V ไม่ใช่ x86 เพราะฉะนั้น gcc บนเครื่องเราคอมไพล์แล้วเอาไปรันไม่ได้ ต้องใช้ cross-compiler ที่ออกไบนารีเป็น riscv64 และที่สำคัญคือต้องเป็นสาย musl ไม่ใช่ glibc เพราะ userland บนกล้องเป็น musl ถ้าหยิบ toolchain ผิดสาย ไบนารีจะคอมไพล์ผ่านแต่พอเอาไปรันบนกล้องจะเด้ง error ทันที

ผมใช้ host-tools ของ sophgo โคลนมาไว้บนเครื่องลินุกซ์ตัวไหนก็ได้ (ผมใช้ Ubuntu):

git clone https://github.com/sophgo/host-tools.git

ตัว gcc ที่ต้องการอยู่ใน host-tools/gcc/riscv64-linux-musl-x86_64/bin เอา path เต็มของโฟลเดอร์นั้น (ดูด้วย pwd) มาผูกเข้า PATH ในไฟล์ .bashrc ตัวเอง:

# ต่อท้าย ~/.bashrc — เปลี่ยน path ให้ตรงกับที่โคลนไว้
export PATH=$PATH:/home/user/host-tools/gcc/riscv64-linux-musl-x86_64/bin
source ~/.bashrc

พอ PATH มาแล้วก็คอมไพล์ตรงๆ ได้เลย ชื่อ compiler เต็มๆ คือ riscv64-unknown-linux-musl-gcc:

riscv64-unknown-linux-musl-gcc can_send.c -o can_send

ส่งไฟล์ขึ้น device แล้วสั่งมอเตอร์ตัวแรก

ได้ไบนารีแล้วก็ scp ขึ้นกล้องผ่าน IP default 192.168.42.1 แล้ว ssh เข้าไปรัน ตัวโปรแกรม can_send จะคอยอ่านไฟล์ชื่อ Angle.txt วนไปเรื่อยๆ ที่เหลือเราแค่เขียนมุมลงไฟล์นั้น มอเตอร์ก็หัน

1. ส่งไบนารีขึ้นกล้อง: scp ./can_send [email protected]:~/
2. ล็อกอินเข้ากล้อง: ssh [email protected]
3. สร้างไฟล์คุมมุมแล้วรันโปรแกรม: touch Angle.txt แล้ว ./can_send
4. เปิดอีกหน้าต่างหนึ่ง เขียนมุมลงไฟล์: echo "1_90_90+2_90_90" >> Angle.txt

scp ./can_send [email protected]:~/
ssh [email protected]

# บนกล้อง
touch Angle.txt
./can_send

# อีกหน้าต่าง — สั่งมอเตอร์ ID1 ไป 90° และ ID2 ไป 90° เพดานความเร็ว 90 dps
echo "1_90_90+2_90_90" >> Angle.txt

รูปแบบในไฟล์คือ motorID_มุม_ความเร็ว ถ้าจะสั่งพร้อมกันสองตัวก็คั่นด้วยเครื่องหมาย + เป็น 1_90_90+2_90_90 ตัวเลขความเร็วมีหน่วยเป็น dps (องศาต่อวินาที) เป็นเพดานว่าจะให้หันเร็วสุดแค่ไหน ถ้าอยากหยุดโปรแกรมก็เขียนตัว q ลงไฟล์ หรือกด q ที่คีย์บอร์ด มันจะสั่ง can0 ลงให้เองก่อนออก

เบื้องหลัง: CAN frame ที่คุมมอเตอร์

ทีนี้มาดูของจริงว่าเวลาเขียน 1_90_90 ลงไฟล์แล้วเกิดอะไรขึ้น ตัวโปรแกรมใช้ SocketCAN ของลินุกซ์ตรงๆ เปิด socket แบบ PF_CAN ผูกกับ can0 ก่อนใช้งานต้องตั้ง bitrate ให้ตรงกับมอเตอร์คือ 1Mbps ไม่งั้นคุยกันไม่รู้เรื่อง ส่วนนี้โปรแกรมสั่งให้เองผ่าน system():

// ตั้งความเร็ว CAN ให้ตรงกับมอเตอร์ แล้วยกอินเทอร์เฟซขึ้น
system("sudo ip link set can0 type can bitrate 1000000");
system("sudo ip link set can0 up");

หัวใจอยู่ที่การประกอบ frame มอเตอร์แต่ละตัวมี CAN ID เป็น 0x140 + motor_id (ตัว ID1 ก็ 0x141) คำสั่งหมุนไปมุมสัมบูรณ์ใช้ไบต์แรกเป็น 0xA4 (position loop แบบมีเพดานความเร็ว) มุมที่ส่งคูณ 100 เพราะหน่วยของมอเตอร์คือ 0.01 องศาต่อ 1 LSB นี่คือเหตุผลว่าทำไมสั่งได้ละเอียดถึงเศษองศา:

// angle = องศาที่ต้องการ, maxSpeed = เพดานความเร็ว dps
int32_t angleControl = angle * 100;   // 0.01 องศา/LSB
struct can_frame rotate_frame;
rotate_frame.can_id  = 0x140 + motor_id;
rotate_frame.can_dlc = 8;
rotate_frame.data[0] = 0xA4;                          // position loop 2
rotate_frame.data[1] = 0x00;
rotate_frame.data[2] = (uint8_t)(maxSpeed & 0xFF);   // เพดานความเร็ว low byte
rotate_frame.data[3] = (uint8_t)(maxSpeed >> 8);     // เพดานความเร็ว high byte
rotate_frame.data[4] = (uint8_t)(angleControl);      // มุม byte 0
rotate_frame.data[5] = (uint8_t)(angleControl >> 8); // มุม byte 1
rotate_frame.data[6] = (uint8_t)(angleControl >> 16);// มุม byte 2
rotate_frame.data[7] = (uint8_t)(angleControl >> 24);// มุม byte 3

ไบต์แรกนี่แหละที่กำหนดโหมดควบคุมทั้งหมด มอเตอร์ตระกูลนี้มีชุดคำสั่งพอสมควร ที่ใช้บ่อยมีประมาณนี้ ผมดึงมาจากไฟล์ตัวอย่างคำสั่ง CAN ของมอเตอร์โดยตรง:

0x88  เปิดมอเตอร์ (run)          0x81  หยุดมอเตอร์ (stop)
0x80  ปิดมอเตอร์ (off)           0xA2  speed loop (คุมความเร็ว)
0xA4  position loop 2 (คุมมุม + เพดานความเร็ว)
0x90  อ่านค่า encoder            0x9C  อ่านสถานะ 2 (อุณหภูมิ/กระแส/ความเร็ว/encoder)
0x19  ตั้งตำแหน่งปัจจุบันเป็นศูนย์ ลง ROM
0x30  อ่าน PID   0x31  เขียน PID ลง RAM   0x32  เขียน PID ลง ROM

# ตัวอย่าง: A4 00 78 00 50 46 00 00 = position loop2 ความเร็ว 120dps มุม +180°

Angle.txt: อินเทอร์เฟซที่ทำให้สั่งง่าย

จุดที่ผมชอบในดีไซน์ของชุดนี้คือการใช้ไฟล์ข้อความเป็นตัวกลาง ตัว can_send ไม่ได้เปิด socket ให้ใครมายิงคำสั่ง แต่วน loop อ่าน Angle.txt แล้ว ล้างไฟล์ทิ้งทุกรอบ พอมีใครเขียนบรรทัดใหม่เข้าไป รอบถัดไปมันก็หยิบไปแปลงเป็น CAN frame แล้วล้างต่อ กลไกนี้ทำให้เราจะสั่งจากอะไรก็ได้ที่เขียนไฟล์เป็น ไม่ว่าจะ shell, Node-RED หรือสคริปต์ Python

char buffer[256] = {0};
fgets(buffer, sizeof(buffer), file);
int motor_id, angle, maxSpeed, motor_id2, angle2, maxSpeed2;
int n = sscanf(buffer, "%d_%d_%d+%d_%d_%d",
               &motor_id, &angle, &maxSpeed,
               &motor_id2, &angle2, &maxSpeed2);
// n == 3 สั่งตัวเดียว, n == 6 สั่งพร้อมกันสองตัว (หน่วง 30ms ระหว่างสองเฟรม)

ข้อดีคือ debug ง่ายมาก อยากรู้ว่าสั่งอะไรก็แค่ echo เข้าไฟล์แล้วดูมอเตอร์ ข้อเสียคือมันคือ fire-and-forget ไม่มี ack กลับมาว่าไปถึงมุมจริงหรือยัง เพราะฉะนั้นถ้าจะทำ closed-loop จริงจังต้องอ่านสถานะกลับมาเองอีกทาง ซึ่งเป็นเรื่องของหัวข้อถัดไป

อ่านสถานะกลับมา: encoder 15-bit, มุม, อุณหภูมิ, กระแส

การสั่งอย่างเดียวไม่พอ ผมอยากรู้ด้วยว่ามอเตอร์อยู่มุมไหน ร้อนแค่ไหน กินกระแสเท่าไหร่ ตัวชุดพัฒนามีโปรแกรม can_receive แยกมาให้ ยิงคำสั่งอ่าน 0x90 (ค่า encoder) หรือ 0x9C (สถานะ 2) แล้วแกะไบต์ที่ตอบกลับมา รันแบบวนอ่านตามความถี่ก็ได้:

./can_receive --help              # ดูวิธีใช้ทั้งหมด
./can_receive --state 1           # อ่านอุณหภูมิ กระแส ความเร็ว encoder
./can_receive --fre 10 --state 0  # อ่านมุมซ้ำ 10 ครั้ง/วินาที (กด q เพื่อออก)

การแกะ frame ตอบกลับก็ตรงไปตรงมา อุณหภูมิอยู่ไบต์ที่ 1, กระแสแรงบิดสองไบต์ถัดมา, ความเร็วเป็น dps, และ encoder สองไบต์ท้าย ตัว encoder เป็น 15-bit โค้ดตัวอย่างแมปช่วง 0–16383 เป็นมุม 0–180 องศา:

int8_t   temperature = recv_frame.data[1];
int16_t  power   = (recv_frame.data[3] << 8) | recv_frame.data[2]; // กระแสแรงบิด
int16_t  speed   = (recv_frame.data[5] << 8) | recv_frame.data[4]; // dps
uint16_t encoder = (recv_frame.data[7] << 8) | recv_frame.data[6]; // 15-bit
float    angle   = encoder / 16383.0 * 180.0;                      // แปลงเป็นองศา

หาศูนย์มอเตอร์ด้วยการชนสุด (auto calibration)

ปัญหาของ gimbal ประกอบเองคือมอเตอร์ไม่รู้ว่า "ตรงกลาง" อยู่ตรงไหน ไม่มี limit switch ให้ด้วย ตัวชุดพัฒนาแก้ด้วยลูกเล่นที่ผมว่าฉลาด คือใช้กระแสแรงบิดเป็นตัวจับขอบ ในโปรแกรม Gimbal_init มันสั่งมอเตอร์ด้วย speed loop ให้ค่อยๆ หมุนไปทางเดียว แล้วอ่านสถานะ 0x9C วนไปเรื่อยๆ พอแกนหมุนไปชนสุดทางกลไก มอเตอร์จะออกแรงดันสุดตัว กระแสแรงบิดจะพุ่งขึ้น พอเกินเกณฑ์ (ราว 5) ก็รู้ได้ว่าถึงขอบแล้ว จากนั้นเขียนตำแหน่งนั้นเป็นศูนย์ลง ROM ด้วยคำสั่ง 0x19 แล้วค่อยสั่งกลับมาตรงกลาง

power = power / 2048.0 * 33;      // แปลงกระแสแรงบิดเป็นหน่วยจริงโดยประมาณ
if (power >= 5.00) {              // แรงบิดพุ่ง = แกนชนสุดแล้ว
    send_frame(s, &zero_frame);  // 0x19 ตั้งตำแหน่งนี้เป็นศูนย์ ลง ROM
    if (motor_id == 1)  send_frame(s, &center_frame_1); // 0xA4 กลับมา 180°
    else if (motor_id == 2) send_frame(s, &center_frame_2); // กลับมา 90°
    break;                       // จบการหาศูนย์
}

ส่วนตัวผมชอบวิธีนี้เพราะไม่ต้องเพิ่มฮาร์ดแวร์อะไรเลย ใช้เซนเซอร์ที่มีอยู่แล้วในมอเตอร์ทำงานสองหน้าที่ แต่มีข้อควรระวังคือเกณฑ์กระแสต้องตั้งให้พอดี ถ้าตั้งต่ำไปยังไม่ทันชนก็หยุด ถ้าสูงไปมอเตอร์จะดันชนแรงเกินจนเคสพลาสติกล้าได้ ตรงนี้ปรับตามน้ำหนักกล้องและความฝืดของแกนที่เราประกอบเองครับ

Node-RED dashboard + PID + auto-track

ถ้าไม่อยากสั่งจาก shell ตลอด ชุดนี้มี flow ของ Node-RED มาให้ import แล้วได้หน้า dashboard ครบ มีทั้งภาพจากกล้องสด, ตั้งค่า confidence/IoU ของโมเดล, ปุ่มบังคับมือ, auto-track ให้กล้องหันตามวัตถุ, ปุ่มลัด sleep / standby / calibration / Emergency Stop และหน้าปรับ PID แยกต่อแกน เพราะแต่ละคนประกอบเองน้ำหนักไม่เท่ากัน ค่าจูนเลยต้องปรับเอง

PID แยกเป็นสองชั้นต่อแกน คือ angle loop (นอก) กับ speed loop (ใน) ค่าเริ่มต้นที่ flow แนะนำมาเป็นแบบนี้:

# แกน Yaw
angle:  Kp = 50   Ki = 0    Kd = 0
speed:  Kp = 200  Ki = 20   Kd = 0     (แนะนำ ~260 / 20 / 0)

# แกน Pitch
angle:  Kp = 50   Ki = 0    Kd = 0
speed:  Kp = 100  Ki = 20   Kd = 0

เวลากด save flow จะเขียนค่าลงเป็นไฟล์คอนฟิกในบ้าน user ของกล้อง รูปแบบแต่ละบรรทัดคือ motorID axis Kp Ki Kd โดย 0A คือ angle loop และ 0B คือ speed loop:

1 0A 50 0 0      # motor1 (yaw) angle PID
1 0B 260 20 0    # motor1 (yaw) speed PID
2 0A 50 0 0      # motor2 (pitch) angle PID
2 0B 260 20 0    # motor2 (pitch) speed PID

ส่วน auto-track จริงๆ ก็คือวนอ่านกล่องวัตถุจากโมเดล เอาระยะห่างจากจุดกลางภาพมาผ่าน gain แล้วเขียนมุมเป้าหมายลง Angle.txt ตัวเดิม เพราะ can_send คอยอ่านไฟล์นั้นอยู่แล้ว เราเลยทำ controller เป็นภาษาอะไรก็ได้ ผมลองเขียนเป็น Python สั้นๆ ให้เห็นภาพว่ามันต่อกับไฟล์เดียวกันยังไง (ค่า gain กับ deadzone ปรับเอาเองตามหน้างาน):

# auto_track.py — host-side controller เขียนมุมลง Angle.txt ให้ can_send อ่านต่อ
FRAME_W, FRAME_H = 640, 480          # ขนาดภาพจากโมเดล
KP_YAW, KP_PITCH = 0.03, 0.03        # แปลง error พิกเซล -> องศา
DEADZONE = 20                        # นิ่งได้ ไม่ต้องขยับ
yaw, pitch = 90, 90                  # มุมกลางเริ่มต้น

def track(box_cx, box_cy):
    global yaw, pitch
    ex = box_cx - FRAME_W / 2        # error แนวนอน
    ey = box_cy - FRAME_H / 2        # error แนวตั้ง
    if abs(ex) > DEADZONE:
        yaw = clamp(yaw - ex * KP_YAW, 0, 180)
    if abs(ey) > DEADZONE:
        pitch = clamp(pitch + ey * KP_PITCH, 0, 180)
    with open("Angle.txt", "a") as f:
        f.write(f"1_{int(yaw)}_90+2_{int(pitch)}_90\n")

def clamp(v, lo, hi):
    return max(lo, min(hi, v))

จะเห็นว่าสุดท้ายมันก็มาลงที่บรรทัดเดียวกันกับที่เราพิมพ์มือตอนแรก คือ 1_มุม_ความเร็ว+2_มุม_ความเร็ว ต่างกันแค่ว่าคราวนี้มีโมเดล AI เป็นคนเลือกมุมให้แทนเรา

พิมพ์เคส 3D จากไฟล์ STP

ตัวโครง gimbal ทั้งชุดมีไฟล์ STP มาให้พิมพ์เอง ตั้งแต่แขนยึดกล้อง, กล่องฐาน, ฝาครอบเซนเซอร์, ไปจนถึงเคสสำหรับบอร์ด CAN แต่ละชิ้นแยกไฟล์ โหลดไปเปิดในโปรแกรม slice แล้วสั่งพิมพ์ วัสดุเดิมของตัวสำเร็จเป็นไนลอน PA แต่พิมพ์ด้วย PETG หรือ PLA+ ก็พอไหวถ้าไม่ได้ใช้งานหนัก เสร็จแล้วประกอบมอเตอร์สองตัวเข้าแกน ตั้ง ID เป็น 01 กับ 02 ด้วยเครื่องมือปรับมอเตอร์ก่อน แล้วค่อยรัน Gimbal_init หาศูนย์อีกที เป็นอันครบวงจรตั้งแต่โค้ดยันโครง

ที่เล่ามาเป็นแกนหลักพอให้เอากล้องขึ้นแกนแล้วสั่งหันได้จริง ส่วนที่ผมยังเล่นไม่จบคือการจูน PID ให้ตามวัตถุเร็วๆ แล้วไม่แกว่ง กับการอัปเดตเฟิร์มแวร์มอเตอร์ ใครจะเอาโครงนี้ไปต่อ ปรับ gain, เกณฑ์กระแสหาศูนย์ และค่า PID ตามฮาร์ดแวร์ที่ประกอบเองได้ตามสะดวกครับ ตรงไหนผมอธิบายตกหล่นหรือพิมพ์ผิด เดี๋ยวกลับมาแก้เพิ่มให้ภายหลังครับ