با عرض سلام مجدد خدمت دوستان عزیز
امروز قسمت چهارم و پایانی ساخت ربات تعقیب خط با استفاده از آردوینو رو براتون می زاریم.
آموزش کامل ساخت این پروژه رو می تونید در سایت کافه ربات مشاهده کنید.
آموزش ساخت ربات تعقیب خط با استفاده از آردوینو| پروژه آردوینو
گام ششم: کد
کد زیر در واقع با توجه به سنسورهای IR و خواندن مقادیر از آن ها به موتورها فرمان می دهد که با چه سرعتی حرکت کنند تا ربات به سمت دلخواه بچرخد. این ربات دارای 8 سنسور است. اگر فرض کنیم ربات همواره در یک طرف خط قرار دارد در این صورت در هر زمان ربات می تواند در یکی از 9 موقعیت از 0 تا 8 باشد که با توجه به اینکه کدامیک از سنسورها روشن هستند این موقعیت مشخص می شود. ما می خواهیم در هر زمان مرکز ربات روی لبه خط سیاه قرار گیرد بنابراین setpoint را روی عدد 4 تنظیم می کنیم که در واقع فاصله بین سنسور چهارم و پنجم می باشد. کنترلر PID به این صورت عمل می کند که یک مقدار مطلوب را در نظر گرفته و با توجه به مقدار فعلی و تفاوت آن با مقدار مطلوب، که آن را خطا می نامیم عملکرد مناسب را نشان می دهد.
در این حالت خطا برابر است با تفاضل setpoint از موقعیت مکانی ربات. حال باید ثابت های KP و Kd را که به آن ها ثایت های تناسبی و مشتقی گفته می شود بیابیم.
برای محاسبه ترم تناسبی کنترلر PID ، باید تفاضل PWM بیت دو موتور را بر واحد خطا تقسیم کرده و در در خود خطا ضرب نماییم. ترم مشتقی این کنترلر نیز با ضرب کردن ضریب مشتقی Kd در تغییرات خطا محاسبه می گردد.
و سپس این دو ترم با هم جمع شده و فرمان کنترل را صادر می نمایند.
کد نوشته شده در زیر این مراحل را انجام می دهد. این کد را کپی کرده و در نرم افزار آردوینوی خود پیست نمایید.
#define NUM_SENSORS 8
#define avgSpeed 255
int leftWheelf=3;
int leftWheelr = 5;
int rightWheelf=6;
int rightWheelr = 9;
int setpoint=4, val;
unsigned long lastTime=0, timeChange=0;
int Sampletime=20, outMax=255, outMin=-255;
float error,sumerr,lastError,output,ITerm,DTerm;
float Kp=avgSpeed/4, Ki=0, Kd=.25*Kp;
int pos;
unsigned int sensor[]={A0,A1,A2,A3,A4,A5,A6,A7};
unsigned int sensorValue[NUM_SENSORS];
int threshold = 200;
byte incomingByte;
int bias=5;
void setup() {
Serial.begin(115200);
pinMode(4,INPUT);
}
void loop() {
// Serial.println(digitalRead(4));
if (digitalRead(4)== true) {
unsigned int Wsum=0;
int sum=0;
for (int i=0;i<NUM_SENSORS;i++) {
sensorValue[i]=analogRead(sensor[i]);
if (sensorValue[i] < threshold) sensorValue[i]=1;
else sensorValue[i]=0;
//Serial.print(i); Serial.print(": "); Serial.println(sensorValue[i]);
//delay(250);
}
for (int i=0;i<NUM_SENSORS;i++) {
sum=sensorValue[i]+sum;
pos=sum;
}
// Serial.println(pos);
//delay(100);
timeChange = millis()-lastTime;
if (timeChange >= Sampletime){
pid(pos);
}
}
else {
if (Serial.available() > 0) {
incomingByte = Serial.read();
}
if (incomingByte == 'w') {
forward();
}
else if (incomingByte == 's') {
reverse();
}
else if (incomingByte == 'd') {
rightTurn();
}
else if (incomingByte == 'a') {
leftTurn();
}
else {
brake();
}
}
}
void pid (int val) {
error=setpoint-val;
ITerm+=(Ki*error);
DTerm=Kd*(error-lastError)/(Sampletime/1000.0);
lastError=error;
if(ITerm > outMax) ITerm=outMax;
else if (ITerm < outMin) ITerm=outMin;
output=Kp*error+ITerm+DTerm;
if (output>outMax) output=outMax;
else if (output<outMin) output=outMin;
lastTime=millis();
Serial.println(val);
if (output>0) {
analogWrite(leftWheelf,avgSpeed);
analogWrite(rightWheelf,avgSpeed);
analogWrite(leftWheelr,abs(output));
analogWrite(rightWheelr,0);
}
else if (output<0) {
analogWrite(leftWheelf,avgSpeed);
analogWrite(rightWheelf,avgSpeed);
analogWrite(leftWheelr,0);
analogWrite(rightWheelr,abs(output));
}
else {
analogWrite(leftWheelf,avgSpeed);
analogWrite(rightWheelf,avgSpeed);
analogWrite(leftWheelr,0);
analogWrite(rightWheelr,0);
}
}
void forward() {
analogWrite(leftWheelf, avgSpeed - bias);
analogWrite(leftWheelr, 0);
analogWrite(rightWheelr, 0);
analogWrite(rightWheelf, avgSpeed);
}
void leftTurn() {
analogWrite(leftWheelf,0);
analogWrite(leftWheelr, 0);
analogWrite(rightWheelr,0);
analogWrite(rightWheelf, avgSpeed);
}
void rightTurn() {
analogWrite(leftWheelf,avgSpeed);
analogWrite(leftWheelr, 0);
analogWrite(rightWheelr,0);
analogWrite(rightWheelf, 0);
}
void reverse() {
analogWrite(leftWheelf, 0);
analogWrite(leftWheelr, avgSpeed-bias);
analogWrite(rightWheelr, avgSpeed);
analogWrite(rightWheelf, 0);
}
void brake() {
analogWrite(leftWheelf, 0);
analogWrite(leftWheelr, 0);
analogWrite(rightWheelr, 0);
analogWrite(rightWheelf, 0);
}
گام هفتم: کنترل از راه دور
اگر دوست دارید ربات را به صورت دستی و از راه دور کنترل کنید، به یک آداپتور بلوتوث نیاز دارید.
اگر GND را به زمین، Vin را به +5 ولت، Tx را به پین 0 روی آردوینو و Rx را به پین 1 روی آردوینو وصل کنید، می توانید کامپیوتر خود را با آردینو تان جفت کنید. این کار درست مانند یک ارتباط سریال است که می توانید از آن برای آپلود اسکریپ یا ارسال اطلاعات استفاده کنید. کد اسکریپ زیر به شما اجازه میدهد تا ربات را با استفاده از کلیدهای WASD کنترل کنید.
// Click on the image to give it focus,
// and then press any key.
import processing.serial.*;
int value = 0;
String word = " ";
String mode = " ";
boolean auto = false;
Serial myPort;
void setup() {
size(300, 90); // size always goes first!
String portName = Serial.list() [0];
myPort = new Serial(this, portName, 115200);
}
void draw() {
background(0);
myPort.write(key);
if (auto) {
mode = " AUTO ";
word = " ";
}
else {
mode = "MANUAL";
}
fill(255);
textSize(40);
textAlign(CENTER);
text(mode, width/2, 40);
textAlign(CENTER);
text(word, width/2, 80);
}
void keyPressed() {
if (key == ' ') {
auto = !auto;
}
if (!auto) {
if (key == 'w') {
word = "FORWARD";
}
else if (key == 'd') {
word = " RIGHT ";
}
else if (key == 'a') {
word = " LEFT ";
}
else if (key == 's') {
word = "REVERSE";
}
else {key = 'p';}
}
}
void keyReleased() {
key = 'p';
}
با آرزوی موفقیت دوستان
کافه ربات | الهام بخش خلاقیت
