Việc thực hiện trên mô hình đã hoàn thành những yêu cầu đặt ra, tuy nhiên do thời gian, kinh phí, công nghệ còn hạn chế nên mô hình chỉ là mô hình đáp ứng việc nghiên cứu và học tập. Từ mô hình robot với 3 cảm biến siêu âm, ta có thể mở rộng số cảm biến ra, để robot có thể phát hiện vật cản từ nhiều phía.
Ngoài ra, ta có thể sử dụng thêm camera và áp dụng xử lý ảnh kết hợp với cảm biến siêu âm để xây dựng bản đồ hoàn chỉnh, chính xác về môi trường xung quanh.
83
TÀI LIỆU THAM KHẢO Tiếng Việt
[1] Báo cáo khoa học: “Đo Khoảng Cách Và Xác Định Vị Trí Vật Thể Bằng Phương Pháp Siêu Âm”, Trần Thị Thủy, Nguyễn Quang Thắng , Đinh Sơn Thạch, Khoa Khoa học Ứng dụng, trường Đại học Bách khoa – ĐHQG TP.HCM, Phòng thí nghiệm Công nghệ Nano, ĐHQG TP.HCM.
[2] Tăng Quốc Nam, “Điều khiển robot tự hành bám quĩ đạo và tránh vật cản”. Tuyển tập công trình khoa học Hội nghị Cơ học toàn quốc lần thứ 8 (2007). Tập 1, tr. 314-323.
[3] Tăng Quốc Nam, “Ứng dụng điều khiển mờ trong bài toán tránh vật cản của robot tự hành dùng cảm biến siêu âm”. Tạp chí Khoa học Giáo dục Kỹ thuật - Đại học Sư phạm Kỹ thuật thành phố Hồ Chí Minh. Số 7 (1/2008), tr.82-88.
[4] Website: tapchilaptrinh.vn, codientu.org
Tiếng Anh
[5] Atsushi Fujimori, Peter N. Nikiforuk. “Adaptive Navigation of Mobile Robot with Obstacle Avoidance”, IEEE Transactions on Robotics and Automation, Vol-13, August 1997.
[6] J. Borenstein, Member, IEEE and Y. Koren, Senior Member, IEEE. “The Vector Field Histogram - Fast Obstacle Avoidance For Mobile Robots”.
[7] Roland Siegwart and Illah R. Nourbakhsh (2004), “Introduction to Autonomous Mobile Robots”.
[8] Reinhard Braunstingl - Pedro Sanz - Jose Manuel Ezkerra, IKERLAN Centro de Investigaciones Tecnológicas, “Fuzzy Logic Wall Following of a Mobile Robot Based on the Concept of General Perception”.
[9] Y. Koren, Senior Member, IEEE and J. Borenstein, Member, IEEE. “Potential Field Methods and Their Inherent Limitations for Mobile Robot Navigation”, Proceedings of the IEEE Conference on Robotics and Automation, Sacramento, California, April 7-12, 1991.
[10] Paul Fitzpatrick. “Behaviour-Based Control in Mobile Robotics”.
[11] Arkin, R. C. (1998). “Behavior-Based Robotics”, The MIT Press.
[12] J. Borenstein, H. R. Everett, and L. Feng. “Navigating Mobile Robots: Sensors and Techniques”.
84
PHỤ LỤC 1. Code chương trình đọc cảm biến siêu âm
/*srf05*/
unsigned long duration;
int distance1; // biến lưu khoảng cách
const int trig = 50; // chân trig của srf05 = chân digital 50 của mega2560 const int echo = 52; // chân echo của srf05 = chân digital 52 của mega 2560 void setup()
{ Serial.begin(9600); // giao tiếp Serial với baudrate 9600 pinMode(trig,OUTPUT); // chân trig sẽ phát tín hiệu
pinMode(echo,INPUT); // chân echo sẽ nhận tín hiệu
}
void loop() {
/* Phát xung từ chân trig */
digitalWrite(trig,0); // tắt chân trig delayMicroseconds(2);
digitalWrite(trig,1); // phát xung từ chân trig
delayMicroseconds(5); // xung có độ dài 5 microSeconds digitalWrite(trig,0); // tắt chân trig
/* Tính toán thời gian */
// Đo độ rộng xung HIGH ở chân echo. duration = pulseIn(echo,HIGH); // Tính khoảng cách đến vật. distance = int(duration/2/29.412); /* In kết quả ra Serial Monitor */ Serial.print(distance);
Serial.println("cm"); delay(200);
85
2. Code chương trình trả hướng từ cảm biến la bàn số
#include <Wire.h> #include "HMC5883L.h" HMC5883L compass; void setup() { Serial.begin(9600); compass.setRange(HMC5883L_RANGE_1_3GA); compass.setMeasurementMode(HMC5883L_CONTINOUS); compass.setDataRate(HMC5883L_DATARATE_15HZ); compass.setSamples(HMC5883L_SAMPLES_1); } void loop() {
Vector raw = compass.readRaw();
Vector norm = compass.readNormalize(); int MilliGauss_OnThe_XAxis = norm.XAxis; float heading = atan2(norm.YAxis, norm.XAxis); float declinationAngle = -0.0457;
heading += declinationAngle; if(heading < 0)
heading += 2*PI;
// Check for wrap due to addition of declination. if(heading > 2*PI)
heading -= 2*PI;
// Convert radians to degrees for readability. float headingDegrees = heading * 180/M_PI; curr_heading=(int)headingDegrees;
delay(30); }
86
3. Code chương trình đọc xung encoder
#define phase_a 2 // kênh A của encoder nối với chân PWM 02 của Mega2560 #define phase_b 3 // kênh B của encoder nối với chân PWM 03 của Mega2560 #define interrupt0 0 // định nghĩa ngắt tương ứng trên Mega2560
/* Khai bao xung encoder */
volatile int pulse = 0; // biến đếm số xung/vòng void setup() { Serial.begin(9600); //---Encoder---// pinMode(phase_a, INPUT_PULLUP); // digitalWrite(phase_a, HIGH); pinMode(phase_b, INPUT_PULLUP); digitalWrite(phase_b, HIGH); //---interrupt---//
attachInterrupt(interrupt0, int_, RISING); // gọi hàm int_ khi trạng thái của chân digital chuyển từ mức điện áp thấp sang mức điện áp cao.
}
//---void-interrupt dem xung encoder---// void int_() { if (digitalRead (phase_a)==HIGH) { pulse++; } else { pulse--; } } void loop() {
Serial.println(pulse); //hiển thị số xung qua cổng Serial delay(10);
87
4. Code chương trình điều khiển PID
volatile float V = 0; //biến chứa giá trị vận tốc ban đầu
volatile float V_sp = 200.0 ; //vận tốc mà ta mong muốn Vsetpoint = 200v/p /* Thong so bo PID */
volatile float del_err,sum_err =0;
volatile float Kp=2.2,Ki = 0.0005,Kd =0.000125; volatile float error,pre_error;
volatile float p_part,i_part,i_part_last,d_part; volatile int PWM;
volatile float rpm;// biến chứa giá trị vận tốc thực tại thời điểm mà ta đang xét volatile float sampletime = 0.001;
//---PID---// int PID_control ()
{
error = V_sp - V; //tính toán sai số
p_part = Kp * error; //khâu p
i_part = i_part_last + Ki * error * sampletime; //khâu i d_part = Kd *(error - pre_error) / sampletime; //khâu d
i_part_last = i_part; //reset lại sai số và các giá trị lien quan đến nó pre_error = error;
PWM = (int)(p_part + i_part + d_part); //output xung PWM if (PWM >= 255) PWM = 255;
if (PWM <= 0) PWM = 0;
return PWM; //trả về xung PWM điều khiển tốc độ động cơ }
88
5. Code xử lý pixel
private void btd_Click(object sender, RoutedEventArgs e) {
plot_map(); }
private void plot_map() { int i = 0; height = height * 2; for (i = 0; i < cout - 3; i += 4) { d1 = Int32.Parse(test[i]); d2 = Int32.Parse(test[i + 1]); d3 = Int32.Parse(test[i + 2]); String hdd = test[i + 3]; hd = hdd[0]; if (d2 > 30) d2 = 30; if (d3 > 30) d3 = 30; if (hd == 'n') {
bt.SetPixel((width / 2) - d2, (height / 2), System.Drawing.Color.Yellow); bt.SetPixel((width / 2) + d3, (height / 2), System.Drawing.Color.Green); bt.SetPixel((width / 2), (height / 2), System.Drawing.Color.White); if (height > 5)
height = height - 5; }
if (hd == 'e') {
bt.SetPixel((width / 2), (height / 2) - d2, System.Drawing.Color.Yellow); bt.SetPixel((width / 2), (height / 2) + d3, System.Drawing.Color.Green); bt.SetPixel((width / 2), (height / 2), System.Drawing.Color.White); width = width + 5;
}
if (hd == 'w') {
bt.SetPixel((width / 2), (height / 2) + d2, System.Drawing.Color.Yellow); bt.SetPixel((width / 2), (height / 2) - d3, System.Drawing.Color.Green); bt.SetPixel((width / 2), (height / 2), System.Drawing.Color.White); if (width > 5) width = width - 5; } if (hd == 's') {
89
bt.SetPixel((width / 2) + d2, (height / 2), System.Drawing.Color.Yellow); bt.SetPixel((width / 2) - d3, (height / 2), System.Drawing.Color.Green); bt.SetPixel((width / 2), (height / 2), System.Drawing.Color.White); height = height + 5;
}
bt.RotateFlip(RotateFlipType.Rotate180FlipXY); ImageSource img = Bitmap2ImageSource(bt); imbox.Source = img; bt.Save("E:\\Image.bmp"); } btd.Content = "Drawing"; this.Close(); }
private ImageSource Bitmap2ImageSource(Bitmap bitmapImage) {
using (MemoryStream outStream = new MemoryStream()) {
MemoryStream ms = new MemoryStream(); bitmapImage.Save(ms, ImageFormat.Bmp); ms.Seek(0, SeekOrigin.Begin);
BitmapImage bi = new BitmapImage(); bi.BeginInit(); bi.StreamSource = ms; bi.EndInit(); bi.Freeze(); return bi; } }