Bu yazıda projenin son hali ve otopilot hareket için yazılan          algoritmalar açıklanacaktır.

DOSYALAR

              * main.cpp

              * oto_pilot.h

             *data.h

 

 

 

Araç ağ üzerinde bağlı olan bilgisayardan hedef konumun koordinatlarını alır. Sonrasında kendi konumuna göre gideceği  yönü belirler. Aşağıdaki şemada görüldüğü gibi hedef konum o anki konuma göre kuzeyde ve doğuda kalmaktadır. İlk olarak araç kuze-doğu yönünde ilerleyecektir, ilerlerken de devamlı konum ve yön kontrolü yapacaktır. eğer araç enlem ve boylamda şekilde görüldüğü gibi tolerans bolgelerinden birine girecek olursa hareket doğrultusunu tek yöne düşürebilmektedir. şekilde araç enlemde girdiğinden sadece doğu istikametine gitmesi yeterli olacaktır. Eğer bu bölgelerin herhangibirinden çıkarsa tekrar herhangibirine girene kadar ara yönlerde ilerlemeye devam edecektir. Hem enlem hem de boylamda sınır bölgelerinin içinde ise hedef konuma varılmış demektir. tolerans ne kadar az tutulursa aracın hedefe varış hatası o kadar düşük olur. bizim yaptığımız bütün testlerde hata payının 1 m ye yakın olduğunu gördük.

 

 

Otopilot hareket fonksiyonlarını teker teker inceleyelim

 

Projede aracın hareket fonksiyonlarını teker teker oto_pilot.h içerisinde tanımlandı.PortAc0()  PortAc1() fonksiyonları ile iki adet beagleboar üzerine usb hub ile taktığımız stellarisler için seri port oluşturduk. port 0 da gps+magnetometre datalarını toplayıp tek paket oluşturup beagleboarda gönderen ve web kamerası servolarını kontrol eden stellaris bulunmaktadır. port 1 de ise aracın hareket fonksiyonlarını+ engel tarama servosunu+ engel algılama sensörlerinin kontrolü ve engel hakkında beagleboard a bilgi veren farklı bir stellaris işlemci bulunmaktadır.

Her iki stellaristen gelecek (engel ve konum dataları) bilgiler kayıpsız şekilde diğer fonksiyonlar tarafından değerlendirilebilecek halde olmaları gerektiğinden bunların okuma işlemlerini iki bağımsız thread yürütmektedir.

giriş   
PortAc0();
PortAc1();
if (pthread_create(&thread_s, NULL, Port_read0, NULL)) {}
if (pthread_create(&thread_s1, NULL, Port_read1, NULL)) {}

Artık devamlı alt işlemcilerden gelen önemli bilgiler elimizin altında olduğuna göre kendi kendine hareket işlemi nasıl ilerlediğini inceleyelim. Aslında bütün bu işler 3 satırlık kod tarafından yapılmaktadır. head_control() fonksiyonu engele varılıp varılmadığını, varılmadı ise aracın baş açısını düzenlemektedir. Engele varılmadığı zaman 1, varıldığında da 0 göndererek kullanıcıya hedefe ulaşıldığını bildirmektedir.

 

while(head_control())
	{
		move_forward();
		delay(1000);
		//move_stop();
	}
 
	move_stop();
	printf("\n\n\t\t\tJOB COMPLETED SUCCESSFULLY\n");

 

Bu projenin en önemli kısmı hed_contro() fonksiyonunun yaptığı iştir.  Fonksiyon öncelikle seri port haberleşme threadleri tarafından hazırlanan enlem boylam bilgilerini kullanarak hedef değer ile enlem , boylama ve en önemlisi hassasiyete  göre durumunu belirler. bu durumuna 4 ana 4 de ara yön olmak üzere 8 isim verir. Eğer önceden belirlenmiş hassasiyet bölgesinin içerisinde ise hedefe ulaşıldığına karar verir. Eğer hedefte değil ise hesapladığı baş açısı değeri ile mevcut halinin değerini karşılatırır. olması gerektiği açıda değilse araç baş açısını en kısa yoldan düzenler ve ilerleme işini main fonksiyonuna bırakır.

