BÀI 48 – ĐIỀU KHIỂN VỊ TRÍ MOTOR BẰNG ENCODER TRÊN STM32F103

Đây là bước rất quan trọng sau Bài 45–47.

Ở Bài 45, chúng ta biết encoder đo được vị trí.

Ở Bài 46, chúng ta dùng:

PWM + Encoder

để biết motor đang chạy bao nhiêu RPM.

Ở Bài 47, chúng ta đưa PID vào để điều khiển tốc độ.

Còn Bài 48 sẽ nâng cấp thêm một tầng:

Ra lệnh motor quay đến đúng vị trí, rồi tự dừng tại vị trí đó.

---

1. Từ điều khiển tốc độ sang điều khiển vị trí

Ví dụ ta muốn motor quay:

1000 counts

Ta không muốn chỉ nói:

PWM = 50%

mà muốn:

Motor → 1000 counts → STOP

Sơ đồ:

Target Position
      │
      ▼
   So sánh
      │
      ▼
 Position Error
      │
      ▼
 Controller
      │
      ▼
     PWM
      │
      ▼
    MOTOR
      │
      ▼
   ENCODER
      │
      ▼
Current Position
      │
      └───────────────► So sánh

Đây chính là:

Position Closed Loop

---

2. Encoder đang đo cái gì?

Giả sử encoder của motor có:

400 counts/revolution

Khi motor quay:

0
100
200
300
400

ta biết motor đã quay:

0°
90°
180°
270°
360°

Công thức:

[
Angle = \frac{Count}{CPR}\times360^\circ
]

---

3. Vị trí không nhất thiết phải tính bằng độ

Trong robot, đôi khi thuận tiện hơn nếu dùng:

count

Ví dụ:

Target = 2000 counts

Encoder:

Current = 1500 counts

thì:

[
Error = 2000-1500=500
]

Motor phải tiếp tục quay.

---

4. Khi gần đến vị trí

Ví dụ:

Target = 2000
Current = 1900

thì:

Error = 100

Ta không nên tiếp tục chạy motor với PWM lớn.

Nếu làm:

PWM = 80%

motor có thể vượt qua:

2000

và bắt đầu:

1900 → 2000 → 2100 → 2200

Đây gọi là:

Overshoot

---

5. Vì vậy cần giảm tốc khi gần đích

Ví dụ:

Error lớn
    ↓
PWM lớn

nhưng:

Error nhỏ
    ↓
PWM nhỏ

Đây chính là ý tưởng của P – Proportional.

---

6. Bộ điều khiển P đơn giản

Công thức:

[
PWM = K_p \times Error
]

Ví dụ:

Kp = 0.2
Error = 500

thì:

[
PWM=0.2\times500=100
]

Nếu giới hạn PWM:

0 → 100%

thì:

PWM = 100%

---

Ví dụ:

Error = 100

thì:

[
PWM=0.2\times100=20
]

→ PWM chỉ còn 20%.

---

7. Đây là một điểm cực kỳ quan trọng

Ta có:

Target
  ↓
Error lớn
  ↓
Motor chạy nhanh

Khi motor tiến gần:

Error giảm
  ↓
PWM giảm
  ↓
Motor chậm lại

Đến:

Error ≈ 0

thì:

PWM ≈ 0

Motor dừng.

---

8. Nhưng P đơn thuần có vấn đề

Giả sử:

Target = 1000
Current = 995

thì:

Error = 5

và:

Kp = 0.2

suy ra:

PWM = 1%

Motor có thể không đủ lực để quay.

Khi đó motor đứng ở:

995

thay vì:

1000

Đây là:

Static error / steady-state error

---

9. Vì vậy điều khiển vị trí thực tế thường phức tạp hơn

Một cách rất phổ biến là:

Position Controller
       ↓
Target Speed
       ↓
Speed Controller
       ↓
PWM
       ↓
Motor
       ↓
Encoder

Tức là có hai vòng điều khiển.

---

10. Cascade Control

Đây là kiến trúc rất quan trọng.

              POSITION LOOP
Target Position
      │
      ▼
Position Error
      │
      ▼
 Position PID
      │
      ▼
