BİR ÇUBUĞU PID KONTROLU İLE İSTENEN AÇIDA TUTMAK-Bölüm 2 (2 motor)

İki ucunda birer adet pervaneli drone motoru bağlı olan bir çubuğumuz var. Bunu istediğimiz eğimde tutmak için STM32F103 mikrodenetleyici ile ADXL345 ivme sensörü kullanarak PID kontrolu altında çalışan bir sistem geliştiriyoruz.

Bu projenin ilk adımı çubuğun sadece tek tarafındaki bir motor ile dengede tutulması idi, o aşamayı anlattığım yayına ulaşmak için burayı tıklayabilirsiniz. İlk yayında anlattıklarımı burada tekrarlamayacağım için önce onu okumanızı öneririm.

GENEL

Bu yayın bir öncekinin devamı olduğundan tekrarlardan kaçınacağım.

Öncekinden farklı olarak bu bölümde çubuğun iki ucunda da birer drone motoru var, her ikisi de kaldırma yönünde birlikte çalışıyorlar. Motorların devir hızları, çubuğu istenen eğime getirip orada tutacak şekilde PID kontrolu altında ayarlanıyor.

Önceki bölüme göre buradaki temel fark motor devir hızlarının birbirini tamamlayacak çekilde hesaplanarak PWM sinyallerinin ona göre uygulanmasından ibaret. Sürecin geri kalanında bir fark yok.

İki motorlu yapıda motorlar eğim açısına birbirine ters yönde etki yaptıkları için sistem eğim değişikliklerine öncekine göre daha hızlı tepki veriyor.

Önceki tek motorlu kuruluma göre donanım açısından tek fark var, o da ikinci motorun MOSFET sürücüsüne giden PWM sinyali :

• İkinci motor için ikinci bir PWM çıkışı kullanıyorum: PB10 üzerinden TIM2_CH3 (PWM_out_2). Bu pin, ikinci motorun MOSFET sürücüsünün girişine bağlı.
• İkinci motoru da sözünü ettiğim ikinci MOSFET çıkışına bağlıyorum.

Sistemin genel görünümü de aşağıdaki gibi oluyor. Darmadağınık telli bağlantılarda dikkat edilmesi gereken birşey var: Motor bağlantı tellerinin diğerlerine yaklaştırılmaması gerekiyor, aksi halde motorlar çalışır çalışmaz ADXl345 kilitleniyor, beslemesi kesilip açılmadan kendine gelemiyor.

Bu sefer farklı bir ADXL345 modülü kullandım, bu öncekinden farklı olarak bir PCB üzerinde değil, doğrudan çubuğa monte edilmiş durumda:

MOSFET Motor sürücümü de bir PCB üzerine alarak biraz daha derli toplu hale getirdim. Burada 4 sürücü var, projede ikisini kullanıyoruz.

MOTOR HIZLARININ BELİRLENMESİ

Hangi ucu yukarı kaldırmak istersek o taraftaki motorun PWM oranını arttırıyoruz, diğer motorun PWM oranını da aynı ölçüde azaltıyoruz. Sonuçta motorların PWM lerinin ortalaması sabit kalıyor. Bu projede iki motorun PWM oranlarının ortalamasını, izin verilen bandın ortasına %30 olarak sabitledim. Örneğin motorlardan birisi %35 e yükseltilirse diğeri %25 e düşürülüyor.

PWM ortalama değeri iki motorun birlikte kaldırma kuvvetine karşı düşüyor. Çubuğumuz ortasındaki mafsal etrafında dönebilecek şekilde sabitlenmiş durumda olduğundan kaldırma kuvveti onu bir yere götüremiyor. Bu durumda bu projede ortalama değerin çok önemi yok. Eğer serbest uçabilen bir yapı olsaydı ortalama değeri ayarlanabilir yapmanın bir anlamı olurdu, ortalamayı arttırdığımızda çubuk yükselirdi örneğin.

Gene de kodlarımı ortalama değerin ayarlanabilir olduğu duruma uygun olarak geliştirdim.

PWM ORTALAMA DEĞERİNİN LİMİTLERE ETKİSİ

Yukarıda açıkladığım gibi PWM ortalaması dediğim şey, yani (pwm1+pwm2)/2 değeri motorların toplam kaldırma gücüne karşı düşüyor. Sadece ortasındaki mafsal çevresinde sağa sola yatabilen değil de aynı zamanda serbestçe yükselip alçalabilen bir çubuk olsaydı, bu ortalamayı arttırdığımızda çubuk yükselecek, azalttığımızda da alçalacaktı.

Ancak, motorlara uygulayabileceğimiz PWM oranının da bir sınırı var, bu motor gücüyle ilgili bir şey. Motorlarımıza en fazla 3.3V efektif besleme gerilimi verebiliyoruz. Bu da 5V beslememizden en fazla %60 PWM ile sürebileceğimiz anlamına gelir. Bir başka deyişle ortalama PWM oranımız 0 ila %60 arasında değişebilir.

