ربات تعقیب خط با آردوینو

 

مقدمه:

در این پروژه می خواهیم یک ربات تعقیب خط ساده به کمک سنسور های تشخیص مانع مادون قرمز و ماژول  L298N بسازیم.

این ربات با حداقل امکانات برای بهترین کارکرد ممکن طراحی شده است. همانطور که در عکس می بینید سنسورهای مادون قرمز جلوی ربات و با زاویه حدود 60 درجه نسبت به زمین قرار گرفته اند. می توانید سنسورهارا روی تکه ای فوم معماری نصب کنید که هم سبک است و هم استحکام نسبتا مناسبی دارد. ماژول L298 برای راه اندازی موتورها به کمک باتری لیتیومی به کار می رود. برای تغذیه آردوینو و سنسورها از باتری کتابی استفاده شده است. تغذیه موتورها بدلیل توان بالایی که نیاز دارند توسط باتری های کتابی قابل تامین نیست و جدا در نظر گرفته شده است. برای تغذیه موتورها میتوانید از باتری های آلکالاین استفاده کنید. چرخ هرزگرد نیز زیر شاسی ربات نصب می شود تا تعادل این ربات با دو چرخ دیگر حفظ شود.

 

 

 

 

قطعات مورد نیاز


برد آردوینو UNO

یک عدد

درایور L298N

یک عدد

ماژول سنسور TCRT5000 فرستنده و گیرنده مادون قرمز

5 عدد

موتور گیربکس زرد پلاستیکی یکطرفه 6ولت 30rpm با نسبت دور 1 به 220

2 عدد

چرخ پلاستیکی

2 عدد

چرخ هرزگرد

1 عدد

شاسی ربات

یک عدد

سازه ربات

 

باتری لیتیوم تا 6 ولت و حداقل 2 آمپر خروجی

1 عدد

باتری کتابی

1 عدد

جاباتری کتابی

1 عدد

سیم بردبورد

 

بردبورد

1 عدد

 

 

 

توضیح عملکرد:

سنسورهای مادون قرمز که جلوی ربات نصب شده اند در دوحالت دیجیتال و آنالوگ کار می کنند که در اینجا از قابلیت های دیجیتال آنها استفاده می کنیم. عملکرد این سنسور ها این گونه است که LED فرستنده به طور مداوم نور مادون قرمز تولید می کند. اگر این نور به سطح شفاف برخورد کند، بازتاب یافته و توسط LED گیرنده دریافت شده که بیانگر سطح سفید است اما اگر این نور به سطح سیاه بتابد بازتابی نداریم و در ادامه دریافتی نیز نخواهیم داشت. حالا می خواهیم از همین ساز و کار برای تشخیص خطا روی صفحه سفیدی که با خطوط سیاه مسیرکشی شده است استفاده کنیم. با تشخیص مرز بین سفید و سیاه می توانیم چرخ های ربات را در مسیر راست و چپ با سرعت متغیر(به کمک PWM) حرکت دهیم تا ربات کاملا در مسیر خطوط سیاه باقی بماند.

این ربات یکی از ساده ترین انواع ربات برای ورود به دنیای رباتیک و الکترونیک است و قطعا لازم نیست که یادآوری کنیم که بسیار هم سرگرم کننده است!

 

 

برنامه: 

float pTerm, iTerm, dTerm;

int error;

int previousError;

float kp = 11; //11

float ki = 0;

float kd = 11; //11

float output;

int integral, derivative;

int irSensors[] = {13, 12, 11, 8, 7}; //IR sensor pins

int irReadings[5];

int motor1Forward = A0;

int motor1Backward = A1;

int motor1pwmPin = 5;

int motor2Forward = A3;

int motor2Backward = A2;

int motor2pwmPin = 3;

int motor1newSpeed;

int motor2newSpeed;

int motor2Speed = 70; //Default 70

int motor1Speed = 120; //Default 120

 

void setup() {

  //Declare all IR sensors as inputs

  for (int pin = 0; pin < 5; pin++) {

    int pinNum = irSensors[pin];

    pinMode(pinNum, INPUT);

  }

  pinMode(motor1Forward, OUTPUT);

  pinMode(motor1Backward, OUTPUT);

  pinMode(motor1pwmPin, OUTPUT);

  pinMode(motor2Forward, OUTPUT);

  pinMode(motor2Backward, OUTPUT);

  pinMode(motor2pwmPin, OUTPUT);

}

 

void loop() {

  //Put all of our functions here

  readIRSensors();

  calculateError();

  pidCalculations();

  changeMotorSpeed();

}

 

void readIRSensors() {

  //Read the IR sensors and put the readings in irReadings array

  for (int pin = 0; pin < 5; pin++) {

    int pinNum = irSensors[pin];

    irReadings[pin] = digitalRead(pinNum);

  }

}

 

void calculateError() {

  //Determine an error based on the readings

  if ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2] == 0) && (irReadings[3] == 0) && (irReadings[4] == 1)) {

    error = 4;

  } else if ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2] == 0) && (irReadings[3] == 1) && (irReadings[4] == 1)) {

    error = 3;

  } else if ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2] == 0) && (irReadings[3] == 1) && (irReadings[4] == 0)) {

    error = 2;

  } else if ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2] == 1) && (irReadings[3] == 1) && (irReadings[4] == 0)) {

    error = 1;

  } else if ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2] == 1) && (irReadings[3] == 0) && (irReadings[4] == 0)) {

    error = 0;

  } else if ((irReadings[0] == 0) && (irReadings[1] == 1) && (irReadings[2] == 1) && (irReadings[3] == 0) && (irReadings[4] == 0)) {

    error = -1;

  } else if ((irReadings[0] == 0) && (irReadings[1] == 1) && (irReadings[2] == 0) && (irReadings[3] == 0) && (irReadings[4] == 0)) {

    error = -2;

  } else if ((irReadings[0] == 1) && (irReadings[1] == 1) && (irReadings[2] == 0) && (irReadings[3] == 0) && (irReadings[4] == 0)) {

    error = -3;

  } else if ((irReadings[0] == 1) && (irReadings[1] == 0) && (irReadings[2] == 0) && (irReadings[3] == 0) && (irReadings[4] == 0)) {

    error = -4;

  } else if ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2] == 0) && (irReadings[3] == 0) && (irReadings[4] == 0)) {

    if (previousError == -4) {

      error = -5;

    } else {

      error = 5;

    }

  } else if ((irReadings[0] == 1) && (irReadings[1] == 1) && (irReadings[2] == 1) && (irReadings[3] == 1) && (irReadings[4] == 1)) {

    error = 0;

  }

}

 

void pidCalculations() {

  pTerm = kp * error;

  integral += error;

  iTerm = ki * integral;

  derivative = error - previousError;

  dTerm = kd * derivative;

  output = pTerm + iTerm + dTerm;

  previousError = error;

}

 

void changeMotorSpeed() {

  //Change motor speed of both motors accordingly

  motor2newSpeed = motor2Speed + output;

  motor1newSpeed = motor1Speed - output;

  //Constrain the new speed of motors to be between the range 0-255

  constrain(motor2newSpeed, 0, 255);

  constrain(motor1newSpeed, 0, 255);

  //Set new speed, and run motors in forward direction

  analogWrite(motor2pwmPin, motor2newSpeed);

  analogWrite(motor1pwmPin, motor1newSpeed);

  digitalWrite(motor2Forward, HIGH);

  digitalWrite(motor2Backward, LOW);

  digitalWrite(motor1Forward, HIGH);

  digitalWrite(motor1Backward, LOW);

}

 

 

مدار:

 

 

 

 

Top