BİR ÇUBUĞU PID KONTROL ALTINDA DENGEDE TUTMAK – Bölüm 3 (MPU6050)

Önceki bölümlerde bu işi yapmak için üç eksenli ADXL345 ivme sensörü kullanmıştım. Projeyi bu defa da MPU6050 ivme sensörü ile gerçekleştirdim. MPU6050 sensörü gyro ölçümleri de yapabilen altı eksenli tipte olduğundan motor titreşimlerinden ve sarsıntılardan etkilenmeyen bir sistem oldu.

PID kontrol prensibi öncekiyle aynı da olsa, açı hesaplamaları, kontrol parametreleri oldukça farklı.

Bundan önce bu konudaki diğer yayınlarıma göz atmanızı öneririm :

• Bir çubuğu PID kontrolu ile istenen açıda tutmak-Bölüm 2 (2 motor)
• İvme ve gyro sensörleri ile eğim açılarının ölçülmesi
• Bir çubuğu PID kontrolu altında istenen açıda tutmak -Bölüm 1 (1 motor)

Bu projede çok küçük drone motorlarını 3055 MOSFET lerle sürüyoruz. Daha büyük motorları L298 ile hem de akım sınırlaması altında sürmek için “Akım sınırlaması altında DC motor sürmek” başlıklı yayınıma bakmanızı öneririm. Böyle bir sistemi akım sınırlaması olmadan kullanmak işi eksik yapmak anlamına gelecektir.

KONFİGÜRASYON BİLGİLERİ

Yazılım : Szb_Mapple_MPU6050_1AxisPID_R3 (Eylül 2021)
Donanım : STM32F103CB Mapple modülü + MPU6050 ivme ölçer

GENEL

Önceki gibi yine ortadaki mafsal çevresinde dönebilen bir “tahtırevalli” çubuğumuz var. Bunun iki ucunda 3.3V maksimum gerilimli, yüksek devirli, pervaneli birer drone motorumuz var.
Çubuk üzerine monte edilmiş olan MPU6050 ivme sensörümüz ile X ekseni çevresindeki eğim açımızı ölçüyoruz.

Denetçimiz STM32F103C8 tabanlı bir geliştirme modülü, üzerinde de 320×240 SPI TFT takılı. Sisteme istediğimiz eğim açısını bir ADC girişine bağlı potansiyometre ile bildiriyoruz. Motorları sürmek için de bir MOSFET sürücümüz var. Motorları sürmek üzere MOSFET’leri besleyen besleme kaynağımız mikro denetleyiciden ayrı bir 5V besleme kaynağı.

ADXL345 İLE YAŞANAN MOTOR TİTREŞİMLERİ SORUNU

ADXL345 üç eksenli bir sensör, sadece doğrusal ivmeleri ölçüyor, eğim açılarını da bu ivmelere dayanarak hesaplıyoruz. Ancak, doğrusal ivme ölçüm sonuçları motor titreşimlerinden çok etkileniyor. Bu etkilerden kurtulmak için çok sayıda ölçüm yapıp ortalamalarını kullanmak zorunda kalmıştım.

MPU6050 ise açısal ivme (gyro) ölçebiliyor. Gyro değerleri doğrusal titreşimlerden ve sarsıntılardan etkilenmediğinden çok daha sağlıklı açı değerleri alabiliyoruz. Bu konuya önceki yayınlarda ayrıntıları ile değinmiştim.

ÖNCEKİ UYGULAMA İLE YENİSİ ARASINDAKİ FARKLAR

Önceki bölümleri okumuş olanlar için bu bölümde yeni olan şeylerin neler olduğunu şöylece özetliyeyim:

