Projekt Mähroboter

[Feuerring]

Hallo zusammen,

würde gerne mein Projekt Mähroboter ( Planung und Umsetzung ) hier vorstellen, derzeit bin ich noch in der Planungsphase ...

Habe in den letzten Wochen viel Zeit damit verbracht, die Projekte von den Pionieren hier im Forum und im RN-Wissen zu studieren.

Zu meinem Projekt:

- Mähroboter

Anhang 31764 Anhang 31763
Um möglichst gut in die Ecken zu kommen haben ich mich entschieden vorn zwei Mähwerke zu setzen,
dazwischen ein Servo-gelegtes Rad 100 mm, hinten ein drittes Mähwerk.
Als antrieb kommen hinten zwei 260 mm Räder, die getrennt angetrieben werden.
Je nach Einschlag der Lenkrolle werden die Antriebsmotoren unterstützend verschieden stark angetrieben.
Die drei Mähwerke sitzen auf einer Trägerplatte, diese kann zur Grundplatte in verschieden höhen arretiert werden.
Die Akkus kommen hinten unter die Grundplatte bei der Achse, um den Schwerpunkt möglichst tief zu halten ...

- Schleifen-Begrenzung

In Anlehnung an der Schaltung Begrenzungsschleife_-_Induktionsschleife
habe ich mir folgendes Überlegt:

Ein Netzteil lädt über einen Widerstand ein Hochstrom-Kondensator, dieser wird über einen MOS-FET an die Schleife gelegt,
je nach länge (ca. 100m) und Durchmesser (1-1,5 mm²) soll ein Strom (Piek) von 10-20 Ampere durchfieseln,
durch die Entladung des Kondensator aber rech schnell dann zusammenbrechen und nicht stetig anliegen,
ich möchte damit die Induktive Rückenbespannung in der Empfängespule minimieren.

- Positionserkennung auf der Begrenzungs-Schleife
Ich möchte an /auf der Schleife Fix-Punkte positionieren, die eine Orientierung des Bot zu erleichtern.
eine erste Überlegung war an wichtigen Poritionen IR-Sender in den Boden einzulassen und am Bot-Boden ein IR-Empfänger.
Eine andere Überlegung ist es den Induktions-Draht mit zwei-drei Windungen 90° dreht zur Induktionsschleife darunter zu positionieren,
ein waagerecht dazu aufgerichteter Empfänger(Spule) kann so beim Überfahren dessen Ausrichtung ermitteln ...
Der Bot bekommt 6 bzw 7 Empfänger(Spulen) für die Induktionsschleife 2x vorn, 2x Mitte und 2x hinten und eine in der Mitte(waagerecht ausgerechtet) um die Fix-Punkte zu erkennen.
Die 6 Empfänger an den Ecken / Mitte sollen nicht nur detektieren ob der Empfänger(Spule) innerhalb oder außerhalb sich der Schleife befinden, sondern bei der Schleifen-Fahrt auch den Draht genau mittig durch die Feldstärke ermitteln / positionieren.


- Funktionen
Der Bot bekommt eine Funkstrecke zu einem PC (Server 24x7) um seine Aktivitäten / Positionen zu übermitteln, von dort bekommt er auch seine Befehle, wie dieser wo und wie zu mähen hat (Mähprogramme)
Der ganze Bot bekommt eine Gummi-Puffer gelagerte Haube die knapp bis zum Boden reicht, um ein anstoßen ringsum bzw. ein anheben zu detektieren.

Die Ladestation wird durch ein FIX-Punkt auf der Begrenzungs-Schleife angezeigt und soll dann Rückwarts angefahren werden ...

Das ganze soll ein Winter-Projekt werden und im nächsten Jahr(Sommer) seine Dienst leisten ...

Gruß Ralf

- - - Aktualisiert - - -

hier noch mal die Bilder ...

[Rabenauge]

Erster Kritikpunkt: das Dreirad.
Das Ding wird sich in jedem Mauseloch recht zuverlässig fest fahren.
Bei anderen Unebenheiten hast du dann öfter mal die Messer im Dreck- ich hab _einmal_ mit nem dreirädrigen Mäher arbeiten "dürfen"- nach ner Stunde war ich mit dem Ding durch.

[Feuerring]

Hallo Sly,

