BÀI 52 – ĐIỀU KHIỂN GÓC QUAY ROBOT BẰNG 2 ENCODER TRÊN STM32F103

Posted on by congnghe9s

BÀI 52 – ĐIỀU KHIỂN GÓC QUAY ROBOT BẰNG 2 ENCODER TRÊN STM32F103

Ở Bài 51, chúng ta đã đưa robot từ điều khiển motor lên điều khiển chuyển động:

Robot_MoveDistance(1000);

Bài 52 sẽ thêm một khả năng cực kỳ quan trọng:

Robot_TurnAngle(90);

Tức là robot có thể hiểu:

“Hãy quay đúng 90°.”

Đây chính là bước đầu tiên để xây dựng odometry cho robot 2 bánh.

1. Mục tiêu Bài 52

Sau bài này, robot có thể thực hiện chuỗi:

START
  │
  ▼
Đi thẳng 500 mm
  │
  ▼
Quay trái 90°
  │
  ▼
Đi thẳng 1000 mm
  │
  ▼
Quay phải 90°
  │
  ▼
STOP

Kiến trúc:

             TARGET ANGLE
                  │
                  ▼
           ANGLE CONTROLLER
                  │
          ┌───────┴───────┐
          ▼               ▼
       MOTOR L          MOTOR R
       Forward           Reverse
          │               │
          ▼               ▼
       ENCODER L        ENCODER R
          │               │
          └───────┬───────┘
                  ▼
             ANGLE ERROR

2. Tại sao 2 encoder có thể đo góc quay?

Hãy nhìn robot từ trên xuống:

             FRONT
               ↑

       ┌─────────────────┐
       │                 │
       O                 O
     LEFT              RIGHT

       <---- WHEEL BASE ---->

Gọi khoảng cách giữa tâm hai bánh là:

[
W
]

Ví dụ:

W = 150 mm

3. Khi robot quay tại chỗ

Ta cho:

Left  → lùi
Right → tiến

Robot quay:

             ↑
             │
        ↖    │    ↗
          \  │  /
           \ │ /
        L ← ROBOT → R

Hai bánh di chuyển quãng đường ngược chiều nhau.

4. Công thức cực kỳ quan trọng

Khi robot quay một góc:

[
\theta
]

quãng đường mỗi bánh đi là:

[
s = \frac{\theta W}{2}
]

với θ tính bằng radian.

Do đó:

[
\theta =
\frac{s_L-s_R}{W}
]

Đây là một trong những công thức quan trọng nhất của robot differential drive.

5. Tại sao là sL - sR?

Ví dụ robot quay trái:

Left  = -100 mm
Right = +100 mm

thì:

[
s_L-s_R=-200
]

Góc quay có dấu âm/dương tùy quy ước của chúng ta.

Điều quan trọng là:

Độ chênh lệch quãng đường hai bánh quyết định góc quay.

6. Ví dụ thực tế

Giả sử:

Wheel base = 150 mm

Muốn quay:

90°

Đổi sang radian:

[
90^\circ = \frac{\pi}{2}
]

Quãng đường mỗi bánh:

[
s =
\frac{1.5708\times150}{2}
]

≈

117.81 mm

Tức là khi quay tại chỗ 90°:

Left  ≈ -117.8 mm
Right ≈ +117.8 mm

7. Đổi mm thành encoder counts

Giả sử:

COUNTS_PER_MM = 1.961

thì:

[
117.81\times1.961
]

≈

231 counts

Do đó quay 90° cần khoảng:

Left  = -231 counts
Right = +231 counts

8. Đây là cách đơn giản nhất để quay góc

Ta có thể tạo:

#define WHEEL_BASE_MM 150.0f
#define COUNTS_PER_MM 1.961f

Sau đó:

float AngleToWheelDistance(float angle_deg)
{
    float angle_rad;

    angle_rad =
        angle_deg * 3.14159265f / 180.0f;

    return
        (angle_rad * WHEEL_BASE_MM) / 2.0f;
}

9. Ví dụ

float distance;

distance =
    AngleToWheelDistance(90.0f);

Kết quả:

distance ≈ 117.8 mm

10. Đổi sang encoder

int32_t TurnAngleToCounts(
    float angle_deg)
{
    float distance;

    distance =
        AngleToWheelDistance(angle_deg);

    return (int32_t)
           (distance * COUNTS_PER_MM);
}

Ví dụ:

TurnAngleToCounts(90.0f);

→ khoảng:

