Hi,
I am a beginner to robot programming and I wrote the following code for my obstacle avoidance robot.
#define trig 3
#define echo 2
#define thresholddistance 20.0
long duration;
float forwarddistance;
float distance[7];
#include <Servo.h>
#define pinServo 4
//----------------------------------------------------- Setting up Servo Angles
int servoArray[7] = {0, 30, 60, 90, 120, 150, 180};
Servo myServo;
//----------------------------------------------------- Motors
int motor_left[] = {9, 6};
int motor_right[] = {11, 10};
//----------------------------------------------------- Setup Module
void setup() {
Serial.begin(9600);
myServo.attach(pinServo);
//Setup Motors
for (int i = 0; i < 2; i++)
{
pinMode(motor_left[i], OUTPUT);
pinMode(motor_right[i], OUTPUT);
}
//Setup Sensor
pinMode(trig, OUTPUT);
pinMode(echo, INPUT);
}
//----------------------------------------------------- Motor Module
// Following Module is for the Robot to brake
void brake()
{
digitalWrite(motor_left[0], LOW);
digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], LOW);
digitalWrite(motor_right[1], LOW);
}
// Following Module is for the Robot to go forward
void drive_forward()
{
digitalWrite(motor_left[0], HIGH);
digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], LOW);
digitalWrite(motor_right[1], HIGH);
}
// Following Module is for the Robot to slightly go towards left
void slight_left()
{
digitalWrite(motor_left[0], HIGH);
digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], LOW);
digitalWrite(motor_right[1], LOW);
delay(300);
}
// Following Module is for the Robot to slightly go towards right
void slight_right()
{
digitalWrite(motor_left[0], LOW);
digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], LOW);
digitalWrite(motor_right[1], HIGH);
delay(300);
}
// Following Module is for the Robot to turn left
void turn_left()
{
digitalWrite(motor_left[0], HIGH);
digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], LOW);
digitalWrite(motor_right[1], LOW);
delay(600);
}
// Following Module is for the Robot to turn right
void turn_right()
{
digitalWrite(motor_left[0], LOW);
digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], LOW);
digitalWrite(motor_right[1], HIGH);
delay(600);
}
// Following Module is to make a uturn if there are obstacles to the right, to the left and in front
void turn_around()
{
digitalWrite(motor_left[0], LOW);
digitalWrite(motor_left[1], HIGH);
digitalWrite(motor_right[0], HIGH);
digitalWrite(motor_right[1], LOW);
delay(600);
}
//----------------------------------------------------- Sensor Module
float distance_front() {
myServo.write(90);
digitalWrite(trig, HIGH);
delayMicroseconds(10);
digitalWrite(trig, LOW);
duration = pulseIn(echo, HIGH);
forwarddistance = duration / 58;
delay(100);
return forwarddistance;
}
//----------------------------------------------------- Sensor Array Module
float sensor_array() {
for (int i = 0; i < 7; i++) {
myServo.write(servoArray[i]);
digitalWrite(trig, HIGH);
delayMicroseconds(10);
digitalWrite(trig, LOW);
duration = pulseIn(echo, HIGH);
distance[i] = duration / 58;
delay(100);
return distance[i];
}
}
//----------------------------------------------------- Loop Module
void loop() {
distance_front();
if (forwarddistance > thresholddistance)
{
drive_forward();
}
else {
brake();
sensor_array();
if (distance[2] > thresholddistance && distance[1] > thresholddistance)
{
slight_left();
}
else if (distance[1] > thresholddistance && distance[0] > thresholddistance)
{
turn_left();
}
else if (distance[4] > thresholddistance && distance[5] > thresholddistance)
{
slight_right();
}
else if (distance[5] > thresholddistance && distance[6] > thresholddistance)
{
turn_right();
}
else
{
turn_around();
brake();
slight_left();
}
}
delay(100);
}
Basically, I want it to get input from the sensor and to decide which path to choose based on the loop, but it only performs the turn_around(); -> brake(); -> slight_left(); part of the loop.
I would appreciate it if you could tell me if I am calling the functions properly or if there is anything I need to fix.
Thanks.