PID Motordrehzahl Frage zum Bascomcode

[schilly]

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:

 '>>> 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:

'###############################################################################
'# 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