I started a Robotics Club recently at the Institute I’m currently working at in which I’ve been slowly getting my beginner students to become acquianted with Arduino-based robots and Arduino coding.
We purchased a set of Lafvin 2WD Robot Kits (see image above) from Little Bird Electronics and step-by-step I’ve been getting my students to learn how to control each piece of hardware attached to the robot. We’re working towards getting the robot to move around a room without bumping into anything it might encounter along the way.
In this post I thought I’d share some code I’ve written for this club that uses an ultrasonic sensor mounted on a servo motor to detect the distance of obstacles from the robot.
The code gets the rotor to sweep 180 degrees in one direction and then back again. It stops, however, every 45 degrees to use the ultrasonic sensor to detect if there is an object within 30cm (1 foot) of the machine. If there is, a message is printed on the Serial Monitor.
Key equipment used is as follows:
- Arduino Uno R3
- Arduino Sensor Expansion Shield V5.0 (attached to Uno)
- SG90 Micro Servo motor
- Ultrasonic Sensor (HC-SR04)
This video shows how we have put together the pieces and connections. The relevant connection diagrams are as follows:
The code below is well-commented so there’s no need for me to explain any more. But please note that you will need to install the “Servo” library before running this code.
If you have any questions, post them below and I’ll get back to you as soon as I can.
/*
This code uses an Ultrasonic (US) sensor mounted on a servo motor to detect obstacles
for a moving robot. The rotor sweeps 180 degrees from left to right and then
back again. It takes readings every 45 degrees (and 200 milliseconds). If there is
an object within 30cm, it prints a message to the Serial monitor.
Once the obstacle is cleared, the rotor will start sweeping again.
My code works for an Arduino Uno R3 board and a Sensor ExapansionShield v5.0.
The rotor and sensor are connected to pins A0-A2 of the Power Shield.
I have also put in skeleton code (via "sweep" variable) to indicate where one might
write code to manoeuvre a robot if an obstacle is encountered.
*/
#include <Servo.h> // Servo library
#include "SR04.h" // Ultrasonic (US) sensor library
#define TRIG_PIN A1 // UltraSonic I/O
#define ECHO_PIN A0 // UltraSonic I/O
// Create Ultrasonic object
SR04 sr04 = SR04(ECHO_PIN,TRIG_PIN);
int obj_distance; // variable to store distance returned by US sensor
int distance_threshold = 30; // how close to get to an object before deciding to act
// Create a servo object
Servo myServo;
int sweep_directions[] = {0, 45, 90, 135, 180}; // possible rotor positions (degrees)
int direction_index = 0; // current index of rotor position
int direction = 1; // Direction of rotor: 1 for forward (sweep right), -1 for backward (sweep left)
// Time tracking variables to only move the rotor between certain elapsed time intervals
unsigned long previousMillis = 0;
const unsigned int interval = 200; // interval in milliseconds between US readings
// this boolean is not used in this code but I left it here to give you an idea of how
// you could build an obstacle avoiding robot in the future
bool sweep = true;
void setup() {
// Start the serial monitor
Serial.begin(9600);
// Connect servo motor to shield
myServo.attach(A2);
}
void loop() {
// Get the current time
unsigned long currentMillis = millis();
// Check if the interval has passed from last reading
if (currentMillis - previousMillis >= interval) {
obj_distance = sr04.Distance(); // get obstacle distance
if(obj_distance <= distance_threshold) {
// obstacle detected within distance threshold
Serial.print("STOP! Obstacle distance: ");
Serial.println(obj_distance);
/*
You would write code here to stop the robot from moving,
set sweep to false to give the robot time to work out
which direction to turn and then to manoeuvre.
Once, we're good to go again, set sweep to true.
*/
}
else if(sweep) {
// If we're in "sweep" state, we change rotor direction and
// wait another 200 milliseconds from the last reading
myServo.write(sweep_directions[direction_index]);
Serial.print("Sweep direction:");
Serial.println(sweep_directions[direction_index]);
// Check if the end or beginning of the array is reached
if (direction_index == 4) {
direction = -1; // Reverse direction of sweeping at the end
} else if (direction_index == 0) {
direction = 1; // Reverse direction at the beginning
}
direction_index += direction; // increment or decrement index accordingly
}
previousMillis = currentMillis;
}
}
To be informed when new content like this is posted, subscribe to the mailing list:
(Note: If this post is found on a site other than zbigatron.com, a bot has stolen it – it’s been happening a lot lately)
