'Roboter vermeidet Wände - 1-2 Magnet pro Moter pro Zeiteinheit 'Je nach Tastendruck weicht der Roboter anders aus 'by Malte Marwedel, www.marwedels.de/malte 'für Robo3, AT90S2313 MCU, Programmierumgebung: Bascom-AVR 'Benutzung auf eigene Gefahr 'Version 1.01 'Roboter hält an 'Roboter fährt zurück 'Roboter dreht sich je nach Tastendruck in eine entsprechende Richtung 'Roboter fährt weiter geradeaus $baud = 1200 $crystal = 4000000 Baud = 1200 Config Portb = Output Config Portd = &B00110000 'Variablen Dim Empfang As Byte 'Empfangenes Byte Dim Lmotor As Byte 'Geschwindigkeit in relativen Werten Dim Rmotor As Byte Dim Lmsneu As Integer 'Soll Geschindigkeit in Millisekunden Dim Rmsneu As Integer Dim Lmscurr As Byte 'Ist Geschwindigkeit in Millisekunden Dim Rmscurr As Byte Dim Lrelpos As Byte 'Relative Radposition Dim Rrelpos As Byte Dim Lradwert As Byte 'Der 4 Bit Wert, der an die Motoren muss Dim Rradwert As Byte Dim Durchlauf As Word 'Momentaner Durchlauf, wird jede Millisekunde erhöht Dim Raus As Byte 'Der Wert, der auf portb gelegt wird Dim Temp1 As Integer Dim Temp2 As Byte Dim Modus As Byte 'Beschreibt die Bewegung, wie der Roboter ausweichen soll Config1: Config Timer0 = Timer , Prescale = 256 Reinit: 'Neustart Wait 5 Enable Interrupts Reinit2: Portb = 0 'Alle Magnete aus Set Portd.4 'grüne LED an Reset Portd.5 'rote LED aus Rmscurr = 15 Lmscurr = 15 Empfang = &HAA 'Mäßiges forwärtsfahren If Pind.2 = 1 Or Pind.3 = 1 Then Goto Reinit2 End If Main: 'main wird 1041 mal pro Sekunde durchlaufen Timer0 = 0 Incr Durchlauf 'Durchlauf berechen If Pind.2 = 1 And Modus = 0 Then 'auf rechter Seite ein Gegenstand Durchlauf = 0 Empfang = &H77 'Motoren Stop Modus = 1 End If If Pind.3 = 1 And Modus = 0 Then 'auf linker Seite ein Gegenstand Durchlauf = 0 Empfang = &H77 'Motoren Stop Modus = 11 End If If Durchlauf = 1000 And Modus = 1 Then 'Gegenstand rechts, rückwärts Empfang = &H66 Modus = 2 End If If Durchlauf = 1000 And Modus = 11 Then 'Gegenstand links, rückwärts Empfang = &H66 Modus = 12 End If If Durchlauf = 2500 And Modus = 2 Then 'Gegenstand rechts, stoppen Empfang = &H77 Modus = 3 End If If Durchlauf = 2500 And Modus = 12 Then 'Gegenstand links, stoppen Empfang = &H77 Modus = 13 End If If Durchlauf = 3000 And Modus = 3 Then 'Gegenstand rechts, drehen Empfang = &H68 Modus = 4 End If If Durchlauf = 3000 And Modus = 13 Then 'Gegenstand links, drehen Empfang = &H86 Modus = 14 End If If Durchlauf = 5000 And Modus = 4 Then 'Gegenstand rechts, wieder forwärts Empfang = &HAA Modus = 0 End If If Durchlauf = 5000 And Modus = 14 Then 'Gegenstand links, wieder forwärts Empfang = &HAA Modus = 0 End If Lmotor = Empfang Mod 16 'jeder Motor erhält seine Zahl Rmotor = Empfang / 16 Lmsneu = 7 - Lmotor 'differenz berechet (+-) Lmsneu = Abs(lmsneu) 'differenz berechnet (+) Lmsneu = 15 - Lmsneu 'Die Zahl bestimmt die max und min Motor geschwindigkeit Rmsneu = 7 - Rmotor Rmsneu = Abs(rmsneu) Rmsneu = 15 - Rmsneu 'Millisekunden Werte Berechnet, nun aktuelle ms Werte berechnen (langsames Anfahren und Abbremsen) 'Incr Rmsneu 'Da Räder leider nicht gleich schnell Temp1 = Durchlauf Mod 255 'Temp1 = 0 'nur fürs testen (simulieren) If Temp1 = 0 Then '4 Mal pro Sec schneller-langsamer Toggle Portd.5 If Lmsneu > Lmscurr Then Incr Lmscurr End If If Lmsneu < Lmscurr Then Decr Lmscurr End If If Rmsneu > Rmscurr Then Incr Rmscurr End If If Rmsneu < Rmscurr Then Decr Rmscurr End If End If 'OK, curr ms Werte berechnet 'Setzen von Radwert l,r Temp1 = Durchlauf Mod Lmscurr If Temp1 = 0 Then If Lmotor < 7 Then Decr Lrelpos End If If Lmotor > 7 Then Incr Lrelpos End If If Lrelpos <= 0 Then Lrelpos = 8 End If If Lrelpos >= 9 Then Lrelpos = 1 End If End If 'Ende Lradwert Temp1 = Durchlauf Mod Rmscurr If Temp1 = 0 Then If Rmotor < 7 Then Decr Rrelpos End If If Rmotor > 7 Then Incr Rrelpos End If If Rrelpos <= 0 Then Rrelpos = 8 End If If Rrelpos >= 9 Then Rrelpos = 1 End If End If 'Ende Rradwert 'Radwerte Berechnet, nun Ausgabewerte Setzen Select Case Lrelpos Case 1 : Lradwert = &B10000000 Case 2 : Lradwert = &B10010000 Case 3 : Lradwert = &B00010000 Case 4 : Lradwert = &B01010000 Case 5 : Lradwert = &B01000000 Case 6 : Lradwert = &B01100000 Case 7 : Lradwert = &B00100000 Case 8 : Lradwert = &B10100000 End Select Select Case Rrelpos Case 1 : Rradwert = &B00001001 Case 2 : Rradwert = &B00000001 Case 3 : Rradwert = &B00000101 Case 4 : Rradwert = &B00000100 Case 5 : Rradwert = &B00000110 Case 6 : Rradwert = &B00000010 Case 7 : Rradwert = &B00001010 Case 8 : Rradwert = &B00001000 End Select 'Ausgabewert gesetzt, nun ausgeben 'Rradwert = 0 'Testweise, damit nur 1 Motor strom zieht Raus = Lradwert + Rradwert Portb = Raus 'Warteschleife, auf 1milli sec Wait1: If Timer0 > 15 Then Goto Main End If Goto Wait1