in der ersten Planung habe ich ein 100 mm Rad mit entsprechender Breite 3-4 cm vorgesehen, irgendwas mit Vollgummi und Profil, soll ja auch zum lenken verwendet werden.

wenn ich vorn zwei Lenkräder verwendet Zeichnung (3) verliere ich die Möglichkeit beim mähen vernünftig in die Ecke zu kommen ...
mein Rasen ist bereits mit einer 10cm breiten Mähkante eingefasst ... durch den Schleifen-Draht und einer Rand-Mäh-Funktion möchte ich mir ein nachschneider der Ränder ersparen.

In der Zeichnung (2) habe ich meinen ersten Entwurf Zeichnung (1) noch mal angepasst, grösseres Lenkrad (Mitte) ca. 140 und zwei kleinere Mähwerke vorn,
das Mähwerk in der Mitte muss dann aber grösser werden ... die Mähwerke kommen auf eine eigene Trägerplatte, so kann ich auch später noch Anpassungen vornehmen,
ohne gleich den ganzen Bot neubauen zu müssen, er Bot wird erstmal ohne Mähwerk / Trägerplatte aufgebaut ohne die aber aus den Augen zu verlieren.
Als erstes muss der Bot vernünftig fahren und die Schleife (Begrenzung) erkennen und auch dieser optimal folgen können ...



Bin auch am Überlegen das nicht angetriebene Lenkrad als Referenz-Rad für Wegstrecken Ermittlung mit zu nehmen.

[Feuerring]

Bin gerade dabei das Lenkrad und dessen Lenk-Antrieb zu planen ...

es gibt mehrere Möglichkeiten:

1. DC-Schnecken-Getriebe-Motor und ein Poti um die Position des Rades zu ermitteln.
2. Schritt-Motor mit Planeten-Getriebe und einem RNST01-I2C / L289 Treiber.

Ich benötige einen Lenkwinkel von 180° (+/- 90°) während der Fahr sollte so gut wie keine Kräfte auf den Lenk-antrieb wirken.
Die Stellzeiten bei 180° werde ich mit max 2 Sekunden ansetzen / 30-40 RPM am Getriebeausgang.

Bin gerade noch die vor und nachteile abzuwiegen und dabei noch unschlüssig ...

Vor und Nachteile:
Lösung (1a) DC-Motor mit Poti und einem M51660L als PWM-Positions-Treiber ... bin mir aber nicht sicher wie die PWM-Auflösung für ein Servo dann damit ist.
Lösung (1b) DC-Motor mit Poti und einem MC33030 als -Treiber der vergleicht zwei-Spannungen(Poti und z.B. ein DA-Ausgang) und stellt danach den Motor über eine H-Brücke
Lösung (1C) DC-Motor mit Poti direkt von der CPU über eine H-Brücke gesteuert und einem analogen Eingang um das Poti abzufragen

Zu Lösung(2) exakte Positionierung, benötigt aber immer einen Haltestrom und muss auch immer mal wieder per Endschalter justiert werden, wegen möglicher Schrittverluste ...

Favorisiere derzeit noch 1b

[damfino]

Bei dem Design hast das meiste Gewicht auf dem Lenkrad. Das wird bremsen, einsinken, und die Antriebräder drehen durch.

Absolut kein Schutz vor den Messern möglich, wenn der dir über die Zehen fährt macht er sofort Hackfleisch.
Bei Robotern sollte man mehr Sicherheit als bei normalen Rasenmähern vorsehen, man weiß nie was zB Besucher machen.

Zum lenken nimm doch ein Modellbauservo...

[Feuerring]

Hallo damfino,

besten Dank für die Anregungen ...

Bei dem Design hast das meiste Gewicht auf dem Lenkrad. Das wird bremsen, einsinken, und die Antriebräder drehen durch.

hinten zwischen den Rädern kommen die Akkus, die kann ich so verschieben, so das das Lenkrad optimal belastet wird, ein gewisser Druck ist aber auch von Nöten.

Absolut kein Schutz vor den Messern möglich, wenn der dir über die Zehen fährt macht er sofort Hackfleisch.

Das ist natürlich klar, um die Mähwerke hatte ich vor eine Reihe von langen 4M Schrauben an der Mäh-Trägerplatte zu schrauben ... so eine Art von Kamm,
so lang das die Messer dahinter laufen, der ganze Bot bekommt ein Cover, der bis knapp über den Boden reicht und auf Verschiebungen zur Grundplatte reagiert.