• Yukarıda da belirttiğim gibi Üç eksenli ADXL345 yerine altı eksenli MPU6050 ivme sensörü kullanıyoruz. ADXL in sadece doğrusal ivmeleri 10 bitlik değişkenlerle vermesine karşılık MPU6050 altı eksende 16 bit ile verebiliyor. Bu kadar yüksek çözünürlük bizim için pek de önemli değil, açı değerini 3 anlamlı hane ile bir ondalık kesirli olarak bilmek bize yetiyor, yani 10 bit fazlasıyla yeterli.
• Önceki uygulamada ADXL den 10 milisaniyede bir gelen kesmeler ile okuma değerlerini alıp kaydediyorduk. Burada ise MPU6050 her milisaniyede bir ölçüm yapıyor, bizim sürecimiz bu kadar hızlı değil, sadece ekrandaki yazılar her çevrimde 150 milisaniye vakit alıyor. Ekrana hiç bir şey yazmasak da kontrol çevrimimiz 10 milisaniye sürüyor. O nedenle kesme kullanmıyoruz, her çevrim başlarken okuma yapıyoruz.
• En önemli fark: Önceki uygulamada ADXL den okunan değerleri ham haliyle +-256 aralığındaki değişkenler halinde kullanıyorduk. Bu uygulamada derece olarak açı değerlerine geçtik. Böylece MPU6050 nin derece/sn olarak verdiği açısal ivmeler ile doğrusal ivmeleri birleştirerek sensör düzleminin yatay konumdan sapmalarını hesaplıyoruz. Yani artık tek bir eksenin düşey yöndeki ivme bileşenine değil, düzlemin yatay konuma göre eğim açısına dayanarak çalışıyoruz.

DEĞİŞKEN SEÇİMLERİ HAKKINDA – int DEĞİŞKENLERLE ÇALIŞMAK

Sensörlerden gelen ivme değerleri 10/16 bitlik pozitif ya da negatif değerler. Bizim alışık olduğumuz açı değerleri ise 0..360 derece ve birkaç ondalık kesir. Algoritma içinde açılar ile karşılaştığımız anda kayan noktalı değişkenlere geçip öyle çalışmak mümkün, C kütüphanesinin trigonometrik fonksiyonları da böyle sonuçlar veriyor zaten.

Ancak, STM32F103 de kayan nokta işlemleri için yazılım kullanılıyor, bu da süreci yavaşlatan bir işlem. Kayan noktalı değişkenler çok küçük ve çok büyük değerlerdeki rakamların güvenle saklanıp kullanılabilmesini, bunlarla işlem yapılabilmesini sağlıyor. Halbuki bizim değişkenlerimizin 0 ila 360 arasında olduğu belli, hatta bu bile değil +-90 aralığının dışına çıkmıyoruz. Bu kadarı için kayan nokta aritmetiğine girmek gereksiz bir lüks.

Int değişkenlere virgülden sonraki iki haneyi de saklamak için açıları 100 katı değerleriyle saklayarak işlem yapıyorum. Sadece ekranda görüntülemek gerektiğinde oracıkta kayan noktalı formata çevirerek işimi görüyorum.

Zamanında sadece 1 Kb lık program bellekleriyle, 100 adet değişken saklanabilen RAM alanlarıyla çalışmış bir dinozor mühendis olarak kaynak kullanımındaki cimriliğimin anlayışla karşılanacağını umuyorum.

STM32 YAPILANDIRMA AYARLARI

STM32F103 PIN ATAMALARI

TIMER AYARLARI (TIM2 PWM)

ADC AYARLARI (POTANSİYOMETRE OKUMALARI İÇİN ADC1/IN1)

KODLAR

Program satırları arasına her zaman olduğu gibi çok ayrıntılı açıklamalar koyduğum için ayrıca açıklamalar yapmaya gerek görmüyorum.

MPU6050 FONKSİYONLARI

