Hallo Leute,
für mein derzeitiges Projekt (https://www.roboternetz.de/phpBB2/viewtopic.php?t=50525) brauche ich einen PID-Regler. Natürlich habe ich schon einen Code, aber ich glaube, dass ich ein paar Fehler gemacht habe. Mein Anliegen ist, ob ihr mal meinen Code euch anschauen könntet und mir dann sagen könntet, obs so passt oder ob irgendwo grobe fehler enthalten sind! Bitte nicht die Reglerconstanten beachten, das sind nur Probewerte, da ich das ganze nicht am Roboter, sondern an einem normalen Motor probiere. Und es wäre toll, wenn ihr mir sagen könntet, wie ich damit 2 Motoren zum synchronlauf "zwingen" kann. Hier nun der Code:
Code:
$regfile = "m32def.dat"
$crystal = 16000000
$hwstack = 150
$swstack = 150
$framesize = 150
$baud = 19200
Config Portc.0 = Output
Portc.0 = 0
Config Pind.2 = Input
Config Portd.4 = Output
Config Portd.5 = Output
Config Portb.0 = Output
Config Portb.1 = Output
Portb.0 = 1
Portb.1 = 0
Portd.2 = 1
Config Timer1 = Pwm , Pwm = 10 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 8
Pwm1a = 0
Pwm1b = 0
Config Timer2 = Timer , Prescale = 64
Timer2 = 6
On Timer2 Isr_von_timer2
Enable Timer2
Config Int0 = Rising
On Int0 Isr_von_int0
Enable Int0
Dim Abtastrate_flag As Byte
Dim Drehzahl_links As Word
'3
'10
'70
Const Kp = 1.2 '0.5
Const Ki = 0 '0.6
Const Kd = 0
Dim Esum As Single
Dim E As Single
Dim Ta As Single
Dim Ealt As Single
Dim Soll As Word
Dim Ist As Word
Dim Kp_buffer As Single
Dim Ki_buffer As Single
Dim Kd_buffer As Single
Dim Kpid_buffer As Single
'Dim Y As Word
Dim Y As Integer
Ta = 0.1 'ta = 0.1 --> 10 mal pro sek abtasten
Soll = 250
Print "Let's go"
Print
Wait 1
Portc.0 = 1
Pwm1a = 400
Enable Interrupts
Waitms 500
Do
E = Soll - Ist
'Esum = Esum * 0.95
Esum = Esum + E
Kp_buffer = Kp * E
Ki_buffer = Ki * Ta
Ki_buffer = Ki_buffer * Esum
Kd_buffer = E - Ealt
Kd_buffer = Kd_buffer / Ta
Kd_buffer = Kd_buffer * Kd
Kpid_buffer = Kp_buffer + Ki_buffer
Kpid_buffer = Kpid_buffer + Kd_buffer
Ealt = E
'Pwm1a = Pwm1a * Kpid_buffer
'Y = Kpid_buffer
If Kpid_buffer <= 0 Then
Y = Abs(kpid_buffer)
Pwm1a = Pwm1a - Y
Elseif Kpid_buffer >= 32000 Then
Y = 32000
Pwm1a = Pwm1a + Y
Else
Y = Kpid_buffer
Pwm1a = Pwm1a + Y
End If
'Pwm1a = Pwm1a + Y
'Pwm1a = Pwm1a + Kpid_buffer
'
Print "Ist = " ; Ist
Print "Soll = " ; Soll
Print "PWM1A = " ; Pwm1a
Print "kp_buffer = " ; Kp_buffer
Print "ki_buffer = " ; Ki_buffer
Print "kd_buffer = " ; Kd_buffer
Print "kpid_buffer = " ; Kpid_buffer
Print "Y = " ; Y
Print
'
Loop
End
Isr_von_timer2:
Timer2 = 6
Incr Abtastrate_flag
If Abtastrate_flag = 100 Then
Abtastrate_flag = 0
Ist = Drehzahl_links
Drehzahl_links = 0
End If
Return
Isr_von_int0:
Incr Drehzahl_links
Return
Vielen Dank schonmal!
Gruß
Chris
Berechtigungen
Zitieren
Lesezeichen