Target RPM
      │
      ▼
              SPEED LOOP
   Actual RPM ◄────────────┐
      │                    │
      ▼                    │
 Speed PID                 │
      │                    │
      ▼                    │
     PWM                   │
      │                    │
      ▼                    │
    MOTOR ───► ENCODER ────┘

Đây gọi là:

Cascade Control – điều khiển vòng tầng.

---

11. Nhưng trong Bài 48 chúng ta chưa cần phức tạp ngay

Đầu tiên hãy hiểu:

Position P Controller

Sau đó mới nâng cấp thành:

Position PID

hoặc tốt hơn:

Position → Speed → PWM

---

12. Đọc vị trí từ Encoder

Theo Bài 45:

int32_t position;

Ví dụ:

position = (int16_t)TIM2->CNT;

Nhưng có một vấn đề.

---

13. Counter 16-bit có giới hạn

TIM2:

0 → 65535

Nếu motor quay quá nhiều:

65535
↓
0

Do đó nếu chúng ta muốn điều khiển vị trí lâu dài:

100000 counts

thì chỉ dùng:

TIM2->CNT

không đủ.

---

14. Tạo Position Counter 32-bit

Ta có thể duy trì:

volatile int32_t encoder_position;

và cập nhật dựa trên delta.

Ví dụ:

int16_t current;
int16_t delta;

current = (int16_t)TIM2->CNT;

delta = current - last_count;

encoder_position += delta;

last_count = current;

---

15. Tại sao cách này xử lý overflow?

Ví dụ:

last = 65530
current = 5

Nếu xử lý đúng kiểu signed 16-bit:

delta = +11

Motor thực sự đã quay tiến 11 counts.

Ngược lại:

last = 5
current = 65530

thì:

delta = -11

Đây là kỹ thuật rất quan trọng khi làm encoder.

---

16. Driver Encoder nâng cấp

Ta có thể viết:

typedef struct
{
    int16_t last_count;
    int32_t position;
} Encoder_t;

Khai báo:

Encoder_t encoder;

---

17. Hàm cập nhật

void Encoder_Update(void)
{
    int16_t current;
    int16_t delta;

    current =
        (int16_t)TIM2->CNT;

    delta =
        current - encoder.last_count;

    encoder.position += delta;

    encoder.last_count =
        current;
}

---

18. Khởi tạo

void Encoder_Position_Init(void)
{
    TIM2->CNT = 0;

    encoder.last_count = 0;
    encoder.position = 0;
}

---

19. Đọc vị trí

int32_t Encoder_GetPosition(void)
{
    return encoder.position;
}

Bây giờ:

position = Encoder_GetPosition();

có thể là:

0
500
1000
5000
10000
...

thậm chí:

-5000

nếu quay ngược.

---

20. Đặt vị trí mục tiêu

Ta định nghĩa:

int32_t target_position;

Ví dụ:

target_position = 1000;

---

21. Tính sai số

int32_t error;

error =
    target_position -
    encoder.position;

Ví dụ:

Target = 1000
Current = 700

thì:

Error = +300

→ cần quay thuận.

---

Nếu:

Target = 1000
Current = 1300

thì:

Error = -300

→ cần quay ngược.

---

22. Đây chính là lý do Encoder cho biết chiều

Ta không cần tự hỏi:

Motor đang quay trái hay phải?

Chỉ cần:

Error > 0

→ một chiều.

Error < 0

→ chiều còn lại.

---

23. Hàm điều khiển vị trí P

Ví dụ:

#define KP_POSITION 0.2f
#define PWM_MAX     1000
#define PWM_MIN      100

Hàm:

void Position_Control(void)
{
    int32_t error;
    float output;

    error =
        target_position -
        encoder.position;

    output =
        KP_POSITION *
        error;

    if (output > PWM_MAX)
        output = PWM_MAX;

    if (output < -PWM_MAX)
        output = -PWM_MAX;

    Motor_SetPWM((int16_t)output);
}

---

24. Vấn đề PWM_MIN

Nếu:

output = 10

thì motor có thể không quay vì ma sát.

Do đó:

0 < PWM < PWM_MIN

ta có thể ép thành:

PWM_MIN

Ví dụ:

if (output > 0 &&
    output < PWM_MIN)
{
    output = PWM_MIN;
}

Tương tự chiều âm.

---

