5
Home
Paris_Lizaj edited this page 5 years ago
//KWDIKAS AUTONOMOU AMAKSIOU
#include <Servo.h> // Includes servo library.
// defines pins numbers
const int trigPin = 12;
const int echoPin = 11;
const int lm1=7;
const int lm2=6;
const int rm1=5;
const int rm2=4;
const int servoPin=13;
Servo myservo;
// orizw katholikes metavlites gia na tis peirazw apo tis sunartiseis kai na boroun na allazoun kai timh
long duration;
int distance;
int distance_f; //apostash tou amaksiou apo antikeimeno eutheia
int distance_r; //apostash tou amaksiou apo antikeimeno deksia
int distance_l; //apostash tou amaksiou apo antikeimeno aristera
int maxLowDistance=20; //elaxisth apostash sthn opia ama vrethw stamataw
void setup() {
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(lm1,OUTPUT); //orizw tis metavlites gia tous kinitires mou gia na borw na tous diaxiristw
pinMode(lm2,OUTPUT);
pinMode(rm1,OUTPUT);
pinMode(rm2,OUTPUT);
myservo.attach(servoPin);
myservo.write(90);
Serial.begin(9600); // Starts the serial communication
}
void loop() {
distance_f=ping(); //Psaxnw gia thn apostash pou exw brosta mou
if(distance_f > maxLowDistance){ //An einai megaluteri apo 20cm h apostash tou amaksiou apo to prwto ebodio (diladi thn elaxisth apostash pou tou exw orisei) tote tha paei eutheia
displayDistance(); //to kanw gia na kanw debug kai na kserw oti leitourgei o aisthititras apostashs
front(); //kalw thn sunartisi gia na paei brosta to amaksi mou
delay(300);
}else{ //An den einai megaluteri apo 20cm h apostash tou amaksiou apo to prwto ebodio
Break(); //Kalw thn sunartisi break gia na stamatisei na kineite to amaksi
get_Distance(); //kalw thn get_distance kai pernw plirofories gia tis apo staseis pou exw gyrw mou
if(distance_r > maxLowDistance){ //pleon gnorizw ti exw deksia mou kai elegxw an yparxei ebodio sta epomena 20cm brosta mou.
right(); //an den exw ebodio stivw epitopou deksia
delay(300);
front();
}else if(distance_l > maxLowDistance){
left();
delay(300);
front();
}else{
back();
delay(300);
Break();
}
}
}
//thn exw gia debug ayth thn sunartisi gia na dw an leitourgei o esthitiras swsta kai an katagrafontai oi times gia tis apostaseis eutheia deksia kai aristera
void displayDistance(){
Serial.print("Right Distance : ");
Serial.print(distance_r);
Serial.println("");
Serial.print("Front Distance : ");
Serial.print(distance_f);
Serial.println("");
Serial.print("Left Distance : ");
Serial.print(distance_l);
Serial.println("");
}
//Logo tou oloklirwmenou LM298 borw na diaxiristw kai pio eukola to amaksaki mou
//exw ftiaksei 5 sunartiseis gia na boreis na pigainei brosta pisw aristera deksia kai na stamataei
//shnarthsh gia na pigenei brosta to amaksi
void front(){
Serial.println("Forward Move"); //vazw se leitourgeia kai tous 4 troxous
digitalWrite(lm2,HIGH);
digitalWrite(rm2,HIGH);
digitalWrite(lm1,LOW);
digitalWrite(rm1,LOW);
}
//shnarthsh gia na pigenei pisw to amaksi
void back(){
Serial.println("Back Move"); //vazw se leitourgeia kai tous 4 troxous alla antistrefw thn polikothta gia na paei pros ta pisw
digitalWrite(lm1,HIGH);
digitalWrite(rm1,HIGH);
digitalWrite(lm2,LOW);
digitalWrite(rm2,LOW);
}
//shnarthsh gia na pigenei aristera to amaksi
void left(){
digitalWrite(rm2,HIGH); //vazw se leitourgeia mono tous dyo deksious troxous
digitalWrite(rm1,LOW);
digitalWrite(lm1,HIGH);
digitalWrite(lm2,LOW);
}
//shnarthsh gia na pigenei deksia to amaksi
void right(){
digitalWrite(lm2,HIGH); //vazw se leitourgeia mono tous dyo aristerous troxous
digitalWrite(lm1,LOW);
digitalWrite(rm1,HIGH);
digitalWrite(rm2,LOW);
}
//shnarthsh gia na pigenei stamataei to amaksi
void Break(){
digitalWrite(lm2,LOW); //vgazw apo thn leitourgeia kai tous 4 troxous
digitalWrite(lm1,LOW);
digitalWrite(rm1,LOW);
digitalWrite(rm2,LOW);
}
void get_Distance(){
myservo.write(0); //gurnaw to servo mou deksia gia na boresei na parei metrisi o aisthitiras
delay(500);
int temp_r1=ping(); //pernw thn apostash deksia mou
myservo.write(45); //gurnaw to servo mou 45 moires (diladi loksa deksia)
delay(500);
int temp_r2=ping(); //pernw thn apostash gia tis 45 moires
if(temp_r1<temp_r2){ //kai apo tis duo apostaseis tha parw thn mikroteri gia na borw na exw mia pio kathari eikona tou ti vriksete deksia mou. Peiramatika apodikniete oti einai polu kalutero na kserw ti vriskete diksia mou gnorizontas kai gia tis 0 moires
distance_r=temp_r1; //kai gia tis 45 gia na borw na stripsw me asfaleia
}else{
distance_r=temp_r2;
}
myservo.write(90); //pleon o servo mou einai stis 90 moires ara o aisthitiras "koitaei" eutheia
delay(500);
distance_f=ping(); //pernw thn apostash eutheia mou
myservo.write(135); //kai omoia gia thn diadikasia pou ekana gia ta deksia tha kanw kai gia ta aristera.Tha parw metrisi kai apo tis 135 moires kai apo tis 180 moires (diladi teleiws aristera) kai apo tis duo tha thewrisw gia thn apostash aristera mou thn mikroteri
delay(500);
int temp_l1=ping();
myservo.write(180);
delay(500);
int temp_l2=ping();
if(temp_l1<temp_l2){
distance_l=temp_l1;
}else{
distance_l=temp_l2;
}
myservo.write(90);
}
//thn xrisimopoiw gia na parw tis metriseis tou aisthitira kai na kanw thn katalili metatropi.
int ping(){
// Clears the trigPin
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// Sets the trigPin on HIGH state for 10 micro seconds
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
duration = pulseIn(echoPin, HIGH);
// Calculating the distance
distance= duration*0.034/2;
// Prints the distance on the Serial Monitor
return distance;
}