LAPORAN PRAKTIKUM

MK 4407 Robotika

Kinematika Diffrensial Wheels Robot

JEKI ARMENTARIA
4211531030
Program Studi: Teknik Mekatronika

JURUSAN TEKNIK ELEKTRO

POLITEKNIK NEGERI
BATAM 2017
 4 KINEMATIKA DIFFERENTIAL

WHEELS ROBOT

Bab ini membahas tentang cara menggunakan rotary encoder

sebagai sensor untuk mengetahui kecepatan roda dan mengatur
kecepatan roda menggunakan kontroler PID.

Tujuan

Memahami perhitungan trayektori differential wheel robot.

Memahami kinematika differential wheel mobile robot.

Pokok Bahasan
Differential Wheels Robot

Perhitungan Trayektori

Eksperimen 4-1: Perhitungan Trayektori

Differential Wheels Robot Forward Kinematics

Eksperimen 4-2: Differential Wheels Forward Kinematics

Differential Wheels Robot Inverse Kinematics

Eksperimen 4-3: Differential Wheels Inverse Kinematics Tugas

dan Pertanyaan

Peralatan Eksperimen
1 unit 2WD Nexus Robot
1 unit PC dengan software Arduino IDE
 4.Kinematika Differential Wheels Robot

A. Differential Wheels Robot

Differential wheel robot adalah mobile robot yang pergerakannya menggunakan dua motor
penggerak yang dipasang terpisah pada sisi kiri dan kanan robot. Gambar 4-1
memperlihatkan differential wheels mobile robot. Untuk bergerak maju, maka roda kiri dan
kanan sama-samu maju ke depan. Untuk belok ke kiri, maka roda kiri mundur, roda kanan
maju, demikian seterusnya.

Gambar 4-1 Differential Wheels Mobile Robot

B. Perhitungan Trayektori
Untuk menghitung trayektori (lintasan) robot saat ini, maka dapat dilakukan dengan cara
membaca secara terus menerus nilai rotary encoder yang dipasang pada kedua roda.
Gambar 4-2 memperlihatkan jarak tempuh yang dilalui differential wheels robot.

Belajar Robotika, Hak Cipta: Hendawan Soebhakti, 2017 Hal. | 2

 Gambar 4-2 Jarak Tempuh Differential Wheels Robot

Dimana:

: radius roda
: jarak antar kedua roda
: jumlah pulsa encoder untuk satu putaran penuh
: jumlah pulsa encoder selama pengukuran pada encoder kiri
: jumlah pulsa encoder selama pengukuran pada encoder kanan

Berikut adalah tahapan untuk menghitung trayektori differential wheel:

1. Hitung nilai dan dalam satuan meter yaitu jarak tempuh roda kiri dan kanan.
2. Membagi jumlah pulsa encoder yang terukur dengan jumlah pulsa encoder untuk
satu putaran penuh untuk mendapatkan jumlah putaran roda.
3. Mengalikan jumlah putaran roda dengan keliling roda, sehingga diperoleh jarak yang
telah ditempuh dalam satuan meter menggunakan persamaan

(4-1)

4. Maka jarak tempuh robot adalah:

( ) (4-2)

5. Jika radius belok (turning radius) robot adalah , maka:

(

(4-3)
)

6. Kedua persamaan dapat disubstraksi untuk menghilangkan , sehingga:

(4-4)

Belajar Robotika, Hak Cipta: Hendawan Soebhakti, 2017 Hal. | 3

 7. Akhirnya untuk menghitung :
( )
(4-5)

8. Kita akan mendapatkan velocity menggunakan kecepatan roda masing-masing dan

(4-6)

Gambar 4-3 menunjukkan rotasi robot terhadap jarak tempuh

Gambar 4-3 Rotasi Robot Terhadap Jarak Tempuh

Belajar Robotika, Hak Cipta: Hendawan Soebhakti, 2017 Hal. | 4

 Gambar 4-4 Pergerakan Robot Differential Wheels

C. Eksperimen 4-1: Perhitungan Trayektori

