PRAKTIKUM 5

ROBOTIKA
Perencanaan Trayektori pada Manipulator 2 DoF
Planar

Disusun oleh:
BAYU BIMASAKTI 2120600031
2 D4 Elektronika B

PROGRAM STUDI TEKNIK ELEKTRONIKA

DEPARTEMEN TEKNIK ELEKTRO
POLITEKNIK ELEKTRONIKA NEGERI
SURABAYA
2022
 PANDUAN PRAKTIKUM ROBOTIKA
Percobaan 5:
Perencanaan Trayektori pada Manipulator 2 DoF Planar

A. TUJUAN
Mahasiswa mampu mengaplikasikan perencanaan trayektori pada robot manipulator
balik melalui praktek pada alat peraga manipulator planar 2 derajat kebebasan (DoF).

B. ALAT dan BAHAN

• PC (Personal Computer) dilengkapi system operasi Windows/Linux
• Alat Peraga Manipulator 2 DoF
• Perangkat lunak Arduino IDE (Windows/Linux)
• Program Kinematika Balik Manipulator 2 DoF
• Kabel USB male tipe A ke male tipe B
• Kertas milimeter ukuran A4

C. DASAR TEORI
C.1 Perencanaan Trayektori Manipulator

Gambar 5.1. Ilustrasi trayektori (A→B) pada suatu robot

Trayektori (trajectory) merupakan perubahan menurut waktu dari posisi,

kecepatan atau percepatan dalam suatu jalur (pathway). Tayektori dalam robotika
adalah perubahan posisi, kecepatan atau percepatan pada titik-titik lintasan mencakup
semua titik-titik laluan ditambah titik awal dan titik target.
 Gambar 5.2 Bentuk lintasan trayektori dapat bervariasi
Urutan yang diinginkan untuk titik-titik laluan (frame-frame) dapat
ditentukan/direncanakan. Perencanaan dituangkan dalam bentuk algoritma /rumus
pada program. Program pada sistem memutuskan bentuk lintasan (path), waktu yang
dibutuhkan, kecepatan dan lainnya dalam pencapaian target.

Gambar 5.3 Perencanaan Trayektori pada Manipulator 2 DoF planar

Pada contoh gambar 5.3, lintasan yang akan dilalui direncanakan terlebih dahulu
dalam bentuk set joint space atau work space. Dapat dilakukan secara manual atau
dengan bantuan alat. Selanjutnya hasil perencanaan disimpan dalam bentuk array
dalam program komputer.
Dalam perencanaan gambar trayektori perlu diperhatikan batas-batas workspace
dari manipulator. Workspace adalah daerah yang mungkin dijangkau oleh end of
effector. Dengan asumsi servo 1 dapat bergerak 0-180 dan servo 2 bergerak dari -90
s.d. +90 maka batasan workspace akan tampak seperti pada gambar 5.4.
 Gambar 5.4 Workspace pada modul manipulator

D. SETTING PERCOBAAN
Seting percobaan adalah sebagai berikut:
1. Siapkan semua peralatan dan seting sebagaimana kondisi akhir dari percobaan
sebelumnya.
2. Gunakan program percobaan 4 untuk memastikan kinematika balik pada modul masih
berfungsi dengan baik.
3. Jika masih sesuai, lanjutkan ke prosedur percobaan ini. Jika terdapat ketidaksesuaian
maka perlu dilakukan kalibrasi ulang pada koefisien-koefisien persamaan sebagaimana
pada percobaan 2.

