Mình thấy chẳng có giải thuật nào quá kinh khủng đâu bạn à
Bạn đầu tiên nên nghiên cứu con servo của bạn, khi xuất xung PWM có pulse width là bao nhiêu thì servo sẽ quay tới góc bao nhiêu độ. Khi bạn đã nắm đc công thức đó thì chỉ việc áp các góc trong đồ thị kia vào 1 array. Rồi cứ thế xuất xung để servo quay đến từng góc một trong array ấy theo thời gian mà đồ thị quy định.