void MPU6050_Init (void)
{
	uint8_t check;
	uint8_t Data;

/* Açıları x100 int olarak saklıyoruz, bu nedenle radyan 
*  olarak gelen açıları dereceye çevirirken 180 yerine 
*  18000 değeri kullanıyoruz.
*/

radToDegree = 18000/3.1416; 

	last_gyro_x_angle = 0;
	last_gyro_y_angle = 0;
	last_gyro_z_angle = 0;

	last_x_angle = 0;
	last_y_angle = 0;
	last_z_angle = 0;

	t_last = HAL_GetTick();

HAL_GPIO_WritePin(M6050_EN_GPIO_Port, M6050_EN_Pin, SET); // Beslemeyi verelim
    HAL_Delay(1000);
	// check device ID WHO_AM_I

HAL_I2C_Mem_Read (&M6050_I2C_Handle, MPU6050_I2C_ADDR, MPU6050_WHO_AM_I, 1, &check, 1, 1000);

if (check == 104)  // MPU605 ile iletişim sağlandı ise 0x68 değeri dönmeli
	{
// power management register 0X6B we should write all 0's to wake the sensor up
		Data = 0;
HAL_I2C_Mem_Write(&M6050_I2C_Handle, MPU6050_I2C_ADDR, MPU6050_PWR_MGMT_1, 1,&Data, 1, 1000);

	// Set DATA RATE of 1KHz by writing SMPLRT_DIV register
		Data = 0x07;
HAL_I2C_Mem_Write(&M6050_I2C_Handle, MPU6050_I2C_ADDR, MPU6050_SMPLRT_DIV, 1, &Data, 1, 1000);

	// Set accelerometer configuration in ACCEL_CONFIG Register
	// XA_ST=0,YA_ST=0,ZA_ST=0, FS_SEL=0 -> ± 2g
		Data = 0x00;
HAL_I2C_Mem_Write(&M6050_I2C_Handle, MPU6050_I2C_ADDR, MPU6050_ACCEL_CONFIG, 1, &Data, 1, 1000);

// Set Gyroscopic configuration in GYRO_CONFIG Register
// XG_ST=0,YG_ST=0,ZG_ST=0, FS_SEL=0 -> ± 250 °/s
		Data = 0x00;
HAL_I2C_Mem_Write(&M6050_I2C_Handle, MPU6050_I2C_ADDR, MPU6050_GYRO_CONFIG, 1, &Data, 1, 1000);

// Set Data Ready Interrupt Enable, Interrupt Enable register 0x38 bit 0=1
		Data = 0x01;
HAL_I2C_Mem_Write(&M6050_I2C_Handle, MPU6050_I2C_ADDR, MPU6050_INT_ENABLE, 1, &Data, 1, 1000);
	}
/* Aşağıda sensör hareketsiz durumda iken ivme offset değerlerini okuyoruz.
 * Bu işlem 20ms sürüyor, bu arada sensör hareket ettirilmemelidir.
*/
	MPU6050_Read_Offsets();
}


void MPU6050_Read_Accel (MPU6050_t* sensor)
{
	int16_t Accel_X_RAW, Accel_Y_RAW, Accel_Z_RAW;
	uint8_t Rec_Data[6];
	uint8_t SampleCount = 1;
	sensor->Ax = 0;
	sensor->Ay = 0;
	sensor->Az = 0;

	// ACCEL_XOUT_H registerdan başlayarak 6 byte oku

 for(uint8_t i = 0; i<SampleCount; i++){
HAL_I2C_Mem_Read (&M6050_I2C_Handle, MPU6050_I2C_ADDR, MPU6050_ACCEL_XOUT_H, 1, Rec_Data, 6, 1000);

Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data [1]);// - offset_x_accel;
Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data [3]);// - offset_y_accel;
Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data [5]);

	/*** RAW değerleri G ye çevirelim. Bunun için okunan ham değeri tam skala
	 *   değerine bölmek gerekiyor. Tam skala seçimi ACCEL_CONFIG register
	 *   ayarlarında FS_SEL ile yapılıyor. FS_SEL = 0 yani 2g seçtiğimiz için
	 *   MPU6050_ACC_SENS_2 yani 16384 kullanıyoruz.
	 *
	 *   Bu değerleri x100 olarak saklıyoruz, float değişkenlerden kaçınmak
	 *   için. Bu nedenle 16394 olan katsayıyı 164 olarak uyguluyoruz.
	 */

	sensor->Ax += Accel_X_RAW/MPU6050_ACC_SENS_2;
	sensor->Ay += Accel_Y_RAW/MPU6050_ACC_SENS_2;
	sensor->Az += Accel_Z_RAW/MPU6050_ACC_SENS_2;
}

