+ Hai thành phần y và pixel_y có mối quan hệ ngược nhau, với độ phân giải
pixel_y max = 960 thì ta có y = 960 - pixel_y.
+ Với tọa độ gốc ở điểm x = 0, ta lấy pixel_x = 640, x = 640 - pixel_x. Như vậy x sẽ có cả giá trị âm và dương.
+ Thực tế ta chọn tâm O(x,y) với các góc quay của Servo tương ứng là 78 và 99o, ta sẽ thay đổi góc từ tọa độ này.
4.4. Chương trình điều khiển trên Adruino Mega 2560
#include <Servo.h> #define L0 1300 Servo myservo_1; Servo myservo_2;
Luận văn cao học – Chuyên ngành: Kỹ thuật Điện tử
char TEXT[50]; void setup() { Serial.begin(9600);
Serial1.begin(9600); // Khởi tạo các cổng nối tiếp với tốc độ Baurate bằng 9600 delay(1000);
myservo_1.attach(9);
myservo_2.attach(10); // Cấu hình các chân điều khiển động cơ RC-Servo }
void loop() {
LABVIEW_TEMINAL();
if(D_X!=POS_X){myservo_1.write(POS_X);D_X = POS_X;} if(D_Y!=POS_Y){myservo_2.write(POS_Y);D_Y = POS_Y;} // Chỉ cập nhập vị trí mới khi tọa độ thay đổi
}
void LABVIEW_TEMINAL(){
while(Serial1.available() > 0) {// Kiểm tra xem nếu có dữ liệu truyền tới TEXT[i] = Serial1.read(); // Lưu giá trị
if(TEXT[i]=='X')i=0; // Nếu xuất hiện ký tự “X” bắt đầu lưu tọa độ i++;
if(i == 7){A = TEXT[1] - 48; B = TEXT[2] - 48; C = TEXT[3] - 48; U = TEXT[4] - 48; V = TEXT[5] - 48; W = TEXT[6] - 48;
// Các ký tự muốn chuyển về dạng thập phân phải trừ đi 48 theo bảng mã ASCII PIXEL_X = A*100+B*10+C; PIXEL_Y = U*100+V*10+W;
// Tính toán ra giá trị thập phân
X = 640 - PIXEL_X; Y = 960 - PIXEL_Y; // Quy chiếu về gốc tọa độ của Laze
DEC_X = atan(X/L0)*180/PI;
Luận văn cao học – Chuyên ngành: Kỹ thuật Điện tử
// Tính các góc cần xoay cho RC-Servo POS_X = 78 + DEC_X;
POS_Y = 99 - DEC_Y; // Cộng với giá trị tương ứng với gốc O(0,0) }
}}
Luận văn cao học – Chuyên ngành: Kỹ thuật Điện tử