int head_control()
{
 
	double cur_b_m=((double)current_pos.b_m + current_pos.b_s / 60);
	double cur_e_m=((double)current_pos.e_m + current_pos.e_s / 60);
 
	double des_b_m=((double)dest_pos.b_m + dest_pos.b_s / 60);
	double des_e_m=((double)dest_pos.e_m + dest_pos.e_s / 60);
 
	printf("******** %f    %f     %f      %f ********\n",cur_b_m,cur_e_m,des_b_m,des_e_m);
 
	double b = double_abs(& cur_b_m , & des_b_m);
	double e = double_abs(&cur_e_m , &des_e_m);
 
	printf("farklar : b =%f   e= %f\n",b,e);
 
	char direction[2];
 
	if ((b < sens)& (e > sens)) // sadece kuzey güney hesaplama
		{
			if(cur_e_m < des_e_m)  // kuzeyse
			{
				direction[0] ='K';
				direction[1] ='U';
			}
			else   // güneyse
			{
				direction[0] ='G';
				direction[1] ='U';
			}
 
		}
	if ((b > sens)& (e < sens)) // sadece doğu batı hesaplama
		{
			if(cur_b_m < des_b_m) // doğu
			{
				direction[0] ='D';
				direction[1] ='O';
			}
			else // batı
			{
				direction[0] ='B';
				direction[1] ='A';
			}
		}
	if ((b > sens)& (e > sens))	// ara yön hesaplama
		{
			if(cur_e_m < des_e_m)
			{
				direction[0] ='K';
			}
			else
			{
				direction[0] ='G';
			}
			if(cur_b_m < des_b_m)
			{
				direction[1] ='D';
			}
			else
			{
				direction[1] ='B';
			}
		}
	if ((b < sens)& (e < sens))	//hedefteyse
	{
		return 0;
	}
 
	printf ("\nyon %s\n",direction);
 
	while((direction[0] != current_pos.cur_mag[0])||(direction[1] != current_pos.cur_mag[1]))
	{
		if (turn_priority(direction)){	move_right();}
		else {move_left();}
 
		usleep(30000);
		move_forward();
		for(int i=0;i<15;i++)
					usleep(30000);
 
		move_stop();
 
		if (!((direction[0] != current_pos.cur_mag[0])||(direction[1] != current_pos.cur_mag[1])))
			break;
 
		if (! turn_priority(direction) ){	move_right();}
				else {move_left();}
		//move_left();
		usleep(30000);
		move_rear();
		for(int i=0;i<15;i++)
					usleep(30000);
		move_stop();
 
		//Port_read0();
	}
	return 1;
}

 

Yukarıda baş açısını kontrol etme işinde bir noktayı vurgulamak istedim. Aracın ön takımında çok fazla boşluk olduğundan hareket halinde iken sık sık açısı değişmektedir. Tekrar istenilen yöne sokmak  için aracı tam tura yakın tek yönde döndürmektense saat yönünde yada saatin tersi yönünde dönmeye karar verebilmesi gerekmektedir. Bu iş için aşağıdaki şekilde görüldüğü gibi bir circular buffer tasarlandı. Çizgi ile seçilen alanın başı mevcut yönün başına getirildiğinde, hedef yön seçili alanın içinde kalıyorsa saat yönünün tersine eğer dışarısında kalıyorsa saat yönünde dönmeye karar verilmektedir.

 

İşlem kod kısmında ise geçici mevcut ve hedef pozisyon için ayrı değişkenler tanımlanarak karşılıklarına göre kuzey 0 kuzey doğu 1, .. şeklinde değerler verilmektedir. Mevcut pozisyonun değeri devamlı 1 azaltılarak (saat yönünün tersine) 3 döngüde kontrol işlemi uygulanmaktadır. Eğer saat yönünün tersinde bulunursa fonksiyon 0 döndürerek ( saat yönü için 1)  head_control() fonksiyonu saat yönünün tersi yönünde  hedef yöne yerleşmeye çalışacaktır.

 