25. Nhưng có một lỗi nếu làm như vậy

Giả sử:

Error = 1

P controller cho:

PWM = 0.2

Ta ép:

PWM = 100

Motor có thể chạy:

Target = 1000
Current = 999

→ motor chạy qua:

1000

→ 1001

→ 1002

→ …

→ controller đổi chiều.

Robot có thể:

← → ← → ← →

gần vị trí đích.

Đây là:

Hunting / Oscillation

---

26. Cần vùng chết – Deadband

Ví dụ:

#define POSITION_TOLERANCE 3

Nếu:

abs(error) <= 3

thì coi như đã tới vị trí.

if (abs(error) <=
    POSITION_TOLERANCE)
{
    Motor_Stop();
    return;
}

---

27. Đây là kỹ thuật cực kỳ hữu ích

Ví dụ:

Target = 1000

Nếu:

Current = 998

thì:

Error = 2

và:

|Error| <= 3

→ STOP.

Không cần cố ép motor đến đúng từng count tuyệt đối.

---

28. Code Position Controller cơ bản

void Position_Control(void)
{
    int32_t error;
    float output;

    error =
        target_position -
        encoder.position;

    /*
        Deadband
    */

    if (error <= 3 &&
        error >= -3)
    {
        Motor_Stop();
        return;
    }

    /*
        P Controller
    */

    output =
        KP_POSITION *
        error;

    /*
        Limit
    */

    if (output > PWM_MAX)
        output = PWM_MAX;

    if (output < -PWM_MAX)
        output = -PWM_MAX;

    /*
        Minimum PWM
    */

    if (output > 0 &&
        output < PWM_MIN)
    {
        output = PWM_MIN;
    }

    if (output < 0 &&
        output > -PWM_MIN)
    {
        output = -PWM_MIN;
    }

    Motor_SetPWM(
        (int16_t)output);
}

---

29. Motor_SetPWM phải hiểu dấu

Ta cần một hàm kiểu:

void Motor_SetPWM(int16_t command)
{
    if (command > 0)
    {
        Motor_Forward();

        Motor_SetDuty(
            command);
    }

    else if (command < 0)
    {
        Motor_Reverse();

        Motor_SetDuty(
            -command);
    }

    else
    {
        Motor_Stop();
    }
}

Như vậy:

+500

→ quay thuận.

-500

→ quay ngược.

0

→ dừng.

---

30. Đây là một kiến trúc rất đẹp

Position Error
      │
      ▼
Position Controller
      │
      ▼
 -1000 ... +1000
      │
      ▼
Motor_SetPWM()
      │
 ┌────┴────┐
 ▼         ▼
DIR       PWM

---

31. Ví dụ thực tế

Giả sử:

Target = 2000
Current = 0
Kp = 0.5
PWM_MAX = 1000

Ban đầu:

Error = 2000

P:

PWM = 1000

Motor chạy nhanh.

---

Khi:

Current = 1200

thì:

Error = 800

PWM:

400

---

Khi:

Current = 1800

thì:

Error = 200

PWM:

100

---

Khi:

Current = 1998

thì:

Error = 2

nếu tolerance = 3:

STOP

---

32. Nhìn trực quan

PWM
100% ┤████████████
     │
 80% ┤██████████
     │
 60% ┤████████
     │
 40% ┤████
     │
 20% ┤██
     │
  0% ┼──────────────────
       0             Target
       Position →

Càng gần target:

PWM ↓

---

33. Nhưng đây vẫn chưa phải giải pháp tốt nhất

Bạn sẽ thấy một vấn đề:

Khi motor chạy nhanh:

Error lớn

P controller cấp PWM lớn.

Khi gần đích:

Error nhỏ

PWM giảm.

Nhưng motor có quán tính.

Ví dụ:

Current = 1950
Target = 2000

PWM vẫn còn 25%.

Motor đang chạy nhanh và có thể vượt:

2000

đến:

2040

sau đó controller đảo chiều.

---

34. Đây là lúc Bài 47 phát huy tác dụng

Ở Bài 47 ta đã học:

Speed PID

Do đó kiến trúc tốt hơn là:

Target Position
       │
       ▼
Position Controller
       │
       ▼
Target RPM
       │
       ▼
Speed PID
       │
       ▼
