Aus RN-Wissen.de
Version vom 8. Februar 2013, 11:22 Uhr von Listrik (Diskussion | Beiträge) (→Listrik01 - Ein selbstbalancierendes Elektroeinrad)
Dieser Artikel ist noch in Bearbeitung.
Inhaltsverzeichnis
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
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)
class="code">
'******************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)
class="code">
'( ******************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)
class="code">
'****************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
Hauptsteuerung
Motorsteuerung Blatt 1
Motorsteuerung Blatt 2
Hall D Signal
Aussenverdrahtung
Stückliste
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....