Ortalama PWM %30 olduğunda, bu orta değerin %30 altında ve üzerinde ayar sahamız var demektir. Motorlardan birisini %50 ye ayarlarsak, ortalamayı sabit tutmak için diğerini %10 yapmamız gerekir.

Ortalama değerimizi %50 ye çıkartırsak, yani çubuğumuza uygulanan kaldırma kuvvetini arttırırsak, eğimini ayarlamak için +-%10 ayar sahası kalıyor, %60 ın üzerine çıkamıyoruz. Demek ki, PWM alt ve üst sınırları ortalama PWM değerine bağlı olarak değişiyor. Algoritma içinde dinamik olarak hesaplanan bu dinamik pwm limitlerini dynPwmMax ve dynPwmMin olarak adlandırdım.

Sonuçta, ortalama PWM değerinin alt ve üst sınırlara yaklaşması halinde eğim kontrolunda kısıtlamalarla karşılaşacağımız görülüyor.

Aşağıda vereceğim algoritmadaki PWM hesaplamalarını bu gözle incelemek gerekiyor.

ALGORİTMA VE KODLAR

Yazılımda da önceki bölüme eklenen yeni değişkenler var, çünkü ikinci motor devreye girdi :

uint16_t pwm_o;// Motor1 ve Motor2 orta değeri
uint16_t pwmMin, pwmMax;// Motorlara bağlı mutlak alt ve üst sınırlar
uint16_t dynPwmMax, dynPwmMin;//Değişen pwm_o değerlerine bağlı dinamik sınırlar
uint16_t pwm_1, pwm_2;// Motorların pwm pulse değerleri

Main.c içindeki süreç kontrol bölümü aşağıdaki gibi. Kodların arasında fazlasıyla açıklama koydum.

/* motorlarımızın maksimum çalışma gerilimi 3.3V.
 * CMOS üzerindeki motor besleme gerimimiz 5V olduğu
 * için en fazla %60 PWM duty cycle uyguluyoruz.
 * Aşağıdaki sınırlar izin verilen motor gerilimi
 * ve besleme kaynağı gerilimine göre belirlenmelidir.
 */

pwmMin = 0;
pwmMax =600;

/* pwm_o iki motorun ortalama gerilimi.
 * Bu gerilim iki motorun birlikte yukarı kaldırma
 * gücünü belirliyor. Bu gücü ayarlamak için bir
 * potansiyometre ve ADC kullanılarak pwm_o değiştirilebilir. 
 * Bu sürümde pwm_o değeri pwmMax/pwmMin aralığının ortasına 
 * sabitlenmiş durumda.
 */

pwm_o = (pwmMax +pwmMin)/2;  // Orta noktaya ayarlıyalım.

/* Başlangıçta motorlar eşit güçle dengede */

pwm_1 = pwm_o;
pwm_2 = pwm_o;

/* Timer pwm kanallarını başlatalım */

HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_3);

adxl_init();  // adxl345 i başlat

get_adxl_RawValues(&adxlRawValues); // İlk okuma, interrupt temizlemek için

float radToDegree = 180/3.1416;

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
// ADXL345 den gelen ham okuma değerlerini görüntüle 

TFT_Printf(0,0,24,"X:%d Y:%d Z:%d",adxlRawValues.x, adxlRawValues.y, adxlRawValues.z);

/**  X ekseni eğimi Y ekseninin çevresindeki dönüş açısına eşit. Aşağıda
*   Bu eğimin açısal karşılığını hesaplayıp görüntülüyoruz. PID algoritması
*   bu açı cinsinden değerleri kullanmıyor, ham ivme değerleri ile çalışıyor.
*   Zira aşağıdaki kayan noktalı çarpma, SQRT ve ATAN hesaplamaları çok
*   vakit alıyor
*
*   HW kayan nokta aritmetiği yapabilen mikro denetçilerle bu sorun olmaz.
*/

	  float x = adxlRawValues.x;
	  float y = adxlRawValues.y;
	  float z = adxlRawValues.z;
	  
	/* aci_y y ekseni çevresindeki dönüş açısı, X ekseninin eğim açısı oluyor
	 * Bu açıyı sadece ekranda gösterim için hesaplıyoruz, PID kontrolunda
	 * kullanılmıyor.
	 */
	  
	  int aci_y = atan(-1*x/sqrt(y*y + z*z))*radToDegree;
	  TFT_Printf(0,30,24,"Angle_Y: %d     ",aci_y);

 /* Hedef ivme değerini (setPoint) ADC den okunan pot konumundan hesapla */
	adc = ADC_Read();
	setPoint = (adc * outSpan)/4095; // adc değerine karşı düşen hedef çıkış

		/*
		 * Son ölçülen 10 değer ICbuffer[] de saklanıyor.
		 * En yeni değer ICbuffer[0] da olacak şekilde.
		 * Her yeni okunan değer ile kayıtlı değerler birer
		 * kaydırılarak ICbuffer[9] deki kayıt dışarı atılıyor,
		 * yeni değer ICbuffer[0] a yazılıyor.
		 *
		 * Başka uygulamalarda ICValue yerine başka geri besleme
		 * değerleri gelecektir. Örneğin:
            -TIM3 input capture ile ölçülen periyod,
            -Gyro
            -pozisyon,
            -sıcaklık,
            -basınç vb.
		 *
		 */