Also auf anstoßen an allen Seiten, anheben oder auffahren reagiert ...

Zum lenken nimm doch ein Modellbauservo

Darüber habe ich schon nachgedacht, möchte aber an der Achse, das Lenkrad direkt befestigen, dafür sind diese Servos nicht ausgelegt ...


Üpdate ...

habe mir heute Gedanken über den Antrieb gemacht ...


Hatte erst für die Hinterräder einen Scheibenwischer-Motor mit Schnecken-Getriebe in betracht gezogen,
hat aber nachteile in der symmetrischen Befestigung, desweiteren müssen die beiden Motore immer in entgegen gesetzter Richtung laufen und das tuen Sie nun mal mit unterschiedlicher Leistung ...

Nach längerem suchen habe ich einen Faulhaber-Motor mit Getriebe und Encoder (Servo-Motor) gefunden.
Jeder der Beiden Motoren hat 12V und 17W mit 120 U/min , der Inkrementale AB-Drehgeber liefert dabei 768 CPR an der Antriebswelle.
Die Räder(26cm) werden über eine Kette mit einer Untersetzung von 40/8 angetrieben ...

So ist bei 100% Motor-Leistung eine Geschwindigkeit von 19,6 Meter / Minute bei einem Drehmoment von ca. 8,5 Nm möglich.
Die Geber-Auflösung ergibt sich dabei mit 4706 Impulse / Meter.
Die Inkremental-Drehgeber will ich zu einem für die Wegstrecken Ermittlung verwenden und eins der beiden Signale um die Drehzahl des jeweiligen Motor zu regeln ...

Das Lenkrad 120-125 mm bekommt noch einen zusätzlichen Inkrementalen AB-Drehgeber mit einer Auflösung von 400 CPR was auf einen Meter dann 1062 Impulse / Meter ergibt und so unabhängig vom Antrieb funktioniert.

[Feuerring]

Update ...

habe gestern mit mehreren Arduino(Nano) als I2C-Slave und einem Arduino(Nano) als I2C-Master verscheide Situationen durchgespielt,
um zu sehen wie sicher der I2C-BUS ist.
Dabei habe ich festgestellt, das ich was wichtiges übersehen habe, ein abgeschaltetes Modul zieht über die Schutzdioden des 329p den I2C-BUS (SDA/SCL) runter,
bedeutet ich muss diese PINs sowie die Not-AUS-Leitung beim deaktivieren des Modul von der BUS-Platine trennen, dazu werde ich die Level-Shift-Schaltung von NXP nehmen
die Gates der drei MOSFET (IRLML2402) werden dann mit über den BUS-Anschluss POWER(EN) im jeweiligem Modul mit geschaltet geschaltet.

Damit der Master handlungsfähig bleibt bekommt er einen Festen Platz auf der Bus-Platine, diese Slot kann über eine direkte Steuerleitung vom restlichen TWI-Bus getrennt werden,
die beiden "PCF-8574" sind da aber von nicht betroffen und bleiben mit dem Master direkt verbunden.

Sollte es zu einer Störung auf dem I2C-Bus kommen, wird über direkt gestaltete Leitung (NOT-AUS) alle Aktoren erst mal abgeschaltet,
dann kann der Master, das oder die störenden Module abschalten (neu-starten) ...

Auch musste ich festgestellten, das die Bascom Befehle ( I2creceive / I2csend ) mir mehrmals die Hauptschleife do / Loop zum stehen gebracht haben, ohne das eine Aktivität auf dem BUS vorlag ...
werde deswegen den I2C-Master ebenfalls direkt über die TWI-Register ansprechen, so wie ich es bei den TWI-Slave auch schon umgesetzt habe,
werde mich dabei an der hervorragenden Beschreibung TWI-Praxis orientieren.

So kann ich besser auf Fehler-Situationen reagieren und auch selber Timeouts festlegen ... Das Projekt mit den aufgeteilten Funktionen über Module, Fällt oder Steht mit der sicheren Kommunikation der Module ...

[Feuerring]

Update ...

habe angefangen die BUS-Platine aufzubauen,
auf der Platine gibt es zwei PCF-8574,
einmal adr(64) Input für Ready-Meldungen und adr(66) Output für Power-Abschaltung von jedem Modul.