E. PROSEDUR PERCOBAAN
Seting percobaan adalah sebagai berikut:
1. Buatlah batas workspace dari solusi kinematika balik yang mungkin berupa dua bentuk
setengah lingkaran, sebagaimana pada gambar 5.4. Setengah lingkaran besar memiliki
jari-jari sebesar L1+L2, yaitu panjang lengan 1 ditambah lengan 2. Sedangkan setengah
lingkaran kecil memiliki jari-jari √𝐿12 + 𝐿22, yaitu akar dari jumlah kuadrat kedua
pajang lengan. Nilai L1 dan L2 harus sesuai pengukuran pada manipulator.
2. Rencanakan titik-titik trayektori berbentuk persegi panjang seperti tampak pada
gambar 5.4. Posisi semua titik trayektori harus berada didalam area workspace dan
dianjurkan tidak terlalu mendekati garis batas. Beri jarak minimal sekitar 5 mm dari
kedua batas workspace.
3. Buka kembali program percobaan 4 pada Arduino IDE kemudian simpan program
percobaan 4 dengan nama lain (save as) sebagai percobaan 5.
4. Modifikasilah program tersebut dimulai dengan bagian pembuka (header) yaitu paling
atas sebelum fungsi “void setup()”. Sesuaikan seperti lampiran kode program pada
gambar 5.5. Nilai XB dan YB disesuaikan dengan perencanaan masing-masing.

Indeks data

Data set trayektori

Gambar 5.5. Bagian header program

5. Untuk bagian fungsi void setup() hanya mengganti tampilan awal menjadi “Prak 5
Robotika” dan judul “Tarajectory Plan” sebagaimana kode program pada gambar 5.6.

diganti

Gambar 5.6. Bagian setup

6. Berikutnya adalah memodifikasi bagian void loop () seperti pada lampiran kode
program pada gambar 5.7. Buatlah for loop untuk membaca satu per satu data XB(i)
dan YB(i). Hapus bagian pembacaan push button dan ganti dengan Xb=XB[i] dan
Yb=YB[i]

Gambar 5.7. Bagian loop utama

Untuk sub-program fungsi Kinemtika_Balik() seperti lampiran kode program pada
gambar 5.8 tidak ada perubahan.

Gambar 5.8. Sub-program Kinematika_Balik()

7. Hubungkan port USB pada modul kontroler menggunakan kabel USB yang tersedia.
Pastikan indikator LED merah pada modul menyala. Lakukan pengecekan status
koneksi modul mikrokontroler dengan PC seperti biasa.
8. Lakukan proses compile hingga tidak terdapat error atau warning, kemudian upload.
Tampilan LCD akan terlihat seperti gambar 5.9. dan lengan akan bergerak secara
kontinyu membentuk pola trayektori yang direncanakan. Biasanya terdapat perbedaan
(error) yang tidak konsisten pada masing-masing titik. Pada salah satu sisi persegi
panjang dapat mengalami error yang besar sementara sisi yang lain dapat lebih akurat.
 Gambar 5.9. Tampilan pada LCD

9. Untuk keperluan pengamatan, fungsikan salah satu push button, misal BT1 (pin 2)
sebagai tombol “PAUSE”. Aktifkan baris pada gambar 5.8 yang dilingkari merah
untuk fungsi tombol PAUSE tersebut.
10. Compile dan upload kembali program. EoE akan berhenti pada titik pertama.
11. Catat posisi XbT dan YbT yang tampil pada LCD pada kolom 2 dan 3 tabel 5.1. Contoh
pada gambar 5.10 menunjukkan pengambilan data untuk XbT =-2.9 dan YbT = 11.9.
12. Lakukan pengamatan visual secara seksama koordinat posisi EoE (XbT dan YbT) tepat
diujung lengan 2 pada milimeter blok. Contoh pada gambar 5.10 menunjukkan hasil
XbP =10.4 dan YbP = 10.5. Catat hasil XbP dan YbP pada kolom 4 dan 5 tabel 5.1.
13. Lanjutkan ke titik berikutnya dengan menekan PB1 (tombol pause) sekali tekan.
14. Ulangi langkah 11 s.d. 13 untuk pasangan XbT dan YbT lainnya hingga sebanyak 36
titik yang direncanakan.

YbP = 11.9

XbP =-2.9

Gambar 5.10. Cara pengambilan data posisi EoE dari workspace

 Tabel 5.1. Hasil pengamatan pada LCD dan Workspace