231 counts

11. Nhưng chúng ta cần phân biệt hai loại chuyển động

Đi thẳng

Left  = +
Right = +

hoặc:

Left  = -
Right = -

Quay tại chỗ

Left  = -
Right = +

hoặc:

Left  = +
Right = -

12. Hàm Robot_TurnAngle()

Ta tạo:

void Robot_TurnAngle(float angle_deg)
{
    int32_t counts;

    counts =
        TurnAngleToCounts(angle_deg);

    robot_turn.left_start =
        left_encoder.position;

    robot_turn.right_start =
        right_encoder.position;

    robot_turn.target_counts =
        counts;

    robot_turn.active = 1;
}

Nhưng ở đây còn thiếu dấu của góc.

13. Quy ước góc

Ta thống nhất:

+ angle = quay trái
- angle = quay phải

Ví dụ:

Robot_TurnAngle(+90);

→ trái 90°.

Robot_TurnAngle(-90);

→ phải 90°.

14. Khi quay trái

Ta muốn:

Left  → âm
Right → dương

Do đó:

target_left_rpm  = -turn_speed;
target_right_rpm = +turn_speed;

Ví dụ:

-40 RPM
+40 RPM

15. Khi quay phải

Left  → dương
Right → âm

Ví dụ:

+40 RPM
-40 RPM

16. Tính góc thực tế từ encoder

Đây là phần quan trọng nhất của Bài 52.

Ta có:

left_distance
right_distance

Sau đó:

angle_rad =
    (left_distance -
     right_distance)
    / WHEEL_BASE_MM;

17. Đổi radian → độ

[
angle_{deg}

angle_{rad}
\times
\frac{180}{\pi}
]

Code:

float RadToDeg(float rad)
{
    return
        rad * 180.0f /
        3.14159265f;
}

18. Hàm tính góc robot

float Robot_GetAngle(void)
{
    float left_mm;
    float right_mm;

    float angle_rad;

    left_mm =
        left_encoder.position /
        COUNTS_PER_MM;

    right_mm =
        right_encoder.position /
        COUNTS_PER_MM;

    angle_rad =
        (left_mm - right_mm) /
        WHEEL_BASE_MM;

    return RadToDeg(angle_rad);
}

Tuy nhiên cách này có một vấn đề.

19. Không nên dùng vị trí tuyệt đối mãi mãi

Ví dụ:

Robot chạy 100 mét

encoder position:

hàng trăm nghìn counts

Sai số nhỏ tích lũy dần.

Ta nên xây dựng:

Odometry

với:

delta_left
delta_right

sau mỗi chu kỳ.

20. Odometry là gì?

Hiểu đơn giản:

Odometry là dùng chuyển động của bánh xe để ước lượng robot đang ở đâu và đang quay hướng nào.

Robot không cần GPS.

Nó tự tính từ:

Encoder Left
Encoder Right

21. Mô hình Odometry

Ta lưu:

typedef struct
{
    float x;
    float y;
    float theta;

} Odometry_t;

Ví dụ:

x = 500 mm
y = 200 mm
theta = 90°

nghĩa là robot đang ở:

(500, 200)

và hướng:

90°

22. Khởi tạo

Odometry_t odom;

void Odometry_Init(void)
{
    odom.x = 0.0f;
    odom.y = 0.0f;
    odom.theta = 0.0f;
}

Ta quy ước:

x → hướng ban đầu
y → sang trái
theta → góc quay

23. Mỗi chu kỳ đọc encoder

Giả sử:

ΔL
ΔR

là quãng đường hai bánh đi trong khoảng thời gian vừa qua.

Tính:

[
\Delta S =
\frac{\Delta L+\Delta R}{2}
]

Code:

float delta_s =
    (delta_left_mm +
     delta_right_mm)
    / 2.0f;

24. Tính góc thay đổi

[
\Delta\theta =
\frac{\Delta L-\Delta R}{W}
]

Code:

float delta_theta =
    (delta_left_mm -
     delta_right_mm)
    / WHEEL_BASE_MM;

25. Cập nhật theta

odom.theta += delta_theta;

Ví dụ:

theta = 0°

sau một lúc:

theta = 20°

robot đã quay 20°.

26. Nhưng cập nhật X/Y như thế nào?

Nếu robot đang đi:

theta = 0°

thì:

x tăng
y không đổi

Nếu:

theta = 90°

thì:

y tăng
x gần như không đổi

Do đó:

