Điều khiển động cơ SERVO với Arduino với biên độ góc cực nhỏ

II.Tìm hiểu xung điều khiển SERVO.

Là xung PPM (Cũng là một xung PWM ). Với độ rộng xung cao tỷ lệ thuận với góc lệch của SERVO.

Với độ rộng xung  từ 1ms (1000us) => 2ms (2000us) thì servo được đặt với góc lệch tương ứng trong khoảng 0 =>180 (độ). Xung có chu kì 20ms, cho phép đặt lên 10 kênh servo khác nhau.

III.Tạo xung điều khiển có độ chính xác cao hơn

Ta đã biết ưu điểm của bộ TIMER 1 là cho phép đếm với độ phân giải 16bit. Cùng với đó là khả năng tùy chỉnh ngắt tràn (TOP_value).

3.1.Đề bài

Tạo xung chu kì 5ms, với độ rộng xung cao thay đổi được trong khoảng 1000us => 2000 us.

3.2.Code tạo xung trên pin ~9 theo đề bài

unsigned int Gia_tri_moi;
void setup()
{
 
    TCCR1A = 0;
    TCCR1B = 0;
    // RESET lại 2 thanh ghi
    DDRB |= (1 << PB1);
    // Đầu ra PB1 là OUTPUT ( pin 9)
 
    TCCR1A |= (1 << WGM11);
    TCCR1B |= (1 << WGM12) | (1 << WGM13);
    // chọn Fast PWM, chế độ chọn TOP_value tự do  ICR1
    TCCR1A |= (1 << COM1A1);
    // So sánh thường( none-inverting)
 
    TCCR1B |= (1 << CS11);
    // P_clock=16mhz/8=2mhz
    // mỗi P_clock bằng 1/2mhz= 0.5 us
    OCR1A = 2000;
    Gia_tri_moi = OCR1A;
    // Value=2000 , tương đương xung có độ rộng 2000*0.5us=1000us (1ms)
    // Value=4000, tương đương xung có độ rộng 4000*0.5us=2000us (2ms)
    ICR1 = 40000;
    // xung răng cưa tràn sau 40000 P_clock, tương đương (20ms)
}
 
void loop()
{
    set(2000); // 0 độ
}
void set(unsigned int x)
{
    if (Gia_tri_moi != x) {
        OCR1A = x;
        Gia_tri_moi = OCR1A;
    }
    else {
        return; // thoát ngay
    }
    // x : 2000 - 4000
    //Độ rộng: 1ms - 2 ms
}

3.3.Test servo

Giữ nguyên code trên, sửa hàm loop thành:

void loop()
{
    set(2000); // 0 độ
    delay(1000);
    set(4000); // 180 độ
    delay(1000);
}

Kết quả

Servo chỉ lệch góc trong khoảng 0 đến 90 độ !!!!!!!!!!!!!!!!!!!

Code sai ??!!!

Đến đây, mình đã bối rối khi làm đúng lý thuyết mà nó lại không như lý thuyết tí nào .

Lý thuyết nói: Xung có độ rộng 1ms => lệch 0, xung rộng 2ms => lệch 180. Xung có tần số 50 hz. (xung PPM cần độ dài tối thiểu 20ms để (có thể) ghép tối đa 10 servo).

Tất nhiên mình đã đáp ứng đúng tiêu chí trên.

Sự thật đằng sau!

Đã vậy thì mình sẽ kiểm tra xung của thư viện điều khiển Servo (servo.h) xem có sự sai khác gì không.

#include <Servo.h>
 
Servo s9, s10; // tạo 2 Object
void setup()
{
    // pin ra ~9, ~10
    s9.attach(9);
    s10.attach(10);
    // xuất xung
    s9.write(0); // 0 độ
    s10.write(180); //180 độ
}
 
void loop()
{
}

Kết quả

• F_pwm=50hz.
• Độ rộng xung: 530us (0.53ms) tương ứng góc lệch 0.
• Độ rộng xung : 2410us (2.41ms) tương ứng góc lệc 180.

Đó, thực tế đó.!

Sửa lại độ rộng xung

unsigned int Gia_tri_moi;
void setup()
{
 
    TCCR1A = 0;
    TCCR1B = 0;
    // RESET lại 2 thanh ghi
    DDRB |= (1 << PB1);
    // Đầu ra PB1 là OUTPUT ( pin 9)
 
    TCCR1A |= (1 << WGM11);
    TCCR1B |= (1 << WGM12) | (1 << WGM13);
    // chọn Fast PWM, chế độ chọn TOP_value tự do  ICR1
    TCCR1A |= (1 << COM1A1);
    // So sánh thường( none-inverting)
 
    TCCR1B |= (1 << CS11);
    // P_clock=16mhz/8=2mhz
    // mỗi P_clock bằng 1/2mhz= 0.5 us
    OCR1A = 1060;
    Gia_tri_moi = OCR1A;
    // Value=1060 , tương đương xung có độ rộng 1060*0.5us=530us (0.53ms)
    // Value=4820, tương đương xung có độ rộng 4820*0.5us=2410us (2,41ms)
    ICR1 = 40000;
    // xung răng cưa tràn sau 40000 P_clock, tương đương (20ms)
}
 
void set(unsigned int x)
{
    if (Gia_tri_moi != x) {
        OCR1A = x;
        Gia_tri_moi = OCR1A;
    }
    else {
        return; // thoát ngay
    }
    // x : 1060 - 4820
    //Độ rộng: 0.53ms - 2.41 ms
}
 
void loop()
{
    set(1060); // 0 độ
    delay(1000);
    set(4820); // 180 độ
    delay(1000);
}

