| #include <Servo.h> //includes the servo library
int motor_pin1 = 4;
int motor_pin2 = 5;
int motor_pin3 = 6;
int motor_pin4 = 7;
int servopin = 8;
int sensorpin = 0;
int dist = 0;
int leftdist = 0;
int rightdist = 0;
int object = 500; //distance à laquelle le robot doit chercher une autre route
Servo myservo;
void setup ()
{
pinMode(motor_pin1,OUTPUT);
pinMode(motor_pin2,OUTPUT);
pinMode(motor_pin3,OUTPUT);
pinMode(motor_pin4,OUTPUT);
myservo.attach(servopin);
myservo.write(90);
delay(700);
}
void loop()
{
dist = analogRead(sensorpin); //reads the sensor
if(dist < object) { //si la distance est inférieure à 550
forward(); //then move forward
}
if(dist >= object) { //si la distance est supérieure ou égale à 550
findroute();
}
}
void forward() { // use combination which works for you
digitalWrite(motor_pin1,HIGH);
digitalWrite(motor_pin2,LOW);
digitalWrite(motor_pin3,HIGH);
digitalWrite(motor_pin4,LOW);
return;
}
void findroute() {
halt(); // stop
backward(); //go backwards
lookleft(); //go to subroutine lookleft
lookright(); //go to subroutine lookright
if ( leftdist < rightdist )
{
turnleft();
}
else
{
turnright ();
}
}
void backward() {
digitalWrite(motor_pin1,LOW);
digitalWrite(motor_pin2,HIGH);
digitalWrite(motor_pin3,LOW);
digitalWrite(motor_pin4,HIGH);
delay(500);
halt();
return;
}
void halt () {
digitalWrite(motor_pin1,LOW);
digitalWrite(motor_pin2,LOW);
digitalWrite(motor_pin3,LOW);
digitalWrite(motor_pin4,LOW);
delay(500); //wait after stopping
return;
}
void lookleft() {
myservo.write(150);
delay(700); //wait for the servo to get there
leftdist = analogRead(sensorpin);
myservo.write(90);
delay(700); //wait for the servo to get there
return;
}
void lookright () {
myservo.write(30);
delay(700); //wait for the servo to get there
rightdist = analogRead(sensorpin);
myservo.write(90);
delay(700); //wait for the servo to get there
return;
}
void turnleft () {
digitalWrite(motor_pin1,HIGH); //use the combination which works for you
digitalWrite(motor_pin2,LOW); //right motor rotates forward and left motor backward
digitalWrite(motor_pin3,LOW);
digitalWrite(motor_pin4,HIGH);
delay(1000); // wait for the robot to make the turn
halt();
return;
}
void turnright () {
digitalWrite(motor_pin1,LOW); //use the combination which works for you
digitalWrite(motor_pin2,HIGH); //left motor rotates forward and right motor backward
digitalWrite(motor_pin3,HIGH);
digitalWrite(motor_pin4,LOW);
delay(1000); // wait for the robot to make the turn
halt();
return; } |