/* okumaların ortalamasını hesaplayalım, bu değerler "g" cinsinden */

sensor->Ax = sensor->Ax/SampleCount;
sensor->Ay = sensor->Ay/SampleCount;
sensor->Az = sensor->Az/SampleCount;
}


void MPU6050_Read_Gyro (MPU6050_t* sensor)
{
	int16_t Gyro_X_RAW, Gyro_Y_RAW, Gyro_Z_RAW;
	uint8_t Rec_Data[6];

	/* MPU6050_GYRO_XOUT_H dan başlayarak 6 byte oku.
	 * offset değerleri sistem açılışında sensör hareketsiz iken
	 * ilk 20 ms içinde ölçülüp kaydediliyor.
	 */

HAL_I2C_Mem_Read (&M6050_I2C_Handle, MPU6050_I2C_ADDR, MPU6050_GYRO_XOUT_H, 1, Rec_Data, 6, 1000);

	Gyro_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data [1])- offset_x_gyro;
	Gyro_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data [3])- offset_y_gyro;
	Gyro_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data [5])- offset_z_gyro;

	/*** Ham değerleri derece/sn ye çevireceğiz. Tam skalayı GYRO_CONFIG
	 *   registerde 250 Derece/sn olarak seçtiğimiz için
	 *   MPU6050_GYRO_SENS_250, yani 131 çarpanını kullanıyoruz.
	 * ****/

	/* bu değerler x100 derece/sn cinsinden saklıyoruz */

	sensor->Gx = (Gyro_X_RAW*100)/MPU6050_GYRO_SENS_250;
	sensor->Gy = (Gyro_Y_RAW*100)/MPU6050_GYRO_SENS_250;
	sensor->Gz = (Gyro_Z_RAW*100)/MPU6050_GYRO_SENS_250;
}

void MPU6050_Read_Offsets(void)
{   uint8_t SampleCount = 10;
	uint8_t Rec_Data[6];

	offset_x_accel = 0;
	offset_y_accel = 0;
	offset_z_accel = 0;

	for(uint8_t i = 0; i <= SampleCount; i++){
	// ACCEL_XOUT_H registerdan başlayarak 6 byte oku
HAL_I2C_Mem_Read (&M6050_I2C_Handle, MPU6050_I2C_ADDR, MPU6050_ACCEL_XOUT_H, 1, Rec_Data, 6, 1000);

	offset_x_accel += (int16_t)(Rec_Data[0] << 8 | Rec_Data [1]);
	offset_y_accel += (int16_t)(Rec_Data[2] << 8 | Rec_Data [3]);
	offset_z_accel += (int16_t)(Rec_Data[4] << 8 | Rec_Data [5]);

	HAL_Delay(2); // MPU6050 yaklaşık 1ms de bir yeni çıkış veriyor
	}
	/* okumaların ortalamasını hesaplayalım */

	offset_x_accel = offset_x_accel/SampleCount;
	offset_y_accel = offset_y_accel/SampleCount;
	offset_z_accel = offset_z_accel/SampleCount;


	// GYRO_XOUT_H register den başlayarak 6 byte oku
HAL_I2C_Mem_Read (&M6050_I2C_Handle, MPU6050_I2C_ADDR, MPU6050_GYRO_XOUT_H, 1, Rec_Data, 6, 1000);

	offset_x_gyro = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
	offset_y_gyro = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
	offset_z_gyro = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
}