OK, thành công!

IV.TỐI ƯU HÓA

Sử dụng P_clock=16mhz nâng tối đa độ phân giải của Timer 16bit. Khi đó độ phân giải nhỏ nhất là 0.0625us trên 1 xung P_clock.

unsigned int Gia_tri_moi;
void setup()
{
 
    TCCR1A = 0;
    TCCR1B = 0;
    // RESET lại 2 thanh ghi
    DDRB |= (1 << PB1);
    // Đầu ra PB1 là OUTPUT ( pin 9)
 
    TCCR1A |= (1 << WGM11);
    TCCR1B |= (1 << WGM12) | (1 << WGM13);
    // chọn Fast PWM, chế độ chọn TOP_value tự do  ICR1
    TCCR1A |= (1 << COM1A1);
    // So sánh thường( none-inverting)
 
    TCCR1B |= (1 << CS10);
    // P_clock=16mhz
    // mỗi P_clock bằng 1/16mhz= 0.0625 us
    // Hay 16  (P_clock ) bằng  1 us.
    // F_pwm=p_clock/50001=319.993hz
    OCR1A = 8480;
    // Value=8480 , tương đương xung có độ rộng 8484*0.0625us=530us (0.53ms)
    // Value=38550, tương đương xung có độ rộng 38550*0.0625us=2410us (2,41ms)
    ICR1 = 65535;
    // xung răng cưa tràn sau 65536 P_clock, tương đương (6,5 ms)
}
 
void set(unsigned int x)
{
    if (Gia_tri_moi != x) {
        OCR1A = x;
        Gia_tri_moi = OCR1A;
    }
    else {
        return; // thoát ngay
    }
    // x : 8480 - 38550
    //Độ rộng: 0.53ms - 2.41 ms
    // Góc lệch: 0 ->180 độ
}
 
void loop()
{
    set(8480); // 0 độ
    delay(2000);
    set(38550); // 180 độ
    delay(2000);
}

V.Nén thư viện và sử dụng.

1. Tải về thư viện

Chú ý:

• Thư viện này chỉ sử dụng cho 2 pin là ~9 (PB1) và ~10 (PB2)
• (Ở phiên bản này) Bạn sẽ không thể sử dụng thư viện <Servo.h> để điều khiển Servo ở các pin còn lại. (Tranh chấp timer1)

Link: <Tải lên lần 1 . 3:29PM 8/1/2017>

https://drive.google.com/file/d/0BzM… (mirror)

2. Giới thiệu thư viện

Thực chất việc cài thư viện  chỉ là Inlude các hàm .

— void  port_attach(select_port);

Gọi hàm này 1 lần để cài đặt đầu ra cho chân PORT.

“select_port” chỉ được phép mang 2 giá trị là “PB1” tương ứng pin ~9, và “PB2” tương ứng pin ~10.

Ví dụ: port_attach(PB1);// chọn pin ~9 làm đàu ra điều khiển Servo

— void correct_write(select_port,  val);

Ví dụ

correct_write(PB1,8480);// pin~9, servo lệch góc 0.000 độ

Mỗi đơn vị của “val” tương ứng với góc lệch 0.006 độ. Đây cũng là độ chia nhỏ nhất mà Servo có thể nhích được nhờ thư viện này .

— void write(select_port,  goc);

Một kiểu khác để điều khiển thay cho hàm correct_write.

“goc” có kiểu dữ liệu ở dạng số thực. Float.

Ví dụ:

write(PB1, 30.005);// pin~9, servo lệch góc 30,005 độ

3. Code ví dụ

Sử dụng 1 servo

#include <TIMER1_SERVO.h>
 
void setup()
{
port_attach(PB1); // chọn pin ~9
}
void loop()
{
write(PB1, 90.005); // pin ~9 // góc hiện tại 90.005 độ
delay(5000); //5s
}

#include <TIMER1_SERVO.h>
 
void setup()
{
port_attach(PB2); // chọn pin ~10
}
void loop()
{
// val: 8480 đến 38550
correct_write(PB2, 8480); // pin ~10 , // góc hiện tại 0.000 độ
delay(5000); //5s
}

Sử dụng 2 servo

#include <TIMER1_SERVO.h>
 
void setup()
{
    port_attach(PB1); // chọn pin ~9
    port_attach(PB2); // chọn pin ~10
}
void loop()
{
    // góc : 0.000 đến 180.000
    write(PB1, 90.005); // pin ~9 trên arduino  UNO R3
    write(PB2, 30.005); // pin ~10 trên arduino  UNO R3
    delay(5000); //5s
}

Sử dụng 2 thư viện một cách an toàn. 

Do có sự tranh chấp TIMER 1, cách duy nhất để sử dụng cùng lúc 2 thư viện là ta cần phải đặt attach/detach mỗi lần sử dụng.

#include <Servo.h>
Servo myservo;
#include <TIMER1_SERVO.h>
void setup()
{
    pinMode(13, 1); //led
    port_attach(PB2); // chọn pin ~10
    myservo.attach(3); // pin 3 (Servo khác)
}
void loop()
{
 
    // điều khiển servo pin ~10, vô hiệu hóa thư viện " Servo.h"
    correct_write(PB2, 8800); // pin ~10 , servo quay ở góc 0.000 độ
    digitalWrite(13, 1);
    delay(5000);
 
    // điều khiển servo pin ~3, vô hiệu hóa thư viện " TIMER1_SERVO.h"
    myservo.attach(3); // đặt
    myservo.write(90);
    digitalWrite(13, 0);
    delay(5000);
    myservo.detach(); // gỡ
}