Genel

Çizgi İzleyen Robot ve Pid Algoritması

Basic kodlarıyla , PID tekniği kullanılarak yazılmış, çizgi izleyen robot programı.Pic ile yapmak için, bazı değerleri ayarlanabilir… (Değerleri düzenlemeniz gereklidir.)

Not: Her yapılan robot için katsayılar farklılık gösterecektir.

// Linefollower Robot coding by d.g.
$regfile = “attiny44.dat”
$crystal = 8000000
$framesize = 32
$hwstack = 64
$swstack = 48

Waitms 100

Dim Diff As Long ‘Difference between target position and measured position
Dim Difflast As Long ‘Previous Difference value
Dim Prop As Single ‘Proportional Value
Dim Integral As Single ‘Integral Value
Dim Deriv As Single ‘Derivative Value
Dim Rate As Single ‘Rate calculation used for Derivative
Dim Control As Single ‘Calculated value used to control steering
Dim Mpos As Long ‘Measured Position
Dim Sensors As Byte ‘Byte used to hold sensor values
Dim I As Long , X As Long , Temp As Single , Mystr As String * 16
Dim Tlong As Long , Myint As Integer

‘Aliases
Speed Alias Pwm1a
Motor Alias Porta.7
Steeringfront Alias Servo(2)
Steeringrear Alias Servo(1)

‘PID Constants
Const Kp = 4.5 ‘Gain factor for proportional
Const Kd = .4 ‘Gain factor for derivitive
Const Ki = .1 ‘Gain Factor for Integral

Const Interval = .01 ‘Not used
Const Tpos = 0 ‘Target position of sensors
Const Upperlimit = 30 ‘Maximum control value
Const Lowerlimit = -30 ‘Minimum control value

‘Other Constants ‘
Const Scenterf = 101
Const Scenterr = 105

Const Fast = 650
Const Medium = 640
Const Slow = 600
Const Nogo = 512

‘Configuration Statements
Config Timer1 = Pwm , Prescale = 1 , Pwm = 10 , Compare A Pwm = Clear Down , Compare B Pwm = Disconnect
Config Servos = 2 , Servo1 = Portb.0 , Servo2 = Portb.1 , Reload = 8

‘Config Ports
Config Porta = Input
Config Porta.6 = Output
Config Porta.7 = Output
Config Portb = Output

‘Turn on Internal Pullups
Porta = &B00011111

‘Turn off Motor
Set Motor
Speed = Nogo

‘Enable everything
Enable Timer1
Enable Interrupts

Steeringfront = Scenterf
Steeringrear = Scenterr

Wait 2

Difflast = 0
Reset Motor
Speed = Medium

Do
Sensors = Not Pina
Sensors = Sensors And &B00011111

Select Case Sensors

Case &B00001 ‘Right most sensor
Mpos = 4
‘Speed = Slow

Case &B00011 ‘Right Sensors
Mpos = 3

Case &B00010 ‘Right Mid sensor
Mpos = 2

Case &B00111 ‘Center Right
Mpos = 1

Case &B00110 ‘Center Right
Mpos = 1

Case &B00100 ‘Centered on the line
Mpos = 0

Case &B01100 ‘Center Left
Mpos = -1

Case &B11100 ‘Center Left
Mpos = -1

Case &B01000 ‘ Left Mid Sensor
Mpos = -2

Case &B11000 ‘ Left sensors
Mpos = -3

Case &B10000 ‘Left most sensor
Mpos = -4

Case &B00000 ‘lost line
If Mpos < 0 Then Mpos = -5 Else Mpos = 5

End Select

Diff = Tpos – Mpos
Prop = Diff * Kp ‘Calculate Proportional

Integral = Integral + Diff ‘Calculate Integral
Integral = Integral * Ki

Rate = Diff – Difflast ‘Calculate Differential
Deriv = Rate * Kd

Difflast = Diff

Control = Prop + Deriv ‘New control value
Control = Control + Integral

If Control > Upperlimit Then Control = Upperlimit ‘Keep within safe range
If Control < Lowerlimit Then Control = Lowerlimit

Temp = Scenterf – Control
Myint = Temp ‘Convert Single to Integer
Steeringfront = Myint
Temp = Scenterr + Control
Myint = Temp
Steeringrear = Myint ‘Convert Single to Integer

Waitms 5 ‘wait 5ms between loops (200 times a second)

Loop

End

Doğucan Güler

Doğucan Güler

2015 Ege Üniversitesi Grafik Tasarımı Bölümü Mezunu. Web Geliştirici, Görsel İletişim Tasarımcı. Amatör Android ve iOS Geliştirici. İstanbul'da yaşıyor. Detaylı Bilgi İçin: DogucanGuler.com 'u ziyaret edebilirsiniz.

Kimler Neler Demiş?

avatar
  Subscribe  
Bildir
)
    is typing...

    Formu doldurarak Hemen Gönder'e tıklayın. 24 Saat İçerisinde Hızlı Dönüş.

    Hemen Gönder Cancel