Tuning Kontrol Pid Line Follower
37
TUNING KONTROL PID LINE FOLLOWER Tunning kontrol PID ini bertujuan untuk menentukan paramater aksi kontrol Proportional, Integratif, Derivatif pada robot line follower. Proses ini dapat dilakukan dengan cara trial and error . Keunggulan cara ini kita gak perlu mengidentifikasi plant, membuat model matematis plant, menentukan parameter plant dengan grafis ataupun analitis…cukup dengan cara coba-coba memberikan konstanta P-I-D pada formula PID hingga di peroleh hasil yang di inginkan, dengan mengacu pada karakteristik masing-masing kontrol P-I-D. Nach kalo kita menggunakan kendali PID artinya kita nantinya bertujuan mengolah suatu sinyal kesalahan atau error, nilai error tersebut diolah dengan formula PID untuk dijadikan suatu sinyal kendali atau sinyal kontrol yang akan diteruskan ke aktuator. Gampangnya perhatikan saja blok diagram umpan balik loop tertutup pada perancangan kedali PID pada robot line follower berikut ini: Dari blok diagram diatas dapat q jelasin sebagai berikut 1. SP = Set point, secara simple maksudnya ialah suatu prameter nilai acuan atau nilai yang kita inginkan. 2. PV = Present Value, kalo yang ini maksudnya ialah nilai bobot pembacaan sensor saat itu atau variabel terukur yang di umpan balikan oleh sensor (sinyal feedback dari sensor). 3. Error = nilai kesalahan, nach kalo yang ini pengertiannya ialah Deviasi atau simpangan antar variabel terukur atau bobot sensor (PV) dengan nilai acuan (SP)
-
Upload
dimas-kartika -
Category
Documents
-
view
165 -
download
11
Transcript of Tuning Kontrol Pid Line Follower
TUNING KONTROL PID LINE FOLLOWER
Tunning kontrol PID ini bertujuan untuk menentukan paramater aksi kontrol Proportional, Integratif, Derivatif pada robot line follower. Proses ini dapat dilakukan dengan cara trial and error . Keunggulan cara ini kita gak perlu mengidentifikasi plant, membuat model matematis plant, menentukan parameter plant dengan grafis ataupun analitis…cukup dengan cara coba-coba memberikan konstanta P-I-D pada formula PID hingga di peroleh hasil yang di inginkan, dengan mengacu pada karakteristik masing-masing kontrol P-I-D.
Nach kalo kita menggunakan kendali PID artinya kita nantinya bertujuan mengolah suatu sinyal kesalahan atau error, nilai error tersebut diolah dengan formula PID untuk dijadikan suatu sinyal kendali atau sinyal kontrol yang akan diteruskan ke aktuator. Gampangnya perhatikan saja blok diagram umpan balik loop tertutup pada perancangan kedali PID pada robot line follower berikut ini:
Dari blok diagram diatas dapat q jelasin sebagai berikut
1. SP = Set point, secara simple maksudnya ialah suatu prameter nilai acuan atau nilai yang kita inginkan.
2. PV = Present Value, kalo yang ini maksudnya ialah nilai bobot pembacaan sensor saat itu atau variabel terukur yang di umpan balikan oleh sensor (sinyal feedback dari sensor).
3. Error = nilai kesalahan, nach kalo yang ini pengertiannya ialah Deviasi atau simpangan antar variabel terukur atau bobot sensor (PV) dengan nilai acuan (SP)
Setelah memahami alur pengendalian PID nach berikut ilustrasi pemberian bobot sensor (nilai kesalahan pembacaan sensor) pada robot line follower:
dari blok iliustrasi tersebut dapat q jelasin sebagai berikut:
proses pemberian bobot sensor dapat dilakukan dengan proses pemetaan (mapping) pembacaan sensor terhadap garis, berikut salah satu sample proses mapping sensor:
11111110 (PV=-7)
11111000 (PV=-6)
11111100 (PV=-6)
11111101 (PV=-5)
11110001 (PV=-4)
11111001 (PV=-4)
11111011 (PV=-3)
11100011 (PV=-2)
11110011 (PV=-2)
11110111 (PV=-1)
11100111 (PV=0)
11101111 (PV=1)
11000111 (PV=2)
11001111 (PV=2)
11011111 (PV=3)
10001111 (PV=4)
10011111 (PV=4)
10111111 (PV=5)
00011111 (PV=6)
00111111 (PV=6)
01111111 (PV=7)
11111111 (PV=8 / -8)
kondisi ideal robot terjadi saat kondisi robot pada PV= 0 (misal kondisi nilai sensor = 11100111, nilai 0 merepresentasikan sensor mengenai garis). Atau SP = 0 ialah kondisi ideal dari robot line follower. Jika PV tidak sama dengan 0 berarti robot tidak berada pada kondisi ideal dan artinya ada sinyal kesalahan (error). Pada kondisi error inilah formula PID akan menentukan hasil sinyal kendalinya. Nach berikut ini penjelasan tentang penerapan PID pada robot line follower:
Proporsional kontrol
Kondisi ideal pada robot adalah bergerak maju lurus mengikuti garis, dengan kata lain PV = 0 (nilai sensor = 11100111). Dari sini dapat diasumsikan bahwa Set Point (SP) / kondisi ideal adalah saat SP = 0. Nilai sensor yang dibaca oleh sensor disebut Process Variable (PV) / nilai aktual pembacaan. Menyimpangnya posisi robot dari garis disebut sebagai error (e), yang didapat dari e = SP – PV. Dengan mengetahui besar error, mikrokontroler dapat memberikan nilai PWM motor kiri dan kanan yang sesuai agar dapat menuju ke posisi ideal (SP = 0). Besarnaya nilai PWM ini dapat diperoleh dengan menggunakan kontrol Proporsional (P), dimana P = e Kp (Kp adalah konstanta proporsional yang nilainya di set sendiri dari hasil tuning).
Saat Sensor = 11111111 nilai PV dapat ditentukan dengan cara membandingkan kondisi PV sebelumnya, jika PV lebih besar dari 0, maka nilai PV dapat diasumsikan 30 dan jika PV kurang dari 0, maka nilai PV dapat diasumsikan -30 atau dengan cara menyimpan nilai error yang lalu.
Dalam pemrograman robot line follower ini kodenya ditulis secara sederhana seperti berikut:
If Sensor = &B11111111 Then
If Pv < 0 Then : Pv = -30
End If
If Pv > 0 Then : Pv = 30
End If
End If
Perhitungan kendali proporsional
Sp_sensor = 0 ‘setpoint sensor
Error = Sp_sensor – Pv ‘nilai error
P = Kp * Error ‘proporsional kontrol
Aplikasi kontrol proporsional pada PWM ialah sebagai berikut:
Pwm = Sp_kecepatan + P ‘motor kiri
Pwm = Sp_kecepatan – P ‘motor kanan
Derivatif kontrol
Jika pergerakan robot masih terlihat bergelombang, bisa ditambahkan kontrol Derivatif (D). Kontrol D digunakan untuk mengukur seberapa cepat robot bergerak dari kiri ke kanan atau dari kanan ke kiri. Semakin cepat bergerak dari satu sisi ke sisi lainnya, maka semakin besar nilai D. Konstanta D (Kd) digunakan untuk menambah atau mengurangi imbas dari derivatif. Dengan mendapatkan nilai Kd yang tepat pergerakan sisi ke sisi yang bergelombang akibat dari kontrol proporsional bisa diminimalisasi. Nilai D didapat dari D = Kd/Ts * rate, dimana Ts ialah time sampling atau waktu cuplik dan rate = e(n) – e(n-1). Dalam program nilai error (SP – PV) saat itu menjadi nilai last_error, sehingga rate didapat dari error – last_error. Untuk menambahkan kontrol D, program dimodifikasi menjadi:
Perhitungan kendali proporsional + Derivatif
Sp_sensor = 0 ‘setpoint sensor
Error = Sp_sensor – Pv ‘nilai error
P = Kp * Error ‘proporsional kontrol
D1 = Kd * 10 ‘derivatif kontrol
D2 = D1 / Ts
D3 = Error – Last_error ‘rate
D = D2 * D3
Last_error = Error ‘error lampau
Pd = P + D ‘proporsional-derivatif kontrol
Aplikasi kontrol proporsional dan drivatif pada PWM ialah sebagai berikut:
Pwm = Sp_kecepatan + Pd ‘motor kiri
Pwm = Sp_kecepatan – Pd ‘motor kanan
Integratif kontrol
Jika dengan P + D sudah membuat pergerakan robot cukup smooth, maka penambahan Integratif menjadi opsional. Jika ingin mencoba-coba bisa ditambahakan Integratif (I). I digunakan untuk mengakumulasi error dan mengetahui durasi error. Dengan menjumlahkan error disetiap pembacaan PV akan memberikan akumulasi offset yang harus diperbaiki sebelumnya. Saat robot bergerak menjauhi garis, maka nilai error akan bertambah. Semakin lama tidak mendapatkan SP, maka semakin besar nilai I. Degan mendapatkan nilai Ki yang tepat, imbas dari Integratif bisa dikurangi. Nilai akumulasi error didapat dari: error + last_error. Untuk menambahkan kontrol I, maka program di modifikasi menjadi:
Perhitungan kendali proporsional + integratif + derivatif
Sp_sensor = 0 ‘setpoint sensor
Error = Sp_sensor – Pv ‘nilai error
P = Kp * Error ‘proporsional kontrol
D1 = Kd * 10 ‘derivatif kontrol
D2 = D1 / Ts
D3 = Error – Last_error ‘rate
D = D2 * D3
I1 = Ki / 10 ’integratif kontrol
I2 = Error + Last_error ‘akumulasi error
I3 = I1 * I2
I = I3 * Ts
Last_error = Error ‘error lampau
Pd = P + D ‘proporsional-derivatif kontrol
Pid = Pd+I ‘proporsional-integratif-derivatif
Aplikasi kontrol proporsional, integratif dan drivatif pada PWM ialah sebagai berikut:
Pwm = Sp_kecepatan + Pid ‘motor kiri
Pwm = Sp_kecepatan – Pid ‘motor kanan
Parameter Nilai Konstanta PID
Nilai konstanta perhitungan PID di tuning secara trial and error, proses ini dilakukan dengan metode mencoba-coba (eksperimental) nilai proporsional, derivatif dan integratif pada formula PID hingga ditemukan hasil sistem yag stabil, adapun cara yang dilakukan untuk mentuning PID pada robot line follower ialah sebagai berikut:
1. Langkah awal gunakan kontrol proporsional terlebih dahulu, abaikan konstanta integratif dan derivatifnya dengan memberikan nilai nol pada integratif dan derivatif.
2. Tambahkan terus konstanta proporsional maksimum hingga keadaan stabil namun robot masih berosilasi.
3. Untuk meredam osilasi, tambahkan konstanta derivatif dengan membagi dua nilai proporsional, amati keadaan sistem robot hingga stabil dan lebih responsif.
4. Jika sistem robot telah stabil, kontrol integratif dapat menjadi opsional, dalam artian jika ingin mencoba-coba tambahkan kontrol integratif tersebut, namun pemberian nilai integratif yang tidak tepat dapat membuat sistem robot menjadi tidak stabil.
5. Nilai set point kecepatan dan nilai batas bawah/atas memberikan patokan kecepatan robot.
6. Nilai time sampling (waktu cuplik) juga mempengaruhi perhitungan PID, tentunnya saat penggunaan kontrol integratif dan derivatif.
7. Periksa kembali perfoma sistem hingga mendapatkan hasil yang memuaskan.
Acuan penentuan parameter Kp, Ki dan Kd dapat diadopsi dari watak dari kontroler itu masing seperti yang dijelaskan pada tabel berikut ini:
Dan berikut ini sample koding PID line followerku dengan pemrograman BASCOM AVR :
$regfile = “m8535.dat”$crystal = 12000000$eeprom
‘—————————inisialisasi pwm————————————Config Timer1 = Pwm , Pwm = 10 , Prescale = 64 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down ‘pwm dengan settingan fast pwm 10 bit
‘—————————inisialisasi adc————————————Config Adc = Single , Prescaler = Auto
‘—————————inisialisasi lcd————————————Config Lcdpin = Pin , Db4 = Portc.4 , Db5 = Portc.5 , Db6 = Portc.6 , Db7 = Portc.7 , E = Portc.2 , Rs = Portc.0Config Lcd = 16 * 2 : Cursor Off
‘——————————————————————————-‘PENDEFINISIAN PIN MIKRO‘——————————————————————————-Config Portd = Output
‘alias logika motorM1a Alias Portd.0M1b Alias Portd.1M2a Alias Portd.2M2b Alias Portd.3
Config Portb = InputPortb = 255‘alias tombolSw_ok Alias Pinb.3Sw_cancel Alias Pinb.2Sw_down Alias Pinb.1Sw_up Alias Pinb.0
Config Portc.3 = Output
‘——————————————————————————-‘DEKLARASI VARIABEL‘——————————————————————————-Dim B1 As Bit , B2 As Bit , B3 As Bit , B4 As Bit , B5 As Bit , B6 As Bit , B7 As Bit , B8 As Bit,Dim S1 As Byte , S2 As Byte , S3 As Byte , S4 As Byte , S5 As Byte , S6 As Byte , S7 As Byte , S8 As ByteDim Adc1 As Integer , Adc2 As Integer , Adc3 As Integer , Adc4 As Integer , Adc5 As Integer , Adc6 As Integer , Adc7 As Integer , Adc8 As Integer,
Dim S12 As Byte , S123 As Byte , S1234 As Byte , S12345 As Byte , S123456 As Byte , S1234567 As Byte , S12345678 As ByteDim C As ByteDim B As ByteDim Sensor As ByteDim Konversi_sensor As ByteDim Data1 As ByteDim Menu As Byte
‘——————————-declarasi variabel——————————‘variabel eepromDim Kp_eprom As Eram IntegerDim Ki_eprom As Eram IntegerDim Kd_eprom As Eram IntegerDim Ts_eprom As Eram IntegerDim Sp_kecepatan_eprom As Eram IntegerDim Up_eprom As Eram IntegerDim Lp_eprom As Eram IntegerDim Nos_eprom As Eram IntegerDim Hold_eprom As Eram Integer
Dim Kp As IntegerDim Ki As IntegerDim Kd As IntegerDim Ts As IntegerDim Sp_sensor As IntegerDim Sp_kecepatan As IntegerDim Up As IntegerDim Lp As IntegerDim Nos As IntegerDim Hold As Integer
Dim Error As IntegerDim Last_error As IntegerDim Pv As IntegerDim P As IntegerDim Pd As IntegerDim Pid As IntegerDim Pwm As Integer
Dim I1 As IntegerDim I2 As IntegerDim I3 As IntegerDim I As Integer
Dim D1 As IntegerDim D2 As IntegerDim D3 As IntegerDim D As Integer
Dim N1 As IntegerDim N2 As Integer
‘——————————————————————————-‘DEKLARASI SUBRUTIN‘——————————————————————————-
Declare Sub Baca_sensor()Declare Sub Kiri_maju()Declare Sub Kanan_maju()Declare Sub Kiri_mundur()Declare Sub Kanan_mundur()Declare Sub Bantingkanan()Declare Sub Bantingkiri()
Portc.3 = 0 ‘lcd on
Cls : HomeLcd “ADC LINEfollower”Waitms 500LowerlineLcd “Fahmizal_dte2006″Waitms 1500
Portc.3 = 1 ‘lcd off
Menu_utama:ClsHome : Lcd “‘menu’<<<<^>>>>>”LowerlineLcd “Please choice…”DoIf Sw_up = 0 Then : Waitms 250 : Menu = 1End IfIf Sw_down = 0 Then : Waitms 250 : Menu = 2End If
If Menu = 1 Then : Locate 2 , 1 : Lcd “Setting ROBOT…”If Sw_ok = 0 Then : Waitms 250 : Gosub Setting_pid : End IfEnd If
If Menu = 2 Then : Locate 2 , 1 : Lcd “Go……..Ready?”If Sw_ok = 0 Then : Waitms 250 : Gosub Go : End IfEnd IfLoop
‘——————————-menu Konstanta PID——————————Setting_pid:ClsMenu_kp:Do
Kp = Kp_epromIf Sw_up = 0 Then : Incr Kp : Waitms 10End IfIf Sw_down = 0 Then : Decr Kp : Waitms 10End If
If Sw_ok = 0 Then : Waitms 250 : Gosub Menu_kiEnd IfIf Sw_cancel = 0 Then : Waitms 250 : Gosub Menu_utamaEnd If
If Kp > 50 Then : Kp = 0 : Waitms 10End IfIf Kp < 0 Then : Kp = 50 : Waitms 10End If
Home : Lcd “Tuning PID ^–^ ” : Locate 2 , 1 : Lcd “KP:” : Locate 2 , 4 : Lcd “ ” : Locate 2 , 4 : Lcd Kp : Waitms 100
Kp_eprom = KpLoop
‘———————————————
Menu_ki:Do
Ki = Ki_epromIf Sw_up = 0 Then : Incr Ki : Waitms 10End IfIf Sw_down = 0 Then : Decr Ki : Waitms 10End If
If Sw_ok = 0 Then : Waitms 250 : Gosub Menu_kdEnd If
If Sw_cancel = 0 Then : Waitms 250 : Gosub Menu_kpEnd If
If Ki > 20 Then : Ki = 0 : Waitms 10End IfIf Ki < 0 Then : Ki = 20 : Waitms 10End If
Home : Lcd “Tuning PID ^–^ ” : Locate 2 , 1 : Lcd “KI:” : Locate 2 , 4 : Lcd “ ” : Locate 2 , 4 : Lcd Ki : Waitms 100
Ki_eprom = KiLoop
‘———————————————
Menu_kd:Do
Kd = Kd_epromIf Sw_up = 0 Then : Kd = Kd + 5 : Waitms 10End IfIf Sw_down = 0 Then : Kd = Kd – 5 : Waitms 10End If
If Sw_ok = 0 Then : Waitms 250 : Gosub Menu_tsEnd IfIf Sw_cancel = 0 Then : Waitms 250 : Gosub Menu_kiEnd If
If Kd > 150 Then : Kd = 0 : Waitms 10End IfIf Kd < 0 Then : Kd = 150 : Waitms 10End If
Home : Lcd “Tuning PID ^–^ ” : Locate 2 , 1 : Lcd “KD:” : Locate 2 , 4 : Lcd “ ” : Locate 2 , 4 : Lcd Kd : Waitms 100
Kd_eprom = Kd
Loop
‘———————————————
Menu_ts:Do
Ts = Ts_epromIf Sw_up = 0 Then : Ts = Ts + 5 : Waitms 10End IfIf Sw_down = 0 Then : Ts = Ts – 5 : Waitms 10End If
If Sw_ok = 0 Then : Waitms 250 : Gosub Menu_sp_kecepatanEnd IfIf Sw_cancel = 0 Then : Waitms 250 : Gosub Menu_kdEnd If
If Ts > 20 Then : Ts = 0 : Waitms 10End IfIf Ts < 0 Then : Ts = 20 : Waitms 10End If
Home : Lcd “TimeSampling(ms)” : Locate 2 , 1 : Lcd “TS:” : Locate 2 , 4 : Lcd “ ” : Locate 2 , 4 : Lcd Ts : Waitms 100
Ts_eprom = Ts
Loop
‘———————————————
Menu_sp_kecepatan:Do
Sp_kecepatan = Sp_kecepatan_epromIf Sw_up = 0 Then : Sp_kecepatan = Sp_kecepatan + 50 : Waitms 10End IfIf Sw_down = 0 Then : Sp_kecepatan = Sp_kecepatan – 50 : Waitms 10End If
If Sw_ok = 0 Then : Waitms 250 : Gosub Menu_upEnd IfIf Sw_cancel = 0 Then : Waitms 250 : Gosub Menu_tsEnd If
If Sp_kecepatan > 600 Then : Sp_kecepatan = 400 : Waitms 5End IfIf Sp_kecepatan < 400 Then : Sp_kecepatan = 600 : Waitms 5End If
Home : Lcd “Set Point ^–^ ” : Locate 2 , 1 : Lcd “kecepatan:” : Locate 2 , 11 : Lcd “ ” : Locate 2 , 11 : Lcd Sp_kecepatan : Waitms 100
Sp_kecepatan_eprom = Sp_kecepatan
Loop
‘———————————————Menu_up:Do
Up = Up_epromIf Sw_up = 0 Then : Up = Up + 50 : Waitms 10End IfIf Sw_down = 0 Then : Up = Up – 50 : Waitms 10End If
If Sw_ok = 0 Then : Waitms 250 : Gosub Menu_nosEnd IfIf Sw_cancel = 0 Then : Waitms 250 : Gosub Menu_sp_kecepatanEnd If
If Up > 1000 Then : Up = 700 : Waitms 10End IfIf Up < 700 Then : Up = 1000 : Waitms 10End If
Home : Lcd “TOP speed ROBOT ” : Locate 2 , 1 : Lcd “top:” : Locate 2 , 5 : Lcd “ ” : Locate 2 , 5 : Lcd Up : Waitms 100
Up_eprom = Up
Loop
‘———————————————Menu_nos:Do
Nos = Nos_epromIf Sw_up = 0 Then : Nos = Nos + 100 : Waitms 10End IfIf Sw_down = 0 Then : Nos = Nos -100 : Waitms 10End If
If Sw_ok = 0 Then : Waitms 250 : Gosub Menu_hold:End IfIf Sw_cancel = 0 Then : Waitms 250 : Gosub Menu_upEnd If
If Nos > 500 Then : Nos = 100 : Waitms 10End IfIf Nos < 100 Then : Nos = 500 : Waitms 10End If
Home : Lcd “POWER ^–^ ROBOT” : Locate 2 , 1 : Lcd “start:” : Locate 2 , 7 : Lcd “ ” : Locate 2 , 7 : Lcd Nos : Waitms 100
Nos_eprom = Nos
Loop‘——————————————————————————-
Menu_hold:Do
Hold = Hold_epromIf Sw_up = 0 Then : Hold = Hold + 5 : Waitms 10End IfIf Sw_down = 0 Then : Hold = Hold – 5 : Waitms 10End If
If Sw_ok = 0 Then : Waitms 250 : Gosub Rampung_setting:End IfIf Sw_cancel = 0 Then : Waitms 250 : Gosub Menu_nosEnd If
If Hold > 75 Then : Hold = 25 : Waitms 10End IfIf Hold < 25 Then : Hold = 75 : Waitms 10End If
Home : Lcd “TressHold^SENSOR” : Locate 2 , 1 : Lcd “hold:” : Locate 2 , 6 : Lcd “ ” : Locate 2 , 6 : Lcd Hold : Waitms 100
Hold_eprom = Hold
Loop‘——————————————————————————-
‘——————————-tampilansaveepprom——————————Rampung_setting:
ClsLocate 1 , 1 : Lcd “Write’_to_EEPROM”Locate 2 , 1 : Lcd “=” : Waitms 200
Locate 2 , 2 : Lcd “=” : Waitms 180Locate 2 , 3 : Lcd “=” : Waitms 155Locate 2 , 4 : Lcd “=” : Waitms 125Locate 2 , 5 : Lcd “=” : Waitms 90Locate 2 , 6 : Lcd “=” : Waitms 70Locate 2 , 7 : Lcd “=” : Waitms 50Locate 2 , 8 : Lcd “=” : Waitms 35Locate 2 , 9 : Lcd “=” : Waitms 20Locate 2 , 10 : Lcd “=” : Waitms 15Locate 2 , 11 : Lcd “=” : Waitms 13Locate 2 , 12 : Lcd “=” : Waitms 11Locate 2 , 13 : Lcd “=” : Waitms 9Locate 2 , 14 : Lcd “=” : Waitms 7Locate 2 , 15 : Lcd “=” : Waitms 5Locate 2 , 16 : Lcd “=” : Waitms 300Locate 2 , 1 : Lcd “writesucsesfully” : Waitms 500 : Gosub Go:‘——————————————————————————-
Go:
Kp = Kp_epromKi = Ki_epromKd = Kd_epromTs = Ts_epromSp_kecepatan = Sp_kecepatan_epromUp = Up_epromNos = Nos_epromHold = Hold_eprom
Portc.3 = 0 ‘lcd onClsLocate 2 , 5 : Lcd “Ready..!!”Locate 1 , 8 : Lcd “-” : Waitms 150Locate 1 , 8 : Lcd “3″ : Waitms 150Locate 1 , 8 : Lcd “2″ : Waitms 150Locate 1 , 8 : Lcd “1″ : Waitms 150Locate 1 , 8 : Lcd “-” : Waitms 150Cls : Locate 2 , 7 : Lcd “GO…” : Waitms 200 : Cls
Do
Portc.3 = 1 ‘lcd off
Call Baca_sensor()
Sensor = Konversi_sensor
If Sensor = &B00000001 Then : Pv = 20 : End IfIf Sensor = &B00000010 Then : Pv = 11 : End IfIf Sensor = &B00000100 Then : Pv = 5 : End IfIf Sensor = &B00001000 Then : Pv = 0 : End IfIf Sensor = &B00010000 Then : Pv = 0 : End IfIf Sensor = &B00100000 Then : Pv = -5 : End IfIf Sensor = &B01000000 Then : Pv = -11 : End IfIf Sensor = &B10000000 Then : Pv = -20 : End If
If Sensor = &B00000011 Then : Pv = 16 : End IfIf Sensor = &B00000110 Then : Pv = 8 : End IfIf Sensor = &B00001100 Then : Pv = 2 : End IfIf Sensor = &B00011000 Then : Pv = 0 : End IfIf Sensor = &B00110000 Then : Pv = -2 : End IfIf Sensor = &B01100000 Then : Pv = -8 : End IfIf Sensor = &B11000000 Then : Pv = -16 : End If
If Sensor = &B00000111 Then : Pv = 12 : End IfIf Sensor = &B00001110 Then : Pv = 6 : End IfIf Sensor = &B00011100 Then : Pv = 3 : End IfIf Sensor = &B00111000 Then : Pv = -3 : End IfIf Sensor = &B01110000 Then : Pv = -6 : End IfIf Sensor = &B11100000 Then : Pv = -12 : End If
If Sensor = &B00001111 Then : Pv = 8 : End IfIf Sensor = &B00011110 Then : Pv = 2 : End IfIf Sensor = &B00111100 Then : Pv = 0 : End IfIf Sensor = &B01111000 Then : Pv = -2 : End IfIf Sensor = &B11110000 Then : Pv = -8 : End If
If Sensor = &B00000000 ThenPortc.3 = 0 ‘lcd onIf Pv < -10 Then : Pv = -30 : Nos = Up / 2End IfIf Pv > 10 Then : Pv = 30 : Nos = Up / 2End IfEnd If
If Pv < 0 ThenN1 = Up / 2N2 = Nos / 3Nos = N1 + N2End If
If Pv > 0 ThenN1 = Up / 2
N2 = Nos / 3Nos = N1 + N2End If
‘setpoint sensorSp_sensor = 0‘nilai errorError = Sp_sensor – Pv‘proportional controlP = Kp * Error
‘integrativ control
I1 = Error + Last_errorI2 = I1 / 2I = Ki * I2
‘derivative controlD1 = Kd * 10D2 = Error – Last_errorD = D1 * D2
‘error lampauLast_error = Error
‘proportional-derivative controlPd = P + D
‘proportional-integrativ-derivative controlPid = Pd + I
‘===== Hitung Kondisi Pertama=============’sebelah kiriPwm = Sp_kecepatan + PidIf Pwm > 0 Then : Call Kiri_maju() : Locate 2 , 7 : Lcd “F” : End IfIf Pwm > Up Then : Pwm = Up : End IfIf Pwm < 0 Then : Call Kiri_mundur() : Pwm = 0 – Pwm : Locate 2 , 7 : Lcd “R” : End IfIf Pv = 0 Then : Pwm = Nos : If Nos < Up Then : Nos = Nos + 25 : End If : End IfPwm1a = Pwm
‘=====Hitung Kondisi Kedua===============’sebelah kananPwm = Sp_kecepatan – PidIf Pwm > 0 Then : Call Kanan_maju() : Locate 2 , 16 : Lcd “F” : End IfIf Pwm > Up Then : Pwm = Up : End IfIf Pwm < 0 Then : Call Kanan_mundur() : Pwm = 0 – Pwm : Locate 2 , 16 : Lcd “R” : End IfIf Pv = 0 Then : Pwm = Nos : If Nos < Up Then : Nos = Nos + 25 : End If : End IfPwm1b = Pwm
Locate 2 , 1 : Lcd “L:” : Locate 2 , 3 : Lcd “ ” : Locate 2 , 3 : Lcd Pwm1a : Locate 2 , 10 : Lcd “R:” : Locate 2 , 12 : Lcd “ ” : Locate 2 , 12 : Lcd Pwm1bLocate 1 , 11 : Lcd “PV:” : Locate 1 , 14 : Lcd “ ” : Locate 1 , 14 : Lcd Pv
‘selang waktu cuplikWaitms Ts ‘time samplingLoopEnd
Sub Baca_sensor()Start AdcAdc1 = Getadc(0) : Adc1 = Adc1 / 5Adc2 = Getadc(1) : Adc2 = Adc2 / 5Adc3 = Getadc(2) : Adc3 = Adc3 / 5Adc4 = Getadc(3) : Adc4 = Adc4 / 5Adc5 = Getadc(4) : Adc5 = Adc5 / 5Adc6 = Getadc(5) : Adc6 = Adc6 / 5Adc7 = Getadc(6) : Adc7 = Adc7 / 5Adc8 = Getadc(7) : Adc8 = Adc8 / 5If Adc1 < Hold Then : S1 = &B00000000 : B1 = 0 : End If : If Adc1 > Hold Then : S1 = &B00000001 : B1 = 1 : End IfIf Adc2 < Hold Then : S2 = &B00000000 : B2 = 0 : End If : If Adc2 > Hold Then : S2 = &B00000010 : B2 = 1 : End IfIf Adc3 < Hold Then : S3 = &B00000000 : B3 = 0 : End If : If Adc3 > Hold Then : S3 = &B00000100 : B3 = 1 : End IfIf Adc4 < Hold Then : S4 = &B00000000 : B4 = 0 : End If : If Adc4 > Hold Then : S4 = &B00001000 : B4 = 1 : End IfIf Adc5 < Hold Then : S5 = &B00000000 : B5 = 0 : End If : If Adc5 > Hold Then : S5 = &B00010000 : B5 = 1 : End IfIf Adc6 < Hold Then : S6 = &B00000000 : B6 = 0 : End If : If Adc6 > Hold Then : S6 = &B00100000 : B6 = 1 : End IfIf Adc7 < Hold Then : S7 = &B00000000 : B7 = 0 : End If : If Adc7 > Hold Then : S7 = &B01000000 : B7 = 1 : End IfIf Adc8 < 120 Then : S8 = &B00000000 : B8 = 0 : End If : If Adc8 > 120 Then : S8 = &B10000000 : B8 = 1 : End IfS12 = S1 Or S2 : S123 = S12 Or S3 : S1234 = S123 Or S4 : S12345 = S1234 Or S5 : S123456 = S12345 Or S6 : S1234567 = S123456 Or S7 : S12345678 = S1234567 Or S8Konversi_sensor = S12345678 And &B11111111Locate 1 , 1 : Lcd “s”Locate 1 , 2 : Lcd B1Locate 1 , 3 : Lcd B2Locate 1 , 4 : Lcd B3Locate 1 , 5 : Lcd B4Locate 1 , 6 : Lcd B5Locate 1 , 7 : Lcd B6Locate 1 , 8 : Lcd B7
Locate 1 , 9 : Lcd B8End Sub
Sub Kiri_maju()Reset M2aSet M2bEnd Sub
Sub Kanan_maju()Reset M1aSet M1bEnd Sub
Sub Kiri_mundur()Set M2aReset M2bEnd Sub
Sub Kanan_mundur()Set M1aReset M1bEnd Sub
===============================================================
Dan berikut ini sample koding PID line followerku dengan pemrograman CODEVISION AVR :
#include<mega8535.h>#include <delay.h>#define sensor PINB//===pwm motor alias#define motor_ki OCR1A //sip#define motor_ka OCR1B
#define m1a PORTD.0#define m1b PORTD.1#define m2a PORTD.2#define m2b PORTD.3
void maju (){m1a=1;m1b=0;m2a=0;m2b=1;}
void rem_kanan (){m1a=0;m1b=1;m2a=0;m2b=1;}
void rem_kiri (){m1a=1;m1b=0;m2a=1;m2b=0;}
//===Tombol Yang Digunakan======#define sw_ok PINC.3#define sw_cancel PINC.2#define sw_down PINC.1#define sw_up PINC.0
#asm.equ __lcd_port=0x1B ;PORTA#endasm#include <lcd.h>#include <stdio.h>
//====Variabel di eeprom=====eeprom int Kp = 0;eeprom int Ki = 0;eeprom int Kd = 0;eeprom int Ts= 0;eeprom int Upper= 0;eeprom int Lower= 0;eeprom int Set_point=0;//====Nilai-nilai eror=======eeprom int e0= 0;eeprom int e1 = 0;eeprom int e2 = 0;eeprom int e3 = 0;eeprom int e4 = 0;eeprom int e5 = 0;eeprom int e6 = 0;eeprom int e7 = 0;
int error,error1,nil_pid,pwm,kode;char lcd_buff[33];
void seting_awal (){set:Kp=Kp;if(sw_up==0){Kp++;delay_ms(10);}if(sw_down==0){Kp=Kp-1;delay_ms(10);}if(Kp>100){Kp=0;delay_ms(10);}if(Kp<0){Kp=100;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“***************”);lcd_gotoxy(0,1);sprintf(lcd_buff,”Nilai Kp:%i”,Kp);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0){delay_ms(150);goto set1;}else goto set;//====================================set1:Ki=Ki;if(sw_up==0) {Ki++;delay_ms(10);}if(sw_down==0) {Ki=Ki-1;delay_ms(10);}if(Ki>100){Ki=0;delay_ms(10);}if(Ki<0) {Ki=100;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“***************”);lcd_gotoxy(0,1);sprintf(lcd_buff,”Nilai Ki:%i.%i”,Ki/10,Ki%10);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0) {delay_ms(150);goto set2;}if(sw_cancel==0){delay_ms(150);goto set;}elsegoto set1;//======================================set2:Kd=Kd;if(sw_up==0) {Kd++;delay_ms(10);}if(sw_down==0) {Kd=Kd-1;delay_ms(10);}if(Kd>100){Kd=0;delay_ms(10);}if(Kd<0) {Kd=100;delay_ms(10);}lcd_clear();
lcd_gotoxy(0,0);lcd_putsf(“***************”);lcd_gotoxy(0,1);sprintf(lcd_buff,”Nilai Kd:%i”,Kd);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0) {delay_ms(150);goto set3;}if(sw_cancel==0){delay_ms(150);goto set1;}elsegoto set2;//========================================set3:Ts=Ts;if(sw_up==0) {Ts++;delay_ms(10);}if(sw_down==0) {Ts=Ts-1;delay_ms(10);}if(Ts>100){Ts=0;delay_ms(10);}if(Ts<0) {Ts=100;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“Time Sampling”);lcd_gotoxy(0,1);sprintf(lcd_buff,”Delay:%ims”,Ts);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0) {delay_ms(150);goto set4;}if(sw_cancel==0){delay_ms(150);goto set2;}elsegoto set3;//=============================================set4:Ts=Ts;if(sw_up==0) {Set_point++;delay_ms(10);}if(sw_down==0) {Set_point=Set_point-1;delay_ms(10);}if(Set_point>255){Set_point=0;delay_ms(10);}if(Set_point<0) {Set_point=255;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“Set Point”);lcd_gotoxy(0,1);sprintf(lcd_buff,”Kec:%i”,Set_point);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0) {delay_ms(150);goto set5;}if(sw_cancel==0){delay_ms(150);goto set3;}elsegoto set4;
//=================================================set5:Upper=Upper;if(sw_up==0) {Upper++;delay_ms(10);}if(sw_down==0) {Upper=Upper-1;delay_ms(10);}if(Upper>255){Upper=0;delay_ms(10);}if(Upper<0) {Upper=255;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“Limit Speed”);lcd_gotoxy(0,1);sprintf(lcd_buff,”Upper:%i”,Upper);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0) {delay_ms(150);goto set6;}if(sw_cancel==0){delay_ms(150);goto set4;}elsegoto set5;//====================================================set6:Lower=Lower;if(sw_up==0) {Lower++;delay_ms(10);}if(sw_down==0) {Lower=Lower-1;delay_ms(10);}if(Lower>255){Lower=0;delay_ms(10);}if(Lower<0) {Lower=255;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“Limit Speed”);lcd_gotoxy(0,1);sprintf(lcd_buff,”Lower:%i”,Lower);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0) {delay_ms(150);goto set7;}if(sw_cancel==0){delay_ms(150);goto set5;}elsegoto set6;
//========Menampilkan Eror-Eror===============set7:e0=e0;if(sw_up==0) {e0++;delay_ms(10);}if(sw_down==0) {e0=e0-1;delay_ms(10);}if(e0>100){e0=0;delay_ms(10);}if(e0<0) {e0=100;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);
lcd_putsf(“Error 0″);lcd_gotoxy(0,1);sprintf(lcd_buff,”e0:%i”,e0);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0) {delay_ms(150);goto set8;}if(sw_cancel==0){delay_ms(150);goto set6;}elsegoto set7;//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&//========Menampilkan Eror-Eror===============set8:e1=e1;if(sw_up==0) {e1++;delay_ms(10);}if(sw_down==0) {e1=e1-1;delay_ms(10);}if(e1>100){e1=0;delay_ms(10);}if(e1<0) {e1=100;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“Error 1″);lcd_gotoxy(0,1);sprintf(lcd_buff,”e1:%i”,e1);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0) {delay_ms(150);goto set9;}if(sw_cancel==0){delay_ms(150);goto set7;}elsegoto set8;//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&//========Menampilkan Eror-Eror===============set9:e2=e2;if(sw_up==0) {e2++;delay_ms(10);}if(sw_down==0) {e2=e2-1;delay_ms(10);}if(e2>100){e2=0;delay_ms(10);}if(e2<0) {e2=100;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“Error 2″);lcd_gotoxy(0,1);sprintf(lcd_buff,”e2:%i”,e2);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0) {delay_ms(150);goto set10;}if(sw_cancel==0){delay_ms(150);goto set8;}else
goto set9;//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&//========Menampilkan Eror-Eror===============set10:e3=e3;if(sw_up==0) {e3++;delay_ms(10);}if(sw_down==0) {e3=e3-1;delay_ms(10);}if(e3>100){e3=0;delay_ms(10);}if(e3<0) {e3=100;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“Error 3″);lcd_gotoxy(0,1);sprintf(lcd_buff,”e3:%i”,e3);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0) {delay_ms(150);goto set11;}if(sw_cancel==0){delay_ms(150);goto set9;}elsegoto set10;//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&//========Menampilkan Eror-Eror===============set11:e4=e4;if(sw_up==0) {e4++;delay_ms(10);}if(sw_down==0) {e4=e4-1;delay_ms(10);}if(e4>100){e4=0;delay_ms(10);}if(e4<0) {e4=100;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“Error 4″);lcd_gotoxy(0,1);sprintf(lcd_buff,”e4:%i”,e4);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0) {delay_ms(150);goto set12;}if(sw_cancel==0){delay_ms(150);goto set10;}elsegoto set11;//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&//========Menampilkan Eror-Eror===============set12:e5=e5;if(sw_up==0) {e5++;delay_ms(10);}if(sw_down==0) {e5=e5-1;delay_ms(10);}if(e5>100){e5=0;delay_ms(10);}
if(e5<0) {e5=100;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“Error 5″);lcd_gotoxy(0,1);sprintf(lcd_buff,”e5:%i”,e5);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0) {delay_ms(150);goto set13;}if(sw_cancel==0){delay_ms(150);goto set11;}elsegoto set12;//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&//========Menampilkan Eror-Eror===============set13:e6=e6;if(sw_up==0) {e6++;delay_ms(10);}if(sw_down==0) {e6=e6-1;delay_ms(10);}if(e6>100){e6=0;delay_ms(10);}if(e6<0) {e6=100;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“Error 6″);lcd_gotoxy(0,1);sprintf(lcd_buff,”e6:%i”,e6);lcd_puts(lcd_buff);delay_ms(100);if(sw_ok==0) {delay_ms(150);goto set14;}if(sw_cancel==0){delay_ms(150);goto set12;}elsegoto set13;//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&//========Menampilkan Eror-Eror===============set14:e7=e7;if(sw_up==0) {e7++;delay_ms(10);}if(sw_down==0) {e7=e7-1;delay_ms(10);}if(e7>100){e7=0;delay_ms(10);}if(e7<0) {e7=100;delay_ms(10);}lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“Error 7″);lcd_gotoxy(0,1);sprintf(lcd_buff,”e7:%i”,e7);lcd_puts(lcd_buff);delay_ms(100);
if(sw_ok==0) {delay_ms(150);goto Magelang;}if(sw_cancel==0){delay_ms(150);goto set13;}elsegoto set14;//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&Magelang:lcd_clear();lcd_gotoxy(1,0);lcd_putsf(” Save To EEPROM”);delay_ms(200);Kp=Kp;Ki=Ki;Kd=Kd;Ts=Ts;Set_point=Set_point;Lower=Lower;Upper=Upper;e0=e0;e1=e1;e2=e2;e3=e3;e4=e4;e5=e5;e6=e6;e7=e7;
}
void tampilan_awal(){lcd_gotoxy(0,0);lcd_putsf(” C_Tools__! “);delay_ms(1000);lcd_gotoxy(6,1);lcd_putsf(“Present “);delay_ms(1000);lcd_clear();lcd_gotoxy(0,0);lcd_putsf(“Fahmizal_DTE_UGM”);delay_ms(500);lcd_gotoxy(0,1);lcd_putsf(” Line Follower “);delay_ms(1000);lcd_clear();}
//=====Perlihatkan Sensor=========//#define sensor PINA#define s0 PINB.0#define s1 PINB.1#define s2 PINB.2#define s3 PINB.3#define s4 PINB.4#define s5 PINB.5#define s6 PINB.6#define s7 PINB.7
void show_sensor(){
lcd_gotoxy(2,1);if (s7) lcd_putchar(’1′);else lcd_putchar(’0′);if (s6) lcd_putchar(’1′);else lcd_putchar(’0′);if (s5) lcd_putchar(’1′);else lcd_putchar(’0′);if (s4) lcd_putchar(’1′);else lcd_putchar(’0′);if (s3) lcd_putchar(’1′);else lcd_putchar(’0′);if (s2) lcd_putchar(’1′);else lcd_putchar(’0′);if (s1) lcd_putchar(’1′);else lcd_putchar(’0′);if (s0) lcd_putchar(’1′);else lcd_putchar(’0′);}//=====Tutup Perlihatkan Sensor====
void PID (){//***hitam=mati/0 & putih=nyala/1
//=====sensor mengarah ke Kiriif (sensor==0b11101110) {error=e0;}else if (sensor==0b11001110) {error=-e1;}else if (sensor==0b11011110) {error=-e2;}else if (sensor==0b10011110) {error=-e3;}else if (sensor==0b10111110) {error=-e4;}else if (sensor==0b00111110) {error=-e5;}else if (sensor==0b01111110) {error=-e6;kode=12;}
//=====sensor mengarah ke Kananelse if (sensor==0b11101110) {error=e0;}else if (sensor==0b11100110) {error=e1;}else if (sensor==0b11110110) {error=e2;}else if (sensor==0b11110010) {error=e3;}else if (sensor==0b11111010) {error=e4;}else if (sensor==0b11111000) {error=e5;}else if (sensor==0b11111100) {error=e6;kode=24;}
else if (sensor==0b11111110)//di putih semua{if (kode==12){
error=-e7;rem_kiri();delay_ms(20);}else if (kode==24){error=e7;rem_kanan();delay_ms(20);}}
maju();nil_pid=((Kp*error)+((Ki/10)*(error+error1)*Ts)+((Kd/Ts)*(error-error1)));error1=error;
//===== Hitung Kondisi Pertama=============pwm=Set_point-(nil_pid);if(pwm>Upper){pwm=Upper;}if(pwm<Lower){pwm=Lower;}motor_ka=pwm;//=====Hitung Kondisi Kedua===============pwm=Set_point+(nil_pid);if(pwm>Upper){pwm=Upper;}if(pwm<Lower){pwm=Lower;}motor_ki=pwm;//kode=0;delay_ms(Ts);}
void show_detail(){lcd_gotoxy(0,0);sprintf(lcd_buff,”e:%i”,error);lcd_puts(lcd_buff);lcd_gotoxy(5,0);sprintf(lcd_buff,”Ki%i”,motor_ki);lcd_puts(lcd_buff);lcd_gotoxy(11,0);sprintf(lcd_buff,”Ka%i”,motor_ka);lcd_puts(lcd_buff);}void inisialisasi_port(){//===Seting untuk sensor============DDRB=0b00000000;
PORTB=0b11111111;//===Seting untuk fungsi Tombol======DDRC=0×00;PORTC=0xff;//===Seting motor====================DDRD=0xff;PORTD=0×00;//——-inisialisasi PWM—–//TCCR1A=0xA3;TCCR1B=0x0B;TCNT1=0×0000;//PWM 16 bit//OCR1A=0x3FF;OCR1B=0x3FF;TIFR=0;
}void main(void){inisialisasi_port();lcd_init(16);lcd_clear();tampilan_awal();seting_awal();while(1){lcd_clear();PID();show_sensor();kode=0;show_detail();delay_ms(Ts);}}
Nach ni dia hasil tunning pid pada robotku disini….dan disana…
Dan berikut aplikasi kontroler PID pada Robot-Wall Follower ato yg lebih dikenal dengan robot telusur dinding:
Referensi:
http://en.wikipedia.org/wiki/PID_controller http://www.chibots.org/drupal/?q=node/339 http://gedex.web.id/archives/2008/07/09/line-follower-robot-dengan-pid/
