Obstacle Avoiding Robot, simpele sketch

Toon hier Uw afgeronde projecten aan anderen.
Berichten: 4
Geregistreerd: 03 Mrt 2016, 10:12

Obstacle Avoiding Robot, simpele sketch

Berichtdoor Bar » 13 Apr 2016, 07:22

Hier een filmpje (en sketch) van mijn simpele obstacle avoiding robot. De servo laat de afstandssensor 'rondkijken' . Onder het rijden ook om te voorkomen dat een obstakel wat onder een hoek wordt benaderd, ook wordt gezien. Na het ,zien' van het obstakel 'kijkt' de robot naar links en naar rechts. Daarna wordt bepaald in welke richting wordt gereden (richting met meeste afstand). Voor de motorbesturing een shield gebruikt. Nog simpeler te maken met 2 DPDT relais. Die zijn er ook in micro varianten (3,3V of 5V).


Filmpje hier: https://www.youtube.com/watch?v=PTPsJPdGPXQ

Sketch hier:
cpp code
#include <Servo.h> 
Servo myservo; // create servo object to control a servo

const int echoPin = 12; // ultrasonic receive=echo pin
const int trigPin = 13; // ultrasonic send=trigger pin
const int MotorRight1 = 7; // pin 7 to IN1 MotorRight1 and MotorRight2 control direction of motor (same goes for MotorLeft1 & 2)
const int MotorRight2 = 8; // pin 8 to IN2
const int MotorLeft1 = 9; // pin 9 to IN3
const int MotorLeft2 = 10; // pin 10 to IN4
const int MotorRightPWM = 6; // pin 6 to ENA
const int MotorLeftPWM = 11; // pin 11 to ENB
int iSpeed = 255; // speed, range 0 to 255. You can adjust speed here

void setup() {
myservo.attach(2); //pin 2 = servo
pinMode(7,OUTPUT);//Rmotor1 // on motorshield IN1
pinMode(8,OUTPUT);//Rmotor2
pinMode(9,OUTPUT);//Lmotor1
pinMode(10,OUTPUT);//Lmotor2
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(6,OUTPUT);//Rmotor on/off PWM
pinMode(11,OUTPUT);//Lmotor on/off PWM
}
//look(1) lets robot 'look' straight, left and right while moving forward. The reason is that if the robot encounters an object in an angle, a static straight (servo on 98) ping won't register the obstacle proparly
void loop() {
myservo.write(40);look1(); delay(200); myservo.write(98);look1();delay(200);myservo.write(150);look1();delay(200); //to avoid the robot is only looking straight ahead
analogWrite(MotorRightPWM, iSpeed); // turn Rmotor on
analogWrite(MotorLeftPWM, iSpeed); // turn Lmotor on
digitalWrite(MotorRight1, HIGH);//forward
digitalWrite(MotorRight2, LOW);// forward
digitalWrite(MotorLeft1, LOW);//forward
digitalWrite(MotorLeft2, HIGH);//forward
}

void look1() {
long duration, distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;

if (distance <= 15 ){
look();}
else { analogWrite(MotorRightPWM, iSpeed); // if distance >15 cm, leave motor on
analogWrite(MotorLeftPWM, iSpeed); // leave motor on
digitalWrite(MotorRight1, HIGH);
digitalWrite(MotorRight2, LOW);
digitalWrite(MotorLeft1, LOW);
digitalWrite(MotorLeft2, HIGH);}
}

// looking if there are obstacles in 150, or 40 degrees direction
void look() {
myservo.write(40);
long durationR, distanceR;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
durationR = pulseIn(echoPin, HIGH);
distanceR = (durationR/2) / 29.1;
delay(200);

myservo.write(150);
long durationL, distanceL;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
durationL = pulseIn(echoPin, HIGH);
distanceL = (durationL/2) / 29.1;
delay(200);myservo.write(98); // returns servo to 'front looking modus'

// now determine where to go
if (distanceR <= distanceL ){
digitalWrite(MotorLeft1, HIGH); // turn left
digitalWrite(MotorLeft2, LOW);
digitalWrite(MotorRight1, HIGH);
digitalWrite(MotorRight2, LOW);
analogWrite(MotorRightPWM, iSpeed);
analogWrite(MotorLeftPWM, iSpeed);

delay(500);// this delay is on a 90rpm motor
}
else {
digitalWrite(MotorLeft1, LOW); // turn right
digitalWrite(MotorLeft2, HIGH);
digitalWrite(MotorRight1, LOW);
digitalWrite(MotorRight2, HIGH);
analogWrite(MotorRightPWM, iSpeed);
analogWrite(MotorLeftPWM, iSpeed);
myservo.write(98);
delay(500);// this delay is on a 90rpm motor
}

}

Advertisement

Gebruikers-avatar
Berichten: 5043
Geregistreerd: 13 Mei 2013, 20:57
Woonplaats: Heemskerk

Re: Obstacle Avoiding Robot, simpele sketch

Berichtdoor nicoverduin » 13 Apr 2016, 08:17

Leuk!!!
Docent HBO Technische Informatica, Embedded ontwikkelaar & elektronicus
http://www.verelec.nl

Berichten: 4064
Geregistreerd: 16 Okt 2013, 14:31
Woonplaats: s hertogenbosch

Re: Obstacle Avoiding Robot, simpele sketch

Berichtdoor shooter » 14 Apr 2016, 10:22

ja mooi gedaan, simpel en goed nagedacht wat te doen,
uiteraard wel een paar opmerkingen:
als je de servo laat draaien dan even wachten tot deze in positie is, dat duurt even dus je delays(pas na de servo zetten)
opdracht is dan ook om de delay te vervangen door een timer.
verder alleen iets veranderen in motorinstellingen als dat nodig is, en niet elke keer.
dan krijg je een karretje dat zeer snel reageert, ipv nu telkens een halve seconde wacht.
volgende opdracht zou zijn, langzaam optrekken en remmen.
paul deelen
shooter@home.nl

Terug naar Afgeronde projecten

Wie is er online?

Gebruikers in dit forum: Geen geregistreerde gebruikers en 7 gasten