Aus RN-Wissen.de
Wechseln zu: Navigation, Suche
Rasenmaehroboter fuer schwierige und grosse Gaerten im Test

IMGP0415small.JPG

Dieser Artikel ist noch in Bearbeitung.

Listrik01 - Ein selbstbalancierendes Elektroeinrad

Beim Listrik01 handelt es sich um einen selbstbalancierenden Elektroroller. Die Elektronik basiert auf den Veröffentlichungen des Rutscherle 2 [1]. Die Schaltung wurde verändert um I2C Gyro und Accelerometer anschließen zu können. Die Listing wurden außerdem umfangreicher kommentiert.

Antrieb

Der Prototyp fährt mit einem 24V / 500W Gleichstrommotor. Das Gefährt ist allerdings extrem koflastig, deshalb baue ich es um auf einen 48V/1000W BLDC Motor. Vom Hersteller in China direkt importiert. Das Bild oben zeigt den GS-Prototyp. Bei der BLDC Variante werden Batterie, Steuerung und Motor in die Felge integriert. Dies sorgt für einen niedrigen Schwerkunkt. Habe mir ein 18" Motorrad Hinterrad besorgt in das alles integriert wird.

Die Sensorik wurde um 3 weitere Hallsensoren erweitert um einen Trigger je 60 Grad elektrisch zu bekommen. Die Geschwindigkeit ist auf 20km/h begrenzt.

Akkus

Der Gleichstrom Prototyp fährt mit 2 Bleiakkus mit 12V/7AH in Reihe geschaltet. Sie sind für Motoren und Elektronik. Die Entscheidung für Bleiakkus wurde bewusst getroffen. Bei einem Prototyp sind Ergebnisse wichtiger als Eleganz.

Der BLDC Prototyp fährt mit 4 Bleiakkus mit 12V/7AH in Reihe geschaltet. Sie sind für Motoren und Elektronik.

Elektronik

IMGP0416small.JPG

Motorregler mit Atmega 168A sowie eine Hauptsteuerung mit einem Atmega 644PA. Ein Display von Display3000 als Tacho (und Datenlogger). Siehe Stückliste.

Software

Die Software ist in Basic mit BASCOM geschrieben. Zeitkritische Teile in Assembler. Siehe Link oben zum Rutscherle 2 [2].


Motorcontroller.bas (BASCOM)

'******************Listrik01 Motorsteuerung, Version 01 **********************

'Es gibt keine Stromregelung, sondern eine Lageregelung im Hauptcontroller

'LED1 blinkt mit gültigem seriellen Telegramm

'LED2 blinkt wenn Zwangskommutierung
'LED2 dauerlicht wenn serielle Verbindung fehlt, d.h. Not Halt

'LED1 und LED2 blinken abwechselnd mit 1Hz für 3 Sekunden während CPU Anlauf

'LED1 und LED2 blinken mit 2Hz wenn Hallstecker nicht gesteckt ist.
'              Ausserdem antwortet der Controller jetzt nicht auf Telegramme.

'LockBit, FuseBit , FuseBitHigh , FuseBitExtended
'$prog &HFF , &HF9 , &HDF , &HF6       'Fusebits 168A CPU

'Use AMTEL Studio / Fuses Menu instead to set fusebits
'BOOTSZ = 1024W_1C00
'BOOTRST = unchecked
'RSTDISBL = unchecked
'DWEN = unchecked
'SPIEN = checked
'WDTON = unchecked
'EESAVE = unchecked
'BODLEVEL = unchecked
'CKDIV8 = unchecked
'CKOUT = unchecked
'SUT_CKSEL = EXTFSXTAL_1KCK_14CK_4MS1

$regfile = "m168def.dat"       'ATMEGA168A CPU
$crystal = 11059200
$baud = 57600
$hwstack = 32
$swstack = 20
$framesize = 40

Const _pcicr = &B00000101       'Bit0 = PCIE0 = Port B0..B7
                                 'Bit2 = PCIE2 = Port D0..D7

Const Numhall = 3       '<<<<<<<<<< TEST, Normalerweise = 4
Const Pclink = 1       '<<<<<<<<<< TEST, Normalerweise = 0, Sie Delphi

Declare Sub Setspeed
Declare Sub Pwm_start
Declare Sub Pwm_stop

Config Portb.0 = Input       'Hall_D
Config Portb.1 = Output       'B-
Config Portb.2 = Output       'B+
Config Portb.3 = Output       'A-(MOSI)
Config Portb.4 = Output       'LED2 (MISO)
Config Portb.5 = Input       'SCK
'       PortB.6                XTAL
'       PortB.7                XTAL

Config Portc.0 = Input       'UBatterie
Config Portc.1 = Input       'I-Ist
Config Portc.2 = Input       'Kühlkörper Temperatur
Config Portc.3 = Output       'LED1
Config Portc.4 = Output       'Lüfter
Config Portc.5 = Input       'Adress Jumper
'       PortC.6                Reset
'       PortC.7                nicht rausgeführt

'       PortD.0                RxD
'       PortD.1                TxD
Config Portd.2 = Input       'Hall_C
Config Portd.3 = Output       'A+
Config Portd.4 = Input       'Hall_B
Config Portd.5 = Output       'C+
Config Portd.6 = Output       'C-
Config Portd.7 = Input       'Hall_A

Hall_a Alias Pind.7
Hall_b Alias Pind.4
Hall_c Alias Pind.2
Hall_d Alias Pinb.0
Led1 Alias Portc.3
Led2 Alias Portb.4
Fan Alias Portc.4
Adrjumper Alias Pinc.5

'A-Werte stellen die Rotorposition in Abhängigkeit der Halleingänge dar
'Siehe Exceltabelle "Rotorposition Array A().xls"
Dim A(15) As Byte At $100

'S-Werte stellen die aktuelle Soll PWM-Breite dar
Dim S(12) As Byte At $10f

'0 = Vorwärts,  1 = Links,  2 = Leerlauf,  3 =
Dim Richtung(255) As Byte At $11b

'IstRichtung-Werte stellen den 0..2, siehe Zeile oberhalb
Dim Istrichtung As Integer At $21a

'Ticks-Werte stellen die Anzahl verbrauchter Ticks während eine Do..Loop dar
Dim Ticks As Integer At $221

'Anzahl Kommutierungen zwischen seriellen Telegrammen
Dim Uzu As Integer       'Netzspannung
Dim M1 As Byte At Uzu + 0 Overlay
Dim M2 As Byte At Uzu + 1 Overlay

Dim Izu As Integer       'Motorstrom
Dim M3 As Byte At Izu + 0 Overlay
Dim M4 As Byte At Izu + 1 Overlay

Dim Ausgabeticks As Integer       'Anzahl Ticks für ein Do ... Loop
Dim M5 As Byte At Ausgabeticks + 0 Overlay
Dim M6 As Byte At Ausgabeticks + 1 Overlay

Dim Temperatur As Integer       'Kühlkörpertemperatur
Dim M7 As Byte At Temperatur + 0 Overlay
Dim M8 As Byte At Temperatur + 1 Overlay

Dim Drehzahl As Integer       'Drehzahl
Dim M9 As Byte At Drehzahl + 0 Overlay
Dim M10 As Byte At Drehzahl + 1 Overlay

Dim Kalibrierwert As Integer
Dim Kal(2) As Byte At Kalibrierwert + 0 Overlay

Dim Sinus(12) As Byte
Dim Motorposition As Byte
Dim Altrichtung As Integer
Dim Aposition As Byte
Dim Zposition As Byte
Dim Zwangskommutieren As Byte
Dim Temp As Word
Dim Motoradresse As Byte
Dim Vwinkel As Byte
Dim Rwinkel As Byte
Dim N As Byte
Dim Notaus As Byte
Dim Loopcount As Word
Dim Sollrichtung As Byte
Dim Altsollrichtung As Byte
Dim Sollspeed As Byte
Dim Altsollspeed As Byte
Dim Bdummy As Byte
Dim Intbdummy As Byte
Dim Intidummy As Integer
Dim Sdummy As Single
Dim Idummy1 As Integer
Dim Idummy2 As Integer
Dim Uw As Word
Dim Iw As Word
Dim Uwsum As Long
Dim Iwsum As Long
Dim Wcount As Long
Dim Fancount As Long
Dim Uoffset As Single
Dim Ufaktor As Single
Dim Ioffset As Single
Dim Ifaktor As Single
Dim Empfpuffer(6) As Byte       'Empfangspuffer
Dim Ta As Byte
Dim Tb As Byte
Dim Tc As Byte
Dim Fehler As Byte

Dim Errorword As Word
Dim Errorword1 As Byte At Errorword + 0 Overlay
Dim Errorword2 As Byte At Errorword + 1 Overlay

'Integer bzw. Word (2Byte) = %
'Single (4Byte)            = !
'Double (8Byte)            = #
'Long (4Byte)              = & ==> 32Bit Integer
'Byte (1Byte)              = ohne Zusatz

$eeprom       'wird nicht benutzt
 U_offset:
  Data 0.0!       'Single
 U_faktor:
  Data 1.0!       'Single
 I_offset:
  Data 0.0!       'Single
 I_faktor:
  Data 1.0!       'Single
 V_winkel:
  Data 0       'Byte
 R_winkel:
  Data 0       'Byte
 Sinus_12:       'Sinus mit 4 Hallgebern
  Data 0 , 0 , 33 , 66 , 100 , 100 , 100 , 100 , 66 , 33 , 0 , 0
 Sinus_6:       'Sinus mit 3 Hallgebern
  Data 0 , 33 , 66 , 100 , 66 , 33
$data

Sollspeed = 0
Pwm_stop       'Erstmal alles auschalten

For N = 1 To 15
 A(n) = 0
Next

'Variablen für die Richtungserkennung vorbelegen
'Diese Variante frisst zwar viel Arbeitsspeicher, ist aber sehr schnell
For N = 1 To 255
 Richtung(n) = 2
Next

If Adrjumper = 0 Then       'Rechter Motor
 Motoradresse = 82
 #if Numhall = 3       'Anzahl Hallsensoren
  A(5) = 1
  A(1) = 2
  A(3) = 3
  A(2) = 4
  A(6) = 5
  A(4) = 6
 #else
  A(12) = 12
  A(4) = 11
  A(6) = 10
  A(14) = 9
  A(10) = 8
  A(2) = 7
  A(3) = 6
  A(11) = 5
  A(9) = 4
  A(1) = 3
  A(5) = 2
  A(13) = 1
 #endif

 Richtung(21) = 3
 Richtung(93) = 3
 Richtung(220) = 3
 Richtung(196) = 3
 Richtung(70) = 3
 Richtung(110) = 3
 Richtung(234) = 3
 Richtung(162) = 3
 Richtung(35) = 3
 Richtung(59) = 3
 Richtung(185) = 3
 Richtung(145) = 3
 Richtung(81) = 1
 Richtung(25) = 1
 Richtung(155) = 1
 Richtung(179) = 1
 Richtung(50) = 1
 Richtung(42) = 1
 Richtung(174) = 1
 Richtung(230) = 1
 Richtung(100) = 1
 Richtung(76) = 1
 Richtung(205) = 1
 Richtung(213) = 1
Else
 Motoradresse = 76       'Linker Motor
 #if Numhall = 3       'Anzahl Hallsensoren
  A(3) = 1
  A(1) = 2
  A(5) = 3
  A(4) = 4
  A(6) = 5
  A(2) = 6
 #else
  A(11) = 1
  A(3) = 2
  A(1) = 3
  A(9) = 4
  A(13) = 5
  A(5) = 6
  A(4) = 7
  A(12) = 8
  A(14) = 9
  A(6) = 10
  A(2) = 11
  A(10) = 12
 #endif

 Richtung(236) = 3
 Richtung(196) = 3
 Richtung(69) = 3
 Richtung(93) = 3
 Richtung(217) = 3
 Richtung(145) = 3
 Richtung(19) = 3
 Richtung(59) = 3
 Richtung(186) = 3
 Richtung(162) = 3
 Richtung(38) = 3
 Richtung(110) = 3
 Richtung(42) = 1
 Richtung(171) = 1
 Richtung(179) = 1
 Richtung(49) = 1
 Richtung(25) = 1
 Richtung(157) = 1
 Richtung(213) = 1
 Richtung(84) = 1
 Richtung(76) = 1
 Richtung(206) = 1
 Richtung(230) = 1
 Richtung(98) = 1
End If

'AD-Wandler einstellen
Config Adc = Single , Prescaler = Auto , Reference = Off
Start Adc

'<Test Hallsensoren>
Fehler = 1
Fan = 1
While Fehler = 1
 Fehler = 0
 Motorposition = 0
 Motorposition.0 = Hall_a
 Motorposition.1 = Hall_b
 Motorposition.2 = Hall_c
 #if Numhall = 3       'Anzahl Hallsensoren
  Motorposition.3 = 0
 #else
  Motorposition.3 = Hall_d
 #endif
 If Motorposition = 0 Then Fehler = 1       'Stecker nicht gesteckt
 If Motorposition > 0 Then
  'Das währe eine Motorposition die es nicht gibt
  If A(motorposition) = 0 Then Fehler = 1
 End If
 If Fehler = 1 Then
  Toggle Led1
  Toggle Led2
  Waitms 250       '==> 2Hz
 End If
Wend
'</Test Hallsensoren>

On Timer2 Update_pwm Nosave

'Pin-Change Interrupt einstellen: (Hall Impulse)
Pcmsk2 = &B10010100       'Hall A, B, C
Pcmsk1 = &B00000000
Pcmsk0 = &B00000001       'Hall D
Pcicr = _pcicr

'Wenn sich das Hallsignal ändert, wird kommutiert
On Pcint2 Kommutieren Nosave
On Pcint0 Kommutieren Nosave

'EEPROM mit dem Programm Motorcontroller_EEPROM.bas vorbesetzen
' Datenreihenfolge dort ist ab Byte(1):
'     U_offset, U_faktor, I_offset, I_faktor im Single Format
'     Voreilwinkel, Nacheilwinkel im Byte Format
'Sinuskurve 12 Werte im Byte Format für 4 Hall Sensoren
'Sinuskurve 6 Werte im Byte Format für 3 Hall Sensoren

'Readeeprom Uoffset , 1
Uoffset = 0

'Readeeprom Ufaktor , 5
Ufaktor = 5.37109375       'Siehe Hardwarezeichnung für Berechnung

'Readeeprom Ioffset , 9
Ioffset = 512       '2,5V

'Readeeprom Ifaktor , 13
Ifaktor = 7.8125       'Siehe Hardwarezeichnung für Berechnung

'Voreilwinkel:
'Readeeprom Vwinkel , 17
Vwinkel = 0

'Readeeprom Rwinkel , 18
Rwinkel = 0

If Ufaktor = 0 Then Ufaktor = 1.0
If Ifaktor = 0 Then Ifaktor = 1.0

'Angepasste Sinuskurve aus dem EEprom laden
#if Numhall = 3       'Anzahl Hallsensoren
'  For N = 1 To 6
'   Bdummy = N + 30
'   Readeeprom Sinus(n) , Bdummy
'  Next
   Sinus(1) = 0
   Sinus(2) = 33
   Sinus(3) = 66
   Sinus(4) = 100
   Sinus(5) = 66
   Sinus(6) = 33
#else       'Anzahl Hallsensoren = 4
'  For N = 1 To 12
'   Bdummy = N + 18
'   Readeeprom Sinus(n) , Bdummy
'  Next
   Sinus(1) = 0
   Sinus(2) = 0
   Sinus(3) = 33
   Sinus(4) = 66
   Sinus(5) = 100
   Sinus(6) = 100
   Sinus(7) = 100
   Sinus(8) = 100
   Sinus(9) = 66
   Sinus(10) = 33
   Sinus(11) = 0
   Sinus(12) = 0
#endif

Fan = 1       'Lüfter kurz einschalten damit die Lager nicht fest werden
For N = 1 To 3       '3 sekunden
 Led1 = 1
 Led2 = 0
 Waitms 500
 Toggle Led1
 Toggle Led2
 Waitms 500
Next
Led2 = 0
Fan = 0

On Urxc Datenempfang Nosave
Enable Urxc       'serielles UART. No parity, 1 stop bit, 8 data bits
Enable Interrupts

Do
 Led1 = 0
 '<Messen>
 Uw = Getadc(0) + 14       'Versorgungsspannung. Siehe Hardwarezeichnung
 Uwsum = Uwsum + Uw
 Iw = Getadc(1)       'Gesamtstrom
 Iwsum = Iwsum + Iw
 Incr Wcount
 'Kühkörpertemperatur in counts. Siehe Datenblätter\10K_NTC_Kennlinie.odt
 Temp = Getadc(2)
 '</Messen>
 '<Serielle Telegramme abarbeiten>
 ' Empfpuffer(1) Empfpuffer(2)   Empfpuffer(3) Empfpuffer(4) Empfpuffer(5) Empfpuffer(6)
 ' <CR>          Contr.Adresse      Richtung       PWM        Checksum        <CR>
 If Empfpuffer(1) = 13 Then       'Das letzte im Ring verschobene empfangene CR
  If Empfpuffer(6) = 13 Then
   Led1 = 1
   If Empfpuffer(2) = Motoradresse Then       '"R" oder "L"
    Disable Urxc
     'BCC für Empfangsmessage berechnen
     !lds R24,{Empfpuffer(2)}       'Adresse. Load Direct from data space @address xxx
     !LDs R25,{Empfpuffer(3)}       'Richtung.Load Direct from data space @address xxx
     !add R24,R25
     !LDs R25,{Empfpuffer(4)}       'PWM. Load Direct from data space @address xxx
     !add R24,R25
     !STS {Bdummy},R24       'Write Rxx to address loaction
    If Bdummy = Empfpuffer(5) Then       'Die Prüfsumme stimmt
     'Kommutieren deaktivieren, weil sonst die Ticks während dem Zugriff
     'geändert werden könnten
     Pcicr = &B00000000
     Ausgabeticks = Ticks
     Ticks = 0
     Pcicr = _pcicr
     'BCC für Sendemessage berechnen
     !lds R24,{M1}       'Load Direct from data space @address xxx into Rxx
     !LDs R25,{M2}       'Load Direct from data space @address xxx into Rxx
     !add R24,R25       'Add Rxx + Ryy, Result in Rxx
     !LDs R25,{M3}       'Load Direct from data space @address xxx into Rxx
     !add R24,R25       'Add Rxx + Ryy, Result in Rxx
     !LDs R25,{M4}       'Load Direct from data space @address xxx into Rxx
     !add R24,R25       'Add Rxx + Ryy, Result in Rxx
     !LDs R25,{M5}       'Load Direct from data space @address xxx into Rxx
     !add R24,R25       'Add Rxx + Ryy, Result in Rxx
     !LDs R25,{M6}       'Load Direct from data space @address xxx into Rxx
     !add R24,R25       'Add Rxx + Ryy, Result in Rxx
     !STS {Bdummy},R24       'Write Rxx to address loaction
     'Antwort am UART ausgeben
     'M1/M2=Spg  M3/M4=Strom M5/M6=Loop Ticks Bdummy=Checksum
     'Print M1 ; M2 ; M3 ; M4 ; M5 ; M6 ; Bdummy ; 13
     'in Assembler geht das so...
     !LdS     R24,{M1}       'Load Direct from data space @address xxx into Rxx
     !STS     udr,R24       'Write Rxx to address loaction
    Warten1:
     !LDS     R24,UCSR0A       'Load Direct from data space @address xxx into Rxx
     !BST     R24,5       'Store bit x of Rxx in T Flag
     !Brtc warten1
     !LDS     R24,{M2}       'Load Direct from data space @address xxx into Rxx
     !STS     udr,R24       'Write Rxx to address loaction
    Warten2:
     !LDS     R24,UCSR0A       'Load Direct from data space @address xxx into Rxx
     !BST     R24,5       'Store bit x of Rxx in T Flag
     !Brtc warten2
     !lds     R24,{M3}       'Load Direct from data space @address xxx into Rxx
     !STS     udr,R24       'Write Rxx to address loaction
    Warten3:
     !LDS     R24,UCSR0A       'Load Direct from data space @address xxx into Rxx
     !BST     R24,5       'Store bit x of Rxx in T Flag
     !Brtc warten3
     !LDS     R24,{M4}       'Load Direct from data space @address xxx into Rxx
     !STS     udr,R24       'Write Rxx to address loaction
    Warten4:
     !LDS     R24,UCSR0A       'Load Direct from data space @address xxx into Rxx
     !BST     R24,5       'Store bit x of Rxx in T Flag
     !Brtc warten4
     !LDS     R24,{M5}       'Load Direct from data space @address xxx into Rxx
     !STS     udr,R24       'Write Rxx to address loaction
    Warten5:
     !LDS     R24,UCSR0A       'Load Direct from data space @address xxx into Rxx
     !BST     R24,5       'Store bit x of Rxx in T Flag
     !Brtc warten5
     !LDS     R24,{M6}       'Load Direct from data space @address xxx into Rxx
     !STS     udr,R24       'Write Rxx to address loaction
    Warten6:
     !LDS     R24,UCSR0A       'Load Direct from data space @address xxx into Rxx
     !BST     R24,5       'Store bit x of Rxx in T Flag
     !Brtc warten6
     !LDS     R24,{bdummy}
     !STS     udr,R24       'Write Rxx to address loaction
    Warten7:
     !LDS     R24,UCSR0A       'Load Direct from data space @address xxx into Rxx
     !BST     R24,5       'Store bit x of Rxx in T Flag
     !Brtc warten7
     !LDi     R24,13
     !STS     udr,R24       'Write Rxx to address loaction
    Warten8:
     !LDS     R24,UCSR0A       'Load Direct from data space @address xxx into Rxx
     !BST     R24,5       'Store bit x of Rxx in T Flag
     !Brtc warten8
     If Empfpuffer(3) < 3 Then       'Richtung
      Notaus = 0
      Loopcount = 0
      Sollrichtung = Empfpuffer(3)       'Richtung
      Sollspeed = Empfpuffer(4)       'PWM Breite
      If Sollrichtung = 2 Then
       Sollrichtung = 1
       Notaus = 1
       Loopcount = 250
       Sollspeed = 0
      End If
      Setspeed
      '<Messwerte berechnen>
      'Messwerte werden hier berechnet, weil genau jetzt 1/100s Zeit ist.
      'Spannung berechnen
      Sdummy = Uwsum / Wcount
      Sdummy = Sdummy - Uoffset
      Sdummy = Sdummy * Ufaktor
      Uzu = Int(sdummy)
      Uwsum = 0
      'Strom berechnen
      Sdummy = Iwsum / Wcount
      Sdummy = Sdummy - Ioffset
      Sdummy = Sdummy * Ifaktor
      Izu = Int(sdummy)
      Iwsum = 0
      Wcount = 0
      '</Messwerte berechnen>
     Else       'wird nie erreicht weil weder Hauptsteuerung noch Display diese
                'Werte senden
      Select Case Empfpuffer(3)
       Case 3       'Sinus(1) empfangen
        Sinus(1) = Empfpuffer(4)
        Writeeeprom Sinus(1) , 17
       Case 4       'Sinus(2) empfangen
        Sinus(2) = Empfpuffer(4)
        Writeeeprom Sinus(2) , 18
       Case 5       'Sinus(3) empfangen
        Sinus(3) = Empfpuffer(4)
        Writeeeprom Sinus(3) , 19
       Case 6       'Sinus(4) empfangen
        Sinus(4) = Empfpuffer(4)
        Writeeeprom Sinus(4) , 20
       Case 7       'Sinus(5) empfangen
        Sinus(5) = Empfpuffer(4)
        Writeeeprom Sinus(5) , 21
       Case 8       'Sinus(6) empfangen
        Sinus(6) = Empfpuffer(4)
        Writeeeprom Sinus(6) , 22
       Case 9       'Sinus(7) empfangen
        Sinus(7) = Empfpuffer(4)
        Writeeeprom Sinus(7) , 23
       Case 10       'Sinus(8) empfangen
        Sinus(8) = Empfpuffer(4)
        Writeeeprom Sinus(8) , 24
       Case 11       'Sinus(9) empfangen
        Sinus(9) = Empfpuffer(4)
        Writeeeprom Sinus(9) , 25
       Case 12       'Sinus(10) empfangen
        Sinus(10) = Empfpuffer(4)
        Writeeeprom Sinus(10) , 26
       Case 13       'Sinus(11) empfangen
        Sinus(11) = Empfpuffer(4)
        Writeeeprom Sinus(11) , 27
       Case 14       'Sinus(12) empfangen
        Sinus(12) = Empfpuffer(4)
        Writeeeprom Sinus(12) , 28
       Case 15       'Spannung Kalibrieren (Byte 1)
        Kal(1) = Empfpuffer(4)
       Case 16       'Spannung Kalibrieren (Byte 2)
        Kal(2) = Empfpuffer(4)
        Sdummy = Uwsum / Wcount
        Ufaktor = Kalibrierwert / Sdummy
        Writeeeprom Ufaktor , 5
        Uoffset = 0       'Schaltungsbedingt
        Writeeeprom Uoffset , 1
       Case 17       'Strom Kalibrieren (Byte 1)
        Kal(1) = Empfpuffer(4)
       Case 18       'Strom Kalibrieren Byte (2)
        Kal(2) = Empfpuffer(4)
        Sdummy = Iwsum / Wcount
        If Kalibrierwert = 0 Then       'Das ist der Offset
         Ioffset = Sdummy + 1
         Writeeeprom Ioffset , 9
        Else       'Jetzt den Faktor berechnen
         Sdummy = Sdummy - Ioffset
         Ifaktor = Kalibrierwert / Sdummy
         Writeeeprom Ifaktor , 13
        End If
       Case 19
        Vwinkel = Empfpuffer(4)
        Writeeeprom Vwinkel , 29
       Case 20
        Rwinkel = Empfpuffer(4)
        Writeeeprom Rwinkel , 30
      End Select
     End If
     !LDI     R24,0
     !sTS     {Empfpuffer(1)},R24       'Write Rxx to address loaction
     !sTS     {Empfpuffer(2)},R24       'Write Rxx to address loaction
     !sTS     {Empfpuffer(3)},R24       'Write Rxx to address loaction
     !sTS     {Empfpuffer(4)},R24       'Write Rxx to address loaction
     !sTS     {Empfpuffer(5)},R24       'Write Rxx to address loaction
    End If
    Enable Urxc
   End If
  End If
 End If
 '</Serielle Telegramme abarbeiten>

 'Lüfter einschalten wenn Temperatur > 57 GradC
 'Lüfter ausschalten wenn Temperatur < 45 GradC
 If Temp < 350 Then
  Fan = 1
  Fancount = 120000       'Nachlaufzeit, Zykluszeit 1/100 Sekunde = 150 Sekunden
 Else
  If Temp > 532 Then
   If Fancount = 0 Then       'Nachlaufzeit beendet
    Fan = 0
   End If
  End If
 End If
 If Fancount > 0 Then Decr Fancount

 'Notaus mit Freilauf wenn keine Nachricht von Seriell
 If Loopcount < 50 Then       'Das sind schätzungsweise 0.1s
  Notaus = 0
  Incr Loopcount
 Else
  Notaus = 1
  Sollspeed = 0
  Setspeed
 End If