void getAndCalculateAngles(angles_t* acilar){

    MPU6050_Read_Accel(&MPU6050_Sensor);
    MPU6050_Read_Gyro(&MPU6050_Sensor);

	t_now = HAL_GetTick();

  gyro_x = MPU6050_Sensor.Gx;  // Değerler x100 derece/sn
  gyro_y = MPU6050_Sensor.Gy;
  gyro_z = MPU6050_Sensor.Gz;

  // Aşağıdaki değerler g cinsinden, düzeltme faktörlerini devre dışı bırakıyoruz.
  accel_x = MPU6050_Sensor.Ax ; // - base_x_accel;
  accel_y = MPU6050_Sensor.Ay;  // - base_y_accel;
  accel_z = MPU6050_Sensor.Az ; // - base_z_accel;

// Aşağıda g cinsinden X ve Y ivmelerinden pitch ve roll açılarını hesaplıyoruz.

accel_angle_y=atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*radToDegree;
accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*radToDegree;
accel_angle_z = 0; // z açısı için gyro kullanacağız.

  // Compute the (filtered) gyro angles

  DT =t_now - t_last;
  dt = DT/1000.0;// Saniye olarak geçen süre
  t_last = t_now;

  /* Geçen ölçümden bu yana ne kadar dönmüş ?
   * Örnekleme süresi içindeki gyro değerinin
   * sabit kaldığını varsayıyoruz.
   *
   * Belki bir önceki gyro okuması ile bu son
   * okumanın ortalamasını kullanmak daha iyi
   * sonuç verebilir.
   */

  gyro_angle_x = gyro_x*dt + last_x_angle;
  gyro_angle_y = gyro_y*dt + last_y_angle;

int gz_threshold = 2; // Sensör hareketsiz iken z ekseni titreşimlerini ayıklayalım.
if(gyro_z < gz_threshold && gyro_z > -gz_threshold)
	gyro_z = 0;

gyro_angle_z = gyro_z*dt + last_z_angle;

  /* Açıdaki değişim (gyro) ve static ivme değerlerini birleştirerek
   * "complementary filter" ile nihai düzlem açılarını hesaplayalım.
   * Pitch(y ya da x), Roll (y ya da x) ve Yaw (z) açıları.
   *
   * Buradaki alpha katsayısı Gyro ve düz ivme okumalarına verilecek
   * önemi belirleyen ağırlık faktörü. Titreşimlere karşı filtreleme
   * etkisi yapıyor. Sarsıntılı, titreşimli sistemlerde düz ivme
   * değerlerinin payını küçük tutuyoruz.
   *
   */

  const int16_t alpha = 96;

  acilar->x = (alpha*gyro_angle_x + (100 - alpha)*accel_angle_x)/100;
  acilar->y = (alpha*gyro_angle_y + (100 - alpha)*accel_angle_y)/100;
  acilar->z = gyro_angle_z;
  acilar->DT = DT;

  last_x_angle = acilar->x ;
  last_y_angle = acilar->y ;
  last_z_angle = acilar->z ;
}

Buradaki son fonksiyondaki açıların hesaplanış ayrıntıları için aşağıdaki yayınıma bakabilirsiniz:

Doğrusal ve açısal ivmelerden hareketle eksen açılarının hesaplanması.

ANA PROGRAM

/* USER CODE BEGIN 0 */

uint16_t ADC_Read(void)
{
uint16_t adcVal;
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1);
adcVal = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
return adcVal;
}