Bei jeder Modul kann über zwei Kommandos 21=Ready-HIGH / 20=Ready-LOW die Ready-Leitung direkt gesteuert werde,
so kann das Master-Modul erkennen, auf welchem Steckplatz welches Modul(adr) sich befindet.



Auf der Bus-Platine gibt es noch zwei Spannungs_Regler (LM317) einer für 5V (Dauer) und einer für 8,5V(Vin-Nano)

Zum testen der TWI (I2C) verbindung habe ich erst mal ein Master mit einem einfach Programm gesteckt,
dieser Master setzt RS232-Kommandos als I2C-Master um und sendet eingelesenen I2C-Daten über RS232 zurück.

$regfile = "m328pdef.dat"$crystal = 16000000
$hwstack = 100
$swstack = 100
$framesize = 100
$baud = 19200


' ------------ DIM ------------------------
Dim X As Byte
Dim T1x As Byte                                             ' Zähler 1/1000
Dim T2x As Byte                                             ' Zähler 1/100
Dim T3x As Byte                                             ' Zähler 1/10


' ------------ I2C-Master  ------------------------
$lib "i2c_twi.lbx"
Config Scl = Portc.5
Config Sda = Portc.4
I2cinit
Config Twi = 100000
Dim Tw_buf(20)as Byte
Dim Tw_x As Byte
Dim Tw_s As Byte
Dim Tw_n As Byte
Dim Xout As String * 250


' ------------ RS232 - Input ------------------------
Dim Na As String * 30
Dim Nax As String * 30
Dim Si As Byte
Config Serialin = Buffered , Size = 40


' ------ Split -------
Dim Sb As Byte                                              ' Split_Zähler
Dim Sbx(20) As String * 10                                  ' Split_Array


' ------------ Timer_0 Zeitvorgabe ------------------------------
Config Timer0 = Timer , Prescale = 256
On Ovf0 Timer0_ovf
Enable Timer0


Enable Interrupts


Print "Start_Main"


Do
   If Ischarwaiting() <> 0 Then
      Si = Inkey()
      If Si <> 13 And Si <> 10 And Len(na) < 30 Then
         Na = Na + Chr(si)
      Else
         Nax = Ucase(na)
         Sb = Split(nax , Sbx(1) , "|")
         If Sbx(1) = "STATUS" Then
            Print "ACK|STATUS"
            Print "Status|"
         Elseif Sbx(1) = "TW_W" Then
            ' ------------ TW zum slave senden ----
            Print "ACK|" ; Nax
            If Sb > 2 Then
               Tw_s = Val(sbx(2) )
               Tw_n = Sb - 2
               For X = 3 To Sb
                    Tw_buf(x -2) = Val(sbx(x) )
               Next X
               I2csend Tw_s , Tw_buf(1) , Tw_n
            Else
               Print "ERROR"
            End If
         Elseif Sbx(1) = "TW_R" Then
            ' ------------ TW vom slave lesen ----
            Print "ACK|" ; Nax
            If Sb = 3 Then
               Tw_s = Val(sbx(2) )
               Tw_n = Val(sbx(3) )
               I2creceive Tw_s , Tw_buf(1) , 0 , Tw_n
               Xout = "TW_R|DATA"
               For X = 1 To Tw_n
                  Xout = Xout + "|" + Str(tw_buf(x))
               Next X
               Print Xout
            Else
               Print "ERROR"
            End If
         Else
            Print "NACK"
         End If
         Na = ""
      End If
   End If
Loop


Timer0_ovf:
   ' ---- Timer_0 ----- Interrupt alle 1 mS ----------------
   Timer0 = 194                                             ' Resrwert = 61
   Incr T1x
   If T1x > 9 Then
      ' -------- Zeitbasis 10 mS ------------------
      Incr T2x
      T1x = 0
      If T2x = 8 Then Portb.5 = 0                           ' Herzschlag-LED (ein)
      If T2x > 9 Then
      ' -------- Zeitbasis 100 mS ------------------
         Incr T3x
         T2x = 0
         If T3x > 9 Then
            T3x = 0
            ' -------------  Zeitbasis 1 Sec -------------------
            ' Print "Status "
            Portb.5 = 1                                     ' Herzschlag-LED (aus)
         End If
      End If
   End If
Return
Return

Hier kann man die Kommandos am PC-Programm(RS232) und die eingelesenen Daten sehen

[Feuerring]

Update ...

bin einen guten Schritt weiter gekommen ...