Posisi EoE Posisi EoE
Error
Input/Target pada papan
No. (cm)
(LCD) (cm) kerja (cm)
XbT YbT XbP YbP Xb Yb
1. -3.0 11.5 -2.9 11.9 0.1 0.4
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
..
..
36.

F. DATA PERCOBAAN
Berikut data hasil pengujian pada modul planar yang telah dibuat
Posisi EoE Posisi EoE Hasil
Input/Target Pada Papan Error (cm) Error
NO (LCD) (cm) Kerja (cm) Posisi EoE
(cm)
XbT YbT Xbp Ybp AXb AYb
1 -3 11.5 -3.6 10.8 -0.6 -0.7 0.9220
2 -2.5 11.5 -3.2 11 -0.7 -0.5 0.8602
3 -2 11.5 -2.5 11.4 -0.5 -0.1 0.5099
4 -1.5 11.5 -2.1 11.5 -0.6 0 0.6000
5 -1 11.5 -1.9 11.5 -0.9 0 0.9000
6 -0.5 11.5 -1.1 11.7 -0.6 0.2 0.6325
7 0 11.5 -0.7 11.6 -0.7 0.1 0.7071
8 0.5 11.5 -0.1 11.8 -0.6 0.3 0.6708
9 1 11.5 0.1 11.8 -0.9 0.3 0.9487
10 1.5 11.5 0.5 11.8 -1 0.3 1.0440
11 2 11.5 1 11.3 -1 -0.2 1.0198
12 2.5 11.5 1.8 11.4 -0.7 -0.1 0.7071
13 3 11.5 2.5 11.6 -0.5 0.1 0.5099
14 3 12 2 11.9 -1 -0.1 1.0050
15 3 12.5 2.2 12.5 -0.8 0 0.8000
16 3 13 2.2 12.8 -0.8 -0.2 0.8246
17 3 13.5 2.1 13.4 -0.9 -0.1 0.9055
18 3 14 2.1 13.8 -0.9 -0.2 0.9220
19 3 14.5 2.5 14.4 -0.5 -0.1 0.5099
20 3 15 2.7 14.7 -0.3 -0.3 0.4243
21 2.5 15 2.6 14.6 0.1 -0.4 0.4123
22 2 15 1.2 14.5 -0.8 -0.5 0.9434
23 1.5 15 0.7 14.3 -0.8 -0.7 1.0630
24 1 15 0 14.3 -1 -0.7 1.2207
25 0.5 15 -0.2 14.3 -0.7 -0.7 0.9899
26 0 15 -1 14.2 -1 -0.8 1.2806
27 -0.5 15 -1.2 14.3 -0.7 -0.7 0.9899
28 -1 15 -1.8 14.3 -0.8 -0.7 1.0630
29 -1.5 15 -2.3 14.4 -0.8 -0.6 1.0000
30 -2 15 -2.7 14.3 -0.7 -0.7 0.9899
31 -2.5 15 -2.9 14.2 -0.4 -0.8 0.8944
32 -3 15 -3.5 14.5 -0.5 -0.5 0.7071
33 -3 14.5 -3.5 13.8 -0.5 -0.7 0.8602
34 -3 14 -3.5 13.2 -0.5 -0.8 0.9434
35 -3 13.5 -3.3 13 -0.3 -0.5 0.5831
36 -3 13 -3.7 12.5 -0.7 -0.5 0.8602
37 -3 12.5 -3.5 12.2 -0.5 -0.3 0.5831
38 -3 12 -3.5 11.6 -0.5 -0.4 0.6403
39 -3 11.5 -3.6 11.2 -0.6 -0.3 0.6708
G. ANALISA
Berdasarkan percobaan yang telah dilakukann dengan menggunakan modul
planar yang telah dibuat, kita dapat melakukan perancangan Trayektori pada
manipulator 2 DoF planar. Pada percobaan ini terbih dahulu kita harus menentukan
rancangan pergerakan target yang diharapkan sehinga nantinya dapat dijadikan acuan
dalam perancangan pergerakan lengan 2 palanar DoF, selain itu kita juga harus
memperhatikan workspace atau daerah kerja dari Modul manipulator kita sehingga
mempermudah dalam melakukan perancangan pergerakan lengan nantinya. Pada
percobaan yang telah dilakukan dapat diketahui bahwa pada rancangan pergerakan
trayektori yang akan dibuat berupa pola persegi panjang yang terdiri dari 39 kali step
yang mana tiap stepnya memiliki rentang jarak sekitar 0,5cm dan digunakan sebuah
Push button untuk mempermudah melakukan pengambilan data pada percobaan ini.
Pada percobaan ini diperoleh data hasil percobaan seperti pada gambar diatas
dimana pada percobaan pertama dengan niali Xb dan Yb pada LCD adalah -3 dan
11.5 sedangkan pada hasil pengujian diperoleh Xb dan Yb adalah -3.6 dan 10.8
sehingga diperoleh besarnya erro pada Axb dan Ayb adalah -0.6 cm dan -0.7 cm, pada
saat nilai posisi EoE pada LCD adalah -2.5 dan 11.5 untuk Xb dan Yb namun pada
saat dilakukan pengujian justru di peroleh nilai Xb dan Yb adalah -3.1 dan 11 dengan
error berurutan sebesar -0.7 dan -0.5. pada percobaan tersebut dilakukan sebnyak 39
kali pengujian dimana besarnya error yang dihasilkan bervariasi.
Pada percobaan yang telah dilakukan apa bila dibandingkan hasil perhitungan
dengan hasil pengujian dan digambarkan dalam grafik maka diperoleh bentuk
pergerakannnya yaitu sebagai berikut:
Bentuk Visualisasi Trayektoru
20