correction = computePID(ICbuffer); //ICbuffer'a kayıtlı son okumalara göre
                                   //göre düzeltme faktörü
if(pwm_1<=(-correction)) pwm_1 = 0; // PWM'in negatife geçmesini engelle
else pwm_1 += correction;     

/* maksimum ve minimum kontrolları:
*
* Bu bölümde pwm_o nun mutlak max/min aralığının ortasında olmaması
* durumuna göre uygulanacak dinamik alt/üst sınırları hesaplayıp
* uyguluyoruz.
* Değişim +- aralığı pwm_o nun en yakın olduğu mutlak sınırdan
* uzaklığı ile sınırlanıyor.
*
* Böylece her iki motora gönderilen PWM gerilimleri dengeli olabiliyor.
*
* Bu algoritmaya göre en geniş manevra alanı pwm_o nun mutlak sınırların
* tam ortasında olduğunda oluyor. pwm_o mutlak sınırlardan birine
* yaklaştıkça PWM değişimi aralığı daralıyor.
*
* Motorun minimum ve maksimum devirlerinde her iki motor da zorunlu olarak
* eşit gerilimlerle sürülüyor.
*
* */
		if((pwm_o-pwmMin)<(pwmMax-pwm_o)){ //pwm_o alt sınıra yakın
				dynPwmMin = pwmMin;
				dynPwmMax = pwm_o+(pwm_o-pwmMin);
			} else {                   //pwm_o üst sınıra yakın
				dynPwmMax = pwmMax;
				dynPwmMin = pwm_o+(pwm_o-pwmMax);
			}
		if (pwm_1 < dynPwmMin) { pwm_1 = dynPwmMin;}
		if (pwm_1 > dynPwmMax) { pwm_1 = dynPwmMax;}

		/* pwm_1 ve pwm_2 pwm_o'nun altında ve üstünde
		 * eşit uzaklıklarda olacaklar. pwm_2 yi buna
		 * göre hesaplayalım.
		 * */

		pwm_2 = pwm_o - (pwm_1-pwm_o);

		__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, pwm_1);
		__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, pwm_2);

	printf("pwm_1=%d, IC1= %d \n", pwm_1, ICValue); //(SW Debug çıkışına)

		TFT_Printf(0,60, 24,  "Ani CIKIS:%d  ", ICValue);
		TFT_Printf(0,90, 24,  "Ort CIKIS:%d  ", ortalamaIC);
		TFT_Printf(0,120, 24, "Kum HATA:%d   ", kumHata);
		TFT_Printf(0,150, 24, "SET POINT:%d  ", setPoint);
		TFT_Printf(0,180, 24, "PWM_1:%d      ", pwm_1);
		TFT_Printf(0,210, 24, "PWM_2:%d      ", pwm_2);

    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

SONUÇLAR

Ekranda da görüldüğü gibi potansiyometre ile seçilen hedef değeri tutturabiliyoruz. Dengeyi elimle bozduğumda 1-2 saniye içinde tekrar düzeltiyor. PID katsayıları ile henüz hiç oynamadım, bir ince ayar ile sistemin tepki süresi çok daha kısaltılabilir sanırım.

Motorlar çalışmaz iken görüntülenen ivme ve açı değerleri çok stabil, çubuk hareketsiz kaldığında açı değeri hiç değişmiyor (kesirleri görüntülemiyorum), X Y Z ivme değerleri de birkaç sayım değişebiliyor. Fakat motorların çalışmaya başlamasıyla birlikte titreşimlerden dolayı ivme değerlerinde +- 30, açı değerlinde de +-5 derecelik salınımlar oluşuyor. Buna rağmen PID kontrolu çok sayıda örneğin ortalaması ile çalıştığından sonuçta çubuk istendiği gibi konumlanıyor.

Ekranda ilk satırda görülen ivme değerleri ADXL345 den okunan +-256 arasındaki ham değerlerdir. Ani çıkış, ortalama çıkış ve Setpoint değerleri ise ADXL den okunan ve hedeflenen ivmelerin 0 ila 512 arasına taşınmış karşılığıdır.

ADXL345 üç eksenli bir sensör olduğundan titreşim kaynaklı bu parazitlerden kurtulamıyoruz. Ama MPU6050 gibi gyro da ölçebilen 6 eksenli bir sensör kullanırsak daha stabil sonuçlar alabiliriz. Bu ölçümlere ve açı hesaplamalarına yönelik bilgiler için aşağıdaki yayınlara bakabilirsiniz:

• İvme ve gyro sensör okumalarından hareketle eğim açılarının hesaplanması
• Bir çubuğu PID kontrol altında istenen eğimde tutmak

MPU6050 ve gyro ölçümlerini de kullandığım bir uygulamayı aşağıdaki yayında anlatıyorum:

Bir çubuğu PID kontrol atında istenen açıda tutmak – Bölüm 3 (MPU6050)

Bu yayının sonu – Selçuk Özbayraktar, Eylül 2021