Das Antriebs-Modul ist weitgehend fertig, Das Modul wird wie alle anderen Module dann auf eine BUS-Platine gesteckt,
dadurch kann ich flexibel auch spätere Erweiterungen nachrüsten.



Meine Test-Umgebung:


Das Modul steuere ich derzeit über einen IO-Warrior (I2C) und dem Programm All-In-One,
zur Auswertung von Parametern nehme ich die RS232-Schnittstelle und einem einfachen eigenen PC-Programm.

Das Antriebs-Modul steuert die Motoren über Treiber-Module per PWM/DIR/Brake,
die Drehzahl und Drehrichtung der beiden Motore wird über die Tacho-Einheit ausgewertet.

Dank der Hilfe im Bascom-Forum, konnte ich recht schnell die TWI-Hardware-Lösung ohne LIB umsetzen ... siehe hier

Das Modul wird wie folgt über TWI(I2C) angesprochen

schreiben:
ADR|Mode|W1|W2|Wn
lesen:
ADR|Mode|R1|R2|Rn

Funktionen
Mode=0 ( schnelles bremsen ) ... funktioniert sicher
schreiben:
ADR|00 ... beide Motoren fahren schnell Ihre Drehzahl runter
lesen:
ADR|00|FF ... keine Daten außer Mode, Rest wir mit FF gefüllt

Mode=1 ( Speed ) ... funktioniert sicher
schreiben:
ADR|01|DR|V0|V1 ... beide Motoren werden beschleunigt bzw. abgebremst um auf die vorgegeben Drehzahl zu kommen und halten diese konstant.
lesen:
ADR|01|DR|V0|V1|P0|P1
DR = Drehrichtung 1= Motor_0 rückwärts / 2=Motor_1 rückwärts / 0= vorwärts beide / 3= rückwärts beide ... kann nur bei stehenden Motoren geändert werden
V0 = Drehzahl-Vorgabe Motor_0 0=Null / 150 = 7500 U/min ( Werte für die Faulhaber-Motoren)
V1 = Drehzahl-Vorgabe Motor_1 0=Null / 150 = 7500 U/min ( Werte für die Faulhaber-Motoren)
P0 = PWM-Wert Motor_0 ( nur lesen )
P1 = PWM-Wert Motor_1 ( nur lesen )

Mode=2 ( Strecke) .... noch nicht implementiert ... eine Wegstrecke wird mit Vmax gefahren ... auch Kurven oder drehen ist möglich.
schreiben:
ADR|02|DR|VM|S0|S0|S1|S1
lesen:
ADR|02|DR|VM|V0|V1|P0|P1

DR = Drehrichtung 1= Motor_0 rückwärts / 2=Motor_1 rückwärts / 0= vorwärts beide / 3= rückwärts beide
VM = Vmax
S0 = Strecke für Motor_0
S1 = Strecke für Motor_1

.....
zischen den Mode-1 und Mode-2 kann nicht direkt umgeschaltet werden nur über Mode-0 ... je nach Mode kann auch nur die dafür vorgegeben Werte gelesen werden.

ist das Modul am beschleunigen oder bremsen oder fährt die vorgegeben Wegstrecke wird die Leitung Ready auf LOW gelegt,
so muss der Master nicht immer über I2C pollen.

Wird die NOT-AUS-Leitung (BUS) auf LOW gelegt werden die Motoren über die Gatter sofort deaktiviert und das Modul fährt die Werte intern runter.

-----

derzeitiger Code:

'  Projekt Mäh-Roboter
'  R. Greinert
'  (CC) 07 / 2016 
'
'  Antrieb_Steuerung  Ver.008
'
'
' ------------ Bascom - Parameter ------------------------
$regfile = "m328pdef.dat"
$crystal = 16000000
$hwstack = 100
$swstack = 100
$framesize = 100
$baud = 19200


Print "Start_Main"


' ------------ Timer_0 Zeitvorgabe ------------------------------
Config Timer0 = Timer , Prescale = 256
On Ovf0 Timer0_ovf
Enable Timer0


' ------------ Output ------------------------------
Config Portb.0 = Output                                     ' Ready-Leitung
Config Portb.3 = Output                                     ' DIR Motor_0
Config Portb.4 = Output                                     ' DIR Motor_1
Config Portb.5 = Output                                     ' Herzschlag
Config Portd.4 = Output                                     ' Break Motor_0
Config Portd.5 = Output                                     ' Break Motor_1
Config Portd.7 = Output                                     ' I2C-Data-Anzeige