PWM
       │
       ▼
Motor
       │
       ▼
Encoder

Đây là kiến trúc cascade.

---

35. Vòng ngoài

Position Controller:

Position Error
     ↓
Target Speed

Ví dụ:

Error = 1000 counts
→ Target Speed = 100 RPM

Khi gần đích:

Error = 100 counts
→ Target Speed = 20 RPM

---

36. Vòng trong

Speed Controller:

Target RPM
    ↓
Speed PID
    ↓
PWM

Encoder đo:

Actual RPM

và PID liên tục điều chỉnh PWM.

---

37. Vì sao cascade tốt hơn?

Vì:

Vòng vị trí

quan tâm:

"Tôi đã tới đâu?"

Vòng tốc độ

quan tâm:

"Tôi đang chạy nhanh bao nhiêu?"

Hai vòng phối hợp:

Position
   ↓
Speed
   ↓
PWM

---

38. Ví dụ

Target:

1000 counts

Current:

0

Position loop:

Error = 1000

→ yêu cầu:

Target RPM = 100 RPM

---

Khi:

Current = 800

thì:

Error = 200

Position loop giảm:

Target RPM = 30 RPM

---

Khi:

Current = 980

thì:

Error = 20

Target RPM:

5 RPM

---

Khi:

Current ≈ 1000

thì:

Target RPM = 0

→ Motor dừng.

---

39. Đây là cách robot công nghiệp làm

Không nhất thiết phải điều khiển:

Position → PWM trực tiếp

mà thường:

Position Loop
      ↓
Velocity Loop
      ↓
Current/Torque Loop
      ↓
Motor

Các hệ thống servo chuyên nghiệp thậm chí có:

Position
 ↓
Velocity
 ↓
Current

nhiều vòng điều khiển lồng nhau.

---

40. Bài tập 1

Giả sử:

CPR = 400
Target = 800 counts

Motor cần quay:

[
800/400=2
]

→ 2 vòng.

---

41. Bài tập 2

Encoder:

400 CPR

Target:

1000 counts

Góc:

[
1000/400\times360
]

=

900°

Tức:

2.5 vòng

---

42. Bài tập 3 – quay chính xác 90°

Encoder:

400 CPR

Muốn:

90°

Target:

[
400\times90/360
]

=

100 counts

Do đó:

target_position = 100;

Motor quay tới:

100 counts

→ khoảng 90°.

---

43. Bài tập 4 – robot

Giả sử bánh xe:

D = 70 mm
CPR = 400

Muốn robot đi:

500 mm

Số vòng:

[
500/(\pi\times70)
]

≈

2.27 vòng

Số count:

[
2.27\times400
]

≈

908 counts

Vậy:

target_position = 908;

---

44. Nhưng robot hai bánh có một vấn đề

Nếu chỉ có một encoder:

Encoder
   ↓
Motor

ta biết bánh đó đã đi bao xa.

Nhưng robot có:

Left Motor
Right Motor

thì nên có:

Left Encoder
Right Encoder

---

45. Robot đi thẳng

Ví dụ:

Left = 1000 counts
Right = 1000 counts

→ hai bánh đi cùng khoảng cách.

Robot có xu hướng đi thẳng.

Nhưng nếu:

Left = 1000
Right = 900

thì:

Left bánh đi xa hơn

robot sẽ lệch hướng.

---

46. Điều này dẫn đến bài học tiếp theo

Ta sẽ cần:

Left Position
Right Position

và so sánh:

Left Error
Right Error

để điều khiển từng motor độc lập.

---

47. Từ Bài 48 tới robot thực tế

Kiến trúc sẽ dần trở thành:

             TARGET
                │
                ▼
        Position Controller
          ┌─────┴─────┐
          ▼           ▼
       Left Ref     Right Ref
          │           │
          ▼           ▼
       Speed PID    Speed PID
          │           │
          ▼           ▼
       Left PWM     Right PWM
          │           │
          ▼           ▼
       Left Motor   Right Motor
          │           │
          ▼           ▼
       Left ENC     Right ENC
          │           │
          └─────┬─────┘
                ▼
             Feedback

Đây chính là nền móng của robot di chuyển chính xác.

---

48. Một lưu ý rất quan trọng về Motor_Stop()