Loop

Kommutieren:
 !push    R10       'Rxx sichern
 !PUSH    R11       'Rxx sichern
 !PUSH    R16       'Rxx sichern
 !PUSH    R17       'Rxx sichern
 !PUSH    R20       'Rxx sichern
 !PUSH    R21       'Rxx sichern
 !PUSH    R24       'Rxx sichern
 !PUSH    R26       'Rxx sichern
 !PUSH    R27       'Rxx sichern
 !IN      R24,sreg
 !PUSH    R24
 !clr R16       'Clear Rxx
 #if Numhall = 3
  !andi R17,&B01110111       'Clear Hall-D Bits
 #else
  !lds R17, $0023       'Port B input pins address. Hall D
  !BST R17,0       'Store bit x of Rxx in T Flag. Hall D-Bit
  !BLD R16,3       'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
 #endif
 !LDS R17, $0029       'Port D input pins address. Hall A,B,C
 !BST R17,2       'Store bit x of Rxx in T Flag. Hall C-Bit
 !bld R16,2       'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
 !BST R17,4       'Store bit x of Rxx in T Flag. Hall B-Bit
 !bld R16,1       'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
 !BST R17,7       'Store bit x of Rxx in T Flag. Hall A-Bit
 !bld R16,0       'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
 'R16 hat nun 0000DCBA Hall Signale
 !lds R17,{aposition}       'Load Direct from data space @address xxx
 !Swap R17       'R17 hat nun DCBA0000 Hall Signale
 !andi R17,&B11110000       'Bit-And of Rxx with a constant value. Die 4 Bit sind Hall A...D
 'R17 hat nun nur noch die oberen 4 Bit
 !add R17,R16       'Rxx + Ryy. Ergebnis in Rxx
 !STS {aposition},R17       'Write Rxx to address loaction
 !CLR     R17
 !LDI     R26,$FF       'Startadresse von A() - 1
 !LDI     R27,$00
 !ADD     R26,R16
 !ADC     R27,R17
 !LD      R24,X       'Load Direct from address xxx
 !LDS     R16,{Sollrichtung} 'Load Direct from data space @address xxx
 !CPI     R16,0
 !BREQ    Sollrichtungnull       'Branch out if equal
 !Jmp     Sollrichtungnichtnull
Sollrichtungnull:
 !LDS     R20,{Vwinkel}   'Load Direct from data space @address xxx
 !Add     R24,R20
 !JMP     Richtunggesetzt
Sollrichtungnichtnull:
 !LDS     R20,{Rwinkel}
 !Add     R24,R20
Richtunggesetzt:
 #if Numhall = 3
  !CPI     R24,$07       'Compare Rxx with constant 6 + 1 Sinusschritte
 #else
  !CPI     R24,$0D       'Compare Rxx with constant 12 + 1 Sinusschritte
 #endif
 !BRCC    Istgroesser13_1
 !JMP     Istkleiner13_1
Istgroesser13_1:
 #if Numhall = 3
   !SUBI    R24,$06       'Subtract constant from Rxx, result in Rxx. 6 Sinusschritte
 #else
   !SUBI    R24,$0C       'Subtract constant from Rxx, result in Rxx. 12 Sinusschritte
 #endif
Istkleiner13_1:
 !CLR     R20
 !Ldi     R26,$0E       'Startadresse von S() - 1
 !LDI     R27,$01       'Startadresse von S() - 1
 !ADD     R26,R24
 !ADC     R27,R20
 !LD      R16,X       'Load Direct from address xxx
 !STS     {Ta},R16       'Write Rxx to address loaction
 !subi     R24,$fc
 #if Numhall = 3
   !CPI R24,$07       'Compare Rxx with constant 6 + 1 Sinusschritte
 #else
   !CPI R24,$0D       'Compare Rxx with constant 12 + 1 Sinusschritte
 #endif
 !BRCC    Istgroesser13_2
 !JMP     Istkleiner13_2
Istgroesser13_2:
 #if Numhall = 3
   !SUBI    R24,$06       'Subtract constant from Rxx, result in Rxx. 6 Sinusschritte
 #else
   !SUBI    R24,$0C       'Subtract constant from Rxx, result in Rxx. 12 Sinusschritte
 #endif
Istkleiner13_2:
 !CLR     R20
 !Ldi     R26,$0E       'Startadresse von S() - 1
 !LDI     R27,$01       'Startadresse von S() - 1
 !ADD     R26,R24
 !ADC     R27,R20
 !LD      R16,X    'Load Direct from address xxx
 !STS     {Tc},R16 'Write Rxx to address loaction
 !subi     R24,$fc
 #if Numhall = 3
   !CPI     R24,$07       'Compare Rxx with constant 6 + 1 Sinusschritte
 #else
   !CPI     R24,$0D       'Compare Rxx with constant 12 + 1 Sinusschritte
 #endif
 !BRCC    Istgroesser13_3
 !JMP     Istkleiner13_3
Istgroesser13_3:
 #if Numhall = 3
   !SUBI    R24,$06       'Subtract constant from Rxx, result in Rxx. 6 Sinusschritte
 #else
   !SUBI    R24,$0C       'Subtract constant from Rxx, result in Rxx. 12 Sinusschritte
 #endif
Istkleiner13_3:
 !CLR     R20
 !Ldi     R26,$0E       'Startadresse von S() - 1
 !LDI     R27,$01       'Startadresse von S() - 1
 !ADD     R26,R24
 !ADC     R27,R20
 !LD      R16,X    'Load Direct from address xxx
 !STS     {Tb},R16 'Write Rxx to address loaction
 !lds     R10,{aposition}
 !CLR     R11       'Clear Rxx
 !LDI     R26,$1A       'Startadresse von Richtung() - 1
 !LDI     R27,$01       'Startadresse von Richtung() - 1
 !ADD     R26,R10       'move Ptr by {aposition} innnerhalb Richtung()
 !ADC     R27,R11       'Add Zero
 !LD      R16,X+       'Load Rxx with data from address in X Register(R26, R27) and increment Ptr
 'R16 hat Inhalt von Richtung(X+)
 !CLR     R17       'Clear Rxx
 !LDI     R20,$02       '2 Byte
 !LDI     R21,$00
 !Sub R16 , R20       'Werte von einander abziehen
 !SBC     R17,R21       'Werte von einander abziehen
 !LDI     R26,$1A       'Startadresse von IstRichtung()
 !LDI     R27,$02       'Startadresse von IstRichtung()
 !ST      X+,R16       'Store Rxx in address in X + 1 Register
 !ST      X,R17
 !LDI     R26,$41      'Startadresse von Ticks()
 !LDI     R27,$02      'Startadresse von Ticks()
 !LD      R16,X+       'Load Rxx with data from address in X Register
 !LD      R17,X
 !LDI     R26,$1A       'Startadresse von IstRichtung()
 !LDI     R27,$02       'Startadresse von IstRichtung()
 !LD      R20,X+       'Load Rxx with data from address in X Register
 !LD      R21,X
 !ADD     R16,R20
 !ADC     R17,R21
!LDI     R26,$41       'Startadresse von Ticks()
 !LDI     R27,$02      'Startadresse von Ticks()
 !ST      X+,R16       'Store Rxx in address in X + 1 Register
 !ST      X,R17
 !LDS     R24,timsk2
 !ORI     R24,$01
 !STS     timsk2,R24 'Write Rxx to address loaction
 !POP     R24
 !Out Sreg , R24
 !POP     R27 'Rxx wieder herstellen
 !POP     R26'Rxx wieder herstellen
 !POP     R24'Rxx wieder herstellen
 !POP     R21 'Rxx wieder herstellen
 !POP     R20 'Rxx wieder herstellen
 !POP     R17'Rxx wieder herstellen
 !POP     R16 'Rxx wieder herstellen
 !POP     R11 'Rxx wieder herstellen
 !POP     R10       'Rxx wieder herstellen
Return

Update_pwm:
'Ta, Tb und Tc sind die Amplituden der 3 Phasen
 !PUSH    R24
 !IN      R24,sreg
 !PUSH    R24
 !lds     R24,{Ta}
 !STS     ocr2a,R24 'Write Rxx to address loaction
 !STS     ocr2b,R24
 !LDS     R24,{Tb}
 !STS     ocr1al,R24 'Write Rxx to address loaction
 !STS     ocr1bl,R24
 !lds     R24,{Tc}
 !Out Ocr0a , R24
 !Out Ocr0b , R24
 !LDS     R24,timsk2
 !ANDI    R24,$FE
 !STS     timsk2,R24 'Write Rxx to address loaction
 !POP     R24 'Rxx wieder herstellen
 !Out Sreg , R24
 !POP     R24 'Rxx wieder herstellen
Return

Datenempfang:
 'Die empfangenen Zeichen im nach links laufenden Umlauf in Empfpuffer(1)..Empfpuffer(6) geschrieben
 'Dadurch wir sichergestellt, dass das Startzeichen (CR) irgendwann in EP0 steht
 !PUSH R24       'Rxx sichern
 !IN R24, SREG       'Statusregistr sichern
 !PUSH R24
 'Zeichen im Puffer nach links schieben
 !LDS R24,{Empfpuffer(2)}       'Load Direct from data space @address xxx
 !STS {Empfpuffer(1)},R24       'Write Rxx to address loaction
 !LDS R24,{Empfpuffer(3)}       'Load Direct from data space @address xxx
 !STS {Empfpuffer(2)},R24       'Write Rxx to address loaction
 !LDS R24,{Empfpuffer(4)}       'Load Direct from data space @address xxx
 !STS {Empfpuffer(3)},R24       'Write Rxx to address loaction
 !LDS R24,{Empfpuffer(5)}       'Load Direct from data space @address xxx
 !STS {Empfpuffer(4)},R24       'Write Rxx to address loaction
 !LDS R24,{Empfpuffer(6)}       'Load Direct from data space @address xxx
 !STS {Empfpuffer(5)},R24       'Write Rxx to address loaction
 'neues Zeichen einlesen
 !IN  R24,UDR       'Load an I/O Location to Rxx
 !STs {Empfpuffer(6)},R24       'Write Rxx to address loaction
 !POP R24       'Rxx wieder herstellen
 !Out Sreg , R24       'Statusregister wieder herstellen
 !POP R24       'Rxx wieder herstellen
Return

Sub Setspeed
 If Notaus = 1 Then
  Pwm_stop
  Sollspeed = 0
  For N = 1 To 12
   S(n) = 0
  Next
 Else
  #if Numhall = 3       'Anzahl Hallsensoren
    For N = 1 To 6
     Idummy1 = Sollspeed * Sinus(n)
     Idummy1 = Idummy1 / 100
     S(n) = Idummy1
    Next
  #else
    For N = 1 To 12
     Idummy1 = Sollspeed * Sinus(n)
     Idummy1 = Idummy1 / 100
     S(n) = Idummy1
    Next
  #endif

  If Tccr0a = 0 Then Pwm_start       'Wenn die PWM aus ist

  '<Zwangskommutieren??>
  Zwangskommutieren = 0
  If Sollrichtung <> Altsollrichtung Then Zwangskommutieren = 1
  If Sollspeed <> Altsollspeed Then Zwangskommutieren = 1

  If Zwangskommutieren = 1 Then
   Pcicr = &B00000000       'Den eigentlichen Kommutierinterrupt abschalten
   !clr R16       'Clear Register 16
   !lds R17, $0023       'Load Direct from data space @address xxx
   !BST R17,0       'Store bit x of Rxx in T Flag
   !BLD R16,3       'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
   !LDS R17, $0029       'Load Direct from data space @address xxx
   !BST R17,2       'Store bit x of Rxx in T Flag
   !bld R16,2       'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
   !BST R17,4       'Store bit x of Rxx in T Flag
   !bld R16,1       'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
   !BST R17,7       'Store bit x of Rxx in T Flag
   !bld R16,0       'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
   !lds R17,{aposition}       'Load Direct from data space @address xxx
   !Swap R17       'Swap high and low nibbles in Rxx. aposition is now in the upper nibble
   !andi R17,&B11110000       'Logical AND Rxx and a constant, Result in Rxx. Clear lower nibble
   !add R17,R16       'Add Rxx + Ryy, Result in Rxx
   !STS {aposition},R17       'Write Rxx to address loaction
   !CLR     R17       'Clear Rxx
   !LDI     R26,$FF       'Startadresse von A() - 1
   !LDI     R27,$00       'Startadresse von A() - 1
   !ADD     R26,R16       'Add Rxx + Ryy without carry, Result in Rxx
   !ADC     R27,R17       'Add Rxx + Ryy with carry, Result in Rxx
   !LD      R24,X       'Load Indirect Rxx with address in X Register
   !LDS     R16,{Sollrichtung}       'Load Direct from data space @address xxx
   !CPI     R16,0       'Compare Rxx with constant
   !BREQ    Sollrichtungnullz       'Branch if Equal
   !Jmp     Sollrichtungnichtnullz       'Relative Jump
  Sollrichtungnullz:
   !LDS     R20,{Vwinkel}       'Load Direct from data space @address xxx
   !Add     R24,R20       'Add Rxx + Ryy, Result in Rxx
   !JMP     Richtunggesetztz
  Sollrichtungnichtnullz:
   !LDS     R20,{Rwinkel}       'Load Direct from data space @address xxx
   !Add     R24,R20       'Add Rxx + Ryy without carry, Result in Rxx
  Richtunggesetztz:
   #if Numhall = 3
     !CPI     R24,$07       'Compare Rxx with constant 6 + 1 Sinusschritte
   #else
     !CPI     R24,$0D       'Compare Rxx with constant 12 + 1 Sinusschritte
   #endif
   !BRCC    Istgroesser13_1z       'Branch if Carry Cleared
   !JMP     Istkleiner13_1z       'Relative Jump
  Istgroesser13_1z:
   #if Numhall = 3
    !SUBI    R24,$06       'Subtract constant from Rxx, result in Rxx. 6 Sinusschritte
   #else
    !SUBI    R24,$0C       'Subtract constant from Rxx, result in Rxx. 12 Sinusschritte
   #endif
  Istkleiner13_1z:
   !CLR     R20       'Clear Rxx
   !Ldi     R26,$0E       'Startadresse von S() - 1
   !LDI     R27,$01       'Startadresse von S() - 1
   !ADD     R26,R24       'Add Rxx + Ryy without carry, Result in Rxx
   !ADC     R27,R20       'Add Rxx + Ryy with carry, Result in Rxx
   !LD      R16,X       'Load Rxx with data from address in X Register
   !STS     {Ta},R16       'Write Rxx to address loaction
   !subi     R24,$fc       'Subtract constant from Rxx, result in Rxx
   #if Numhall = 3
     !CPI     R24,$07       'Compare Rxx with constant 6 + 1 Sinusschritte
   #else
     !CPI     R24,$0D       'Compare Rxx with constant 12 + 1 Sinusschritte
   #endif
   !BRCC    Istgroesser13_2z       'Branch if Carry Cleared
   !JMP     Istkleiner13_2z       'Relative Jump
  Istgroesser13_2z:
   #if Numhall = 3
    !SUBI    R24,$06       'Subtract constant from Rxx, result in Rxx. 6 Sinusschritte
   #else
    !SUBI    R24,$0C       'Subtract constant from Rxx, result in Rxx. 12 Sinusschritte
   #endif
  Istkleiner13_2z:
   !CLR     R20       'Clear Rxx
   !Ldi     R26,$1a       'Startadresse von S() - 1
   !LDI     R27,$01       'Startadresse von S() - 1
   !ADD     R26,R24       'Add Rxx + Ryy without carry, Result in Rxx
   !ADC     R27,R20       'Add Rxx + Ryy with carry, Result in Rxx
   !LD      R16,X       'Load Rxx with data from address in X Register
   !STS     {Tc},R16       'Write Rxx to address loaction
   !subi     R24,$fc       'Subtract constant from Rxx, result in Rxx
   #if Numhall = 3
    !CPI     R24,$07       'Compare Rxx with constant 6 + 1 Sinusschritte
   #else
    !CPI     R24,$0D       'Compare Rxx with constant 12 + 1 Sinusschritte
   #endif
   !BRCC    Istgroesser13_3z       'Branch if Carry Cleared
   !JMP     Istkleiner13_3z       'Relative Jump
  Istgroesser13_3z:
   #if Numhall = 3
    !SUBI    R24,$06       'Subtract constant from Rxx, result in Rxx. 6 Sinusschritte
   #else
    !SUBI    R24,$0C       'Subtract constant from Rxx, result in Rxx. 12 Sinusschritte
   #endif
  Istkleiner13_3z:
   !CLR     R20       'Clear Rxx
   !Ldi     R26,$1a       'Startadresse von S() - 1
   !LDI     R27,$01       'Startadresse von S() - 1
   !ADD     R26,R24       'Add Rxx + Ryy without carry , Result in Rxx
   !ADC     R27,R20       'Add Rxx + Ryy with carry, Result in Rxx
   !LD      R16,X       'Load Rxx with data from address in X Register
   !STS     {Tb},R16       'Write Rxx to address loaction
   Pcicr = _pcicr       'Den eigentlichen Kommutierinterrupt einschalten
   Timsk2.toie2 = 1       'Timer2 Overflow Interrupt aktivieren
   Toggle Led2
  End If
  '</Zwangskommutieren>
 End If
 Altsollrichtung = Sollrichtung
 Altsollspeed = Sollspeed
End Sub

Sub Pwm_start
Disable Interrupts       'Damit die Timer auch wirklich synchron laufen
 Tccr0a = 161   'Timer Counter Control Register A
 Tccr0b = 1       'Timer Counter Control Register B
 Ocr0a = 0      'Set output compare Ctr 0, Register A, continuously compared with TCNT0
 Ocr0b = 0       'Set output compare Ctr 0, Register B, continuously compared with TCNT0
 Tccr1a = 161      'Timer Counter Control Register 1A
 Tccr1b = 1       'Timer Counter Control Register 1B
 Ocr1ah = 0      'Output A compare value in H and L Bytes
 Ocr1al = 0
 Ocr1bh = 0       'Output B compare value in H and L Bytes
 Ocr1bl = 0
 Tccr2a = 161     'Timer Counter Control Register 2A
 Tccr2b = 1       'Timer Counter Control Register 2B
 Ocr2a = 0        'Set output compare Ctr 2, Register A, continuously compared with TCNT0
 Ocr2b = 0       'Set output compare Ctr 2, Register B, continuously compared with TCNT0
 Tcnt0 = 0       'Timer Startwert
 Tcnt1 = 6       'Timer Startwert, Wert anders damit sie alle synchron laufen
 Tcnt2 = 9       'Timer Startwert, Wert anders damit sie alle synchron laufen
Enable Interrupts
End Sub

Sub Pwm_stop
 Tccr0a = 0       'Timer Counter Control Register A
 Tccr0b = 0       'Timer Counter Control Register B
 Ocr0a = 0       'Set output compare Ctr 0, Register A, continuously compared with TCNT0
 Ocr0b = 0       'Set output compare Ctr 0, Register B, continuously compared with TCNT0
 Tccr1a = 0       'Timer Counter Control Register 1A
 Tccr1b = 0       'Timer Counter Control Register 1B
 Ocr1ah = 0       'Output A compare value in H and L Bytes
 Ocr1al = 0
 Ocr1bh = 0       'Output B compare value in H and L Bytes
 Ocr1bl = 0
 Tccr2a = 0       'Timer Counter Control Register 2A
 Tccr2b = 0       'Timer Counter Control Register 2B
 Ocr2a = 0       'Set output compare Ctr 2, Register A, continuously compared with TCNT0
 Ocr2b = 0       'Set output compare Ctr 2, Register B, continuously compared with TCNT0
 'alle Phasen Aus
 Portd.3 = 0       'A+
 Portb.3 = 1       'A-
 Portb.2 = 0       'B+
 Portb.1 = 1       'B-
 Portd.5 = 0       'C+
 Portd.6 = 1       'C-
End Sub

Hauptsteuerung.bas (BASCOM)

'( ******************Listrik01 Hauptsteuerung, Version 01 ******************

1. LED1 blinkt mit 1 Hz wenn alles OK

2. LED1 und LED2 blinken mit 5Hz wenn I2c-Bus gestört ist

3. LED1 und LED2 blinken mit bis zu 10Hz wenn U-Batt zu klein ist. Frequenz hängt
   von der Anzahl Interrups ab, die während der Wartezeit auflaufen, z.B. von COM

4. Lade-LEDs
     - blinken mit 4Hz wenn Gyro nicht auf Null ist
     - blinken mit 5Hz wenn Reset button gedrückt wird und Gyrosignale groß sind
     - machen Lichtorgel wenn Kopplung zum Motorcontroler gestört ist. Siehe Punkt 6

5. Gyro wird nur zur Initialisierung benutzt. Fahrzeug muss still stehen beim START

6. Kommunikation zum Motorcontroller ist gestört (Timeout) wenn der Hallstecker
   am Motorcontroller nicht steckt sind! Dadurch antwortet der Motorcontroller dem
   Hauptcontroller nicht. Er sendet z.B. keine Spannungsinfo an den Hauptcontroller.
   Dadurch macht Dieser ständig einen Restart. Viele Werte werden dann nicht zum
   Dashboard übertragen!

LockBit, FuseBit , FuseBitHigh , FuseBitExtended
$prog &HFF , &HE7 , &HD9 , &HFF       'Fusebits für Atmega644PA 20MHz