Portb.0 = 0                                                 ' NOT-Ready = LOW
Portb.3 = 0                                                 ' DIR Motor_0   0=vorwärts 1=Rückwärts
Portb.4 = 0                                                 ' DIR Motor_1   0=vorwärts 1=Rückwärts
Portd.4 = 0                                                 ' Break Motor_0 low-aktiv
Portd.5 = 0                                                 ' Break Motor_1 low-aktiv
Portd.7 = 1                                                 ' I2C-LED aus


' ------------ Input ------------------------------
Config Pinc.2 = Input                                       ' Motor_0 Decoder Q2
Config Pinc.3 = Input                                       ' Motor_1 Decoder Q2
Config Pinc.0 = Input                                       ' Abfrage NOT-AUS LOW-Aktiv
Config Pinc.1 = Input                                       ' Abfrage RS232-Enabled


' ------------ PWM  ----------------------
Config Portb.1 = Output                                     ' PWM Motor_0
Config Portb.2 = Output                                     ' PWM Motor_1
Portb.1 = 0
Portb.2 = 0
Config Timer1 = Pwm , Pwm = 8 , Compare_a_pwm = Clear_up , Compare_b_pwm = Clear_up , Prescale = 64


' ------------ RS232 - Input ------------------------
Config Serialin = Buffered , Size = 40


' ------------ I2C-Slave  ------------------------
Dim Twi_status As Byte
Dim Twi_control As Byte
Dim Twi_dn As Byte


Twsr = 0
Twdr = 255
Twar = 180
Twcr = &B11000100


' ------------ DIM ------------------------
Dim Na As String * 30                                       ' RS232 Input-String
Dim Nax As String * 30                                      ' RS232 Input-UCASE-String
Dim Si As Byte                                              ' RS232 Input Zeichen
Dim Z0 As Integer                                           ' Zähler Motor_0
Dim Z1 As Integer                                           ' Zähler Motor_1
Dim Z0x As Word                                             ' Zähler cache Motor_0
Dim Z1x As Word                                             ' Zähler cache Motor_1
Dim Tx As Byte                                              ' Zähler ms
Dim Tmx As Byte                                             ' Zähler sec
Dim P0 As Byte                                              ' PWM_Motor_0 Stellwert
Dim P1 As Byte                                              ' PWM_Motor_1 Stellwert
Dim Px As Byte                                              ' Temp-Wert Stellwert_diff soll
Dim Pt As Word                                              ' Temp-wert Stellwert_diff Ist
Dim Vx As Integer                                           ' Temp-Wert Drehzahl_Diff
Dim V0 As Byte                                              ' Vorgabe Drehzahl Motor_0
Dim V1 As Byte                                              ' Vorgabe Drehzahl Motor_1
Dim R_0 As Byte                                             ' Vorgabe Richtung Motor_0
Dim R_1 As Byte                                             ' Vorgabe Richtung Motor_1
Dim Sx As Boolean                                           ' Ready 1=fertig 0=atwork
Dim S0 As Long                                              ' Streckenzähler Motor_0
Dim S1 As Long                                              ' Streckenzähler Motor_1
Dim Xmode As Byte                                           ' Funktion-Mode 0=Stop 1=Speed 2=Strecke
Dim Xt1 As Byte                                             ' temp1
' ------ Split -------
Dim Sb As Byte                                              ' Split_Zähler
Dim Sbx(5) As String * 10                                   ' Split_Array


' -------------- Declare Sub ----------------




' -------------- Interupt-Eingänge ----------------
Config Int0 = Falling                                       ' Motor_0 Decoder Q1
Config Int1 = Falling                                       ' Motor_1 Decoder Q1
Enable Int0
Enable Int1
On Int0 Int0_sub
On Int1 Int1_sub


Enable Interrupts