Có nhiều kiểu dừng motor:

Coast

PWM = 0

Motor tự giảm tốc.

Brake

Hai đầu motor được điều khiển để hãm.

Active Brake

Driver motor chủ động tạo mô-men hãm.

Đối với điều khiển vị trí, kiểu dừng ảnh hưởng rất lớn đến:

overshoot

Nếu motor có quán tính lớn, chỉ:

PWM = 0;

có thể chưa đủ.

---

49. Vì vậy khi tuning Position Controller

Bạn phải quan sát:

Target = 1000

Motor thực tế:

0
200
400
600
800
950
1000

là tốt.

Nhưng nếu:

0
300
700
1100
900
1050
980
1020
...

thì hệ thống đang dao động.

Cần điều chỉnh:

Kp
PWM_MAX
PWM_MIN
deadband
speed limit

---

50. Ba thông số rất quan trọng

Đối với Position Controller đơn giản:

#define POSITION_KP       0.5f
#define POSITION_MAX_PWM 1000
#define POSITION_DEADBAND 3

Đừng bắt đầu với giá trị quá lớn.

Hãy:

Kp nhỏ
 ↓
test
 ↓
tăng dần

---

51. Quy trình tuning

Bước 1

Cho:

Kp rất nhỏ

Ví dụ:

0.05

Bước 2

Ra lệnh:

1000 counts

Bước 3

Quan sát.

Nếu motor quá chậm:

Kp ↑

Nếu motor vượt đích mạnh:

Kp ↓

---

52. Đừng vội thêm PID vị trí

Một lỗi người mới thường mắc là:

Encoder
+
PID
+
PWM
+
Position
+
Speed

tất cả đưa vào cùng một công thức ngay từ đầu.

Rất khó debug.

Nên học theo thứ tự:

1. Encoder
      ↓
2. Position
      ↓
3. P Position
      ↓
4. Speed PID
      ↓
5. Position → Speed
      ↓
6. Cascade

---

53. Bài 48 bạn cần nắm chắc 10 ý

1.

Encoder:

A + B

cho biết vị trí và chiều.

2.

CNT

là bộ đếm phần cứng.

3.

Vị trí có thể biểu diễn bằng:

counts

4.

Góc:

[
Angle=Count/CPR\times360
]

5.

Khoảng cách:

[
Distance=Count/CPR\times\pi D
]

6.

Sai số:

[
Error=Target-Current
]

7.

P Controller:

[
Output=K_p\times Error
]

8.

Deadband:

|Error| <= tolerance
→ STOP

9.

P position có thể bị:

Overshoot
Hunting
Steady-state error

10.

Giải pháp chuyên nghiệp:

Position Loop
      ↓
Speed Loop
      ↓
PWM

---

54. Toàn bộ kiến thức từ Bài 41 → 48

Bây giờ chuỗi kiến thức của bạn đã rất đẹp:

Bài 41
PWM
 │
 ▼
Bài 42
Servo
 │
 ▼
Bài 43
HC-SR04
 │
 ▼
Bài 44
HC-SR04 State Machine
 │
 ▼
Bài 45
Encoder Mode
 │
 ▼
Bài 46
PWM + Encoder
 │
 ▼
Bài 47
Speed PID
 │
 ▼
Bài 48
Position Control

Và bước tiếp theo rất tự nhiên là:

BÀI 49 – PID ĐIỀU KHIỂN VỊ TRÍ MOTOR + CASCADE POSITION → SPEED

Ở Bài 49, chúng ta sẽ không chỉ dùng:

Position P

mà sẽ xây dựng một cấu trúc hoàn chỉnh:

TARGET POSITION
       │
       ▼
┌──────────────────┐
│ POSITION CONTROL │
└────────┬─────────┘
         │ Target RPM
         ▼
┌──────────────────┐
│    SPEED PID     │
└────────┬─────────┘
         │ PWM
         ▼
       MOTOR
         │
         ▼
      ENCODER
         │
     ┌───┴────┐
     ▼        ▼
 POSITION    RPM
     │        │
     └────────┘

Đây là lúc bạn bắt đầu thực sự hiểu servo motor và hệ thống điều khiển chuyển động, chứ không còn chỉ là lập trình GPIO/Timer đơn thuần nữa.