15

10

0
-6 -4 -2 0 2 4

Pada grafik tersebut terlihat bahwa pergerakan dari trayektori hasil pengamatan LCD
terlihat membentuk persegi sempurna sedangkan untuk hasil pengamatan pada papan
manipulator diperoleh hasil seperti pada garis merah dimana terlihat bentuknya
kurang rapih, berdasarkan grafik tersebut kita dapat mengetahui bahwa terdapat error
atau ketidak presisian antara hasil pengujian dengan hasil perancangan, pada
percobaan ini terjadinya ketidak sesuaian antara hasil perhitungan dengan hasil
pengujian adalah karena pengaruh dari hardware servo yang digunakan yang mana
servonya memang tidak presisi, atau bisa juga terjadi karena pengaruh dari
pemasangan beberapa part pada modul planar yang dimiliki sehingga tidak
menghasilkan kepresisian, selain itu bisa juga terjadi karena faktor kesalahan dari
program yang digunakan atau terjadinya kesalahan pada saat melakukan pengukuran
dari panjang lengat servonya sehingga berpengaruh terhadap pergerakan servonya.
Untuk mengatais terjadinya ketidak sesuaian antara hasil perancangan dengan
pengujian dapat dilakukan dengan melakukan kalibarasi terlebih dahulu pada modul
palanar yang digunakan, bisa juga dilaukan dengan mengganti jenis servo yang
digunakan dengan jenis servo yang memilii tingkat kepresisian yang tinggi.
KESIMPULAN
Berdasarkan percobaan yang telah dilakukan dapat disimpulkan bahwa
trayektori merupakan perubahan pergerakan suatu lengan terhadap waktu dari posisi,
kecepatan ataupun percepatan dalan suatu pathaway atau jalur, selain itu trayektori
juga bisa dikatakan sebagai perubahan posisi, kecepatan atau percepatan pada titik-
titik lintasan mencakup semua titik-titik yang dilalui ditambah titik awal dan titik
target.
Berdasrkan percobaan yang telah dilakukan dapat disimpulkan bahwa pada
perancangan trayektori pada modul planar hal terpeting yang perlu diperhatikan
adalah mengamati kepresisian dari komponen yang digunakan, selain itu pentingnya
melakukan kalibarasi agar memperoleh hasil yang presis pada modul planar yang
akan digunakan untuk menghindari adanya ketidak sesuain antara hasil perancangan
dengan hasil pergerakan dari lengan planarnya.
Pada percobaan yang telah dilakukan dapat disimpulkan bahwa selain
kepresisian dari hardware yang digunakan hal lain yang sangat penting untuk
diperhatkan adalah mengamati workspace atau daerah kerja dari modul planar
sehingga menghindarkan kita dari perancangan pergerakan lengan yang tidak mampu
di jangkau oleh lengan planar itu sendiri.
Berdasarkan percobaan yang telah dilakuukan dapat disimpulkan bahwa pada
perancangan pergerakan trayektori perancangan dituangkan dalam bentuk
algoritma/rumus pada program. Program pada sistem akan bertugas memutuskan
benuk lintasan (path), waktu yang dibutuhkan, kecepatan dan dalam pencapaian
target. Pada saat melakukan perancangan pergerakan bentuk lintasan yang akan
dilalui direncanakn terlebih dahulu dalam bentukk set join space, dapat dilakukan
secara manual maupun dengan menggunakan bantuan alat, selanjutnnya hasil
perencanaan disimpan dalam bentu array dalam program.
H. TUGAS
Setelah menyelesaikan pengambilan semua data diatas, praktikum bisa diakhiri. Namun
untuk kelengkapan hasil dan analisa, kerjakan tugas berikut:
• Buatlah visualisasi menggunakan Microsoft Excel atau semacamnya dari kedua
data set (rencana dan kenyataan) menggunakan visulisasi titik dan garis dengan
warna yang berbeda sehingga tampak gambaran kedua trayektori tersebut.
Jawaban:
Bentuk Visualiasi dengan Titik