Print "Start_Hauptschleife"
Do
    ' --------------- Hauptschleife ------------------
    If Ischarwaiting() <> 0 Then
      Si = Inkey()
      If Pinc.1 = 0 Then                                    ' abfrage ob RS232 enabled
         If Si <> 13 And Si <> 10 And Len(na) < 30 Then
            Na = Na + Chr(si)
         Else
            Nax = Ucase(na)
            Sb = Split(nax , Sbx(1) , "|")
            Print Sb
            If Nax = "STATUS" Then
               Print "ACK|STATUS"
               Print "Status|Z0x=" ; Z0x ; "|Z1x=" ; Z1x


            Elseif Nax = "STOP" Then
               Xmode = 0
               Print "ACK|STOP"
            Elseif Left(nax , 5) = "SPEED" Then
               If Sb = 5 Then
                  Print "ACK|SPEED"
                  Xmode = 1
                  R_0 = Val(sbx(2) )
                  V0 = Val(sbx(3) )
                  R_1 = Val(sbx(4) )
                  V1 = Val(sbx(5) )
               Else
                   Print "NAK|" ; Na
               End If
            Else
               Print "NAK|" ; Na
            End If
            Na = ""
         End If
      End If
   End If


   ' ----------------- TWI ------------------
   Twi_control = Twcr And &H80                              ' Bit7 von Controlregister
   If Twi_control = &H80 Then
      ' TWI Transfer
      Portd.7 = 0
      Twi_status = Twsr




         If Twi_status = &H80 Or Twi_status = &H88 Then
            ' -------- Daten von TWI erhalten
            If Twi_dn = 0 Then
               If Twdr = 1 Then
                  Xmode = 1
               Elseif Twdr = 2 Then
                  Xmode = 2
               Else
                  Xmode = 0
               End If
            Elseif Twi_dn = 1 Then
                If Xmode = 1 Then
                  ' Richtung
                  Xt1 = Twdr And 1
                  R_0 = Xt1
                  Xt1 = Twdr And 2
                  If Xt1 = 2 Then
                     R_1 = 1
                  Else
                     R_1 = 0
                  End If
               Elseif Xmode = 2 Then
                  ' ---- kommt noch
               End If
            Elseif Twi_dn = 2 Then
               If Xmode = 1 Then
                  V0 = Twdr
               Elseif Xmode = 2 Then
                  ' ---- kommt noch
               End If
            Elseif Twi_dn = 3 Then
               If Xmode = 1 Then
                  V1 = Twdr
               Elseif Xmode = 2 Then
                  ' ---- kommt noch
               End If
            Elseif Twi_dn = 4 Then


            Elseif Twi_dn = 5 Then


            End If
            Incr Twi_dn
         Elseif Twi_status = &HB0 Or Twi_status = &HB8 Then
            ' ------- Daten an TWI übergeben
            If Xmode = 0 Then
               If Twi_dn = 0 Then
                  Twdr = Xmode
               Else
                  Twdr = 255
               End If
            Elseif Xmode = 1 Then
               If Twi_dn = 0 Then
                  Twdr = Xmode
               Elseif Twi_dn = 1 Then
                  Xt1 = 0
                  If R_0 = 1 Then Xt1 = 1
                  If R_1 = 1 Then Xt1 = Xt1 + 2
                  Twdr = Xt1
               Elseif Twi_dn = 2 Then
                  Twdr = V0
               Elseif Twi_dn = 3 Then
                  Twdr = V1
               Elseif Twi_dn = 4 Then
                  Twdr = P0
               Elseif Twi_dn = 5 Then
                  Twdr = P1
               Else
                  Twdr = 0
               End If
            Elseif Xmode = 2 Then
               If Twi_dn = 0 Then
                  Twdr = Xmode
               Else
                  Twdr = 255
               End If
            Else
               Twdr = 0
            End If


            Incr Twi_dn
         Elseif Twi_status = &HA0 Or Twi_status = &HC0 Then
            ' TWI Transfer beendet
            Portd.7 = 1
            Twi_dn = 0
         End If
         Twcr = &B11000100
   End If
Loop


