UNIVERSITY OF TECHNOLOGY

CONRTOL AND SYSTEM ENGINEERING

DEPARTMENT

MECHATRONICS BRANCH

4 th Stage

EXP.(1)

Servo motor position control using distance

measurements

‫علي تحسين كمال‬

‫ميكا صباحي‬

Objective:
Autonomous robots usually to use distance sensors to learn their
surroundings, and in order to avoid any collision with all obstacles
placed in robot work space during navigation. We need to control the
position of two servo motors for example proportionally with respect to
the measure distance.

Procedure :

#include <Servo.h>

const int pingPin = 4;

const int servoPin1 = 2;

const int servoPin2 = 3;

int Thl;

int Th2;

Servo ServoMotor1;

Servo ServoMotor2;

void setup()

Serial.begin (9600);

ServoMotor1.attach(servoPin1);

ServoMotor2.attach(servoPin2);

void loop()

long duration, cm ;

pinMode(pingPin, OUTPUT);

digitalWrite(pingPin, LOW);

delayMicroseconds (2);

digitalWrite (pingPin, HIGH);

delayMicroseconds (5);

digitalWrite (pingPin, LOW);

pinMode(pingPin, INPUT);

duration = pulseIn(pingPin, HIGH);

// convert the time into a distance

cm = microsecondsToCentimeters(duration);

Thl=cm*180/320;

Th2=180-(cm*180/320);
ServoMotor1.write(Thl);

ServoMotor2.write(Th2);

delay (100);

Serial.print(" cm = ");

Serial.print (cm);

Serial.print (" Th1 = ");

Serial.print (Thl);

Serial.print (" Th2 = ");

Serial.print (Th2);

Serial.println();

long microsecondsToCentimeters(long microseconds) {

return microseconds / 29 / 2;}

Servo 1 Angle Servo 2 Angle

Distance (cm) 0 - 180 180 - 0

10 5 175

50 28 152

100 56 124

150 84 96

200 112 68

250 140 40

300 168 12

350 180 0

Discussion :
1-Simulate an Arduino UNO circuit and write an Arduino code for
Controlling a two servo motors in an opposite directions using Ping
Ultra sonic distance sensor, so that servo one moves from (180to0)
degrees while servo two moves from (0to180)degrees.

ANS /
#include <Servo.h>
const int pingPin = 4;
const int servoPinl = 2;
const int servoPin2 = 3;
int Th1;
int Th2;
Servo ServoMotor1;
Servo ServoMotor2;
long microsecondsToCentimeters(long microseconds) {
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the object we
// take half of the distance travelled.
return microseconds / 29 / 2;
}
void setup ()
{
Serial.begin (9600);
ServoMotor1.attach(servoPinl);
ServoMotor2.attach(servoPin2);
}
void loop()
{
long duration, cm ;
pinMode (pingPin, OUTPUT);
digitalWrite (pingPin, LOW);
delayMicroseconds (2);
digitalWrite (pingPin, HIGH);
delayMicroseconds (5);
digitalWrite (pingPin, LOW);
pinMode (pingPin, INPUT);
duration = pulseIn (pingPin, HIGH);
// convert the time into a distance
cm = microsecondsToCentimeters(duration);

Th1= 180-(cm*180/320);
Th2=cm*180/320;

ServoMotor1.write (Th1);
ServoMotor2.write (Th2);
delay (100);
Serial.print ("cm = ");
Serial.print (cm);
Serial.print (" Th1 = ");
Serial.print (Th1);
Serial.print (" Th2 = ");
Serial.print (Th2);
Serial.println ();
}

Servo 1 Angle Servo 2 Angle

Distance (cm) 180-0 0-180
 10 175 5

50 152 28

100 124 56

150 96 84

200 68 112

250 40 140

300 12 168

350 0 180

2- Simulate an Arduino UNO circuit and write Arduino code for

Controlling a two servo motors in an opposite directions using Ping
ultrasonic distance sensor, so that servo one moves from
(180to90)degrees while servo two moves from (90to0)degrees.

ANS /
#include <Servo.h>
const int pingPin = 4;
const int servoPinl = 2;
const int servoPin2 = 3;
int Thl;
int Th2;
Servo ServoMotor1;
Servo ServoMotor2;
long microsecondsToCentimeters(long microseconds) {
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the object we
// take half of the distance travelled.
return microseconds / 29 / 2;
}
void setup ()
{
Serial.begin (9600);
ServoMotor1.attach(servoPinl);
ServoMotor2.attach(servoPin2);
}
void loop()
{
long duration, cm ;
pinMode (pingPin, OUTPUT);
digitalWrite (pingPin, LOW);
delayMicroseconds (2);
digitalWrite (pingPin, HIGH);
delayMicroseconds (5);
digitalWrite (pingPin, LOW);
pinMode (pingPin, INPUT);
duration = pulseIn (pingPin, HIGH);
// convert the time into a distance
cm = microsecondsToCentimeters(duration);
Th1=180-(cm*90/320);
Th2=90-(cm*90/320);
ServoMotor1.write (Thl);

ServoMotor2.write (Th2);
delay (100);
Serial.print ("cm = ");
Serial.print (cm);
Serial.print (" Th1 = ");
Serial.print (Thl);
Serial.print (" Th2 = ");
Serial.print (Th2);
Serial.println ();
}

Servo 1 Angle Servo 2 Angle

Distance (cm) 180 - 90 90 - 0

10 178 88

50 166 76

100 152 62

150 138 48

200 134 34

250 110 20

300 96 6

350 90 0