Bentuk Visualiasi TrayekTori

17
15
13
11
9
7
5
3
1

-5 -4 -3 -2 -1 -1 0 1 2 3 4

Hasil Targer Hasil Pengujian

Titik biru : Hasil target yang ingin di capai

Titik Merah : Hasil pengujian pada modul papan

Bentuk Visualisasi Trayektoru

16

14

12

10

0
-5 -4 -3 -2 -1 0 1 2 3 4
• Lakukan juga perhitungan secara statistik data-data pada tabel 5.1. Hitung nilai
maksimum, minimum dan rata-rata dari data error kolom 5, 6. Lakukan juga
perhitungan deviasi standar dari masing-masing set data error tersebut.
Jawab:
Posisi EoE Posisi EoE Hasil Error
Input/Target Pada Papan Error (cm) Posisi
NO (LCD) (cm) Kerja (cm) EoE
XbT YbT Xbp Ybp AXb AYb (cm)
1 -3 11.5 -3.6 10.8 -0.6 -0.7 0.9220
2 -2.5 11.5 -3.2 11 -0.7 -0.5 0.8602
3 -2 11.5 -2.5 11.4 -0.5 -0.1 0.5099
4 -1.5 11.5 -2.1 11.5 -0.6 0 0.6000
5 -1 11.5 -1.9 11.5 -0.9 0 0.9000
6 -0.5 11.5 -1.1 11.7 -0.6 0.2 0.6325
7 0 11.5 -0.7 11.6 -0.7 0.1 0.7071
8 0.5 11.5 -0.1 11.8 -0.6 0.3 0.6708
9 1 11.5 0.1 11.8 -0.9 0.3 0.9487
10 1.5 11.5 0.5 11.8 -1 0.3 1.0440
11 2 11.5 1 11.3 -1 -0.2 1.0198
12 2.5 11.5 1.8 11.4 -0.7 -0.1 0.7071
13 3 11.5 2.5 11.6 -0.5 0.1 0.5099
14 3 12 2 11.9 -1 -0.1 1.0050
15 3 12.5 2.2 12.5 -0.8 0 0.8000
16 3 13 2.2 12.8 -0.8 -0.2 0.8246
17 3 13.5 2.1 13.4 -0.9 -0.1 0.9055
18 3 14 2.1 13.8 -0.9 -0.2 0.9220
19 3 14.5 2.5 14.4 -0.5 -0.1 0.5099
20 3 15 2.7 14.7 -0.3 -0.3 0.4243
21 2.5 15 2.6 14.6 0.1 -0.4 0.4123
22 2 15 1.2 14.5 -0.8 -0.5 0.9434
23 1.5 15 0.7 14.3 -0.8 -0.7 1.0630
24 1 15 0 14.3 -1 -0.7 1.2207
25 0.5 15 -0.2 14.3 -0.7 -0.7 0.9899
26 0 15 -1 14.2 -1 -0.8 1.2806
27 -0.5 15 -1.2 14.3 -0.7 -0.7 0.9899
28 -1 15 -1.8 14.3 -0.8 -0.7 1.0630
29 -1.5 15 -2.3 14.4 -0.8 -0.6 1.0000
30 -2 15 -2.7 14.3 -0.7 -0.7 0.9899
31 -2.5 15 -2.9 14.2 -0.4 -0.8 0.8944
32 -3 15 -3.5 14.5 -0.5 -0.5 0.7071
 33 -3 14.5 -3.5 13.8 -0.5 -0.7 0.8602
