เซอร์โว 2 แกนวิ่งพร้อมกัน — XY Linear Interpolation

หลักการ

เครื่อง CNC, plotter, pick-and-place ต้องการวิ่ง 2 แกน พร้อมกัน ให้ end-effector เดินตามเส้นตรง (linear interpolation)

ปัญหาถ้าใช้ DRVA แยก

ถ้า X = 50000 pulse, Y = 20000 pulse, ความถี่เท่ากัน 10 kHz:

Y axis: 20000 / 10000 Hz = 2 sec  (เสร็จก่อน)
X axis: 50000 / 10000 Hz = 5 sec

เส้นทาง: Y พุ่งขึ้นก่อน, ค่อย X พุ่งไปต่อ — เป็น L-shape ไม่ใช่ diagonal

วิธีแก้: Frequency Scaling

ปรับความถี่ของแกนสั้นกว่า ให้ใช้เวลาเท่ากัน:

F_long = 10000 Hz (กำหนดมาตรฐานแกนยาว)
F_short = F_long × pulses_short / pulses_long
        = 10000 × 20000 / 50000
        = 4000 Hz

X: 50000 / 10000 Hz = 5 sec
Y: 20000 / 4000 Hz  = 5 sec  ← เสร็จพร้อมกัน

ทั้งคู่ออก pulse 5 วินาที พร้อมกัน → end-effector เดินเส้นตรง

คำนวณใน PLC

D200 = X_target (pulse, signed)
D201 = Y_target (pulse, signed)
D202 = F_max = 10000 Hz (max frequency for long axis)

Compute (every Start):
|X| = |D200|, |Y| = |D201|
ถ้า |X| >= |Y|:
  F_X = D202 (full speed)
  F_Y = D202 × |Y| / |X|
มิฉะนั้น:
  F_Y = D202
  F_X = D202 × |X| / |Y|

ตัวอย่างนี้ใช้ EDRVA แทน DRVA เพราะรองรับการเปลี่ยนค่าระหว่างทำงาน

ลำดับการทำงาน

First scan
  ↓
M1 (X done) = M2 (Y done) = OFF
F_max = 10000 (D202)
  ↓
HMI กรอก D200, D201
  ↓
กด Start (X0) (rising edge)
  ↓
คำนวณ F_X, F_Y (rungs จะแสดง)
  ↓
EDRVA แกน X → D200 ที่ความถี่ D210
EDRVA แกน Y → D201 ที่ความถี่ D211
  ↓
M8134↓ และ M8132↓ (Y2 pulse flag) → SET M1, M2
  ↓
M1 ∧ M2 → SET M3 (Both Done)

ข้อจำกัด

• ตัวอย่างนี้ทำงานเฉพาะ กรณี ทุกค่าเป็นบวก (ไม่กำลังกลับทิศ)
• งานจริงต้อง:
  • Direction handling (Y4 / Y5) — ตรวจ sign ของ delta แล้วตั้งทิศก่อน
  • Acceleration sync — accel ทั้งคู่จบพร้อมกัน (advanced)
  • Multi-segment path — interpolate G-code style

ขยายเพิ่ม

• 3-axis — เพิ่ม Z (Y4 pulse + Y6 dir) แล้ว F = max ของทั้ง 3
• Circular interpolation — ต้อง sin/cos table (advanced — ใช้ FB หรือ Macro)
• G-code parser — อ่านจาก SD card หรือ Modbus, แปลง 1 G01 = 1 sync move