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
