Aus RN-Wissen.de
Inhaltsverzeichnis
Rutscherle 2 - Ein selbstbalancierender Elektroroller
Beim Rutscherle 2 handelt es sich um einen selbstbalancierenden Elektroroller mit großer Ähnlichkeit zum bekannten Segway. Es war und ist beabsichtigt, besser zu sein als das Original. Auch wenn das bisher noch nicht ganz gelungen ist. Dies ist kein kommerzielles Projekt und soll es auch nicht werden. Noch eine Warnung: Das ist kein ungefährliches Projekt. Schon kleine Programmierfehler können sehr schmerzhafte folgen haben.
Und so fährts: Rutscherle2 auf Youtube
Technische Daten
Antrieb
2 BLDC Felgenmotoren mit je 1000W/36V. Vom Hersteller in China direkt importiert. Mittlerweile gibt es auch in Europa vergleichbare Motoren zu kaufen. Die stammen zwar auch aus China, müssen jedoch nicht selbst importiert werden. Die Motoren sind im Lieferzustand nicht direkt verwendbar. Die Sensorik muss um 3 weitere Hallsensoren erweitert werden. Die bisherige Höchstgeschwindigkeit beträgt 27km/h.
Akkus
4 Bleiakkus mit 12V/18AH von denen 3 in Reihe geschaltet sind für die Motoren, einer allein für die Steuerung und eine zukünftige Beleuchtung. Geladen werden die Akkus an einem handelsüblichen Kfz-Ladegerät. Die Reichweite beträgt auf jeden Fall mehr als 20km. Die Entscheidung für Bleiakkus wurde bewusst getroffen. Eine kWh verteuert sich um ca. 50 cent wenn sie durch den Bleiakku geht bei Litium-Polymer-Akkus sind das ca. 5 Euro. Auch sind Bleiakkus beim Laden wesentlich unempfindlicher.
Elektronik
2 Motorregler mit Atmega 168/10Mhz sowie eine Hauptsteuerung mit einem Atmega 644/20Mhz. Zusätzlich noch ein Display von Display3000 als Tacho und Datenlogger. Beschleunigungssensor: ADXL335, Gyros: LISY300AL (ArduIMU Sensor Board - Six Degrees of Freedom) Die ganze Elektronik wurde sehr einfach gehalten. (Zu mehr hats nicht gereicht) Das hat aber den Vorteil, daß keine besonderen Spezialkentnisse erforderlich sind. Wer schonmal eine Platine mit dem Bügeleisen hergestellt hat bekommt auch das hin.
Software
Die Software ist in Bascom geschrieben. Zeitkritische Teile in Assembler.
Mechanik
Der Mechanische Aufbau besteht nahezu komplett aus Aluminium. Dabei wurde darauf geachtet, dass alles mit einfachen Werkzeugen herstellbar ist. (Eine Ständerbohrmaschine sollte man schon haben.) Die Konstruktion ist so ausgelegt, dass so wenige Teile wie möglich hinter den Rädern hervorstehen. Dies verhindert Kratzer bei den ersten Fahrversuchen.
Sicherheitshinweis
Achtung! Bauen Sie sowas nur wenn Sie genau wissen was Sie machen. Es können an der Elektonik sehr hohe Ströme auftreten. Auch an den Motoren können, vor allem im Fehlerfall, sehr hohe Drehmomente entstehen. Beschweren Sie sich also nicht wenn Sie im Krankenhaus aufwachen. Sie haben es so gewollt.
Funktion
Die Funktion ist im groben recht banal. Im Prinzip macht das Teil das gleiche wie jeder Mensch beim gehen. Kommt ein Mensch nach vorn aus dem Gleichgewicht macht er einen Schritt nach vorn. Entsprechend nach hinten. Hier wird das gleiche elektronisch nachbildet. Bewegt sich die Platform nach vorn wird nach vorn beschleunigt. Die genaue technische Umsetzung wird dann doch etwas aufwändiger. Die Regelung selbst 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 aufwendig wird es bei den Motorreglern. Eine reine Blockkommutierung wie bei solchen Motoren üblich ist nur auf glatten Untergründen brauchbar. Darum müssen die Motoren um zusätzliche Hallsensoren erweitert und mit Drehstrom oder genauer Drehspannung angesteuert werden.
Elektronik
Motorregler
Der Motorregler regelt natürlich den Motor. Zusätzlich 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 Ansteuerung des Motors ist keine Blockkomutierung. Diese hat sich als untauglich gezeigt. Tatsächlich wird auf den 3 Leitungen eine vereinfachte Sinusspannung ausgegeben. Das reduziert die Drehmomentwelligkeit und das Rückspeisen der Energie beim Bremsen ist deutlich besser. Ob und wieweit sich dadurch der Motorwirkungsgrad ändert wurde bisher nicht gemessen. Die ganze Einheit wird vom PC aus über die Serielle Schnittstelle (5V) parametriert und kalibriert.
Der Motorregler ist auf zwei Platinen aufgebaut. Der Grund dafür ist die einfachere Herstellung der Platinen mit der Bügeleisenmethode sowie die räumliche Trennung der hohen Ströme vom Kontroller. Ob dies wegen der Störsicherheit wirklich notwendig ist wurde aus Kostengründen nie getestet. An der Stelle ist aber eine unnötige Sicherheit besser als eine vergessene. Die Verbindung zwischen den beiden Platinen bildet ein 16-Adriges Flachkabel. Programmierstecker und der Serielle Bus werden seitlich am Kontrollerboard gesteckt. Die Verbindung zu den Hallsensoren wird über ein 6-Adriges Flachkabel hergestellt. 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. Nach einem Ausfall wurde auch noch eine Zusatzplatine mit den Snubbern und den Supressordioden nachgerüstet. Ohne diese ist eine Halbbrücke nach ca. 125km Fahrt ausgefallen.
Schaltbilder
Motorregler (ohne Snubber)
Kontrollerboard
Hochstromteil
Snubber
Software Version 19
'Version 19
$prog &HFF , &HFF , &HD4 , &HF9 'Fusebits
$regfile = "m168def.dat"
$crystal = 10000000
$baud = 57600
$hwstack = 32
$swstack = 10
$framesize = 40
Declare Sub Setspeed
Declare Sub Pwm_start
Declare Sub Pwm_stop
Config Portc.0 = Input 'UBAT
Config Portc.1 = Input 'USHUNT
Config Portc.2 = Input 'TEMP
Config Portc.3 = Output 'LED1
Config Portc.4 = Output 'Lüfter
Config Portc.5 = Input 'Jumper1
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
Config Portb.0 = Input 'Hall_D
Config Portb.1 = Output 'B-
Config Portb.2 = Output 'B+
Config Portb.3 = Output 'A-
Config Portb.4 = Output 'LED2
Config Portb.5 = Input 'Unbelegt (SCK)
Hall_a Alias Pind.7 'gelb
Hall_b Alias Pind.4 'blau
Hall_c Alias Pind.2 'Grün
Hall_d Alias Pinb.0
Led1 Alias Portc.3
Led2 Alias Portb.4
Fan Alias Portc.4
Jumper1 Alias Pinc.5
Dim Uzu As Integer At $100
Dim Izu As Integer At $102
Dim Ausgabeticks As Integer At $104 'Anzahl Kommutieren zwischen Seriellen Telegrammen
Dim Temperatur As Integer At $106 '
Dim Drehzahl As Integer At $108 'Drehzahl
Dim M1 As Byte At $100 Overlay
Dim M2 As Byte At $101 Overlay
Dim M3 As Byte At $102 Overlay
Dim M4 As Byte At $103 Overlay
Dim M5 As Byte At $104 Overlay
Dim M6 As Byte At $105 Overlay
Dim M7 As Byte At $106 Overlay
Dim M8 As Byte At $107 Overlay
Dim M9 As Byte At $108 Overlay
Dim M10 As Byte At $109 Overlay
Dim Kalibrierwert As Integer At $10a
Dim Kal(2) As Byte At $10a Overlay
Dim A(15) As Byte At $10c
Dim S(12) As Byte At $11b
Dim Sinus(12) As Byte At $127
Dim Richtung(255) As Byte At $133
Dim Motorposition As Byte At $232
Dim Istrichtung As Integer At $233
Dim Altrichtung As Integer At $235
Dim Aposition As Byte At $237
Dim Zposition As Byte At $238
Dim Zwangskommutieren As Byte At $239
Dim Temp As Word At $23a
Dim Fancount As Word At $23c
Dim Motoradresse As Byte At $23e
Dim Vwinkel As Byte At $23f
Dim Rwinkel As Byte At $240
Dim Ticks As Integer At $241
Dim N As Byte At $243
Dim Notaus As Byte At $244
Dim Loopcount As Word At $245
Dim Sollrichtung As Byte At $247
Dim Altsollrichtung As Byte At $248
Dim Sollspeed As Byte At $249
Dim Altsollspeed As Byte At $24a
Dim Bdummy As Byte At $24b
Dim Intbdummy As Byte At $24c
Dim Intidummy As Integer At $24d
Dim Sdummy As Single At $24f
Dim Idummy1 As Integer At $253
Dim Idummy2 As Integer At $255
Dim Uw As Word At $257
Dim Iw As Word At $259
Dim Uwsum As Long At $25b
Dim Iwsum As Long At $25f
Dim Wcount As Long At $263
Dim Uoffset As Single At $267
Dim Ufaktor As Single At $26b
Dim Ioffset As Single At $26f
Dim Ifaktor As Single At $273
Dim Ep0 As Byte At $277
Dim Ep1 As Byte At $278
Dim Ep2 As Byte At $279
Dim Ep3 As Byte At $27a
Dim Ep4 As Byte At $27b
Dim Ep5 As Byte At $27c
Dim Ta As Byte At $27d
Dim Tb As Byte At $27e
Dim Tc As Byte At $27f
Dim Fehler As Byte At $280
Sollspeed = 0
Pwm_stop 'Erstmal alles auschalten
'Variablen für die Richtungserkennung vorbelegen
'Diese Variante frisst zwar viel Arbeitsspeicher, ist aber sehr schnell
For N = 1 To 15
A(n) = 0
Next
For N = 1 To 255
Richtung(n) = 2
Next
If Jumper1 = 0 Then 'R
Motoradresse = 82
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
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 'L
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
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
Motorposition.3 = Hall_d
If Motorposition = 0 Then Fehler = 1 'Stecker nicht gesteckt
If Motorposition > 0 Then
If A(motorposition) = 0 Then Fehler = 1 'Das währe eine Motorposition die es nicht gibt!
End If
If Fehler = 1 Then
Toggle Led1
Toggle Led2
Waitms 500
End If
Wend
'</Test Hallsensoren>
On Timer2 Update_pwm Nosave
'Pin-Change Interrupt einstellen:
Pcmsk2 = &B10010100
Pcmsk1 = &B00000000
Pcmsk0 = &B00000001
Pcicr = &B00000101
On Pcint2 Kommutieren Nosave
On Pcint0 Kommutieren Nosave
Readeeprom Uoffset , 1
Readeeprom Ufaktor , 5
Readeeprom Ioffset , 9
Readeeprom Ifaktor , 13
'Angepasste Sinuskurve aus dem EEprom laden
For N = 1 To 12
Bdummy = N + 16
Readeeprom Sinus(n) , Bdummy
Next
'Voreilwinkel:
Readeeprom Vwinkel , 29
Readeeprom Rwinkel , 30
If Ufaktor = 0 Then Ufaktor = 1
If Ifaktor = 0 Then Ifaktor = 1
On Urxc Datenempfang Nosave
Enable Urxc
Enable Interrupts
Fancount = 15000 'Lüfter erstmal einschalten
Do
Led1 = 0
'<Messen>
Uw = Getadc(0)
Uwsum = Uwsum + Uw
Iw = Getadc(1)
Iwsum = Iwsum + Iw
Incr Wcount
Temp = Getadc(2)
'</Messen>
'<Serielle Telegramme abarbeiten>
If Ep0 = 13 Then
If Ep5 = 13 Then
If Ep1 = Motoradresse Then '"R", "L"
Disable Urxc
$asm
lds R24,{EP1}
LDs R25,{EP2}
add R24,R25
LDs R25,{EP3}
add R24,R25
STS {Bdummy},R24
$end Asm
If Bdummy = Ep4 Then 'Die Prüfsumme stimmt
Led1 = 1
Pcicr = &B00000000 'Kommutieren kurz deaktivieren, weil sonst die Ticks während dem Zugriff geändert werden könnten
Ausgabeticks = Ticks
Ticks = 0
Pcicr = &B00000101
$asm
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 {Bdummy},R24
'Antwort ausgeben
'Print Chr(m1) ; Chr(m2) ; Chr(m3) ; Chr(m4) ; Chr(m5) ; Chr(m6) ; Chr(bdummy) ; Chr(13) ;
LdS R24,{M1}
STS udr,R24
Warten1:
LDS R24,UCSR0A
BST R24,5
Brtc warten1
LDS R24,{M2}
STS udr,R24
Warten2:
LDS R24,UCSR0A
BST R24,5
Brtc warten2
lds R24,{M3}
STS udr,R24
Warten3:
LDS R24,UCSR0A
BST R24,5
Brtc warten3
LDS R24,{M4}
STS udr,R24
Warten4:
LDS R24,UCSR0A
BST R24,5
Brtc warten4
LDS R24,{M5}
STS udr,R24
Warten5:
LDS R24,UCSR0A
BST R24,5
Brtc warten5
LDS R24,{M6}
STS udr,R24
Warten6:
LDS R24,UCSR0A
BST R24,5
Brtc warten6
LDS R24,{bdummy}
STS udr,R24
Warten7:
LDS R24,UCSR0A
BST R24,5
Brtc warten7
LDi R24,13
STS udr,R24
Warten8:
LDS R24,UCSR0A
BST R24,5
Brtc warten8
$end Asm
If Ep2 < 3 Then
Notaus = 0
Loopcount = 0
Sollrichtung = Ep2
Sollspeed = Ep3
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
Select Case Ep2
Case 3 'Sinus(1) empfangen
Sinus(1) = Ep3
Writeeeprom Sinus(1) , 17
Case 4 'Sinus(2) empfangen
Sinus(2) = Ep3
Writeeeprom Sinus(2) , 18
Case 5 'Sinus(3) empfangen
Sinus(3) = Ep3
Writeeeprom Sinus(3) , 19
Case 6 'Sinus(4) empfangen
Sinus(4) = Ep3
Writeeeprom Sinus(4) , 20
Case 7 'Sinus(5) empfangen
Sinus(5) = Ep3
Writeeeprom Sinus(5) , 21
Case 8 'Sinus(6) empfangen
Sinus(6) = Ep3
Writeeeprom Sinus(6) , 22
Case 9 'Sinus(7) empfangen
Sinus(7) = Ep3
Writeeeprom Sinus(7) , 23
Case 10 'Sinus(8) empfangen
Sinus(8) = Ep3
Writeeeprom Sinus(8) , 24
Case 11 'Sinus(9) empfangen
Sinus(9) = Ep3
Writeeeprom Sinus(9) , 25
Case 12 'Sinus(10) empfangen
Sinus(10) = Ep3
Writeeeprom Sinus(10) , 26
Case 13 'Sinus(11) empfangen
Sinus(11) = Ep3
Writeeeprom Sinus(11) , 27
Case 14 'Sinus(12) empfangen
Sinus(12) = Ep3
Writeeeprom Sinus(12) , 28
Case 15 'Spannung Kalibrieren (Byte 1)
Kal(1) = Ep3
Case 16 'Spannung Kalibrieren (Byte 2)
Kal(2) = Ep3
Sdummy = Uwsum / Wcount
Ufaktor = Kalibrierwert / Sdummy
Writeeeprom Ufaktor , 5
Uoffset = 0 'Schaltungsbedingt
Writeeeprom Uoffset , 1
Case 17 'Strom Kalibrieren (Byte 1)
Kal(1) = Ep3
Case 18 'Strom Kalibrieren Byte (2)
Kal(2) = Ep3
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 = Ep3
Writeeeprom Vwinkel , 29
Case 20
Rwinkel = Ep3
Writeeeprom Rwinkel , 30
End Select
End If
$asm 'Empfangspuffer löschen
LDI R24,0
sTS {EP1},R24
sTS {EP2},R24
sTS {EP3},R24
sTS {EP4},R24
$end Asm
End If
Enable Urxc
End If
End If
End If
'</Serielle Telegramme abarbeiten>
'<Lüfter schalten>
If Temp > 740 Then Fancount = 15000 'Schaltet den Lüfter ein
If Fancount > 0 Then
Fan = 1
Else
Fan = 0
End If
If Fancount > 0 Then Decr Fancount
'</Lüfter schalten>
'Notaus mit Freilauf wenn keine Nachricht von Seriell
If Loopcount < 50 Then 'Das sind schätzungsweise 0.1s
Led2 = 1
Notaus = 0
Incr Loopcount
Else
Led2 = 0
Notaus = 1
Sollspeed = 0
Setspeed
End If
Loop
'********************************************************************************************
Kommutieren:
$asm
PUSH R10
PUSH R11
PUSH R16
PUSH R17
PUSH R20
PUSH R21
PUSH R24
PUSH R26
PUSH R27
IN R24,sreg
PUSH R24
clr R16
lds R17, $0023
BST R17,0
BLD R16,3
LDS R17, $0029
BST R17,2
bld R16,2
BST R17,4
bld R16,1
BST R17,7
bld R16,0
lds R17,{aposition}
!swap R17
andi R17,&B11110000
add R17,R16
STS {Aposition},R17
CLR R17
LDI R26,$0B
LDI R27,$01
ADD R26,R16
ADC R27,R17
LD R24,X
LDS R16,{Sollrichtung}
CPI R16,0
BREQ Sollrichtungnull
Jmp Sollrichtungnichtnull
Sollrichtungnull:
LDS R20,{Vwinkel}
Add R24,R20
JMP Richtunggesetzt
Sollrichtungnichtnull:
LDS R20,{Rwinkel}
Add R24,R20
Richtunggesetzt:
CPI R24,$0D
BRCC Istgroesser13_1
JMP Istkleiner13_1
Istgroesser13_1:
SUBI R24,$0C
Istkleiner13_1:
CLR R20
Ldi R26,$1a
LDI R27,$01
ADD R26,R24
ADC R27,R20
LD R16,X
STS {Ta},R16
subi R24,$fc
CPI R24,$0D
BRCC Istgroesser13_2
JMP Istkleiner13_2
Istgroesser13_2:
SUBI R24,$0C
Istkleiner13_2:
CLR R20
Ldi R26,$1a
LDI R27,$01
ADD R26,R24
ADC R27,R20
LD R16,X
STS {Tc},R16
subi R24,$fc
CPI R24,$0D
BRCC Istgroesser13_3
JMP Istkleiner13_3
Istgroesser13_3:
SUBI R24,$0C
Istkleiner13_3:
CLR R20
Ldi R26,$1a
LDI R27,$01
ADD R26,R24
ADC R27,R20
LD R16,X
STS {Tb},R16
lds R10,{Aposition}
CLR R11
LDI R26,$32
LDI R27,$01
ADD R26,R10
ADC R27,R11
LD R16,X+
CLR R17
LDI R20,$02
LDI R21,$00
!Sub R16 , R20
SBC R17,R21
LDI R26,$33
LDI R27,$02
ST X+,R16
ST X,R17
LDI R26,$41
LDI R27,$02
LD R16,X+
LD R17,X
LDI R26,$33
LDI R27,$02
LD R20,X+
LD R21,X
ADD R16,R20
ADC R17,R21
LDI R26,$41
LDI R27,$02
ST X+,R16
ST X,R17
LDS R24,timsk2
ORI R24,$01
STS timsk2,R24
POP R24
!Out sreg , R24
POP R27
POP R26
POP R24
POP R21
POP R20
POP R17
POP R16
POP R11
POP R10
$end Asm
Return
'********************************************************************************************
'********************************************************************************************
Update_pwm:
$asm
PUSH R24
IN R24,sreg
PUSH R24
lds R24,{Ta}
STS ocr2a,R24
STS ocr2b,R24
LDS R24,{Tb}
STS ocr1al,R24
STS ocr1bl,R24
lds R24,{Tc}
!OUT ocr0a,R24
!OUT ocr0b,R24
LDS R24,timsk2
ANDI R24,$FE
STS timsk2,R24
POP R24
!OUT sreg,R24
POP R24
$end Asm
Return
'********************************************************************************************
'********************************************************************************************
Datenempfang:
$asm
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,UDR
STs {EP5},R24
POP R24;
Out Sreg , R24;
POP R24;
$end Asm
Return
'********************************************************************************************
'********************************************************************************************
Sub Setspeed
If Notaus = 1 Then
Pwm_stop
Sollspeed = 0
For N = 1 To 12
S(n) = 0
Next
Else
For N = 1 To 12
Idummy1 = Sollspeed * Sinus(n)
Idummy1 = Idummy1 / 100
S(n) = Idummy1
Next
If Tccr0a = 0 Then 'Wenn die PWM aus ist
Pwm_start
End If
'<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
$asm
clr R16
lds R17, $0023
BST R17,0
BLD R16,3
LDS R17, $0029
BST R17,2
bld R16,2
BST R17,4
bld R16,1
BST R17,7
bld R16,0
lds R17,{aposition}
!swap R17
andi R17,&B11110000
add R17,R16
STS {Aposition},R17
CLR R17
LDI R26,$0B
LDI R27,$01
ADD R26,R16
ADC R27,R17
LD R24,X
LDS R16,{Sollrichtung}
CPI R16,0
BREQ Sollrichtungnullz
Jmp Sollrichtungnichtnullz
Sollrichtungnullz:
LDS R20,{Vwinkel}
Add R24,R20
JMP Richtunggesetztz
Sollrichtungnichtnullz:
LDS R20,{Rwinkel}
Add R24,R20
Richtunggesetztz:
CPI R24,$0D
BRCC Istgroesser13_1z
JMP Istkleiner13_1z
Istgroesser13_1z:
SUBI R24,$0C
Istkleiner13_1z:
CLR R20
Ldi R26,$1a
LDI R27,$01
ADD R26,R24
ADC R27,R20
LD R16,X
STS {Ta},R16
subi R24,$fc
CPI R24,$0D
BRCC Istgroesser13_2z
JMP Istkleiner13_2z
Istgroesser13_2z:
SUBI R24,$0C
Istkleiner13_2z:
CLR R20
Ldi R26,$1a
LDI R27,$01
ADD R26,R24
ADC R27,R20
LD R16,X
STS {Tc},R16
subi R24,$fc
CPI R24,$0D
BRCC Istgroesser13_3z
JMP Istkleiner13_3z
Istgroesser13_3z:
SUBI R24,$0C
Istkleiner13_3z:
CLR R20
Ldi R26,$1a
LDI R27,$01
ADD R26,R24
ADC R27,R20
LD R16,X
STS {Tb},R16
$end Asm
Pcicr = &B00000101 'Den Eigentlichen Kommutierinterrupt einschalten
'Nur Overflow Interrupt (Seite 105)
Timsk2.toie2 = 1
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
Tccr0b = 1
Ocr0a = 0
Ocr0b = 0
Tccr1a = 161
Tccr1b = 1
Ocr1ah = 0
Ocr1al = 0
Ocr1bh = 0
Ocr1bl = 0
Tccr2a = 161
Tccr2b = 1
Ocr2a = 0
Ocr2b = 0
Tcnt0 = 0 'Timer Startwert, wegen Synchron
Tcnt1 = 6 'Timer Startwert, wegen Synchron
Tcnt2 = 9 'Timer Startwert, wegen Synchron
Enable Interrupts
End Sub
'********************************************************************************************
'********************************************************************************************
Sub Pwm_stop
Tccr0a = 0
Tccr0b = 0
Ocr0a = 0
Ocr0b = 0
Tccr1a = 0
Tccr1b = 0
Ocr1ah = 0
Ocr1al = 0
Ocr1bh = 0
Ocr1bl = 0
Tccr2a = 0
Tccr2b = 0
Ocr2a = 0
Ocr2b = 0
'alles 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
Die Hauptsteuerung erledigt die gesamte Lageregelung. Sie steuert die Motoren und gibt die entsprechenden Meldungen an den Tacho/Datenlogger aus. Sie stellt auch die 5V für den Tacho bereit. Darum die Möglichkeit einen größeren Spannungsregler zu bestücken. 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, also direkt lesbar. Beim Einschalten ist die Gewschwindigkeit auf 6km/h begrenzt. Diese Begrenzung kann jedoch aufgehoben werden. 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.
Schaltbild
Schaltbild Hauptsteuerung
Sensoradapter
Zur Hauptsteuerung gehört auch der Sensoradapter. (Schaltbild siehe Hauptsteuerung) Dieser stellt die 3,3V für die Sensoren bereit. Im Unterschied zu ähnlichen Projekten besitzt das Rutscherle2 kein Potentiometer für die Lenkung. Zum Lenken werden die gesamten Sensoren gekippt. Darum auch dieser Sensoradapter. Die Sensoren werden mit dem Adapter direkt mit der Achse der Lenkung verbunden. Die ideale Position ist dabei natürlich genau im Schnittpunkt von Lenk- und Motorachse.
Software Version 66
$prog &HFF , &HFF , &HD1 , &HFF 'Fusebits für Atmega644
$regfile = "m644def.dat"
$framesize = 64
$swstack = 64
$hwstack = 64
$crystal = 20000000
$baud = 57600
Declare Sub Onbutton1
Declare Sub Logtext(byval Adresse As Word , Text As String )
Declare Sub Akkugrenzen
Declare Sub Unfalldatenschreiber
Declare Sub Betriebsdatenschreiber
Config Portb.0 = Output
Config Portb.1 = Output
Config Portb.2 = Output
Config Portb.3 = Output
Config Portb.4 = Output
Config Portb.6 = Input 'Fusstaster
Config Portc.0 = Input
Config Portc.1 = Input
Config Portc.2 = Input
Config Portc.3 = Input
Config Portc.4 = Input
Config Portc.5 = Input
Config Portc.6 = Input
Config Portc.7 = Input
Config Portd.7 = Output
Config Portd.6 = Output
Config Portd.5 = Output
Config Portd.4 = Output
Config Portd.3 = Output
Config Portd.2 = Input
Led1 Alias Portb.4
Led2 Alias Portb.3
Sensor_aus Alias Portb.2
Sensor_test Alias Portb.1 'Self-Test. 0=Normalbetrieb
Led3 Alias Portd.4
Led4 Alias Portd.5
Led5 Alias Portd.6
Led6 Alias Portd.3
Led7 Alias Portd.7
Button1 Alias Pind.2
Fusstaster Alias Pinb.6
'******************************************************************************************************************************
'******************************************************************************************************************************
'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 Nick 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 Vmax = 60 'Vmax Vorwärts in km/h*10
Const Vmaxr = -60 'Vmax Rückwärts in km/h*10
'Konstanten für die Akkus
Const Akkuleer = 3600 '36V, 12V pro Akku -> Akkus leer
Const Akkufastleer = 3750
'Konstanten für die Signalaufbereitoung
Const Gyro_anz = 10000
Const Acc_anz = 50
Const Gyro_faktor = 91
'******************************************************************************************************************************
'******************************************************************************************************************************
'Variablen für die Rückmeldungen von den Motorreglern
Dim U As Integer At $100
Dim I As Integer At $102
Dim K As Integer At $104
Dim Checksumme As Byte At $106
Dim Ende As Byte At $107
Dim Motormeldung(8) As Byte At $100 Overlay
Dim M1 As Byte At $100 Overlay
Dim M2 As Byte At $101 Overlay
Dim M3 As Byte At $102 Overlay
Dim M4 As Byte At $103 Overlay
Dim M5 As Byte At $104 Overlay
Dim M6 As Byte At $105 Overlay
Dim M7 As Byte At $106 Overlay
Dim M8 As Byte At $107 Overlay
'Variablen für die Telegramme an die Motorregler
Dim Checksum_rechts As Byte At $108
Dim Checksum_links As Byte At $109
Dim Richtung_rechts As Byte At $10a
Dim Richtung_links As Byte At $10b
Dim Pwm_rechts As Byte At $10c
Dim Pwm_links As Byte At $10d
Dim Zeiger_meldung As Byte At $10e
Dim Ta As Byte At $10f
Dim U_links As Integer At $111
Dim U_rechts As Integer At $113
Dim I_links As Integer At $115
Dim I_rechts As Integer At $117
Dim K_links As Integer At $119
Dim K_rechts As Integer At $11b
Dim B(5) As Byte At $11d
Dim Kilometer As Word At $122
Dim Kilometermeldung(2) As Byte At $122 Overlay
Dim Akkustand As Integer At $124
Dim Akkumeldung(2) As Byte At $124 Overlay
Dim Kmh As Integer At $126
Dim Kmhmeldung(2) As Byte At $126 Overlay
Dim Meter As Integer At $128
Dim Metermeldung(2) As Byte At $128 Overlay
Dim Di As Single At $12a 'Distanz pro Kommutieren
Dim Kmhfaktor As Single At $12e
Dim Zentimeter As Single At $132
Dim Strecke As Single At $136 'Gefahrene Strecke während 1/100s
Dim Kx As Single At $13a 'Anzahl Kommutierungen für die Streckenberechnung
Dim Kv As Single At $13e 'Anzahl Kommutierungen für die Geschwindigkeitosberechnung
Dim Vbrems As Long At $142 'Bremse bei Überlast oder zu hoher Geschwindigkeit
Dim Lbrems As Long At $146
Dim Brems As Long At $14a
Dim P_faktor As Single At $14e
Dim Pf1 As Byte At $14e Overlay
Dim Pf2 As Byte At $14f Overlay
Dim Pf3 As Byte At $150 Overlay
Dim Pf4 As Byte At $151 Overlay
Dim I_faktor As Single At $152
Dim If1 As Byte At $152 Overlay
Dim If2 As Byte At $153 Overlay
Dim If3 As Byte At $154 Overlay
Dim If4 As Byte At $155 Overlay
Dim D_faktor As Single At $156
Dim Df1 As Byte At $156 Overlay
Dim Df2 As Byte At $157 Overlay
Dim Df3 As Byte At $158 Overlay
Dim Df4 As Byte At $159 Overlay
Dim Gyro_nick As Word At $015a
Dim Gyro_roll As Word At $015c
Dim Gyro_roll_alt As Word At $015e
Dim Gyro_nick_alt As Word At $0160
Dim acc_nick As Long At $0162
Dim Acc_roll As Long At $0166
Dim Sum_acc_roll As Long At $016a
Dim Sum_acc_nick As Long At $016e
Dim Sum_gyro_roll As Long At $0172
Dim Sum_gyro_nick As Long At $0176
Dim Korr_acc_roll As Long At $017a
Dim Korr_acc_nick As Long At $017e
Dim Maccroll As Long At $0182
Dim Maccroll1 As Byte At $0182 Overlay
Dim Maccroll2 As Byte At $0183 Overlay
Dim Maccroll3 As Byte At $0184 Overlay
Dim Maccroll4 As Byte At $0185 Overlay
Dim Maccnick As Long At $0186
Dim maccnick1 As Byte At $0186 Overlay
Dim maccnick2 As Byte At $0187 Overlay
Dim maccnick3 As Byte At $0188 Overlay
Dim maccnick4 As Byte At $0189 Overlay
Dim Mit_acc_nick_s As Long At $018a
Dim Mit_gyro_roll As Long At $018e
Dim Mit_gyro_nick As Long At $0192
Dim Sig_gyro_roll As Long At $0196
Dim Sig_gyro_nick As Long At $019a
Dim Nullroll As Long At $019e
Dim Nullroll1 As Byte At $019e Overlay
Dim Nullroll2 As Byte At $019f Overlay
Dim Nullroll3 As Byte At $01a0 Overlay
Dim Nullroll4 As Byte At $01a1 Overlay
Dim Nullnick As Long At $01a2
Dim nullnick1 As Byte At $01a2 Overlay
Dim nullnick2 As Byte At $01a3 Overlay
Dim nullnick3 As Byte At $01a4 Overlay
Dim nullnick4 As Byte At $01a5 Overlay
Dim Stellwert As Single At $01a6
Dim Mrg As Single At $01aa
Dim Mrg1 As Byte At $01aa Overlay
Dim Mrg2 As Byte At $01ab Overlay
Dim Mrg3 As Byte At $01ac Overlay
Dim Mrg4 As Byte At $01ad Overlay
Dim Mra As Single At $01ae
Dim Mra1 As Byte At $01ae Overlay
Dim Mra2 As Byte At $01af Overlay
Dim Mra3 As Byte At $01b0 Overlay
Dim Mra4 As Byte At $01b1 Overlay
Dim Kksum As Single At $01b2
Dim Kksum1 As Byte At $01b2 Overlay
Dim Kksum2 As Byte At $01b3 Overlay
Dim Kksum3 As Byte At $01b4 Overlay
Dim Kksum4 As Byte At $01b5 Overlay
Dim Sw As Single At $01b6
Dim Sw1 As Byte At $01b6 Overlay
Dim Sw2 As Byte At $01b7 Overlay
Dim Sw3 As Byte At $01b8 Overlay
Dim Sw4 As Byte At $01b9 Overlay
Dim Gyronick As Long At $01ba
Dim Gyronick1 As Byte At $01ba Overlay
Dim Gyronick2 As Byte At $01bb Overlay
Dim Gyronick3 As Byte At $01bc Overlay
Dim Gyronick4 As Byte At $01bd Overlay
Dim Mp_faktor As Single At $01be
Dim Mpf1 As Byte At $01be Overlay
Dim Mpf2 As Byte At $01bf Overlay
Dim Mpf3 As Byte At $01c0 Overlay
Dim Mpf4 As Byte At $01c1 Overlay
Dim Mv_faktor As Single At $01c2
Dim Mvf1 As Byte At $01c2 Overlay
Dim Mvf2 As Byte At $01c3 Overlay
Dim Mvf3 As Byte At $01c4 Overlay
Dim Mvf4 As Byte At $01c5 Overlay
Dim Mpl_faktor As Single At $01c6
Dim Mplf1 As Byte At $01c6 Overlay
Dim Mplf2 As Byte At $01c7 Overlay
Dim Mplf3 As Byte At $01c8 Overlay
Dim Mplf4 As Byte At $01c9 Overlay
Dim Mvl_faktor As Single At $01ca
Dim Mvlf1 As Byte At $01ca Overlay
Dim Mvlf2 As Byte At $01cb Overlay
Dim Mvlf3 As Byte At $01cc Overlay
Dim Mvlf4 As Byte At $01cd Overlay
Dim P_tacho As Single At $01ce
Dim I_tacho As Single At $01d2
Dim D_tacho As Single At $01d6
Dim Mp_tacho As Single At $01da
Dim Mv_tacho As Single At $01de
Dim Mpl_tacho As Single At $01e2
Dim Mvl_tacho As Single At $01e6
Dim Tdummy As Integer At $01ea
Dim Tachomeldung(30) As Byte At $01ce Overlay
Dim T1 As Byte At $01ce Overlay
Dim T2 As Byte At $01cf Overlay
Dim T3 As Byte At $01d0 Overlay
Dim T4 As Byte At $01d1 Overlay
Dim T5 As Byte At $01d2 Overlay
Dim T6 As Byte At $01d3 Overlay
Dim T7 As Byte At $01d4 Overlay
Dim T8 As Byte At $01d5 Overlay
Dim T9 As Byte At $01d6 Overlay
Dim T10 As Byte At $01d7 Overlay
Dim T11 As Byte At $01d8 Overlay
Dim T12 As Byte At $01d9 Overlay
Dim T13 As Byte At $01da Overlay
Dim T14 As Byte At $01db Overlay
Dim T15 As Byte At $01dc Overlay
Dim T16 As Byte At $01dd Overlay
Dim T17 As Byte At $01de Overlay
Dim T18 As Byte At $01df Overlay
Dim T19 As Byte At $01e0 Overlay
Dim T20 As Byte At $01e1 Overlay
Dim T21 As Byte At $01e2 Overlay
Dim T22 As Byte At $01e3 Overlay
Dim T23 As Byte At $01e4 Overlay
Dim T24 As Byte At $01e5 Overlay
Dim T25 As Byte At $01e6 Overlay
Dim T26 As Byte At $01e7 Overlay
Dim T27 As Byte At $01e8 Overlay
Dim T28 As Byte At $01e9 Overlay
Dim T29 As Byte At $01ea Overlay
Dim T30 As Byte At $01eb Overlay
Dim Werteneu As Byte
Dim Mit_acc_roll As Long
Dim Mit_acc_nick As Long
Dim Null_roll As Long
Dim Null_nick As Long
Dim Mit_regelzeit_ges As Single
Dim Lagefehler As Long
Dim Regelzeit As Word 'Die Zeit in 1/100s die gebraucht wird bis die Nullage erreicht ist
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 Ugemessen As Long 'Batteriespannung in V*100
Dim Igemessen As Long 'Gesamter Motorstrom in mA
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
Dim Akku1 As Long
Dim Akku2 As Long
Dim Akku3 As Long
Dim Akku4 As Long
Dim Akku5 As Long
Dim Erzeugt As Long
Dim Verbraucht As Long
'Single Variablen um die Prozente rechnen zu können
Dim Akkuinhalt As Single
Dim Akkuverbrauch As Single
Dim Akkuprozent As Single
Dim Adresse As Word
Dim Bzeit As Long
Dim Bstate As Byte
Dim Frei As Byte
Dim Timeout As Byte
Dim S_err As Byte
Dim Seriell_err_l As Word
Dim Seriell_err_r As Word
Dim Tamax As Byte
Dim Fusscount As Word
Dim Blinkcount As Byte
Dim Lenkerstellung As Single
Dim Lenkstellwert As Single
Dim Lenkfehler As Single
Dim N As Integer
Dim Utext As String * 32 'Texte für den Unfalldatenschreiber (als Text im eeprom)
Dim Ugrund As String * 32
Dim Text As String * 16
Dim Min_v As Integer
Dim Max_v As Integer
Dim Tachocount As Byte
Led3 = 1
Led4 = 1
Led5 = 1
Led6 = 1
Led7 = 1
Wait 5 '5 Sekunden Warten
Antriebaus = 1
'Ein paar feste Werte vorab berechnen:
Di = Radumpfang / Kom_pro_u
Kmhfaktor = Di * 9 '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
Portb.7 = 0
Portb.5 = 0
'Input-Pins hochziehen
Portc.0 = 1
Portc.1 = 1
Portc.2 = 1
Portc.3 = 1
Portc.4 = 1
Portc.5 = 1
Portc.6 = 1
'Sensorboard schalten:
Sensor_aus = 0
Sensor_test = 0
'Hochziehwiderstände der Knöpfe einschalten
Button1 = 1
Fusstaster = 1
Config Adc = Single , Prescaler = Auto , Reference = Internal_2.56
Start Adc
'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>
Readeeprom P_faktor , $380
Readeeprom I_faktor , $384
Readeeprom D_faktor , $388
Readeeprom Mp_faktor , $38c
Readeeprom Mv_faktor , $390
Readeeprom Mpl_faktor , $394
Readeeprom Mvl_faktor , $398
If P_faktor < 0 Or P_faktor > 1 Then P_faktor = 0.0033 'Halbwegs sinnvolle Werte vorbelegen
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 100mAh als Vorgabe
If Akkukap <= 0 Then Akkukap = 100
If Akkukap > 32000 Then Akkukap = 100
Akkugrenzen
Antriebaus = 1 'Beim Start ist immer alles aus
Notaus = 0
Readeeprom Max_v , $2f0
Readeeprom Min_v , $2f4
Config Watchdog = 128
'<Sicherheitstest 1>
Start Watchdog
If Button1 = 0 Then Wait 10
If Antriebaus = 0 Then Wait 10
If P_faktor > 1 Then Wait 10
If I_faktor > 1 Then Wait 10
If D_faktor > 1 Then Wait 10
If P_faktor < 0 Then Wait 10
If I_faktor < 0 Then Wait 10
If D_faktor < 0 Then Wait 10
If Fusstaster = 0 Then Wait 10
Stop Watchdog
'</Sicherheitstest 1>
'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_l = 0
Seriell_err_r = 0
Ugrund = "----------------" 'Einfach mal vorbelegen um zu sehen obs auch einen Notaus ohne Grund gibt.
On Timer0 Regel_interrupt
Config Timer0 = Timer , Prescale = 1024
Enable Timer0
Enable Interrupts
Waitms 100
Toggle Led3
Toggle Led4
Toggle Led5
Toggle Led6
Toggle Led7
Waitms 100
Toggle Led3
Toggle Led4
Toggle Led5
Toggle Led6
Toggle Led7
Waitms 100
Toggle Led3
Toggle Led4
Toggle Led5
Toggle Led6
Toggle Led7
Waitms 100
Toggle Led3
Toggle Led4
Toggle Led5
Toggle Led6
Toggle Led7
'<Sicherheitstest 2>
Start Watchdog
If Button1 = 0 Then Wait 10
If Antriebaus = 0 Then Wait 10
If P_faktor > 1 Then Wait 10
If I_faktor > 1 Then Wait 10
If D_faktor > 1 Then Wait 10
If P_faktor < 0 Then Wait 10
If I_faktor < 0 Then Wait 10
If D_faktor < 0 Then Wait 10
If Ugemessen < 1000 Then Wait 10
If Ugemessen > 4700 Then Wait 10
If Fusstaster = 0 Then Wait 10
Stop Watchdog
'</Sicherheitstest 2>
If Ugemessen < 2400 Then 'Sensoren ausschalten wenn am Ladegerät oder zu Testzwecken
Sensor_aus = 1
Else
Sensor_aus = 0
End If
While Ugemessen < 2400 'Unterspannung oder am Ladegerät!
Led3 = 1
Led4 = 1
Led5 = 1
Led6 = 0
Led7 = 1
Wend
'Prüfung ob Akku gerade geladen:
If Ugemessen > 3950 Then 'Der Akku wurde gerade geladen
Disable Interrupts 'Interruts abschalten damit die folgenden Zeilen nicht gerade jetzt vom Interrupt unterbrochen werden.
Waitms 50
Summe_as_entladen = 0
Summe_mah_entladen = 0
Summe_as_laden = 0
Summe_mah_laden = 0
Enable Interrupts
End If
Summen_vorbelegen:
acc_roll = Getadc(5) 'Roll
acc_nick = Getadc(7) 'Nick
Gyro_roll = Getadc(2) 'Roll
Gyro_nick = Getadc(4) 'Nick
acc_roll = acc_roll * 100
acc_nick = acc_nick * 100
Sum_gyro_roll = Gyro_roll * Gyro_anz
Sum_gyro_nick = Gyro_nick * Gyro_anz
Gyro_roll_alt = Gyro_roll
Gyro_nick_alt = Gyro_nick
Mit_gyro_nick = Gyro_roll
Mit_gyro_nick = Gyro_nick
Sum_acc_roll = acc_roll * Acc_anz
Sum_acc_nick = acc_nick * Acc_anz
Waitms 100
Toggle Led3
Toggle Led4
Toggle Led5
Toggle Led6
Toggle Led7
Waitms 100
Toggle Led3
Toggle Led4
Toggle Led5
Toggle Led6
Toggle Led7
If Sig_gyro_nick < -3 Then Goto Summen_vorbelegen
If Sig_gyro_nick > 3 Then Goto Summen_vorbelegen
If Sig_gyro_roll < -3 Then Goto Summen_vorbelegen
If Sig_gyro_roll > 3 Then Goto Summen_vorbelegen
'<Sicherheitstest 3>
Start Watchdog
If Button1 = 0 Then Wait 10
If Antriebaus = 0 Then Wait 10
If P_faktor > 1 Then Wait 10
If I_faktor > 1 Then Wait 10
If D_faktor > 1 Then Wait 10
If P_faktor < 0 Then Wait 10
If I_faktor < 0 Then Wait 10
If D_faktor < 0 Then Wait 10
If Ugemessen < 1000 Then Wait 10
If Ugemessen > 4700 Then Wait 10
If Fusstaster = 0 Then Wait 10
Stop Watchdog
'</Sicherheitstest 3>
'********************************************************************************************************************************************************
Do
'<Fusstaster abfragen>
If Fusscount = 0 Then
If Antriebaus = 0 Then
Antriebaus = 1
Waitms 100 'Damit der Regelinterrupt auch was davon mitbekommt
Disable Interrupts
Betriebsdatenschreiber
Enable Interrupts
End If
End If
'</Fusstaster abfragen>
'<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
'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
Ldummy2 = Mit_gyro_nick + 5
If Gyro_nick > Ldummy2 Then Sensor_ok_count = 50
Ldummy2 = Mit_gyro_nick - 5
If Gyro_nick < Ldummy2 Then Sensor_ok_count = 50
Ldummy2 = Mit_gyro_roll + 5
If Gyro_roll > Ldummy2 Then Sensor_ok_count = 50
Ldummy2 = Mit_gyro_roll - 5
If Gyro_roll < Ldummy2 Then Sensor_ok_count = 50
'3. Die Werte müssen ungefähr in einer legalen Nullage sein.
If Null_roll < 66150 Then Sensor_ok_count = 50
If Null_roll > 69150 Then Sensor_ok_count = 50
If null_nick < 63850 Then Sensor_ok_count = 50
If null_nick > 66850 Then Sensor_ok_count = 50
If Sensor_ok_count = 0 Then Sensor_ok = 1 'Bedingungen während 50 Messungen (0.5s) eingehalten
End If
Else
Sensor_ok = 0
End If
'</Sicherheitstest Sensoren>
'<Batterieanzeige>
If Ugemessen > Akkufastleer Then 'alles unter 36V Leerlaufspannung ist LEER
If Verbraucht > Akkukap Then
Akkukap = Verbraucht + 1 'Einfach mal annehmen daß noch mehr als angezeigt im Akku ist
Akkugrenzen
End If
End If
'Mit Blinken wenn Gyros nicht bereit
If Antriebaus = 1 Then
If Ugemessen > Akkuleer Then 'alles unter 36V Leerlaufspannung ist LEER
If Verbraucht > Akkukap Then
Akkukap = Verbraucht + 20 'Einfach mal annehmen daß noch mehr als angezeigt im Akku ist
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 'Schneller Blinken wenn Frei
Blinkcount = Intcount
If Blinkcount > 50 Then Blinkcount = Blinkcount - 50
Else
Blinkcount = Intcount / 2
End If
If Blinkcount > 40 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
Led7 = 1
Led6 = 1
Led5 = 1
Led4 = 1
Led3 = 1
End If
End If
Else
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>
If Intcount > 80 Then Led1 = 1 Else Led1 = 0 'Sekundenblinken
'Daran lässt sich leicht erkennen ob
'noch alles normal läuft.
Debounce Button1 , 0 , Onbutton1 , Sub
Loop
End
'**************************************************************************************************
'Ab hier kommen die Subroutinen und die 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 = "N max=" + Utext
Logtext $10 , Utext
Utext = Str(mit_acc_nick)
Utext = "N=" + Utext
Logtext $20 , Utext
Utext = Str(mit_acc_nick_min)
Utext = "N min=" + Utext
Logtext $30 , Utext
Utext = Str(mit_acc_roll_max)
Utext = "R max=" + Utext
Logtext $40 , Utext
Utext = Str(mit_acc_roll)
Utext = "R=" + Utext
Logtext $50 , Utext
Utext = Str(mit_acc_roll_min)
Utext = "R min=" + Utext
Logtext $60 , Utext
Utext = Str(pwm_rechts)
Utext = "PWM r=" + Utext
Logtext $70 , Utext
Utext = Str(pwm_links)
Utext = "PWM l=" + Utext
Logtext $80 , Utext
Utext = Str(u_rechts)
Utext = "U r=" + Utext
Logtext $90 , Utext
Utext = Str(u_links)
Utext = "U l=" + Utext
Logtext $a0 , Utext
Utext = Str(ta)
Utext = "Ta=" + Utext
Logtext $b0 , Utext
Utext = Str(seriell_err_l)
Utext = Utext + "|"
Utext = Utext + Str(seriell_err_r)
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) 'Die Millimeter sollen nicht geschrieben werden
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_l)
Utext = "S l=" + Utext
Logtext $230 , Utext
Utext = Str(seriell_err_r)
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 Onbutton1
If Antriebaus = 1 Then
If Ugemessen > Akkuleer Then 'alles unter 36V Leerlaufspannung ist LEER
If Sensor_ok = 1 Then 'Prüfen ob Gyros noch driften
Disable Interrupts 'Interupts abschalten damit Fusscount nicht gerade jetzt decrementiert wird
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_r = 0
Seriell_err_l = 0
K_diff_min = 0
K_diff_max = 0
Antriebaus = 0
Fusscount = 50 '0.5 Sekunden zeit zum Aufsteigen
Enable Interrupts
Else
If null_nick > 66850 And null_nick < 67450 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!
Akkukap = Verbraucht * 0.95 'Einfach mal annehmen daß zuviel entnommen wurde
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. Normalerweise sollte kein Interrupt so lange sein.
'Dadurch werden andere Interrupts für diese Dauer blockiert.
'**************************************************************************************************
Timer0 = 61 'Sollte ca. 100Hz ergeben
Led2 = 1
If Fusscount > 0 Then Decr Fusscount
If Fusstaster = 0 Then Fusscount = 25 '0.25 Sekunden ohne Fusstaster
Bdummy1 = Udr
acc_roll = Getadc(5) 'Roll
acc_nick = Getadc(7) 'Nick
Gyro_roll = Getadc(2) 'Roll
Gyro_nick = Getadc(4) 'Nick
acc_roll = acc_roll * 100
acc_nick = acc_nick * 100
'<Mittellage ausgleichen>
'Die Offsetwerte werden langsam an eine früher ermittelte Waagerechte Positoion 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 einfach die Nullage der Plattform verändert.
'Beim Vorwärtsbremsen wird nach hinten geneigt.
'Beim Rückwärtsbremsen nach vorne.
'Höchstgeschwindigkeit im Gesetzlichen Rahmen unter 6Km/h (Ausserhalb der KFZ Zulassungsordnung)
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_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
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
Mit_acc_roll = Sum_acc_roll / Acc_anz
'************************************************************************************************************************************************************************************
Sdummy1 = Mit_acc_roll * K_sum 'Das soll ausgleichen, daß die Gyros bei gekippten Sensoren eine Drehung um die falsche Achse
'registrieren.
Sdummy1 = Sdummy1 / 2000 'Die Sensorplatine muss dazu absolut Waagerecht montiert sein.
Lagefehler = Int(sdummy1)
Gyro_nick = Gyro_nick - Lagefehler 'Das soll den Fehler ausgleichen, der durch die geänderte Lage des Gyros auftritt.
'************************************************************************************************************************************************************************************
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
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-Regler>
Sdummy1 = Mit_acc_nick
Sdummy1 = Sdummy1 * P_faktor
Stellwert = Stellwert - Sdummy1
'</P-Regler>
'<I-Regler>
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
If Mit_acc_nick_s < Min_mit_acc_nick_s Then Mit_acc_nick_s = Min_mit_acc_nick_s
Sdummy1 = Mit_acc_nick_s
Sdummy1 = Sdummy1 * I_faktor
Stellwert = Stellwert - Sdummy1
'</I-Regler>
'<D-Regeler>
Sdummy1 = Sig_gyro_nick
Sdummy1 = Sdummy1 * D_faktor
Stellwert = Stellwert + Sdummy1
'</D-Regler>
'<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 in 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
'Lenkerstellung = Lenkerstellung * MP_anpassung 'Dabei kann sich die Lenkung extrem aufschaukeln.
'Lenkfehler berechnen und zum Stellwert hinzu.
Lenkfehler = Lenkerstellung + K_sum
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.
Unfalldatenschreiber
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 = Getadc(5)
Null_roll = Wdummy1 * 100
Wdummy1 = Getadc(7)
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
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
$asm
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
$end Asm
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:
Checksum_rechts = 82
Checksum_rechts = Checksum_rechts + Richtung_rechts
Checksum_rechts = Checksum_rechts + Pwm_rechts
'Geschwindigkeit an Motorregler senden
Zeiger_meldung = 1
Bdummy1 = Udr 'Puffer leeren
'Print Chr(82) ; Chr(richtung_rechts) ; Chr(pwm_rechts) ; Chr(checksum_rechts) ; Chr(13);
'In Assembler geht das so:
$asm
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}
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
$end Asm
Motormeldung(8) = 0 'Damit keine alte Rückmeldung ausgewertet wird
Timeout = Timer0
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
Exit While 'Timeout
End If
Wend
$asm
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
$end Asm
If Bdummy1 <> Checksumme Then Ende = 0
If Ende = 13 Then
U_rechts = U
I_rechts = I
K_rechts = K
Else
Incr Seriell_err_r
S_err = 1
End If
'Linker Motor:
Checksum_links = 76
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);
'In Assembler geht das so:
$asm
Ldi R24 , 76
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
$end Asm
Motormeldung(8) = 0 'Damit keine alte Rückmeldung ausgewertet wird
Timeout = Timer0
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
Exit While 'Timeout
End If
Wend
$asm
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
$end Asm
If Bdummy1 <> Checksumme Then Ende = 0
If Ende = 13 Then
U_links = U
I_links = I
K_links = K
Else
Incr Seriell_err_l
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
'<Notauskriterien ermitteln>
If Mit_acc_nick > -300 Then
If Mit_acc_nick < 300 Then
Regelzeit = 0
End If
End If
Incr Regelzeit
'</Notauskriterien ermitteln>
'<Notauskriterien 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
Notaus = 1
Antriebaus = 1
Ugrund = Str(mit_acc_roll)
Ugrund = "*Roll " + Ugrund
End If
If Mit_acc_roll < -5000 Then 'Lenker zu weit
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
'</Notauskriterien abprüfen>
End If
'<Akkuspannung berechnen>
Ugemessen = U_links + U_rechts
Shift Ugemessen , Right , 1 , Signed
'</Akkuspannung berechnen>
'<Gesamtstrom berechnen>
Igemessen = I_links + I_rechts
'</Gesamtstrom berechnen>
'<verbrauchte As 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 As rausholen
'um überläufe zu verhindern
'Die 360000 kommen weil:
'Igemessen in mA -> Faktor 1
'100 Messungen pro Sekunde -> mal 100
'3600 mAmperesekunden=1 mAh -> also nochmal mal 3600
If Summe_as_entladen > 360000 Then
Summe_mah_entladen = Summe_mah_entladen + 1
Summe_as_entladen = Summe_as_entladen - 360000
End If
If Summe_as_laden > 360000 Then 'Das kann passieren wenns den Berg runter geht
Summe_mah_laden = Summe_mah_laden + 1
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
Meter = Meter + 1
Zentimeter = Zentimeter - 100
End If
If Zentimeter < 0 Then
Meter = Meter - 1
Zentimeter = Zentimeter + 100
End If
If Meter >= 1000 Then
Kilometer = Kilometer + 1
Meter = Meter - 1000
End If
If Meter < 0 Then
Kilometer = Kilometer - 1
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
Incr Tachocount
Select Case Tachocount
'*******************************************************************************
'Alle möglichen Werte werden an den Tacho geschickt. Manche für die Funktion
'des Tachos, andere um die Steuerung zu optimieren.
'*******************************************************************************
Case 0 'U ausgeben
B(1) = 1
U = Ugemessen
'U = Sig_Gyro_Nick
B(2) = Motormeldung(1)
B(3) = Motormeldung(2)
B(5) = 1
Case 1 'I ausgeben
B(1) = 2
I = Igemessen / 10
B(2) = Motormeldung(3)
B(3) = Motormeldung(4)
B(5) = 1
Case 2 'Km/h
B(1) = 3
B(2) = Kmhmeldung(1)
B(3) = Kmhmeldung(2)
B(5) = 1
Case 3 'Akkuprozente
B(1) = 5
B(2) = Akkumeldung(1)
B(3) = Akkumeldung(2)
B(5) = 1
Case 4 'Meter
B(1) = 7
B(2) = Metermeldung(1)
B(3) = Metermeldung(2)
B(5) = 1
Case 5 'Kilometer
B(1) = 6
B(2) = Kilometermeldung(1)
B(3) = Kilometermeldung(2)
B(5) = 1
Case 6
If Antriebaus = 0 Then
Maccroll = Mit_acc_roll
B(1) = 8
B(2) = Maccroll1
B(3) = Maccroll2
B(4) = 1
Else
Nullroll = Null_roll
B(1) = 12
B(2) = Nullroll1
B(3) = Nullroll2
B(4) = 1
End If
Case 7
If Antriebaus = 0 Then
B(1) = 9
B(2) = Maccroll3
B(3) = Maccroll4
B(4) = 1
Else
B(1) = 13
B(2) = Nullroll3
B(3) = Nullroll4
B(4) = 1
End If
Case 8
B(1) = 4
B(2) = Antriebaus
B(3) = Ta
B(4) = 1
Case 9
If Antriebaus = 0 Then
maccnick = Mit_acc_nick
B(1) = 10
B(2) = maccnick1
B(3) = maccnick2
B(4) = 1
Else
nullnick = null_nick
B(1) = 14
B(2) = nullnick1
B(3) = nullnick2
B(4) = 1
End If
Case 10
If Antriebaus = 0 Then
B(1) = 11
B(2) = maccnick3
B(3) = maccnick4
B(4) = 1
Else
B(1) = 15
B(2) = nullnick3
B(3) = nullnick4
B(4) = 1
End If
Case 11
Sw = Stellwert
B(1) = 16
B(2) = Sw1
B(3) = Sw2
B(4) = 1
Case 12
B(1) = 17
B(2) = Sw3
B(3) = Sw4
B(4) = 1
Case 13
Gyronick = Sig_gyro_nick
B(1) = 18
B(2) = Gyronick1
B(3) = Gyronick2
B(4) = 1
Case 14
B(1) = 19
B(2) = Gyronick3
B(3) = Gyronick4
B(4) = 1
'Tachocount = 7 'Wenn die wichtigsten Parameter schnell übermittelt werden sollen.
Case 15
Kksum = K_sum
B(1) = 20
B(2) = Kksum1
B(3) = Kksum2
B(4) = 1
Case 16
B(1) = 21
B(2) = Kksum3
B(3) = Kksum4
B(4) = 1
If Antriebaus = 0 Then Tachocount = 255 'Gleich zu 0 überlaufen lassen
Case 17
B(1) = 22
B(2) = Pf1
B(3) = Pf2
B(4) = 1
Case 18
B(1) = 23
B(2) = Pf3
B(3) = Pf4
B(4) = 1
Case 19
B(1) = 24
B(2) = If1
B(3) = If2
B(4) = 1
Case 20
B(1) = 25
B(2) = If3
B(3) = If4
B(4) = 1
Case 21
B(1) = 26
B(2) = Df1
B(3) = Df2
B(4) = 1
Case 22
B(1) = 27
B(2) = Df3
B(3) = Df4
B(4) = 1
Case 23
B(1) = 28
B(2) = Mpf1
B(3) = Mpf2
B(4) = 1
Case 24
B(1) = 29
B(2) = Mpf3
B(3) = Mpf4
B(4) = 1
Case 25
B(1) = 30
B(2) = Mvf1
B(3) = Mvf2
B(4) = 1
Case 26
B(1) = 31
B(2) = Mvf3
B(3) = Mvf4
B(4) = 1
Case 27
B(1) = 32
B(2) = Mplf1
B(3) = Mplf2
B(4) = 1
Case 28
B(1) = 33
B(2) = Mplf3
B(3) = Mplf4
B(4) = 1
Case 29
B(1) = 34
B(2) = Mvlf1
B(3) = Mvlf2
B(4) = 1
Case 30
B(1) = 35
B(2) = Mvlf3
B(3) = Mvlf4
B(4) = 1
Tachocount = 255
Case Else
B(5) = 0 'Nix 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 in Assembler:
$asm
LDS R24,285 'b(1)
STS udr,R24
Warten11:
LDS R24,UCSR0A
BST R24,5
Brtc warten11
LDS R24,286 'b(2)
STS udr,R24
Warten12:
LDS R24,UCSR0A
BST R24,5
Brtc warten12
lds R24,287 'B(3)
STS udr,R24
Warten13:
LDS R24,UCSR0A
BST R24,5
Brtc warten13
LDS R24,288 'B(4)
STS udr,R24
Warten14:
LDS R24,UCSR0A
BST R24,5
Brtc warten14
LDI R24,13
STS udr,R24
Warten15:
LDS R24,UCSR0A
BST R24,5
Brtc warten15
$end Asm
Ta = Timer0 'Um festzustellen ob der Interrupt zu lange dauert
End If
Led2 = 0
Return
Tacho
Als Tacho wird ein fertiges Display von Display3000.de mit dem optionalen SD-Katen Modul verwendet. Das Display ist für diese Zwecke gerade brauchbar. Die mitgelieferten Bibliotheken könnten etwas besser, und vor allem schneller sein. Neben der normalen Geschwindigkeitsanzeige können über den Tacho auch die PID-Parameter sowie die diversen Parameter zur Anpassung der Motorkennlinie eingestellt werden. Wenn diese Parameter einmal eingestellt sind ist zum Betrieb der Tacho nicht zwingend erforderlich. Das ganze läuft auch ohne. Wenn eine SD-Karte gesteckt ist können mit dem Tacho auch alle Telegramme, die über die Serielle Schnittstelle gehen, mitgeloggt werden. In diesem Modus muss wegen der langsamen Bibliotheken auf die Aktualisierung des Display verzichtet werden. Bei früheren Versuchen wurde auch versucht einen PDA als Tacho zu verwenden. Dies funktioierte sogar, ein mitloggen war jedoch nicht möglich. Windows-Mobile ist bei 600Mhz langsamer als ein Atmel mit 16Mhz.
Software
Die Software des Tachos wurde nicht ganz so sorgfältig erstellt wie die des Motorreglers oder der Lageregelung. Hier war der Leitgedanke "Haupsache est tut".
$hwstack = 256
$swstack = 256
$framesize = 64
$regfile = "m2561def.dat"
$crystal = 16000000 'enter the used clock of your actual microcontroller
$baud1 = 57600
'############################################################################################
'4 - Definition of used ports and pull up resistors
'At our boards we are using Port B for the SPI-communication to the LCD.
'Now we need to 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
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)
Ddrc = &B00000000 'switch all Ports of Port C to input
Portc = &B11111111 'all pull-up-Resistors turned on
Ddrd = &B00000000 'switch all Ports of Port D to input
Portd = &B11110011 '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()
Declare Sub Activate_sd()
Declare Sub Minmax()
Declare Function Trimtext(t As String) As String * 22
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 Sdummy 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 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 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
Config Clock = User
Config Date = Dmy , Separator = .
$include "Init21_display3000.bas"
$include "Config_mmc.bas"
If Porte.7 = 1 Then Activate_display
Gosub Lcd_init
Wait 1
Orientation = Portrait180
If Porte.7 = 0 Then Activate_sd
Gbdriveerror = Driveinit() ' Init MMC/SD Card
If Gbdriveerror = 0 Then
$include "Config_AVR-DOS.BAS" ' Include AVR-DOS Configuration and library
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 Activate_display
Lcd_cls
Lcd_print Titel , 0 , 0 , 1 , 1 , 1 , Yellow , Blue
Lcd_print "SD-Karte init" , 0 , 18 , 1 , 1 , 1 , Black , White
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 Activate_display
Ltext = Str(gbdriveerror)
Ltext = "Fehler: " + Ltext
Lcd_cls
Lcd_print Titel , 0 , 0 , 1 , 1 , 1 , Yellow , Blue
Lcd_print "SD-Karte init" , 0 , 18 , 1 , 1 , 1 , Black , White
Lcd_print Ltext , 0 , 27 , 1 , 1 , 1 , Black , White
Wait 7
End If
Readeeprom Startkilometer , 0
Readeeprom Xoffset , 4
Readeeprom Yoffset , 8
'Einige Werte vorbelegen um Fehlerhafte Grafik
'zu vermeiden wenn keine Verbindung besteht
Xa = 74
Ya = 130
Antriebaus = 1
Null_x = Xoffset
Null_y = Yoffset
If Porte.7 = 1 Then Activate_display
Lcd_cls
Lcd_print Titel , 0 , 0 , 1 , 1 , 1 , Yellow , Blue
If Modus = 0 Then
Lcd_rect 8 , 84 , 124 , 95 , 0 , Black
Lcd_rect 24 , 125 , 124 , 135 , 0 , Black
Lcd_rect 8 , 100 , 19 , 160 , 0 , Black
Lcd_draw 20 , 130 , 22 , 130 , 0 , Black
Lcd_draw 74 , 136 , 74 , 138 , 0 , Black
Lcd_draw 0 , 167 , 132 , 167 , 0 , Black
End If
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 Pind.1 = 0 Then
Waitms 3
If Pind.1 = 0 Then
While Pind.1 = 0
Wend
Modus = Modus + 1
If Gbdriveerror <> 0 Then 'Logging nicht zulassen wenn DriveError
If Modus > 3 Then Modus = 0
End If
If Modus <> 3 Then 'nur berechnen wenn die Werte nicht editiert werden
Sdummy = P_faktor * 10000
Sdummy = Round(sdummy)
Pfaktor = Sdummy
Sdummy = I_faktor * 1000000
Sdummy = Round(sdummy)
Ifaktor = Sdummy
Sdummy = D_faktor * 1000
Sdummy = Round(sdummy)
Dfaktor = Sdummy
Sdummy = Mp_faktor * 10000
Sdummy = Round(sdummy)
Mpfaktor = Sdummy
Sdummy = Mv_faktor * 1000
Sdummy = Round(sdummy)
Mvfaktor = Sdummy
Sdummy = Mpl_faktor * 10000
Sdummy = Round(sdummy)
Mplfaktor = Sdummy
Sdummy = Mvl_faktor * 1000
Sdummy = Round(sdummy)
Mvlfaktor = Sdummy
End If
If Modus > 4 Then Modus = 0
If Porte.7 = 1 Then Activate_display
Lcd_cls
Lcd_print Titel , 0 , 0 , 1 , 1 , 1 , Yellow , Blue
If Modus = 4 Then
Lcd_print "LOGGING" , 30 , 40 , 2 , 1 , 2 , Black , White
Dateiaktuell = 0
Else
If Modus = 0 Then
Lcd_rect 8 , 84 , 124 , 95 , 0 , Black
Lcd_rect 24 , 125 , 124 , 135 , 0 , Black
Lcd_rect 8 , 100 , 19 , 160 , 0 , Black
Lcd_draw 20 , 130 , 22 , 130 , 0 , Black
Lcd_draw 74 , 136 , 74 , 138 , 0 , Black
Lcd_draw 0 , 167 , 132 , 167 , 0 , Black
End If
End If
End If
End If
If Modus <> 4 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
Close #2
Dateiaktuell = 0
End If
If Porte.7 = 1 Then Activate_display 'Standart-Anzeige
If Pind.0 = 0 Then
Waitms 3
If Pind.0 = 0 Then
While Pind.0 = 0
Wend
Sdummy = Meter / 1000
Sdummy = Sdummy + Kilometer
Startkilometer = Sdummy
Writeeeprom Startkilometer , 0
End If
End If
'Spannung und Strom
Sdummy = U / 100
Utext = Fusing(sdummy , "#.#")
Utext = Utext + "V"
Utext = " " + Utext
Sdummy = I / 100
Itext = Fusing(sdummy , "#.#")
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
'Akkuprozente
Sdummy = Akkuprozente / 10
Sdummy = 100 - Sdummy
Text = Fusing(sdummy , "#.#" )
Text = Text + "% "
Text = " " + Text
Farbe = Red
If Sdummy > 33 Then Farbe = Yellow
If Sdummy > 66 Then Farbe = Green
Sdummy = Sdummy * 1.14
X2 = Sdummy
X2 = X2 + 10
X1 = X2 + 1
Lcd_box X1 , 85 , 123 , 94 , White
Lcd_box 9 , 85 , X2 , 94 , Farbe
L1 = Len(text)
L1 = L1 * 5
L1 = L1 / 2
X1 = 66 - L1
Lcd_print Text , X1 , 98 , 1 , 1 , 1 , Black , White
'km/h
Sdummy = Kmh / 10
Text = Fusing(sdummy , "#.#")
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
Sdummy = Meter / 1000
Sdummy = Sdummy + Kilometer
Text = Fusing(sdummy , "#.#")
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
Sdummy = Sdummy - Startkilometer
Text = Fusing(sdummy , "#.###")
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
'Ta
Text = Str(ta)
Lcd_print Text , 0 , 168 , 1 , 1 , 1 , White , Black
'Motorleistung
Utext = Str(ml)
Utext = Utext + " "
Utext = " " + Utext
Itext = Str(mr)
Itext = Itext + " "
L1 = Len(utext)
L2 = Len(itext)
L1 = L1 + L2
L2 = 19 - L1
Ltext = String(l2 , 32)
Text = Utext + Ltext
Text = Text + Itext
Lcd_print Text , 18 , 168 , 1 , 1 , 1 , White , Black
If Antriebaus = 1 Then
Sdummy = Null_x
Sdummy = Sdummy - Xoffset
Sdummy = Sdummy / 50
If Sdummy > 49 Then Sdummy = 49
If Sdummy < -49 Then Sdummy = -49
Sdummy = Sdummy + 74
X1 = Int(sdummy)
Lcd_draw Xa , 126 , Xa , 134 , 0 , White
Lcd_draw X1 , 126 , X1 , 134 , 0 , Black
Xa = X1
Sdummy = Null_y
Sdummy = Sdummy - Yoffset
Sdummy = Sdummy / 50
If Sdummy > 29 Then Sdummy = 29
If Sdummy < -29 Then Sdummy = -29
Sdummy = Sdummy + 130
Y1 = Int(sdummy)
Lcd_draw 9 , Ya , 18 , Ya , 0 , White
Lcd_draw 9 , Y1 , 18 , Y1 , 0 , Black
Ya = Y1
Else
Sdummy = Mit_acc_x
Sdummy = Sdummy / 50
If Sdummy > 49 Then Sdummy = 49
If Sdummy < -49 Then Sdummy = -49
Sdummy = Sdummy + 74
X1 = Int(sdummy)
Lcd_draw Xa , 126 , Xa , 134 , 0 , White
Lcd_draw X1 , 126 , X1 , 134 , 0 , Black
Xa = X1
Sdummy = Mit_acc_y
Sdummy = Sdummy / 50
If Sdummy > 29 Then Sdummy = 29
If Sdummy < -29 Then Sdummy = -29
Sdummy = Sdummy + 130
Y1 = Int(sdummy)
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
Close #2
Dateiaktuell = 0
End If
If Porte.7 = 1 Then Activate_display
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 , 4
Writeeeprom Yoffset , 8
End If
End If
End If
Sdummy = U / 100
Text = Fusing(sdummy , "#.#")
Text = " U= " + Text
Text = Text + "V"
Text = Trimtext(text)
Lcd_print Text , 0 , 9 , 1 , 1 , 1 , Black , White
Sdummy = I / 100
Text = Fusing(sdummy , "#.#")
Text = " I= " + Text
Text = Text + "A"
Text = Trimtext(text)
Lcd_print Text , 0 , 18 , 1 , 1 , 1 , Black , White
Sdummy = Kmh / 10
Text = Fusing(sdummy , "#.#")
Text = " V= " + Text
Text = Text + "km/h"
Text = Trimtext(text)
Lcd_print Text , 0 , 27 , 1 , 1 , 1 , Black , White
Sdummy = Meter / 1000
Sdummy = Sdummy + Kilometer
Text = Fusing(sdummy , "#.###")
Text = " S= " + Text
Text = Text + "km "
Text = Trimtext(text)
Lcd_print Text , 0 , 36 , 1 , 1 , 1 , Black , White
Sdummy = Ml
Text = Str(ml )
Text = " Pl= " + Text
Text = Trimtext(text)
Lcd_print Text , 0 , 45 , 1 , 1 , 1 , Black , White
Sdummy = Mr
Text = Str(mr )
Text = " Pr= " + Text
Text = Trimtext(text)
Lcd_print Text , 0 , 54 , 1 , 1 , 1 , Black , White
Sdummy = Akkuprozente / 10
Sdummy = 100 - Sdummy
Text = Fusing(sdummy , "#.#")
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 'minmax
If Dateiaktuell = 1 Then
If Porte.7 = 0 Then Activate_sd
Close #2
Dateiaktuell = 0
End If
If Porte.7 = 1 Then Activate_display
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
Sdummy = Umin / 100
Text = Fusing(sdummy , "#.#")
Text = " Umin= " + Text
Text = Text + "V"
Text = Trimtext(text)
Lcd_print Text , 0 , 9 , 1 , 1 , 1 , Black , White
Sdummy = Umax / 100
Text = Fusing(sdummy , "#.#")
Text = " Umax= " + Text
Text = Text + "V"
Text = Trimtext(text)
Lcd_print Text , 0 , 18 , 1 , 1 , 1 , Black , White
'**************************************************
Sdummy = Imin / 100
Text = Fusing(sdummy , "#.#")
Text = " Imin= " + Text
Text = Text + "A"
Text = Trimtext(text)
Lcd_print Text , 0 , 27 , 1 , 1 , 1 , Black , White
Sdummy = Imax / 100
Text = Fusing(sdummy , "#.#")
Text = " Imax= " + Text
Text = Text + "A"
Text = Trimtext(text)
Lcd_print Text , 0 , 36 , 1 , 1 , 1 , Black , White
'**************************************************
Sdummy = Kmhmin / 10
Text = Fusing(sdummy , "#.#")
Text = " Vmin= " + Text
Text = Text + "km/h"
Text = Trimtext(text)
Lcd_print Text , 0 , 45 , 1 , 1 , 1 , Black , White
Sdummy = Kmhmax / 10
Text = Fusing(sdummy , "#.#")
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
Case 3
If Dateiaktuell = 1 Then
If Porte.7 = 0 Then Activate_sd
Close #2
Dateiaktuell = 0
End If
If Porte.7 = 1 Then Activate_display
If Pind.4 = 0 Then
Waitms 3
If Pind.4 = 0 Then
While Pind.4 = 0
Wend
If Editzeile > 1 Then Decr Editzeile
End If
End If
If Pind.7 = 0 Then
Waitms 3
If Pind.7 = 0 Then
While Pind.7 = 0
Wend
If Editzeile < 7 Then Incr Editzeile
End If
End If
If Pind.5 = 0 Then
Waitms 3
If Pind.5 = 0 Then
While Pind.5 = 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.6 = 0 Then
Waitms 3
If Pind.6 = 0 Then
While Pind.6 = 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 Pind.0 = 0 Then
Waitms 3
If Pind.0 = 0 Then
While Pind.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
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
If Porte.7 = 0 Then Activate_sd
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 Activate_display
Lcd_print Dateiname , 25 , 75 , 1 , 1 , 1 , Black , White
If Porte.7 = 0 Then Activate_sd
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:
$asm
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;
$end Asm
'<Serielle Telegramme abarbeiten>
If Ep0 = 13 Then
If Ep5 = 13 Then
'Disable Urxc1
$asm
lds R24,{EP1}
LDs R25,{EP2}
add R24,R25
LDs R25,{EP3}
add R24,R25
STS {Bdummy},R24
$end Asm
If Bdummy = Ep4 Then 'Die Prüfsumme stimmt
Select Case Index
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
Maccx1 = Ep2
Maccx2 = Ep3
Case 9
Maccx3 = Ep2
Maccx4 = Ep3
Mit_acc_x = Maccx
Case 10
Maccy1 = Ep2
Maccy2 = Ep3
Case 11
Maccy3 = Ep2
Maccy4 = Ep3
Mit_acc_y = Maccy
Case 12
Nullx1 = Ep2
Nullx2 = Ep3
Case 13
Nullx3 = Ep2
Nullx4 = Ep3
Null_x = Nullx
Case 14
Nully1 = Ep2
Nully2 = Ep3
Case 15
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
$asm
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
$end Asm
Open "COM2:" For Binary As #1
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 < 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
'Enable Urxc1
End If
End If
'</Serielle Telegramme abarbeiten>
Return
'********************************************************************************************
Sub Activate_display
Reset Porte.7 'remove SD Card from bus, need to be "Reset PortF.7" at board D071x
Config Spi = Hard , Interrupt = Off , Data Order = Msb , Master = Yes , Polarity = Low , Phase = 0 , Clockrate = 4 , Noss = 1 'different to what the SD card needs
Set Spsr.spi2x 'hidden parameter: doubles the speed of the SPI output
Spiinit
End Sub
Sub Activate_sd
Set Lcd_port.lcd_cs 'Deactivate Display
Config Spi = Hard , Interrupt = Off , Data Order = Msb , Master = Yes , Polarity = High , Phase = 1 , Clockrate = 4 , Noss = 1 'different to what the display needs
Reset Spsr.spi2x 'no double speed. You may remove this line to try out with your card. It may work.
Spiinit
Set Porte.7 'connects SD Card to bus, need to be "Set PortF.7" at board D071x
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
$include "Glcd21_display3000.bas"
$include "Glcd21_fonts.bas"
'Dummy Data um Fehlermeldungen bei der Kompilierung der Standardroutinen zu vermeiden
'Die Tabelle wird dann bei Nutzung eines indizierten Grafikdatei mit "echten" Daten ausgetauscht
Colortable:
Data 0
Akkuladestecker
Software
Mechanik
Motorumbau