int16_t computePID(int16_t *inputs){
	int16_t output;
	uint8_t i;

	/* Ortalama hesabı - entegral faktör hesabı için
	 */

	hata = setPoint-(*inputs); // Son ölçüm sonucuna göre
	output = hata/kp;

	/***** Integral hata faktörü hesabı  *****
	 * kayıtlı son intPERIOD adet örneklerden
	 * hareketle hesaplanıyor.
	 */

	kumHata = 0;
	for(i=0;i<=intPERIOD;i++) {
		kumHata += setPoint - *(inputs+i);
	}

/*  Integral faktörü hedeften çok uzakta iken devreye sokmuyoruz.
*   Aksi halde ortaya çıkan büyük Integral düzeltme faktörü nedeniyle
*   sistem stabilitesi bozulabiliyor. Bunun yerine önce oransal faktör
*   ile hatanın makul ölçülere düşürülmesini sağlıyoruz. Ondan sonra
*   görevi küçük hataların düzeltilmesi olan Integral faktörü devreye
*  sokuyoruz. Bu eşik seviyesini IcStart parametresi ile tanımlıyoruz.
*/

	/* Setpoint ile oranlamak/karşılaştırmak için
	 * kümülatif hatanın ortalamasını alıyoruz
	 * */

	kumHata = kumHata/(intPERIOD+1);

if(kumHata==0) kumHata = 1; // Aşağıdaki hesapta payda sıfır olmasın
if(abs((int)(outSpan/kumHata)) > IcStart) {// Integral faktör devreye girsin mi?
	output += kumHata/ki;
	}
    /* Integral faktör bölümünün sonu */

	/* Derivative faktör hesabı */
    /* Aşağıdaki kaba bir hesap, daha iyisi geliştirilmeli */
	derivative = (hata-oncekiHata)/sure;  // Süre sabit olarak "1" şimdilik
	oncekiHata = hata;
    /* Türevsel faktörü şimdilik devre dışı bırakalım
        output += derivative/kd;
    */
        return output;
}


/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */
uint8_t i;
  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

 /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_I2C1_Init();
  MX_SPI1_Init();
  MX_USART1_UART_Init();
  MX_ADC1_Init();
  MX_TIM2_Init();
  /* USER CODE BEGIN 2 */

  M6050_I2C_Handle = hi2c1;

	HAL_Delay(1000);

	TFT_Init();

/* başlangıç varsayılan değerlerini (yatay konum) yerleştir. */

kumHata = 0;
setPoint = 0;
for(i=0;i<=intPERIOD;i++){
	ICbuffer[intPERIOD-i] = 0;
}

/* 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ılabilir. Bu sürümde
 * 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;

/* Min ve max açılarımız +-90 derece
 * Tüm açı değerlerini x100 olarak
 * int16 değişkenlerde saklıyoruz.
 */

angle_Min =-9000;
angle_Max = 9000;

outSpan = angle_Max - angle_Min;

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

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

MPU6050_Init();

TFT_Clear(BLUE);

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
	  getAndCalculateAngles(&Angles);

	/* Çıkış ölçümünden (MPU6050) gelen yeni değerleri buffere koyalım.
	 * Son okunan 10 değeri saklıyoruz. Bunlar entegral hata hesabı
	 * için kullanılacaklar.
	 *  */

for (uint8_t i = intPERIOD; i > 0; --i) { //birer pozisyon sağa kaydıralım
		ICbuffer[i] = ICbuffer[i - 1];
		}
		/* Dengede duran çubuk projesinde sadece X eksenini kullanıyoruz.
		 * Bu nedenle sadece X açısını kaydediyoruz.
		 */
		ICbuffer[0] = Angles.x;

	/* Hedef açı değerini ADC den okunan potansiyometre konumundan hesapla */
		adc = ADC_Read();

		/* adc değerine karşı düşen hedef çıkış. (setpoint) */
		setPoint = (adc * outSpan)/4095 + angle_Min;

		/* ICbuffer'a kayıtlı son okumalara göre düzeltme faktörü */
			correction = computePID(ICbuffer);
	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.
*
* Motorların 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);

	/* x100 olan açı değerlerini gerçek seviyelerinde görüntüleyelim */
		float AniCIKIS = ICbuffer[0]/100.0;
		float KumHATA = kumHata/100.0;
		float setPOINT = setPoint/100.0;

		TFT_Printf(0,60, 24,  "Ani CIKIS:%.2f  ", AniCIKIS);
		TFT_Printf(0,120, 24, "Kum HATA:%.2f   ", KumHATA);
		TFT_Printf(0,150, 24, "SET POINT:%.2f  ", 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 */
}

TANIM VE BİLDİRİMLER

MAIN.c BİLDİRİMLERİ

/* USER CODE BEGIN PV */