Use AMTEL Studio / Fuses Menu instead to set fusebits
BODLEVEL = disabled
OCDEN = Disabled
JTAGEN = Disabled
SPIEN = Checked
WDTON = Disabled
EESAVE = Disabled
BOOTSZ = 4096W_7000
BOOTRST = Disabled
CKDIV8 = Disabled
CKOUT = Disabled
SUT_CKSEL = FSOSZ_16KCK_4MS1_XOSC_FASTPWR
')

$regfile = "m644def.dat"       'ATMEGA644PA CPU
$lib "i2c_twi.lbx"
$framesize = 64
$swstack = 64
$hwstack = 64
$crystal = 20000000
$baud = 57600       'No parity, 1 stop bit, 8 data bits

Declare Sub Onresetbtn
Declare Sub Logtext(byval Adresse As Word , Text As String )
Declare Sub Akkugrenzen
Declare Sub Unfalldatenschreiber
Declare Sub Betriebsdatenschreiber
Declare Sub Flashled(byval Which As Byte , Byval Dauer_ms As Byte)
Declare Sub Resetdata

'       Porta.0 = ADC
'       Porta.1 = ADC
Config Porta.2 = Input       'not used
Config Porta.3 = Input       'not used
Config Porta.4 = Input       'not used
Config Porta.5 = Input       'not used
Config Porta.6 = Input       'not used
Config Porta.7 = Input       'not used
'Pull up Widerstände der unbenutzten Eingänge einschalten
Porta = 1

Config Portb.0 = Input       'not used
Config Portb.1 = Output       'Gyro Self Test
Config Portb.2 = Output       'Gyro Reset
Config Portb.3 = Output       'Led 2
Config Portb.4 = Output       'LED 1
'       PORTB.5               'MOSI
'       PORTB.6               'MISO
'       PORTB.7               'SCK

Config Scl = Portc.0
Config Sda = Portc.1

Config Portc.2 = Input       'Fussschalter 1
Config Portc.3 = Input       'Fussschalter 2
Config Portc.4 = Input       'not used
Config Portc.5 = Input       'not used
Config Portc.6 = Input       'not used
Config Portc.7 = Output       'Beeper
'Pull up Widerstände der Eingänge Ein/Ausschalten
Portc.2 = 0
Portc.3 = 0
Portc.4 = 1
Portc.5 = 1
Portc.6 = 1

'       PORTD.0 =             'RXD
'       PORTD.1 =             'TXD
Config Portd.2 = Input       'Reset Button vom Akku-Display
Config Portd.3 = Output       'Batterieanzeige
Config Portd.4 = Output       'Batterieanzeige
Config Portd.5 = Output       'Batterieanzeige
Config Portd.6 = Output       'Batterieanzeige
Config Portd.7 = Output       'Batterieanzeige
'Pull up Widerstände einschalten
Portd.2 = 1

Led1 Alias Portb.4
Led2 Alias Portb.3
Led3 Alias Portd.4       'Batterieladezustandsanzeige 0..20%
Led4 Alias Portd.5       'Batterieladezustandsanzeige 21..40%
Led5 Alias Portd.6       'Batterieladezustandsanzeige 41..60%
Led6 Alias Portd.3       'Batterieladezustandsanzeige 61..80%
Led7 Alias Portd.7       'Batterieladezustandsanzeige 81..100%

Beeper Alias Portc.7
Resetbtn Alias Pind.2

'Sensortest gibt es bei I2C Gyro nicht
'Sensor_aus Alias Portb.2        '1=Aus        0=Ein
'Sensor_test Alias Portb.1       '1=Self-Test. 0=Normalbetrieb

Config Debounce = 100       '[mS] when the config statement is not used a
'                              default of 25mS will be used
'******************************Gyro über I2C ********************************
'##### 7 - Bit Adresses Gyro ########
Const Who_am_i = &H0F       'auch V2
Const Ctrl_reg1 = &H20       'auch V2
Const Ctrl_reg2 = &H21       'auch V2
Const Ctrl_reg3 = &H22       'auch V2
Const Ctrl_reg4 = &H23       'auch V2
Const Ctrl_reg5 = &H24       'auch V2
Const Out_temp = &H26       'auch V2
Const Status_reg = &H27       'auch V2
Const Out_x_l = &H28       'auch V2
Const Out_x_h = &H29       'auch V2
Const Out_y_l = &H2A       'auch V2
Const Out_y_h = &H2B       'auch V2
Const Out_z_l = &H2C       'auch V2
Const Out_z_h = &H2D       'auch V2
'##### 7-Bit Adresses Accelerometer ########
Const Ctrl_reg1_a = &H20       'auch V2
Const Ctrl_reg2_a = &H21       'auch V2
Const Ctrl_reg3_a = &H22       'auch V2
Const Ctrl_reg4_a = &H23       'auch V2
Const Ctrl_reg5_a = &H24       'auch V2
Const Status_reg_a = &H27       'auch V2
Const Out_x_l_a = &H28       'auch V2
Const Out_x_h_a = &H29       'auch V2
Const Out_y_l_a = &H2A       'auch V2
Const Out_y_h_a = &H2B       'auch V2
Const Out_z_l_a = &H2C       'auch V2
Const Out_z_h_a = &H2D       'auch V2
'##### 7-Bit Adresses Magnetometer ########
'Const Cra_reg_m = &H00       'auch V2
'Const Crb_reg_m = &H01       'auch V2
'Const Mr_reg_m = &H02       'auch V2
'Const Out_x_h_m = &H03       'auch V2

'Immer *2 weil Adresse bei Bit1 beginnt. Bit 0 ist für Read/Write
Const Gyroadress_w = 210       '=105*2, =107d*2 bei V2. Gyro slave adress for write
Const Gyroadress_r = 211       '=(105*2) + 1, =107d*2 bei V2. Gyro slave adress for write
Const Accadress_w = 48       '=24*2, auch bei V2 Accelerometer slave adress for write
Const Accadress_r = 49       '=(24*2) + 1, auch bei V2 Accelerometer slave adress for write

'Const Magadress_w = 60       '=30*2, auch bei V2 Magnetometer slave adress for write
'Const Magadress_r = 61       '=(30*2) + 1, auch bei V2 Magnetometer slave adress for write
'--------------------------------------------------------------------

'>>>>>>>>>>>>>>>> DEFAULT WERTE <<<<<<<<<<<<<<<<<<

Const Fussschalterschwelle = 750       'ADC Wert

'Konstanten für die Geschwindigkeits und Streckenmessung
Const Kom_pro_u = 276       '276*Kommutieren für eine Umdrehung.
Const Radumpfang = 139       'RaduMPFang in Zentimeter
'Maximalwert bis zu dem die Abweichung in y aufaddiert wird
Const Max_mit_acc_nick_s = 1024
Const Min_mit_acc_nick_s = -1024
Const Max_lenk = 128
Const Min_lenk = -128
'Konstanten für die Leistungsbegrenzung, Geschwindigkeitsbegrenzung
'Maximalleistung
Const Maxpwm = 255
Const Minpwm = -255
'PWM ab der automatisch gebremst wird um blos nie an die Grenze zu kommen
Const Spwm = 128
Const Spwmr = -128
'Offsetwerte für die Mittellage
Const Offset_roll = 67700
Const Offset_nick = 65400
Const O_roll_u = 66200
Const O_roll_o = 69200
Const O_nick_u = 63900
Const O_nick_o = 66900
Const O_nick_oo = 67700

Const Vmax = 120       '12kmh Vmax Vorwärts in km/h*10
Const Vmaxr = -120       '-12Kmh Vmax Rückwärts in km/h*10

Const Systemvoltage = 14       '14, 24, 36, 48V

'Konstanten für die Akkus
#if Systemvoltage = 14       'Nur zum Test mit Netzteil
 Const Akkuleer = 1300
 Const Akkufastleer = 1330
#endif

#if Systemvoltage = 24
 '24V, 12V pro Akku * 2 Akku
 Const Akkuleer = 2400
 Const Akkufastleer = 2500
#endif

#if Systemvoltage = 36
'36V, 12V pro Akku * 3 Akku
Const Akkuleer = 3600
Const Akkufastleer = 3750
#endif

#if Systemvoltage = 48
'48V, 12V pro Akku * 4 Akku
Const Akkuleer = 4800
Const Akkufastleer = 4950
#endif

'Konstanten für die Signalaufbereitung
Const Gyro_anz = 10       'war 10000
Const Acc_anz = 5       'Anzahl der Messungen zur Mittelwertbildung
Const Gyro_faktor = 1       'war 91
'****************************************************************************

Dim Ugemessen As Long       'Batteriespannung in V*100, kommt vom Motorcontr.
Dim Igemessen As Long       'Gesamter Motorstrom in mA, kommt vom Motorcontr.

Dim U As Integer
Dim I As Integer At U + 2
Dim K As Integer At U + 4
Dim Checksumme As Byte At U + 6
Dim Ende As Byte At U + 7
Dim Motormeldung(8) As Byte At U + 0 Overlay

Dim M1 As Byte At U + 0 Overlay
Dim M2 As Byte At U + 1 Overlay
Dim M3 As Byte At I + 0 Overlay
Dim M4 As Byte At I + 1 Overlay
Dim M5 As Byte At K + 0 Overlay
Dim M6 As Byte At K + 1 Overlay
Dim M7 As Byte At Checksumme Overlay
Dim M8 As Byte At Ende Overlay

'Variablen für die Telegramme an die Motorregler
Dim Checksum_rechts As Byte
Dim Checksum_links As Byte
Dim Richtung_rechts As Byte
Dim Richtung_links As Byte
Dim Pwm_rechts As Byte
Dim Pwm_links As Byte
Dim Zeiger_meldung As Byte
Dim Ta As Byte       'Interruptdauer. Wenn lang, dann ist der Zählwert nahe 255
Dim U_links As Integer       'Motorspannung linker Motor
Dim U_rechts As Integer       'Motorspannung rechter Motor
Dim I_links As Integer       'Motorstrom linker Motor
Dim I_rechts As Integer       'Motorstrom rechter Motor
Dim K_links As Integer       'Anzahl Kommutierungen linker Motor
Dim K_rechts As Integer       'Anzahl Kommutierungen rechter Motor
Dim B(5) As Byte

Dim Kilometer As Word
Dim Kilometermeldung(2) As Byte At Kilometer Overlay

Dim Akkustand As Integer
Dim Akkumeldung(2) As Byte At Akkustand Overlay

Dim Kmh As Integer
Dim Kmhmeldung(2) As Byte At Kmh Overlay

Dim Meter As Integer
Dim Metermeldung(2) As Byte At Meter Overlay

Dim Di As Single       'Distanz pro Kommutieren
Dim Kmhfaktor As Single
Dim Zentimeter As Single
Dim Strecke As Single       'Gefahrene Strecke während 1/100s
Dim Kx As Single       'Anzahl Kommutierungen für Streckenberechnung
Dim Kv As Single       'Anzahl Kommutierungen für Geschw.Berechnung
Dim Vbrems As Long       'Bremse bei Überlast oder zu hoher Geschw.
Dim Lbrems As Long
Dim Brems As Long

Dim P_faktor As Single
Dim Pf1 As Byte At P_faktor + 0 Overlay
Dim Pf2 As Byte At P_faktor + 1 Overlay
Dim Pf3 As Byte At P_faktor + 2 Overlay
Dim Pf4 As Byte At P_faktor + 3 Overlay

Dim I_faktor As Single
Dim If1 As Byte At I_faktor + 0 Overlay
Dim If2 As Byte At I_faktor + 1 Overlay
Dim If3 As Byte At I_faktor + 2 Overlay
Dim If4 As Byte At I_faktor + 3 Overlay

Dim D_faktor As Single
Dim Df1 As Byte At D_faktor + 0 Overlay
Dim Df2 As Byte At D_faktor + 1 Overlay
Dim Df3 As Byte At D_faktor + 2 Overlay
Dim Df4 As Byte At D_faktor + 3 Overlay

Dim Gyro_nick As Integer       'war word
Dim Gyro_nick1 As Byte At Gyro_nick + 0 Overlay
Dim Gyro_nick2 As Byte At Gyro_nick + 1 Overlay

Dim Gyro_roll As Integer       'war word
Dim Gyro_roll1 As Byte At Gyro_roll + 0 Overlay
Dim Gyro_roll2 As Byte At Gyro_roll + 1 Overlay

Dim Gyro_gier As Integer       'war word
Dim Gyro_gier1 As Byte At Gyro_gier + 0 Overlay
Dim Gyro_gier2 As Byte At Gyro_gier + 1 Overlay

Dim Gyro_roll_alt As Integer       'war word
Dim Gyro_nick_alt As Integer       'war word

Dim Acc_nick As Long
Dim Acc_nick1 As Byte At Acc_nick + 0 Overlay
Dim Acc_nick2 As Byte At Acc_nick + 1 Overlay

Dim Accnick As Integer
Dim Accnick_l As Byte At Accnick + 0 Overlay
Dim Accnick_h As Byte At Accnick + 1 Overlay

Dim Acc_roll As Long
Dim Acc_roll1 As Byte At Acc_roll + 0 Overlay
Dim Acc_roll2 As Byte At Acc_roll + 1 Overlay

Dim Accroll As Integer
Dim Accroll_l As Byte At Accroll + 0 Overlay
Dim Accroll_h As Byte At Accroll + 1 Overlay

Dim Acc_gier As Long
Dim Acc_gier1 As Byte At Acc_gier + 0 Overlay
Dim Acc_gier2 As Byte At Acc_gier + 1 Overlay

Dim Accgier As Integer
Dim Accgier_l As Byte At Accgier + 0 Overlay
Dim Accgier_h As Byte At Accgier + 1 Overlay

Dim Sum_acc_roll As Long
Dim Sum_acc_nick As Long
Dim Sum_gyro_roll As Long
Dim Sum_gyro_nick As Long
Dim Korr_acc_roll As Long
Dim Korr_acc_nick As Long

'Long ==> Long Integer
Dim Mit_acc_nick_s As Long       'Summierter Neigung vorwärts Mittelwert
Dim Mit_gyro_roll As Long
Dim Mit_gyro_nick As Long
Dim Sig_gyro_roll As Long
Dim Sig_gyro_nick As Long

Dim Stellwert As Single
Dim Stellwert1 As Byte At Stellwert + 0 Overlay
Dim Stellwert2 As Byte At Stellwert + 1 Overlay
Dim Stellwert3 As Byte At Stellwert + 2 Overlay
Dim Stellwert4 As Byte At Stellwert + 3 Overlay

Dim Mrg As Single
Dim Mrg1 As Byte At Mrg + 0 Overlay
Dim Mrg2 As Byte At Mrg + 1 Overlay
Dim Mrg3 As Byte At Mrg + 2 Overlay
Dim Mrg4 As Byte At Mrg + 3 Overlay

Dim Mra As Single
Dim Mra1 As Byte At Mra + 0 Overlay
Dim Mra2 As Byte At Mra + 1 Overlay
Dim Mra3 As Byte At Mra + 2 Overlay
Dim Mra4 As Byte At Mra + 3 Overlay

Dim Kksum As Single
Dim Kksum1 As Byte At Kksum + 0 Overlay
Dim Kksum2 As Byte At Kksum + 1 Overlay
Dim Kksum3 As Byte At Kksum + 2 Overlay
Dim Kksum4 As Byte At Kksum + 3 Overlay

Dim Gyronick As Long
Dim Gyronick1 As Byte At Gyronick + 0 Overlay
Dim Gyronick2 As Byte At Gyronick + 1 Overlay
Dim Gyronick3 As Byte At Gyronick + 2 Overlay
Dim Gyronick4 As Byte At Gyronick + 3 Overlay

Dim Mp_faktor As Single
Dim Mpf1 As Byte At Mp_faktor + 0 Overlay
Dim Mpf2 As Byte At Mp_faktor + 1 Overlay
Dim Mpf3 As Byte At Mp_faktor + 2 Overlay
Dim Mpf4 As Byte At Mp_faktor + 3 Overlay

Dim Mv_faktor As Single
Dim Mvf1 As Byte At Mv_faktor + 0 Overlay
Dim Mvf2 As Byte At Mv_faktor + 1 Overlay
Dim Mvf3 As Byte At Mv_faktor + 2 Overlay
Dim Mvf4 As Byte At Mv_faktor + 3 Overlay

Dim Mpl_faktor As Single
Dim Mplf1 As Byte At Mpl_faktor + 0 Overlay
Dim Mplf2 As Byte At Mpl_faktor + 1 Overlay
Dim Mplf3 As Byte At Mpl_faktor + 2 Overlay
Dim Mplf4 As Byte At Mpl_faktor + 3 Overlay

Dim Mvl_faktor As Single
Dim Mvlf1 As Byte At Mvl_faktor + 0 Overlay
Dim Mvlf2 As Byte At Mvl_faktor + 1 Overlay
Dim Mvlf3 As Byte At Mvl_faktor + 2 Overlay
Dim Mvlf4 As Byte At Mvl_faktor + 3 Overlay

Dim P_tacho As Single
Dim Tachomeldung(30) As Byte At P_tacho Overlay
Dim T1 As Byte At P_tacho + 0 Overlay
Dim T2 As Byte At P_tacho + 1 Overlay
Dim T3 As Byte At P_tacho + 2 Overlay
Dim T4 As Byte At P_tacho + 3 Overlay

Dim T5 As Byte At P_tacho + 4 Overlay
Dim T6 As Byte At P_tacho + 5 Overlay
Dim T7 As Byte At P_tacho + 6 Overlay
Dim T8 As Byte At P_tacho + 7 Overlay
Dim I_tacho As Single At P_tacho + 4 Overlay

Dim T9 As Byte At P_tacho + 8 Overlay
Dim T10 As Byte At P_tacho + 9 Overlay
Dim T11 As Byte At P_tacho + 10 Overlay
Dim T12 As Byte At P_tacho + 11 Overlay
Dim D_tacho As Single At P_tacho + 8 Overlay

Dim T13 As Byte At P_tacho + 12 Overlay
Dim T14 As Byte At P_tacho + 13 Overlay
Dim T15 As Byte At P_tacho + 14 Overlay
Dim T16 As Byte At P_tacho + 15 Overlay
Dim Mp_tacho As Single At P_tacho + 12 Overlay

Dim T17 As Byte At P_tacho + 16 Overlay
Dim T18 As Byte At P_tacho + 17 Overlay
Dim T19 As Byte At P_tacho + 18 Overlay
Dim T20 As Byte At P_tacho + 19 Overlay
Dim Mv_tacho As Single At P_tacho + 16 Overlay

Dim T21 As Byte At P_tacho + 20 Overlay
Dim T22 As Byte At P_tacho + 21 Overlay
Dim T23 As Byte At P_tacho + 22 Overlay
Dim T24 As Byte At P_tacho + 23 Overlay
Dim Mpl_tacho As Single At P_tacho + 20 Overlay

Dim T25 As Byte At P_tacho + 24 Overlay
Dim T26 As Byte At P_tacho + 25 Overlay
Dim T27 As Byte At P_tacho + 26 Overlay
Dim T28 As Byte At P_tacho + 27 Overlay
Dim Mvl_tacho As Single At P_tacho + 24 Overlay

Dim T29 As Byte At P_tacho + 28 Overlay
Dim T30 As Byte At P_tacho + 29 Overlay
Dim Tdummy As Integer At P_tacho + 28 Overlay

Dim Wwert As Word
Dim Wwert1 As Byte At Wwert + 0 Overlay
Dim Wwert2 As Byte At Wwert + 1 Overlay

Dim Gyro_gier_alt As Integer       'war word
Dim Sum_gyro_gier As Long
Dim Mit_gyro_gier As Long
Dim Sig_gyro_gier As Long

Dim Werteneu As Byte

Dim Mit_acc_roll As Long       'Neigungsmittelwert seitwärts
Dim Mit_acc_roll1 As Byte At Mit_acc_roll + 0 Overlay
Dim Mit_acc_roll2 As Byte At Mit_acc_roll + 1 Overlay
Dim Mit_acc_roll3 As Byte At Mit_acc_roll + 2 Overlay
Dim Mit_acc_roll4 As Byte At Mit_acc_roll + 3 Overlay

Dim Mit_acc_nick As Long       'Neigungsmittelwert vorwärts
Dim Mit_acc_nick1 As Byte At Mit_acc_nick + 0 Overlay
Dim Mit_acc_nick2 As Byte At Mit_acc_nick + 1 Overlay
Dim Mit_acc_nick3 As Byte At Mit_acc_nick + 2 Overlay
Dim Mit_acc_nick4 As Byte At Mit_acc_nick + 3 Overlay

Dim Null_roll As Long
Dim Null_roll1 As Byte At Null_roll + 0 Overlay
Dim Null_roll2 As Byte At Null_roll + 1 Overlay
Dim Null_roll3 As Byte At Null_roll + 2 Overlay
Dim Null_roll4 As Byte At Null_roll + 3 Overlay

Dim Null_nick As Long
Dim Null_nick1 As Byte At Null_nick + 0 Overlay
Dim Null_nick2 As Byte At Null_nick + 1 Overlay
Dim Null_nick3 As Byte At Null_nick + 2 Overlay
Dim Null_nick4 As Byte At Null_nick + 3 Overlay

Dim Mit_regelzeit_ges As Single

Dim Lagefehler As Long
'Die Zeit in 1/100s die benötigt wird bis die Nullage erreicht ist
Dim Regelzeit As Word
Dim Regelzeitmax As Word
Dim Mit_acc_roll_max As Long
Dim Mit_acc_nick_max As Long
Dim Mit_acc_roll_min As Long
Dim Mit_acc_nick_min As Long

Dim Pwm_gesamt As Single
Dim Pwm_rechts_max As Byte
Dim Pwm_links_max As Byte
Dim Intcount As Byte
Dim K_diff As Single
Dim K_sum As Single
Dim K_diff_min As Single
Dim K_diff_max As Single
Dim Pwm_lenk_rechts As Integer
Dim Pwm_lenk_links As Integer

Dim Lenkvorgabe As Single
Dim Mp_anpassung As Single
Dim Mv_anpassung As Single
Dim Mpl_anpassung As Single
Dim Mvl_anpassung As Single
Dim K_sum_anpassung As Single

Dim Wdummy1 As Word
Dim Ldummyi As Long
Dim Ldummy1 As Long
Dim Ldummy2 As Long
Dim Bdummy1 As Byte
Dim Idummy1 As Integer
Dim Sdummy1 As Single

Dim Antriebaus As Byte
Dim Notaus As Byte
Dim Sensor_ok As Byte       'Zeigt an ob die Mittelwerte der Gyros stabil stehen
Dim Sensor_ok_count As Byte
Dim Ltest As Byte

Dim Imin As Long       'Kleinster gemessener Motorstrom (größte Rückspeisung)
Dim Imax As Long       'größter gemessener Motorstrom
Dim Summe_as_entladen As Long
Dim Summe_mah_entladen As Long
Dim Summe_as_laden As Long
Dim Summe_mah_laden As Long
Dim Akkukap As Long       'Akkukapazität 0...100 %
Dim Akku1 As Long       '80% Grenze
Dim Akku2 As Long       '60% Grenze
Dim Akku3 As Long       '40% Grenze
Dim Akku4 As Long       '20% Grenze
Dim Akku5 As Long       '0% Grenze
Dim Erzeugt As Long       'Erzeugte Ladung in mA-Stunden
Dim Verbraucht As Long       'Verbrauchte Ladung in mA-Stunden

'Single Variablen um die Prozente rechnen zu können
Dim Akkuinhalt As Single       'wie AkkuKap
Dim Akkuverbrauch As Single
Dim Akkuprozent As Single

Dim Bzeit As Long       'Beruhigungszeit von Gyro und Accelerometer
Dim Bstate As Byte       'Status der Beruhigungszeit von Gyro und Accelerometer
Dim Frei As Byte       'hat es etwas mit Gyro und Acc zu tun?
Dim Timeout As Byte
Dim S_err As Byte       'Serielle Schnittstellen Error
Dim Seriell_err_links As Word       'COM error links
Dim Seriell_err_rechts As Word       'COM error rechts
Dim Tamax As Byte       'Maximalwert von Ta
Dim Fusscount As Word       'Fusschalter nicht betätigt ==> zeitverzögerung
Dim Blinkcount As Byte       'verschiedene Blinkraten
Dim Lenkerstellung As Single
Dim Lenkstellwert As Single
Dim Lenkfehler As Single

