Click here and download App
CODE
/*Obstacle avoidance and Bluetooth control robot
*/
#include <Servo.h>
#include <AFMotor.h>
#define Speed 220
#define Echo A5
#define Trig A4
#define Spoint 135
#define MinPoint 80
#define MaxPoint 180
Servo servo;
AF_DCMotor motor1(3);
AF_DCMotor motor2(4);
void setup() {
Serial.begin(9600);
servo.attach(10);
servo.write(Spoint);
pinMode(Trig, OUTPUT);
pinMode(Echo, INPUT);
motor1.setSpeed(Speed);
motor2.setSpeed(Speed);
delay(2000);
}
void loop() {
obstacle ();
//bluetooth();
}
void bluetooth() {
if (Serial.available() > 0) {
char value = Serial.read();
Serial.println(value);
if (value == 'F') {
forward();
} else if (value == 'B') {
backward();
} else if (value == 'L') {
left();
} else if (value == 'R') {
right();
} else if (value == 'S') {
Stop();
}
}
}
void obstacle () {
int Distance = distance();
if (Distance < 10) {
Stop();
Serial.println("Stop");
delay(500);
servoLeft();
int Left = distance();
Serial.println(Left);
servoRight();
int Right = distance();
Serial.println(Right);
if (Left < Right) {
right();
Serial.println("Right");
delay(1000);
} else {
left();
Serial.println("Left");
delay(1000);
}
} else {
forward();
Serial.println("Forward");
}
}
void forward() {
motor1.run(FORWARD);
motor2.run(FORWARD);
}
void backward() {
motor1.run(BACKWARD);
motor2.run(BACKWARD);
}
void left() {
motor1.run(BACKWARD);
motor2.run(FORWARD);
}
void right() {
motor1.run(FORWARD);
motor2.run(BACKWARD);
}
void Stop() {
motor1.run(RELEASE);
motor2.run(RELEASE);
}
int distance() {
//pulse output
digitalWrite(Trig, LOW);
delayMicroseconds(4);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
long t = pulseIn(Echo, HIGH);
long cm = t / 29 / 2;
return cm;
}
void servoLeft() {
for (int a = Spoint; a < MaxPoint; a++) {
servo.write(a);
delay(20);
}
for (int b = MaxPoint; Spoint < b; b--) {
servo.write(b);
delay(20);
}
}
void servoRight() {
for (int a = Spoint; a > MinPoint; a--) {
servo.write(a);
delay(20);
}
for (int b = MinPoint; Spoint > b; b++) {
servo.write(b);
delay(20);
}
}