Arduino Sketch
#define m1p 7
#define m1n 6
#define m2p 5
#define m2n 4
#define echopin 9
#define trigpin 10
void motor_forward();
void motor_stop();
void motor_left();
void motor_right();
void motor_back();
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
pinMode(m1p, OUTPUT);
pinMode(m1n, OUTPUT);
pinMode(m2p, OUTPUT);
pinMode(m2n, OUTPUT);
pinMode(echopin, INPUT);
pinMode(trigpin, OUTPUT);
}
long duration, distance;
void loop() {
// put your main code here, to run repeatedly:
digitalWrite(trigpin, LOW);
delayMicroseconds(2);
digitalWrite(trigpin, HIGH);
delayMicroseconds(10);
digitalWrite(trigpin, LOW);
duration = pulseIn(echopin, HIGH);
distance = (duration *0.0343 / 2);
Serial.println(distance);
// Serial.println(" cm");
//delay(1000);
//Serial.println(distance); //print data serially
if (distance > 25) { // if distance is more than 25 move bot forward
motor_forward();
delay(20);
motor_forward();
}
else {
// motor_stop();
//delay(500);
// motor_back();
// delay(3000);
motor_stop(); // if distance is less than 25 move bot right
delay(50);
motor_back();
delay(100);
motor_right();
delay(100);
}
}
void motor_back(){
digitalWrite(m1p, LOW);
digitalWrite(m1n, HIGH);
digitalWrite(m2p, HIGH);
digitalWrite(m2n, LOW);
}
void motor_forward(){
digitalWrite(m1p, HIGH);
digitalWrite(m1n, LOW);
digitalWrite(m2p, LOW);
digitalWrite(m2n, HIGH);
}
void motor_stop(){
digitalWrite(m1p, LOW);
digitalWrite(m1n, LOW);
digitalWrite(m2p, LOW);
digitalWrite(m2n, LOW);
}
void motor_left(){
digitalWrite(m1p, HIGH);
digitalWrite(m1n, LOW);
digitalWrite(m2p, LOW);
digitalWrite(m2n, LOW);
}
void motor_right(){
digitalWrite(m1p, LOW);
digitalWrite(m1n, LOW);
digitalWrite(m2p, HIGH);
digitalWrite(m2n, LOW);
}Code Explanation
The code is a simple obstacle avoidance program for a robot using an ultrasonic sensor and two motors. Let’s break down the code and explain each section:
#define m1p 7
#define m1n 6
#define m2p 5
#define m2n 4
#define echopin 9
#define trigpin 10In this section, the code defines constants for motor pins (m1p, m1n, m2p, m2n) and pins for the ultrasonic sensor (echopin for echo and trigpin for trigger).
void motor_forward();
void motor_stop();
void motor_left();
void motor_right();
void motor_back();Here, the code declares five functions (motor_forward, motor_stop, motor_left, motor_right, motor_back) that will be used to control the movement of the robot.
void setup() {
Serial.begin(9600);
pinMode(m1p, OUTPUT);
pinMode(m1n, OUTPUT);
pinMode(m2p, OUTPUT);
pinMode(m2n, OUTPUT);
pinMode(echopin, INPUT);
pinMode(trigpin, OUTPUT);
}In the setup function, the serial communication is initialized, and the pin modes for motor control and the ultrasonic sensor are set.
long duration, distance;Here, two variables duration and distance are declared to store the duration of the ultrasonic pulse and the calculated distance, respectively.
void loop() {
digitalWrite(trigpin, LOW);
delayMicroseconds(2);
digitalWrite(trigpin, HIGH);
delayMicroseconds(10);
digitalWrite(trigpin, LOW);
duration = pulseIn(echopin, HIGH);
distance = (duration * 0.0343 / 2);
Serial.println(distance);
if (distance > 25) {
motor_forward();
delay(20);
motor_forward();
} else {
motor_stop();
delay(50);
motor_back();
delay(100);
motor_right();
delay(100);
}
}In the loop function, the ultrasonic sensor is triggered to measure the distance. If the measured distance is greater than 25, the robot moves forward. Otherwise, it stops, moves back a bit, and then turns right.
void motor_back() {
digitalWrite(m1p, LOW);
digitalWrite(m1n, HIGH);
digitalWrite(m2p, HIGH);
digitalWrite(m2n, LOW);
}
void motor_forward() {
digitalWrite(m1p, HIGH);
digitalWrite(m1n, LOW);
digitalWrite(m2p, LOW);
digitalWrite(m2n, HIGH);
}
void motor_stop() {
digitalWrite(m1p, LOW);
digitalWrite(m1n, LOW);
digitalWrite(m2p, LOW);
digitalWrite(m2n, LOW);
}
void motor_left() {
digitalWrite(m1p, HIGH);
digitalWrite(m1n, LOW);
digitalWrite(m2p, LOW);
digitalWrite(m2n, LOW);
}
void motor_right() {
digitalWrite(m1p, LOW);
digitalWrite(m1n, LOW);
digitalWrite(m2p, HIGH);
digitalWrite(m2n, LOW);
}These functions define the motor control logic for moving the robot in different directions. For example, motor_forward makes the robot move forward by setting the appropriate motor pins.
In summary, this code implements a basic obstacle avoidance mechanism for a robot using an ultrasonic sensor. If the robot detects an obstacle within 25 cm, it stops, moves back a bit, and then turns right. Otherwise, it continues moving forward. The motor control is achieved through the defined functions that set the appropriate pin states for the motor driver.