'Texte für den Unfalldatenschreiber (als Text im eeprom)
Dim Utext As String * 32

Dim Ugrund As String * 32       'Auslösegrund
Dim Text As String * 16       'Allg. Ttext
Dim Min_v As Integer       'Minimalwert der Geschwindigkeit
Dim Max_v As Integer       'Maximalwert der Geschwindigkeit

Dim Tachocount As Byte       'Schleifenzähler um Werte an Display zu schicken
Dim Fussschalter As Byte       'Die beiden Fussschalter
Dim Adc0 As Word       'Analogwert Fussschalter 1
Dim Adc1 As Word       'Analogwert Fussschalter 2

Dim Slaveadress As Byte       'Adress of the I2C slave to read or write
Dim Subadress As Byte       'the I2C register to read or write
Dim Databyte As Byte       'I2C Byte to read or write

Dim Resetbtndone As Byte

Dim Nummotors As Byte       '1 oder 2
Dim Errorword As Word       'See Delphi Dashboard.prj
Dim Errorword1 As Byte At Errorword + 0 Overlay
Dim Errorword2 As Byte At Errorword + 1 Overlay

Led1 = 0
Led2 = 0

Led3 = 1
Led4 = 1
Led5 = 1
Led6 = 1
Led7 = 1

'***************** Check! *********************
Nummotors = 1
'***************** Check! *********************

'Ein paar feste Werte vorab berechnen:
Di = Radumpfang / Kom_pro_u
'Eigentlich 3.6 aber bei der Mittelwertbildung entsteht der 4-Fache Wert,
'darum 9, und es ergibt sich automateisch V in 0.1km/h
'Unbenutzte Pins auf Masse legen
Kmhfaktor = Di * 9

'Sensorboard Schalten
'Sensor_aus = 0
'Sensor_test = 0

Config Adc = Single , Prescaler = Auto , Reference = Internal_2.56       'Avcc
'Start Adc not required because ADC will start immediately after Config ADC

'Inititialise I2C-Bus
I2cinit
Config Twi = 200000       ' wanted clock frequency in Hertz
'will set TWBR and TWSR
'Twbr = 12 'bit rate register
'Twsr = 0 'pre scaler bits

'TWI gleich einschalten, das macht Bascom ansonsten erst beim I2CStart !
Twcr = &B00000100

Gosub Init_gyro
Gosub Init_acc

'Wartezeit bis die Motorregler soweit sind
Readeeprom Max_v , $2f0
Readeeprom Min_v , $2f2
Readeeprom Summe_as_entladen , $300       'Entnommene Wh und Ws aus EEPROM lesen
Led3 = 0
Waitms 150
Readeeprom Summe_mah_entladen , $310
Led4 = 0
Waitms 150
Readeeprom Summe_as_laden , $320
Led5 = 0
Waitms 150
Readeeprom Summe_mah_laden , $330
Led6 = 0
Waitms 150
Readeeprom Akkukap , $340
Led7 = 0
Waitms 150
Readeeprom Kilometer , $350
Readeeprom Meter , $360
Readeeprom Zentimeter , $370

'<Faktoren für den PID-Regler einlesen>
P_faktor = 0.0033       'Readeeprom P_faktor , $380
I_faktor = 0.00006       'Readeeprom I_faktor , $384
D_faktor = 0.08       'Readeeprom D_faktor , $388
Mp_faktor = 0.006       'Readeeprom Mp_faktor , $38c
Mv_faktor = 0       'Readeeprom Mv_faktor , $390
Mpl_faktor = 0       'Readeeprom Mpl_faktor , $394
Mvl_faktor = 0       'Readeeprom Mvl_faktor , $398

'Sinnvolle Werte vorbelegen
If P_faktor < 0 Or P_faktor > 1 Then P_faktor = 0.0033
If I_faktor < 0 Or I_faktor > 1 Then I_faktor = 0.00006
If D_faktor < 0 Or D_faktor > 1 Then D_faktor = 0.08
If Mp_faktor < 0 Or Mp_faktor > 1 Then Mp_faktor = 0.006

If Mv_faktor < 0 Or Mv_faktor > 1 Then Mv_faktor = 0
If Mpl_faktor < 0 Or Mpl_faktor > 1 Then Mpl_faktor = 0
If Mvl_faktor < 0 Or Mvl_faktor > 1 Then Mvl_faktor = 0

'</Faktoren für den PID-Regler einlesen>
If Kilometer = $ffff Then       'Kommt bei frischem EEPROM vor
 Zentimeter = 0
 Meter = 0
 Kilometer = 0
 Min_v = 0
 Max_v = 0
End If
'Bei Fehlerhaften Werten aus dem EEPROM einfach 10000mAh als Vorgabe
If Akkukap <= 0 Then Akkukap = 10000

#if Systemvoltage = 14       'Nur zum Test mit Netzteil
 If Akkukap > 32000 Then Akkukap = 100
#endif

#if Systemvoltage = 24
 If Akkukap > 32000 Then Akkukap = 100
#endif

#if Systemvoltage = 36
 If Akkukap > 32000 Then Akkukap = 100
#endif

#if Systemvoltage = 48
 If Akkukap > 32000 Then Akkukap = 100
#endif

Call Akkugrenzen
Fusscount = 0
Notaus = 0
Readeeprom Max_v , $2f0
Readeeprom Min_v , $2f4
Config Watchdog = 128       'mSec

'Maximalwerte für Datenschreiber/Notauserkennung auf 0 setzen
Regelzeit = 0
Regelzeitmax = 0
Mit_acc_nick_max = 0
Mit_acc_roll_max = 0
Pwm_rechts_max = 0
Pwm_links_max = 0
K_diff_min = 0
K_diff_max = 0
Richtung_links = 2       'Leerlauf
Richtung_rechts = 2       'Leerlauf
Pwm_links = 0
Pwm_rechts = 0
Pwm_gesamt = 0
Mit_acc_nick_s = 0
Lenkerstellung = 0

K_rechts = 0
K_links = 0
Seriell_err_links = 0
Seriell_err_rechts = 0
'Vrbelegen um zu sehen ob es einen Notaus ohne Grund gibt
Ugrund = "----------------"

Disable Interrupts

'Timer wird vorgeladen um die gewünschte Zykluszeit nach
'Timerüberlauf zu erreichen
'Nachteil: Timer Count ist nicht exakt gleich der Zykluszeit
On Timer0 Regel_interrupt
Config Timer0 = Timer , Prescale = 1024       ' (1/20MHz) * 1024 = 51,2uS pro Tick
Enable Timer0

Antriebaus = 1       'Beim Start ist immer alles aus
Enable Interrupts
Waitms 100

 '< Sicherheitstest. CPU will Reset if one of these conditions are met
 Start Watchdog       'nach 128mS wird CPU Reset ausgelöst
 If Resetbtn = 0 Then Wait 10
 If Antriebaus = 0 Then Wait 10
 'If Ugemessen < 1000 Then Wait 10       '10V
 'If Ugemessen > 5500 Then Wait 10       '55V
 Stop Watchdog
 '< Sicherheitstest

'Prüfung ob Akku gerade geladen:
#if Systemvoltage = 14
 'Sensoren ausschalten wenn am Ladegerät oder zu Testzwecken
 'If Ugemessen < 2200 Then
 ' Sensor_aus = 1
 'Else
 ' Sensor_aus = 0
 'End If
 Errorword.0 = 1

 While Ugemessen < 1100       'Unterspannung oder am Ladegerät
  Call Flashled(0 , 50)
 Wend
 If Ugemessen > 1520 Then       'Akku ist ganzgeladen
#endif

#if Systemvoltage = 24
 'Sensoren ausschalten wenn am Ladegerät oder zu Testzwecken
 'If Ugemessen < 2200 Then
 ' Sensor_aus = 1
 'Else
 ' Sensor_aus = 0
 'End If
 Errorword.1 = 1
 While Ugemessen < 2200       'Unterspannung oder am Ladegerät
  Call Flashled(0 , 50)
 Wend
 If Ugemessen > 2650 Then       'Akku ist ganzgeladen
#endif

#if Systemvoltage = 36
 'Sensoren ausschalten wenn am Ladegerät oder zu Testzwecken
 'If Ugemessen < 3300 Then
 ' Sensor_aus = 1
 'Else
 ' Sensor_aus = 0
 'End If
 Errorword.2 = 1
 While Ugemessen < 3300       'Unterspannung oder am Ladegerät
  Call Flashled(0 , 50)
 Wend
 If Ugemessen > 4350 Then       'Akku ist ganzgeladen
#endif

#if Systemvoltage = 48
 'Sensoren ausschalten wenn am Ladegerät oder zu Testzwecken
 'If Ugemessen < 2400 Then
 ' Sensor_aus = 1
 'Else
 ' Sensor_aus = 0
 'End If
 Errorword.3 = 1
 While Ugemessen < 4300       'Unterspannung oder am Ladegerät
  Call Flashled(0 , 50)
 Wend
 If Ugemessen > 5280 Then       'Akku ist ganzgeladen
#endif

  'Interruts abschalten damit die folgenden Zeilen nicht gerade jetzt vom
  'Interrupt unterbrochen werden.
  Disable Interrupts
  Waitms 50
  Summe_as_entladen = 0
  Summe_mah_entladen = 0
  Summe_as_laden = 0
  Summe_mah_laden = 0
  Enable Interrupts
 End If

Led3 = 0
Led4 = 0
Led5 = 0
Led6 = 0
Led7 = 0

Goto Jp

'Summen_vorbelegen:
Do
 Errorword.4 = 1
' Acc_roll = Acc_roll * 100
' Acc_nick = Acc_nick * 100

 'Sum_gyro_roll = Gyro_roll * Gyro_anz
 'Sum_gyro_nick = Gyro_nick * Gyro_anz
 'Sum_gyro_gier = Gyro_gier * Gyro_anz

 Gyro_roll_alt = Gyro_roll
 Gyro_nick_alt = Gyro_nick
 Gyro_gier_alt = Gyro_gier

 Mit_gyro_roll = Gyro_roll
 Mit_gyro_nick = Gyro_nick
 Mit_gyro_gier = Gyro_gier

 Sum_acc_roll = Acc_roll * Acc_anz
 Sum_acc_nick = Acc_nick * Acc_anz

 Waitms 200
 Toggle Led5
 Toggle Led6
 Waitms 200
 Toggle Led5
 Toggle Led6
Loop Until Sig_gyro_nick >= -3 And Sig_gyro_nick <= 3 And Sig_gyro_roll >= -3 And Sig_gyro_roll <= 3 And Sig_gyro_gier >= -3 And Sig_gyro_gier <= 3
Errorword.4 = 0

Jp:

Call Resetdata       'and Enable Interrupt
Resetbtndone = 0

Do
 Errorword.5 = 1
 '<Sicherheitstest Sensoren>
 If Antriebaus = 1 Then
  If Intcount <> Ltest Then
   Ltest = Intcount
   Sensor_ok = 0
   If Sensor_ok_count > 0 Then Decr Sensor_ok_count
   '1. Es darf keine nennenswerte Drehbewegung erkannt werden!

   If Sig_gyro_nick < -3 Then Sensor_ok_count = 50
   If Sig_gyro_nick > 3 Then Sensor_ok_count = 50
   If Sig_gyro_roll < -3 Then Sensor_ok_count = 50
   If Sig_gyro_roll > 3 Then Sensor_ok_count = 50
   'Nach mehrfachen Drehungen könnte der Mittelwert etwas
   'verschoben sein. Darum hier 12
   If Sig_gyro_gier < -15 Then Sensor_ok_count = 50
   If Sig_gyro_gier > 15 Then Sensor_ok_count = 50

   '2. Die Werte dürfen nicht verrauscht sein
   Ldummy2 = Mit_acc_nick + 500
   If Acc_nick > Ldummy2 Then Sensor_ok_count = 50
   Ldummy2 = Mit_acc_nick - 500
   If Acc_nick < Ldummy2 Then Sensor_ok_count = 50
   Ldummy2 = Mit_acc_roll + 500
   If Acc_roll > Ldummy2 Then Sensor_ok_count = 50
   Ldummy2 = Mit_acc_roll - 500
   If Acc_roll < Ldummy2 Then Sensor_ok_count = 50

   '3. Die Werte müssen ungefähr in einer legalen Nullage sein.
   If Null_roll < O_roll_u Then Sensor_ok_count = 50
   If Null_roll > O_roll_o Then Sensor_ok_count = 50
   If Null_nick < O_nick_u Then Sensor_ok_count = 50
   If Null_nick > O_nick_o Then Sensor_ok_count = 50

   'Bedingungen während 50 Messungen (0.5s) eingehalten
   If Sensor_ok_count = 0 Then Sensor_ok = 1
  End If
 Else
  Sensor_ok = 0
 End If
 '</Sicherheitstest Sensoren>

 '<Batterieanzeige>
 'alles unter 36V Leerlaufspannung ist LEER
 If Ugemessen > Akkufastleer Then
  If Verbraucht > Akkukap Then
   'Einfach mal annehmen daß noch mehr als angezeigt im Akku ist
   Akkukap = Verbraucht + 1
   Call Akkugrenzen
  End If
 End If

 If Antriebaus = 1 Then
  Errorword.6 = 1
 Else
  Errorword.6 = 0
 End If

 If Notaus = 1 Then
  Errorword.7 = 1
 Else
  Errorword.7 = 0
 End If

 'Blinken wenn Gyros nicht bereit
 If Antriebaus = 1 Then
  If Ugemessen > Akkuleer Then
   If Verbraucht > Akkukap Then
    'annehmen dass noch mehr als angezeigt im Akku ist
    Akkukap = Verbraucht + 20
    Call Akkugrenzen
   End If
  End If
  If Sensor_ok = 1 Then
   If Verbraucht < Akku1 Then Led7 = 0 Else Led7 = 1
   If Verbraucht < Akku2 Then Led6 = 0 Else Led6 = 1
   If Verbraucht < Akku3 Then Led5 = 0 Else Led5 = 1
   If Verbraucht < Akku4 Then Led4 = 0 Else Led4 = 1
   If Verbraucht < Akku5 Then Led3 = 0 Else Led3 = 1
  Else
   If Frei = 1 Then       'ca. 2 Hz
    Blinkcount = Intcount
    If Blinkcount > 50 Then Blinkcount = Blinkcount - 50
   Else
    Blinkcount = Intcount / 2
   End If
   If Blinkcount > 40 Then       'ca. 1 Hz
    If Verbraucht < Akku1 Then Led7 = 0 Else Led7 = 1
    If Verbraucht < Akku2 Then Led6 = 0 Else Led6 = 1
    If Verbraucht < Akku3 Then Led5 = 0 Else Led5 = 1
    If Verbraucht < Akku4 Then Led4 = 0 Else Led4 = 1
    If Verbraucht < Akku5 Then Led3 = 0 Else Led3 = 1
   Else
    Led7 = 1
    Led6 = 1
    Led5 = 1
    Led4 = 1
    Led3 = 1
   End If
  End If
 Else
  Errorword.6 = 0
  If Verbraucht < Akku1 Then Led7 = 0 Else Led7 = 1
  If Verbraucht < Akku2 Then Led6 = 0 Else Led6 = 1
  If Verbraucht < Akku3 Then Led5 = 0 Else Led5 = 1
  If Verbraucht < Akku4 Then Led4 = 0 Else Led4 = 1
  If Verbraucht < Akku5 Then Led3 = 0 Else Led3 = 1
 End If
 '</Batterieanzeige>

 '< Maximalwerte Für Betriebsdatenschreiber>
 If Igemessen < Imin Then Imin = Igemessen       'Nur zur Information
 If Igemessen > Imax Then Imax = Igemessen       'Nur zur Information
 If Regelzeit > Regelzeitmax Then Regelzeitmax = Regelzeit
 If Mit_acc_nick > Mit_acc_nick_max Then Mit_acc_nick_max = Mit_acc_nick
 If Mit_acc_roll > Mit_acc_roll_max Then Mit_acc_roll_max = Mit_acc_roll
 If Mit_acc_nick < Mit_acc_nick_min Then Mit_acc_nick_min = Mit_acc_nick
 If Mit_acc_roll < Mit_acc_roll_min Then Mit_acc_roll_min = Mit_acc_roll
 If Pwm_links > Pwm_links_max Then Pwm_links_max = Pwm_links
 If Pwm_rechts > Pwm_rechts_max Then Pwm_rechts_max = Pwm_rechts
 If K_diff > K_diff_max Then K_diff_max = K_diff
 If K_diff < K_diff_min Then K_diff_min = K_diff
 If Ta > Tamax Then Tamax = Ta
 '</Maximalwerte für Betriebsdatenschreiber>

 'Debounce macht dasselbe, allerdings stockt der Programmfluss
 Debounce Resetbtn , 0 , Onresetbtn , Sub

Goto Skipit

 If Resetbtndone = 0 Then
  If Resetbtn = 0 Then
   Resetbtndone = 1
   Call Onresetbtn
   Resetbtndone = 2
  Else
  End If
 Else
  If Resetbtndone = 2 Then Resetbtndone = 0
 End If
Skipit:

 If Intcount > 80 Then Led1 = 1 Else Led1 = 0       'Sekundenblinken
 'Daran lässt sich leicht erkennen ob noch alles normal läuft.
Loop
End

'**********************************************************************
'Ab hier Subroutinen und  Interrupts
'**********************************************************************
Sub Akkugrenzen
 'Grenzwerte für die Akku-LEDs berechnen
 Ldummy1 = Akkukap / 5
 Akku1 = Ldummy1
 Akku2 = 2 * Ldummy1
 Akku3 = 3 * Ldummy1
 Akku4 = 4 * Ldummy1
 Akku5 = 5 * Ldummy1
End Sub

Sub Unfalldatenschreiber
 Waitms 10
 Utext = Str(regelzeit)
 Utext = "Rz=" + Utext
 Logtext 0 , Utext
 Utext = Str(mit_acc_nick_max)
 Utext = "Nick max=" + Utext
 Logtext $10 , Utext
 Utext = Str(mit_acc_nick)
 Utext = "Nick=" + Utext
 Logtext $20 , Utext
 Utext = Str(mit_acc_nick_min)
 Utext = "Nick min=" + Utext
 Logtext $30 , Utext
 Utext = Str(mit_acc_roll_max)
 Utext = "Roll max=" + Utext
 Logtext $40 , Utext
 Utext = Str(mit_acc_roll)
 Utext = "Roll=" + Utext
 Logtext $50 , Utext
 Utext = Str(mit_acc_roll_min)
 Utext = "Roll min=" + Utext
 Logtext $60 , Utext
 Utext = Str(pwm_rechts)
 Utext = "PWM re=" + Utext
 Logtext $70 , Utext
 Utext = Str(pwm_links)
 Utext = "PWM li=" + Utext
 Logtext $80 , Utext
 Utext = Str(u_rechts)
 Utext = "U re=" + Utext
 Logtext $90 , Utext
 Utext = Str(u_links)
 Utext = "U li=" + Utext
 Logtext $a0 , Utext
 Utext = Str(ta)
 Utext = "Ta=" + Utext
 Logtext $b0 , Utext
 Utext = Str(seriell_err_links)
 Utext = Utext + "|"
 Utext = Utext + Str(seriell_err_rechts)
 Utext = "S =" + Utext
 Logtext $c0 , Utext
 Utext = Ugrund
 Logtext $d0 , Utext
End Sub

Sub Betriebsdatenschreiber
 'Ob die Wartezeiten beim EEprom schreiben nötig sind ist nicht sicher.
 'Mit den Wartezeiten treten aber keine Fehler bei den Werten mehr auf.
 'Möglich, daß dies auch durch eine andere Änderung bewirkt wurde.
 Writeeeprom Max_v , $2f0
 Writeeeprom Min_v , $2f2
 Zentimeter = Int(zentimeter)       'Keine Millimeter schreiben
 Writeeeprom Summe_as_entladen , $300
 Writeeeprom Summe_mah_entladen , $310
 Writeeeprom Summe_as_laden , $320
 Writeeeprom Summe_mah_laden , $330
 Writeeeprom Akkukap , $340
 Writeeeprom Kilometer , $350
 Writeeeprom Meter , $360
 Writeeeprom Zentimeter , $370
 If Werteneu = 1 Then       'Nur wenn das Einstellklavier gesteckt ist.
  Writeeeprom P_faktor , $380
  Writeeeprom I_faktor , $384
  Writeeeprom D_faktor , $388
  Writeeeprom Mp_faktor , $38c
  Writeeeprom Mv_faktor , $390
  Writeeeprom Mpl_faktor , $394
  Writeeeprom Mvl_faktor , $398
  Werteneu = 0
 End If

 Utext = Str(pwm_rechts_max)
 Utext = "PWM r max=" + Utext
 Logtext $e0 , Utext
 Utext = Str(pwm_links_max)
 Utext = "PWM l max=" + Utext
 Logtext $f0 , Utext
 Utext = Str(ugemessen)
 Utext = "U=" + Utext
 Logtext $100 , Utext
 Utext = Str(imin)
 Utext = "Imin=" + Utext
 Logtext $110 , Utext
 Utext = Str(imax)
 Utext = "Imax=" + Utext
 Logtext $120 , Utext
 Utext = Str(summe_mah_entladen)
 Utext = "-mAh" + Utext
 Logtext $130 , Utext
 Utext = Str(summe_mah_laden)
 Utext = "+mAh=" + Utext
 Logtext $140 , Utext
 Utext = Str(verbraucht)
 Utext = "    =" + Utext
 Logtext $150 , Utext
 Utext = Str(akkukap)
 Utext = "Kap=" + Utext
 Logtext $160 , Utext
 Utext = Str(kilometer)
 Utext = "Km=" + Utext
 Logtext $170 , Utext
 Utext = Str(meter)
 Utext = "M=" + Utext
 Logtext $180 , Utext
 Utext = Str(zentimeter)
 Utext = "cm=" + Utext
 Logtext $190 , Utext
 Utext = Str(regelzeitmax)
 Utext = "Rz Max=" + Utext
 Logtext $1a0 , Utext
 Utext = Str(null_roll)
 Utext = "Null_R=" + Utext
 Logtext $1b0 , Utext
 Utext = Str(null_nick)
 Utext = "null_N=" + Utext
 Logtext $1c0 , Utext
 Utext = Str(p_faktor)
 Utext = "P=" + Utext
 Logtext $1d0 , Utext
 Utext = Str(i_faktor)
 Utext = "I=" + Utext
 Logtext $1e0 , Utext
 Utext = Str(d_faktor)
 Utext = "D=" + Utext
 Logtext $1f0 , Utext
 Utext = Str(mp_faktor)
 Utext = "M=" + Utext
 Logtext $200 , Utext
 Utext = Str(k_diff_min)
 Utext = "K min=" + Utext
 Logtext $210 , Utext
 Utext = Str(k_diff_max)
 Utext = "K max=" + Utext
 Logtext $220 , Utext
 Utext = Str(seriell_err_links)
 Utext = "S l=" + Utext
 Logtext $230 , Utext
 Utext = Str(seriell_err_rechts)
 Utext = "S r=" + Utext
 Logtext $240 , Utext
 Utext = Str(tamax)
 Utext = "Ta_Max=" + Utext
 Logtext $250 , Utext
 Utext = Str(max_v)
 Utext = "V_Max=" + Utext
 Logtext $260 , Utext
 Utext = Str(min_v)
 Utext = "V_Min=" + Utext
 Logtext $270 , Utext
End Sub

