PID Motordrehzahl Frage zum Bascomcode
Hallo,
ich habe hier meinen Roby 2.0 schon gut ans laufen gebracht. Alle Sensoren funktionieren. Ziel soll es sein, das er das Zimmer meiner Tochter aufräumt (wenigstens ein paar Objekte erkennt und wegbringt). Darum ist vorne eine Ultraschallsensor für die Objekterkennung vorhanden und hinten eine zwei Finger Gripper mit Sensoren um das Objekt zu greifen. Mit absoluten Werten (hier meine ich fahre x steps vor und drehe x steps usw.) funktioniert das eigendich schon ganz gut.
Bilderlink: http://www.schilly.net/index.php?id=22
Bild hier
Bild hier
Um ganz genaue Positionen anfahren zu können, soll eine PID Regler die beiden Motoren regeln. Hier habe ich mein Problem zum Code.
Der Code ist aus dem Buch "Roboter Selbst bauen" Cybot Pimp.
Ich steuere meine Motoren über PWM in einem Bereich zwischen 300 und 511. Das gebe ich über pwm an....
Ich verstehe in dem Code von Uli Sommer die PID-Regler Steuerung nicht. Der PID Regler ist in einem Timer_IRQ untergebracht. Soweit alles gut.
Frage: Was bedeutet die 512 ?? Siehe Code
Hier die If-Abfrage:
Code:
'>>> vorwärts
If Motor_l > 512 And Motor_r > 512 Then
If Rad_l_save > Rad_r_save Then
Motor_l = Motor_l + Pid
End If
If Rad_l_save < Rad_r_save Then
Motor_r = Motor_r + Pid
End If
End If
Hier alles:
Code:
'###############################################################################
'# Projekt : Cybot Pimp #
'# Controller: Mega32 RN-Control Board #
'# Autor : Sommer Ulli #
'# Homepage : www.sommer-robotics.de #
'# #
'# Funktionen: 1. PID-Regler #
'# Taster 1 = Start #
'# Taster 2 = Stop #
'###############################################################################
'===============================================================================
'***| Mikrocontroller Config |**************************************************
'===============================================================================
'Microcontroller
'================
$regfile = "m32def.dat"
$crystal = 16000000
$baud = 19200
$hwstack = 100
$swstack = 100
$framesize = 100
'ADC einstellen
'===============
Config Adc = Single , Prescaler = Auto , Reference = Internal
Start Adc
'I/O
'======
'Port A als Eingang und Pullup on
Config Porta.7 = Input
Porta.7 = 1
'Zähler
'======
Dim X As Word
'Taster abfrage und Auswetung
'=============================
Declare Function Tastenabfrage() As Byte
Declare Sub _tastenauswertung()
Dim Ton As Integer
Dim Taste As Byte
Dim Tastelast As Byte
Dim Start_flag As Bit
Dim Int_flag As Bit
'Drehzahlmessung
'================
'Timer0 = 100Hz
Config Timer0 = Timer , Prescale = 1024
Const Timervorgabe = 100
On Timer0 Timer_irq
Enable Timer0
'Antrieb
'========
Declare Sub Fast_forward()
Declare Sub Middle_forward()
Declare Sub Slow_forward()
Declare Sub Fast_backward()
Declare Sub Middle_backward()
Declare Sub Slow_backward()
Declare Sub Fast_left()
Declare Sub Middle_left()
Declare Sub Slow_left()
Declare Sub Fast_right()
Declare Sub Middle_right()
Declare Sub Slow_right()
Dim Speed_l_soll As Word
Dim Speed_r_soll As Word
Dim Motor_l As Word
Dim Motor_r As Word
Dim Speed_l As Word
Dim Speed_r As Word
Dim Dir_l As Bit
Dim Dir_r As Bit
Config Pinc.6 = Output 'linker Motor Kanal 1
Config Pinc.7 = Output 'linker Motor Kanal 2
Config Pind.4 = Output 'linker Motor PWM
Config Pinb.0 = Output 'rechter Motor Kanal 1
Config Pinb.1 = Output 'rechter Motor Kanal 2
Config Pind.5 = Output 'rechter Motor PWM
Config Timer1 = Pwm , Pwm = 10 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down
Pwm1a = 0
Pwm1b = 0
Tccr1b = Tccr1b Or &H02
'Endcoder
'=========
Config Pind.2 = Input
Config Pind.3 = Input
Dim Rad_l As Long
Dim Rad_r As Long
Dim Rad_l_save As Long
Dim Rad_r_save As Long
Config Int0 = Falling
Config Int1 = Falling
On Int0 Int0_int
On Int1 Int1_int
Enable Int0
Enable Int1
'Sonar Servo
'============
Config Porta.2 = Output
Declare Sub _servo()
Dim Servo_pos As Word
Dim Servo_pos_save As Word
Dim Multiplikator_servo As Byte
Dim Setup As Word
Multiplikator_servo = 15
Setup = 3900
'PID Regler
'===========
Dim P As Integer
Dim I As Integer
Dim D As Integer
Dim Kp As Integer
Dim Ki As Integer
Dim Kd As Integer
Dim Pid As Integer
Dim Error As Integer
Dim Error_last As Integer
'je niedriger die Geschwindigkeit desto höher die Werte
Kp = 100
Ki = 50
Kd = 10
'Odometrie
'==========
Declare Sub _stop()
Declare Sub _odo_reset()
Dim Odo_l As Long
Dim Odo_r As Long
Const Pi = 3.14159265
Const Tick_l = 2297 'pro Radumdrehung
Const Tick_r = 2291
Const Wheel_base = 10.7
Enable Interrupts
'===============================================================================
'***| Initzialisierung|*********************************************************
'===============================================================================
Akku_leer = 0
Dir_l = 1
Dir_r = 1
Int_flag = 0
Rad_l_save = 0
Rad_r_save = 0
Rad_l = 0
Rad_r = 0
Motor_r = 512
Motor_l = 512
Odo_l = 0
Odo_r = 0
Lcd_counter = 0
For X = 1 To 50
Servo_pos = 128
Call _servo()
Next X
X = 0
'LCD Start Bildschirm
'=====================
Locate 1 , 1 : Lcd "# Cybot - Pimp #"
Locate 2 , 1 : Lcd "#--------------#"
Locate 3 , 1 : Lcd " PID-Regler "
Locate 4 , 1 : Lcd "Sommer Robotics"
'###############################################################################
'*** Haubtprogramm *************************************************************
'###############################################################################
Main:
Do
' Tasterabfrage und Auswertung
'===============================================================================
Call _tastenauswertung()
' Sensor Abfrage
'===============================================================================
Call _betriebsspannung()
Call _srf02_entfernung()
' LCD Ausgabe
'===============================================================================
Incr Lcd_counter
If Lcd_counter > 10 Then
'Locate 1 , 1 : Lcd "SRF02: " ; Entfernung ; " cm "
'Locate 2 , 1 : Lcd "Bat: " ; Fusing(volt , "##.##") ; " Volt "
'Locate 3 , 1 : Lcd "Odo-l: " ; Odo_l ; " "
'Locate 4 , 1 : Lcd "Odo-r: " ; Odo_r ; " "
Lcd_counter = 0
End If
' Jetzt gehts los
'===============================================================================
If Start_flag = 1 Then
Do
Call _srf02_entfernung()
If Entfernung < 30 Then
Call _stop()
Start_flag = 0
Exit Do
End If
Call Slow_forward()
Call _tastenauswertung()
If Taste = 2 Then
Call _stop()
Exit Do
End If
Loop
End If
Loop
End
'###############################################################################
'*** Haubtprogramm Ende ********************************************************
'###############################################################################
'===============================================================================
'***| Fahrbefehle |*************************************************************
'===============================================================================
'>>> vorwärts
Sub Fast_forward()
Motor_l = 1000
Motor_r = 1000
End Sub
Sub Middle_forward()
Motor_l = 800
Motor_r = 800
End Sub
Sub Slow_forward()
Motor_l = 650
Motor_r = 650
End Sub
'>>> rückwärts
Sub Fast_backward()
Motor_l = 50
Motor_r = 50
End Sub
Sub Middle_backward()
Motor_l = 300
Motor_r = 300
End Sub
Sub Slow_backward()
Motor_l = 450
Motor_r = 450
End Sub
'>>> links drehen
Sub Fast_left()
Motor_l = 300
Motor_r = 850
End Sub
Sub Middle_left()
Motor_l = 400
Motor_r = 800
End Sub
Sub Slow_left()
Motor_l = 450
Motor_r = 700
End Sub
'>>> rechts drehen
Sub Fast_right()
Motor_l = 850
Motor_r = 300
End Sub
Sub Middle_right()
Motor_l = 800
Motor_r = 400
End Sub
Sub Slow_right()
Motor_l = 700
Motor_r = 450
End Sub
'===============================================================================
'***| Stop |********************************************************************
'===============================================================================
Sub _stop()
'>>> Motor_l Stop!
Portd.4 = 0
Pwm1a = 0
'>>> Motor_r Stop!
Portd.5 = 0
Pwm1b = 0
End Sub
'===============================================================================
'***| Odometer Reset |**********************************************************
'===============================================================================
Sub _odo_reset()
Odo_l = 0
Odo_r = 0
End Sub
'===============================================================================
'***| Tastenabfrage |***********************************************************
'===============================================================================
Function Tastenabfrage() As Byte
Local Ws As Word
Tastenabfrage = 0
Ws = Getadc(7)
If Ws < 1010 Then
Waitms 10
If Ws < 1000 Then
'T1 = 404
'T2 = 336
'T3 = 265
'T4 = 188
'T5 = 106
Select Case Ws
Case 390 To 420
Tastenabfrage = 1
Case 320 To 350
Tastenabfrage = 2
Case 250 To 280
Tastenabfrage = 3
Case 170 To 200
Tastenabfrage = 4
Case 90 To 130
Tastenabfrage = 5
End Select
End If
End If
End Function
'===============================================================================
'***| Tastenabfrage |***********************************************************
'===============================================================================
Sub _tastenauswertung()
'>>> Tastenfeld abfragen
Taste = Tastenabfrage()
If Taste <> Tastelast And Taste <> 0 Then
Sound Beep , 400 , 500
End If
Tastelast = Taste
'>>> START
If Taste = 1 Then
Start_flag = 1
Odo_l = 0
Odo_r = 0
End If
'>>> STOP
If Taste = 2 Then
Call _stop()
Start_flag = 0
End If
End Sub
'===============================================================================
'***| SRF02 Entfernung |********************************************************
'===============================================================================
Sub _srf02_entfernung()
Local Lob As Byte
Local Hib As Byte
Local Firmware As Byte
Local Temp As Byte
Local Slaveid_read As Byte
Slaveid_read = Slaveid + 1
'Messvorgang in starten
Incr Srf02_counter
If Srf02_counter = 100 Then
I2cstart
I2cwbyte Slaveid
I2cwbyte 0
I2cwbyte 81 'in Zentimetern messen
I2cstop
End If
If Srf02_counter => 200 Then
I2cstart
I2cwbyte Slaveid
I2cwbyte 2
I2cstop
I2cstart
I2cwbyte Slaveid_read
I2crbyte Hib , Ack
I2crbyte Lob , Nack
I2cstop
Entfernung = Makeint(lob , Hib)
Srf02_counter = 0
End If
End Sub
'===============================================================================
'***| Servo |*******************************************************************
'===============================================================================
Sub _servo()
Servo_pos_save = Servo_pos * Multiplikator_servo
Servo_pos_save = Setup + Servo_pos_save
Pulseout Porta , 2 , Servo_pos_save
Waitms 10
End Sub
'===============================================================================
'***| Interrupt Encoder |*******************************************************
'===============================================================================
Int0_int:
Incr Rad_l
Incr Odo_l
Return
Int1_int:
Incr Rad_r
Incr Odo_r
Return
'===============================================================================
'***| Interrupt Timer0 |********************************************************
'===============================================================================
Timer_irq:
Timer0 = Timervorgabe
Rad_l_save = Rad_l
Rad_r_save = Rad_r
Rad_l = 0
Rad_r = 0
'>>> Akku leer Warnung
If Akku_leer = 1 Then
Incr Akku_beep
If Akku_beep > 50 Then
Sound Beep , 400 , 500
Akku_beep = 0
End If
End If
'Start ?
'========
If Start_flag = 1 Then
If Motor_l <> 512 Or Motor_r <> 512 Then
'***| PID Regler |**************************************************************
'>>> Error differenz
Error = Rad_l_save - Rad_r_save
Error = Abs(error)
'>>> P - Anteil
P = Error
P = P * Kp
'>>> I - Anteil
I = I + Ki
I = Error - Error_last
'>>> D - Anteil
D = Error - Error_last
D = D * Kd
'>>> Error last
Error_last = Error
'>>> PID
Pid = P + I
Pid = Pid + D
'>>> rückwärts
If Motor_l < 512 And Motor_r < 512 Then
If Rad_l_save > Rad_r_save Then
Motor_l = Motor_l - Pid
End If
If Rad_l_save < Rad_r_save Then
Motor_r = Motor_r - Pid
End If
End If
'>>> vorwärts
If Motor_l > 512 And Motor_r > 512 Then
If Rad_l_save > Rad_r_save Then
Motor_l = Motor_l + Pid
End If
If Rad_l_save < Rad_r_save Then
Motor_r = Motor_r + Pid
End If
End If
'>>> links drehen
If Motor_l < 512 And Motor_r > 512 Then
If Rad_l_save > Rad_r_save Then
Motor_l = Motor_l - Pid
End If
If Rad_l_save < Rad_r_save Then
Motor_r = Motor_r + Pid
End If
End If
'>>> rechts drehen
If Motor_l > 512 And Motor_r < 512 Then
If Rad_l_save > Rad_r_save Then
Motor_l = Motor_l + Pid
End If
If Rad_l_save < Rad_r_save Then
Motor_r = Motor_r - Pid
End If
End If
'>>> Begrenzung
If Motor_l > 1023 Then Motor_l = 1023
If Motor_l < 1 Then Motor_l = 0
If Motor_r > 1023 Then Motor_r = 1023
If Motor_r < 1 Then Motor_r = 0
'***| Antrieb |*****************************************************************
'>>> Motor_l rückwärts
If Motor_l < 512 Then
Dir_l = 0
Portc.6 = 1
Portc.7 = 0
Portd.4 = 1
Speed_l = Motor_l * 2
Speed_l = 1023 - Speed_l
Pwm1a = Speed_l
End If
'>>> Motor_l vorwärts
If Motor_l > 512 Then
Dir_l = 1
Portc.6 = 0
Portc.7 = 1
Portd.4 = 1
Speed_l = Motor_l * 2
Pwm1a = Speed_l
End If
'>>> Motor_r rückwärts
If Motor_r < 512 Then
Dir_r = 0
Portb.0 = 1
Portb.1 = 0
Portd.5 = 1
Speed_r = Motor_r * 2
Speed_r = 1023 - Speed_r
Pwm1b = Speed_r
End If
'>>> Motor_r vorwärts
If Motor_r > 512 Then
Dir_r = 1
Portb.0 = 0
Portb.1 = 1
Portd.5 = 1
Speed_r = Motor_r * 2
Pwm1b = Speed_r
End If
'>>> Motor_l Stop!
If Motor_l = 512 Then
Portd.4 = 0
Pwm1a = 0
End If
'>>> Motor_r Stop!
If Motor_r = 512 Then
Portd.5 = 0
Pwm1b = 0
End If
End If
End If
Return
Hier wird der Motor_L (links) in einer If-Abfrage abgefragt > oder < usw.
Waum 512?
Ich habe mir das ein paar Tage angeschaut und komme nicht dahinter. Servos sind es ja nicht...
Das ist mein eigendliches Problem...
Gruß Schilly
Re: PID Motordrehzahl Frage zum Bascomcode
Zitat:
Zitat von schilly
... Ich verstehe in dem Code von Uli Sommer die PID-Regler Steuerung nicht ...
Da sind wir schon zwei - wenn das, was Du präsentierst, der besagte Code ist. Wahrscheinlich liegt es aber daran, dass ich bascom garnicht kann - jedenfalls sehe ich da drin nicht wirklich einen PID-Hintergrund.
Schilly, Sportsfreund: wie bist Du zu den Regelparametern Kp, Ki und Kd gekommen? Hmmmmmm - lass mich mal wetten -- Einsatz (noch ein) Dauerlutscher? -- diese Werte hast Du abgeschrieben?! Schau bitte mal hier nach, da ist die Einstellung eines solchen Reglers beschrieben.
Und hier ein Beispiel zur Programmierung, aber (leider eben) in C. Im anderen Thread habe ich ja etwas von Sprungantwort geschrieben, mit der Übung könntest Du Dir die richtigen Paramter für Dein System bestimmen. Wie - das steht im ersten Link (hier, oben) und ein bisschen, glaube ich, irgendwo in einem meiner Threads (sorry, weiß nicht mehr wo).
Zitat:
Zitat von schilly
... Ziel soll es sein, das er das Zimmer meiner Tochter aufräumt ...
Das darf doch nicht wahr sein! Glaubst Du daran? Ist das mit der Tochter abgesprochen? Hmmmm - ok, das geht mich nix an.
Re: PID Motordrehzahl Frage zum Bascomcode
Hallo Oberallgeier,
Zitat:
Zitat von oberallgeier
Schilly, Sportsfreund: wie bist Du zu den Regelparametern Kp, Ki und Kd gekommen? Hmmmmmm - lass mich mal wetten -- Einsatz (noch ein) Dauerlutscher? -- diese Werte hast Du abgeschrieben?!
Also der Code ist komplett aus dem Buch... Die Parameter dementsprechend auch. Ich bin auch noch gar nicht auf die eigendliche PID Regelung eingegangen... Ich fahre hier den pragmatischen Ansatz das als Black-Box so zu nutzen.. Klar muss man sich mit den Parametern bzw. der eigedlichen Regelung auseinandersetzen...
Was ich an dem Code nicht verstehe ist.. Da wird ein Fahrbefehl Motor links 600 motor und Motor rechts 600 abgesetzt. Der Regelalgorithmus wird durchgeführt und dann kommt man in einer If-Abfrage wo Motor_link > 512 etc... reinrutscht... ok.. dementsprechend Drehzahlanpassungen vorgenommen wird...
Warum denn 512? Klar... die Begrenzung der Fahrtbefehle in dem Codezwischen 0-1023.. da ist 512 in der Mitte... aber das ist mein Verständnissproblem.
Der Fartbefehl sagt doch: liebes Motörchen, 600 solls doch sein...
Na, ich werde mir einfach mal Deine Links anschauen...
Vielleicht ist auch jetzt der Zeitpunkt gekommen, doch die Sachen in C anzugehen, aber in bin total begeistert, wie man mit ein paar Befehlen (Bascom) total weit kommt...
Zitat:
Zitat von oberallgeier
Ziel soll es sein, das er das Zimmer meiner Tochter aufräumt ...Das darf doch nicht wahr sein! Glaubst Du daran? Ist das mit der Tochter abgesprochen? Hmmmm - ok, das geht mich nix an.
Hahaha... Ja, ich glaube Sie wartet schon darauf...
Gruß Schilly