Pada eksperimen ini akan dibuat program untuk menghitung trayektori differential
wheels robot dengan tahapan sebagai berikut:

1. Buatlah sketch sebagai berikut :

#include <PinChangeInt.h>
#include <PinChangeIntConfig.h>

#define PIN1 4
#define PIN2 6

int encoder1PhaseA = 4;
int encoder1PhaseB = 5;
int encoder1Position = 0;

int encoder2PhaseA = 6;
int encoder2PhaseB = 7;
int encoder2Position =
0; int n,m = LOW;

float sL,sR,s;
float r=143/2; //wheel
radius float phi=3.14;
int tickPerRev=775;
float PHI;
float d=290;

void setup() {
pinMode (encoder1PhaseA,INPUT);
pinMode (encoder1PhaseB,INPUT);

Belajar Robotika, Hak Cipta: Hendawan Soebhakti, 2017 Hal. | 5

 pinMode (encoder2PhaseA,INPUT);
pinMode (encoder2PhaseB,INPUT);
PCintPort::attachInterrupt(PIN1, readEncoder1,RISING);
PCintPort::attachInterrupt(PIN2, readEncoder2,RISING);
Serial.begin(9600);
}

void loop(){
sL=(2*phi*r*encoder1Position)/tickPerRev;
sR=-(2*phi*r*encoder2Position)/tickPerRev;
s=(sL+sR)/2;
PHI=(sL-sR)/d;
PHI=(180/3.14)*PHI;
Serial.print ("Enc1="); Serial.print
(encoder1Position); Serial.print ("\t");
Serial.print ("Enc2="); Serial.print
(encoder2Position); Serial.print ("\t");
Serial.print ("sL="); Serial.print (sL); Serial.print ("
"); Serial.print ("mm");
Serial.print ("\t");
Serial.print ("sR="); Serial.print (sR); Serial.print ("
"); Serial.print ("mm");
Serial.print ("\t");
Serial.print ("s="); Serial.print (s); Serial.print ("
"); Serial.print ("mm");
Serial.print ("\t");
Serial.print ("PHI="); Serial.print (PHI); Serial.print ("
"); Serial.print ("deg");
Serial.println ("\t");
}

void readEncoder1()
{
n = digitalRead(encoder1PhaseA);
if (n == HIGH) {
if (digitalRead(encoder1PhaseB) == LOW)
{ encoder1Position--;
} else {
encoder1Position++;
}
}
}

void readEncoder2()
{
m = digitalRead(encoder2PhaseA);
if (m == HIGH) {
if (digitalRead(encoder2PhaseB) == LOW)
{ encoder2Position--;
} else {
encoder2Position++;
}
}

}
 2. Upload kemudian buka serial monitor.
3. Jalankan robot maju sejauh 10 cm. Perhatikan data yang muncul pada serial
monitor. Masukkan pada Tabel 4-1.
4. Tutup serial monitor, kemudian buka kembali. Putar robot kekanan 90 derajat.
Perhatikan data pada serial monitor. Masukkan pada Tabel 4-1.

Tabel 4-1 Robot Trajectory

Enc1 Enc2 sL sR S PHI

No Aksi Robot
(pulse) (pulse) (mm) (mm) (mm) (Deg)
1 Maju 10 cm 187 178 107.59 102.41 105 1.02
2 Kanan 90 derajat 397 354 228.41 203.67 12.37 85.41

D. Differential Wheels Robot Forward Kinematics

Forward kinematic pada differential wheels robot
]
[] [ ][

(4-6)
Dimana:

: Kecepatan linier (meter/detik)
: Kecepatan rotasi (derajat/detik)

: Radius roda (meter)

: Jarak antara kedua roda (meter)
: Kecepatan roda kiri (putaran per detik)

: Kecepatan roda kanan (putaran per detik)

Tabel 4-2 Hasil Simulasi Persamaan Forward Kinematics

Kecepatan Kecepatan Kecepatan Kecepatan

No linier, rotasi,
Roda Kiri, Roda Kanan,

(rps) (rps) (m/s) (deg/s)

1 -2 -2 -1.25 0.00
2 -2 -1 -0.94 3.14
3 -2 0 -0.62 6.28
4 -1 -2 -0.94 -3.14
5 -1 -1 -0.62 0.00
6 -1 0 -0.31 3.14
7 0 -2 -0.62 -6.28
8 0 -1 -0.31 -3.14
9 0 0 0.00 0.00
10 0 1 0.31 3.14

Belajar Robotika, Hak Cipta: Hendawan Soebhakti, 2017 Hal. | 7

 Kecepatan Kecepatan Kecepatan Kecepatan
No Roda Kiri, Roda Kanan,
linier, rotasi,
(rps) (rps) (m/s) (deg/s)
11 0 2 0.62 6.28
12 1 0 0.31 -3.14
13 1 1 0.62 6.28
14 1 2 0.94 3.14
15 2 0 0.62 -6.28
16 2 1 0.94 -3.14
17 2 2 1.25 0.00

E. Eksperimen 4-2: Differential Wheels Forward Kinematics

Pada eksperimen ini akan dibuat program untuk menghitung forward kinematic
differential wheels robot dengan tahapan sebagai berikut:

1. Buatlah sketch sebagai berikut :

#include <PinChangeInt.h>
#include <PinChangeIntConfig.h>

#define PIN1 4
#define PIN2 6

int encoder1PhaseA = 4;
int encoder1PhaseB = 5;
int encoder1Position = 0;

int encoder2PhaseA = 6;
int encoder2PhaseB = 7;
int encoder2Position =
0; int n,m = LOW;

float sL,sR,s;
float r=143/2; //wheel
radius float phi=3.14;
int tickPerRev=775;
float PHI;
float d=290;

int timer2_counter;
int pulse1, pulse2;
float rpm1, rpm2;
float rps1, rps2;

float v,w;
void setup() {
//Setup Timer2 to fire every 1.6ms
TCCR2B = 0x00; //Disable Timer2 while we set it
up TCNT2 = 6; //Reset Timer Count to 6 out of 255
TIFR2 = 0x00; //Timer2 INT Flag Reg: Clear Timer Overflow Flag
TIMSK2 = 0x01; //Timer2 INT Reg: Timer2 Overflow Interrupt Enable

Belajar Robotika, Hak Cipta: Hendawan Soebhakti, 2017 Hal. | 8

 TCCR2A = 0x00; //Timer2 Control Reg A: Wave Gen Mode
normal
TCCR2B = 0x07; // N=1024
pinMode (encoder1PhaseA,INPUT);
pinMode (encoder1PhaseB,INPUT);
pinMode (encoder2PhaseA,INPUT);
pinMode (encoder2PhaseB,INPUT);
PCintPort::attachInterrupt(PIN1, readEncoder1,RISING);
PCintPort::attachInterrupt(PIN2, readEncoder2,RISING);
Serial.begin(19200);
}

void loop(){
v = 2*phi*r*((rps1/2)+(rps2/2)); w
= 2*phi*r*(-(rps1/d)+(rps2/d));
Serial.print (rps1); Serial.print ("
"); Serial.print (rps2); Serial.print
(" "); Serial.print (v); Serial.print
(" "); Serial.println (w);
}

void readEncoder1()
{
n = digitalRead(encoder1PhaseA);
if (n == HIGH) {
if (digitalRead(encoder1PhaseB) ==
LOW) { encoder1Position--;
pulse1--;
} else {
encoder1Position++
; pulse1++;
}
}
}

void readEncoder2()
{
m = digitalRead(encoder2PhaseA);
if (m == HIGH) {
if (digitalRead(encoder2PhaseB) ==
LOW) { encoder2Position--;
pulse2--;
} else {
encoder2Position++
;
pulse2++;
}
}
}

ISR(TIMER2_OVF_vect) // interrupt service routine

{
TCNT2 = 6; // 0.016s;
rps1 = (pulse1* (1 / 0.016))/775;
rps2 = -(pulse2* (1 / 0.016))/775;

Belajar Robotika, Hak Cipta: Hendawan Soebhakti, 2017 Hal. | 9

 rpm1 = 60*rps1;
rpm2 = 60*rps2;
pulse1=0;
pulse2=0;
}
2. Upload kemudian buka serial monitor.
3. Jalankan robot maju kedepan, mundur, pivot kanan dan pivot kiri. Perhatikan angka
pada serial monitor. Lengkapi Tabel 4-3.

Tabel 4-3 Forward Kinematic

No Aksi Robot Kecepatan Kecepatan Kecepatan Kecepatan
roda kiri, rps1 roda kanan, Linier, V Rotasi,
(rps) rps2 (mm/s) (deg/s)
(rps)
1 Maju 0.16 0.16 71.92 0
2 Mundur -0.32 -0.32 143.83 0
3 Pivot kanan 0.16 -0.16 0 0.5
4 Pivot kiri -0.16 0.16 0 0.5

F. Differential Wheels Robot Inverse Kinematics

Inverse kinematic pada differential wheels robot
[
] [ ][ ]

(4-6)

Dimana:

: Kecepatan linier (meter/detik)
: Kecepatan rotasi (derajat/detik)

: Radius roda (meter)
: Jarak antara kedua roda (meter)

: Kecepatan roda kiri (putaran per detik)

: Kecepatan roda kanan (putaran per detik)

Tabel 4-4 Hasil Simulasi Persamaan Inverse Kinematics

Kecepatan Kecepatan Kecepatan Kecepatan

No linier, rotasi,
Roda Kiri, Roda Kanan,

(rps) (rps) (m/s) (deg/s)

1 0.0 0.0 0 0
2 0.1 0.0 0.15 0.15
3 0.2 0.0 0.31 0.31
4 0.3 0.0 0.47 0.47
5 0.0 10.0 -1.59 1.59
6 0.1 10.0 -1.43 1.75
7 0.2 10.0 -1.27 1.90
Belajar Robotika, Hak Cipta: Hendawan Soebhakti, 2017 Hal. | 10
 Kecepatan Kecepatan Kecepatan Kecepatan
No Roda Kiri, Roda Kanan,
linier, rotasi,
(rps) (rps) (m/s) (deg/s)
8 0.3 10.0 -1.11 2.06
9 0.0 20.0 -3.18 3.18
10 0.1 20.0 -3.02 3.34
11 0.2 20.0 -2.86 3.50
12 0.3 20.0 -2.70 3.66
13 0.0 -10.0 1.59 -1.59
14 0.1 -10.0 1.75 -1.43
15 0.2 -10.0 1.90 -1.27
16 0.3 -10.0 2.06 -1.11

G. Eksperimen 4-3: Differential Wheels Inverse Kinematics

Pada eksperimen ini akan dibuat program untuk menghitung inverse kinematic differential
wheels robot dengan tahapan sebagai berikut:

1. Buatlah sketch sebagai berikut :

#include <PinChangeInt.h>
#include <PinChangeIntConfig.h>

#define PIN1 4
#define PIN2 6
boolean M1=8; //DIR
int E1=9; //PWM
boolean M2=11; //DIR
int E2=10; //PWM

int encoder1PhaseA = 4;
int encoder1PhaseB = 5;
int encoder1Position = 0;

int encoder2PhaseA = 6;
int encoder2PhaseB = 7;
int encoder2Position =
0; int n,m = LOW;

float r=143/2; //wheel

radius float phi=3.14;
int tickPerRev=775;
float d=290;

int timer2_counter;
int pulse1, pulse2;
float rpm1, rpm2;
float rps1, rps2;

float v,w;
float thetaDotL, thetaDotR;
float x1,x2,y1,y2;

Belajar Robotika, Hak Cipta: Hendawan Soebhakti, 2017 Hal. | 11

 //PID 1
float Kp1=4; // osilasi di 15,
set setengahnya float Kd1=0.001;
float Ki1=0.1; // mengurangi
sse float dt=1;//0.016;
float setPoint1, PV1;
float outPID1, Sum1;
float minPID1 = -
2.42; float maxPID1 =
2.42; float error1 =
0; float error1_1 =0;

// PID 2
float Kp2=4; // osilasi di 15,
set setengahnya float Kd2=0.001;
float Ki2=0.1; // mengurangi
sse float setPoint2, PV2;
float outPID2, Sum2;
float minPID2 = -
2.42; float maxPID2 =
2.42; float error2 =
0; float error2_1 =0;

void setup() {
//Setup Timer1 as PWM output
TCCR1B=TCCR1B&0xf8|0x01; // Pin9,Pin10 PWM
31250Hz pinMode(M1, OUTPUT);
pinMode(E1, OUTPUT);
pinMode(M2, OUTPUT);
pinMode(E2, OUTPUT);

//Setup Timer2 to fire every 1.6ms

TCCR2B = 0x00; //Disable Timer2 while we set it up
TCNT2 = 6; //Reset Timer Count to 6 out of 255
TIFR2 = 0x00; //Timer2 INT Flag Reg: Clear Timer Overflow Flag
TIMSK2 = 0x01; //Timer2 INT Reg: Timer2 Overflow Interrupt
Enable TCCR2A = 0x00; //Timer2 Control Reg A: Wave Gen Mode
normal TCCR2B = 0x07; // N=1024
pinMode (encoder1PhaseA,INPUT);
pinMode (encoder1PhaseB,INPUT);
pinMode (encoder2PhaseA,INPUT);
pinMode (encoder2PhaseB,INPUT);
PCintPort::attachInterrupt(PIN1, readEncoder1,RISING);
PCintPort::attachInterrupt(PIN2, readEncoder2,RISING);
Serial.begin(19200);
}

void loop(){
v=100; //mm per second w=0;
//deg per second
w=w*(phi/180);
thetaDotL = (1/(2*phi*r))*(v-(w*d/2));
thetaDotR = (1/(2*phi*r))*(v+(w*d/2));

Belajar Robotika, Hak Cipta: Hendawan Soebhakti, 2017 Hal. | 12

 setPoint1 = thetaDotL;
setPoint2 = thetaDotR;

// PID 1 Calculation
PV1 = rps1;
error1 = setPoint1 - PV1;
Sum1 = Sum1 + error1;
outPID1 = Kp1*error1 + Ki1*Sum1*dt + ((Kd1/dt)*(error1
- error1_1));
if (outPID1 > maxPID1) { outPID1 =
maxPID1;} if (outPID1 < minPID1) { outPID1
= minPID1;} error1_1 = error1;

// PID 2 Calculation
PV2 = rps2;
error2 = setPoint2 - PV2;
Sum2 = Sum2 + error2;
outPID2 = Kp2*error2 +Ki2*Sum2*dt + ((Kd2/dt)*(error2
error2_1));
if (outPID2 > maxPID2) { outPID2 =
maxPID2;} if (outPID2 < minPID2) { outPID2
= minPID2;} error2_1 =error2;

x1=abs(outPID1);
x2=abs(outPID2); y1=(21.26*x1*x1*x1*x1*x1)-
(92.68*x1*x1*x1*x1)+(151.1*x1*x1*x1)-
(102.5*x1*x1)+(49.68*x1)+5.047; y2=(21.26*x2*x2*x2*x2*x2)-
(92.68*x2*x2*x2*x2)+(151.1*x2*x2*x2)-
(102.5*x2*x2)+(49.68*x2)+5.047;

if (outPID1<0){ digitalWrite(M1,0);} else

{digitalWrite(M1,1);} if (outPID2<0){ digitalWrite(M2,1);}
else {digitalWrite(M2,0);} analogWrite(E1,y1);
analogWrite(E2,y2);

Serial.print (v); Serial.print (" ");

Serial.print (w); Serial.print (" ");
Serial.print (thetaDotL); Serial.print (" ");
Serial.print (thetaDotR); Serial.print (" ");
Serial.print (outPID1); Serial.print (" ");
Serial.print (outPID2); Serial.print (" ");
Serial.print (y1); Serial.print (" ");
Serial.println (y2);
}

void readEncoder1()
{
n = digitalRead(encoder1PhaseA);
if (n == HIGH) {
if (digitalRead(encoder1PhaseB) ==
LOW) { encoder1Position--;
pulse1--
; } else {

Belajar Robotika, Hak Cipta: Hendawan Soebhakti, 2017 Hal. | 13

 encoder1Position++;
pulse1++;
}
}
}

void readEncoder2()
{
m = digitalRead(encoder2PhaseA);
if (m == HIGH) {
if (digitalRead(encoder2PhaseB) ==
LOW) { encoder2Position--;
pulse2--
} else {
encoder2Position++;
pulse2++;
}
}
}

ISR(TIMER2_OVF_vect) // interrupt service routine

{
TCNT2 = 6;
rps1 = (pulse1* (1 / 0.016))/tickPerRev;
rps2 = -(pulse2* (1 /
0.016))/tickPerRev; pulse1=0;
pulse2=0;
}
2. Upload kemudian perhatikan arah gerak robot. Lengkapi Tabel 4-5.
3. Gantilah v dan seperti pada Tabel 4-5.

Tabel 4-5 Pengaturan Kecepatan Putaran Roda Menggunakan PID Controller

No V (mm/s) (deg/s) Arah Gerak Robot
1 100 0 Maju
2 -100 0 Mundur
3 0 90 Kiri
4 0 -90 Kanan

H. TUGAS DAN PERTANYAAN

1. Buatlah kesimpulan dari percobaan ini!

Belajar Robotika, Hak Cipta: Hendawan Soebhakti, 2017 Hal. | 14

I. JAWAB

1. Berdasarkan tabel 4-1

Robot Trajectory pada saat robot maju 10 cm maka jarak tempuh yang
dihasilkan sebesar 105 mm dengan besaran phi-nya 1.02 mm/s. Sedangkan
pada saat robot berputar ke kanan 90 jarak tempuh 12.37 mm dengan phi-nya
85.41mm/s.
2. Berdasarkan Tabel 4-3
Forward Kinematic pada saat robot maju kecepatan roda kiri dan kecepatan ro-da
kanan bernilai positif dengan kecepatan rotasi (deg/s) sebesar 0 karena tidak
adanya sudut yang terbentuk. Pada saat robot mundur kecepatan roda kiri dan
kecepatan roda kanan bernilai negatif dikarenakan putaran robot terbalik dengan
kecepatan rotasi (deg/s) sebesar 0 karena tidak adanya sudut yang terbentuk.
Pada saat robot pivot kanan kecepatan roda kiri bernilai positif karena putaran
roda kedepan sedangkan kecepatan roda kanan bernilai negatif karena putaran
roda kebelakang dengan kecepatan rotasi (deg/s) sebesar 0.5 karena adanya
sudut yang terbentuk. Pada saat robot pivot kekiri kecepatan roda kiri bernilai
negatif karena putaran roda kebelakang sedangkan kecepatan roda kanan bernilai
positif karena putaran roda kedepan dengan kecepatan rotasi (deg/s) sebesar 0.5
karena adanya sudut yang terbentuk.
3. Berdasarkan Tabel 4-5
Pengaturan Kecepatan Putaran Roda Menggunakan PID Controller ketika nilai
input diprogram dimasukkan V=100 mm/s dengan (deg/s)=0 maka arah gerak
robot maju karena putaran roda kedepan. Ketika nilai input diprogram dimasukkan
V=-100 mm/s dengan (deg/s)=0 maka arah gerak robot mundur karena putaran
roda kebelakang.
Sedangkan ketika nilai input diprogram dimasukkan V=0 mm/s dengan
(deg/s)=90 maka arah gerak robot kekiri karena putaran roda sebelah kiri mundur
dan putaran roda sebelah sebelah kanan maju. Sedangkan ketika nilai input
diprogram dimasukkan V=0 mm/s dengan (deg/s)=-90 maka arah gerak robot
kekanan karena putaran roda sebelah kiri maju dan putaran roda sebelah sebelah
kanan mundur.