Sub Onresetbtn
 If Antriebaus = 1 Then
  'alles unter 36V Leerlaufspannung ist LEER
  If Ugemessen > Akkuleer Then
   If Sensor_ok = 1 Then       'Prüfen ob Gyros noch driften
    'Interupts abschalten damit Fusscount nicht gerade jetzt
    'decrementiert wird
    Call Resetdata
   Else
    If Null_nick > O_nick_o And Null_nick < O_nick_oo Then
     If Bzeit = 500 Then
      Bstate = 1
      Bzeit = 0
     Else
      If Bstate = 1 Then
       If Bzeit < 50 Then
        Disable Interrupts
        Utext = "                "
        Logtext $00 , Utext
        Logtext $10 , Utext
        Logtext $20 , Utext
        Logtext $30 , Utext
        Logtext $40 , Utext
        Logtext $50 , Utext
        Logtext $60 , Utext
        Logtext $70 , Utext
        Logtext $80 , Utext
        Logtext $90 , Utext
        Logtext $a0 , Utext
        Logtext $b0 , Utext
        Logtext $c0 , Utext
        Logtext $d0 , Utext
        Logtext $e0 , Utext
        Logtext $f0 , Utext
        Logtext $100 , Utext
        Logtext $110 , Utext
        Logtext $120 , Utext
        Logtext $130 , Utext
        Logtext $140 , Utext
        Logtext $150 , Utext
        Logtext $160 , Utext
        Logtext $170 , Utext
        Logtext $180 , Utext
        Logtext $190 , Utext
        Logtext $1a0 , Utext
        Logtext $1b0 , Utext
        Logtext $1c0 , Utext
        Logtext $1d0 , Utext
        Logtext $1e0 , Utext
        Logtext $1f0 , Utext
        Logtext $200 , Utext
        Logtext $210 , Utext
        Logtext $220 , Utext
        Logtext $230 , Utext
        Logtext $240 , Utext
        Logtext $250 , Utext
        Logtext $260 , Utext
        Summe_as_entladen = 0
        Summe_mah_entladen = 0
        Summe_as_laden = 0
        Summe_mah_laden = 0
        Writeeeprom Summe_as_entladen , $300
        Writeeeprom Summe_mah_entladen , $310
        Writeeeprom Summe_as_laden , $320
        Writeeeprom Summe_mah_laden , $330
        Frei = 0
        For Bzeit = 1 To 10
         Led3 = 1
         Led4 = 1
         Led5 = 1
         Led6 = 1
         Led7 = 1
         Waitms 100
         Led3 = 0
         Led4 = 0
         Led5 = 0
         Led6 = 0
         Led7 = 0
         Waitms 100
        Next
        Bzeit = 500
        Enable Interrupts
        Waitms 100
       End If
       If Bzeit > 250 And Bzeit < 350 Then
        Frei = 1
        Bzeit = 500
       Else
        Frei = 0
        Bzeit = 500
       End If
      End If
     End If
    Else
     Frei = 0
     Bzeit = 500
    End If
   End If
  Else
   If Verbraucht < Akkukap Then       'Akku ist laut Spannung leer!
    'Einfach mal annehmen daß zuviel entnommen wurde
    Akkukap = Verbraucht * 0.95
    Akkugrenzen
   End If
   'Disable Interrupts
   'Betriebsdatenschreiber
   'Enable Interrupts
  End If
 Else
  Antriebaus = 1
  Waitms 100
  Frei = 0

  'Disable Interrupts
  'Betriebsdatenschreiber
  'Enable Interrupts
 End If
End Sub

Sub Logtext(adresse , Text)
 If Len(text) > 16 Then Text = Mid(text , 1 , 16)
 While Len(text) < 16
  Text = Text + " "
 Wend
 Writeeeprom Text , Adresse
End Sub

Regel_interrupt:
'*****************************************************************************
'Eigentlich ist das kein typischer Interrupt. Der Interrupt wird hier nur
'verwendet um der Regelung einen 100Hz Takt zu geben.
'Andere Timer Interrupts werden für diese Zeit blockiert
'*****************************************************************************
'Timer zählt ab diesem Wert aufwärts bis 256 und macht dann einen Interrupt
'==> je höher die Startzahl, umso kürzer das Intervall
' 20 MHz / 1024 (Prescaler) = 15,625 KHz / 256 = 61Hz
' (61/100) * 256 = 156
' 256 - 156 = 100 Der Timer selbst braucht auch etwas Zeit, d.h. 100 ist tatsächlich 61
Timer0 = 61

Bdummy1 = Udr

Gosub Readfussschalter
Gosub Read_gyro
Gosub Read_acc

'Acc_roll = Acc_roll       '*100, war früher -512...+512 jetzt -30000...+30000
'Acc_nick = Acc_nick       '*100, war früher -512...+512 jetzt -30000...+30000

'<Mittellage ausgleichen>
'Die Offsetwerte werden langsam an eine früher ermittelte Waagerechte
'Position angeglichen.
Null_roll = Null_roll * 99
Null_nick = Null_nick * 99
Null_roll = Null_roll + Offset_roll       'Mittellage
Null_nick = Null_nick + Offset_nick       'Mittellage
Null_roll = Null_roll / 100
Null_nick = Null_nick / 100
'</Mittellage ausgleichen>

'Offsetwerte der Beschleunigungssensoren sofort abziehen
'Diese werden nicht laufend ermittelt
'Acc_roll = Acc_roll - Null_roll
'Acc_nick = Acc_nick - Null_nick
'<Geschwindikgeitsbegrenzung>
'Bei Der Geschwindigkeitsbegrenzung Wird die Nullage Der Plattform Verändert.
'Beim Vorwärtsbremsen wird nach hinten geneigt, beim Rückwärtsbremsen nach vorne.

Vbrems = 0
Lbrems = 0
If Frei = 0 Then
 If Kmh > Vmax Then Vbrems = Vmax - Kmh
 If Kmh < Vmaxr Then Vbrems = Vmaxr - Kmh
End If

'Lastabhängige Bremse
'Es muss immer genug Reserve sein um noch zu Bremsen oder zu Balancieren
Ldummyi = Int(pwm_gesamt)
If Ldummyi > Spwm Then Lbrems = Spwm - Ldummyi
If Ldummyi < Spwmr Then Lbrems = Spwmr - Ldummyi
Brems = Lbrems + Vbrems
Brems = Brems * 50
'Nicht zu heftig Bremsen
If Brems < -2000 Then Brems = -2000
If Brems > 2000 Then Brems = 2000
Acc_nick = Acc_nick + Brems
'</Geschwindikgeitsbegrenzung>

'<Signalaufbereitung der Sensorsignale>
K_diff = K_rechts - K_links
K_sum = K_sum * 0.84
K_sum = K_sum + K_diff
'Gleitender Mittelwert der Gyros berechnen um damit den Offset bei
'Stillstand zu haben. Dieser ist leider Temperaturabhängig.
'Sum_gyro_gier = Sum_gyro_gier - Mit_gyro_gier
'Sum_gyro_gier = Sum_gyro_gier + Gyro_gier
'Mit_gyro_gier = Sum_gyro_gier / Gyro_anz
Sig_gyro_gier = Gyro_gier       '- Mit_gyro_gier       'Polarität

'Sum_gyro_roll = Sum_gyro_roll - Mit_gyro_roll
'Sum_gyro_roll = Sum_gyro_roll + Gyro_roll
'Mit_gyro_roll = Sum_gyro_roll / Gyro_anz
Sig_gyro_roll = Gyro_roll       '- Mit_gyro_roll       'Polarität

'Korr_acc_roll = Sig_gyro_roll * Gyro_faktor
Sum_acc_roll = Sum_acc_roll - Mit_acc_roll
Sum_acc_roll = Sum_acc_roll + Acc_roll
'Sum_acc_roll = Sum_acc_roll + Korr_acc_roll       'Gyrowert hinzu
'Mittelwert bilden
Mit_acc_roll = Sum_acc_roll / Acc_anz
Mit_acc_roll = Sum_acc_roll

'*************************************************************************
'Das soll ausgleichen, daß die Gyros bei gekippten Sensoren eine Drehung
'um die falsche Achse registrieren.
'Sdummy1 = Mit_acc_roll * Sig_gyro_gier
'Die Sensorplatine muss dazu absolut Waagerecht montiert sein.
'Sdummy1 = Sdummy1 / 20000
'Lagefehler = Int(sdummy1)
'Das soll den Fehler ausgleichen, der durch die geänderte Lage des
'Gyros auftritt.
'Gyro_nick = Gyro_nick + Lagefehler
'************************************************************************
Sum_gyro_nick = Sum_gyro_nick - Mit_gyro_nick
Sum_gyro_nick = Sum_gyro_nick + Gyro_nick
Mit_gyro_nick = Sum_gyro_nick / Gyro_anz
Sig_gyro_nick = Gyro_nick - Mit_gyro_nick

'Gyro = Beschleunigung
'Accelerometer = Neigung

'Korr_acc_nick = Sig_gyro_nick * Gyro_faktor
Sum_acc_nick = Sum_acc_nick - Mit_acc_nick
Sum_acc_nick = Sum_acc_nick + Acc_nick
'Sum_acc_nick = Sum_acc_nick - Korr_acc_nick       'Gyrowert hinzu
Mit_acc_nick = Sum_acc_nick / Acc_anz
'</Signalaufbereitung der Sensorsignale>

'<PID-Regler>
Stellwert = 0
'< P-Anteil>
Sdummy1 = Mit_acc_nick
Sdummy1 = Sdummy1 * P_faktor
Stellwert = Stellwert - Sdummy1

'<I-Anteil>
 Mit_acc_nick_s = Mit_acc_nick_s + Mit_acc_nick
 'Überlauf/unsinnige Werte verhindern
 If Mit_acc_nick_s > Max_mit_acc_nick_s Then
  Mit_acc_nick_s = Max_mit_acc_nick_s
 End If
 If Mit_acc_nick_s < Min_mit_acc_nick_s Then
  Mit_acc_nick_s = Min_mit_acc_nick_s
 End If
 Sdummy1 = Mit_acc_nick_s
 Sdummy1 = Sdummy1 * I_faktor
 Stellwert = Stellwert - Sdummy1

'<D-Anteil>
 Sdummy1 = Sig_gyro_nick
 Sdummy1 = Sdummy1 * D_faktor
 'Stellwert ==> Stromsollwert
 Stellwert = Stellwert + Sdummy1

'<Anpassungsfaktoren berechnen>
K_sum_anpassung = K_rechts + K_links
K_sum_anpassung = Abs(k_sum_anpassung)

Mp_anpassung = Pwm_gesamt * Mp_faktor
Mp_anpassung = Abs(mp_anpassung)
Mp_anpassung = Mp_anpassung + 0.95

Mv_anpassung = K_sum_anpassung * Mv_faktor
Mv_anpassung = Abs(mv_anpassung)
Mv_anpassung = Mv_anpassung + 1

Mpl_anpassung = Pwm_gesamt * Mpl_faktor
Mpl_anpassung = Abs(mpl_anpassung)
Mpl_anpassung = Mpl_anpassung + 1

Mvl_anpassung = K_sum_anpassung * Mvl_faktor
Mvl_anpassung = Abs(mvl_anpassung)
Mvl_anpassung = Mvl_anpassung + 1
'</Anpassungsfaktoren berechnen>

'<Verstärkte Regelung bei hoher Leistung>
Stellwert = Stellwert * Mp_anpassung
Stellwert = Stellwert * Mv_anpassung
'<Verstärkte Regelung bei hoher Leistung>

'<Abgeschwächte Regelung im Stand>
If K_sum_anpassung < 8 Then Stellwert = Stellwert * 0.8
If K_sum_anpassung < 6 Then Stellwert = Stellwert * 0.8
If K_sum_anpassung < 4 Then Stellwert = Stellwert * 0.8

'</Abgeschwächte Regelung in Stand>
Pwm_gesamt = Pwm_gesamt + Stellwert

'</PID-Regler>

'<Bremsvibrator einmischen>
'Der Bremsvibrator signalisiert dem Fahrer, daß wegen irgend einem
'Grenzwert gebremst wird. Das kann V-Max oder P-Max sein.
If Brems <> 0 Then
 Bdummy1 = Intcount / 2
 Bdummy1 = Bdummy1 Mod 2
 If Bdummy1 = 0 Then
  Pwm_gesamt = Pwm_gesamt + 5
 Else
  Pwm_gesamt = Pwm_gesamt - 5
 End If
End If
'<Bremsvibrator einmischen>

'<Lenkung>
Lenkvorgabe = Mit_acc_roll / 80

Sdummy1 = Abs(lenkvorgabe)
If Sdummy1 <= 6 Then
 Sdummy1 = Sdummy1 / 12
 Lenkerstellung = Lenkvorgabe * Sdummy1
Else
 If Lenkvorgabe < 0 Then
  Lenkerstellung = Lenkvorgabe + 3
 Else
  Lenkerstellung = Lenkvorgabe - 3
 End If
End If

'Lenkfehler berechnen und zum Stellwert hinzu.
Sdummy1 = Sig_gyro_gier
Sdummy1 = Sdummy1 / 5
Lenkfehler = Lenkerstellung - Sdummy1
Lenkfehler = Lenkfehler * Mpl_anpassung
Lenkfehler = Lenkfehler * Mvl_anpassung

Lenkstellwert = Lenkerstellung + Lenkfehler

If Lenkstellwert > Max_lenk Then Lenkstellwert = Max_lenk
If Lenkstellwert < Min_lenk Then Lenkstellwert = Min_lenk

Pwm_lenk_links = Pwm_gesamt + Lenkstellwert
Pwm_lenk_rechts = Pwm_gesamt - Lenkstellwert

If Pwm_lenk_links > 0 Then Richtung_links = 1 Else Richtung_links = 0
If Pwm_lenk_rechts > 0 Then Richtung_rechts = 1 Else Richtung_rechts = 0

Pwm_lenk_links = Abs(pwm_lenk_links)
Pwm_lenk_rechts = Abs(pwm_lenk_rechts)

If Pwm_lenk_links > Maxpwm Then Pwm_lenk_links = Maxpwm
If Pwm_lenk_rechts > Maxpwm Then Pwm_lenk_rechts = Maxpwm

If Pwm_lenk_links < Minpwm Then Pwm_lenk_links = Minpwm
If Pwm_lenk_rechts < Minpwm Then Pwm_lenk_rechts = Minpwm

Pwm_links = Pwm_lenk_links
Pwm_rechts = Pwm_lenk_rechts
'</Lenkung>

If Antriebaus = 1 Then
 If Notaus = 1 Then
  'Unfalldatenschreiber: *************************************************
  'Dauert zwar Lange, macht aber nix. Bei Notaus muss nix geregelt werden.
  ' call Unfalldatenschreiber
  ' call Betriebsdatenschreiber
  Notaus = 0
  '***********************************************************************
 End If
 'Auf 0 setzen was sonst geregelt wird
 Richtung_links = 2       'Leerlauf
 Richtung_rechts = 2       'Leerlauf
 Pwm_links = 0
 Pwm_rechts = 0
 Pwm_gesamt = 0
 Mit_acc_nick_s = 0
 Lenkerstellung = 0
 K_rechts = 0
 K_links = 0
 'Waagerechte Positoion setzen wenn Zustand Antriebaus:
 Wdummy1 = Acc_roll
 Null_roll = Wdummy1 * 100
 Wdummy1 = Acc_nick
 Null_nick = Wdummy1 * 100
 'Mittelwert der Beschleunigungssensoren auf aktuellen Wert stellen
 'Der Mittelwert braucht sonst zu lange bis er sich eingeregelt hat
 Sum_acc_roll = Acc_roll * Acc_anz
 Sum_acc_nick = Acc_nick * Acc_anz
 If Intcount = 0 Then T30 = 1       'Regelparameter vom Tacho anfordern
 If Intcount = 50 Then T30 = 1       'Regelparameter vom Tacho anfordern
End If

'******************************************************************************
If T30 = 1 Then       'Regelparameter vom Tacho anfordern
 K_links = 0
 K_rechts = 0
 Zeiger_meldung = 1
 Bdummy1 = Udr
 T30 = 0       'Damit keine alte Rückmeldung ausgewertet wird
 Timeout = Timer0
 Timeout = Timeout + 100
 'Anforderung an Display senden
 Print Chr(255) ; Chr(0) ; Chr(0) ; Chr(255) ; Chr(13)
 While Zeiger_meldung < 31
  If Ucsr0a.rxc0 = 1 Then       'Warten bis ein Byte eingetroffen ist
   Tachomeldung(zeiger_meldung) = Udr
   Incr Zeiger_meldung
   Timeout = Timer0
   Timeout = Timeout + 20
  End If
  If Timer0 > Timeout Then
   T30 = 0
   Exit While       'Timeout
  End If
 Wend
 If T30 = 13 Then       'Jetzt BCC berechnen
  !lds R24,{t1}
  !LDs R25,{t2}
  !add R24,R25
  !LDs R25,{t3}
  !add R24,R25
  !LDs R25,{t4}
  !add R24,R25
  !LDs R25,{t5}
  !add R24,R25
  !LDs R25,{t6}
  !add R24,R25
  !LDs R25,{t7}
  !add R24,R25
  !LDs R25,{t8}
  !add R24,R25
  !LDs R25,{t9}
  !add R24,R25
  !LDs R25,{t10}
  !add R24,R25
  !LDs R25,{t11}
  !add R24,R25
  !LDs R25,{t12}
  !add R24,R25
  !LDs R25,{t13}
  !add R24,R25
  !LDs R25,{t14}
  !add R24,R25
  !LDs R25,{t15}
  !add R24,R25
  !LDs R25,{t16}
  !add R24,R25
  !LDs R25,{t17}
  !add R24,R25
  !LDs R25,{t18}
  !add R24,R25
  !LDs R25,{t19}
  !add R24,R25
  !LDs R25,{t20}
  !add R24,R25
  !LDs R25,{t21}
  !add R24,R25
  !LDs R25,{t22}
  !add R24,R25
  !LDs R25,{t23}
  !add R24,R25
  !LDs R25,{t24}
  !add R24,R25
  !LDs R25,{t25}
  !add R24,R25
  !LDs R25,{t26}
  !add R24,R25
  !LDs R25,{t27}
  !add R24,R25
  !LDs R25,{t28}
  !add R24,R25
  !STS {Bdummy1},R24

  If Bdummy1 = T29 Then
   P_faktor = P_tacho
   I_faktor = I_tacho
   D_faktor = D_tacho
   Mp_faktor = Mp_tacho
   Mv_faktor = Mv_tacho
   Mpl_faktor = Mpl_tacho
   Mvl_faktor = Mvl_tacho
   Werteneu = 1
  End If
 End If
Else
 'Errechnete Geschwindigkeiten an Motoren Senden:
 'Rechter Motor:
 If Nummotors = 2 Then
  Checksum_rechts = 82
  Checksum_rechts = Checksum_rechts + Richtung_rechts
  Checksum_rechts = Checksum_rechts + Pwm_rechts
  'Geschwindigkeit an Motorregler senden
  Bdummy1 = Udr       'Puffer leeren
  'Print Chr(82); Chr(richtung_rechts); Chr(pwm_rechts); Chr(checksum_rechts); Chr(13);
  'In Assembler geht das so:
   !Ldi R24 , 82
   !STS     udr,R24
  Warten1:
   !LDS     R24,UCSR0A
   !BST     R24,5
   !Brtc warten1
   !LDS     R24,{Richtung_rechts}
   !STS     udr,R24
  Warten2:
   !LDS     R24,UCSR0A
   !BST     R24,5
   !Brtc warten2
   !lds     R24,{Pwm_rechts}  'PWM Wert
   !STS     udr,R24
  Warten3:
   !LDS     R24,UCSR0A
   !BST     R24,5
   !Brtc warten3
   !LDS     R24,{Checksum_rechts}
   !STS     udr,R24
  Warten4:
   !LDS     R24,UCSR0A
   !BST     R24,5
   !Brtc warten4
   !LDI     R24,13
   !STS     udr,R24
  Warten5:
   !LDS     R24,UCSR0A
   !BST     R24,5
   !Brtc warten5

  Motormeldung(8) = 0       'Damit keine alte Rückmeldung ausgewertet wird

  'Vom Motorcontroller lesen
  Timeout = Timer0
  Zeiger_meldung = 1
  '80 Ticks * 51,2uS pro Tick = 4mS Wartezeit auf Antwort
  'COM benötigt ca. 1,5mS für 9 Byte
  Errorword.8 = 0
  Timeout = Timeout + 80
  While Zeiger_meldung < 9
   If Ucsr0a.rxc0 = 1 Then       'Warten bis ein Byte eingetroffen ist
    Motormeldung(zeiger_meldung) = Udr
    Incr Zeiger_meldung
   End If
   If Timer0 > Timeout Then
    Ende = 0
    Errorword.8 = 1
    Exit While       'Timeout
   End If
  Wend

  !lds R24,{M1}
  !LDs R25,{M2}
  !ADD R24,R25
  !LDs R25,{M3}
  !ADD R24,R25
  !LDs R25,{M4}
  !ADD R24,R25
  !LDs R25,{M5}
  !ADD R24,R25
  !LDs R25,{M6}
  !ADD R24,R25
  !STS {Bdummy1},R24

  If Bdummy1 <> Checksumme Then Ende = 0
  If Ende = 13 Then
   U_rechts = U
   I_rechts = I
   K_rechts = K
  Else
   Incr Seriell_err_rechts
   S_err = 1
  End If
 End If

 'Linker Motor:
 Checksum_links = 76       'Adresse
 Checksum_links = Checksum_links + Richtung_links
 Checksum_links = Checksum_links + Pwm_links
 Zeiger_meldung = 1
 Bdummy1 = Udr       'Puffer leeren
 'Print Chr(76); Chr(richtung_links); Chr(pwm_links); Chr(checksum_links); Chr(13);
 '>>>>>>>>>>>> Message an Motorcontroller senden <<<<<<<<<<<<<<
 'In Assembler geht das so:
   !Ldi     R24 , 76       'Adresse
   !STS     udr,R24
  Warten6:
   !LDS     R24,UCSR0A
   !BST     R24,5
   !Brtc warten6
   !LDS     R24,{Richtung_links}
   !STS     udr,R24
  Warten7:
   !LDS     R24,UCSR0A
   !BST     R24,5
   !Brtc warten7
   !lds     R24,{Pwm_links}
   !STS     udr,R24
  Warten8:
   !LDS     R24,UCSR0A
   !BST     R24,5
   !Brtc warten8
   !LDS     R24,{Checksum_links}
   !STS     udr,R24
  Warten9:
   !LDS     R24,UCSR0A
   !BST     R24,5
   !Brtc warten9
   !LDI     R24,13
   !STS     udr,R24
  Warten10:
   !LDS     R24,UCSR0A
   !BST     R24,5
   !Brtc warten10
 Motormeldung(8) = 0       'Damit keine alte Rückmeldung ausgewertet wird
 Timeout = Timer0
 Timeout = Timeout + 80       'Wartezeit auf Antwort
 '>>>>>>>>>>>> Message vom Motorcontroller empfangen <<<<<<<<<<<<<<
 While Zeiger_meldung < 9
  If Ucsr0a.rxc0 = 1 Then       'Warten bis ein Byte eingetroffen ist
   Motormeldung(zeiger_meldung) = Udr
   Incr Zeiger_meldung
  End If
  If Timer0 > Timeout Then
   Ende = 0
   Errorword.8 = 1
   Exit While       'Timeout
  End If
 Wend

 !lds R24,{M1}
 !LDs R25,{M2}
 !ADD R24,R25
 !LDs R25,{M3}
 !ADD R24,R25
 !LDs R25,{M4}
 !ADD R24,R25
 !LDs R25,{M5}
 !ADD R24,R25
 !LDs R25,{M6}
 !ADD R24,R25
 !STS {Bdummy1},R24

 If Bdummy1 <> Checksumme Then Ende = 0
  If Ende = 13 Then
   U_links = U
   I_links = I
   K_links = K
  Else
   Incr Seriell_err_links
   S_err = 1
  End If
  If S_err = 1 Then       'Sicherheitshalber, damit der Lenkausgleich nicht überreagiert.
   K_links = 0
   K_rechts = 0
   S_err = 0
  End If
 End If

 If Antriebaus = 0 Then
  '<Not-Aus Kriterien ermitteln>
  If Mit_acc_nick > -300 Then If Mit_acc_nick < 300 Then Regelzeit = 0
  Incr Regelzeit
  '</Not-Aus Kriterien ermitteln>

  '<Not-Aus Kriterien abprüfen>
  If Regelzeit > 250 Then       'Wenn nach 2,5s waagerechte Lage nicht erreicht
   Notaus = 1
   Antriebaus = 1
   Ugrund = Str(regelzeit)
   Ugrund = "*Rz " + Ugrund
  End If
  Ldummyi = 4000 + Brems
  If Mit_acc_nick > Ldummyi Then       'zu weit nach hinten gekippt
   Notaus = 1
   Antriebaus = 1
   Ugrund = Str(mit_acc_nick)
   Ugrund = "*Nick " + Ugrund
  End If
  Ldummyi = -4000 + Brems
  If Mit_acc_nick < Ldummyi Then       'zu weit nach vorn gekippt
   Notaus = 1
   Antriebaus = 1
   Ugrund = Str(mit_acc_nick)
   Ugrund = "*Nick " + Ugrund
  End If
  If Mit_acc_roll > 5000 Then       'Lenker zu weit nach rechts
   Notaus = 1
   Antriebaus = 1
   Ugrund = Str(mit_acc_roll)
   Ugrund = "*Roll " + Ugrund
  End If
  If Mit_acc_roll < -5000 Then       'Lenker zu weit nach links
   Notaus = 1
   Antriebaus = 1
   Ugrund = Str(mit_acc_roll)
   Ugrund = "*Roll " + Ugrund
  End If
  If K_diff > 10 Then       'Plattform rotiert zu schnell
   Notaus = 1
   Antriebaus = 1
   Ugrund = Str(k_diff)
   Ugrund = "*Gier " + Ugrund
  End If
  If K_diff < -10 Then       'Plattform rotiert zu schnell
   Notaus = 1
   Antriebaus = 1
   Ugrund = Str(k_diff)
   Ugrund = "*Gier " + Ugrund
  End If
  '</Not-Aus Kriterien abprüfen>
 Else
  If Fusscount > 0 Then Antriebaus = 0
 End If

