با استفاده از این کد شما میتوانید روباتی بسازید که به طرف جسم در حال حرکت رفته و یا جسمی را پیدا کرده و به سمت او حرکت میکند. در روبات شما باید دو سنسور اینفرارد قرار گیرد. این دو سنسور نباید هم جهت باشند و یکی به سمت شمال شرقی باشد و یکی به سمت شمال غربی.
با وارد کرد این کد، 5 حالت وجود خواهد داشت :
1. اگر هردو سنسور جسمی را شناسایی کردند و این جسم بین 5 تا 15 سانتی متر با سنسور فاصله داشت، این جسم در محدوده ی سنسور میباشد و در صورتی که روبات در حال حرکت باشد از حرکت می ایستد و دوباره چک میکند.
2. اگر هردو سنسور جسمی را شناسایی کردند و این جسم کمتر از 5 سانتی متر با سنسور فاصله داشت، این جسم به روبات نزدیک هست و روبات برای جلوگیری از برخورد با جسم، به عقب حرکت میکند.
3. اگر سنسور سمت چپ، جسمی را شناسایی کند اما سنسور سمت راست شناسایی نکند، روبات به سمت چپ میچرخد و سپس به جلو حرکت میکند.
4. اگر سنسور سمت راست، جسمی را شناسایی کند اما سنسور سمت چپ شناسایی نکند، روبات به سمت راست میچرخد و سپس به جلو حرکت میکند.
5. اگر هیچ کدام از سنسور ها جسمی را شناسایی نکردند، برای جستجو به چرخش ادامه میدهد.
#define pin A1 // distance sensor to analog 1
int E1 = 5;
//M1 Speed Control
int E2 = 6;
//M2 Speed Control
int M1 = 4;
//M1 Direction Control
int M2 = 7;
//M1 Direction Control
void stop(void)
// stop
{
digitalWrite(E1,0);
digitalWrite(M1,LOW);
digitalWrite(E2,0);
digitalWrite(M2,LOW);
}
void advance(char a,char b)
//forward
{
analogWrite (E1,a);
//PWM Speed Control
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}
void back_off (char a,char b)
//backward
{
analogWrite (E1,a);
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
void turn_L (char a,char b)
//left
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
void turn_R (char a,char b)
//right
{
analogWrite (E1,a);
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}
void setup() {
// put your setup code here, to run once:
int i;
for(i=4;i<=7;i++)
pinMode(i, OUTPUT);
pinMode(8,INPUT);
pinMode(9,INPUT);
pinMode (pin,INPUT);
Serial.begin(9600);
}
int left=0;
int right=0;
void loop() {
// put your main code here, to run repeatedly:
uint16_t value = analogRead (pin);
int x= get_gp2d120x (value);
left=digitalRead(8);
right=digitalRead(9);
Serial.println(x);
if(!left && !right && x>15)
{
advance(100,100);
}
else if((!left && !right&& x>8 && x<15)||(left &&right))
{
stop();
}
else if(!left && !right&& x<8)
{
back_off(100,100);
}
else if(!left && right)
{
turn_L(100,100);
}
else if(left && !right)
{
turn_R(100,100);
}
}
double get_gp2d120x (uint16_t value)
{
if (value < 16)
value = 16;
return 2076.0 / (value - 11.0);
}