34 -3 14 -3.5 13.2 -0.5 -0.8 0.9434
35 -3 13.5 -3.3 13 -0.3 -0.5 0.5831
36 -3 13 -3.7 12.5 -0.7 -0.5 0.8602
37 -3 12.5 -3.5 12.2 -0.5 -0.3 0.5831
38 -3 12 -3.5 11.6 -0.5 -0.4 0.6403
39 -3 11.5 -3.6 11.2 -0.6 -0.3 0.6708
Nilai Maksimal : 0.1 0.3
Nilai Minimal : -1 -0.8
Nilai Rata-Rata : -0.672 -0.323
Standar Deviasi : 0.233 0.343

• Dari gambar No.1 berikan analisa anda apa penyebab perbedaan bisa terjadi secara
signifikan dan bagaimana cara mengurangi perbedaan tersebut?
Jawab:
Jawaban dari pertanyaan ini dijelaskan dalam analisa

I. LAMPIRAN
Soruce Code Program
//==============PRAKTIKUM 5 ROBOTIKA=============
// Nama : Bayu Bimasakti
// NRP :2120600031
// Kelas : 2D4EB
#include <Servo.h>
#include <LiquidCrystal_I2C.h>
#include <Wire.h>

// Deklarasi Objek
LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo servo1;
Servo servo2;

//int teta1=0, teta2=0, Ton1, Ton2;

char kalimat[17];
int Ton1 = 550, Ton2 = 1500, i;
const float L1 = 7.8, L2 = 8.8, Pi = 3.1428571428571428571428571428571,
mineuc = sqrt(pow(L1, 2) + pow(L1, 2) ), maxeuc = L1 + L2; // euc =
jarak EoE dengan Base
float Xb = maxeuc, Yb = 0.0, teta1, teta2, teta1rad, teta2rad, costeta2rad,
sinteta2rad, euc;
boolean bt1, bt2, bt3, bt4;

// 3rd degree Polynomial Fit : = a+bx+cx^2+dx^3 coefficient data:

const double

a1 = 5.472317156527677E+02,
b1 = 1.124890658017592E+01,
c1 = -9.884671571977971E-03,
d1 = 4.417237389373578E-05,

a2 = 1.471751437417064E+03,
b2 = -1.040839900553522E+01,
c2 = 3.626713843431162E-03,
d2 = -3.365700347046641E-05;