'<Akkuspannung berechnen>
 If Nummotors = 2 Then
  Ugemessen = U_links + U_rechts
  Shift Ugemessen , Right , 1 , Signed       'Div by 2 weil 2 Motoren
 Else
  Ugemessen = U_links
 End If
 '</Akkuspannung berechnen>

 '<Gesamtstrom berechnen>
 If Nummotors = 2 Then
  Igemessen = I_links + I_rechts
 Else
  Igemessen = I_links
 End If
 '</Gesamtstrom berechnen>

 '<verbrauchte Amperestunden berechnen>
 If Igemessen > 0 Then
  Summe_as_entladen = Summe_as_entladen + Igemessen       'Verbrauchte
 End If
 If Igemessen < 0 Then
  Summe_as_laden = Summe_as_laden - Igemessen       'Rückgespeiste
 End If
 'mAh aus den Amperestunden rausholen um Überläufe zu verhindern
 'Die 360000 setzen sich zusammen aus:
    'Igemessen in mA -> Faktor 1
    '100 Messungen pro Sekunde -> mal 100
    '1 mAh = 60 Sekunden * 60 mAsekunden -> 3600
 '360000 = 1 * 100 * 3600
 If Summe_as_entladen > 360000 Then
  Incr Summe_mah_entladen
  Summe_as_entladen = Summe_as_entladen - 360000
 End If
 If Summe_as_laden > 360000 Then       'Das kann passieren wenns den Berg runter geht
  Incr Summe_mah_laden
  Summe_as_laden = Summe_as_laden - 360000
 End If
 Erzeugt = Summe_mah_laden
 Verbraucht = Summe_mah_entladen
 Erzeugt = Erzeugt * 0.7       'Das ist der Akku-Wirkungsgrad 0.7 (0.85) bezogen auf den Stom
 Verbraucht = Verbraucht - Erzeugt
 Akkuinhalt = Akkukap
 Akkuverbrauch = Verbraucht
 Akkuprozent = Akkuverbrauch / Akkuinhalt
 Akkuprozent = Akkuprozent * 1000
 Akkuprozent = Int(akkuprozent)
 Akkustand = Akkuprozent
 '</verbrauchte As berechnen>

 '<Strecke berechnen>
 Kx = K_links + K_rechts
 Kx = Kx / 2
 Strecke = Kx * Di
 Zentimeter = Zentimeter + Strecke
 If Zentimeter >= 100 Then
  Incr Meter
  Zentimeter = Zentimeter - 100
 End If
 If Zentimeter < 0 Then
  Decr Meter
  Zentimeter = Zentimeter + 100
 End If
 If Meter >= 1000 Then
  Incr Kilometer
  Meter = Meter - 1000
 End If
 If Meter < 0 Then
  Decr Kilometer
  Meter = Meter + 1000
 End If
 '</Strecke berechnen>

 '<Geschwindigkeit berechnen>
 Kv = Kv + Kx
 Kmh = Kv * Kmhfaktor
 Kv = Kv * 0.75       'Bei Änderung auch den kmhfaktor neu berechnen
 If Kmh > Max_v Then Max_v = Kmh
 If Kmh < Min_v Then Min_v = Kmh
 '</Geschwindigkeit berechnen>

 If Bzeit < 500 Then Incr Bzeit
 Incr Intcount
'******************************************************************************
'ADiverse Werte werden an den Tacho geschickt. Manche für die Funktion
'des Tachos, andere um die Steuerung zu optimieren.
'******************************************************************************
Incr Tachocount

Select Case Tachocount
 Case 0
  B(1) = 40       'Error Word
  B(2) = Errorword1
  B(3) = Errorword2
  B(5) = 1
 Case 1
  B(1) = 1       'U
  U = Ugemessen
  B(2) = Motormeldung(1)
  B(3) = Motormeldung(2)
  B(5) = 1
 Case 2
  B(1) = 2       'I
  I = Igemessen / 10
  B(2) = Motormeldung(3)
  B(3) = Motormeldung(4)
  B(5) = 1
 Case 3
  B(1) = 3       'Km/h
  B(2) = Kmhmeldung(1)
  B(3) = Kmhmeldung(2)
  B(5) = 1
 Case 4
  B(1) = 5       'Akkuprozente
  B(2) = Akkumeldung(1)
  B(3) = Akkumeldung(2)
  B(5) = 1
 Case 5
  B(1) = 7       'Meter
  B(2) = Metermeldung(1)
  B(3) = Metermeldung(2)
  B(5) = 1
 Case 6
  B(1) = 6       'Kilometer
  B(2) = Kilometermeldung(1)
  B(3) = Kilometermeldung(2)
  B(5) = 1
 Case 7
  If Antriebaus = 0 Then
   B(1) = 8       'Low
   B(2) = Mit_acc_roll1
   B(3) = Mit_acc_roll2
   B(5) = 1
  Else
   B(1) = 12       'Low
   B(2) = Null_roll1
   B(3) = Null_roll2
   B(5) = 1
 End If
 Case 8
  If Antriebaus = 0 Then
   B(1) = 9       'High
   B(2) = Mit_acc_roll3
   B(3) = Mit_acc_roll4
   B(5) = 1
  Else
   B(1) = 13       'High
   B(2) = Null_roll3
   B(3) = Null_roll4
   B(5) = 1
  End If
 Case 9
  B(1) = 4       'Status
  B(2) = Antriebaus
  B(3) = Ta
  B(5) = 1
 Case 10
  If Antriebaus = 0 Then
   B(1) = 10       'acc_Nick
   B(2) = Mit_acc_nick1
   B(3) = Mit_acc_nick2
   B(5) = 1
  Else
   B(1) = 14
   B(2) = Null_nick1
   B(3) = Null_nick2
   B(5) = 1
  End If
 Case 11       'Nick und Roll
  If Antriebaus = 0 Then
   B(1) = 11       'acc_Nick
   B(2) = Mit_acc_nick3
   B(3) = Mit_acc_nick4
   B(5) = 1
  Else
   B(1) = 15
   B(2) = Null_nick3
   B(3) = Null_nick4
   B(5) = 1
  End If
 Case 12
  B(1) = 16       'Stellwert
  B(2) = Stellwert1
  B(3) = Stellwert2
  B(5) = 1
 Case 13
  B(1) = 17       'Stellwert
  B(2) = Stellwert3
  B(3) = Stellwert4
  B(5) = 1
 Case 14
  Gyronick = Sig_gyro_nick
  B(1) = 18       'Gyro Nick_l
  B(2) = Gyronick1
  B(3) = Gyronick2
  B(5) = 1
 Case 15
  B(1) = 19       'Gyro Nick_h
  B(2) = Gyronick3
  B(3) = Gyronick4
  B(5) = 1
 Case 16
  Kksum = K_sum
  B(1) = 20       '??_l
  B(2) = Kksum1
  B(3) = Kksum2
  B(5) = 1
 Case 17
  B(1) = 21       '??_h
  B(2) = Kksum3
  B(3) = Kksum4
  B(5) = 1
  If Antriebaus = 0 Then Tachocount = 255       'Gleich zu 0 überlaufen lassen
 Case 18
  B(1) = 22
  B(2) = Pf1
  B(3) = Pf2
  B(5) = 1
 Case 19
  B(1) = 23
  B(2) = Pf3
  B(3) = Pf4
  B(5) = 1
 Case 20
  B(1) = 24
  B(2) = If1
  B(3) = If2
  B(5) = 1
 Case 21
  B(1) = 25
  B(2) = If3
  B(3) = If4
  B(5) = 1
 Case 22
  B(1) = 26
  B(2) = Df1
  B(3) = Df2
  B(5) = 1
 Case 23
  B(1) = 27
  B(2) = Df3
  B(3) = Df4
  B(5) = 1
 Case 24
  B(1) = 28
  B(2) = Mpf1
  B(3) = Mpf2
  B(5) = 1
 Case 25
  B(1) = 29
  B(2) = Mpf3
  B(3) = Mpf4
  B(5) = 1
 Case 26
  B(1) = 30
  B(2) = Mvf1
  B(3) = Mvf2
  B(5) = 1
 Case 27
  B(1) = 31
  B(2) = Mvf3
  B(3) = Mvf4
  B(5) = 1
 Case 28
  B(1) = 32
  B(2) = Mplf1
  B(3) = Mplf2
  B(5) = 1
 Case 29
  B(1) = 33
  B(2) = Mplf3
  B(3) = Mplf4
  B(5) = 1
 Case 30
  B(1) = 34
  B(2) = Mvlf1
  B(3) = Mvlf2
  B(5) = 1
 Case 31
  B(1) = 35
  B(2) = Mvlf3
  B(3) = Mvlf4
  B(5) = 1
 Case 32
  B(1) = 36       'Port Status
  B(2) = Portc
  B(3) = Porta
  B(5) = 1
 Case 33
  B(1) = 37       'Fussschalter 1
  Wwert = Adc0       'Word
  B(2) = Wwert1
  B(3) = Wwert2
  B(5) = 1
 Case 34
  B(1) = 38       'Fussschalter 2
  Wwert = Adc1       'Word
  B(2) = Wwert1
  B(3) = Wwert2
  B(5) = 1
 Case 35
  B(1) = 39       'Fussschalter gesamt
  B(2) = Fussschalter
  B(3) = 0
  B(5) = 1
  Tachocount = 255
 Case Else
  B(5) = 0       'Nichts ausgeben
End Select

 If Intcount = 99 Then Intcount = 255

 If B(5) = 1 Then
 'Checksumme berechnen
 B(4) = B(1)
 B(4) = B(4) + B(2)
 B(4) = B(4) + B(3)
 'Print Chr(b(1)) ; Chr(b(2)) ; Chr(b(3)) ; Chr(b(4)) ; Chr(13);
 'Print geht so in Assembler:
  !LDS     R24,{B(1)}       'Adresse von b(1)
  !STS     udr,R24
 Warten11:
  !LDS     R24,UCSR0A
  !BST     R24,5
  !Brtc warten11
  !LDS     R24,{B(2)}       'Adresse von b(2)
  !STS     udr,R24
 Warten12:
  !LDS     R24,UCSR0A
  !BST     R24,5
  !Brtc warten12
  !lds     R24,{B(3)}       'Adresse von B(3)
  !STS     udr,R24
 Warten13:
  !LDS     R24,UCSR0A
  !BST     R24,5
  !Brtc warten13
  !LDS     R24,{B(4)}       'Adresse von B(4)
  !STS     udr,R24
 Warten14:
  !LDS     R24,UCSR0A
  !BST     R24,5
  !Brtc warten14
  !LDI     R24,13       'CR
  !STS     udr,R24
 Warten15:
  !LDS     R24,UCSR0A
  !BST     R24,5
  !Brtc warten15

 Ta = Timer0       'Um festzustellen ob der Interrupt zu lange dauert
                   'Wenn Ja, ist der Zählwert nahe 255
End If
Return

Sub Flashled(byval Which As Byte , Byval Dauer_ms As Byte)
 Dim Done As Byte
 Dim Waitstarttime As Byte
 Dim Xx As Byte

 Select Case Which
  Case 0       'Both
   Led1 = 1
   Led2 = 1
   Waitms Dauer_ms
   Led1 = 0
   Led2 = 0
   Waitms Dauer_ms
  Case 1       'LED1
   Led1 = 1
   Waitms Dauer_ms
   Led1 = 0
   Waitms Dauer_ms
  Case 2       'LED2
   Led2 = 1
   Waitms Dauer_ms
   Led2 = 0
   Waitms Dauer_ms
 End Select
End Sub

Readfussschalter:
 Adc0 = Getadc(0)
 If Adc0 > Fussschalterschwelle Then
  Fussschalter.0 = 1
 Else
  Fussschalter.0 = 0
 End If

 Adc1 = Getadc(1)
 If Adc1 > Fussschalterschwelle Then
  Fussschalter.1 = 1
 Else
  Fussschalter.1 = 0
 End If

 If Fussschalter = 0 Then       'Max 0.5 Sekunden ohne Fussschalter
  If Fusscount > 0 Then Decr Fusscount
 Else
  Fusscount = 50       'alles OK
 End If

 If Fusscount = 0 Then
  If Antriebaus = 0 Then
   Antriebaus = 1
   Waitms 100       'Damit der Regel_interrupt auch was davon mitbekommt
   'Disable Interrupts
   'Betriebsdatenschreiber
   'Enable Interrupts
  End If
 End If
Return

Read_gyro:       'Beschleunigung
 I2cstart
 I2cwbyte Gyroadress_w
 'Start address for Read
 I2cwbyte &B10101000       'Out_x_l AND &B10000000
 I2cstart
 I2cwbyte Gyroadress_r
 'I2crbyte Temperatur , Ack
 'I2crbyte Status , Ack

 I2crbyte Gyro_roll1 , Ack
 I2crbyte Gyro_roll2 , Ack

 I2crbyte Gyro_nick1 , Ack
 I2crbyte Gyro_nick2 , Ack

 I2crbyte Gyro_gier1 , Ack
 I2crbyte Gyro_gier2 , Nack
 I2cstop
Return

Read_acc:       'Neigung
 I2cstart
 I2cwbyte Accadress_w
 'Start address for Read
 I2cwbyte &B10101000       'Out_x_l_a AND &B10000000
 I2cstart
 I2cwbyte Accadress_r

 'I2crbyte Status_a , Ack

 I2crbyte Accroll_l , Ack
 I2crbyte Accroll_h , Ack
 Acc_roll = Accroll
 Acc_roll = Acc_roll / 2       'Divisor = Empfindlichkeit

 I2crbyte Accnick_l , Ack
 I2crbyte Accnick_h , Ack
 Acc_nick = Accnick
 Acc_nick = Acc_nick / 10       'Divisor = Empfindlichkeit

 I2crbyte Accgier_l , Ack
 I2crbyte Accgier_h , Nack
 Acc_gier = Accgier
 Acc_gier = Acc_gier / 10       'Divisor = Empfindlichkeit
 I2cstop
Return

'Get_mag_data:
' Slaveadress = Magadress
' Subadress = Out_x_h_m
' Gosub Read_6_mag

' Xmag = Dataarray(1)
' Shift Xmag , Left , 8
' Xmag = Xmag + Dataarray(2)

' Ymag = Dataarray(3)
' Shift Ymag , Left , 8
' Ymag = Ymag + Dataarray(4)

' Zmag = Dataarray(5)
' Shift Zmag , Left , 8
' Zmag = Zmag + Dataarray(6)

' Xmag = Xmag - Xmagoff
' Ymag = Ymag - Ymagoff
'Return

Init_gyro:
 'refer to MiniMu9 L3G4200D_Gyro.pdf, page 29
 Slaveadress = Gyroadress_w
 Subadress = Ctrl_reg1
 '01             ==> 200Hz
 '  11           ==> Bandwidth = 70
 '    0          ==> Power Down Mode
 '     1         ==> Z-Axis enable
 '      1        ==> Y-Axis enable
 '       1       ==> X-Axis enable
 Databyte = &B00001111       'Enable x, y, z and turn off power down
 I2cstart
 I2cwbyte Slaveadress
 I2cwbyte Subadress
 I2cwbyte Databyte
 I2cstop
 Subadress = Ctrl_reg2

 Databyte = &B0010_1001       'adjust/use the HPF
 I2cstart
 I2cwbyte Slaveadress
 I2cwbyte Subadress
 I2cwbyte Databyte
 I2cstop

 Subadress = Ctrl_reg3
 Databyte = &B0000_1000
 I2cstart
 I2cwbyte Slaveadress
 I2cwbyte Subadress
 I2cwbyte Databyte
 I2cstop

 Subadress = Ctrl_reg4
 '1             ==> Block Data Update
 ' 0            ==> BLE
 '  00          ==> 250 dps
 '    0         ==> not used
 '     00       ==> Self Test 0=Normal Mode
 '       0      ==> SIM 0=4-wire interface
 Databyte = &B10000000
 I2cstart
 I2cwbyte Slaveadress
 I2cwbyte Subadress
 I2cwbyte Databyte
 I2cstop

 Subadress = Ctrl_reg5
 Databyte = &B0000_0000       'high-pass filtering of outputs
 I2cstart
 I2cwbyte Slaveadress
 I2cwbyte Subadress
 I2cwbyte Databyte
 I2cstop
Return

Init_acc:
 Slaveadress = Accadress_w
 Subadress = Ctrl_reg1_a
 '001            ==> Normal Mode
 '   01          ==> 100Hz, Bandwith=74
 '     1         ==> Z-Axis enable
 '      1        ==> Y-Axis enable
 '       1       ==> X-Axis enable
 Databyte = &B00101111
 I2cstart
 I2cwbyte Slaveadress
 I2cwbyte Subadress
 I2cwbyte Databyte
 I2cstop

 Slaveadress = Accadress_w
 Subadress = Ctrl_reg4_a
 '1             ==> Continuous, update when finished
 ' 0            ==> BLE
 '  00          ==> FSE = 2g
 '    0         ==> Self Test Sign
 '     0        ==> not used
 '      0       ==> Self Test Enable
 '       0      ==> not used
 Databyte = &B10000000       'Block Data Update, LSB low, Scale +-2g
 I2cstart
 I2cwbyte Slaveadress
 I2cwbyte Subadress
 I2cwbyte Databyte
 I2cstop
Return

'Init_mag:
'  Slaveadress = Magadress
'  Subadress = Cra_reg_m
'  Databyte = &B00011000                                    '75Hz Output
'  I2cstart
'  I2cwbyte Slaveadress
'  I2cwbyte Subadress
'  I2cwbyte Databyte
'  I2cstop

'  Slaveadress = Magadress
'  Subadress = Crb_reg_m
'  Databyte = &B01000000                                    'range +-1.9Gauss
'  I2cstart
'  I2cwbyte Slaveadress
'  I2cwbyte Subadress
'  I2cwbyte Databyte
'  I2cstop

'  Slaveadress = Magadress
'  Subadress = Mr_reg_m
'  Databyte = &B00000000                                    'continious mode
'  I2cstart
'  I2cwbyte Slaveadress
'  I2cwbyte Subadress
'  I2cwbyte Databyte
'  I2cstop
'Return

Sub Resetdata
 Disable Interrupts
 Regelzeit = 0
 Regelzeitmax = 0
 Mit_acc_nick_max = 0
 Mit_acc_roll_max = 0
 Mit_acc_nick_min = 0
 Mit_acc_roll_min = 0
 Pwm_rechts_max = 0
 Pwm_links_max = 0
 Seriell_err_rechts = 0
 Seriell_err_links = 0
 K_diff_min = 0
 K_diff_max = 0
 Antriebaus = 0
 Fusscount = 50       '0,5 Sekunden
 Enable Interrupts
End Sub

Display3000.bas (BASCOM)


'****************Listrik01 LCD Display Steuerung Version 01 *****************
$hwstack = 256
$swstack = 256
$framesize = 64
$regfile = "m2561def.dat"
$crystal = 16000000       'microcontroller crystal frequency
$baud1 = 57600

'(start block comment
  ---------------------
 |                     |
 |   ---------------   |
 |  |               |  |
 |  |               |  |
 |  |               |  |
 |  |               |  |
 |  |               |  |
 |  |               |  |
 |  |               |  |
 |  |               |  | Port D.0 = Tageskilometerzähler Reset
 |  |               |  |
 |  |               |  |
 |  | --------------|  | Display:
 |  | 1| 2|       |3|  |  1 = Zyklusdauer (0..255)
 |   ---------------   |  2 = Motorleistung links
 |                     |  3 = Motorleistung rechts
 |                     |
 |   O1            O2  |
 |          O5         | Schalter:
 |                     |  1: PinD.7 Zeile abwärts
 |       O7    O8      |  2: PinD.4 Zeile aufwärts
 |                     |  3: PinE.5 Blättern
 |          O6         |  4: PinE.7 SD-Karte aktivieren
 |   O3            O4  |  5: PinG.0 Werte übernehmen
 |                     |  6: PinG.3 Taste defekt
 |                     |  7: PinG.1 Zahl kleiner
 |                     |  8: PinD.5 Zahl größer
 |                     |  9: Cpu Reset
 |                     |
 |                     |
 |                 O9  |
  ---------------------
')end block comment

'####################### LCD-Routines ######################################

'Definition of used ports and pull up resistors.
'We are using Port B for the SPI-communication to the LCD Display.

Ddra = &B00000000       'switch all 8 Ports of Port A to input (0),
                        'Pin (PA.0 - PA.7)
Porta = &B11111111       'All port pins have individually selectable pull-up
                        'resistors. Here we enable these pull-up-resisitors,
                        'so these Pins are always at logical 1
                        'You Need To Pull These Pins Against Ground(gnd)

'Select Port B as to an output port (data output to the display)
Ddrb = &B01110110       'DDR = Data direction register; Port B1, B2, B4, B5, B6
                        'switched to output (1) as needed by the display
Portb = &B10001001       'the other ports of Port B are inputs with
                         'switched on pull up resistors

Ddrc = &B00000000       'switch all Ports of Port C to input
Portc = &B11111111       'all pull-up-Resistors turned on

Ddrd = &B00000000       'switchall Ports of Port D to input
Portd = &B11110011       'almost all pull-up-Resistors turned on

Ddre = &B10000000       'E7=Output (SD card activate), all other Input
Porte = &B11111111       'all pull-up-Resistors turned on

Ddrf = &B00000000       'switch all Ports of Port F to input
Portf = &B11111111       'all pull-up-Resistors turned on

Ddrg = &B00000000       'switch all Ports of Port G to input
Portg = &B11111111       'all pull-up-Resistors turned on
'#############################################################################

Declare Sub Activate_display_bus
Declare Sub Activate_sd_bus()
Declare Sub Minmax()
Declare Function Trimtext(t As String) As String * 22
Declare Sub Drawpagestatics

Const Titel = "Tacho 0.1             "

Dim Ep0 As Byte At $0201
Dim Ep1 As Byte At $0202
Dim Ep2 As Byte At $0203
Dim Ep3 As Byte At $0204
Dim Ep4 As Byte At $0205
Dim Ep5 As Byte At $0206
Dim Iwert As Integer At $0203 Overlay
Dim Wwert As Integer At $0203 Overlay
Dim Index As Byte At $202 Overlay

Dim Maccx As Long At $0207
Dim Maccx1 As Byte At $0207 Overlay
Dim Maccx2 As Byte At $0208 Overlay
Dim Maccx3 As Byte At $0209 Overlay
Dim Maccx4 As Byte At $020a Overlay
Dim Maccy As Long At $020b
Dim Maccy1 As Byte At $020b Overlay
Dim Maccy2 As Byte At $020c Overlay
Dim Maccy3 As Byte At $020d Overlay
Dim Maccy4 As Byte At $020e Overlay

Dim Nullx As Long At $020f
Dim Nullx1 As Byte At $020f Overlay
Dim Nullx2 As Byte At $0210 Overlay
Dim Nullx3 As Byte At $0211 Overlay
Dim Nullx4 As Byte At $0212 Overlay
Dim Nully As Long At $0213
Dim Nully1 As Byte At $0213 Overlay
Dim Nully2 As Byte At $0214 Overlay
Dim Nully3 As Byte At $0215 Overlay
Dim Nully4 As Byte At $0216 Overlay
Dim Sw As Single At $00217

Dim Sw1 As Byte At $0217 Overlay
Dim Sw2 As Byte At $0218 Overlay
Dim Sw3 As Byte At $0219 Overlay
Dim Sw4 As Byte At $021a Overlay

Dim Gyronick As Long At $0021b
Dim Gyronick1 As Byte At $021b Overlay
Dim Gyronick2 As Byte At $021c Overlay
Dim Gyronick3 As Byte At $021d Overlay
Dim Gyronick4 As Byte At $021e Overlay

Dim Kksum As Single At $0021f
Dim Kksum1 As Byte At $021f Overlay
Dim Kksum2 As Byte At $0220 Overlay
Dim Kksum3 As Byte At $0221 Overlay
Dim Kksum4 As Byte At $0222 Overlay

Dim P_faktor As Single At $0223
Dim P_f1 As Byte At $0223 Overlay
Dim P_f2 As Byte At $0224 Overlay
Dim P_f3 As Byte At $0225 Overlay
Dim P_f4 As Byte At $0226 Overlay

Dim I_faktor As Single At $0227
Dim I_f1 As Byte At $0227 Overlay
Dim I_f2 As Byte At $0228 Overlay
Dim I_f3 As Byte At $0229 Overlay
Dim I_f4 As Byte At $022a Overlay

Dim D_faktor As Single At $022b
Dim D_f1 As Byte At $022b Overlay
Dim D_f2 As Byte At $022c Overlay
Dim D_f3 As Byte At $022d Overlay
Dim D_f4 As Byte At $022e Overlay

Dim Mp_faktor As Single At $022f
Dim Mp_f1 As Byte At $022f Overlay
Dim Mp_f2 As Byte At $0230 Overlay
Dim Mp_f3 As Byte At $0231 Overlay
Dim Mp_f4 As Byte At $0232 Overlay

Dim Mv_faktor As Single At $0233
Dim Mv_f1 As Byte At $0233 Overlay
Dim Mv_f2 As Byte At $0234 Overlay
Dim Mv_f3 As Byte At $0235 Overlay
Dim Mv_f4 As Byte At $0236 Overlay

Dim Mpl_faktor As Single At $0237
Dim Mpl_f1 As Byte At $0237 Overlay
Dim Mpl_f2 As Byte At $0238 Overlay
Dim Mpl_f3 As Byte At $0239 Overlay
Dim Mpl_f4 As Byte At $023a Overlay

Dim Mvl_faktor As Single At $023b
Dim Mvl_f1 As Byte At $023b Overlay
Dim Mvl_f2 As Byte At $023c Overlay
Dim Mvl_f3 As Byte At $023d Overlay
Dim Mvl_f4 As Byte At $023e Overlay

Dim Pfaktor As Long
Dim Ifaktor As Long
Dim Dfaktor As Long
Dim Mpfaktor As Long
Dim Mvfaktor As Long
Dim Mplfaktor As Long
Dim Mvlfaktor As Long

Dim Stellwert As Single
Dim Gyro_nick As Long
Dim Null_y As Long
Dim Null_x As Long
Dim Mit_acc_y As Long
Dim Mit_acc_x As Long
Dim K_sum As Single
Dim K_summax As Single
Dim K_summin As Single

Dim Bdummy As Byte
Dim Singdummy As Single
Dim Wdummy As Word
Dim Ldummy As Long
Dim Idummy As Integer
Dim Kilometer As Word
Dim Meter As Integer
Dim Akkuprozente As Integer
Dim Kmh As Integer
Dim Kmhmax As Integer
Dim Kmhmin As Integer

Dim I As Integer
Dim Imax As Integer
Dim Imin As Integer

Dim U As Integer
Dim Umax As Integer
Dim Umin As Integer
Dim Ml As Integer
Dim Mlmax As Integer
Dim Mrmax As Integer
Dim Mr As Integer
Dim Mlmin As Integer
Dim Mrmin As Integer
Dim Modus As Byte
Dim Editzeile As Byte
Dim Utext As String * 22
Dim Itext As String * 22
Dim Ltext As String * 22
Dim L1 As Byte
Dim L2 As Byte
Dim L3 As Byte
Dim Text As String * 22
Dim Farbe As Word
Dim X1 As Byte
Dim Y1 As Byte
Dim X2 As Byte
Dim Y2 As Byte
Dim Startkilometer As Single
Dim Ta As Byte
Dim Tamax As Byte

Dim Antriebaus As Byte

Dim Xoffset As Long
Dim Yoffset As Long
Dim Ya As Byte
Dim Xa As Byte

'Dim Lname As Long
'Dim Dateiname As String * 12
'Dim Dateilaenge As Long
'Dim Dateiaktuell As Byte
Dim Gbdriveerror As Byte
Dim Ulog As Single
Dim Ilog As Single
Dim Kmhlog As Single
Dim Kmlog As Single
Dim Akkuprozentelog As Single
Dim Loopcount As Word
Dim Aloopcount As Word
Dim Logtext As String * 255
Dim Zeit As String * 8
Dim Sichern As Byte
Dim Transferok As Byte

Dim Hexstr As String * 2

Dim Hauptstportc As Byte
Dim Hauptstporta As Byte
Dim Ad0 As Word       'AD value 0...1024
Dim Ad1 As Word       'AD value 0...1024
Dim Fussschalter As Byte       'Bit0 und Bit1
Dim Dummybyte As Byte

Config Clock = User
Config Date = Dmy , Separator = .

'No Need To Call Here Because Init21_display3000.bas Has Already Done This
Call Activate_display_bus

$include "Y:\Programme\Tools\BASCOM-AVR\Display 3000\Displaymodul\Bascom Programme\für Include\Init21_display3000.bas"

 'If Porte.7 = 0 Then Activate_sd_bus
 'Gbdriveerror = Driveinit()       'Init MMC/SD Card
 'If Gbdriveerror = 0 Then
 ' Include AVR-DOS Configuration and library
 ' $include "Y:\Programme\Tools\BASCOM-AVR\Display 3000\SD Speicherkartenmodul P001\Bascom Basic\Config_AVR-DOS.BAS"
 ' Bdummy = Initfilesystem(1)       ' Partition 1
 ' Ldummy = Disksize()
 ' Ldummy = Ldummy / 1024
 ' Utext = Str(ldummy)
 ' Utext = "Size: " + Utext
 ' Utext = Utext + " mb"
 ' Ldummy = Diskfree()
 ' Ldummy = Ldummy / 1024
 ' Itext = Str(ldummy)
 ' Itext = "Free: " + Itext
 ' Itext = Itext + " mb"
 ' If Porte.7 = 1 Then call Activate_display_bus
 '  gosub Lcd_cls
 '  call Lcd_print(Titel , 0 , 0 , 1 , 1 , 1 , Yellow , Blue)
 '  call Lcd_print ("SD-Karte init" , 0 , 18 , 1 , 1 , 1 , Black , White)
 '  call Lcd_print (Utext , 0 , 27 , 1 , 1 , 1 , Black , White)
 '  Lcd_print Itext , 0 , 36 , 1 , 1 , 1 , Black , White
 '  Wait 7
 ' Else
 '  If Porte.7 = 1 Then call Activate_display_bus
 '  Ltext = Str(gbdriveerror)
 '  Ltext = "Fehler: " + Ltext
 '  gosub Lcd_cls
 '  call Lcd_print (Titel , 0 , 0 , 1 , 1 , 1 , Yellow , Blue)
 '  call Lcd_print ("SD-Karte init" , 0 , 18 , 1 , 1 , 1 , Black , White)
 '  call Lcd_print (Ltext , 0 , 27 , 1 , 1 , 1 , Black , White)
 '  Wait 7
 ' End If

Readeeprom Startkilometer , 1
Readeeprom Xoffset , 5
Readeeprom Yoffset , 9

'Einige Werte vorbelegen um fehlerhafte Grafik zu vermeiden wenn keine
'serielle Verbindung besteht
Xa = 74
Ya = 130
Antriebaus = 1
Null_x = Xoffset
Null_y = Yoffset

'If Porte.7 = 1 Then call Activate_display_bus
Orientation = Portrait180       'landscape mode
Graphics_mode = 65k_uncompressed       'select the needed color mode, here 65.536 colors
Call Lcd_cls

Call Drawpagestatics

Config Timer2 = Timer , Prescale = 128
Assr.exclk = 0
Assr.as2 = 1

On Timer2 Tick
Enable Timer2
Start Timer2

On Urxc1 Datenempfang
Enable Urxc1
Enable Interrupts
Minmax
Do
 If Pine.5 = 0 Then
  While Pine.5 = 0 : Wend       'Debounce Button
  Incr Modus
  If Gbdriveerror <> 0 Then       'Logging nicht zulassen wenn DriveError
   If Modus > 5 Then
    Modus = 0
   end  if
  end If

  If Modus <> 4 Then'nur berechnen wenn die Werte nicht editiert werden
   Singdummy = P_faktor * 10000
   Singdummy = Round(Singdummy)
   Pfaktor = Singdummy

   Singdummy = I_faktor * 1000000
   Singdummy = Round(Singdummy)
   Ifaktor = Singdummy

   Singdummy = D_faktor * 1000
   Singdummy = Round(Singdummy)
   Dfaktor = Singdummy

   Singdummy = Mp_faktor * 10000
   Singdummy = Round(Singdummy)
   Mpfaktor = Singdummy

   Singdummy = Mv_faktor * 1000
   Singdummy = Round(Singdummy)
   Mvfaktor = Singdummy

   Singdummy = Mpl_faktor * 10000
   Singdummy = Round(Singdummy)
   Mplfaktor = Singdummy

   Singdummy = Mvl_faktor * 1000
   Singdummy = Round(Singdummy)
   Mvlfaktor = Singdummy
  End If

  If Modus > 5 Then Modus = 0
  'If Porte.7 = 1 Then call Activate_display_bus
  Call Lcd_cls
  Call Drawpagestatics
 End If
 If Modus <> 5 Then       'Nur wenn nicht geloggt wird
  If U <> 0 Then       'Nur wenn auch schon eine Spannung übertragen wurde
   If Ta <> 0 Then
    Idummy = U       'Nur wenn auch schon ein Ta übertragen wurde
    If Idummy > Umax Then Umax = Idummy
    If Idummy < Umin Then Umin = Idummy
    Idummy = I
    If Idummy > Imax Then Imax = Idummy
    If Idummy < Imin Then Imin = Idummy
    Idummy = Kmh
    If Idummy > Kmhmax Then Kmhmax = Idummy
    If Idummy < Kmhmin Then Kmhmin = Idummy
    Bdummy = Ta
    If Bdummy > Tamax Then Tamax = Bdummy
    Idummy = Ml
    If Idummy > Mlmax Then Mlmax = Idummy
    If Idummy < Mlmin Then Mlmin = Idummy
    Idummy = Mr
    If Idummy > Mrmax Then Mrmax = Idummy
    If Idummy < Mrmin Then Mrmin = Idummy
   End If
  End If
 End If

Select Case Modus
 Case 0
  'If Dateiaktuell = 1 Then
  ' If Porte.7 = 0 Then Activate_sd_bus
  ' Close #2
  ' Dateiaktuell = 0
  'End If
  'If Porte.7 = 1 Then call Activate_display_bus       'Standard Anzeige
  If Pind.0 = 0 Then
   Waitms 3
   If Pind.0 = 0 Then
    While Pind.0 = 0 : Wend
    Singdummy = Meter / 1000
    Singdummy = Singdummy + Kilometer
    Startkilometer = Singdummy
    Writeeeprom Startkilometer , 1
   End If
  End If

  'Spannung und Strom
  Singdummy = U / 100
  Utext = Fusing(singdummy , "#.#")
  Utext = Utext + "V"
  Utext = " " + Utext
  Singdummy = I / 100
  Itext = Fusing(singdummy , "#.#")
  Itext = Itext + "A "
  L1 = Len(utext)
  L2 = Len(itext)
  L1 = L1 + L2
  L2 = 22 - L1
  Ltext = String(l2 , 32)
  Text = Utext + Ltext
  Text = Text + Itext
  Lcd_print Text , 0 , 75 , 1 , 1 , 1 , Black , White

  'Akku in Prozent
  Singdummy = Akkuprozente / 10
  Singdummy = 100 - Singdummy
  Text = Fusing(singdummy , "#.#" )
  Text = " " + Text + "% "
  If Singdummy > 66 Then
   Farbe = Green
  Elseif Singdummy > 33 Then
   Farbe = Yellow
  Else
   Farbe = Red
  End If
  Singdummy = Singdummy * 1.14       '124 - 8 - 1 - 1 = 114 / 100% = 1.14
  X2 = Singdummy       'Type cast
  X2 = X2 + 10       '10 wegen der "100 - Sdummy" weiter oben
  Lcd_box X2 , 85 , 123 , 94 , White       'Hintergrund löschen
  Lcd_box 9 , 85 , X2 , 94 , Farbe       'Balken zeichnen
  'Text mittig platzieren
  L1 = Len(text)
  L1 = L1 * 5
  L1 = L1 / 2
  X1 = 66 - L1
  Lcd_print Text , X1 , 98 , 1 , 1 , 1 , Black , White       'unterm Balken

  'km/h
  Singdummy = Kmh / 10
  Text = Fusing(Singdummy , "#.#")
  Text = Text + "km/h "
  Text = " " + Text
  L1 = Len(text)
  Text = Text + " "
  L1 = L1 * 4
  X1 = 66 - L1
  Lcd_print Text , X1 , 12 , 2 , 1 , 2 , Black , White

  'km
  Singdummy = Meter / 1000
  Singdummy = Singdummy + Kilometer
  Text = Fusing(Singdummy , "#.#")
  Text = Text + "km  "
  Text = " " + Text
  L1 = Len(text)
  L1 = L1 * 5
  L1 = L1 / 2
  X1 = 66 - L1
  Lcd_print Text , X1 , 46 , 1 , 1 , 1 , Black , White

  'Tageskilometerzähler
  Singdummy = Singdummy - Startkilometer
  Text = Fusing(Singdummy , "#.###")
  Text = Text + "km  "
  Text = " " + Text
  L1 = Len(text)
  L1 = L1 * 5
  L1 = L1 / 2
  X1 = 66 - L1
  Lcd_print Text , X1 , 60 , 1 , 1 , 1 , Black , White

  If Antriebaus = 1 Then
   Text = "AUS"
  Else
   Text = "RUN"
  End If
  'Text mittig platzieren
  L1 = Len(text)
  L1 = L1 * 5       'Schriftlänge in Pixel, jedes Zeichen = 5 Pixel
  L1 = L1 / 2       'Halbe schriftlänge
  X1 = 74 - L1       'Text mittig
  Lcd_print Text , X1 , 148 , 1 , 1 , 1 , Black , White

'>>>>>>>>>>>> Fusszeile im Display >>>>>>>>>>>>
'  01234567891111111111222
'            0123456789012
'  xxx       xxx      xxx
'PmotLinks   Ta       PmotRechts
'  L1        L2       L3

  Text = String(22 , 32)       'Fill with spaces

  Utext = Str(ml)       'Pmot_links (Integer)
  L1 = Len(utext)
  Mid(text , 0 , L1) = Utext       'insert Ml

  Utext = Str(ta)       'Ticks (Byte)
  L2 = Len(utext)
  Mid(text , 10 , L2) = Utext       'insert Ta

  Utext = Str(mr)       'Pmot_rechts (Integer)
  L3 = Len(utext)
  X1 = 23 - L3       'Starting position of Mr
  Mid(text , X1 , L3) = Utext       'insert Mr

  'L2 = X1 + L3
  'L1 = 22 - L2
  'Utext = String(l1 , 32)
  'Mid(text , L2 , L1) = Utext       'insert final spaces

  Lcd_print Text , 0 , 168 , 1 , 1 , 1 , White , Black