[
\Delta x =
\Delta S\cos(\theta)
]

[
\Delta y =
\Delta S\sin(\theta)
]

27. Code Odometry

void Odometry_Update(
    float delta_left_mm,
    float delta_right_mm)
{
    float delta_s;
    float delta_theta;

    delta_s =
        (delta_left_mm +
         delta_right_mm)
        / 2.0f;

    delta_theta =
        (delta_left_mm -
         delta_right_mm)
        / WHEEL_BASE_MM;

    odom.theta +=
        delta_theta;

    odom.x +=
        delta_s *
        cosf(odom.theta);

    odom.y +=
        delta_s *
        sinf(odom.theta);
}

28. Tại sao dùng cosf() và sinf()?

Vì:

theta

là góc hướng robot.

Ví dụ:

theta = 0°

thì:

cos(0) = 1
sin(0) = 0

nên:

Δx = ΔS
Δy = 0

Robot đi thẳng theo trục X.

29. Khi robot quay 90°

theta = 90°

thì:

cos(90°) ≈ 0
sin(90°) ≈ 1

nên:

Δx ≈ 0
Δy ≈ ΔS

Robot đi theo trục Y.

30. Cực kỳ quan trọng: sinf() dùng radian

STM32/C chuẩn sử dụng:

sinf(theta);
cosf(theta);

với:

theta = radian

Không được truyền:

cosf(90);

nếu bạn nghĩ đó là 90°.

Phải:

float theta_rad =
    90.0f * 3.14159265f / 180.0f;

31. Vì vậy nên lưu theta bằng radian

Thay vì:

float theta_deg;

ta dùng:

float theta;

và quy ước:

theta = radian

Ví dụ:

0       → 0°
1.5708  → 90°
3.1416  → 180°

32. Cấu trúc Odometry hoàn chỉnh

typedef struct
{
    float x;
    float y;
    float theta;

    int32_t last_left;
    int32_t last_right;

} Odometry_t;

33. Khởi tạo

void Odometry_Init(void)
{
    odom.x = 0.0f;
    odom.y = 0.0f;
    odom.theta = 0.0f;

    odom.last_left =
        left_encoder.position;

    odom.last_right =
        right_encoder.position;
}

34. Update tự động

void Odometry_Update(void)
{
    int32_t left_now;
    int32_t right_now;

    int32_t delta_left;
    int32_t delta_right;

    float left_mm;
    float right_mm;

    float delta_s;
    float delta_theta;

    left_now =
        left_encoder.position;

    right_now =
        right_encoder.position;

    delta_left =
        left_now -
        odom.last_left;

    delta_right =
        right_now -
        odom.last_right;

    odom.last_left =
        left_now;

    odom.last_right =
        right_now;

    left_mm =
        delta_left /
        LEFT_COUNTS_PER_MM;

    right_mm =
        delta_right /
        RIGHT_COUNTS_PER_MM;

    delta_s =
        (left_mm +
         right_mm) / 2.0f;

    delta_theta =
        (left_mm -
         right_mm) /
        WHEEL_BASE_MM;

    odom.theta +=
        delta_theta;

    odom.x +=
        delta_s *
        cosf(odom.theta);

    odom.y +=
        delta_s *
        sinf(odom.theta);
}

35. Đây chính là Odometry thực sự

Bây giờ STM32 có thể biết:

X
Y
THETA

Ví dụ:

X = 523.4 mm
Y = 101.2 mm
THETA = 1.57 rad

Robot đang ở khoảng:

(523, 101)

và hướng:

90°

36. Ví dụ robot đi hình vuông

Ta có thể yêu cầu:

Đi 500 mm
Quay 90°
Đi 500 mm
Quay 90°
Đi 500 mm
Quay 90°
Đi 500 mm
Quay 90°

Về lý thuyết robot sẽ trở lại gần:

X = 0
Y = 0
theta = 0

37. Nhưng thực tế sẽ không như vậy

Sau một vòng có thể:

X = 37 mm
Y = -21 mm
theta = 4°

Đây chính là:

Odometry drift

38. Tại sao drift?

Sai số đến từ:

  • encoder;
  • đường kính bánh;
  • khoảng cách hai bánh;
  • bánh trượt;
  • sàn không phẳng;
  • backlash hộp số;
  • sai số lượng tử encoder.

Sai số nhỏ mỗi lần:

↓
tích lũy
↓
sai số lớn

39. Đây là giới hạn quan trọng của Encoder Odometry

Encoder cho chúng ta:

ước lượng

chứ không phải:

vị trí tuyệt đối chính xác

Muốn giảm drift lâu dài, robot có thể cần:

Encoder
+
IMU
+
Camera
+
LiDAR
+
GPS

tùy ứng dụng.

40. Quay góc 90° chính xác

Bây giờ quay lại mục tiêu chính của Bài 52.

Ta có:

target_angle = +90°

và:

current_angle

từ encoder.

Tính:

angle_error =
    target_angle -
    current_angle;

41. Angle PID

Ta có thể dùng PID:

TARGET ANGLE
     ↓
 ANGLE PID
     ↓
TURN SPEED
     ↓
┌────┴────┐
↓         ↓
-L        +R

Ví dụ:

turn_speed =
    AnglePID(target_angle,
             current_angle);

42. Nhưng ở Bài 52 chưa cần PID góc đầy đủ

Để dễ hiểu, trước tiên dùng:

P controller

[
V=K_p\times Error
]

Ví dụ:

#define ANGLE_KP 1.0f

Nếu:

Error = 30°

thì:

Turn speed = 30 RPM

43. Giới hạn tốc độ quay

#define MAX_TURN_SPEED 60.0f
#define MIN_TURN_SPEED 10.0f

Ta không muốn:

90°
→ 90 RPM

vì có thể quá nhanh.

44. Code điều khiển góc

float AngleController(
    float target,
    float current)
{
    float error;
    float speed;

    error =
        target - current;

    speed =
        ANGLE_KP * error;

    speed =
        Clamp(
            speed,
            -MAX_TURN_SPEED,
            MAX_TURN_SPEED);

    return speed;
}

45. Khi quay trái

speed > 0

ta đặt:

target_left_rpm  = -speed;
target_right_rpm = +speed;

46. Khi quay phải

Nếu:

speed < 0

thì:

target_left_rpm  = -speed;
target_right_rpm = +speed;

Công thức này thực ra dùng chung được.

Ví dụ:

speed = +40

Left  = -40
Right = +40

→ trái.

Nếu:

speed = -40

Left  = +40
Right = -40

→ phải.

47. Đến gần góc mục tiêu

Ví dụ:

Target = 90°
Current = 85°

error:

5°

nếu:

Kp = 1

thì:

Turn speed = 5 RPM

Robot tự chậm lại.

48. Tolerance

Ta tạo:

#define ANGLE_TOLERANCE 1.0f

Nếu:

abs(error) < 1°

thì:

target_left_rpm = 0;
target_right_rpm = 0;

và:

TURN COMPLETE

49. Một vấn đề rất quan trọng: góc phải wrap

Ví dụ:

Target = -179°
Current = +179°

Nếu tính bình thường:

-179 - 179 = -358°

Nhưng thực tế robot chỉ cần quay:

+2°

Do đó cần:

NormalizeAngle()

50. Hàm Normalize Angle

float NormalizeAngle(float angle)
{
    while (angle > 180.0f)
        angle -= 360.0f;

    while (angle < -180.0f)
        angle += 360.0f;

    return angle;
}

Sau đó:

error =
    NormalizeAngle(
        target -
        current);

51. Đây là kỹ năng rất quan trọng trong robotics

Mọi góc:

-180° ↔ +180°

phải được xử lý đúng.

Nếu không:

179°
→ 181°

robot có thể chọn quay:

358°

thay vì:

2°

52. State Machine của Bài 52

Bây giờ robot có:

typedef enum
{
    ROBOT_IDLE,
    ROBOT_DRIVE,
    ROBOT_TURN,
    ROBOT_REACHED
} RobotState_t;

53. ROBOT_DRIVE

Position Controller
        ↓
2 Speed PID

54. ROBOT_TURN

Angle Controller
       ↓
Left RPM  = -TurnSpeed
Right RPM = +TurnSpeed
       ↓
2 Speed PID

55. ROBOT_REACHED

Left RPM  = 0
Right RPM = 0

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

                    ROBOT
                      │
             ┌────────┴────────┐
             │                 │
             ▼                 ▼
          DRIVE             TURN
             │                 │
             ▼                 ▼
     POSITION CONTROL    ANGLE CONTROL
             │                 │
             ▼                 ▼
       TARGET SPEED       TURN SPEED
             │                 │
             └────────┬────────┘
                      ▼
                SPEED PID
               ┌──────┴──────┐
               ▼             ▼
            MOTOR L       MOTOR R
               │             │
               ▼             ▼
           ENCODER L     ENCODER R
               └──────┬──────┘
                      ▼
                   ODOMETRY
                  X,Y,THETA

57. Ví dụ chương trình cấp cao

Sau này main.c có thể rất đơn giản:

Robot_MoveDistance(500.0f);

while (!Robot_IsFinished())
{
}

Robot_TurnAngle(90.0f);

while (!Robot_IsFinished())
{
}

Robot_MoveDistance(1000.0f);

while (!Robot_IsFinished())
{
}

Đây là tư duy robot command, thay vì điều khiển PWM trực tiếp.

58. Tốt hơn nữa: Command Queue

Sau này ta có thể tạo:

MOVE 500
TURN 90
MOVE 1000
TURN -90
MOVE 300

STM32 thực hiện lần lượt:

COMMAND 1
    ↓
DONE
    ↓
COMMAND 2
    ↓
DONE
    ↓
COMMAND 3

Đây chính là nền tảng cho:

Robot chạy theo đường đi định trước.

59. Bài thực hành đầu tiên

Hãy cho robot:

Robot_TurnAngle(90);

Quan sát:

Encoder L
Encoder R
Current Angle

Ví dụ:

0°
10°
20°
30°
...
80°
85°
88°
89°
90°

Nếu dừng quanh:

89°–91°

thì hệ thống đã khá tốt.

60. Bài thực hành thứ hai

Cho:

Robot_TurnAngle(-90);

Robot phải quay ngược lại.

Kiểm tra:

+90°
↓
0°
↓
-90°

61. Bài thực hành thứ ba – hình vuông

Chạy:

MOVE 500
TURN 90

MOVE 500
TURN 90

MOVE 500
TURN 90

MOVE 500
TURN 90

Sau đó in:

X
Y
THETA

Nếu mọi thứ lý tưởng:

X ≈ 0
Y ≈ 0
THETA ≈ 0

Thực tế sẽ có sai số.

62. Một điểm rất đáng chú ý

Bài 52 không chỉ dạy bạn:

“Làm sao cho robot quay 90°?”

Mà đang dạy một khái niệm lớn hơn:

Encoder
   ↓
Distance
   ↓
Wheel displacement
   ↓
Robot displacement
   ↓
Robot orientation
   ↓
X, Y, θ

Đó chính là kinematics + odometry.

63. Từ Bài 52 chúng ta đã có 3 đại lượng quan trọng

Robot biết:

Khoảng cách

Distance

Góc

Heading

Vị trí ước lượng

X, Y

Từ đây robot bắt đầu có khả năng:

“biết mình đang ở đâu”

dù chỉ là ước lượng bằng encoder.

64. Sơ đồ kiến thức từ Bài 39 đến Bài 52

Bạn đã đi qua một chuỗi rất logic:

B39
Input Capture
   ↓
B40
Output Compare
   ↓
B41
PWM
   ↓
B42
Servo
   ↓
B43
HC-SR04
   ↓
B44
Timer + Capture + State Machine
   ↓
B45
Encoder Mode
   ↓
B46
Motor + Encoder + PWM
   ↓
B48
Position Control
   ↓
B50
2 Motor + 2 PID
   ↓
B51
Distance Control
   ↓
B52
Angle + Odometry

Đây đã là một chuỗi kiến thức robotics thực sự, chứ không còn chỉ là học từng ngoại vi STM32 riêng lẻ.

65. Bài 53 sẽ là bước tiếp theo rất tự nhiên

BÀI 53 – ODOMETRY NÂNG CAO: TÍNH X, Y, θ CHO ROBOT 2 BÁNH + ĐIỀU KHIỂN ROBOT ĐẾN MỘT TỌA ĐỘ

Khi đó ta không chỉ nói:

Đi 500 mm
Quay 90°

mà có thể nói:

Robot_GoTo(1000, 500);

và hệ thống sẽ tính:

Robot hiện tại:
X = ...
Y = ...
θ = ...

Target:
X = 1000
Y = 500

sau đó tính:

        TARGET (1000,500)
                ↑
                │
             khoảng
             cách
                │
ROBOT ──────────┘
 (X,Y,θ)

rồi biến sai số vị trí (X,Y) + sai số góc θ thành tốc độ bánh trái/phải.

Đó sẽ là bước chuyển từ “robot đi theo lệnh” sang “robot đi tới tọa độ”.

Leave a Reply

Your email address will not be published. Required fields are marked *