Timer0_ovf:
   ' ---- Timer_0 ----- Interrupt alle 1 mS ----------------
   Timer0 = 194                                             ' Resrwert = 61
   Incr Tx
   If Tx = 80 Then Portb.5 = 0                              ' Herzschlag-LED (ein)


   If Tx > 99 Then
      ' -------- Zeitbasis 100 mS ------------------
      Sx = 1                                                ' Ready-Leitung =1
      Tx = 0
      Incr Tmx
      Z0x = Z0
      Z0 = 0
      Z1x = Z1
      Z1 = 0


      If P0 = 0 And P1 = 0 Then
         ' ------ Richtung setzen wenn Motoren stehen
         If R_0 = 0 Then Portb.3 = 0 Else Portb.3 = 1
         If R_1 = 0 Then Portb.4 = 0 Else Portb.4 = 1
      End If


      If Xmode = 0 Then
         ' ------ Mode - Stop ----------
         If P0 > 30 Then
            P0 = P0 - 30
            Sx = 0
         Else
            P0 = 0


         End If
         If P1 > 30 Then
            P1 = P1 - 30
            Sx = 0
         Else
            P1 = 0
         End If
      Elseif Xmode = 1 Then
         ' ------ Mode - Speed ----------
         If V0 > Z0x And P0 < 255 And Pinc.0 = 1 Then
            Px = 1
            Vx = V0 - Z0x
            If Vx > 6 Then
               Px = 5
               Sx = 0
            End If
            If Vx > 30 Then
               Px = 20
            End If
            Pt = P0 + Px
            If Pt > 255 Then
               P0 = 255
            Else
               P0 = P0 + Px
            End If
         End If


         If V1 > Z1x And P1 < 255 And Pinc.0 = 1 Then
            Px = 1
            Vx = V1 - Z1x
            If Vx > 6 Then
               Px = 5
               Sx = 0                                       ' Ready-Leitung =0
            End If
            If Vx > 30 Then
               Px = 20
            End If
            Pt = P1 + Px
            If Pt > 255 Then
               P1 = 255
            Else
               P1 = P1 + Px
            End If
         End If


         If V0 < Z0x And P0 > 0 Or Pinc.0 = 0 Then
            Px = 1
            Vx = Z0x - V0
            If Vx > 3 Then
               Px = 6
               Sx = 0
            End If                                          ' Ready-Leitung =0
            If Vx > 35 Then
               Px = 30
            End If
            If Px > P0 Then
               P0 = 0
            Else
               P0 = P0 - Px
            End If
         End If


         If V1 < Z1x And P1 > 0 Or Pinc.0 = 0 Then
            Px = 1
            Vx = Z1x - V1
            If Vx > 3 Then
               Px = 6
               Sx = 0
            End If                                          ' Ready-Leitung =0
            If Vx > 35 Then
               Px = 30
            End If
            If Px > P1 Then
               P1 = 0
            Else
               P1 = P1 - Px
            End If
         End If
      Elseif Xmode = 2 Then
         ' ---- Mode-Strecke ------------------
      End If


      ' ------ NOT - STOP ---- und Treiber auf null setzen ----
      If V0 = 0 And Z0x = 0 And P0 > 0 Or Pinc.0 = 0 Then P0 = 0
      If V1 = 0 And Z1x = 0 And P1 > 0 Or Pinc.0 = 0 Then P1 = 0


      ' ------ Treiber-Bremse -------
      If P0 > 0 Then
         Portd.4 = 1
      Else
         Portd.4 = 0
      End If


      If P1 > 0 Then
         Portd.5 = 1
      Else
         Portd.5 = 0
      End If


      ' ----- PWM-Wert setzen ---------
      Compare1a = P0
      Compare1b = P1


      Portb.0 = Sx                                          ' Ausgabe Ready-Leitung
   End If


   If Tmx > 9 Then
      ' -------------  Zeitbasis 1 Sec -------------------
      Tmx = 0
      If Pinc.1 = 0 Then                                    ' abfrage ob RS232 enabled
         Print "Status XM=" ; Xmode ; " Z0x=" ; Z0x ; " Z1x=" ; Z1x ; " P0=" ; P0 ; " P1=" ; P1 ; " S0=" ; S0 ; " S1=" ; S1 ; " NA=" ; Pinc.0
      End If
      Portb.5 = 1                                           ' Herzschlag-LED (aus)


   End If
Return
Return


Int0_sub:                                                   ' Interrupt Motor_0 Decoder_Q1
   Incr Z0
   If Pinc.2 = 0 Then                                       ' Abfrage Motor_0 Decoder_Q2 ( Drehrichtung )
      Incr S0
   Else
      Decr S0
   End If
Return
Return


Int1_sub:                                                   ' Interrupt Motor_1 Decoder_Q1
   Incr Z1
   If Pinc.3 = 0 Then                                       ' Abfrage Motor_1 Decoder_Q2 ( Drehrichtung )
      Incr S1
   Else
      Decr S1
   End If
Return
Return