I2C_HandleTypeDef M6050_I2C_Handle;

/* PID degiskenleri */

// MPU6050 ölçüm aralığı

int16_t angle_Min;
int16_t angle_Max;

int16_t outSpan;

uint16_t adc;
uint16_t pwm_o; // Motor1 ve 2 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

int16_t ICbuffer[intPERIOD+1];

int32_t derivative = 0;

/* PID çarpanlarını ben bölen olarak veriyorum,
 * float değişkenlerden kaçınmak için
 * */

int16_t kp = 500;
int16_t ki = 300;
int16_t kd = 4;

int16_t hata;
int16_t correction;
int16_t setPoint;

/* Integral için eklenen değişkenler */

uint32_t guncel_zaman, sure = 1 , baslangic_zaman = 0;

int16_t oncekiHata = 0;
int16_t kumHata = 0;
int16_t IcStart = 10; // Integral faktörün devreye giriş seviyesi (5 >> %20 hata)

/* USER CODE END PV */

MPU.h BİLDİRİMLERİ

typedef struct {
	int16_t Ax; /* X ekseni ivme  */
	int16_t Ay; /* Y ekseni ivme  */
	int16_t Az; /* Z ekseni ivme  */
	int16_t Gx;     /*!< Gyroscope value X axis */
	int16_t Gy;     /*!< Gyroscope value Y axis */
	int16_t Gz;     /*!< Gyroscope value Z axis */
	float Temperature;       /*!< Temperature in degrees */
} MPU6050_t;


typedef struct {
	int8_t x; /* X ekseni açı x100  */
	int8_t y; /* Y ekseni açı x100  */
	int8_t z; /* Z ekseni açı x100 */
	uint16_t DT; /*!< süre ms */
} intAngles_t;


typedef struct {
	int16_t x; /* X ekseni açı x100  */
	int16_t y; /* Y ekseni açı x100  */
	int16_t z; /* Z ekseni açı x100 */
	uint16_t DT; /*!< süre ms */
} angles_t;

angles_t Angles;

void MPU6050_Init (void);
void getAndCalculateAngles(angles_t* acilar);

#define MPU6050_I2C_ADDR			0xD0

/* MPU6050 registers */
#define MPU6050_AUX_VDDIO			0x01
#define MPU6050_SMPLRT_DIV			0x19
#define MPU6050_CONFIG				0x1A
#define MPU6050_GYRO_CONFIG			0x1B
#define MPU6050_ACCEL_CONFIG		0x1C
#define MPU6050_MOTION_THRESH		0x1F
#define MPU6050_INT_PIN_CFG			0x37
#define MPU6050_INT_ENABLE			0x38
#define MPU6050_INT_STATUS			0x3A
#define MPU6050_ACCEL_XOUT_H		0x3B
#define MPU6050_ACCEL_XOUT_L		0x3C
#define MPU6050_ACCEL_YOUT_H		0x3D
#define MPU6050_ACCEL_YOUT_L		0x3E
#define MPU6050_ACCEL_ZOUT_H		0x3F
#define MPU6050_ACCEL_ZOUT_L		0x40
#define MPU6050_TEMP_OUT_H			0x41
#define MPU6050_TEMP_OUT_L			0x42
#define MPU6050_GYRO_XOUT_H			0x43
#define MPU6050_GYRO_XOUT_L			0x44
#define MPU6050_GYRO_YOUT_H			0x45
#define MPU6050_GYRO_YOUT_L			0x46
#define MPU6050_GYRO_ZOUT_H			0x47
#define MPU6050_GYRO_ZOUT_L			0x48
#define MPU6050_MOT_DETECT_STATUS	0x61
#define MPU6050_SIGNAL_PATH_RESET	0x68
#define MPU6050_MOT_DETECT_CTRL		0x69
#define MPU6050_USER_CTRL			0x6A
#define MPU6050_PWR_MGMT_1			0x6B
#define MPU6050_PWR_MGMT_2			0x6C
#define MPU6050_FIFO_COUNTH			0x72
#define MPU6050_FIFO_COUNTL			0x73
#define MPU6050_FIFO_R_W			0x74
#define MPU6050_WHO_AM_I			0x75