int turn_priority(char direction[2]){
	 int temp_cur_mag, temp_dest_mag;
 
	 if((direction[0]=='K')&(direction[1]=='U')){	temp_dest_mag=0; }
	 else if((direction[0]=='K')&(direction[1]=='D')){	temp_dest_mag=1; }
	 else if((direction[0]=='D')&(direction[1]=='O')){	temp_dest_mag=2; }
	 else if((direction[0]=='G')&(direction[1]=='D')){	temp_dest_mag=3; }
	 else if((direction[0]=='G')&(direction[1]=='U')){	temp_dest_mag=4; }
	 else if((direction[0]=='G')&(direction[1]=='B')){	temp_dest_mag=5; }
	 else if((direction[0]=='B')&(direction[1]=='A')){	temp_dest_mag=6; }
	 else if((direction[0]=='K')&(direction[1]=='B')){	temp_dest_mag=7; }
 
	 if((direction[0]=='K')&(direction[1]=='U')){	temp_cur_mag=0; }
	 else if((current_pos.cur_mag[0]=='K')&(current_pos.cur_mag[1]=='D')){	temp_cur_mag=1; }
	 else if((current_pos.cur_mag[0]=='D')&(current_pos.cur_mag[1]=='O')){	temp_cur_mag=2; }
	 else if((current_pos.cur_mag[0]=='G')&(current_pos.cur_mag[1]=='D')){	temp_cur_mag=3; }
	 else if((current_pos.cur_mag[0]=='G')&(current_pos.cur_mag[1]=='U')){	temp_cur_mag=4; }
	 else if((current_pos.cur_mag[0]=='G')&(current_pos.cur_mag[1]=='B')){	temp_cur_mag=5; }
	 else if((current_pos.cur_mag[0]=='B')&(current_pos.cur_mag[1]=='A')){	temp_cur_mag=6; }
	 else if((current_pos.cur_mag[0]=='K')&(current_pos.cur_mag[1]=='B')){	temp_cur_mag=7; }
 
	 for(int a =0;a<3;a++)
	 {
		 temp_cur_mag-=1;
		 if(temp_cur_mag<0){
			 if((temp_cur_mag+8)== temp_dest_mag){ return 0; } 		//ccv
		 }
		 else {
			 if((temp_cur_mag)== temp_dest_mag){ return 0; } 	//ccv
		 }
	 }
 
	 return 1; // cv
 }

programı ssh kodu ile beagleboarda network üzerinden bağlanıp terminalden araç üzerinde çalıştırıyoruz. Programda başta aşağıdaki google earth görüntüsünde görülen birkaç noktanın koordinatlarını tanımladık. Her defasında teker teker koordinat girmektense direk nokta seçmek daha az zaman almakta. Aracı nereye bırakırsak bırakalım yukarıda açıkladığım algortimalarımız sayesinde istediğimiz konuma videodaki gibi gitmektedir.  Hedefe vardığında hata payı doğrudan algoritmadaki hassasiyet değerine, gps in hassasiyetine, havanın bulutlu olmasına bağlıdır.

 

Engel Algılama – Aşma

Bu algoritmaları bitirdikten sonra da engel görme ve aşma üzerinde çalışmaya başladık. Kendimize aracın önünde,sağında,solunda olmak üzere 3 farklı bölge için 8 ayrı durumun senaryosunu çıkardık. Ben burada sadece iki durumu tüm detaylarıyla anlatacağım.

 

keil_projeler

*adc_kontrol

* gps_magneto

keil_projeler.zip dosyasının içerisinde projede kullandığımız iki adet  Stellaris LM3S811 in keil proje dosyaları bulunmaktadır. Projelerin işlevlerini özetleyecek olursak:

-> adc_kontrol

  • IR sensörlerden gelen verileri adc işlemi ie yorumlar
  • Engel algılama kontrol etme, beagleboard ile oluşturulan engel aşma protokolünü uygulama
  • IR sensörlerden oluşan hareketli tarama sistemini yönetme
  • İleri – geri hareketini kontrol etme
  • Araca yön verecek servo sistemi yönetme
  • Beagleboar a durum bilgisi gönderme, beagleboarddan gelecek komutları uygulama

->gps_magneto

  •  Gps den gelen verileri ayıklayıp beagleboardda kullanılacak olan enlem boylam bilgilerini hazırda bekletme
  • Magnetometreden gelen açı değerini 4 ana ve 4 ara yöne çevirme
  • Konum ve açı bilgisini tek pakette toplayıp sürekli beagleboarda gönderme
  • 2 eksenli kamera sisteminin servolarını kontrol etme

 

 beagleboard head_control() den sonra belirli bir süre ileri gitmek isteyecektir. İleri fonksiyonu stellaris üzerinde çalışırken bir yandan da hareketli engel tarama sistemi devreye girecektir. Bu sırada aracın etrafında aşağıdaki gibi bir engel çıkarsa (solda engel var , ön ve sağ boş) :

 

