' VERSION: 0.8
' MERKMALE: Autonomer Modus wurde überarbeitet ..
' Usbbuchsen von links nach rechts:
' I2C, Mähwerk, Messrad, Shunts, USM
'_______________________________________________________________________________
'*******************************************************************************
'****************************** Konfiguration **********************************
'*******************************************************************************
'-------------------------------------------------------------------------------
'_______________________________________________________________________________
'******************************* Allgemeines ***********************************
'-------------------------------------------------------------------------------
$baud = 38400 ' Serielle Schnittstelle - Datenübertragungsrate
$crystal = 16000000 ' Quarzfrequenz
$regfile "m32def.dat" ' Controllerfile
$framesize = 128 ' ?
$swstack = 256 ' Speicher für Interrupts, usw.
$hwstack = 256
Const Versionsnummer = 0.85 ' Versionsnummer (für Bootscreen)
'_______________________________________________________________________________
'*********************************** I2C ***************************************
'-------------------------------------------------------------------------------
$lib "i2c_TWI.lbx" ' Library
Config Twi = 100000 ' Bustaktung auf 100 kbit/s
Config Scl = Portc.0 ' Portkonfigurationen
Config Sda = Portc.1
Const Md22_adresse = 186 ' Adresse des Motortreibers
'_______________________________________________________________________________
'**************************** Analoge Eingänge *********************************
'-------------------------------------------------------------------------------
Config Adc = Single , Prescaler = Auto , Reference = Off ' Einstellungen für ADC allgemein
Dim Adc_wert As Word ' Allgemeine Variable zum Auslesen der ADC Eingänge
Const Referenz = 5 / 1023 ' Konstante zur Berechnung von Spannungen: 5V Referenzspannung / (1024 - 1) Bit Auflösung
Enable Adc
'_______________________________________________________________________________
'******************************** Display **************************************
'-------------------------------------------------------------------------------
Config Lcd = 16 * 4 ' Das Display hat 16 Zeichen und 4 Zeilen
' Die Pinbelegung des Displays
Config Lcdpin = Pin , Db4 = Portb.3 , Db5 = Portb.4 , Db6 = Portb.5 , Db7 = Portb.6 , E = Portb.1 , Rs = Portb.0
' Fernsteuerung: Parametermenü Autonom
' ---------------- ' ---------------- ' ----------------
'| * Fern.* 7,33V| '|* Parametermenu | '| * Auto.* 6,22V|
'|Mahwerk: ein | '|Parameter: Wert | '|RK-Sen: 00111111| Displaylayouts
'|Imah: xx Ian: xx| '| T3(-) (+)T4 | '|Imah: xx Ian: xx|
'|Fehler: ------- | '|<T1<< >>T2>| '|Fehler: ------- |
' ---------------- ' ---------------- ----------------
' Hauptmenü Fehleranzeige
' ---------------- ' ----------------
'|*** Auswahl: ***| '|ACHTUNG FEHLER! |
'| T1: Einstell. | '|I2C Übertr. |
'| T2: Fernst. | '|T1: Fortfahren |
'| T3: Automatik | '|T5: Reset |
' ---------------- ' ----------------
'-------------------- Benutzerdefinierte Zeichen für Bootscreen ----------------
Deflcdchar 0 , 28 , 28 , 28 , 28 , 28 , 28 , 28 , 28
Deflcdchar 1 , 28 , 28 , 28 , 28 , 28 , 31 , 31 , 31
Deflcdchar 2 , 31 , 31 , 31 , 28 , 28 , 31 , 31 , 31
Deflcdchar 3 , 31 , 31 , 31 , 7 , 7 , 31 , 31 , 31
Deflcdchar 4 , 7 , 7 , 7 , 7 , 7 , 7 , 7 , 7
Deflcdchar 5 , 24 , 28 , 28 , 30 , 14 , 7 , 7 , 3
Deflcdchar 6 , 31 , 3 , 3 , 3 , 3 , 31 , 31 , 31
Deflcdchar 7 , 7 , 7 , 7 , 32 , 32 , 7 , 7 , 7
'_______________________________________________________________________________
'******************** Routinen und Variablen (allgemein) ***********************
'-------------------------------------------------------------------------------
Dim X As Byte ' Zählvariablen
Dim N As Word
Dim N_fernsteuerung As Word ' Displayzählvariable fürs Fernsteuerungsmenü
Dim I As Integer
Dim Integ As Integer
Dim Sng As Single ' Allgemein Single - Variable für Rundungen etc.
Dim S As String * 8 ' Allgemeine Stringvariable
' Variablen fürs Fehlerentfernen im empfangen Byte bezüglicher der Klappen
Dim Zw1_rk As Byte ' Zwischenergebnis des vergleichs
Dim Zw2_rk As Byte ' Geshiftete Variable Rk_sensor
Dim Zu_addieren1 As Byte ' Standardmäßig 1 - zum "hochshiften"
Dim Zu_addieren2 As Byte ' Betrag der zu addieren ist
Dim X_addieren As Byte
Dim Bed1 As Byte 'Die beiden Bedingungen werden gesetzt, wenn beim Bitverschieben auf eine 1 eine 0 folgt
Dim Bed2 As Byte
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Tasterabfrage ' Routine gibt Tasten aus: 1 bis 5 sind normal, 1+5=6; 2+4=7
Dim Taste As Byte ' Zurückgegebene Tastenvariable
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
' SINN ?!? Lieber im entsprechenden Programm anzeigen ..
Declare Sub Akkuspannung ' Gibt Akkuspannung aus
Const Spannungsteiler = 7.465 ' Spannungsteilerfaktor vor ADC(0) --> gemessener Wert, zur Berech. der Akkusp.
Dim Spannung As Single ' Variable für die Akkuspannung
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Strommessung
Dim U_mahwerk As Word
Dim U_antrieb As Word
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Rc_ausw ' Routine fürs Einlesen der Empfängerwerte
Dim Empfanger(4) As Word ' Empfängerarray für 4 Kanäle
Dim Kanal As Word ' Die Kanalnummer um das Array nachher anzusprechen
Dim T2_zahler As Word ' Variable zur Ermittelung der Anzahl der Timer2 - Überläufe
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Fernsteuerung ' Routine für das Programm: Fernsteuerung
'--------------------------- Timer2 Konfigurationen ----------------------------
' 2^8 = 256 Zählschritte, zählt alle 256 Takte eins hoch, benötigt für den Empfänger
Config Timer2 = Timer , Prescale = 256 , Capture Edge = Falling , Noise Cancel = 1
On Timer2 Pauseerkannt ' Wenn der Timer2 überläuft, wird zum Label - Pauseerkannt - gesprungen
Disable Timer2 ' Timer2 deaktivieren
'-------------------------------------------------------------------------------
Dim Rampenparameter As Word ' Einstellvariable für die Anpassrate der Rampe
Dim Rampenschritt As Byte
Dim Bedingung As Byte ' Ohne gesetzte Bedingung keine Anpassung
'-------------------------- Interrupt 1 Konfiguration --------------------------
Config Int1 = Falling
On Int1 Zeitmessung ' Registriert der Interrupt eine fallende Flanke, wird zum Label - Zeitmessung - gesprungen
Disable Int1 ' Deaktiveren von Int1
'-------------------------------------------------------------------------------
Dim Motor_rechts As Integer ' Motorwerte
Dim Motor_links As Integer
Dim Lenkung As Integer ' Lenkungswert für Motorberechnung
Dim Lenkungsparameter As Single ' Einfluss der Lenkung
Dim Differenz As Integer ' Differenz der beiden Motorvariablen
Dim Motor_min As Byte ' Obere und untere Grenze für Motorwerte (absolute Grenzen sind 0 und 255)
Dim Motor_max As Byte
Dim Au_motor_max As Byte ' Parameter für die maximale Geschwindigkeit im autonomen Modus
Dim Z As Byte ' Zählvariable
Dim Fern_aktivieren As Byte ' Variable um im autonomen Modus in den ferngesteuerten Modus zu springen
Fern_aktivieren = 0
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Beep ' Routine für den Signalton
Dim Beepparameter As Word 'Parameter um lautlos zu schalten und Tonlänge variieren zu können
Config Portd.2 = Output
Summer Alias Portd.2
Summer = 0 ' Summer ausschalten
'--------------------------- Timer1 Konfigurationen ----------------------------
' 2^16 = 65380 Zählschritte, zählt alle 1024 Takte eins hoch, benötigt um den Summer wieder auszuschalten
Config Timer1 = Timer , Prescale = 1024 , Capture Edge = Falling , Noise Cancel = 1
On Timer1 Timer1_uberlauf
Disable Timer1
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Md22_init ' Initialisierung um Motortreiber einzustellen, I2C zu initialisieren und Motoren in Stillstand setzen
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Motorausgabe ' Routine fürs Senden der Motorwerte an das Motortreibermodul
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Motorstop ' Routine für den sofortigen Stop der Motoren.
Dim Motorstopparameter As Byte
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Mahwerk_einschalten ' Mähwerkroutine fürs Einschalten
Dim Differenz_alt As Word ' Benötigt zum Wert kopieren
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Mahwerk_ausschalten ' Mähwerkroutine fürs Ausschalten
Dim Mahwerk As Byte
Dim Mahwerk_vw As Byte ' Wird benützt um bei anlaufendem Mähwerk im Gras das Mähwerk abzuschalten
Config Portd.5 = Output ' Mähwerkports als Ausgang definieren
Config Portc.2 = Output
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Parametermenu
Dim Menuindex As Byte ' Programm zum Einstellen der Parameter
Menuindex = 1 ' Der erste Parameter soll standardmäßig angezeigt werden
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Usm_abfrage ' Ultraschallmodulabfrage - Routine
Config Porta.1 = Output ' Port als Ausgang definieren - fürs USM
Config Portd.7 = Output ' Fürs Servo
Config Servos = 1 , Servo1 = Portd.7 , Reload = 10 ' Servokonfiguration (interne Bascomroutine)
Const Servo_0 = 140
Dim Servo_90 As Byte ' Servo steht 90° Rechts
Dim Hindernis As Word ' Variable für den Abstand eines Hindernisses
Dim Hind_x_faktor As Byte
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Messrad_notaus ' Routine zur Erfassung des Messradstatuses
' Config Pinc.4 = Input ' Portkonfiguration
' Notaus Alias Pinc.4
' Notaus = 1 ' Pullup für Notaus
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Hindernis ' Routine zur Hindernisumfahrung
Dim V_dreh As Byte ' Drehgeschwindigkeit z.B. bei Hindernisumgehung
Dim V_hindernis As Byte ' Geschwindigkeitsparameter
Dim Max_hindernis As Byte ' Entfehrnung ab der Hindernisroutine eingeleitet wird
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Autonom ' Programm für den autonomen Modus
Dim Rk_sensor As Byte ' Variable für den Status der jeweiligen Klappen des Sensors
Dim Klappen_status As Byte ' Statusvariable die Aussage über das Änderungsverhalten (Frequenz...) der Klappen gibt
Dim Buf(2) As Byte ' Array für I2C Daten vom Mega8
Dim Drossel As Byte ' Variable um festzulegen, wie stark die Motoren gebremst / beschleunigt werden sollen
Dim Xoo As Byte
Dim Xxo As Byte
Dim Xxx As Byte
Dim Xoo_alt As Byte ' Zum Zwischenspeichern
Dim Xxo_alt As Byte
Dim Xxx_alt As Byte
Dim Autonom_aktivieren As Byte ' Notwendig um aus der Fernsteuerungsroutine zu springen und anschließen den autonomen Modus zu starten
Autonom_aktivieren = 0
Const K1 = &B00000001
Const K2 = &B00000011
Const K3 = &B00000111
Const K4 = &B00001111
Const K5 = &B00011111
Const K6 = &B00111111 ' Die 6 niederwertigeren Bits entsprechenden 6 Klappen
Const B2 = &B01000000 ' Und die 2 höherwertigen Bits den 2 Bumpern
Const B1 = &B10000000
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Fehler
Dim Fehler As Byte
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
Config Pina.2 = Input ' Notausschalter - 0 entspricht gedrückt
Porta.2 = 1
'_______________________________________________________________________________
'*******************************************************************************
'**************************** ENDE Konfiguration *******************************
'*******************************************************************************
'-------------------------------------------------------------------------------
'_______________________________________________________________________________
'*******************************************************************************
'****************************** Startvorgang ***********************************
'*******************************************************************************
'-------------------------------------------------------------------------------
'------------------------------- Bootscreen ------------------------------------
Cls ' Diplay löschen
Servo(1) = Servo_0 ' Servo auf Nullstellung fahren
Locate 1 , 1
Lcd Chr(0) ; " " ; Chr(2) ; Chr(3) ; " " ; Chr(2) ; Chr(3) ; " " ; Chr(4) ; Chr(2)
Locate 2 , 1
Lcd Chr(1) ; " " ; Chr(0) ; Chr(4) ; " " ; Chr(0) ; Chr(5) ; " " ; Chr(7) ; Chr(3)
Locate 3 , 1
Lcd " ... bootet";
Locate 4 , 1
Lcd "Version: " ; Versionsnummer ' Version anzeigen
Cursor Off 'Curser-Blinken ausschalten
'------------------------------ Einstellungen ----------------------------------
'--- Parameter aus dem EEPROM holen ---
Readeeprom Rampenparameter , 1 ' Rampengeschwindigkeit
Readeeprom Beepparameter , 10 ' Dauer der Pieptöne (auch zum Ausschalten geeignet =0)
Readeeprom Lenkungsparameter , 20 ' Einfluss der Lenkung bei Fernst. (Achtung invertiert)
Readeeprom Motor_min , 30 ' Minimaler Motorwert
Readeeprom Motor_max , 40 ' Maximaler Motorwert
Readeeprom Motorstopparameter , 50 ' Parameter umd die "Notbremsengeschwindigkeit" einzustellen
Readeeprom Servo_90 , 60 ' 90° rechts Stellung des Servo
Readeeprom V_hindernis , 70 ' Geschwindigkeit der Hindernisumfahrung
Readeeprom V_dreh , 80 ' Drehgeschwindigkeit
Readeeprom Xoo , 90 ' Parameter für Anpassung beim autonomen Modus
Readeeprom Xxo , 100 ' - " -"
Readeeprom Xxx , 110 ' - " -"
Readeeprom Drossel , 120 ' - " -"
Readeeprom Max_hindernis , 130 ' Hindernisabstand
Readeeprom Au_motor_max , 140 ' Maximale Geschwindigkeit im autonomen Modus
Readeeprom Hind_x_faktor , 150
Xoo_alt = Xoo
Xxo_alt = Xxo
Xxx_alt = Xxx
Enable Interrupts ' Interrups allgemein aktivieren
Call Mahwerk_ausschalten ' Mähwerk ausmachen
Call Md22_init ' Motoren in Stillstand setzen und Motortreiber einstellen
Waitms 1200 ' Kurz warten :), damit man den Bootscreen anschauen kann (Versionsnummer etc.)
Call Md22_init
'_______________________________________________________________________________
'*******************************************************************************
'************************** ENDE Startvorgang *********************************
'*******************************************************************************
'-------------------------------------------------------------------------------
'_______________________________________________________________________________
'*******************************************************************************
'***************************** Hauptschleife ***********************************
'*******************************************************************************
'-------------------------------------------------------------------------------
Do
Cls
Locate 1 , 1 .
Lcd "*** Auswahl: ***"
Locate 2 , 1
Lcd " T1: Einstell. "
Locate 3 , 1
Lcd " T2: Fernst. Mo."
Locate 4 , 1
Lcd " T3: Automatik "
Cursor Off
'------------------ Tastenabfrage und Aufrufen der Programme -------------------
Do
Call Tasterabfrage
If Taste <> 0 Then ' Wenn eine Taste gedrückt wurde
Select Case Taste ' Springe zum jeweiligen Programm
Case 1 : Call Parametermenu
Case 2 : Call Fernsteuerung
Case 3 : Call Autonom
Case 4 : Call Strommessung
Case 5 : Goto &H1C00 ' Softreset durchführen
End Select
Waitms 200 ' Doppeltastendruck vermeiden
Exit Do ' Nach dem jeweiligen Programm soll die Displaymaske wieder aufgebaut werden
End If
If Autonom_aktivieren = 1 Then Call Autonom ' Falls diese Variable im Fernsteuerungsmodus gesetzt wurde, wird direkt zum autonomen Modus gesprungen
Loop
Loop
'_______________________________________________________________________________
'*******************************************************************************
'************************* ENDE Hauptschleife *********************************
'*******************************************************************************
'-------------------------------------------------------------------------------
'_______________________________________________________________________________
'*******************************************************************************
'******************************* Routinen **************************************
'*******************************************************************************
'-------------------------------------------------------------------------------
'_______________________________________________________________________________
'****************************** Tasterabfrage **********************************
'-------------------------------------------------------------------------------
Sub Tasterabfrage
Taste = 0 ' Tasterwert zurücksetzen
Start Adc
Adc_wert = Getadc(7) ' ADC Wert einlesen
Stop Adc
Select Case Adc_wert
Case 729 To 733 : Taste = 1 ' Bereiche für die jeweiligen Taster ..
Case 597 To 601 : Taste = 2
Case 505 To 509 : Taste = 3
Case 415 To 420 : Taste = 4
Case 175 To 180 : Taste = 5
Case 165 To 170 : Taste = 6 ' T1 und T5
Case 321 To 327 : Taste = 7 ' T2 und T4
End Select
If Taste <> 0 Then Call Beep ' Signalton bei Tastendruck
End Sub
'_______________________________________________________________________________
'******************************* Beeproutine ***********************************
'-------------------------------------------------------------------------------
Sub Beep
If Beepparameter <> 0 Then ' Wenn der Beeper nicht auf lautlos ist:
Summer = 1 ' Summer einschalten
Timer1 = 65536 - Beepparameter ' Zählerpreload - sprich: wie lange der Beeper piepst
Enable Timer1 ' Timer aktivieren, bei Überlauf (Interrupt) wird der Summer ausgemacht
End If
End Sub
'_______________________________________________________________________________
'******************************* Akkuspannung **********************************
'-------------------------------------------------------------------------------
Sub Akkuspannung
Start Adc
Adc_wert = Getadc(0) 'ADC Wert einlesen
Stop Adc
Spannung = Adc_wert * Referenz ' Mit Konstante ( = 5/1023 ) multiplizieren, um absoluten Wert in Volt ändern
Spannung = Spannung * Spannungsteiler ' Mit Spannungsteilerfaktor ( = 7.465 ) multiplizieren, um tats. Spannung zu errechnen
S = Fusing(spannung , "#.##") ' Auf 2 Nachkommastellen runden, in String umwandeln
End Sub
'_______________________________________________________________________________
'******************************* Strommessung **********************************
'-------------------------------------------------------------------------------
Sub Strommessung
N = 0
Do
Incr N ' Zählvariable erhöhen (für Display benötigt)
Start Adc ' ADC Messungen aktivieren
Adc_wert = Getadc(5) ' ADC-Wert am ADC 4 messen
U_mahwerk = Adc_wert ' ADC-Wert speichern
'Print "ADC am Mähwerk: " ; U_mahwerk
Start Adc
Adc_wert = Getadc(4) ' ADC-Wert am ADC 5 messen
Stop Adc
U_antrieb = Adc_wert ' Spannung des Antriebes speichern
'Print "ADC an den Antriebsmotoren: " ; U_antrieb
If N = 20 Then ' jeden .20. Durchlauf das Display anpassen
N = 0 ' Zähler zurücksetzen
Cls
Locate 1 , 1 : Lcd " ADC - Werte"
Locate 2 , 1 : Lcd "Mahmo: " ; U_mahwerk
Locate 3 , 1 : Lcd "A-Mot: " ; U_antrieb
End If
Call Tasterabfrage
If Taste = 2 Then
Motor_links = 200
Motor_rechts = 200
Call Motorausgabe
End If
If Taste = 1 Then
Motor_links = 128
Motor_rechts = 128
Call Motorausgabe
End If
If Taste = 4 Then Call Mahwerk_einschalten
If Taste = 3 Then Call Mahwerk_ausschalten
Loop Until Taste = 5 ' Zurück ins Hauptmenü bei Druck von Taste 5
Call Mahwerk_ausschalten
Motor_links = 128
Motor_rechts = 128
Call Motorausgabe
End Sub
'_______________________________________________________________________________
'************************* RC Empfänger auswerten ******************************
'-------------------------------------------------------------------------------
Sub Rc_ausw
Kanal = 5 ' Startvariable für Kanalwert, damit die ersten Timerwerte nicht gespeichert werden
T2_zahler = 0 ' Timer2 Überlaufzähler zurücksetzen
Timer2 = 0 ' Startwert zurücksetzen
Enable Timer2 ' Timer2 aktivieren
Enable Int1 ' Interrupt1 aktivieren
Do ' Es sollen fünf mal alle Kanäle eingelesen werden, die letzte Messreihe wird verwendet (5 ist ein Erfahrungswert)
Loop Until T2_zahler = 4 ' Anschließend kann die Schleife verlassen werden
Disable Timer2 ' Timer2 und den Interrupt wieder deaktivieren
Disable Int1
'Print "K1: " ; Empfanger(1) ; " --- K2: " ; Empfanger(2) ; " --- K3: " ; Empfanger(3) ; " --- K4: " ; Empfanger(4)
End Sub
'_______________________________________________________________________________
'************************* Fernsteuerungsprogramm ******************************
'-------------------------------------------------------------------------------
Sub Fernsteuerung
N = 0 'Zählvariable für die Displayaktualisierung
Do
'------------------------- Notaus abfragen -------------------------------------
Call Messrad_notaus
'------------------------- Empfängerwerte einlesen -----------------------------
Call Rc_ausw ' Empfänger auswerten
If Empfanger(1) >= 116 And Empfanger(2) >= 116 And Mahwerk = 0 Then Call Mahwerk_einschalten ' Bei einer bestimmten Knüppelposition wird das Mähwerk eingeschalten
If Empfanger(1) <= 72 And Empfanger(2) > 116 And Mahwerk = 1 Then Call Mahwerk_ausschalten ' Ansonsten wird es aussgeschalten wenn der Hebel von Kanal 1 zurückgefahren wird
If Empfanger(2) < 72 And Empfanger(1) < 72 Then ' Bei einer anderen Knüppelposition wird der autonome Modus gestartet.
Autonom_aktivieren = 1
Exit Sub
End If
'-------------------------- Gaswerte aufmodulieren -----------------------------
Motor_rechts = Empfanger(3) ' Kanal 3 als "Gaswert" anpassen.
Motor_rechts = Motor_rechts - 94 ' Der Wert [66..94..118 -> Diff. = 52] muss auf [0..255] bzw. [Motor_min..Motor_max] gebracht werden
Motor_rechts = Motor_rechts * 5 ' Dazu wird er erstmal auf die normale Nullstellung gebracht. Nun wird er aufmoduliert und anschließend
Motor_rechts = Motor_rechts + 128 ' entsprechend verschoben
If Motor_rechts > 120 And Motor_rechts < 136 Then ' Damit leichte Schwankungen um den Nullpunkt nicht stören, wird hier ein Bereich definiert
' in dem die Motoren nicht drehen sollen.
Motor_rechts = 128
End If
Motor_links = Motor_rechts ' Fertig modulierten "Gaswert" auf anderen Motor übertragen
'-------------------------- Lenkungswerte modulieren ---------------------------
Lenkung = Empfanger(4) ' Lenkungswert modulieren.
Lenkung = Lenkung - 94
Lenkung = Lenkung * 5 ' Default von [66..118] auf [-128..128]
Sng = Lenkung / Lenkungsparameter ' Einstellung der Lenkstärke,
Lenkung = Round(sng)
If Lenkung < 6 And Lenkung > -6 Then Lenkung = 0 ' Lenkungswerte runden für Zentralstellung
Motor_rechts = Motor_rechts - Lenkung ' Hier wird nun der Lenkungswert dem linken Motor addiert
Motor_links = Motor_links + Lenkung ' und dem rechten subtrahiert
'--------- Berechnungen anpassen für max. / min. Werte und weiteres ------------
If Motor_rechts < Motor_min Then ' Falls der Motorwert nun unter dem motor_min wert ist (z.B. <0),
' dann wird der Betrag auf der anderen Seite hinzuaddiert
Differenz = Motor_min - Motor_rechts ' Differenz bilden (Bsp: Motor_min = 20, Motor_rechts = -20, Differenz von 40
Motor_links = Motor_links + Differenz ' Muss addiert werden zum linken Motor
Motor_rechts = Motor_min ' Und der rechte Motor wird auf Motor_min gesetzt
End If
If Motor_links < Motor_min Then ' Selbes Spiel andere Seite
Differenz = Motor_min - Motor_links
Motor_rechts = Motor_rechts + Differenz
Motor_links = Motor_min
End If
If Motor_rechts > Motor_max Then ' Und nochmal das Selbe für die obere Grenze.
Differenz = Motor_rechts - Motor_max ' Hier muss man den Betrag, der über Motor_max zu viel ist abziehn
Motor_links = Motor_links - Differenz
Motor_rechts = Motor_max
End If
If Motor_links > Motor_max Then ' Für den linken Motor
Differenz = Motor_links - Motor_max
Motor_rechts = Motor_rechts - Differenz
Motor_links = Motor_max
End If
'---------------------- Taster abfragen für evtl. Abbruch ----------------------
Call Tasterabfrage ' Tasterroutine aufrufen
If Taste = 4 And N_fernsteuerung = 1 Then
Call Mahwerk_einschalten ' Mähwerk einschalten
End If
If Taste = 3 Then
Call Mahwerk_ausschalten ' Mähwerk ausschalten
End If
'----------------- Displayausgabe jedes 5. mal --------------------------------
Incr N_fernsteuerung
If N_fernsteuerung = 5 Then
N_fernsteuerung = 0
Call Akkuspannung
Cls
Locate 1 , 1
Lcd "*FM* U=" ; S ; "V M=" ; Mahwerk ' Akkuspannung anzeigen
Locate 2 , 1
Lcd "K1: " ; Empfanger(1)
Locate 2 , 9
Lcd "K2: " ; Empfanger(2)
Locate 3 , 1
Lcd "K3: " ; Empfanger(3)
Locate 3 , 9
Lcd "K4: " ; Empfanger(4)
Locate 4 , 1
Lcd "l_M:" ; Motor_links ; " r_M:" ; Motor_rechts
End If
'------------------------------ Motorwerte ausgeben ----------------------------
Call Motorausgabe
Loop Until Taste = 5 ' Bei Taste 5: Abbruch, und zurück
Call Motorstop ' Motoren sofort stoppen
Call Mahwerk_ausschalten ' Mähwerk ausschalten
Fern_aktivieren = 0
End Sub
'_______________________________________________________________________________
'****************** Inititalisierung des Motortreibers *************************
'-------------------------------------------------------------------------------
Sub Md22_init
'--------------------------------- Einstellungen -------------------------------
'----- erste Übertragung ----
I2cstart
I2cwbyte Md22_adresse
I2cwbyte 0 ' Modus-Register des Motortreibers
I2cwbyte 0 ' Dessen Wert auf 0 setzen für getrennte ansteuerung links/rechts
I2cstop
'---- Fehler Handhabung ----
If Err = 1 Then
Fehler = 1
Call Fehler
End If
'----- zweite Übertragung ----
I2cstart
I2cwbyte Md22_adresse
I2cwbyte 3 ' Acceleration-Register
I2cwbyte Rampenparameter ' Zeit/Schritt= Rampenparameter * 64µs.
I2cstop ' (255 entspricht längster zeit (ca 16,4ms pro schritt))
'---- Fehler Handhabung ----
If Err = 1 Then
Fehler = 1
Call Fehler
End If
'---------------------------- Motoren ausschalten ------------------------------
Motor_rechts = 128
Motor_links = 128
Call Motorausgabe ' Motorwerte senden
End Sub
'_______________________________________________________________________________
'****************** Motorwerte an Motortreiber senden **************************
'-------------------------------------------------------------------------------
Sub Motorausgabe
'------------------------------ rechter Motor ----------------------------------
I2cstart
I2cwbyte Md22_adresse ' I2C_Adresse des Motortreibers
I2cwbyte 1 ' Register für M1
I2cwbyte Motor_rechts ' Wert senden
I2cstop ' (0 entspricht längster zeit (ca 16,4ms pro schritt))
'---- Fehler Handhabung ----
If Err = 1 Then
Fehler = 1
Call Fehler
End If
'------------------------------ linker Motor -----------------------------------
I2cstart
I2cwbyte Md22_adresse ' I2C_Adresse des Motortreibers
I2cwbyte 2 ' Register für M2
I2cwbyte Motor_links ' Wert senden
I2cstop
'---- Fehler Handhabung ---
If Err = 1 Then
Fehler = 1
Call Fehler
End If
End Sub
'_______________________________________________________________________________
'******************************** Motorstop ***********************************
'-------------------------------------------------------------------------------
Sub Motorstop
'-------------------------- Einstellungen anpassen -----------------------------
I2cstart
I2cwbyte Md22_adresse
I2cwbyte 3 ' Acceleration-Register
I2cwbyte Motorstopparameter
I2cstop
'---- Fehler Handhabung ---
If Err = 1 Then
Fehler = 1
Call Fehler
End If
'----------------------- Motorwert "stopp" ausgeben ----------------------------
Motor_links = 128 ' Motorwerte auf "stopp" setzen
Motor_rechts = 128
Call Motorausgabe
'------------------------- Einstellungen rücksetzen ----------------------------
I2cstart
I2cwbyte Md22_adresse
I2cwbyte 3 ' Acceleration-Register zurücksetzen
I2cwbyte Rampenparameter
I2cstop
'---- Fehler Handhabung ----
If Err = 1 Then
Fehler = 1
Call Fehler
End If
End Sub
'_______________________________________________________________________________
'***************************** Mähwerk aktivieren ******************************
'-------------------------------------------------------------------------------
Sub Mahwerk_einschalten
'-------------------Motoren stoppen --------------------------------------------
'Zunächst werden die Antriebsmotoren gestoppt, da diese während der Einschaltroutine nicht gesteuert werden können
'Außerdem sollte das Mähwerk idealerweise nicht unter Belastung (im Gras) eingeschalten werden.
Print "Mahwerk einschalten .. "
Motor_rechts = 128
Motor_links = 128
Differenz = 0
Call Motorausgabe
If Mahwerk = 0 Then ' Wenn das Mähwerk aus ist
'( Start Adc
U_mahwerk = Getadc(5)
' Prescaler für Timer1, der spätestens nach 1s Anlaufen das Mähwerk abschaltet mit einem Fehler
Mahwerk_vw = 1
Print Mahwerk_vw
Timer1 = 0
Enable Timer1
')
Portc.2 = 1 ' Motor mit Vorwiderstand anfahren lassen
Waitms 500
'(
'For I = 1 To 80
' Start Adc
' Adc_wert = Getadc(5)
' Differenz = U_mahwerk - Adc_wert
' Print I ; ". - U_Mähwerk: " ; U_mahwerk ; " ADC_Wert: " ; Adc_wert ; " Differenz: " ; Differenz
'Next I
Print "VW angemacht " ; Mahwerk_vw
Do
Print Timer1
Differenz_alt = Differenz ' Wert kopieren
Start Adc
Adc_wert = Getadc(5)
Differenz = U_mahwerk - Adc_wert
Print "Anfahrwiderstand: " ; Mahwerk_vw
If Mahwerk_vw = 0 Then Exit Do
Loop Until Differenz_alt > Differenz ' So lange warten bis der Strom maximal wird
Print "1. Loopschleife verlassen -- Differenz: " ; Differenz ; " -- Alte Diff: " ; Differenz_alt
Do
Start Adc
Adc_wert = Getadc(5)
Differenz = U_mahwerk - Adc_wert
If Mahwerk_vw = 0 Then Exit Do
Loop Until Differenz <= 30 ' Vorwiderstand überbrücken wenn der Strom entsprechend gesunken ist
Print "2. Loopschleife verlassen"
Stop Adc
If Mahwerk_vw = 1 Then
Disable Timer1
')
Portd.5 = 1 ' Mähwerk komplett anschalten
Print "Mähwerk ganz anschalten"
Mahwerk = 1 ' Status auf 1 setzen
'(
End If
If Mahwerk_vw = 0 Then
Print "Fehler beim Anfahren"
Portc.2 = 0
Fehler = 5
Call Fehler
End If
')
End If
End Sub
'_______________________________________________________________________________
'*************************** Mähwerk deaktivieren ******************************
'-------------------------------------------------------------------------------
Sub Mahwerk_ausschalten
Portd.5 = 0 ' Entsprechende Ports deaktvieren
Portc.2 = 0
Mahwerk = 0 ' Status zurücksetzen
End Sub
'_______________________________________________________________________________
'******************************* Parametermenü *********************************
'-------------------------------------------------------------------------------
Sub Parametermenu
' 1. Rampenparameter [0(längste).. 255(kürzeste)]
' 2. 1 / Lenkungsparameter
' 3. Motorminimumwert
' 4. Motormaximalwert
' 5. Beepparameter
' 6. Motorstopprampe
' 7. Servo 90° (Stellung des Servos bei 90°)
' 8. V_dreh
' 9. V_hindernis
' 10. Xoo (Anpassung der Geschwindigkeit bei einer Klappe Unterschied zur Mittelstellung)
' 11. Xxo (bei 2 Klappen)
' 12 Xxx (bei 3 Klappen)
' 13. Drossel (Die Geschwindigkeit wird nochmals herabgesetzt ab 2 Klappen Unterschied)
' 14. Max_Hindernis (Abstand, ab dem die Hindernisroutine aufgerufen wird
' 15 Au_motor_max (Maximale Geschwindigkeit im autonomen Modus
' 16. Hind_x_faktor (Faktor mit der die Differenz des Abstandes beim Hindernisfahren multipliziert wird)
'-------------------------------- Displaymaske ---------------------------------
Cls
Locate 1 , 1
Lcd "** Parameter **"
Locate 3 , 1
Lcd " T1(-) (+)T2 "
Locate 4 , 1
Lcd "<<T3<< >>T4>>"
N = 0 ' Zählvariable, damit das Display nur alle 100 Durchläufe aktualisiert wird ..
Servo(1) = Servo_90 ' Servo zurückfahren, damit man
Do
Incr N
Waitms 70
Call Tasterabfrage ' Taste abfragen
If Taste <> 0 Then ' Falls eine Taste gedrückt ist
Select Case Taste
'------------------------------ voriger Parameter ------------------------------
Case 3 ' Bei Taste 3 soll der vorige Parameter ausgewählt werden
If Menuindex = 1 Then ' Falls man bereits beim 1. Parameter ist, soll zum letzten gesprungen werden
Menuindex = 16
Else ' Ansonsten einfach den Menüindex um eins veringern
Decr Menuindex
End If
'----------------------------- nächster Parameter ------------------------------
Case 4 ' Bei Taste 4 soll der nächste Parameter ausgewählt werden, gleich wie vorhin
If Menuindex = 16 Then ' wenn letzer Parameter gewähtl ist
Menuindex = 1 ' springe zum ersten
Else
Incr Menuindex
End If
'------------------------------- Wert erhöhen ----------------------------------
Case 2 ' Wenn der entsprechende Wert erhöht werden soll
Select Case Menuindex
Case 1 ' Rampenparameter gewählt:
If Rampenparameter <= 254 Then Incr Rampenparameter ' Parameter um eins erhöhen
Case 2 ' Lenkungsparameter gewählt:
If Lenkungsparameter <= 20 Then ' Vorsicht, invertiert, sprich, umso größer, desto größer der maximale Lenkradius
Lenkungsparameter = Lenkungsparameter + 0.1
End If
Case 3 ' Motorminimumswert
If Motor_min <= 40 Then Incr Motor_min
Case 4 ' Motormaximumswert
If Motor_max < 255 Then Incr Motor_max
Case 5 ' Beepparameter
If Beepparameter <= 65408 Then Beepparameter = Beepparameter + 128
Case 6 ' Notbremsengeschwindigkeit
If Motorstopparameter < 255 Then Incr Motorstopparameter
Case 7 ' Rechte Winkel Stellung des Servos
If Servo_90 < 150 Then Incr Servo_90
Case 8 ' Hindernisparameter, wie stark die Motordrosselung erfolgt
If V_hindernis < 255 Then Incr V_hindernis
Case 9 ' Geschwindigkeit, mit der bei Automatik/Hindernis gedreht wird
If V_dreh < 128 Then Incr V_dreh
Case 10 ' Geschwindigkeitsanpassungen im autonomen Modus
If Xoo < 100 Then Incr Xoo
Case 11
If Xxo < 100 Then Incr Xxo
Case 12
If Xxx < 100 Then Incr Xxx
Case 13
If Drossel < 100 Then Incr Drossel
Case 14
If Max_hindernis < 254 Then Incr Max_hindernis ' Entfernung, bei der die Hindernisroutine aufgerufen wird
Case 15
If Au_motor_max < 255 Then Incr Au_motor_max ' Maximale Geschwindigkeit im aut. Modus
Case 16
If Hind_x_faktor < 255 Then Incr Hind_x_faktor
End Select
'----------------------------- Wert verkleinern --------------------------------
Case 1 ' Wenn der entsprechende Wert verkleinert werden soll
Select Case Menuindex
Case 1
If Rampenparameter > 0 Then Decr Rampenparameter
Case 2
If Lenkungsparameter > 1 Then
Lenkungsparameter = Lenkungsparameter - 0.1
End If
Case 3
If Motor_min > 0 Then Decr Motor_min
Case 4
If Motor_max > 215 Then Decr Motor_max
Case 5
If Beepparameter >= 128 Then Beepparameter = Beepparameter - 128
Case 6
If Motorstopparameter > 0 Then Decr Motorstopparameter
Case 7
If Servo_90 > 50 Then Decr Servo_90
Case 8
If V_hindernis > 0 Then Decr V_hindernis
Case 9
If V_dreh > 5 Then Decr V_dreh
Case 10
If Xoo > 1 Then Decr Xoo
Case 11
If Xxo > 1 Then Decr Xxo
Case 12
If Xxx > 1 Then Decr Xxx
Case 13
If Drossel > 1 Then Decr Drossel
Case 14
If Max_hindernis > 10 Then Decr Max_hindernis
Case 15
If Au_motor_max > 1 Then Decr Au_motor_max
Case 16
If Hind_x_faktor > 1 Then Decr Hind_x_faktor
End Select
'------------------------- Parametermenü verlassen -----------------------------
Case 5
Writeeeprom Rampenparameter , 1 ' Daten ins EEPROM schreiben
Writeeeprom Beepparameter , 10
Writeeeprom Lenkungsparameter , 20
Writeeeprom Motor_min , 30
Writeeeprom Motor_max , 40
Writeeeprom Motorstopparameter , 50
Writeeeprom Servo_90 , 60
Writeeeprom V_hindernis , 70
Writeeeprom V_dreh , 80
Writeeeprom Xoo , 90
Writeeeprom Xxo , 100
Writeeeprom Xxx , 110
Writeeeprom Drossel , 120
Writeeeprom Max_hindernis , 130
Writeeeprom Au_motor_max , 140
Writeeeprom Hind_x_faktor , 150
Call Md22_init
Servo(1) = Servo_0 ' Servo zurückfahren
Xoo_alt = Xoo
Xxo_alt = Xxo
Xxx_alt = Xxx
Exit Do
End Select
End If
'------------------------------- Display anpassen ------------------------------
If N = 2 Then ' Alle 3 Durchläufe das Display anpassen
Locate 2 , 1 ' Entsprechender Parameter auf LCD anzeigen
Lcd " "
Locate 2 , 1
Select Case Menuindex
Case 1
Lcd "Rampenp.: " ; Rampenparameter
Case 2
S = Fusing(lenkungsparameter , "##.##") ' Wert wird in String umgewandelt
Lcd "Lenk.: " ; S
Case 3
Lcd "Mot.min.: " ; Motor_min
Case 4
Lcd "Mot.max.: " ; Motor_max
Case 5
Lcd "Beep.: " ; Beepparameter
Case 6
Lcd "M-Stop.: " ; Motorstopparameter
Case 7
Lcd "Servo 90°: " ; Servo_90
Case 8
Lcd "Hind_Vmax: " ; V_hindernis
Case 9
Lcd "Hind_V_Dreh: " ; V_dreh
Case 10
Lcd "Aut.M. xoo: " ; Xoo
Case 11
Lcd "Aut.M. xxo: " ; Xxo
Case 12
Lcd "Aut.M. xxx: " ; Xxx
Case 13
Lcd "Aut.M. dro: " ; Drossel
Case 14
Lcd "Hind.Abst.: " ; Max_hindernis
Case 15
Lcd "Aut.M. Vmax: " ; Au_motor_max
Case 16
Lcd "Hind.X_Fakt: " ; Hind_x_faktor
End Select
N = 0
End If
Loop
End Sub
'_______________________________________________________________________________
'***************************** Ultraschallmodul ********************************
'-------------------------------------------------------------------------------
Sub Usm_abfrage
Config Porta.1 = Output ' Port als Ausgang definieren
Porta.1 = 0 ' 0 setzen
Pulseout Porta , 1 , 40 '>10uS Impuls senden [Port, Portnummer, Impulsdauer in uS / 4]
Config Porta.1 = Input ' Port als Eingang definieren
Pulsein Hindernis , Pina , 1 , 1 ' Eingehende Impulsdauer (in 10uS) messen [Variable, Port, Portnummer, Art]
Hindernis = Hindernis * 10 ' Wert auf Zentimeter berechnen
Hindernis = Hindernis / 58
End Sub
'_______________________________________________________________________________
'***************************** Messrad-Notaus-Abfrage **************************
'-------------------------------------------------------------------------------
Sub Messrad_notaus
'If Notaus = 1 Then ' Wenn das Messrad nach unten gefallen ist, der Roboter also angehoben wurde,
' wird gewartet ob es nach 2/3 Sekunden immer noch unten ist, dann wird alles gestoppt.
' Timer1 = 55000 ' Preload dass der Timer ca eine 2/3 Sekunde bis zum Overflow braucht
' Enable Timer1 ' Timer1 einschalten
'End If
End Sub
'_______________________________________________________________________________
'***************************** Hindernisroutine ********************************
'-------------------------------------------------------------------------------
Sub Hindernis
N = 0 'Displayzählvariable
Motor_links = 128 ' Motoren zunächst in Stillstand versetzen
Motor_rechts = 128
Call Motorausgabe
Cls
Locate 1 , 1
Lcd "*Hind.umfahrung*"
Locate 3 , 1
Lcd "Abstand: "
For I = 1 To 2 ' Sensor und Status abfrage
I2creceive &H40 , Buf(i)
Next I
Rk_sensor = Buf(1)
Klappen_status = Buf(2)
Call Usm_abfrage
If Rk_sensor >= B2 Or Hindernis < 10 Then ' wenn Bumper gedrückt oder Hindernis < 10 cm enfernt
'------ Stückchen zurückfahren, um genug Platz für die Drehung zu haben --------
Locate 2 , 1
Lcd "- Zuruckfahren"
Motor_links = 58
Motor_rechts = 58 ' Rückwärts fahren
Call Motorausgabe
' Locate 4 , 1
' Lcd "l_M:" ; Motor_links ; " r_M:" ; Motor_rechts
Do
Call Usm_abfrage ' Solange fahren, bis das Hindernis weiter als der maximale Hindernisparameter entfernt ist
Incr N
If N = 5 Then ' LCD anzeige
Locate 3 , 10
Lcd " "
Locate 3 , 10
Lcd Hindernis
N = 0
End If
Loop Until Hindernis > Max_hindernis
Motor_links = 128
Motor_rechts = 128
Call Motorausgabe
' Locate 4 , 1
' Lcd "l_M:" ; Motor_links ; " r_M:" ; Motor_rechts
End If
Call Beep
'---------------- Nun drehen, bis Hindernis auf 90° liegt ----------------------
Locate 2 , 1
Lcd " "
Locate 2 , 1
Lcd "- 90 Winkel anf."
N = 0
Call Usm_abfrage
I = Hindernis ' Abstand zwischenspeichern
Motor_rechts = 128 - V_dreh ' Roboter in Drehung versetzen
Motor_links = 128 + V_dreh
Call Motorausgabe
Do
Call Usm_abfrage
Differenz = Hindernis - I
If Differenz > 7 Then ' 7 Zentimeter Toleranz
Servo(1) = Servo(1) - 1 ' Drehe Servo
End If
Incr N
If N = 5 Then
Locate 3 , 10
Lcd " "
Locate 3 , 10
Lcd Hindernis
Locate 4 , 1
Lcd " "
Locate 4 , 1
Lcd "StW:" ; I ; " - Dif:" ; Differenz
N = 0
End If
Call Tasterabfrage ' Möglichkeit um Routine zu verlasssen bei Fehlern
If Taste = 5 Then Exit Sub
Loop Until Servo(1) <= Servo_90 ' Roboter drehen bis kurz vor 90° Position
Motor_rechts = 128 ' Kurz anhalten
Motor_links = 128
Call Motorausgabe
' Warten bis der Roboter steht
Call Beep
'------------------------- Umfahrung des Hindernisses --------------------------
Locate 2 , 1
Lcd " "
Locate 2 , 1
Lcd "Beginn der Umfa."
N = 0
Call Usm_abfrage ' Abstand abfragen
I = Hindernis ' Abstand zwischenspeichern
Motor_links = V_hindernis ' Langsam Vorwärtsfahren
Motor_rechts = V_hindernis
Call Motorausgabe
N = 0
Do
For I = 1 To 2 ' Sensor und Status abfrage
I2creceive &H40 , Buf(i)
Next I
Rk_sensor = Buf(1)
Klappen_status = Buf(2)
If Rk_sensor = 1 Then
Motor_rechts = 128
Motor_links = 128
Call Motorausgabe
Servo(1) = Servo_0
Exit Sub
Else
Call Usm_abfrage ' Abstand abfragen
Differenz = Hindernis - I ' Differenz bilden
If Differenz < 2 And Differenz > -2 Then
Motor_rechts = V_hindernis
Motor_links = V_hindernis
Else
Differenz = Differenz * Hind_x_faktor ' Mit dem Faktor multiplizieren um stärkere Anpassungen vorzunehmen
Motor_rechts = V_hindernis + Differenz ' Wenn das Hindernis weiter legt als zuvor, so wird die Differenz positiv -> rechter Motor dreht schneller -> Linkskurve
Motor_links = V_hindernis - Differenz ' Falls es näher ist, wird die Differenz negativ und der rechte Motor dreht langsamer -> Rechtskurve
If Motor_rechts > Au_motor_max Then Motor_rechts = Au_motor_max
If Motor_rechts < Motor_min Then Motor_rechts = Motor_min
If Motor_links > Au_motor_max Then Motor_links = Au_motor_max
If Motor_links < Motor_min Then Motor_links = Motor_min
End If
End If
Incr N
If N = 5 Then
Differenz = Differenz / 5 ' Zurückrechnen für Anzeige
Locate 3 , 10
Lcd " "
Locate 3 , 10
Lcd Hindernis
Locate 4 , 1
Lcd " "
Locate 4 , 1
Lcd "StW:" ; I ; " - Dif:" ; Differenz
N = 0
End If
Call Motorausgabe
Call Tasterabfrage
Loop Until Taste = 5 ' Die Schleife verlassen, wenn eine Taste gedrückt wird.
Motor_rechts = 128
Motor_links = 128
Call Motorausgabe
Servo(1) = Servo_0
End Sub
'_______________________________________________________________________________
'***************************** Autonomer Modus *********************************
'-------------------------------------------------------------------------------
Sub Autonom
'------------------------------------ Sensorauswertung -------------------------
N = 0
Cls
Locate 1 , 1 : Lcd "* Auto.*" ' Maske für Display aufbauen
Locate 4 , 1 : Lcd "Fehler:"
Do
For I = 1 To 2
I2creceive &H40 , Buf(i)
Next I
Rk_sensor = Buf(1)
Klappen_status = Buf(2)
'------------------------------ Bumper, Hindernis oder fertig? --------------------------
If Klappen_status = 50 Then ' Falls längere Zeit alle Klappen nicht betätigt sind, ist der Rasen vermutlich fertig gemäht
Locate 4 , 8 : Lcd "*FERTIG*"
Do
Integ = Beepparameter ' Summer aktivieren für kurze Zeit
Beepparameter = 3000
Call Beep
Beepparameter = Integ
Call Tasterabfrage
Loop Until Taste = 5
Exit Sub
End If
Call Usm_abfrage ' Ultraschallmodul abfragen
If Rk_sensor >= B2 Then ' Falls das Byte Rk_Sensor 01 xxx xxx ist oder größer (sprich: Bumper gedrückt), sofort anhalten
Call Motorstop ' FEHLT: Die Auswertung ob der linke, rechte oder gar beide Bumper gedrückt sind...
Elseif Hindernis < Max_hindernis Then ' Bei einem Hindernis in weniger als 20cm Entfernung
Call Hindernis ' wird die Hindernisroutine aufgerufen
'------------------------------ normales Verfahren ansonsten -------------------
Else
Xoo = Xoo_alt ' Zurücksetzen der Anpassungsparameter
Xxo = Xxo_alt ' - " - " -
Xxx = Xxx_alt ' - " - " -
Integ = Xoo * Klappen_status ' Je nach Zeitdauer werden die Parameter zusätzlich beeinflusst
Integ = Integ / 10 ' Das Statusbyte (5, 10, 15, 20) soll als direkter Faktor, jedoch noch geteilt durch 10, verwendet werden.
Xoo = Integ
Integ = Xxo * Klappen_status
Integ = Integ / 10
Xxo = Integ
Integ = Xxx * Klappen_status
Integ = Integ / 10
Xxx = Integ
'(----------------------------- VORSICHT WIRR ----------------------------------
Zu_addieren1 = &B00000001
Zu_addieren2 = 0
Zw2_rk = Rk_sensor
For I = 1 To 5
Zw1_rk = Zw2_rk And &B00000001 ' Wenn das letzte Bit vom Byte eine 1 ist bekommt zw_Rk_sensor den Wert 1
If Zw1_rk = 1 Then
Bed1 = 1 ' Wenn das letzte Bit eine 1 ist, wird die erste Bedingung Bed1 1
Else
Bed1 = 0
End If
If Bed1 = 1 And Zw1_rk = 0 Then
Bed2 = 1 ' Wenn Bed1 1 war und das letzte Bit jetzt 0 war, wird Bed2 = 1
Else
Bed2 = 0
End If
If Bed1 = 1 And Bed2 = 1 Then ' Wenn Bed2 1 war und jetzt das letzte Bit auch 1 ist, wurde eine Lücke erkannt
X_addieren = I - 1
For Z = 1 To X_addieren
Shift Zu_addieren1 , Left ' Wert generieren, der dem Sensor addiert werden muss, damit die eingeschlossene 0 zu 1 wird
Next Z
Zu_addieren2 = Zu_addieren2 + Zu_addieren1 '
Zu_addieren1 = &B00000001
Bed2 = 0 ' Bedingung 2 zurücksetzen
End If
Shift Zw2_rk , Right ' Byte nach rechts schieben
Next I
Rk_sensor = Rk_sensor + Zu_addieren2 ' Die Nullen werden ausgebügelt
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
')
'--------------------------------- Motoranpassung ------------------------------
Select Case Rk_sensor ' Byte: Rk_sensor abfragen für Motoranpassung
'----------------------------------- normale Zustände --------------------------
Case &B00000111 ' Für: _ _ _ - - - ; (oder den Fehler - _ -) also dass die Kante genau in der Mitte ist:
Motor_links = Au_motor_max ' Große Rechtskurve, damit die vierte Klappe auch noch gedrückt wird.
Motor_rechts = Au_motor_max - Drossel
Case &B00000011 ' Für: _ _ _ _ - - ; also der Roboter ist zu weit im gemähten und muss mehr RECHTS
Motor_links = Au_motor_max
Motor_rechts = Au_motor_max - Xoo ' Leichte Rechtskurve; FEHLT: Parameter für _ _ _ _ - -
Case &B00000001 ' Für: _ _ _ _ _ - , also der Roboter ist zu weit im gemähten und muss mehr RECHTS
Motor_links = Au_motor_max - Drossel ' Langsamere Gesamtgeschwindigkeit; FEHLT: Parameter für Drosselung
Motor_rechts = Motor_links - Xxo ' Stärkere Rechtskurve: FEHLT: Parameter für _ _ _ _ _ -
Case &B00000000 ' Für: _ _ _ _ _ _ , also der Roboter ist zu weit im gemähten und muss mehr RECHTS
Motor_links = Au_motor_max - Drossel ' FEHLT: Auch hier statt der Drossel 10 ein Parameter
Motor_rechts = Motor_links - Xxx ' Starke Rechtskurve: FEHLT: Parameter für _ _ _ _ _ _
Case &B00001111 ' Für: _ _ - - - - , also der Roboter ist zu weit im ungemähten und muss mehr LINKS
Motor_rechts = Au_motor_max ' Große Linkskurve; FEHLT: gleicher Parameter wie bei _ _ _ - - -
Motor_links = Au_motor_max - Drossel
Case &B00011111 ' Für: _ - - - - - , also der Roboter ist zu weit im ungemähten und muss mehr LINKS
Motor_rechts = Au_motor_max - Drossel ' Langsamere Geschwindigkeit; FEHLT: Auch hier der Parameter wie oben
Motor_links = Motor_rechts - Xxo ' Stärkere Linkskurve; FEHLT: gleicher Parameter wie bei _ _ _ _ - -
Case &B00111111 ' Für: - - - - - - , also der Roboter ist zu weit im ungemähten und muss mehr LINKS
Motor_rechts = Au_motor_max - Drossel ' Langsamere Geschwindigkeit, FEHLT auch hier der Parameter statt der 10
Motor_links = Motor_rechts - Xxx ' Starke Linkskurve, FEHLT: auch hier der Parameter
End Select
End If
Call Motorausgabe ' Motorenwert ausgeben
'---------------- LCD Ausgabe --------------------------------------------------
Incr N
If N = 6 Then ' in jedem 3. Durchgang
N = 0 ' Zählvariable zurücksetzen
Call Akkuspannung
Locate 1 , 12 : Lcd S ; "V"
Locate 2 , 14 : Lcd Hindernis ' Abstand bis zum nächsten Hindernis anzeigen
S = Bin(rk_sensor) ' Klappen formatieren als String, zur Ausgabe
Locate 2 , 1 : Lcd "RK: " ; S ' Sieht z.B. folgendermaßen aus: 00000011
Locate 4 , 1 : Lcd " "
Locate 4 , 1 ' Motorenwerte
Lcd "l_M:" ; Motor_links ; " r_M:" ; Motor_rechts
Locate 3 , 1 : Lcd " "
Locate 3 , 1 : Lcd "SB: " ; Klappen_status
Call Tasterabfrage ' Abbruchbedingung (Taster)
If Taste = 4 Then Call Mahwerk_einschalten
If Taste = 3 Then Call Mahwerk_ausschalten
End If
Loop Until Taste = 5
Call Mahwerk_ausschalten
Motor_rechts = 128
Motor_links = 128
Call Motorausgabe
Autonom_aktivieren = 0
Xoo = Xoo_alt ' Zurücksetzen der Anpassungsparameter
Xxo = Xxo_alt
Xxx = Xxx_alt
End Sub
'_______________________________________________________________________________
'***************************** Fehlerhandhabung ********************************
'-------------------------------------------------------------------------------
Sub Fehler
' Summer = 1 ' Signalton ein
'-------------------------- Fehler auf LCD ausgeben ----------------------------
Cls ' Display löschen
Locate 1 , 1
Lcd "Achtung Fehler!"
Locate 2 , 1 ' Fehler anzeigen
Select Case Fehler ' Entsprechende Fehlermeldung zum Fehler aussuchen
Case 1 : Lcd "I2C Ubertragung"
Case 2 : Lcd "Klappe hangt?"
Case 3 : Lcd "Verfahren?"
Case 4 : Lcd "Fertig gemaht?"
Case 5 : Lcd "MaW: Vorw. heiss"
Case 15 : Lcd "Messrad Notaus!"
Case 100 : Lcd " *** FERTIG *** "
Case Else : Lcd "Fehlernr.: " ; Fehler
End Select
Locate 3 , 1
Lcd "T1: Fortfahren.."
Locate 4 , 1
Lcd "T5: Reset"
'------------------ auf weitere Instruktionen warten ---------------------------
Do
Call Tasterabfrage ' Tastendruck abfragen
If Taste = 5 Then ' Bei Resettaste
Goto &H1C00 ' Softreset durchführen
End If
Loop Until Taste = 1 ' Schleife ansonsten solange durchlaufen lassen,
' bis Taste 1 gedrückt wird, dann zurückspringen
Fehler = 0 ' bzw. davor noch die Fehlervariable auf 0 setzen
End Sub
'_______________________________________________________________________________
'*******************************************************************************
'*************************** ENDE Routinen ************************************
'*******************************************************************************
'-------------------------------------------------------------------------------
'_______________________________________________________________________________
'*******************************************************************************
'************************* Interrupts und Labels *******************************
'*******************************************************************************
'-------------------------------------------------------------------------------
'_______________________________________________________________________________
'**************** Überlauf des Timers für den Signalton u. Messrad *************
'-------------------------------------------------------------------------------
Timer1_uberlauf:
Disable Timer1 ' Timer1 deaktivieren
Summer = 0 'Signalton ausschalten
Print "Auf 0 setzen"
Mahwerk_vw = 0 ' Signalisieren, dass der Anlaufwiderstand zu heiß ist - Mähwerk abschalten
'-------------------------------- Messrad --------------------------------------
'If Notaus = 1 Then ' Wenn das Messrad immer noch unten ist
' Call Mahwerk_ausschalten ' Mähwerk stoppen
' Call Motorstop ' Motoren stoppen
' Fehler = 15 ' Fehlermeldung ausgeben
' Call Fehler
'End If
Return
'_______________________________________________________________________________
'****************** Überlauf des Timers für die Fernsteuerung ******************
'-------------------------------------------------------------------------------
Pauseerkannt: ' Pausenfunktion, wird aufgerufen durch Überlauf von Timer 0, passiert theoretisch alle gute 4ms
Kanal = 0 ' Wenn die lange Pause kommt, gehen die Kanäle wieder von vorne los
Incr T2_zahler
Return ' Zurück zum Code, wo er vorher war
'_______________________________________________________________________________
'****************** Interrupt 1 erkennt eine fallende Flanke *******************
'-------------------------------------------------------------------------------
Zeitmessung: ' Wird aufgerufen bei fallender Flanke
If Kanal > 0 And Kanal < 5 Then ' Da nur die ersten vier Kanäle wichtig sind...
Empfanger(kanal) = Timer2
End If
Timer2 = 0 ' Der Timer wird wieder auf 0 gesetzt
Incr Kanal ' Der Kanal wird um 1 erhöht
Return
'_______________________________________________________________________________
'*******************************************************************************
'*********************** ENDE Interrupts und Labels ****************************
'*******************************************************************************
'-------------------------------------------------------------------------------
End ' formales Ende des Quelltextes