/* Gyro sensitivities in °/s Açıları x100 olarak sakladığımız
 * için, katsayıları da /100 olarak kullanıyoruz. */

#define MPU6050_GYRO_SENS_250		((int16_t) 131.0)
#define MPU6050_GYRO_SENS_500		((int16_t) 65.5)
#define MPU6050_GYRO_SENS_1000		((int16_t) 32.8)
#define MPU6050_GYRO_SENS_2000		((int16_t) 16.4)

/* Acceleration sensitivities in g */
/*
#define MPU6050_ACC_SENS_2 ((float) 16384)
#define MPU6050_ACC_SENS_4  ((float) 8192)
#define MPU6050_ACC_SENS_8  ((float) 4096)
#define MPU6050_ACC_SENS_16 ((float) 2048)
*/

#define MPU6050_ACC_SENS_2 ((int16_t)164)
#define MPU6050_ACC_SENS_4 ((int16_t)82)
#define MPU6050_ACC_SENS_8 ((int16_t)41)
#define MPU6050_ACC_SENS_16 ((int16_t)20)

int16_t   temperature=0;
MPU6050_t MPU6050_Sensor;
char     data[120];

int16_t gyro_x, gyro_y, gyro_z;
int16_t accel_x, accel_y, accel_z;
int16_t accel_angle_x, accel_angle_y, accel_angle_z;

// rotation angle of the sensor

int16_t         last_x_angle; 
int16_t         last_y_angle;
int16_t         last_z_angle;

int16_t gyro_angle_x, gyro_angle_y, gyro_angle_z = 0;
int16_t angle_x, angle_y, angle_z;

int16_t unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z;

int16_t    offset_x_gyro;
int16_t    offset_y_gyro;
int16_t    offset_z_gyro;
int16_t    offset_x_accel;
int16_t    offset_y_accel;
int16_t    offset_z_accel;

unsigned long t_now=0, t_last=0;

float dt=0;
uint32_t DT;

int16_t Accel_X_RAW = 0;
int16_t Accel_Y_RAW = 0;
int16_t Accel_Z_RAW = 0;

int16_t Gyro_X_RAW = 0;
int16_t Gyro_Y_RAW = 0;
int16_t Gyro_Z_RAW = 0;

int16_t x_angle;
int16_t y_angle;

int16_t x_low, x_high;
int16_t y_low, y_high;
int16_t x_offset, y_offset;

int16_t Ax, Ay, Az, Gx, Gy, Gz;

int16_t radToDegree;

extern I2C_HandleTypeDef M6050_I2C_Handle;

SONUÇLAR

Üç eksenli ADXL345 modeline göre çok daha stabil sonuçlar aldım. Motor titreşimlerinin gözlenebilir bir etkisi olmuyor. Kp, Ki katsayıları sistemin stabilitesi açısından çok önemli. Hatalı değerler sistemin kontroldan çıkıp çılgın gibi sağa sola çarpmasına yol açıyor.

Önce Integral kontrolu devre dışı bırakıp sadece oransal kontrol ile Kp katsayısının belirlenip, bunun ardından entegral faktör devreye sokularak Ki katsayısının belirlenmesi gerekiyor. Bu projede de türev faktörü kullanmadım, bir iyileştirme projesi olarak bunu da ele alabilirim. Aslında bu tür bir yapı için türevsel faktörün yararlı olacağını düşünüyorum, pervaneli itki, sisteme verdiğiniz düzeltme komutlarının ani sonuçlar vermesini engelliyor, sistem ataleti yüksek.

Böylece PID kontrol konusundaki yayınlar serisini tamamlamış oldum. Umarım birilerinin işine yarar. Bir sonraki aşamada çubuğun ortasındaki bağı çözüp uçurmayı deneyebilirim, ama şimdilik önceliği yok.

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