Robot 2WD Roboromania autonom ocolire obstacole realizat de Ștefan Ziegler din Arad
Aveti nevoie de :
o placă de dezvoltare tip Arduino UNO (oricare)
un modul driver motoare L298N
un senzor cu ultrasunete
șasiu cu 2 motoare 2WD
cabluri Dupont
//––––-
// roboromania.ro
// Robot 2WD Roboromania autonom ocolire obstacole – versiunea testata 2018.02.03
// Ștefan Ziegler din Arad stefan@ecomania.ro
#include <NewPing.h>
#define TRIG_PIN 8
#define ECHO_PIN 7
#define MAX_DISTANCE 400
#define COLL_DIST 15 // distanta de coliziune la care robot stop si inapoi este de : 15cm
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
#define SPEED 10
// L298n module
// 1(+)inainte dreapta = DrFr
// 2(+)inapoi dreapta = DrSp
// 3(+)inainte stanga = StFr
// 4(+)inapoi stanga = StSp
int DrFr = 3;
int DrSp = 2;
int StFr = 4;
int StSp = 5;
void setup() {
Serial.begin(9600);
pinMode(DrFr,OUTPUT);
pinMode(StFr,OUTPUT);
pinMode(DrSp,OUTPUT);
pinMode(StSp,OUTPUT);
digitalWrite(DrFr,LOW);
digitalWrite(StFr,LOW);
digitalWrite(DrSp,LOW);
digitalWrite(StSp,LOW);
}
int scan() {
return (sonar.ping() / US_ROUNDTRIP_CM); //masurare distanta in cm
}
void loop() {
int Dist = scan(); // masuram distanta curenta
// Serial.println(Dist);
if ((Dist < COLL_DIST) && (Dist > 0)) { // daca distanta curenta < decit distanta de coliziune
moveStop();
moveBackward();
delay(500);
turnRight();
delay(300);
} else {
moveForward();
}
}
void moveStop() {
digitalWrite(DrFr,LOW);
digitalWrite(StFr,LOW);
digitalWrite(DrSp,LOW);
digitalWrite(StSp,LOW);
}
void moveForward() {
digitalWrite(DrFr,HIGH);
digitalWrite(StFr,HIGH);
digitalWrite(DrSp,LOW);
digitalWrite(StSp,LOW);
}
void moveBackward() {
digitalWrite(DrFr,LOW);
digitalWrite(StFr,LOW);
digitalWrite(DrSp,HIGH);
digitalWrite(StSp,HIGH);
}
void turnRight() {
digitalWrite(DrFr,LOW);
digitalWrite(StFr,HIGH);
digitalWrite(DrSp,HIGH);
digitalWrite(StSp,LOW);
}
void turnLeft() {
digitalWrite(DrFr,HIGH);
digitalWrite(StFr,LOW);
digitalWrite(DrSp,LOW);
digitalWrite(StSp,HIGH);
}
//––––––
Pentru întrebări suplimentare : Ștefan Ziegler din Arad – stefan@ecomania.ro