KINEMATIKA DIFFERENTIAL

4 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+
+;
}
}
Belajar Robotika, Hak Cipta: Hendawan Soebhakti, 2017 Hal. | 6
 }

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

No Aksi Robot Enc1 Enc2 sL sR S PHI

(pulse) (pulse) (mm) (mm) (mm) (Deg)
1 Maju 10 cm
2 Kanan 90 derajat

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 Roda Kiri,
̇
Roda Kanan,
̇ linier, rotasi,
(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
2 Mundur
3 Pivot kanan
4 Pivot kiri

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 Roda Kiri,
̇
Roda Kanan,
̇ linier, rotasi,
(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
2 -100 0
3 0 90
4 0 -90

H. TUGAS DAN PERTANYAAN

1. Buatlah kesimpulan dari percobaan ini!

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