'>>>>>>>>>>>> Fusszeile im Display >>>>>>>>>>>>

  If Antriebaus = 1 Then
   Singdummy = Null_x
   Singdummy = Singdummy - Xoffset
   Singdummy = Singdummy / 50
   If Singdummy > 49 Then Singdummy = 49
   If Singdummy < -49 Then Singdummy = -49
   Singdummy = Singdummy + 74
   X1 = Int(Singdummy)
   Lcd_draw Xa , 126 , Xa , 134 , 0 , White
   Lcd_draw X1 , 126 , X1 , 134 , 0 , Black
   Xa = X1
   Singdummy = Null_y
   Singdummy = Singdummy - Yoffset
   Singdummy = Singdummy / 50
   If Singdummy > 29 Then Singdummy = 29
   If Singdummy < -29 Then Singdummy = -29
   Singdummy = Singdummy + 130
   Y1 = Int(Singdummy)
   Lcd_draw 9 , Ya , 18 , Ya , 0 , White
   Lcd_draw 9 , Y1 , 18 , Y1 , 0 , Black
   Ya = Y1
  Else
   'Horizontaler Balken
   Singdummy = Mit_acc_x
   Singdummy = Singdummy / 50
   If Singdummy > 49 Then Singdummy = 49
   If Singdummy < -49 Then Singdummy = -49
   Singdummy = Singdummy + 74
   X1 = Int(Singdummy)
   Lcd_draw Xa , 126 , Xa , 134 , 0 , White
   Lcd_draw X1 , 126 , X1 , 134 , 0 , Black
   Xa = X1

   'Vertikaler Balken
   Singdummy = Mit_acc_y
   Singdummy = Singdummy / 50
   If Singdummy > 29 Then Singdummy = 29
   If Singdummy < -29 Then Singdummy = -29
   Singdummy = Singdummy + 130
   Y1 = Int(Singdummy)
   Lcd_draw 9 , Ya , 18 , Ya , 0 , White
   Lcd_draw 9 , Y1 , 18 , Y1 , 0 , Black
   Ya = Y1
  End If
 Case 1       'Details
  'If Dateiaktuell = 1 Then
  ' If Porte.7 = 0 Then Activate_sd_bus
  ' Close #2
  ' Dateiaktuell = 0
  'End If
  'If Porte.7 = 1 Then call Activate_display_bus
  If Pind.0 = 0 Then
   Waitms 3
   If Pind.0 = 0 Then
    While Pind.0 = 0 : Wend
    If Antriebaus = 1 Then
     Yoffset = Null_y
     Xoffset = Null_x
     Writeeeprom Xoffset , 5
     Writeeeprom Yoffset , 9
    End If
   End If
  End If
  Singdummy = U / 100
  Text = Fusing(Singdummy , "#.#")
  Text = "   U= " + Text
  Text = Text + "V"
  Text = Trimtext(text)
  Lcd_print Text , 0 , 9 , 1 , 1 , 1 , Black , White

  Singdummy = I / 100
  Text = Fusing(Singdummy , "#.#")
  Text = "   I= " + Text
  Text = Text + "A"
  Text = Trimtext(text)
  Lcd_print Text , 0 , 18 , 1 , 1 , 1 , Black , White

  Singdummy = Kmh / 10
  Text = Fusing(Singdummy , "#.#")
  Text = "   V= " + Text
  Text = Text + "km/h"
  Text = Trimtext(text)
  Lcd_print Text , 0 , 27 , 1 , 1 , 1 , Black , White

  Singdummy = Meter / 1000
  Singdummy = Singdummy + Kilometer
  Text = Fusing(Singdummy , "#.###")
  Text = "   S= " + Text
  Text = Text + "km  "
  Text = Trimtext(text)
  Lcd_print Text , 0 , 36 , 1 , 1 , 1 , Black , White

  Singdummy = Ml
  Text = Str(ml )
  Text = "  Pl= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 45 , 1 , 1 , 1 , Black , White

  Singdummy = Mr
  Text = Str(mr )
  Text = "  Pr= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 54 , 1 , 1 , 1 , Black , White

  Singdummy = Akkuprozente / 10
  Singdummy = 100 - Singdummy
  Text = Fusing(Singdummy , "#.#")
  Text = "Akku= " + Text
  Text = Text + "%"
  Text = Trimtext(text)
  Lcd_print Text , 0 , 63 , 1 , 1 , 1 , Black , White

  Text = Str(mit_acc_x)
  Text = "Roll= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 72 , 1 , 1 , 1 , Black , White

  Text = Str(mit_acc_y )
  Text = "Nick= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 81 , 1 , 1 , 1 , Black , White

  Text = Str(null_x )
  Text = "  R0= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 90 , 1 , 1 , 1 , Black , White

  Text = Str(null_y )
  Text = "  N0= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 99 , 1 , 1 , 1 , Black , White

  Text = Fusing(stellwert , "#.###")
  Text = "  SW= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 108 , 1 , 1 , 1 , Black , White

  Text = Str(gyro_nick )
  Text = "  GN= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 117 , 1 , 1 , 1 , Black , White

  Text = Str(ta )
  Text = "  TA= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 126 , 1 , 1 , 1 , Black , White

  Text = Str(k_sum)
  Text = "Ksum= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 135 , 1 , 1 , 1 , Black , White
 Case 2       'Min/Max
  'If Dateiaktuell = 1 Then
  ' If Porte.7 = 0 Then Activate_sd_bus
  ' Close #2
  ' Dateiaktuell = 0
  'End If
  'If Porte.7 = 1 Then call Activate_display_bus

  If Pind.0 = 0 Then
   Waitms 3
   If Pind.0 = 0 Then
    While Pind.0 = 0 : Wend
    Minmax       'Min-Maxwerte auf 0 setzen
   End If
  End If
  Singdummy = Umin / 100
  Text = Fusing(Singdummy , "#.#")
  Text = "   Umin= " + Text
  Text = Text + "V"
  Text = Trimtext(text)
  Lcd_print Text , 0 , 9 , 1 , 1 , 1 , Black , White
  Singdummy = Umax / 100
  Text = Fusing(Singdummy , "#.#")
  Text = "   Umax= " + Text
  Text = Text + "V"
  Text = Trimtext(text)
  Lcd_print Text , 0 , 18 , 1 , 1 , 1 , Black , White
  '**************************************************
  Singdummy = Imin / 100
  Text = Fusing(Singdummy , "#.#")
  Text = "   Imin= " + Text
  Text = Text + "A"
  Text = Trimtext(text)
  Lcd_print Text , 0 , 27 , 1 , 1 , 1 , Black , White
  Singdummy = Imax / 100
  Text = Fusing(Singdummy , "#.#")
  Text = "   Imax= " + Text
  Text = Text + "A"
  Text = Trimtext(text)
  Lcd_print Text , 0 , 36 , 1 , 1 , 1 , Black , White
  '**************************************************
  Singdummy = Kmhmin / 10
  Text = Fusing(Singdummy , "#.#")
  Text = "   Vmin= " + Text
  Text = Text + "km/h"
  Text = Trimtext(text)
  Lcd_print Text , 0 , 45 , 1 , 1 , 1 , Black , White
  Singdummy = Kmhmax / 10
  Text = Fusing(Singdummy , "#.#")
  Text = "   Vmax= " + Text
  Text = Text + "km/h"
  Text = Trimtext(text)
  Lcd_print Text , 0 , 54 , 1 , 1 , 1 , Black , White
  '**************************************************
  Text = Str(mlmin )
  Text = "  Plmin= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 63 , 1 , 1 , 1 , Black , White
  Text = Str(mlmax )
  Text = "  Plmax= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 72 , 1 , 1 , 1 , Black , White
  '**************************************************
  Text = Str(mlmin )
  Text = "  Prmin= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 81 , 1 , 1 , 1 , Black , White
  Text = Str(mrmax )
  Text = "  Prmax= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 90 , 1 , 1 , 1 , Black , White
  '**************************************************
  Text = Str(tamax )
  Text = "  TAmax= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 99 , 1 , 1 , 1 , Black , White
  '**************************************************
  Text = Bin(hauptstportc)
  Text = "  HauptSt_C= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 108 , 1 , 1 , 1 , Black , White
  '**************************************************
  Text = Bin(hauptstporta)
  Text = "  HauptSt_A= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 118 , 1 , 1 , 1 , Black , White
  '**************************************************
  Text = Str(ad0)
  Text = "  AD(0)= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 127 , 1 , 1 , 1 , Black , White
  '**************************************************
  Text = Str(ad1)
  Text = "  AD(1)= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 136 , 1 , 1 , 1 , Black , White
  '**************************************************
  Dummybyte = Fussschalter And &H1
  If Dummybyte > 0 Then
   Text = "1"
  Else
   Text = "0"
  End If
  Text = "  Fuss1= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 145 , 1 , 1 , 1 , Black , White
  '**************************************************
  Dummybyte = Fussschalter And &H2
  If Dummybyte > 0 Then
   Text = "1"
  Else
   Text = "0"
  End If
  Text = "  Fuss2= " + Text
  Text = Trimtext(text)
  Lcd_print Text , 0 , 153 , 1 , 1 , 1 , Black , White
 Case 3       'Werte editieren
  'If Dateiaktuell = 1 Then
  ' If Porte.7 = 0 Then Activate_sd_bus
  ' Close #2
  ' Dateiaktuell = 0
  'End If
  'If Porte.7 = 1 Then call Activate_display_bus

  If Pind.7 = 0 Then       'Zeile abwärts
   Waitms 3
   If Pind.7 = 0 Then
    While Pind.7 = 0 : Wend
    If Editzeile > 1 Then Decr Editzeile : End If
  End If

  If Pind.4 = 0 Then       'Zeile aufwärts
   Waitms 3
   If Pind.4 = 0 Then
   While Pind.4 = 0 : Wend
   If Editzeile < 7 Then Incr Editzeile : End If
  End If

  If Ping.1 = 0 Then       'Zahl dekrementieren
   Waitms 3
   If Ping.1 = 0 Then
    While Ping.1 = 0 : Wend
    Select Case Editzeile
     Case 1
      Decr Pfaktor
     Case 2
      Decr Ifaktor
     Case 3
      Decr Dfaktor
     Case 4
      Decr Mpfaktor
     Case 5
      Decr Mvfaktor
     Case 6
      Decr Mplfaktor
     Case 7
      Decr Mvlfaktor
    End Select
   End If
  End If

  If Pind.5 = 0 Then       'Zahl inkrementieren
   Waitms 3
   If Pind.5 = 0 Then
    While Pind.5 = 0 : Wend
    Select Case Editzeile
     Case 1
      Incr Pfaktor
     Case 2
      Incr Ifaktor
     Case 3
      Incr Dfaktor
     Case 4
      Incr Mpfaktor
     Case 5
      Incr Mvfaktor
     Case 6
      Incr Mplfaktor
     Case 7
      Incr Mvlfaktor
    End Select
   End If
  End If

  If Ping.0 = 0 Then 'Werte aus Display übernehmen
   Waitms 3
   If Ping.0 = 0 Then
    While Ping.0 = 0 : Wend
    P_faktor = Pfaktor
    P_faktor = P_faktor / 10000

    I_faktor = Ifaktor
    I_faktor = I_faktor / 1000000

    D_faktor = Dfaktor
    D_faktor = D_faktor / 1000

    Mp_faktor = Mpfaktor
    Mp_faktor = Mp_faktor / 10000

    Mv_faktor = Mvfaktor
    Mv_faktor = Mv_faktor / 1000

    Mpl_faktor = Mplfaktor
    Mpl_faktor = Mpl_faktor / 10000

    Mvl_faktor = Mvlfaktor
    Mvl_faktor = Mvl_faktor / 1000

    Sichern = 1
    Call Lcd_cls
    Lcd_print Titel , 0 , 0 , 1 , 1 , 1 , Yellow , Blue
    Wait 2
   End If
  End If

  Text = "   Regelparameter:"
  Text = Trimtext(text)
  Lcd_print Text , 0 , 18 , 1 , 1 , 1 , Black , White
  '**************************************************
  Text = Str(pfaktor)
  Text = "   P = " + Text
  Text = Trimtext(text)
  If Editzeile = 1 Then
   Lcd_print Text , 0 , 36 , 1 , 1 , 1 , Black , Green
   '**************************************************
  Else
   Lcd_print Text , 0 , 36 , 1 , 1 , 1 , Black , White
   '**************************************************
  End If
  Text = Str(ifaktor)
  Text = "   I = " + Text
  Text = Trimtext(text)
  If Editzeile = 2 Then
   Lcd_print Text , 0 , 46 , 1 , 1 , 1 , Black , Green
   '**************************************************
  Else
   Lcd_print Text , 0 , 46 , 1 , 1 , 1 , Black , White
   '**************************************************
  End If
  Text = Str(dfaktor)
  Text = "   D = " + Text
  Text = Trimtext(text)
  If Editzeile = 3 Then
   Lcd_print Text , 0 , 56 , 1 , 1 , 1 , Black , Green
   '**************************************************
  Else
   Lcd_print Text , 0 , 56 , 1 , 1 , 1 , Black , White
   '**************************************************
  End If
  Text = Str(mpfaktor)
  Text = "  MP = " + Text
  Text = Trimtext(text)
  If Editzeile = 4 Then
   Lcd_print Text , 0 , 66 , 1 , 1 , 1 , Black , Green
   '**************************************************
  Else
   Lcd_print Text , 0 , 66 , 1 , 1 , 1 , Black , White
   '**************************************************
  End If
  Text = Str(mvfaktor)
  Text = "  MV = " + Text
  Text = Trimtext(text)
  If Editzeile = 5 Then
   Lcd_print Text , 0 , 76 , 1 , 1 , 1 , Black , Green
   '**************************************************
  Else
   Lcd_print Text , 0 , 76 , 1 , 1 , 1 , Black , White
   '**************************************************
  End If
  Text = Str(mplfaktor)
  Text = " MPL = " + Text
  Text = Trimtext(text)
  If Editzeile = 6 Then
   Lcd_print Text , 0 , 86 , 1 , 1 , 1 , Black , Green
   '**************************************************
  Else
   Lcd_print Text , 0 , 86 , 1 , 1 , 1 , Black , White
   '**************************************************
  End If
  Text = Str(mvlfaktor)
  Text = " MVL = " + Text
  Text = Trimtext(text)
  If Editzeile = 7 Then
   Lcd_print Text , 0 , 96 , 1 , 1 , 1 , Black , Green
   '**************************************************
  Else
   Lcd_print Text , 0 , 96 , 1 , 1 , 1 , Black , White
   '**************************************************
  End If
  If Transferok = 1 Then
   Lcd_print "   Transfer OK" , 0 , 106 , 1 , 1 , 1 , Black , White
   Transferok = 0
  End if
 Case 4       'siehe COM IN
  'Siehe Datenempfang
 Case 5       'Logging
  'If Porte.7 = 0 Then Activate_sd_bus
  'If Antriebaus = 0 Then       'Motoren laufen ->Werte Loggen
  ' If Dateiaktuell = 0 Then       'Keine Datei geöffnet
  '  Lname = 1
  '  Dateilaenge = 1       'Nächste Schleife einmal erzwingen
  '  While Dateilaenge > 0
  '   Dateiname = Str(lname)
  '   Dateiname = Dateiname + ".log"
  '   '0 voranstellen, damit sich die Dateien besser sortieren lassen.
  '   While Len(dateiname) < 12
  '    Dateiname = "0" + Dateiname
  '   Wend
  '   Dateilaenge = Filelen(dateiname)
  '   Incr Lname
  '  Wend
  '  If Porte.7 = 1 Then call Activate_display_bus
  '  Lcd_print Dateiname , 25 , 75 , 1 , 1 , 1 , Black , White
  '  If Porte.7 = 0 Then Activate_sd_bus
  '  Dateiaktuell = 1
  '  Loopcount = 0
  '  _sec = 0
  '  _min = 0
  '  _hour = 0
  '  Zeit = "00:00:00"
  '  Open Dateiname For Append As #2
  '  Logtext = "Zeit;Loopcount;U;I;Nick;GyroNick;Stellwert;Roll;Pl;Pr;Ksum;V"
  '  Print #2 , Logtext
  ' End If

  ' If Loopcount <> Aloopcount Then
  '  Ulog = U / 100
  '  Ilog = I / 100
  '  Kmhlog = Kmh / 10
  '  Logtext = Zeit
  '  Logtext = Logtext + ";"
  '  Logtext = Logtext + Str(loopcount)
  '  Logtext = Logtext + ";"
  '  Logtext = Logtext + Fusing(ulog , "#.#")
  '  Logtext = Logtext + ";"
  '  Logtext = Logtext + Fusing(ilog , "#.#")
  '  Logtext = Logtext + ";"
  '  Logtext = Logtext + Str(mit_acc_y)
  '  Logtext = Logtext + ";"
  '  Logtext = Logtext + Str(gyro_nick)
  '  Logtext = Logtext + ";"
  '  Logtext = Logtext + Fusing(stellwert , "#.###")
  '  Logtext = Logtext + ";"
  '  Logtext = Logtext + Str(mit_acc_x)
  '  Logtext = Logtext + ";"
  '  Logtext = Logtext + Str(ml)
  '  Logtext = Logtext + ";"
  '  Logtext = Logtext + Str(mr)
  '  Logtext = Logtext + ";"
  '  Logtext = Logtext + Fusing(k_sum , "#.##")
  '  Logtext = Logtext + ";"
  '  Logtext = Logtext + Fusing(kmhlog , "#.#")
  '  Print #2 , Logtext
  '  Aloopcount = Loopcount
  ' End If
  'Else
  ' Close #2
  ' Dateiaktuell = 0
  'End If
End Select
Loop

'***************************************************************************
Datenempfang:
 !PUSH R24;
 !IN R24, SREG;
 !PUSH R24;
 !LDS R24,{EP1}
 !STS {EP0},R24
 !LDS R24,{EP2}
 !STS {EP1},R24
 !LDS R24,{EP3}
 !STS {EP2},R24
 !LDS R24,{EP4}
 !STS {EP3},R24
 !LDS R24,{EP5}
 !STS {EP4},R24
 !IN  R24,UDR1
 !STs {EP5},R24
 !POP R24;
 !Out Sreg , R24;
 !POP R24;

'Debugging
If Modus = 4 Then
 Hexstr = Hex(ep0)
 Text = Hexstr + " "
 Hexstr = Hex(ep1)
 Text = Text + Hexstr + " "
 Hexstr = Hex(ep2)
 Text = Text + Hexstr + " "
 Hexstr = Hex(ep3)
 Text = Text + Hexstr + " "
 Hexstr = Hex(ep4)
 Text = Text + Hexstr + " "
 Hexstr = Hex(ep5)
 Text = Text + Hexstr + " "
 Lcd_print Text , 0 , 75 , 1 , 1 , 1 , Black , White
End If

'< Serielle Telegramme abarbeiten>
If Ep0 = 13 Then
 If Ep5 = 13 Then
  !lds R24,{EP1}
  !LDs R25,{EP2}
  !add R24,R25
  !LDs R25,{EP3}
  !add R24,R25
  !STS {Bdummy},R24

  If Bdummy = Ep4 Then       'Die Prüfsumme stimmt
   Select Case Index       'Telegrammnummer
    Case 1       'Spannung
     U = Iwert
    Case 2       'Strom
     I = Iwert
    Case 3       'km/h
     Kmh = Iwert
    Case 4       'Ta und Antriebaus
     Antriebaus = Ep2
     Ta = Ep3
    Case 5       'Akkuprozente
     Akkuprozente = Iwert
    Case 6       'Kilometer
     Kilometer = Wwert
    Case 7       'Meter
     Meter = Iwert
    Case 8       'Roll_l
     Maccx1 = Ep2
     Maccx2 = Ep3
    Case 9       'Roll_h
     Maccx3 = Ep2
     Maccx4 = Ep3
     Mit_acc_x = Maccx
    Case 10       'Nick_l
     Maccy1 = Ep2
     Maccy2 = Ep3
    Case 11       'Nick_h
     Maccy3 = Ep2
     Maccy4 = Ep3
     Mit_acc_y = Maccy
    Case 12       'Null_Roll_l
     Nullx1 = Ep2
     Nullx2 = Ep3
    Case 13       'Null_Roll_h
     Nullx3 = Ep2
     Nullx4 = Ep3
     Null_x = Nullx
    Case 14       'Null_Nick_l
     Nully1 = Ep2
     Nully2 = Ep3
    Case 15       'Null_Nick_h
     Nully3 = Ep2
     Nully4 = Ep3
     Null_y = Nully
    Case 16
     Sw1 = Ep2
     Sw2 = Ep3
    Case 17
     Sw3 = Ep2
     Sw4 = Ep3
     Stellwert = Sw
    Case 18
     Gyronick1 = Ep2
     Gyronick2 = Ep3
    Case 19
     Gyronick3 = Ep2
     Gyronick4 = Ep3
     Gyro_nick = Gyronick
    Case 20
     Kksum1 = Ep2
     Kksum2 = Ep3
    Case 21
     Kksum3 = Ep2
     Kksum4 = Ep3
     K_sum = Kksum
    Case 255
     If Sichern = 1 Then
       !lds R24,{P_f1}
       !LDs R25,{P_f2}
       !add R24,R25
       !LDs R25,{P_f3}
       !add R24,R25
       !LDs R25,{P_f4}
       !add R24,R25
       !LDs R25,{I_f1}
       !add R24,R25
       !LDs R25,{I_f2}
       !add R24,R25
       !LDs R25,{I_f3}
       !add R24,R25
       !LDs R25,{I_f4}
       !add R24,R25
       !LDs R25,{D_f1}
       !add R24,R25
       !LDs R25,{D_f2}
       !add R24,R25
       !LDs R25,{D_f3}
       !add R24,R25
       !LDs R25,{D_f4}
       !add R24,R25
       !LDs R25,{Mp_f1}
       !add R24,R25
       !LDs R25,{Mp_f2}
       !add R24,R25
       !LDs R25,{Mp_f3}
       !add R24,R25
       !LDs R25,{Mp_f4}
       !add R24,R25
       !LDs R25,{Mv_f1}
       !add R24,R25
       !LDs R25,{Mv_f2}
       !add R24,R25
       !LDs R25,{Mv_f3}
       !add R24,R25
       !LDs R25,{Mv_f4}
       !add R24,R25
       !LDs R25,{Mpl_f1}
       !add R24,R25
       !LDs R25,{Mpl_f2}
       !add R24,R25
       !LDs R25,{Mpl_f3}
       !add R24,R25
       !LDs R25,{Mpl_f4}
       !add R24,R25
       !LDs R25,{Mvl_f1}
       !add R24,R25
       !LDs R25,{Mvl_f2}
       !add R24,R25
       !LDs R25,{Mvl_f3}
       !add R24,R25
       !LDs R25,{Mvl_f4}
       !add R24,R25
       !STS {Bdummy},R24

      'Open "COM2:" For Binary As #1  'SD-Karte
      'Print #1 , Chr(p_f1) ; Chr(p_f2) ; Chr(p_f3) ; Chr(p_f4);
      'Print #1 , Chr(i_f1) ; Chr(i_f2) ; Chr(i_f3) ; Chr(i_f4);
      'Print #1 , Chr(d_f1) ; Chr(d_f2) ; Chr(d_f3) ; Chr(d_f4);
      'Print #1 , Chr(mp_f1) ; Chr(mp_f2) ; Chr(mp_f3) ; Chr(mp_f4);
      'Print #1 , Chr(mv_f1) ; Chr(mv_f2) ; Chr(mv_f3) ; Chr(mv_f4);
      'Print #1 , Chr(mpl_f1) ; Chr(mpl_f2) ; Chr(mpl_f3) ; Chr(mpl_f4);
      'Print #1 , Chr(mvl_f1) ; Chr(mvl_f2) ; Chr(mvl_f3) ; Chr(mvl_f4);
      'Print #1 , Chr(bdummy) ; Chr(13);
      'Close #1
      Transferok = 1
      Sichern = 0
     End If
    Case 76       'Linker Motor
     Ml = Ep3
     If Ep2 = 0 Then Ml = Ml * -1
     Incr Loopcount
    Case 82       'Rechter Motor
     Mr = Ep3
     If Ep2 = 0 Then Mr = Mr * -1
   End Select

   If Modus = 2 Then
    Select Case Index
     Case 36 'Port a und C Status
      Hauptstportc = Ep2
      Hauptstporta = Ep3
     Case 37  'Fussschalter 1
      Ad0 = Wwert
     Case 38  'Fussschalter 2
      Ad1 = Wwert
     Case 39 'Fussschalter gesamt
      Fussschalter = Ep2
    End Select
   End If

   If Modus < 3 Then
    Select Case Index
     Case 22
      P_f1 = Ep2
      P_f2 = Ep3
     Case 23
      P_f3 = Ep2
      P_f4 = Ep3
     Case 24
      I_f1 = Ep2
      I_f2 = Ep3
     Case 25
      I_f3 = Ep2
      I_f4 = Ep3
     Case 26
      D_f1 = Ep2
      D_f2 = Ep3
     Case 27
      D_f3 = Ep2
      D_f4 = Ep3
     Case 28
      Mp_f1 = Ep2
      Mp_f2 = Ep3
     Case 29
      Mp_f3 = Ep2
      Mp_f4 = Ep3
     Case 30
      Mv_f1 = Ep2
      Mv_f2 = Ep3
     Case 31
      Mv_f3 = Ep2
      Mv_f4 = Ep3
     Case 32
      Mpl_f1 = Ep2
      Mpl_f2 = Ep3
     Case 33
      Mpl_f3 = Ep2
      Mpl_f4 = Ep3
     Case 34
      Mvl_f1 = Ep2
      Mvl_f2 = Ep3
     Case 35
      Mvl_f3 = Ep2
      Mvl_f4 = Ep3
    End Select
   End If
  End If       'Prüfsumme stimmt
 End If
End If
'</Serielle Telegramme abarbeiten>
Return
'******************************************************************************

Sub Activate_display_bus:
 'remove SD Card from bus, must be "Reset PortF.7" for board D071x
 'Reset Porte.7
 'different to what the SD card needs
 'Config Spi = Hard , Interrupt = Off , Data Order = Msb , Master = Yes , Polarity = Low , Phase = 0 , Clockrate = 4 , Noss = 1
 'Set Spsr.spi2x       'hidden parameter: doubles the speed of the SPI output
 'Spiinit
End Sub

Sub Activate_sd_bus
 'Deactivate Display
 'Set Lcd_port.lcd_cs
 'different to what the display needs
 'Config Spi = Hard , Interrupt = Off , Data Order = Msb , Master = Yes , _
 '            Polarity = High , Phase = 1 , Clockrate = 4 , Noss = 1
 'no double speed. You may remove this line to try out with your card.
 'It may work.
 'Reset Spsr.spi2x
 'Spiinit
  'connects SD Card to bus, must to be "Set PortF.7" for board D071x
 'Set Porte.7
End Sub

Function Trimtext(t As String) As String
 If Len(t) > 22 Then T = Left(t , 22)
 While Len(t) < 22 : T = T + " " : Wend
End Function

Sub Minmax
 Umin = 10000
 Umax = -10000
 Imin = 0
 Imax = 0
 Kmhmin = 0
 Kmhmax = 0
 Mlmax = 0
 Mlmin = 0
 Mrmax = 0
 Mrmin = 0
 Tamax = 0
End Sub

Tick:
 Incr _sec
 If _sec > 59 Then
  _sec = 0
  Incr _min
 End If

 If _min > 59 Then
  _min = 0
  Incr _hour
 End If

 Zeit = Time$
Return

Getdatetime:
Return

Sub Drawpagestatics
 Call Lcd_print(titel , 0 , 0 , 1 , 1 , 1 , Yellow , Blue)
 Select Case Modus
  Case 0
   Lcd_rect 8 , 84 , 124 , 95 , 0 , Black       'Rahmen für Akku Balken
   Lcd_rect 24 , 125 , 124 , 135 , 0 , Black       'Rahmen für Horizontal Lenker
   Lcd_rect 8 , 100 , 19 , 160 , 0 , Black       'Rahmen für Vertikal Lenker
   Lcd_draw 20 , 130 , 22 , 130 , 0 , Black       'Vertikaler Balken, Mitteltick
   Lcd_draw 74 , 136 , 74 , 138 , 0 , Black       'Horizontaler Balken, Mitteltick
   Lcd_draw 0 , 167 , 132 , 167 , 0 , Black       'Hintergrund der Fußzeile
  Case 4
   Call Lcd_print( "COM-LOG" , 30 , 10 , 2 , 1 , 2 , Black , White)
   Text = " 0  1  2  3  4  5"
   Lcd_print Text , 0 , 60 , 1 , 1 , 1 , Black , White
  Case 5
   Call Lcd_print( "LOGGING" , 30 , 40 , 2 , 1 , 2 , Black , White)
   'Dateiaktuell = 0
 End Select
End Sub

End

$include "Y:\Programme\Tools\BASCOM-AVR\Display 3000\Displaymodul\Bascom Programme\für Include\GLCD21_display3000.bas"
$include "Y:\Programme\Tools\BASCOM-AVR\Display 3000\Displaymodul\Bascom Programme\für Include\GLCD21_fonts.bas"

Colortable:
 Data 0

Sicherheitshinweis

Achtung! Gehen Sie so ein Projekt nur an, wenn Sie sehr gute Kenntisse in Mechanik und Elektronik haben.

Funktion

Kippt das Fahrzeug nach vorn, wird nach vorn beschleunigt und umgekehrt. Die Regelung erledigt ein PID-Regler. Danach kommen jedoch noch einige zusätzliche Faktoren die z.B. die Motorkennlinie ausgleichen sowie ungewollte Richtungsänderungen bei unebenen Oberflächen reduzieren. Ähnlich aufwändig wird es bei den Motorreglern. Eine reine Blockkommutierung, wie bei solchen Motoren üblich, ist auf unebenen Untergründen suboptimal. Hier wurde zusätzliche Hallsensoren installiert.

Die seitliche Stabilität garantiert der Kreiseleffekt den jeder vom Fahrrad kennt. Das endgültige Fahrzeug wird eine Lenkung bekommen, die auf dem Beharrungsmoment basiert. Eine konventionelle Lenkung ist nicht möglich weil es nur ein Rad gibt.

Elektronik

Motorcontroller

Der Motorcontroller regelt den Motor, misst und übermittelt der Motorregler auch Spannung, Strom und Fahrstrecke. Die Temperatur des Kühlkörpers wird ebenfalls gemessen und wenn nötig ein Lüfter zugeschaltet. Die Einheit wird vom PC aus über eine ISP Schnittstelle programmiert.

Der Motorregler ist auf einer doppelseitigen Platine aufgebaut. Die Verbindung zwischen den beiden Platinen bildet ein 10-adriges Flachkabel. Zusätzlich zu den beiden Platinen gehört zum Motorregler auch die im Motor verbaute Sensorplatine. Auf dieser werden die Signale der drei zusätzlichen Hallsensoren zu einem Signal zusammengefasst.

Hauptcontroller

Die Hauptcontroller erledigt die Lageregelung. Sie steuert die Motoren und gibt die entsprechenden Meldungen an den Anzeigecontroller aus. Sie stellt auch die 12V für den Tacho bereit. Bei beiden Controllern wird ein Chopperspannungsregler eingesetzt der bis zu 63V verträgt. Deshalb ist eine Versorgung aus dem 24..48V Netz möglich. Die Lageregelung speichert auch die gefahrenen Km, einigen Min-Max Werte und diverse Werte bei einem Notaus. Diese Werte werden im EEprom als Text abgelegt. Beachten Sie, daß Sie das Gerät dann nur noch auf Privatgelände mit Zustimmung des Eigentümers benutzen dürfen. Eine Bertriebserlaubnis für das Gerät existiert nicht. Eine Einzelabnahme kann beim TÜV beantragt werden.

Schaltbild

Neigungssensoradapter

Im Unterschied zu ähnlichen Projekten besitzt dieses Fahrzeug einen über den I2C Bus gesteuerten Gyro und Accelerometer. Zum Beschleunigen und Bremsen werden die Sensoren gekippt.

Wie geht es weiter

Ich werde diese Seite updaten, je nach Fortschritt. Habe die nächsten paar Monate wenig Zeit....


LiFePO4 Speicher Test