while (move==1)
    	{    		
	    	for(i=0;i<=4;i++){
	    		if (move==1) 	{ 
					 if (Ir_Check_right()||Ir_Check_left()){UARTSend((unsigned char *)"E", 1);stop();move=0;}
		    	 PWMPulseWidthSet(PWM_BASE, PWM_OUT_4, ulPeriod2 * (98-(12* i))/ 1000);	   		
		    	 SysCtlDelay(SysCtlClockGet()/25 );
 
					}
	    	}
 
	    	for(i=0;i<=2;i++){   
	    		if (move==1) { 	  
					 if (Ir_Check_right()||Ir_Check_left()){UARTSend((unsigned char *)"E", 1);stop();move=0;}
		    	 PWMPulseWidthSet(PWM_BASE, PWM_OUT_4, ulPeriod2 * (62+(12*i))/ 1000);		   
		    	 SysCtlDelay(SysCtlClockGet()/25);		    	
				 }
	    	}   
    	}

 

 

stellaris e ileri hareket emri geldiğinde yukarıdaki engel tarama kısmı da çalışmaya başlamaktadır. Sağ sol taramaları ara değer taramaları ile (her iki taramada da ön taraf kontrol edilmekte) durana kadar yapılmaktadır.herhangi bir yönde engel görüldüğünde sadece ‘E’ mesajı beagleboard a iletilmektedir.

 

Beagleboardda void* Port_read1(void* arg) threadı içerisinde bir fsm in tasarlanmış olduğu görülecektir. FSM de başlangıç durumu E_wait tir.

case E_wait:
			read(Port_Dosya1, &temp,1);
 
			if(temp[0]=='E'){c_flag=0;next_state = control;}
			else {next_state=E_wait;}
 
			break;

 

 

hareket halinde herhangi bir engel yakalama durumu olduğunda sadece engel uyarısı beagleboarda iletilerek hareket işlemi durdurulur. Threadimiz control state e geçerek stellaristen engelin durumunu ister.

 

Kontrol için stellaris e  “k” gönderirlir. Stellaris ise sağ,ön,sol u  kontrol eder ve  sırası ile “OOX”  gönderir ( sadece solda engel olduğu için).

 

void check(){
	PWMPulseWidthSet(PWM_BASE, PWM_OUT_4, ulPeriod2 * 98/ 1000);
	SysCtlDelay(SysCtlClockGet() /5);
	Ir_Check_right();
	if(Ir_Check_right()){UARTSend((unsigned char *)"X", 1); } // sag kontrol
		else{UARTSend((unsigned char *)"O", 1); }
	SysCtlDelay(SysCtlClockGet() /25);	
	Ir_Check_left();
	if(Ir_Check_left()){UARTSend((unsigned char *)"X", 1); }// on kontrol
		else{UARTSend((unsigned char *)"O", 1); }
 
	PWMPulseWidthSet(PWM_BASE, PWM_OUT_4, ulPeriod2 *50/ 1000);
 
	SysCtlDelay(SysCtlClockGet() /5);	
 
		Ir_Check_left();
	if(Ir_Check_left()){UARTSend((unsigned char *)"X", 1); }// sol kontrol
			else{UARTSend((unsigned char *)"O", 1); }
 
}

 

 

 

 

3 karakterlik engel dizisini bekleyen kontrol durumu “OOX” e göre sadece sol tarafa odaklanarak hareket algoritmalarını yürüteceği L_control state e geçer.

 

 

case control:
			Port_Write1(k,1);
			x=0;
			while(x!=3){
				read(Port_Dosya1, &temp,1);
				temp2[x]=temp[0];
				x++;
			}
 