float XB[] = {-3, -2.5, -2, -1.5, -1, -0.5, 0, 0.5, 1, 1.5, 2, 2.5, 3, 3, 3, 3,
3, 3, 3, 3, 2.5, 2, 1.5, 1, 0.5, 0, -0.5, -1, -1.5, -2, -2.5, -3, -3, -3, -3, -3,
-3, -3, -3};
float YB[] = {11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5, 11.5,
11.5, 11.5, 12, 12.5, 13, 13.5, 14, 14.5, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15,
15, 15, 15, 14.5, 14, 13.5, 13, 12.5, 12, 11.5};

// Deklarasi Variabel

void setup()
{
Serial.begin(9600);
// put your setup code here, to run once:
lcd.init(); // Aktifkan LCD (Untuk Inisialisasi)
lcd.backlight(); // Aktifkan Backlight
 lcd.clear();
lcd.setCursor(0, 0); // Set Cursor pada posisi 0,0
lcd.print("Prak 5 Robotika"); // Tampiilkan Tulisan
lcd.setCursor(0, 1); // Set cursor pada posisi 0,1
lcd.print("Trajectory Plan"); // Tampilkan Tulisan

servo1.attach(9); // hubungkan kabel data servo ke pin 9

servo2.attach(10); // hubungkan kabel data servo ke pin 10
pinMode(2, INPUT);
pinMode(3, INPUT);
pinMode(4, INPUT);
pinMode(5, INPUT);
delay(200);
}

void loop()
{

for (int i = 0; i <= (sizeof(XB) / sizeof(float)-1); i++) {

Xb = XB[i]; Yb = YB[i];

sprintf(kalimat, "Xb=%2d.%1d Yb=%2d.%1d", (int)Xb, (int) abs(Xb * 10) % 10,

(int)Yb, (int)abs(Yb * 10) % 10);
lcd.setCursor(0, 0); lcd.print(kalimat);

Kinematika_Balik(); //Menghitung nilai teta dari posisi Xb,Yb EoE yang

diinputkan

sprintf(kalimat, "Q1=%2d.%1d Q2=%2d.%1d", (int)teta1, (int) abs(teta1 * 10) %

10, (int)teta2, (int)abs(teta2 * 10) % 10);
lcd.setCursor(0, 1); lcd.print(kalimat);

Ton1 = a1 + b1 * teta1 + c1 * pow(teta1, 2) + d1 * pow(teta1, 3); //Konversi

teta1 ke Ton1
Ton2 = a2 + b2 * teta2 + c2 * pow(teta2, 2) + d2 * pow(teta2, 3); //Konversi
teta2 ke Ton2
servo1.writeMicroseconds(Ton1);
servo2.writeMicroseconds(Ton2);
while(digitalRead(2)) delay(1500); // Kontrol Oleh push button 1
delay(2000);
}
}

void Kinematika_Balik() {
costeta2rad = (pow(Xb, 2) + pow(Yb, 2) - pow(L1, 2) - pow(L2, 2)) / (2 * L1 *
L2);
sinteta2rad = sqrt(1 - pow(costeta2rad, 2)); //Nilai Xb pada atan2 (Yb,Xb)
tidak boleh nol
if (Xb == 0.0 ) Xb = 1E-20; //Costeta2rad dan Xb tidak boleh nol
if (costeta2rad == 0) costeta2rad = 1E-20;
teta2rad = atan2 (sinteta2rad, costeta2rad); //nilaicosteta2rad tidak boleh nol
 teta1rad = atan2(Yb, Xb) - atan2((L2 * sinteta2rad), (L1 + (L2 *
costeta2rad))); // Nilai Xb tidak boleh nol
teta2 = teta2rad * 180 / Pi;
teta1 = teta1rad * 180 / Pi;
// Baris dibawah ini mengantisipasi munculnya nilai negatif pada teta 1
if (teta1 < 0.0) {
teta1 = abs(teta1) + (2 * atan2(Yb, Xb) * 180 / Pi);
teta2 = (-teta2);
}
}

// Inputkan Sudut dari 0 - 180 derajat dengan kelipatan 10

// Lalu di buatkan tabelvoid setup() {
// put your setup code here, to run once: