Electronix | GFX | Robotix

Die Software von MARS ist in einem BASIC-Dialekt geschrieben. AVR-Prozessoren können wahlweise in C, Assembler oder Basic programmiert werden. Die Programmiersprachen haben gewisse Vor- und Nachteile. Assembler ist eine sehr hardwarenahe Sprache, jedoch ist der Programmieraufwand extrem hoch. Der Vorteil ist jedoch die vollkommene Kontrolle über den Prozessor. Basic ist das Gegenstück zu Assembler. Basic und dessen Compiler hat schon von Haus aus viele Funktionen eingebaut. So lässt sich ein Text mit nur einer Zeile über die serielle Schnittstelle ausgeben. In Assembler sind es ca. 10 Zeilen. Der BASCOM-Compiler, das ist der Name des Basic-Compilers, versteht auch Assembler, was eine Kombination der zwei Sprachen zulässt. C ist ein Kompromiss zwischen den zwei Sprachen. So lässt C trotz einiger Funktionen eine gewisse Hardwarenähe zu.

1. Die Hauptschleife

Die Hauptschleife ist eine Endlosschleife Sie hat keine Abbruchbedingung und wird immer wieder ausgeführt. In dieser Schleife befinden sich die Sub’s die immer wieder aufgerufen werden.

In der Hauptschleife wird zunächst der Sub “UART” aufgerufen.

Wenn die Variable “aut “den Wert 1 (=True) besitzt kommt der Roboter nun in den autonomen Modus.

Danach werden die Bumper wieder auf 0 zurückgesetzt und die Hauptschleife beginnt von vorne.

1.1 SUB: UART

Dieser Sub-Prozess dient zur Fernsteuerung des Roboters. Das Prinzip ist eigentlich recht einfach.

Mit

Sub UART()
   if ischarwaiting(#2) = 1 Then
      inByte = inkey(#2)

wird der UART-Puffer ausgelesen wenn sich etwas im Puffer befindet. Danach folgen viele Zeilen Code. Diese beinhalten die Kommandos. Zum Beispiel:

      If inByte = 106 Then
         Call Drive(0 , 0)
         wait 20
         Exit Sub
      End If

Wenn Byte 120 übertragen wurde dann bleibe sofort stehen und warte 20 Sekunden.

Das Exit Sub am Ende beendet die Ausführung des Sub’s vorzeitig. Das beschleunigt die Ausführung des Programmes. Da unser Byte ja fix 106 war wird es keine Treffer bei den nächsten if-Abfragen kommen.

Es gibt 3 Arten von Befehlen:

1. Mulit-Level-Befehle (Motoren, Servos usw)

2. Slave-Befehle (Befehle die an die Slave µC weitergeleitet werden)

3. Execute Befehle (Auslesen von Sensoren)

1.1.1 Multilevel Befehle

Wenn man zum Beispiel die Servoposition des vertikalen Kameraschwenkkopfes setzten möchte, dann braucht der µC zwei Parameter:

1. Servonummer

2. Servoposition

Diese Informationen werden mit zwei Befehlen übertragen. Zunächst wird ein Servo selektiert indem man das Byte sendet z.b. Byte 41. Der Roboter setzt dann die Variable “Servonr” auf 1. Dann wird die Servoposition übertragen. Wenn der µC nun wieder in den UART-Sub kommt sieht er, dass die Variable “Servonr” gesetzt ist und setzt das empfangene Byte 1:1 um.

Genauso funktioniert das mit den Motoren

1.1.2 Slave-Befehle

Hier werden die empfangenen Bytes einfach an die Slaves weitergeleitet. Das heißt, dass z.b. Byte 35 für Kamera=AUS steht. Der PC sendet BYTE 35 -> µC empfängt Byte 35 µC weiß, das ist Kamera=AUS sendet Byte 35 an Slave weiter.

1.1.3 Execute Befehle

Execute Befehle werden direkt ausgeführt. So kann man Sensoren auslesen und an der UART-Schnittstelle wieder ausgeben. z.b.:

      If inByte = 118 Then
         outByte = getSens( "IR_BO")
         Print #2 , "#,118," ; outByte ; ",*"
         inByte = 0
         outByte = 0
         Exit Sub
      End If

Die Werte werden im Format:

#,SENSBYTE,SENSVALUE,*

ausgegeben. Das Rautezeichen (#) sagt dem Empfänger, dass jetzt eine Übertragung gestartet wird. SENSBYTE sendet die Sensorkennung. Diese ist identisch mit dem gesendeten Byte. SENSVALUE enthält den Sensorwert. Mit dem Stern * wird die Übertragung beendet. Die Werte sind mit einem Komma (,) getrennt, dies ermöglicht ein leichtes einlesen in ein Array und das nachträgliche Aufspalten.

1.2 Das autonome Fahren oder auch: die KI

Bei einem Roboter besteht die wirkliche Herausforderung darin, den Roboter autonom fahren zu lassen.

Das Funktioniert bei M.A.R.S. wie folgt:

   If aut = 1 Then
      IR_RADAR = Radar_Dist()
      If IR_RADAR > 280 Then Call Ausweichman()
      Kette_Links = GetSpeed( "Left")
      Kette_Rechts = GetSpeed( "Right")
      IR_Boden = getSens( "IR_BO")

      If BumpL = 1 And BumpR = 1 Then Call Ausweichman

      Call Drive(Kette_Links , Kette_Rechts)
      BumpL = 0
      BumpR = 0

   End If

Zunächst wird die Entfernung nach vorne gemessen. Das geschieht indem zunächst die SUB-Routine RADAR_DIST aufgerufen wird. Diese Routine schwenkt den IR-Kopf in die Fahrtrichtung und misst den Abstand. (Genaueres folgt)

Wenn die Entferung einen gewissen Wert unterschreitet wird die Sub-Routine Ausweichman() aufgerufen. Dazu ebenfalls später mehr.

1.2.1 Die Geschwindigkeitsbestimmung

Die Sensoren werden über eine mathematische Funktion mit den Motoren verknüpft. Diese Verknüpfung nennt man auch Fuzzy-Logic („Unscharfe Logik“). Sie steht im Gegensatz zur Boolschen-Logik. Die Boolsche Logik lässt zwei Zustände zu: True oder False, 1 oder 0. Würde man die Steuerung eines Roboters durch eine Boolsche-Logik realisieren ergäbe das eine sehr unruhige und ruckkelige Fahrt, da die Motoren nur den Zustand Vor oder Zurück oder überhaupt nur Vor oder Aus annehmen könnten. Es ist möglich diese Boolsche-Logik zu „glätten“, allerdings erhält man je nach „Glättungsgrad“ sehr viele if-Schleifen, was auch die Reaktionsgeschwindigkeit negativ beeinflusst.

Stattdessen habe ich mir etwas anderes Überlegt. Es gibt grundsätzlich die Möglichkeit die Regelungstechnik anzuwenden. Diese hat vorgefertigte Vorgehensweisen. Jedoch habe ich damals noch nichts davon gewusst und konnte auch daher die Regelungstechnik nicht zu Rate ziehen. Was mir schon bekannt war, war die Fuzzy-Logik. Allerdings ist die Fuzzy-Logik für kleinere Prozessoren nur bedingt geeignet weil der Rechenaufwand recht hoch ist. Außerdem wird ständig mit der Zeit Integriert, was aber sehr Zeitkritisch ist. Der Schwingquarz vom Prozessor ist so gesehen recht ungenau was dann zu Problemen führen kann. So habe ich mir die Frage gestellt wie ich die Sensoren mit den Motoren mathematisch so Verknüpfen kann, dass ich eine Art Fuzzy-Logik erhalte ohne, dass mit der Zeit Probleme auftreten und die errechneten Werte fließend und nicht „kantig“ sind.

Um das zu veranschaulichen ein kleines Beispiel:

MARS hat zwei Ultraschall Entfernungssensoren, die nach Vorne gerichtet sind. Die Boolsche Logik verlangt einen Schwellwert. Die Sensoren reichen von 30cm bis 6m. Die Boolsche Logik würde so aussehen:

Wenn Ultraschall Links ist kleiner oder gleich 50cm, Motor Rechts stopp.

Dies würde in einer extremen Reaktion ausarten, der Roboter dreht sich mit voller Geschwindigkeit nach rechts.

Meine Anforderung besagt aber, dass die Motorgeschwindigkeit in Abhängigkeit der Sensoren geregelt werden soll. Je nähere ein Objekt kommt, desto kleiner ist der Sensorwert und in Folge dessen desto kleiner soll auch die Motordrehzahl werden. Misst also zum Beispiel Ultraschall links, dass ein Objekt 80cm vor dem Roboter ist, muss die rechte Kette leicht langsamer laufen als die Linke damit der Roboter in 80cm die Kollision vermeidet. Habe ich einen Wert von 10cm muss der Roboter scharf nach rechts lenken um die Kollision zu vermeiden.

Zusätzlich kommt noch hinzu, dass ab einer gewissen Wendestelle die Motoren rückwärts laufen sollen, da ab einer gewissen Entfernung kein Kurvenradius mehr klein genug ist und daher eine Drehung auf der Stelle erforderlich ist.

Die letzte Bedingung ist, dass die Werte von -255 (maximal Rückwärts) bis 255 (maximal Vorwärts) sein müssen. Außerdem darf die Wendestelle nicht zu lang sein, so, dass die Werte -180 bis 180 möglichst schnell „überwindet“ werden, da die Motoren erst ab einen Wert von 150 loslaufen.

Wenn man in der Mathematik und der Fuzzy-Logik das Wort extrem oder viel hört verwendet man automatisch Potenzialgleichungen. Die Gleichung y=x² sieht auf den ersten Blick geeignet aus. Die ersten Werte variieren nur schwach, doch je größer x Wert desto größer wird auch y.

Da aber diese Gleichung ausschließlich positive Werte ausgibt und die Funktion nach oben hin nicht beschränkt ist, ist sie ungeeignet.

Mein nächster Tipp war y=x³. Diese Funktion ergibt zwar auch nur positive Werte aber berücksichtigt nicht die den schnellen Umschlag der Werte -180 und 180. Außerdem sind meine Werte, welche die Ultraschallsensoren liefern im Bereich von 27cm-6m. Der Graph sieht daher so aus:

Nach ein wenig überlegen kam ich zu der logistischen Funktion auch S-Kurve oder Sigmoidfunktion genannt.

Die allgemeine Gleichung für eine logistische Funktion ist:

Setzt man da jetzt direkt unsere Werte für X ein, erhält man noch immer nicht das was benötigt ist.

Als erster müssen wir den Wendepunkt bestimmen. Der Wendepunkt ist jener Punkt bei dem die Funktion von Vorwärts auf Rückwärts umschlägt, beziehungsweise der Punkt bei dem die Geschwindigkeit 0 beträgt.

Man sieht, die Funktion ist ideal. Bei einem US-Wert von 0 schlägt die Gleichung um. Egal wie hoch der US-Wert ist, die Geschwindigkeit nie größer als 220 bzw. -220. Auch der Umschlag geschieht sehr schnell.

Der Grund wieso die Funktion eigentlich aus drei Funktionen besteht ist, dass man so den schnellen Umschlag erreicht. Man könnte auch genauso nur die mittlere Funktion nehmen, doch das sieht dann so aus:

Man sieht, dass der Übergang viel langsamer ist. Wenn die Motoren bei einem PWM-Wert von 1 sich schon anfangen zu drehen ist die Funktion noch besser, allerdings muss man bedenken, dass das bei MARS nicht der Fall ist. Die Werte -150 bis 150 ergeben keine Änderung, also 0.

Man hätte einfach sagen können, dass bei diesen „Totwerten“ die Gleichung 180 ist, das ergibt aber eine sehr unschöne Kurve. Daher habe ich einfach eine Parabel genommen und sie in 1. Hautplage mit der Hauptfunktion verbunden.

Man sieht, dass das Maximum bei 220 PWM und das Minimum bei -220 legen. Das hat den Grund, dass ich die Software noch um einen Fahrtregler ergänzen möchte und daher noch Platz zum Regeln brauche.

1.2.2 Function GetSpeed

Da die Werte nicht nur von den Senoren zu OWM Werten umgerechent werden sondern auch noch die seitlichen IR-Sensoren miteinbezgen werden, damit nicht an einer Wand geschliffen wird, müssen auch diese Senoren miteinbezogen werden.

'======================= BERECHNUNG DER KETTENDREHZAHL =========================
Function GetSpeed(byVal Side as String) As Integer
   If Side = "Right" Then                                                                                               'ermitteln der verlangten Seite

      Rechte_entfernung = getSens( "US_R")
      Linke_entfernung = getSens( "US_L")

      Kettenspeedrechts = sigm(Linke_entfernung)

            IR_Seite_links = getSens( "IR_SL")
      If IR_Seite_links <= 240 Then
         Left_error = 0
      Else
         Left_error = IR_Seite_links - Ir_s_threshold
         Left_error = Left_error / 10
         If Kettenspeedrechts < -1 Then Left_error = Left_error * -1
      End If

      If Rechte_entfernung = Linke_entfernung And Rechte_entfernung < 70 THen
         IR_Seite_links = getSens( "IR_SL")
         IR_Seite_Rechts = getSens( "IR_SR")

         Left_error = IR_seite_rechts - IR_Seite_links
         left_error = Left_error / 10
         GetSpeed = Kettenspeedrechts - Left_error
         Exit Sub
      End if
      GetSpeed = Kettenspeedrechts - Left_error
    Exit Sub
    End If

   If Side = "Left" Then
      Rechte_entfernung = getSens( "US_R")
      Linke_entfernung = getSens( "US_L")
      Kettenspeedlinks = sigm(Rechte_entfernung)

      IR_Seite_Rechts = getSens( "IR_SR")
      If IR_Seite_Rechts <= 240 Then
         Right_error = 0
      Else
         Right_error = IR_Seite_Rechts - Ir_s_threshold
         Right_error = Right_error / 10
         If Kettenspeedlinks < -1 Then Right_error = Right_error * -1
      End If

      If Rechte_entfernung = Linke_entfernung And RECHTE_ENTFERNUNG < 70 THen
         IR_Seite_links = getSens( "IR_SL")
         IR_Seite_Rechts = getSens( "IR_SR")

         Right_error = IR_Seite_links - IR_seite_rechts
         Right_error = Left_error / 10
         GetSpeed = Kettenspeedlinks - Right_error
       Exit Sub
      End if
      GetSpeed = Kettenspeedlinks - Right_error
   End If
End Function

'========= Sigmoidfunktion zum Koppeln der Sensoren mit den Motoren ============
Function sigm(byval x As word) As Integer
      buffer1 = x - 70
      buffer1 = buffer1 * -1
      buffer1 = buffer1 / 8
      buffer1 = 2.718281828 ^ buffer1
      buffer1 = 1 + buffer1
      buffer1 = 1 / buffer1
      buffer1 = buffer1 - 0.5
      buffer1 = buffer1 * 440
      If buffer1 => -185 and buffer1 =< 0 Then
         buffer1 = buffer1 ^ 4
         buffer1 = buffer1 / 80000000
         buffer1 = buffer1 + 180
         buffer1 = buffer1 * -1
      End If
      If buffer1 =< 185 and buffer1 >= 0 Then
         buffer1 = buffer1 ^ 4
         buffer1 = buffer1 / 80000000
         buffer1 = buffer1 + 180
      End if
      sigm = int(buffer1)
End Function

Man sieht da auch gleich die S-Funktion in Bascom realisiert.