مهفوم کامل PID با برد آردوینو

در این فیلم قصد داریم توسط برد آردوینو یک موتور را با استفاده از الگوریتم PID کنترل کنیم. ابتدا برای این کار لازم است مقدمه‌ای در مورد کنترل PID بیان شود. برای این منظور به این لینک مراجعه کنید.

پس از آشنایی مقدماتی با این نوع از کنترل به سراغ مثالی از این کنترل در برد آردوینو می‌رویم.

در این مثال با استفاده از یک عدد سنسور التراسونیک SRF05 و یک موتور DC این کنترلر را پیاده‌سازی می‌کنیم، به این صورت که تا زمانی که فاصله سنسور از جسم مقابل سنسور ۱۰۰ سانتی متر (1متر) باشد، موتور می‌ایستد و هرچه فاصله بیشتر شود پاد ساعت‌گرد و اگر فاصله کمتر شود ساعت‌گرد خواهد چرخید. به عبارت دیگر قصد داریم سرعت دوران و جهت دوران یک موتور دی‌سی را توسط برد آردوینو و سنسور التراسونیک SFR05 کنترل کنیم.

ابتدا توضیح کوتاهی در مورد سنسور التراسونیک می‌دهیم. این سنسور دارنده یک فرستنده و گیرنده می‌باشد که یک موج صوتی فرستاده می‌شود و با محاسبه‌ی زمان بازگشت و سرعت صوت در هوا می‌توان فاصله را محاسبه کرد. دقت داشته باشید که فرستنده باید حداقل به مدت ۱۰ میکروثانیه روشن بوده تا فاصله زمانی مناسب را خروجی بدهد.

در شکل زیر نحوه‌ی اتصال سیم‌ها به سنسور نمایش داده شده است.

نحوه‌ی اتصال سیم‌ها به سنسور SRF05

حال به سراغ نحوه‌ی سیم کشی کامل برد می‌رویم:

همانطور که در شکل زیر مشاهده می‌شود سیم‌های سنسور التراسونیک بر اساس data sheet به آردوینو متصل شده است. همچنین موتور به دو عدد از خروجی‌های PWM برد آردینو متصل می‌شود تا بتوانیم دور موتور را کنترل کنیم.

نحوه‌ی اتصال سنسور التراسونیک به آردوینو

سپس به سراغ نرم افزار آردوینو می‌رویم تا کدنویسی را آغاز کنیم. کدهای این برنامه به تفصیل توضیح داده شده است. در ابتدا به معرفی متغیر ها می‌پردازیم:

برای محاسبه دقیق تر فاصله ها بین 10 فاصله گرفته می شود و میانگین مورد محاسبه قرار خواهد گرفت // 
const int numOfReadings = 10; 
int readings[numOfReadings]; // مقادیر خوانده شده از سنسور در یک ماتریس ریخته می شود
int arrayIndex = 0; // آرایه ماتریس
int total = 0; // مجموع آرایه های ماتریس
//سنسور التراسونیک   ultrasonic 
int echoPin = 2; // SRF05 echo pin (digital 2)
int initPin = 3; // SRF05 trigger pin (digital 3)
unsigned long pulseTime = 0; // میزان زمان طی شده در هر رفت و برگشت
unsigned long distance = 0; // فاصله به سانتیمتر
// موتور
int dir1 = 9; // connected to digital PWM pin 9
int dir2 = 11; // connected to digital PWM pin 11
// PID Parameters
int Actual = 0; // میانگین فاصله های محاسبه شده
int Setpoint=100;      // میزان فاصله مطلوب ما برای فعالیت موتور
int Error=0;  //خطای محاسبه شده را صفر قرار می دهیم
int Integral=0; //خطای انتگرال گیر
int Derivative=0;  //خطای مشتق گیر
int Drive=0; //مقدار محاسبه شده در  فرمول کنترلر 
int Previous_error=0;
// PIDضرایب
int kP=5;
int kI=0.1;
int kD=0;
float dt=0.001;
//setup
void setup() {
pinMode(dir1, OUTPUT); // sets pin 9 as output
pinMode(dir2, OUTPUT); // sets pin 11 as output
pinMode(initPin, OUTPUT); // set init pin 3 as output
pinMode(echoPin, INPUT); // set echo pin 2 as input
// تعریف یک ماتریس ده آرایه صفر
for (int thisReading = 0; thisReading < numOfReadings; thisReading++) {
readings[thisReading] = 0;
}
نمایش مقادیر//
Serial.begin(9600);
}
void loop() {
while (arrayIndex<numOfReadings)
{
digitalWrite(initPin, HIGH); // به مدت 10 میکرو ثانیه پورت تریگر پالس میفرستد
delayMicroseconds(10); // wait 10 microseconds before turning off
digitalWrite(initPin, LOW); // stop sending the pulse
pulseTime = pulseIn(echoPin, HIGH); // Look for a return pulse, it should be high 
distance = pulseTime/58; // Distance = pulse time / 58 to convert to cm.
readings[arrayIndex] = distance; // add distance reading to array
total= total + readings[arrayIndex]; // add the reading to the total
arrayIndex = arrayIndex + 1; // go to the next item in the array
if (arrayIndex > numOfReadings) {
arrayIndex = 0;
break;
}
}
Actual = total / numOfReadings; // calculate the average distance
Error = Setpoint - Actual;
Integral = Integral + (Error*dt);
Derivative = (Error - Previous_error)/dt;
Drive = (Error*kP) + (Integral*kI) + (Derivative*kD);
Previous_error = Error;

if (Drive>0)
{
  if (Drive>255)
  {
    Drive=255;
  }
digitalWrite(dir1,LOW); // set direction motor
analogWrite(dir2,Drive); // send PWM command motor 
}

if (Drive<0)
{
  if (Drive<-255)
  {
    Drive=-255;
  }
digitalWrite(dir2,LOW); // set direction motor
analogWrite(dir1,abs(Drive)); // send PWM command motor 
}
Serial.println(Actual, DEC); // print out the average distance to the debugger
delay(1); // wait 1 milli seconds before looping again
}

تهیه کنندگان: مهندس سید امیر اشرفی – مهندس سینا اصفهانی