/*************************************
		RIGHT	 FRONT	LEFT
L_control 	  O       O      X	+
F_control 	  O       X      O
LF_control 	  O       X      X
R_control 	  X       O      O	+
RL_control 	  X	  O      X
RF_control 	  X	  X      O
RLF_control       X       X      X
*************************************/
 
			if((temp2[0]=='O') & (temp2[1]=='O') & (temp2[2]=='O')){c_flag=1;next_state = E_wait;	}	//engel yok
			if((temp2[0]=='O') & (temp2[1]=='O') & (temp2[2]=='X')){next_state= L_control;	}	//sadece solda
		    if((temp2[0]=='O') & (temp2[1]=='X') & (temp2[2]=='O')){next_state= F_control;	}	//sadece önde
			if((temp2[0]=='O') & (temp2[1]=='X') & (temp2[2]=='X')){next_state= LF_control;	}	//sol ve önde
			if((temp2[0]=='X') & (temp2[1]=='O') & (temp2[2]=='O')){next_state= R_control;	}	//sadece sağda
		   // if((temp2[0]=='X') & (temp2[1]=='O') & (temp2[2]=='X')){next_state= RL_control;	}	//sağ ve solda
			if((temp2[0]=='X') & (temp2[1]=='X') & (temp2[2]=='O')){next_state= RF_control;	}	//sağda ve önde
		  //if((temp2[0]=='X') & (temp2[1]=='X') & (temp2[2]=='X')){next_state= RLF_control;}	//bütün yönlerde
 
			break;

 

 

L_control durumunda beagleboard stellaris e “n” göndererek sola odaklı ilerleme algoritmalarını yürütmesini ister ve ya farklı engel yada sorunsuz soldaki engel aşıldı mesajını bekler. Farklı engelde tekrar mekanizme baştan başlar ve engelin duruma göre state lere karar verilir, yada engel olmaması durumunda normla ilerleyiş için çıkabilecek engel bekleme durumuna geçilir. ( stellaris için “n” sola odaklı ilerleme, “m” sağa odaklı ilerleme)

 

 

case L_control://OOX
 
			Port_Write1(n,1);
 
			read(Port_Dosya1, &temp,1);
			if(temp[0]=='E'){next_state = control;}
			if(temp[0]=='O'){c_flag=1;next_state = E_wait;}
 
			break;

 

 

 

 stellaris te sola odaklı ilerlemede, hareketli tarama sistemi  sola doğru döner(sensörün teki solu diğeri ön terafı kontrol eder) ve devamlı kontrol edilerek araç ileri gider. ilerleyiş sırasında soldaki engel bittiğinde ve herhangi başka engele rastlanılmadığında “O” mesajı beagleboarda iletirlir, İlerleyiş sırasında soldaki engel aşılmadan önüne engel çıkarsa direk “E” mesajı iletilerek en baştan engel konumları tespit edilerek engel durumuna göre aşma algoritması çalıştırılır.

 

 

void L_check(){// sol kontrol edilerek soldaki engelden kurtulana kadar ileri gider
	PWMPulseWidthSet(PWM_BASE, PWM_OUT_4, ulPeriod2 * 50/ 1000);
	SysCtlDelay(SysCtlClockGet() /5);
 
		flag = 1;
 
	forward();
	while (Ir_Check_left()){
			if (Ir_Check_right()){UARTSend((unsigned char *)"E", 1); flag=0;break;} // on kontrol, eger önde engel varsa E gönder
			} // sag kontrol
	stop();
	if (flag){UARTSend((unsigned char *)"O", 1); }
}

 

 

 

Peki ilerleyiş sırasında aşağıdaki gibi bir engel farkedilse, kontrol durumunda “OXO” (sadece önde engel var) mesajı gelirse fsm F_control state yi çalıştırmaya başlayacaktır.

 

 

case F_control://OXO
 
			Port_Write1(s,1);    //geri git
			delay(1000);
			Port_Write1(z,1);
 
			Port_Write1(d,1);	 //saga dön
			delay(200);
 
			Port_Write1(w,1);	//ileri git
			delay(1000);
 
			Port_Write1(z,1);	//dur
 
			next_state = control; //solu kontrol edeceğin duruma git
 
			break;

 

 

bu durumda yazdığımız senaryoya göre araç önce geri gidecek, sonda 90 derece sağa dönecek ve sol kontrol algoritmasını çalıştırmaya başlayacak. Sol kontrole göre engel aşıldığında kaldığı yerden hedefe gitme fonksiyonları devam edecektir. Engel aşma threadi ise E_wait de olası engeli bekleyecektir.

Geri kalan engel aşma durumları gerek stellaris gerek beagleboarda yazdığımız yazılımlarda mevcuttur. İki farklı durum için paylaştığımız ve açıkladığımız kodlar son halleridir.